From 51b0ccca0e01e3981b52d34f28060178719c8af7 Mon Sep 17 00:00:00 2001 From: autoware Date: Thu, 29 Aug 2024 15:54:30 +0900 Subject: [PATCH 1/6] Add curve_judgment_threshold in velocity scale factor estimator --- .../eagleye_navigation/eagleye_navigation.hpp | 5 +++-- .../navigation/src/velocity_scale_factor.cpp | 15 +++++++++------ eagleye_rt/config/eagleye_config.yaml | 1 + eagleye_rt/src/velocity_scale_factor_node.cpp | 2 ++ 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp index e3e77d11..16bd3ecf 100644 --- a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp +++ b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp @@ -77,6 +77,7 @@ struct VelocityScaleFactorParameter double estimated_maximum_interval; double gnss_receiving_threshold; bool save_velocity_scale_factor{false}; + double curve_judgment_threshold; }; struct VelocityScaleFactorStatus @@ -500,9 +501,9 @@ struct RollingStatus bool data_status; }; -extern void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter, +extern void velocity_scale_factor_estimate(const sensor_msgs::msg::Imu, const rtklib_msgs::msg::RtklibNav, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter, VelocityScaleFactorStatus*, geometry_msgs::msg::TwistStamped*, eagleye_msgs::msg::VelocityScaleFactor*); -extern void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter, +extern void velocity_scale_factor_estimate(const sensor_msgs::msg::Imu, const nmea_msgs::msg::Gprmc, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter, VelocityScaleFactorStatus*, geometry_msgs::msg::TwistStamped*, eagleye_msgs::msg::VelocityScaleFactor*); extern void distance_estimate(const geometry_msgs::msg::TwistStamped, DistanceStatus*, eagleye_msgs::msg::Distance*); extern void yaw_rate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu, const YawrateOffsetStopParameter, diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index f3bcb2ff..2f313673 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -33,7 +33,7 @@ #define knot2mps 0.51477 -void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, +void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { @@ -91,7 +91,9 @@ void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velo if (velocity_scale_factor_status->estimated_number >= estimated_buffer_number_min && velocity_scale_factor_status->gnss_status_buffer[velocity_scale_factor_status->estimated_number - 1] == true && velocity_scale_factor_status->velocity_buffer[velocity_scale_factor_status->estimated_number - 1] > - velocity_scale_factor_parameter.moving_judgment_threshold) + velocity_scale_factor_parameter.moving_judgment_threshold && + imu.angular_velocity.z < velocity_scale_factor_parameter.curve_judgment_threshold + ) { for (i = 0; i < velocity_scale_factor_status->estimated_number; i++) { @@ -164,7 +166,7 @@ void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velo } -void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav rtklib_nav, const geometry_msgs::msg::TwistStamped velocity, +void velocity_scale_factor_estimate(const sensor_msgs::msg::Imu imu,onst rtklib_msgs::msg::RtklibNav rtklib_nav, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { @@ -216,10 +218,11 @@ void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav rtklib_nav velocity_scale_factor_status->doppler_velocity_buffer.push_back(doppler_velocity); velocity_scale_factor_status->velocity_buffer.push_back(velocity.twist.linear.x); - velocity_scale_factor_estimate_(velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, correction_velocity, velocity_scale_factor); + velocity_scale_factor_estimate_(imu, velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, correction_velocity, velocity_scale_factor); } -void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc nmea_rmc, const geometry_msgs::msg::TwistStamped velocity, +void velocity_scale_factor_estimate(const sensor_msgs::msg::Imu imu, + const nmea_msgs::msg::Gprmc nmea_rmc, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { @@ -243,5 +246,5 @@ void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc nmea_rmc, const velocity_scale_factor_status->doppler_velocity_buffer.push_back(doppler_velocity); velocity_scale_factor_status->velocity_buffer.push_back(velocity.twist.linear.x); - velocity_scale_factor_estimate_(velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, correction_velocity, velocity_scale_factor); + velocity_scale_factor_estimate_(imu, velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, correction_velocity, velocity_scale_factor); } diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml index 67375604..8ca0f8bf 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -46,6 +46,7 @@ save_velocity_scale_factor: false velocity_scale_factor_save_duration: 100.0 th_velocity_scale_factor_percent: 10.0 + curve_judgment_threshold: 0.0873 yaw_rate_offset_stop: estimated_interval: 4 diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index 17005487..fef4348c 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -217,6 +217,7 @@ int main(int argc, char** argv) _velocity_scale_factor_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_minimum_interval"].as(); _velocity_scale_factor_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_maximum_interval"].as(); _velocity_scale_factor_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["gnss_receiving_threshold"].as(); + _velocity_scale_factor_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["curve_judgment_threshold"].as(); node->declare_parameter("velocity_scale_factor_save_str",_velocity_scale_factor_save_str); node->declare_parameter("velocity_scale_factor.save_velocity_scale_factor",_velocity_scale_factor_parameter.save_velocity_scale_factor); @@ -239,6 +240,7 @@ int main(int argc, char** argv) std::cout << "estimated_minimum_interval " << _velocity_scale_factor_parameter.estimated_minimum_interval << std::endl; std::cout << "estimated_maximum_interval " << _velocity_scale_factor_parameter.estimated_maximum_interval << std::endl; std::cout << "gnss_receiving_threshold " << _velocity_scale_factor_parameter.gnss_receiving_threshold << std::endl; + std::cout << "curve_judgment_threshold " << _velocity_scale_factor_parameter.curve_judgment_threshold << std::endl; std::cout<< "velocity_scale_factor_save_str " << _velocity_scale_factor_save_str << std::endl; std::cout<< "save_velocity_scale_factor " << _velocity_scale_factor_parameter.save_velocity_scale_factor << std::endl; From b4426de83740dd108e13e6d860a4656ccb05232b Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Thu, 29 Aug 2024 16:10:18 +0900 Subject: [PATCH 2/6] Fix build error --- eagleye_core/navigation/src/velocity_scale_factor.cpp | 2 +- eagleye_rt/src/velocity_scale_factor_node.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index 2f313673..a0495080 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -166,7 +166,7 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom } -void velocity_scale_factor_estimate(const sensor_msgs::msg::Imu imu,onst rtklib_msgs::msg::RtklibNav rtklib_nav, const geometry_msgs::msg::TwistStamped velocity, +void velocity_scale_factor_estimate(const sensor_msgs::msg::Imu imu,const rtklib_msgs::msg::RtklibNav rtklib_nav, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index fef4348c..320b592f 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -97,12 +97,12 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) if (_use_gnss_mode == "rtklib" || _use_gnss_mode == "RTKLIB") // use RTKLIB mode { - velocity_scale_factor_estimate(_rtklib_nav,_velocity,_velocity_scale_factor_parameter, + velocity_scale_factor_estimate(_imu,_rtklib_nav,_velocity,_velocity_scale_factor_parameter, &_velocity_scale_factor_status,&_correction_velocity,&_velocity_scale_factor); } else if (_use_gnss_mode == "nmea" || _use_gnss_mode == "NMEA") // use NMEA mode { - velocity_scale_factor_estimate(_nmea_rmc,_velocity,_velocity_scale_factor_parameter, + velocity_scale_factor_estimate(_imu,_nmea_rmc,_velocity,_velocity_scale_factor_parameter, &_velocity_scale_factor_status,&_correction_velocity,&_velocity_scale_factor); } From 5fea85aa2babfebff8a2d9caf1fe113654bc4e6c Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Thu, 29 Aug 2024 18:19:32 +0900 Subject: [PATCH 3/6] Fix curve judgment --- eagleye_core/navigation/src/velocity_scale_factor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index a0495080..768462ec 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -92,7 +92,7 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom velocity_scale_factor_status->gnss_status_buffer[velocity_scale_factor_status->estimated_number - 1] == true && velocity_scale_factor_status->velocity_buffer[velocity_scale_factor_status->estimated_number - 1] > velocity_scale_factor_parameter.moving_judgment_threshold && - imu.angular_velocity.z < velocity_scale_factor_parameter.curve_judgment_threshold + std::abs(imu.angular_velocity.z) < velocity_scale_factor_parameter.curve_judgment_threshold ) { for (i = 0; i < velocity_scale_factor_status->estimated_number; i++) From 78233898225799fc64759f378720185e1fd1a394 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Thu, 29 Aug 2024 21:32:37 +0900 Subject: [PATCH 4/6] Remove outlier values for instantaneous velocity scale factor --- .../navigation/src/velocity_scale_factor.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index 768462ec..7d1a7ae1 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -36,7 +36,7 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) -{ +{ int i; double initial_velocity_scale_factor = 1.0; double raw_velocity_scale_factor = 0.0; @@ -88,12 +88,21 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom std::vector index; std::vector velocity_scale_factor_buffer; - if (velocity_scale_factor_status->estimated_number >= estimated_buffer_number_min && + bool velocity_scale_factor_estimate_flag = (velocity_scale_factor_status->estimated_number >= estimated_buffer_number_min && velocity_scale_factor_status->gnss_status_buffer[velocity_scale_factor_status->estimated_number - 1] == true && velocity_scale_factor_status->velocity_buffer[velocity_scale_factor_status->estimated_number - 1] > velocity_scale_factor_parameter.moving_judgment_threshold && std::abs(imu.angular_velocity.z) < velocity_scale_factor_parameter.curve_judgment_threshold - ) + ); + + double tmp_velocity_scale_factor = 0.0; + if(velocity_scale_factor_estimate_flag){ + tmp_velocity_scale_factor = velocity_scale_factor_status->doppler_velocity_buffer[velocity_scale_factor_status->estimated_number - 1] + / velocity_scale_factor_status->velocity_buffer[velocity_scale_factor_status->estimated_number - 1]; + } + + bool valid_velocity_scale_factor_flag = (tmp_velocity_scale_factor > 0.5 && tmp_velocity_scale_factor < 1.5); + if (valid_velocity_scale_factor_flag) { for (i = 0; i < velocity_scale_factor_status->estimated_number; i++) { From c4984c93a014ab941906cc8ffb4dbb4f2f84d25d Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Fri, 30 Aug 2024 10:12:38 +0900 Subject: [PATCH 5/6] Fix outlier remover --- .../navigation/src/velocity_scale_factor.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index 7d1a7ae1..eb4837e5 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -95,14 +95,7 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom std::abs(imu.angular_velocity.z) < velocity_scale_factor_parameter.curve_judgment_threshold ); - double tmp_velocity_scale_factor = 0.0; - if(velocity_scale_factor_estimate_flag){ - tmp_velocity_scale_factor = velocity_scale_factor_status->doppler_velocity_buffer[velocity_scale_factor_status->estimated_number - 1] - / velocity_scale_factor_status->velocity_buffer[velocity_scale_factor_status->estimated_number - 1]; - } - - bool valid_velocity_scale_factor_flag = (tmp_velocity_scale_factor > 0.5 && tmp_velocity_scale_factor < 1.5); - if (valid_velocity_scale_factor_flag) + if (velocity_scale_factor_estimate_flag) { for (i = 0; i < velocity_scale_factor_status->estimated_number; i++) { @@ -123,10 +116,17 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom if (index_length > velocity_scale_factor_status->estimated_number * enabled_data_ratio) { + // RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "--------------------------"); for (i = 0; i < index_length; i++) { - velocity_scale_factor_buffer.push_back(velocity_scale_factor_status->doppler_velocity_buffer[index[i]] / - velocity_scale_factor_status->velocity_buffer[index[i]]); + double tmp_velocity_scale_factor = velocity_scale_factor_status->doppler_velocity_buffer[index[i]] / + velocity_scale_factor_status->velocity_buffer[index[i]]; + bool valid_velocity_scale_factor_flag = (tmp_velocity_scale_factor > 0.5 && tmp_velocity_scale_factor < 1.5); + if(valid_velocity_scale_factor_flag) + { + // RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "tmp_velocity_scale_factor = %f", tmp_velocity_scale_factor); + velocity_scale_factor_buffer.push_back(tmp_velocity_scale_factor); + } } velocity_scale_factor->status.estimate_status = true; @@ -146,10 +146,12 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom { // median size_t size = velocity_scale_factor_buffer.size(); + RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "size = %d", size); double* t = new double[size]; std::copy(velocity_scale_factor_buffer.begin(), velocity_scale_factor_buffer.end(), t); std::sort(t, &t[size]); raw_velocity_scale_factor = size % 2 ? t[size / 2] : (t[(size / 2) - 1] + t[size / 2]) / 2; + RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "raw_velocity_scale_factor = %f", raw_velocity_scale_factor); delete[] t; velocity_scale_factor->scale_factor = raw_velocity_scale_factor; } From 40dcf1f8460c21f5d57f1db102eb77de4c4207b3 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Fri, 30 Aug 2024 12:22:25 +0900 Subject: [PATCH 6/6] Rm outlier remover --- .../navigation/src/velocity_scale_factor.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index eb4837e5..0b7d2581 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -93,7 +93,7 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom velocity_scale_factor_status->velocity_buffer[velocity_scale_factor_status->estimated_number - 1] > velocity_scale_factor_parameter.moving_judgment_threshold && std::abs(imu.angular_velocity.z) < velocity_scale_factor_parameter.curve_judgment_threshold - ); + ); if (velocity_scale_factor_estimate_flag) { @@ -116,17 +116,11 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom if (index_length > velocity_scale_factor_status->estimated_number * enabled_data_ratio) { - // RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "--------------------------"); for (i = 0; i < index_length; i++) { double tmp_velocity_scale_factor = velocity_scale_factor_status->doppler_velocity_buffer[index[i]] / velocity_scale_factor_status->velocity_buffer[index[i]]; - bool valid_velocity_scale_factor_flag = (tmp_velocity_scale_factor > 0.5 && tmp_velocity_scale_factor < 1.5); - if(valid_velocity_scale_factor_flag) - { - // RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "tmp_velocity_scale_factor = %f", tmp_velocity_scale_factor); - velocity_scale_factor_buffer.push_back(tmp_velocity_scale_factor); - } + velocity_scale_factor_buffer.push_back(tmp_velocity_scale_factor); } velocity_scale_factor->status.estimate_status = true; @@ -146,12 +140,10 @@ void velocity_scale_factor_estimate_(const sensor_msgs::msg::Imu imu, const geom { // median size_t size = velocity_scale_factor_buffer.size(); - RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "size = %d", size); double* t = new double[size]; std::copy(velocity_scale_factor_buffer.begin(), velocity_scale_factor_buffer.end(), t); std::sort(t, &t[size]); raw_velocity_scale_factor = size % 2 ? t[size / 2] : (t[(size / 2) - 1] + t[size / 2]) / 2; - RCLCPP_INFO(rclcpp::get_logger("velocity_scale_factor"), "raw_velocity_scale_factor = %f", raw_velocity_scale_factor); delete[] t; velocity_scale_factor->scale_factor = raw_velocity_scale_factor; }