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..0b7d2581 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -33,10 +33,10 @@ #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) -{ +{ int i; double initial_velocity_scale_factor = 1.0; double raw_velocity_scale_factor = 0.0; @@ -88,10 +88,14 @@ void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velo 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) + 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) { for (i = 0; i < velocity_scale_factor_status->estimated_number; i++) { @@ -114,8 +118,9 @@ void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velo { 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]]; + velocity_scale_factor_buffer.push_back(tmp_velocity_scale_factor); } velocity_scale_factor->status.estimate_status = true; @@ -164,7 +169,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,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) { @@ -216,10 +221,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 +249,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..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); } @@ -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;