From 4a6f4e336ca08424d77b1f47c2ae9758ae44c3a6 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Sat, 9 Mar 2024 07:46:05 +0900 Subject: [PATCH] Fixed heading estimation bug in multi-antenna mode at standstill. --- eagleye_rt/src/heading_node.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index cdb5c5c0..872e6db8 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,11 @@ int main(int argc, char** argv) rclcpp::shutdown(); } + if(use_multi_antenna_mode) + { + 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);