made glider be able to publish at IMU rate

Author: jhughes50

README.md

@@ -27,7 +27,7 @@ as this is standard for robotics, but we are working on supporting the NED frame
 ## ROS2 Setup
 We recommend using Glider with ROS2, you can configure the ros parameters in `config/ros-params.yaml`. Here's more detail about what
 the parameters mean:
- - `publishers.rate`: the rate at which odometry is published in hz.
+ - `publishers.rate`: the rate at which odometry is published in hz, setting this to 0 publishes at IMU rate.
  - `publishers.nav_sat_fix`: if true will publish the odometry as a `NavSatFix` msg, the default is an `Odometry` msg.
  - `publishers.viz.use`: if true will publish an `Odometry` topic for visualization centered around the origin.
  - `publishers.viz.origin_easting`: the easting value you want to viz odometry to center around.

glider/config/ros-params.yaml

@@ -1,7 +1,7 @@
 glider_node:
   ros__parameters:
     publishers:
-      rate: 100.0
+      rate: 0.0
       nav_sat_fix: false
       viz:
         use: true

glider/include/ros/glider_node.hpp

@@ -76,6 +76,7 @@ class GliderNode : public rclcpp::Node
         std::string utm_zone_;
         double origin_easting_;
         double origin_northing_;
+        double freq_;
 
         // tracker
         Glider::OdometryWithCovariance current_state_;

glider/ros/glider_node.cpp

@@ -21,7 +21,7 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
     declare_parameter("path", "");
 
     // Get parameters
-    double freq = this->get_parameter("publishers.rate").as_double();
+    freq_ = this->get_parameter("publishers.rate").as_double();
 
     publish_nsf_ = this->get_parameter("publishers.nav_sat_fix").as_bool();
     viz_ = this->get_parameter("publishers.viz.use").as_bool();
@@ -60,13 +60,13 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
     if (publish_nsf_)
     {
         LOG(INFO) << "[GLIDER] Publishing NavSatFix msg on /glider/fix";
-        LOG(INFO) << "[GLIDER] Using prediction rate: " << freq;
+        LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_;
         gps_pub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("/glider/fix", 10);
     }
     else
     {
         LOG(INFO) << "[GLIDER] Publishing Odometry msg on /glider/odom";
-        LOG(INFO) << "[GLIDER] Using prediction rate: " << freq;
+        LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_;
         odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/glider/odom", 10);
     }
 
@@ -76,9 +76,11 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
         odom_viz_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/glider/odom/viz", 10);
     }
 
-    // TODO add in predictor
-    std::chrono::milliseconds d = GliderROS::Conversions::hzToDuration(freq);
-    timer_ = this->create_wall_timer(d, std::bind(&GliderNode::interpolationCallback, this));
+    if (freq_ > 0)
+    {
+        std::chrono::milliseconds d = GliderROS::Conversions::hzToDuration(freq_);
+        timer_ = this->create_wall_timer(d, std::bind(&GliderNode::interpolationCallback, this));
+    }
     LOG(INFO) << "[GLIDER] GliderNode Initialized";
 }
 
@@ -106,6 +108,12 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
     int64_t timestamp = getTime(msg->header.stamp);
 
     glider_->addImu(timestamp, accel, gyro, orient);
+
+    if (freq_ == 0 && current_state_.isInitialized())
+    {
+        Glider::Odometry odom = glider_->interpolate(timestamp);
+        (publish_nsf_) ? publishNavSatFix(odom) : publishOdometry(odom);
+    }
 }
 
 void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)