Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add curve_judgment_threshold in velocity scale factor estimator #337

Closed
wants to merge 6 commits into from
Closed

Add curve_judgment_threshold in velocity scale factor estimator #337

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
51b0ccc
Add curve_judgment_threshold in velocity scale factor estimator
rsasaki0109 Aug 29, 2024
b4426de
Fix build error
rsasaki0109 Aug 29, 2024
5fea85a
Fix curve judgment
rsasaki0109 Aug 29, 2024
7823389
Remove outlier values for instantaneous velocity scale factor
rsasaki0109 Aug 29, 2024
c4984c9
Fix outlier remover
rsasaki0109 Aug 30, 2024
40dcf1f
Rm outlier remover
rsasaki0109 Aug 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down
26 changes: 16 additions & 10 deletions eagleye_core/navigation/src/velocity_scale_factor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -88,10 +88,14 @@ void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velo
std::vector<int> index;
std::vector<double> 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++)
{
Expand All @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
}
1 change: 1 addition & 0 deletions eagleye_rt/config/eagleye_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 4 additions & 2 deletions eagleye_rt/src/velocity_scale_factor_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down Expand Up @@ -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<double>();
_velocity_scale_factor_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_maximum_interval"].as<double>();
_velocity_scale_factor_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["gnss_receiving_threshold"].as<double>();
_velocity_scale_factor_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["curve_judgment_threshold"].as<double>();

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);
Expand All @@ -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;
Expand Down