From a4b5072e5f29dcbabd28356db676439c454b29b5 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 21:11:33 -0400 Subject: [PATCH] made glider be able to publish at IMU rate --- README.md | 2 +- glider/config/ros-params.yaml | 2 +- glider/include/ros/glider_node.hpp | 1 + glider/ros/glider_node.cpp | 20 ++++++++++++++------ 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index abbc220..9e99866 100644 --- a/README.md +++ b/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. diff --git a/glider/config/ros-params.yaml b/glider/config/ros-params.yaml index d223bd8..09dfc6b 100644 --- a/glider/config/ros-params.yaml +++ b/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 diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index 5feda06..8a410af 100644 --- a/glider/include/ros/glider_node.hpp +++ b/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_; diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp index 59f13ce..9d2e089 100644 --- a/glider/ros/glider_node.cpp +++ b/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("/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("/glider/odom", 10); } @@ -76,9 +76,11 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide odom_viz_pub_ = this->create_publisher("/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)