From dd710671b120e954e04a186797f33456b9f0a119 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Sat, 9 Mar 2024 07:57:30 +0900 Subject: [PATCH] [ROS2]Fixed heading estimation bug in multi-antenna mode at standstill. (#322) * Fixed heading estimation bug in multi-antenna mode at standstill. * Add comment about is_first_correction_velocity in multi-antenna mode --- eagleye_rt/src/heading_node.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index cdb5c5c0..3543c877 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -68,6 +68,8 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { velocity = *msg; + // To avoid unnecessary buffering when it's just sitting there right after start-up, we're making it so it doesn't buffer. + // Multi-antenna mode is an exception. if (is_first_correction_velocity == false && msg->twist.linear.x > heading_parameter.moving_judgment_threshold) { is_first_correction_velocity = true; @@ -118,8 +120,16 @@ void rmc_callback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - if (!is_first_correction_velocity) return; - if(use_can_less_mode && !velocity_status.status.enabled_status) return; + if (!is_first_correction_velocity) + { + RCLCPP_WARN(rclcpp::get_logger(node_name), "is_first_correction_velocity is false."); + return; + } + if(use_can_less_mode && !velocity_status.status.enabled_status) + { + RCLCPP_WARN(rclcpp::get_logger(node_name), "velocity_status is not enabled."); + return; + } if(!yaw_rate_offset_stop.status.enabled_status) { if(skip_static_initialization) @@ -251,6 +261,12 @@ int main(int argc, char** argv) rclcpp::shutdown(); } + if(use_multi_antenna_mode) + { + // When using multi-antenna mode, set is_first_correction_velocity to true even when stationary for the first time. + is_first_correction_velocity = true; + } + auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); auto sub3 = node->create_subscription(subscribe_rmc_topic_name, 1000, rmc_callback);