diff --git a/README.md b/README.md index f1fb26f1..5736ecf7 100755 --- a/README.md +++ b/README.md @@ -123,6 +123,21 @@ Clone and build the necessary packages for Eagleye. - [eagleye_pp](eagleye_pp) : post-processing version +## Optional Features + +### canless mode + +While normal eagleye requires CAN (wheel speed) information, this option allows localization estimation with GNSS/IMU alone, without vehicle speed. + +However, in this mode, note the following +- Accuracy degrades in non-open sky environments. +- RTK positioning is required. +- Localization estimating is not possible when driving backward. + +To use this mode +- Use launch/eagleye_rt_canless.launch for eagleye_rt instead of launch/eagleye_rt.launch. +- Set use_canless_mode in eagleye_pp_config.yaml to true for eagleye_pp. + ## Sample data ### ROSBAG @@ -155,4 +170,4 @@ Eagleye is provided under the [BSD 3-Clause](https://github.com/MapIV/eagleye/bl ## Contacts -If you have further question, email to map4@tier4.jp. +If you have further question, email to support@map4.jp. diff --git a/docs/parameter_overview.png b/docs/parameter_overview.png new file mode 100644 index 00000000..010163ed Binary files /dev/null and b/docs/parameter_overview.png differ diff --git a/eagleye_core/navigation/include/navigation/navigation.hpp b/eagleye_core/navigation/include/navigation/navigation.hpp index 8b471ba1..d12d4fd4 100644 --- a/eagleye_core/navigation/include/navigation/navigation.hpp +++ b/eagleye_core/navigation/include/navigation/navigation.hpp @@ -58,10 +58,12 @@ struct VelocityScaleFactorParameter { - double estimated_number_min; - double estimated_number_max; - double estimated_velocity_threshold; - double estimated_coefficient; + double imu_rate; + double gnss_rate; + double moving_judgment_threshold; + double estimated_minimum_interval; + double estimated_maximum_interval; + double gnss_receiving_threshold; bool save_velocity_scale_factor{false}; }; @@ -83,8 +85,9 @@ struct DistanceStatus struct YawrateOffsetStopParameter { - double stop_judgment_velocity_threshold; - double estimated_number; + double imu_rate; + double estimated_interval; + double stop_judgment_threshold; double outlier_threshold; }; @@ -98,10 +101,12 @@ struct YawrateOffsetStopStatus struct YawrateOffsetParameter { - double estimated_number_min; - double estimated_number_max; - double estimated_coefficient; - double estimated_velocity_threshold; + double imu_rate; + double gnss_rate; + double moving_judgment_threshold; + double estimated_minimum_interval; + double estimated_maximum_interval; + double gnss_receiving_threshold; double outlier_threshold; }; @@ -121,21 +126,23 @@ struct YawrateOffsetStatus struct HeadingParameter { - double estimated_number_min; - double estimated_number_max; - double estimated_gnss_coefficient; - double estimated_heading_coefficient; + double imu_rate; + double gnss_rate; + double stop_judgment_threshold; + double moving_judgment_threshold; + double estimated_minimum_interval; + double estimated_maximum_interval; + double gnss_receiving_threshold; double outlier_threshold; - double estimated_velocity_threshold; - double stop_judgment_velocity_threshold; - double estimated_yawrate_threshold; - + double outlier_ratio_threshold; + double curve_judgment_threshold; }; struct HeadingStatus { int tow_last; double rmc_time_last; + double ros_time_last; int estimated_number; std::vector time_buffer; std::vector heading_angle_buffer; @@ -149,17 +156,17 @@ struct HeadingStatus struct RtkHeadingParameter { - double estimated_distance; - double estimated_heading_buffer_min; - double estimated_number_min; - double estimated_number_max; - double estimated_gnss_coefficient; - double estimated_heading_coefficient; + double imu_rate; + double gnss_rate; + double stop_judgment_threshold; + double slow_judgment_threshold; + double update_distance; + double estimated_minimum_interval; + double estimated_maximum_interval; + double gnss_receiving_threshold; double outlier_threshold; - double estimated_velocity_threshold; - double stop_judgment_velocity_threshold; - double estimated_yawrate_threshold; - + double outlier_ratio_threshold; + double curve_judgment_threshold; }; struct RtkHeadingStatus @@ -184,8 +191,9 @@ struct RtkHeadingStatus struct HeadingInterpolateParameter { - double stop_judgment_velocity_threshold; - double number_buffer_max; + double imu_rate; + double stop_judgment_threshold; + double sync_search_period; }; struct HeadingInterpolateStatus @@ -202,12 +210,6 @@ struct HeadingInterpolateStatus struct PositionParameter { - double estimated_distance; - double separation_distance; - double estimated_velocity_threshold; - double outlier_threshold; - double estimated_enu_vel_coefficient; - double estimated_position_coefficient; double ecef_base_pos_x; double ecef_base_pos_y; double ecef_base_pos_z; @@ -220,6 +222,15 @@ struct PositionParameter double tf_gnss_rotation_w; std::string tf_gnss_parent_frame; std::string tf_gnss_child_frame; + + double imu_rate; + double gnss_rate; + double moving_judgment_threshold; + double estimated_interval; + double update_distance; + double gnss_receiving_threshold; + double outlier_threshold; + double outlier_ratio_threshold; }; struct PositionStatus @@ -241,8 +252,9 @@ struct PositionStatus struct PositionInterpolateParameter { - double number_buffer_max; - double stop_judgment_velocity_threshold; + double imu_rate; + double stop_judgment_threshold; + double sync_search_period; }; struct PositionInterpolateStatus @@ -263,18 +275,19 @@ struct PositionInterpolateStatus struct SlipangleParameter { - double stop_judgment_velocity_threshold; double manual_coefficient; + double stop_judgment_threshold; }; struct SlipCoefficientParameter { - double estimated_number_min; - double estimated_number_max; - double estimated_velocity_threshold; - double estimated_yawrate_threshold; + double imu_rate; + double estimated_minimum_interval; + double estimated_maximum_interval; + double stop_judgment_threshold; + double moving_judgment_threshold; + double curve_judgment_threshold; double lever_arm; - double stop_judgment_velocity_threshold; }; struct SlipCoefficientStatus @@ -286,12 +299,13 @@ struct SlipCoefficientStatus struct SmoothingParameter { + double gnss_rate; + double moving_judgment_threshold; + double moving_average_time; + double moving_ratio_threshold; double ecef_base_pos_x; double ecef_base_pos_y; double ecef_base_pos_z; - int estimated_number_max; - double estimated_velocity_threshold; - double estimated_threshold; }; struct SmoothingStatus @@ -305,8 +319,8 @@ struct SmoothingStatus struct TrajectoryParameter { - double stop_judgment_velocity_threshold; - double stop_judgment_yawrate_threshold; + double stop_judgment_threshold; + double curve_judgment_threshold; }; struct TrajectoryStatus @@ -318,14 +332,16 @@ struct TrajectoryStatus struct HeightParameter { - double estimated_distance; - double estimated_distance_max; - double separation_distance; - double estimated_velocity_threshold; - double estimated_velocity_coefficient; - double estimated_height_coefficient; + double imu_rate; + double gnss_rate; + double moving_judgment_threshold; + double estimated_minimum_interval; + double estimated_maximum_interval; + double update_distance; + double gnss_receiving_threshold; double outlier_threshold; - int average_num; + double outlier_ratio_threshold; + double moving_average_time; }; struct HeightStatus @@ -360,9 +376,11 @@ struct HeightStatus struct AngularVelocityOffsetStopParameter { - double stop_judgment_velocity_threshold; - double estimated_number; + double imu_rate; + double estimated_interval; + double stop_judgment_threshold; double outlier_threshold; + }; struct AngularVelocityOffsetStopStatus @@ -379,7 +397,7 @@ struct AngularVelocityOffsetStopStatus struct RtkDeadreckoningParameter { - double stop_judgment_velocity_threshold; + double stop_judgment_threshold; double ecef_base_pos_x; double ecef_base_pos_y; double ecef_base_pos_z; @@ -414,11 +432,12 @@ struct RtkDeadreckoningStatus struct EnableAdditionalRollingParameter { - double matching_update_distance; - double stop_judgment_velocity_threshold; - double rolling_buffer_num; - double link_Time_stamp_parameter; - double imu_buffer_num; + double imu_rate; + double stop_judgment_threshold; + double update_distance; + double moving_average_time; + double sync_judgment_threshold; + double sync_search_period; }; struct EnableAdditionalRollingStatus @@ -444,7 +463,7 @@ struct EnableAdditionalRollingStatus struct RollingParameter { - double stop_judgment_velocity_threshold; + double stop_judgment_threshold; double filter_process_noise; double filter_observation_noise; }; @@ -471,6 +490,8 @@ extern void heading_estimate(const rtklib_msgs::RtklibNav, const sensor_msgs::Im HeadingStatus*,eagleye_msgs::Heading*); extern void heading_estimate(const nmea_msgs::Gprmc, const sensor_msgs::Imu, const geometry_msgs::TwistStamped, const eagleye_msgs::YawrateOffset, const eagleye_msgs::YawrateOffset, const eagleye_msgs::SlipAngle, const eagleye_msgs::Heading, const HeadingParameter, HeadingStatus*,eagleye_msgs::Heading*); +extern void heading_estimate(const eagleye_msgs::Heading, const sensor_msgs::Imu, const geometry_msgs::TwistStamped, const eagleye_msgs::YawrateOffset, + const eagleye_msgs::YawrateOffset, const eagleye_msgs::SlipAngle, const eagleye_msgs::Heading, const HeadingParameter, HeadingStatus*,eagleye_msgs::Heading*); extern void position_estimate(const rtklib_msgs::RtklibNav, const geometry_msgs::TwistStamped, const eagleye_msgs::StatusStamped, const eagleye_msgs::Distance, const eagleye_msgs::Heading, const geometry_msgs::Vector3Stamped, const PositionParameter, PositionStatus*, eagleye_msgs::Position*); extern void position_estimate(const nmea_msgs::Gpgga gga, const geometry_msgs::TwistStamped, const eagleye_msgs::StatusStamped, const eagleye_msgs::Distance, const eagleye_msgs::Heading, @@ -539,6 +560,8 @@ class VelocityEstimator private: struct Param { + int imu_rate; + int estimated_interval; int buffer_count_max; }; PitchrateOffsetStopEstimator::Param param; @@ -563,10 +586,16 @@ class VelocityEstimator private: struct Param { - int buffer_max; + double imu_rate; + double gnss_rate; + double estimated_interval; + double buffer_max; double outlier_threshold; double estimated_velocity_threshold; + double slow_judgment_threshold; + double gnss_receiving_threshold; double estimated_gnss_coefficient; + double outlier_ratio_threshold; double estimated_coefficient; }; PitchingEstimator::Param param; @@ -594,6 +623,10 @@ class VelocityEstimator private: struct Param { + double imu_rate; + double gnss_rate; + double estimated_minimum_interval; + double estimated_maximum_interval; double buffer_min; double buffer_max; double filter_process_noise; @@ -618,14 +651,21 @@ class VelocityEstimator double ecef_base_pos_z; bool use_ecef_base_position; + double imu_rate; + double gnss_rate; + double gga_downsample_time; + double stop_judgment_interval; double stop_judgment_velocity_threshold; double stop_judgment_buffer_maxnum; double variance_threshold; // doppler fusion parameter + double estimated_interval; double buffer_max; + double gnss_receiving_threshold; double estimated_gnss_coefficient; + double outlier_ratio_threshold; double estimated_coefficient; double outlier_threshold; }; @@ -637,6 +677,7 @@ class VelocityEstimator double imu_time_last; // rtklib_nav variables + double gnss_rate; double doppler_velocity; double rtklib_nav_time_last; bool rtklib_update_status; diff --git a/eagleye_core/navigation/package.xml b/eagleye_core/navigation/package.xml index aa57ec8b..2e621426 100755 --- a/eagleye_core/navigation/package.xml +++ b/eagleye_core/navigation/package.xml @@ -30,6 +30,7 @@ rtklib_msgs eagleye_msgs eagleye_coordinate + yaml-cpp diff --git a/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp b/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp index 879d1258..133f19f5 100755 --- a/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp +++ b/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp @@ -28,8 +28,8 @@ * Author MapIV Sekino */ - #include "coordinate/coordinate.hpp" - #include "navigation/navigation.hpp" +#include "coordinate/coordinate.hpp" +#include "navigation/navigation.hpp" void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped velocity, const sensor_msgs::Imu imu, const AngularVelocityOffsetStopParameter angular_velocity_stop_parameter, AngularVelocityOffsetStopStatus* angular_velocity_stop_status, @@ -39,11 +39,13 @@ void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped vel int i; double roll_tmp, pitch_tmp, yaw_tmp; double initial_angular_velocity_offset_stop = 0.0; - double estimated_time_buffer_num = angular_velocity_stop_parameter.estimated_number; std::size_t rollrate_buffer_length; std::size_t pitchrate_buffer_length; std::size_t yawrate_buffer_length; + double estimated_buffer_number = angular_velocity_stop_parameter.imu_rate * angular_velocity_stop_parameter.estimated_interval; + double estimated_time_buffer_number = angular_velocity_stop_parameter.imu_rate * angular_velocity_stop_parameter.estimated_interval; + // data buffer generate if (angular_velocity_stop_status->estimate_start_status == false) { @@ -63,14 +65,14 @@ void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped vel pitchrate_buffer_length = std::distance(angular_velocity_stop_status->pitchrate_buffer.begin(), angular_velocity_stop_status->pitchrate_buffer.end()); yawrate_buffer_length = std::distance(angular_velocity_stop_status->yawrate_buffer.begin(), angular_velocity_stop_status->yawrate_buffer.end()); - if (yawrate_buffer_length > angular_velocity_stop_parameter.estimated_number + estimated_time_buffer_num) + if (yawrate_buffer_length > estimated_buffer_number + estimated_time_buffer_number) { angular_velocity_stop_status->rollrate_buffer.erase(angular_velocity_stop_status->rollrate_buffer.begin()); angular_velocity_stop_status->pitchrate_buffer.erase(angular_velocity_stop_status->pitchrate_buffer.begin()); angular_velocity_stop_status->yawrate_buffer.erase(angular_velocity_stop_status->yawrate_buffer.begin()); } - if (velocity.twist.linear.x < angular_velocity_stop_parameter.stop_judgment_velocity_threshold) + if (velocity.twist.linear.x < angular_velocity_stop_parameter.stop_judgment_threshold) { ++angular_velocity_stop_status->stop_count; } @@ -80,20 +82,20 @@ void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped vel } // mean - if (angular_velocity_stop_status->stop_count > angular_velocity_stop_parameter.estimated_number + estimated_time_buffer_num) + if (angular_velocity_stop_status->stop_count > estimated_buffer_number + estimated_time_buffer_number) { roll_tmp = 0.0; pitch_tmp = 0.0; yaw_tmp = 0.0; - for (i = 0; i < angular_velocity_stop_parameter.estimated_number; i++) + for (i = 0; i < estimated_buffer_number; i++) { roll_tmp += angular_velocity_stop_status->rollrate_buffer[i]; pitch_tmp += angular_velocity_stop_status->pitchrate_buffer[i]; yaw_tmp += angular_velocity_stop_status->yawrate_buffer[i]; } - angular_velocity_offset_stop->angular_velocity_offset.x = -1 * roll_tmp / angular_velocity_stop_parameter.estimated_number; - angular_velocity_offset_stop->angular_velocity_offset.y = -1 * pitch_tmp / angular_velocity_stop_parameter.estimated_number; - angular_velocity_offset_stop->angular_velocity_offset.z = -1 * yaw_tmp / angular_velocity_stop_parameter.estimated_number; + angular_velocity_offset_stop->angular_velocity_offset.x = -1 * roll_tmp / estimated_buffer_number; + angular_velocity_offset_stop->angular_velocity_offset.y = -1 * pitch_tmp / estimated_buffer_number; + angular_velocity_offset_stop->angular_velocity_offset.z = -1 * yaw_tmp / estimated_buffer_number; angular_velocity_offset_stop->status.enabled_status = true; angular_velocity_offset_stop->status.estimate_status = true; angular_velocity_stop_status->estimate_start_status = true; diff --git a/eagleye_core/navigation/src/enable_additional_rolling.cpp b/eagleye_core/navigation/src/enable_additional_rolling.cpp index de3e4b58..11948223 100644 --- a/eagleye_core/navigation/src/enable_additional_rolling.cpp +++ b/eagleye_core/navigation/src/enable_additional_rolling.cpp @@ -49,18 +49,19 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci double rolling_estimated_sum = 0.0; double rolling_estimated_average = 0.0; double rolling_interpolate = 0.0; - double rolling_offset_buffer_num = rolling_parameter.rolling_buffer_num / 2; // Parameter to correct for time delay caused by moving average. - - rolling_status->yawrate = imu.angular_velocity.z; + double moving_average_buffer_number = rolling_parameter.moving_average_time * rolling_parameter.imu_rate; + double search_buffer_number = rolling_parameter.sync_search_period * rolling_parameter.imu_rate; + double rolling_delay_interpolation_buffer_num = search_buffer_number / 2; // Parameter to correct for time delay caused by moving average. + rolling_status->yawrate = imu.angular_velocity.z; rolling_status->rollrate = imu.angular_velocity.x; rolling_status->rollrate_offset_stop = angular_velocity_offset_stop.angular_velocity_offset.x; rolling_status->imu_acceleration_y = imu.linear_acceleration.y; // data buffer - if (rolling_status->imu_time_buffer.size() < rolling_parameter.imu_buffer_num && velocity_status.status.enabled_status) + if (rolling_status->imu_time_buffer.size() < search_buffer_number && velocity_status.status.enabled_status) { rolling_status->imu_time_buffer.push_back(imu.header.stamp.toSec()); rolling_status->yawrate_buffer.push_back(rolling_status->yawrate); @@ -90,11 +91,11 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci /// acc_y_offset /// if (data_buffer_status && localization_pose.header.stamp.toSec() != rolling_status->localization_time_last) { - for ( int i = 0; i < rolling_parameter.imu_buffer_num - 1; i++ ) + for ( int i = 0; i < search_buffer_number - 1; i++ ) { - if (std::abs(rolling_status->imu_time_buffer[i] - localization_pose.header.stamp.toSec()) < rolling_parameter.link_Time_stamp_parameter) + if (std::abs(rolling_status->imu_time_buffer[i] - localization_pose.header.stamp.toSec()) < rolling_parameter.sync_judgment_threshold) { - if (std::abs(rolling_status->distance_last - rolling_status->distance_buffer[i]) >= rolling_parameter.matching_update_distance ) + if (std::abs(rolling_status->distance_last - rolling_status->distance_buffer[i]) >= rolling_parameter.update_distance ) { acc_offset_status = true; rolling_status->distance_last = rolling_status->distance_buffer[i]; @@ -103,7 +104,7 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci if (acc_offset_status) { - if (rolling_status->velocity_buffer[i] > rolling_parameter.stop_judgment_velocity_threshold) + if (rolling_status->velocity_buffer[i] > rolling_parameter.stop_judgment_threshold) { tf::Quaternion localization_quat; quaternionMsgToTF(localization_pose.pose.orientation, localization_quat); @@ -134,7 +135,7 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci /// estimated rolling angle /// if (acc_y_offset->status.enabled_status) { - if (velocity.twist.linear.x > rolling_parameter.stop_judgment_velocity_threshold) + if (velocity.twist.linear.x > rolling_parameter.stop_judgment_threshold) { rolling_estimated_tmp = std::asin((velocity.twist.linear.x*(rolling_status->yawrate+yawrate_offset_2nd.yawrate_offset)/g)- (rolling_status->imu_acceleration_y-acc_y_offset->acc_y_offset)/g); @@ -150,7 +151,7 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci double rolling_interpolate_tmp = (rolling_status->rollrate - rolling_status->rollrate_offset_stop)*diff_imu_time; /// buffering estimated rolling angle offset /// - if (rolling_status->roll_rate_interpolate_buffer.size() < rolling_offset_buffer_num) + if (rolling_status->roll_rate_interpolate_buffer.size() < rolling_delay_interpolation_buffer_num) { rolling_status->roll_rate_interpolate_buffer.push_back(rolling_interpolate_tmp); } @@ -163,7 +164,7 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci /// buffering estimated roll angle /// - if (rolling_status->rolling_estimated_buffer.size() < rolling_parameter.rolling_buffer_num && acc_y_offset->status.enabled_status) + if (rolling_status->rolling_estimated_buffer.size() < moving_average_buffer_number && acc_y_offset->status.enabled_status) { rolling_status->rolling_estimated_buffer.push_back(rolling_estimated_tmp); } @@ -181,7 +182,7 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci /// buffering rolling offset /// if (rolling_buffer_status) { - for( int i = 0; i roll_rate_interpolate_buffer[i]; } @@ -190,11 +191,11 @@ void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped veloci /// Moving average estimation of roll angle /// if (rolling_estimated_buffer_status ) { - for ( int i = 0; i rolling_estimated_buffer[i]; } - rolling_estimated_average = rolling_estimated_sum/rolling_parameter.rolling_buffer_num; + rolling_estimated_average = rolling_estimated_sum/moving_average_buffer_number; rolling_angle->rolling_angle = rolling_estimated_average + rolling_interpolate; rolling_angle->status.enabled_status=true; rolling_angle->status.estimate_status=true; diff --git a/eagleye_core/navigation/src/heading.cpp b/eagleye_core/navigation/src/heading.cpp old mode 100755 new mode 100644 index 67b40e66..a21c5df1 --- a/eagleye_core/navigation/src/heading.cpp +++ b/eagleye_core/navigation/src/heading.cpp @@ -35,26 +35,27 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity eagleye_msgs::YawrateOffset yawrate_offset,eagleye_msgs::SlipAngle slip_angle,eagleye_msgs::Heading heading_interpolate,HeadingParameter heading_parameter, HeadingStatus* heading_status,eagleye_msgs::Heading* heading) { - int i,index_max; - double yawrate = 0.0; - double avg = 0.0,tmp_heading_angle; - bool gnss_status,gnss_update; + int index_max; + double avg = 0.0, tmp_heading_angle; std::size_t index_length; std::size_t time_buffer_length; - std::size_t inversion_up_index_length; - std::size_t inversion_down_index_length; std::vector::iterator max; - if (heading_status->estimated_number < heading_parameter.estimated_number_max) + double estimated_buffer_number_min = heading_parameter.estimated_minimum_interval * heading_parameter.imu_rate; + double estimated_buffer_number_max = heading_parameter.estimated_maximum_interval * heading_parameter.imu_rate; + double enabled_data_ratio = heading_parameter.gnss_rate / heading_parameter.imu_rate * heading_parameter.gnss_receiving_threshold; + double remain_data_ratio = enabled_data_ratio * heading_parameter.outlier_ratio_threshold; + + if (heading_status->estimated_number < estimated_buffer_number_max) { ++heading_status->estimated_number ; } else { - heading_status->estimated_number = heading_parameter.estimated_number_max; + heading_status->estimated_number = estimated_buffer_number_max; } - yawrate = imu.angular_velocity.z; + double yawrate = imu.angular_velocity.z; // data buffer generate heading_status->time_buffer .push_back(imu.header.stamp.toSec()); @@ -66,7 +67,7 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity time_buffer_length = std::distance(heading_status->time_buffer .begin(), heading_status->time_buffer .end()); - if (time_buffer_length > heading_parameter.estimated_number_max) + if (time_buffer_length > estimated_buffer_number_max) { heading_status->time_buffer .erase(heading_status->time_buffer .begin()); heading_status->heading_angle_buffer .erase(heading_status->heading_angle_buffer .begin()); @@ -82,10 +83,10 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity std::vector velocity_index; std::vector index; - if (heading_status->estimated_number > heading_parameter.estimated_number_min && + if (heading_status->estimated_number > estimated_buffer_number_min && heading_status->gnss_status_buffer [heading_status->estimated_number -1] == true && - heading_status->correction_velocity_buffer [heading_status->estimated_number -1] > heading_parameter.estimated_velocity_threshold && - fabsf(heading_status->yawrate_buffer [heading_status->estimated_number -1]) < heading_parameter.estimated_yawrate_threshold) + heading_status->correction_velocity_buffer [heading_status->estimated_number -1] > heading_parameter.moving_judgment_threshold && + fabsf(heading_status->yawrate_buffer [heading_status->estimated_number -1]) < heading_parameter.curve_judgment_threshold) { heading->status.enabled_status = true; } @@ -94,15 +95,15 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity heading->status.enabled_status = false; } - if (heading->status.enabled_status == true) + if (heading->status.enabled_status) { - for (i = 0; i < heading_status->estimated_number ; i++) + for (int i = 0; i < heading_status->estimated_number ; i++) { - if (heading_status->gnss_status_buffer [i] == true) + if (heading_status->gnss_status_buffer [i]) { gnss_index.push_back(i); } - if (heading_status->correction_velocity_buffer [i] > heading_parameter.estimated_velocity_threshold) + if (heading_status->correction_velocity_buffer [i] > heading_parameter.moving_judgment_threshold) { velocity_index.push_back(i); } @@ -113,25 +114,25 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity index_length = std::distance(index.begin(), index.end()); - if (index_length > heading_status->estimated_number * heading_parameter.estimated_gnss_coefficient) + if (index_length > heading_status->estimated_number * enabled_data_ratio) { std::vector provisional_heading_angle_buffer(heading_status->estimated_number , 0); - for (i = 0; i < heading_status->estimated_number ; i++) + for (int i = 0; i < heading_status->estimated_number ; i++) { if (i > 0) { - if (std::abs(heading_status->correction_velocity_buffer [heading_status->estimated_number -1]) > heading_parameter.stop_judgment_velocity_threshold) + if (std::abs(heading_status->correction_velocity_buffer [heading_status->estimated_number -1]) > heading_parameter.moving_judgment_threshold) { + double dt = heading_status->time_buffer [i] - heading_status->time_buffer [i-1]; provisional_heading_angle_buffer[i] = provisional_heading_angle_buffer[i-1] + - ((heading_status->yawrate_buffer [i] + heading_status->yawrate_offset_buffer [i]) * - (heading_status->time_buffer [i] - heading_status->time_buffer [i-1])); + ((heading_status->yawrate_buffer [i] + heading_status->yawrate_offset_buffer [i]) * dt); } else { + double dt = heading_status->time_buffer [i] - heading_status->time_buffer [i-1]; provisional_heading_angle_buffer[i] = provisional_heading_angle_buffer[i-1] + - ((heading_status->yawrate_buffer [i] + heading_status->yawrate_offset_stop_buffer [i]) * - (heading_status->time_buffer [i] - heading_status->time_buffer [i-1])); + ((heading_status->yawrate_buffer [i] + heading_status->yawrate_offset_stop_buffer [i]) * dt); } } } @@ -139,10 +140,8 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity std::vector base_heading_angle_buffer; std::vector base_heading_angle_buffer2; std::vector diff_buffer; - std::vector inversion_up_index; - std::vector inversion_down_index; - if(heading_interpolate.status.enabled_status == false) + if(!heading_interpolate.status.enabled_status) { heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]]; } @@ -152,13 +151,13 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity copy(heading_status->heading_angle_buffer .begin(), heading_status->heading_angle_buffer .end(), back_inserter(heading_angle_buffer2) ); - for (i = 0; i < heading_status->estimated_number ; i++) + for (int i = 0; i < heading_status->estimated_number ; i++) { base_heading_angle_buffer.push_back(heading_interpolate.heading_angle - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[i]); } - for (i = 0; i < index_length; i++) + for (int i = 0; i < index_length; i++) { ref_cnt = (base_heading_angle_buffer[index[i]] - std::fmod(base_heading_angle_buffer[index[i]],2*M_PI))/(2*M_PI); if(base_heading_angle_buffer[index[i]] < 0) ref_cnt = ref_cnt -1; @@ -170,14 +169,14 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity index_length = std::distance(index.begin(), index.end()); base_heading_angle_buffer.clear(); - for (i = 0; i < heading_status->estimated_number ; i++) + for (int i = 0; i < heading_status->estimated_number ; i++) { base_heading_angle_buffer.push_back(heading_angle_buffer2[index[index_length-1]] - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[i]); } diff_buffer.clear(); - for (i = 0; i < index_length; i++) + for (int i = 0; i < index_length; i++) { diff_buffer.push_back(base_heading_angle_buffer[index[i]] - heading_angle_buffer2[index[i]]); } @@ -186,14 +185,14 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity tmp_heading_angle = heading_angle_buffer2[index[index_length-1]] - avg; base_heading_angle_buffer2.clear(); - for (i = 0; i < heading_status->estimated_number ; i++) + for (int i = 0; i < heading_status->estimated_number ; i++) { base_heading_angle_buffer2.push_back(tmp_heading_angle - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[i]); } diff_buffer.clear(); - for (i = 0; i < index_length; i++) + for (int i = 0; i < index_length; i++) { diff_buffer.push_back(fabsf(base_heading_angle_buffer2[index[i]] - heading_angle_buffer2[index[i]])); } @@ -212,13 +211,13 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity index_length = std::distance(index.begin(), index.end()); - if (index_length < heading_status->estimated_number * heading_parameter.estimated_heading_coefficient) + if (index_length < heading_status->estimated_number * remain_data_ratio) { break; } } - if (index_length == 0 || index_length > heading_status->estimated_number * heading_parameter.estimated_heading_coefficient) + if (index_length == 0 || index_length > heading_status->estimated_number * remain_data_ratio) { if (index[index_length-1] == heading_status->estimated_number -1) { @@ -244,7 +243,7 @@ void heading_estimate(rtklib_msgs::RtklibNav rtklib_nav,sensor_msgs::Imu imu,geo double enu_vel[3]; double doppler_heading_angle = 0.0; - bool gnss_status,gnss_update; + bool gnss_status, gnss_update = true; ecef_vel[0] = rtklib_nav.ecef_vel.x; ecef_vel[1] = rtklib_nav.ecef_vel.y; @@ -262,17 +261,14 @@ void heading_estimate(rtklib_msgs::RtklibNav rtklib_nav,sensor_msgs::Imu imu,geo enu_vel[2] = 0; gnss_update = false; } - else{ - gnss_update = true; - } doppler_heading_angle = std::atan2(enu_vel[0], enu_vel[1]); - if(doppler_heading_angle<0){ - doppler_heading_angle = doppler_heading_angle + 2*M_PI; + if(doppler_heading_angle < 0){ + doppler_heading_angle = doppler_heading_angle + 2 * M_PI; } - if (heading_status->tow_last == rtklib_nav.tow || rtklib_nav.tow == 0 || gnss_update == false) + if (heading_status->tow_last == rtklib_nav.tow || rtklib_nav.tow == 0 || !gnss_update) { gnss_status = false; doppler_heading_angle = 0; @@ -291,6 +287,8 @@ void heading_estimate(rtklib_msgs::RtklibNav rtklib_nav,sensor_msgs::Imu imu,geo heading_estimate_(imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading); } +double _rmc_track = 0; + void heading_estimate(const nmea_msgs::Gprmc nmea_rmc,sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity, eagleye_msgs::YawrateOffset yawrate_offset_stop,eagleye_msgs::YawrateOffset yawrate_offset,eagleye_msgs::SlipAngle slip_angle, eagleye_msgs::Heading heading_interpolate,HeadingParameter heading_parameter, HeadingStatus* heading_status,eagleye_msgs::Heading* heading) @@ -298,6 +296,7 @@ void heading_estimate(const nmea_msgs::Gprmc nmea_rmc,sensor_msgs::Imu imu,geome bool gnss_status; double doppler_heading_angle = 0.0; + _rmc_track = nmea_rmc.track; if (heading_status->rmc_time_last == nmea_rmc.utc_seconds || nmea_rmc.utc_seconds == 0 || nmea_rmc.track == 0) { gnss_status = false; @@ -307,7 +306,7 @@ void heading_estimate(const nmea_msgs::Gprmc nmea_rmc,sensor_msgs::Imu imu,geome else { gnss_status = true; - doppler_heading_angle = nmea_rmc.track * M_PI/180; + doppler_heading_angle = nmea_rmc.track * M_PI / 180; heading_status->rmc_time_last = nmea_rmc.utc_seconds; } @@ -316,3 +315,34 @@ void heading_estimate(const nmea_msgs::Gprmc nmea_rmc,sensor_msgs::Imu imu,geome heading_estimate_(imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading); } + +void heading_estimate(const eagleye_msgs::Heading multi_antenna_heading,sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity, + eagleye_msgs::YawrateOffset yawrate_offset_stop,eagleye_msgs::YawrateOffset yawrate_offset,eagleye_msgs::SlipAngle slip_angle, + eagleye_msgs::Heading heading_interpolate,HeadingParameter heading_parameter, HeadingStatus* heading_status,eagleye_msgs::Heading* heading) +{ + bool gnss_status; + double heading_angle = 0.0; + + double multi_anttena_time = multi_antenna_heading.header.stamp.toSec(); + if (heading_status->ros_time_last == multi_anttena_time || multi_anttena_time == 0) + { + gnss_status = false; + heading_angle = 0; + heading_status->ros_time_last = multi_anttena_time; + } + else + { + gnss_status = true; + heading_angle = multi_antenna_heading.heading_angle; + heading_status->ros_time_last = multi_anttena_time; + } + + heading_status->heading_angle_buffer .push_back(heading_angle); + heading_status->gnss_status_buffer .push_back(gnss_status); + + heading_estimate_(imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading); + + if(!heading->status.estimate_status) heading->heading_angle = heading_angle; + if(gnss_status) heading->status.estimate_status = true; + +} diff --git a/eagleye_core/navigation/src/heading_interpolate.cpp b/eagleye_core/navigation/src/heading_interpolate.cpp old mode 100755 new mode 100644 index 30fb8990..08781a5b --- a/eagleye_core/navigation/src/heading_interpolate.cpp +++ b/eagleye_core/navigation/src/heading_interpolate.cpp @@ -40,45 +40,43 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg int estimate_index = 0; double yawrate = 0.0; double diff_estimate_heading_angle = 0.0; - bool heading_estimate_status; + bool heading_estimate_status = false; std::size_t imu_stamp_buffer_length; + double search_buffer_number = heading_interpolate_parameter.sync_search_period * heading_interpolate_parameter.imu_rate; + yawrate = imu.angular_velocity.z; - if (std::abs(velocity.twist.linear.x) > heading_interpolate_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity.twist.linear.x) > heading_interpolate_parameter.stop_judgment_threshold) { - yawrate = yawrate + yawrate_offset.yawrate_offset; + yawrate += yawrate_offset.yawrate_offset; } else { - yawrate = yawrate + yawrate_offset_stop.yawrate_offset; + yawrate += yawrate_offset_stop.yawrate_offset; } - if (heading_interpolate_status->number_buffer < heading_interpolate_parameter.number_buffer_max) + if (heading_interpolate_status->number_buffer < search_buffer_number) { ++heading_interpolate_status->number_buffer; } else { - heading_interpolate_status->number_buffer = heading_interpolate_parameter.number_buffer_max; + heading_interpolate_status->number_buffer = search_buffer_number; } - if (heading_interpolate_status->heading_stamp_last != heading.header.stamp.toSec() && heading.status.estimate_status == true) + if (heading_interpolate_status->heading_stamp_last != heading.header.stamp.toSec() && heading.status.estimate_status) { heading_estimate_status = true; heading_interpolate_status->heading_estimate_start_status = true; ++heading_interpolate_status->heading_estimate_status_count; } - else - { - heading_estimate_status = false; - } if(heading_interpolate_status->time_last != 0 && std::abs(velocity.twist.linear.x) > - heading_interpolate_parameter.stop_judgment_velocity_threshold) + heading_interpolate_parameter.stop_judgment_threshold) { - heading_interpolate_status->provisional_heading_angle = heading_interpolate_status->provisional_heading_angle + - (yawrate * (imu.header.stamp.toSec() - heading_interpolate_status->time_last)); + double dt = imu.header.stamp.toSec() - heading_interpolate_status->time_last; + heading_interpolate_status->provisional_heading_angle += yawrate * dt; } // data buffer generate @@ -86,15 +84,16 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg heading_interpolate_status->imu_stamp_buffer.push_back(imu.header.stamp.toSec()); imu_stamp_buffer_length = std::distance(heading_interpolate_status->imu_stamp_buffer.begin(), heading_interpolate_status->imu_stamp_buffer.end()); - if (imu_stamp_buffer_length > heading_interpolate_parameter.number_buffer_max) + if (imu_stamp_buffer_length > search_buffer_number) { heading_interpolate_status->provisional_heading_angle_buffer.erase(heading_interpolate_status->provisional_heading_angle_buffer .begin()); heading_interpolate_status->imu_stamp_buffer.erase(heading_interpolate_status->imu_stamp_buffer .begin()); } - if (heading_interpolate_status->heading_estimate_start_status == true) + if (heading_interpolate_status->heading_estimate_start_status) { - if (heading_estimate_status == true) + int estimate_index = 0; + if (heading_estimate_status) { for (estimate_index = heading_interpolate_status->number_buffer; estimate_index > 0; estimate_index--) { @@ -105,14 +104,13 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg } } - if (heading_estimate_status == true && estimate_index > 0 && heading_interpolate_status->number_buffer >= estimate_index && + if (heading_estimate_status && estimate_index > 0 && heading_interpolate_status->number_buffer >= estimate_index && heading_interpolate_status->heading_estimate_status_count > 1) { - diff_estimate_heading_angle = (heading_interpolate_status->provisional_heading_angle_buffer[estimate_index-1] - heading.heading_angle); - for (i = estimate_index; i <= heading_interpolate_status->number_buffer; i++) + double diff_estimate_heading_angle = (heading_interpolate_status->provisional_heading_angle_buffer[estimate_index-1] - heading.heading_angle); + for (int i = estimate_index; i <= heading_interpolate_status->number_buffer; i++) { - heading_interpolate_status->provisional_heading_angle_buffer[i-1] = heading_interpolate_status->provisional_heading_angle_buffer[i-1] - - diff_estimate_heading_angle; + heading_interpolate_status->provisional_heading_angle_buffer[i-1] -= diff_estimate_heading_angle; } heading_interpolate_status->provisional_heading_angle = heading_interpolate_status->provisional_heading_angle_buffer[heading_interpolate_status->number_buffer-1]; @@ -132,7 +130,7 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg } } - if (heading_interpolate_status->heading_estimate_start_status == true) + if (heading_interpolate_status->heading_estimate_start_status) { heading_interpolate->heading_angle = heading_interpolate_status->provisional_heading_angle + slip_angle.slip_angle; } diff --git a/eagleye_core/navigation/src/height.cpp b/eagleye_core/navigation/src/height.cpp index 4f6c9466..70b1c748 100755 --- a/eagleye_core/navigation/src/height.cpp +++ b/eagleye_core/navigation/src/height.cpp @@ -66,6 +66,10 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con int buffer_erase_count = 0; + double moving_average_buffer_number = height_parameter.moving_average_time * height_parameter.imu_rate; + double enabled_data_ratio = height_parameter.gnss_rate / height_parameter.imu_rate * height_parameter.gnss_receiving_threshold; + double remain_data_ratio = enabled_data_ratio * height_parameter.outlier_ratio_threshold; + /// GNSS FLAG /// if (height_status->gga_time_last == gga.header.stamp.toSec()) { @@ -96,7 +100,7 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con } /// buffering /// - if (distance.distance-height_status->distance_last >= height_parameter.separation_distance && gnss_status == true && gps_quality == 4) + if (distance.distance-height_status->distance_last >= height_parameter.update_distance && gnss_status == true && gps_quality == 4) { height_status->height_buffer.push_back(gnss_height); height_status->relative_height_G_buffer.push_back(height_status->relative_height_G); @@ -107,7 +111,7 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con height_status->distance_buffer.push_back(distance.distance); data_status = true; - if (height_status->distance_buffer[height_status->data_number-1] - height_status->distance_buffer[0] > height_parameter.estimated_distance_max) + if (height_status->distance_buffer[height_status->data_number-1] - height_status->distance_buffer[0] > height_parameter.estimated_maximum_interval) { height_status->height_buffer.erase(height_status->height_buffer.begin()); height_status->relative_height_G_buffer.erase(height_status->relative_height_G_buffer.begin()); @@ -121,7 +125,7 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con height_status->data_number = height_status->distance_buffer.size(); - if (height_status->distance_buffer[height_status->data_number-1]- height_status->distance_buffer[0] > height_parameter.estimated_distance) + if (height_status->distance_buffer[height_status->data_number-1]- height_status->distance_buffer[0] > height_parameter.estimated_minimum_interval) { height_status->estimate_start_status = true; } @@ -199,8 +203,8 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con /// height estimate /// if (height_status->estimate_start_status == true) { - if (distance.distance > height_parameter.estimated_distance && gnss_status == true && gps_quality ==4 && data_status == true && - velocity.twist.linear.x > height_parameter.estimated_velocity_threshold ) + if (distance.distance > height_parameter.estimated_minimum_interval && gnss_status == true && gps_quality ==4 && data_status == true && + velocity.twist.linear.x > height_parameter.moving_judgment_threshold ) { height_status->correction_relative_height_buffer2.clear(); height_status->height_buffer2.clear(); @@ -216,11 +220,11 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con for (i = 0; i < height_status->data_number; i++) { - if (height_status->distance_buffer[height_status->data_number-1] - height_status->distance_buffer[i] <= height_parameter.estimated_distance) + if (height_status->distance_buffer[height_status->data_number-1] - height_status->distance_buffer[i] <= height_parameter.estimated_minimum_interval) { distance_index.push_back(i); - if (height_status->correction_velocity_buffer[i] > height_parameter.estimated_velocity_threshold) + if (height_status->correction_velocity_buffer[i] > height_parameter.moving_judgment_threshold) { velocity_index.push_back(i); } @@ -239,9 +243,7 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con std::vector diff_height_buffer2; std::vector erase_number; - - - if (index_length > velocity_index_length * height_parameter.estimated_velocity_coefficient) + if (index_length > velocity_index_length * enabled_data_ratio) { while (1) @@ -312,7 +314,7 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con index_length = std::distance(index.begin(), index.end()); velocity_index_length = std::distance(velocity_index.begin(), velocity_index.end()); - if (index_length < velocity_index_length * height_parameter.estimated_height_coefficient) + if (index_length < velocity_index_length * remain_data_ratio) { break; } @@ -340,7 +342,7 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con index_length = std::distance(index.begin(), index.end()); velocity_index_length = std::distance(velocity_index.begin(), velocity_index.end()); - if (index_length >= velocity_index_length * height_parameter.estimated_height_coefficient) + if (index_length >= velocity_index_length * remain_data_ratio) { if (index[index_length - 1] == height_status->data_number-1) { @@ -375,13 +377,13 @@ void pitching_estimate(const sensor_msgs::Imu imu,const nmea_msgs::Gpgga gga,con height_status->acc_buffer.push_back((velocity.twist.linear.x-height_status->correction_velocity_x_last)/(imu.header.stamp.toSec()-height_status->time_last) - correction_acceleration_linear_x); data_num_acc = height_status->acc_buffer.size(); - if (data_num_acc > height_parameter.average_num) + if (data_num_acc > moving_average_buffer_number) { height_status->acc_buffer.erase(height_status->acc_buffer.begin()); data_num_acc--; } -if (data_num_acc >= height_parameter.average_num && height_status->estimate_start_status == true) +if (data_num_acc >= moving_average_buffer_number && height_status->estimate_start_status == true) { sum_acc = 0; for (i = 0; i < data_num_acc; i++) diff --git a/eagleye_core/navigation/src/position.cpp b/eagleye_core/navigation/src/position.cpp index 8b1c24b6..bbe338ef 100755 --- a/eagleye_core/navigation/src/position.cpp +++ b/eagleye_core/navigation/src/position.cpp @@ -36,7 +36,7 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu PositionStatus* position_status, eagleye_msgs::Position* enu_absolute_pos) { int i; - int estimated_number_max = position_parameter.estimated_distance/position_parameter.separation_distance; + int estimated_number_max = position_parameter.estimated_interval/position_parameter.update_distance; int max_x_index, max_y_index; double enu_pos[3]; double avg_x, avg_y, avg_z; @@ -50,6 +50,9 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu std::vector diff_x_buffer, diff_y_buffer, diff_z_buffer; std::vector::iterator max_x, max_y; + double enabled_data_ratio = position_parameter.gnss_rate / position_parameter.imu_rate * position_parameter.gnss_receiving_threshold; + double remain_data_ratio = enabled_data_ratio * position_parameter.outlier_ratio_threshold; + enu_pos[0] = position_status->enu_pos[0]; enu_pos[1] = position_status->enu_pos[1]; enu_pos[2] = position_status->enu_pos[2]; @@ -104,7 +107,7 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu } - if (distance.distance-position_status->distance_last >= position_parameter.separation_distance && gnss_status == true && + if (distance.distance-position_status->distance_last >= position_parameter.update_distance && gnss_status == true && position_status->heading_estimate_status_count > 0) { @@ -147,8 +150,8 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu if (data_status == true) { - if (distance.distance > position_parameter.estimated_distance && gnss_status == true && - velocity.twist.linear.x > position_parameter.estimated_velocity_threshold && position_status->heading_estimate_status_count > 0) + if (distance.distance > position_parameter.estimated_interval && gnss_status == true && + velocity.twist.linear.x > position_parameter.moving_judgment_threshold && position_status->heading_estimate_status_count > 0) { std::vector distance_index; std::vector velocity_index; @@ -156,11 +159,11 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu for (i = 0; i < position_status->estimated_number; i++) { - if (position_status->distance_buffer[position_status->estimated_number-1] - position_status->distance_buffer[i] <= position_parameter.estimated_distance) + if (position_status->distance_buffer[position_status->estimated_number-1] - position_status->distance_buffer[i] <= position_parameter.estimated_interval) { distance_index.push_back(i); - if (position_status->correction_velocity_buffer[i] > position_parameter.estimated_velocity_threshold) + if (position_status->correction_velocity_buffer[i] > position_parameter.moving_judgment_threshold) { velocity_index.push_back(i); } @@ -174,7 +177,7 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu index_length = std::distance(index.begin(), index.end()); velocity_index_length = std::distance(velocity_index.begin(), velocity_index.end()); - if (index_length > velocity_index_length * position_parameter.estimated_enu_vel_coefficient) + if (index_length > velocity_index_length * enabled_data_ratio) { while (1) @@ -271,7 +274,7 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu index_length = std::distance(index.begin(), index.end()); velocity_index_length = std::distance(velocity_index.begin(), velocity_index.end()); - if (index_length < velocity_index_length * position_parameter.estimated_position_coefficient) + if (index_length < velocity_index_length * remain_data_ratio) { break; } @@ -281,7 +284,7 @@ void position_estimate_(geometry_msgs::TwistStamped velocity,eagleye_msgs::Statu index_length = std::distance(index.begin(), index.end()); velocity_index_length = std::distance(velocity_index.begin(), velocity_index.end()); - if (index_length >= velocity_index_length * position_parameter.estimated_position_coefficient) + if (index_length >= velocity_index_length * remain_data_ratio) { if (index[index_length - 1] == position_status->estimated_number-1) { diff --git a/eagleye_core/navigation/src/position_interpolate.cpp b/eagleye_core/navigation/src/position_interpolate.cpp index a2748feb..1f645e66 100755 --- a/eagleye_core/navigation/src/position_interpolate.cpp +++ b/eagleye_core/navigation/src/position_interpolate.cpp @@ -48,15 +48,17 @@ void position_interpolate_estimate(eagleye_msgs::Position enu_absolute_pos, geom bool position_estimate_status; std::size_t imu_stamp_buffer_length; + double search_buffer_number = position_interpolate_parameter.sync_search_period * position_interpolate_parameter.imu_rate; + enu_absolute_pos_interpolate->ecef_base_pos = enu_absolute_pos.ecef_base_pos; - if (position_interpolate_status->number_buffer < position_interpolate_parameter.number_buffer_max) + if (position_interpolate_status->number_buffer < search_buffer_number) { ++position_interpolate_status->number_buffer; } else { - position_interpolate_status->number_buffer = position_interpolate_parameter.number_buffer_max; + position_interpolate_status->number_buffer = search_buffer_number; } if (position_interpolate_status->position_stamp_last != enu_absolute_pos.header.stamp.toSec() && enu_absolute_pos.status.estimate_status == true) @@ -71,7 +73,7 @@ void position_interpolate_estimate(eagleye_msgs::Position enu_absolute_pos, geom } if(position_interpolate_status->time_last != 0 && std::sqrt((enu_vel.vector.x * enu_vel.vector.x) + (enu_vel.vector.y * enu_vel.vector.y) + - (enu_vel.vector.z * enu_vel.vector.z)) > position_interpolate_parameter.stop_judgment_velocity_threshold) + (enu_vel.vector.z * enu_vel.vector.z)) > position_interpolate_parameter.stop_judgment_threshold) { position_interpolate_status->provisional_enu_pos_x = enu_absolute_pos_interpolate->enu_pos.x + enu_vel.vector.x * (enu_vel.header.stamp.toSec() - position_interpolate_status->time_last); @@ -88,7 +90,7 @@ void position_interpolate_estimate(eagleye_msgs::Position enu_absolute_pos, geom position_interpolate_status->imu_stamp_buffer.push_back(enu_vel.header.stamp.toSec()); imu_stamp_buffer_length = std::distance(position_interpolate_status->imu_stamp_buffer.begin(), position_interpolate_status->imu_stamp_buffer.end()); - if (imu_stamp_buffer_length > position_interpolate_parameter.number_buffer_max) + if (imu_stamp_buffer_length > search_buffer_number) { position_interpolate_status->provisional_enu_pos_x_buffer.erase(position_interpolate_status->provisional_enu_pos_x_buffer .begin()); position_interpolate_status->provisional_enu_pos_y_buffer.erase(position_interpolate_status->provisional_enu_pos_y_buffer .begin()); diff --git a/eagleye_core/navigation/src/rolling.cpp b/eagleye_core/navigation/src/rolling.cpp index 8f4e96d3..693a40b2 100755 --- a/eagleye_core/navigation/src/rolling.cpp +++ b/eagleye_core/navigation/src/rolling.cpp @@ -53,7 +53,7 @@ void rolling_estimate(sensor_msgs::Imu imu, geometry_msgs::TwistStamped correcti acceleration_y = imu.linear_acceleration.y; velocity = correction_velocity.twist.linear.x; - if (std::abs(velocity) > rolling_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity) > rolling_parameter.stop_judgment_threshold) { yawrate = imu.angular_velocity.z + yawrate_offset.yawrate_offset; } diff --git a/eagleye_core/navigation/src/rtk_deadreckoning.cpp b/eagleye_core/navigation/src/rtk_deadreckoning.cpp index ef1fa847..db11f729 100755 --- a/eagleye_core/navigation/src/rtk_deadreckoning.cpp +++ b/eagleye_core/navigation/src/rtk_deadreckoning.cpp @@ -67,13 +67,15 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg q.setRPY(0, 0, (90* M_PI / 180)-heading.heading_angle); transform.setRotation(q); - tf::Transform transform2; - tf::Quaternion q2(rtk_deadreckoning_parameter.tf_gnss_rotation_x,rtk_deadreckoning_parameter.tf_gnss_rotation_y,rtk_deadreckoning_parameter.tf_gnss_rotation_z,rtk_deadreckoning_parameter.tf_gnss_rotation_w); - transform2.setOrigin(transform*tf::Vector3(-rtk_deadreckoning_parameter.tf_gnss_translation_x, -rtk_deadreckoning_parameter.tf_gnss_translation_y,-rtk_deadreckoning_parameter.tf_gnss_translation_z)); - transform2.setRotation(transform*q2); - + tf::Transform transform2, transform3; + tf::Quaternion q2(rtk_deadreckoning_parameter.tf_gnss_rotation_x,rtk_deadreckoning_parameter.tf_gnss_rotation_y, + rtk_deadreckoning_parameter.tf_gnss_rotation_z,rtk_deadreckoning_parameter.tf_gnss_rotation_w); + transform2.setOrigin(tf::Vector3(rtk_deadreckoning_parameter.tf_gnss_translation_x, rtk_deadreckoning_parameter.tf_gnss_translation_y, + rtk_deadreckoning_parameter.tf_gnss_translation_z)); + transform2.setRotation(q2); + transform3 = transform * transform2.inverse(); tf::Vector3 tmp_pos; - tmp_pos = transform2.getOrigin(); + tmp_pos = transform3.getOrigin(); enu_rtk[0] = tmp_pos.getX(); enu_rtk[1] = tmp_pos.getY(); @@ -87,7 +89,7 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg enu_absolute_rtk_deadreckoning->status.enabled_status = true; enu_absolute_rtk_deadreckoning->status.estimate_status = true; } - else if(rtk_deadreckoning_status->time_last != 0 && sqrt((enu_vel.vector.x * enu_vel.vector.x) + (enu_vel.vector.y * enu_vel.vector.y) + (enu_vel.vector.z * enu_vel.vector.z)) > rtk_deadreckoning_parameter.stop_judgment_velocity_threshold) + else if(rtk_deadreckoning_status->time_last != 0 && sqrt((enu_vel.vector.x * enu_vel.vector.x) + (enu_vel.vector.y * enu_vel.vector.y) + (enu_vel.vector.z * enu_vel.vector.z)) > rtk_deadreckoning_parameter.stop_judgment_threshold) { rtk_deadreckoning_status->provisional_enu_pos_x = enu_absolute_rtk_deadreckoning->enu_pos.x + enu_vel.vector.x * (enu_vel.header.stamp.toSec() - rtk_deadreckoning_status->time_last); rtk_deadreckoning_status->provisional_enu_pos_y = enu_absolute_rtk_deadreckoning->enu_pos.y + enu_vel.vector.y * (enu_vel.header.stamp.toSec() - rtk_deadreckoning_status->time_last); diff --git a/eagleye_core/navigation/src/rtk_heading.cpp b/eagleye_core/navigation/src/rtk_heading.cpp index fb77dbfc..539506f5 100755 --- a/eagleye_core/navigation/src/rtk_heading.cpp +++ b/eagleye_core/navigation/src/rtk_heading.cpp @@ -39,6 +39,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg int i,index_max; double yawrate = 0.0 , rtk_heading_angle = 0.0; double avg = 0.0,tmp_heading_angle; + double estimated_heading_buffer = 2; bool gnss_status; std::size_t index_length; std::size_t time_buffer_length; @@ -46,13 +47,18 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg std::size_t inversion_down_index_length; std::vector::iterator max; - if (heading_status->estimated_number < heading_parameter.estimated_number_max) + double estimated_buffer_number_min = heading_parameter.estimated_minimum_interval * heading_parameter.imu_rate; + double estimated_buffer_number_max = heading_parameter.estimated_maximum_interval * heading_parameter.imu_rate; + double enabled_data_ratio = heading_parameter.gnss_rate / heading_parameter.imu_rate * heading_parameter.gnss_receiving_threshold; + double remain_data_ratio = enabled_data_ratio * heading_parameter.outlier_ratio_threshold; + + if (heading_status->estimated_number < estimated_buffer_number_max) { ++heading_status->estimated_number ; } else { - heading_status->estimated_number = heading_parameter.estimated_number_max; + heading_status->estimated_number = estimated_buffer_number_max; } yawrate = imu.angular_velocity.z; @@ -71,9 +77,9 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg int distance_length; distance_length = std::distance(heading_status->distance_buffer.begin(), heading_status->distance_buffer.end()); - while (heading_status->distance_buffer[distance_length-1] - heading_status->distance_buffer[0] > heading_parameter.estimated_distance) + while (heading_status->distance_buffer[distance_length-1] - heading_status->distance_buffer[0] > heading_parameter.update_distance) { - if (distance_length <= heading_parameter.estimated_heading_buffer_min) + if (distance_length <= estimated_heading_buffer) { break; } @@ -92,7 +98,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg gga_length = std::distance(heading_status->gga_status_buffer.begin(), heading_status->gga_status_buffer.end()); if (heading_status->gga_status_buffer[0] == 4 && heading_status->gga_status_buffer[gga_length-1] == 4 && - abs(yawrate) < heading_parameter.estimated_yawrate_threshold) + abs(yawrate) < heading_parameter.curve_judgment_threshold) { tmp_llh[0] = heading_status->latitude_buffer[0] *M_PI/180; tmp_llh[1] = heading_status->longitude_buffer[0]*M_PI/180; @@ -116,7 +122,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg if (heading_status->tow_last == gga.header.stamp.toSec() || gga.header.stamp.toSec() == 0 || rtk_heading_angle == 0 || heading_status->last_rtk_heading_angle == rtk_heading_angle || - velocity.twist.linear.x < heading_parameter.stop_judgment_velocity_threshold) + velocity.twist.linear.x < heading_parameter.stop_judgment_threshold) { gnss_status = false; rtk_heading_angle = 0; @@ -142,7 +148,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg time_buffer_length = std::distance(heading_status->time_buffer .begin(), heading_status->time_buffer .end()); - if (time_buffer_length > heading_parameter.estimated_number_max) + if (time_buffer_length > estimated_buffer_number_max) { heading_status->time_buffer .erase(heading_status->time_buffer .begin()); heading_status->heading_angle_buffer .erase(heading_status->heading_angle_buffer .begin()); @@ -158,10 +164,10 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg std::vector velocity_index; std::vector index; - if (heading_status->estimated_number > heading_parameter.estimated_number_min && + if (heading_status->estimated_number > estimated_buffer_number_min && heading_status->gnss_status_buffer [heading_status->estimated_number -1] == true && - heading_status->correction_velocity_buffer [heading_status->estimated_number -1] > heading_parameter.estimated_velocity_threshold && - fabsf(heading_status->yawrate_buffer [heading_status->estimated_number -1]) < heading_parameter.estimated_yawrate_threshold) + heading_status->correction_velocity_buffer [heading_status->estimated_number -1] > heading_parameter.slow_judgment_threshold && + fabsf(heading_status->yawrate_buffer [heading_status->estimated_number -1]) < heading_parameter.curve_judgment_threshold) { heading->status.enabled_status = true; } @@ -178,7 +184,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg { gnss_index.push_back(i); } - if (heading_status->correction_velocity_buffer [i] > heading_parameter.estimated_velocity_threshold) + if (heading_status->correction_velocity_buffer [i] > heading_parameter.slow_judgment_threshold) { velocity_index.push_back(i); } @@ -189,7 +195,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg index_length = std::distance(index.begin(), index.end()); - if (index_length > heading_status->estimated_number * heading_parameter.estimated_gnss_coefficient) + if (index_length > heading_status->estimated_number * enabled_data_ratio) { std::vector provisional_heading_angle_buffer(heading_status->estimated_number , 0); @@ -197,7 +203,7 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg { if (i > 0) { - if (std::abs(heading_status->correction_velocity_buffer [heading_status->estimated_number -1]) > heading_parameter.stop_judgment_velocity_threshold) + if (std::abs(heading_status->correction_velocity_buffer [heading_status->estimated_number -1]) > heading_parameter.stop_judgment_threshold) { provisional_heading_angle_buffer[i] = provisional_heading_angle_buffer[i-1] + ((heading_status->yawrate_buffer [i] + heading_status->yawrate_offset_buffer [i]) * @@ -288,13 +294,13 @@ void rtk_heading_estimate(nmea_msgs::Gpgga gga,sensor_msgs::Imu imu,geometry_msg index_length = std::distance(index.begin(), index.end()); - if (index_length < heading_status->estimated_number * heading_parameter.estimated_heading_coefficient) + if (index_length < heading_status->estimated_number * remain_data_ratio) { break; } } - if (index_length == 0 || index_length > heading_status->estimated_number * heading_parameter.estimated_heading_coefficient) + if (index_length == 0 || index_length > heading_status->estimated_number * remain_data_ratio) { if (index[index_length-1] == heading_status->estimated_number -1) { diff --git a/eagleye_core/navigation/src/slip_angle.cpp b/eagleye_core/navigation/src/slip_angle.cpp index 1c1ac3ce..df7a496a 100755 --- a/eagleye_core/navigation/src/slip_angle.cpp +++ b/eagleye_core/navigation/src/slip_angle.cpp @@ -43,7 +43,7 @@ void slip_angle_estimate(sensor_msgs::Imu imu, geometry_msgs::TwistStamped veloc yawrate = imu.angular_velocity.z; - if (std::abs(velocity.twist.linear.x) > slip_angle_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity.twist.linear.x) > slip_angle_parameter.stop_judgment_threshold) { yawrate = yawrate + yawrate_offset_2nd.yawrate_offset; } diff --git a/eagleye_core/navigation/src/slip_coefficient.cpp b/eagleye_core/navigation/src/slip_coefficient.cpp index d405b8d8..e1bb89f8 100755 --- a/eagleye_core/navigation/src/slip_coefficient.cpp +++ b/eagleye_core/navigation/src/slip_coefficient.cpp @@ -35,7 +35,6 @@ void slip_coefficient_estimate(sensor_msgs::Imu imu,rtklib_msgs::RtklibNav rtkli eagleye_msgs::YawrateOffset yawrate_offset_stop,eagleye_msgs::YawrateOffset yawrate_offset_2nd,eagleye_msgs::Heading heading_interpolate_3rd, SlipCoefficientParameter slip_coefficient_parameter,SlipCoefficientStatus* slip_coefficient_status,double* estimate_coefficient) { - int i; double doppler_heading_angle; double doppler_slip; @@ -48,6 +47,9 @@ void slip_coefficient_estimate(sensor_msgs::Imu imu,rtklib_msgs::RtklibNav rtkli double enu_vel[3]; std::size_t acceleration_y_buffer_length; + double estimated_buffer_number_min = slip_coefficient_parameter.estimated_minimum_interval * slip_coefficient_parameter.imu_rate; + double estimated_buffer_number_max = slip_coefficient_parameter.estimated_maximum_interval * slip_coefficient_parameter.imu_rate; + ecef_vel[0] = rtklib_nav.ecef_vel.x; ecef_vel[1] = rtklib_nav.ecef_vel.y; ecef_vel[2] = rtklib_nav.ecef_vel.z; @@ -72,7 +74,7 @@ void slip_coefficient_estimate(sensor_msgs::Imu imu,rtklib_msgs::RtklibNav rtkli yawrate = imu.angular_velocity.z; - if (std::abs(velocity.twist.linear.x) > slip_coefficient_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity.twist.linear.x) > slip_coefficient_parameter.stop_judgment_threshold) { yawrate = yawrate + yawrate_offset_2nd.yawrate_offset; } @@ -85,8 +87,8 @@ void slip_coefficient_estimate(sensor_msgs::Imu imu,rtklib_msgs::RtklibNav rtkli if (heading_interpolate_3rd.status.estimate_status == true) { - if ((velocity.twist.linear.x > slip_coefficient_parameter.estimated_velocity_threshold) && - (fabs(yawrate) > slip_coefficient_parameter.estimated_yawrate_threshold)) + if ((velocity.twist.linear.x > slip_coefficient_parameter.moving_judgment_threshold) && + (fabs(yawrate) > slip_coefficient_parameter.curve_judgment_threshold)) { double imu_heading; @@ -107,22 +109,22 @@ void slip_coefficient_estimate(sensor_msgs::Imu imu,rtklib_msgs::RtklibNav rtkli acceleration_y_buffer_length = std::distance(slip_coefficient_status->acceleration_y_buffer.begin(), slip_coefficient_status->acceleration_y_buffer.end()); - if (acceleration_y_buffer_length > slip_coefficient_parameter.estimated_number_max) + if (acceleration_y_buffer_length > estimated_buffer_number_max) { slip_coefficient_status->acceleration_y_buffer.erase(slip_coefficient_status->acceleration_y_buffer.begin()); slip_coefficient_status->doppler_slip_buffer.erase(slip_coefficient_status->doppler_slip_buffer.begin()); } - if(slip_coefficient_status->heading_estimate_status_count < slip_coefficient_parameter.estimated_number_max) + if(slip_coefficient_status->heading_estimate_status_count < estimated_buffer_number_max) { ++slip_coefficient_status->heading_estimate_status_count; } else { - slip_coefficient_status->heading_estimate_status_count = slip_coefficient_parameter.estimated_number_max; + slip_coefficient_status->heading_estimate_status_count = estimated_buffer_number_max; } - if(slip_coefficient_status->heading_estimate_status_count > slip_coefficient_parameter.estimated_number_min) + if(slip_coefficient_status->heading_estimate_status_count > estimated_buffer_number_min) { double sum_xy_avg,sum_x_square = 0.0; acceleration_y_buffer_length = std::distance(slip_coefficient_status->acceleration_y_buffer.begin(), slip_coefficient_status->acceleration_y_buffer.end()); diff --git a/eagleye_core/navigation/src/smoothing.cpp b/eagleye_core/navigation/src/smoothing.cpp index 850513d7..3b72d151 100755 --- a/eagleye_core/navigation/src/smoothing.cpp +++ b/eagleye_core/navigation/src/smoothing.cpp @@ -48,6 +48,7 @@ void smoothing_estimate(rtklib_msgs::RtklibNav rtklib_nav, geometry_msgs::TwistS std::vector velocity_index; std::vector index; + double estimated_buffer_number = smoothing_parameter.gnss_rate * smoothing_parameter.moving_average_time; if(gnss_smooth_pos_enu->ecef_base_pos.x == 0 && gnss_smooth_pos_enu->ecef_base_pos.y == 0 && gnss_smooth_pos_enu->ecef_base_pos.z == 0) { @@ -94,7 +95,7 @@ void smoothing_estimate(rtklib_msgs::RtklibNav rtklib_nav, geometry_msgs::TwistS time_buffer_length = std::distance(smoothing_status->time_buffer.begin(), smoothing_status->time_buffer.end()); - if (time_buffer_length > smoothing_parameter.estimated_number_max) + if (time_buffer_length > estimated_buffer_number) { smoothing_status->time_buffer.erase(smoothing_status->time_buffer.begin()); smoothing_status->enu_pos_x_buffer.erase(smoothing_status->enu_pos_x_buffer.begin()); @@ -103,22 +104,22 @@ void smoothing_estimate(rtklib_msgs::RtklibNav rtklib_nav, geometry_msgs::TwistS smoothing_status->correction_velocity_buffer.erase(smoothing_status->correction_velocity_buffer.begin()); } - if (smoothing_status->estimated_number < smoothing_parameter.estimated_number_max) + if (smoothing_status->estimated_number < estimated_buffer_number) { ++smoothing_status->estimated_number; gnss_smooth_pos_enu->status.enabled_status = false; } else { - smoothing_status->estimated_number = smoothing_parameter.estimated_number_max; + smoothing_status->estimated_number = estimated_buffer_number; gnss_smooth_pos_enu->status.enabled_status = true; } - if (smoothing_status->estimated_number == smoothing_parameter.estimated_number_max){ + if (smoothing_status->estimated_number == estimated_buffer_number){ for (i = 0; i < smoothing_status->estimated_number; i++) { index.push_back(i); - if (smoothing_status->correction_velocity_buffer[i] > smoothing_parameter.estimated_velocity_threshold) + if (smoothing_status->correction_velocity_buffer[i] > smoothing_parameter.moving_judgment_threshold) { velocity_index.push_back(i); } @@ -134,7 +135,7 @@ void smoothing_estimate(rtklib_msgs::RtklibNav rtklib_nav, geometry_msgs::TwistS sum_gnss_pos[2] = sum_gnss_pos[2] + smoothing_status->enu_pos_z_buffer[velocity_index[i]]; } - if (velocity_index_length > index_length * smoothing_parameter.estimated_threshold) + if (velocity_index_length > index_length * smoothing_parameter.moving_ratio_threshold) { gnss_smooth_pos[0] = sum_gnss_pos[0]/velocity_index_length; gnss_smooth_pos[1] = sum_gnss_pos[1]/velocity_index_length; diff --git a/eagleye_core/navigation/src/trajectory.cpp b/eagleye_core/navigation/src/trajectory.cpp index 5bb99f41..b442dd7e 100755 --- a/eagleye_core/navigation/src/trajectory.cpp +++ b/eagleye_core/navigation/src/trajectory.cpp @@ -38,7 +38,7 @@ void trajectory_estimate(const sensor_msgs::Imu imu, const geometry_msgs::TwistS eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist) { - if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_velocity_threshold && + if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold && yawrate_offset_2nd.status.enabled_status == true) { // Inverted because the coordinate system is reversed @@ -72,7 +72,7 @@ void trajectory_estimate(const sensor_msgs::Imu imu, const geometry_msgs::TwistS if (trajectory_status->estimate_status_count == 2 && std::abs(velocity.twist.linear.x) > 0 && trajectory_status->time_last != 0) { - if(std::abs(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) < trajectory_parameter.stop_judgment_yawrate_threshold) + if(std::abs(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) < trajectory_parameter.curve_judgment_threshold) { enu_relative_pos->enu_pos.x = enu_relative_pos->enu_pos.x + enu_vel->vector.x * (imu.header.stamp.toSec() - trajectory_status->time_last); enu_relative_pos->enu_pos.y = enu_relative_pos->enu_pos.y + enu_vel->vector.y * (imu.header.stamp.toSec() - trajectory_status->time_last); @@ -110,7 +110,7 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis geometry_msgs::Vector3Stamped* enu_vel, eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist) { - if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_velocity_threshold && + if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold && yawrate_offset_2nd.status.enabled_status == true) { //Inverted because the coordinate system is reversed @@ -144,7 +144,7 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis if (trajectory_status->estimate_status_count == 2 && std::abs(velocity.twist.linear.x) > 0 && trajectory_status->time_last != 0) { - if(std::abs(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) < trajectory_parameter.stop_judgment_yawrate_threshold) + if(std::abs(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) < trajectory_parameter.curve_judgment_threshold) { enu_relative_pos->enu_pos.x = enu_relative_pos->enu_pos.x + enu_vel->vector.x * (imu.header.stamp.toSec() - trajectory_status->time_last); enu_relative_pos->enu_pos.y = enu_relative_pos->enu_pos.y + enu_vel->vector.y * (imu.header.stamp.toSec() - trajectory_status->time_last); diff --git a/eagleye_core/navigation/src/velocity_estimator.cpp b/eagleye_core/navigation/src/velocity_estimator.cpp index acf99c63..e8b37534 100644 --- a/eagleye_core/navigation/src/velocity_estimator.cpp +++ b/eagleye_core/navigation/src/velocity_estimator.cpp @@ -47,8 +47,11 @@ void VelocityEstimator::PitchrateOffsetStopEstimator::setParam(std::string yaml_ try { YAML::Node conf = YAML::LoadFile(yaml_file); - param.buffer_count_max = conf["velocity_estimator"]["pitchrate_offset"]["buffer_count_max"].as(); - // std::cout<< "buffer_count_max "<(); + param.estimated_interval = conf["velocity_estimator"]["pitchrate_offset"]["estimated_interval"].as(); + param.buffer_count_max = param.imu_rate * param.estimated_interval; + // std::cout<< "imu_rate "<(); + + param.imu_rate = conf["common"]["imu_rate"].as(); + param.gnss_rate = conf["common"]["gnss_rate"].as(); + param.estimated_interval = conf["velocity_estimator"]["pitching"]["estimated_interval"].as(); + param.buffer_max = param.imu_rate * param.estimated_interval; param.outlier_threshold = conf["velocity_estimator"]["pitching"]["outlier_threshold"].as(); - param.estimated_velocity_threshold = conf["velocity_estimator"]["pitching"]["estimated_velocity_threshold"].as(); - param.estimated_gnss_coefficient = conf["velocity_estimator"]["pitching"]["estimated_gnss_coefficient"].as(); - param.estimated_coefficient = conf["velocity_estimator"]["pitching"]["estimated_coefficient"].as(); + param.slow_judgment_threshold = conf["common"]["slow_judgment_threshold"].as(); + param.gnss_receiving_threshold = conf["velocity_estimator"]["pitching"]["gnss_receiving_threshold"].as(); + param.estimated_gnss_coefficient = param.gnss_rate/param.imu_rate * param.gnss_receiving_threshold; + param.outlier_ratio_threshold = conf["velocity_estimator"]["pitching"]["outlier_ratio_threshold"].as(); + param.estimated_coefficient = param.estimated_gnss_coefficient * param.outlier_ratio_threshold; + + // std::cout<< "imu_rate "< param.estimated_velocity_threshold || - rtkfix_velocity > param.estimated_velocity_threshold) + if (doppler_velocity > param.slow_judgment_threshold || + rtkfix_velocity > param.slow_judgment_threshold) { use_gnss_status = true; } @@ -231,12 +247,20 @@ void VelocityEstimator::AccelerationOffsetEstimator::setParam(std::string yaml_f try { YAML::Node conf = YAML::LoadFile(yaml_file); - param.buffer_min = conf["velocity_estimator"]["acceleration_offset"]["buffer_min"].as(); - param.buffer_max = conf["velocity_estimator"]["acceleration_offset"]["buffer_max"].as(); + + param.imu_rate = conf["common"]["imu_rate"].as(); + param.gnss_rate = conf["common"]["gnss_rate"].as(); + param.estimated_minimum_interval = conf["velocity_estimator"]["acceleration_offset"]["estimated_minimum_interval"].as(); + param.estimated_maximum_interval = conf["velocity_estimator"]["acceleration_offset"]["estimated_maximum_interval"].as(); + param.buffer_min = param.imu_rate * param.estimated_minimum_interval; + param.buffer_max = param.imu_rate * param.estimated_maximum_interval; param.filter_process_noise = conf["velocity_estimator"]["acceleration_offset"]["filter_process_noise"].as(); param.filter_observation_noise = conf["velocity_estimator"]["acceleration_offset"]["filter_observation_noise"].as(); - // std::cout<< "buffer_min "<(); param.use_ecef_base_position = conf["ecef_base_pos"]["use_ecef_base_position"].as(); + param.imu_rate = conf["common"]["imu_rate"].as(); + param.gnss_rate = conf["common"]["gnss_rate"].as(); + param.gga_downsample_time = conf["velocity_estimator"]["gga_downsample_time"].as(); param.stop_judgment_velocity_threshold = conf["velocity_estimator"]["stop_judgment_velocity_threshold"].as(); - param.stop_judgment_buffer_maxnum = conf["velocity_estimator"]["stop_judgment_buffer_maxnum"].as(); + param.stop_judgment_interval = conf["velocity_estimator"]["stop_judgment_interval"].as(); + param.stop_judgment_buffer_maxnum = param.stop_judgment_buffer_maxnum = param.imu_rate * param.stop_judgment_interval; param.variance_threshold = conf["velocity_estimator"]["variance_threshold"].as(); // doppler fusion parameter - param.buffer_max = conf["velocity_estimator"]["doppler_fusion"]["buffer_max"].as(); - param.estimated_gnss_coefficient = conf["velocity_estimator"]["doppler_fusion"]["estimated_gnss_coefficient"].as(); - param.estimated_coefficient = conf["velocity_estimator"]["doppler_fusion"]["estimated_coefficient"].as(); + param.estimated_interval = conf["velocity_estimator"]["doppler_fusion"]["estimated_interval"].as(); + param.buffer_max = param.imu_rate * param.estimated_interval; + param.gnss_receiving_threshold = conf["velocity_estimator"]["doppler_fusion"]["gnss_receiving_threshold"].as(); + param.estimated_gnss_coefficient = param.gnss_rate/param.imu_rate * param.gnss_receiving_threshold; + param.outlier_ratio_threshold = conf["velocity_estimator"]["doppler_fusion"]["outlier_ratio_threshold"].as(); + param.estimated_coefficient = param.estimated_gnss_coefficient * param.outlier_ratio_threshold; param.outlier_threshold = conf["velocity_estimator"]["doppler_fusion"]["outlier_threshold"].as(); + // std::cout<< "imu_rate "<status.enabled_status) @@ -54,11 +58,11 @@ void velocity_scale_factor_estimate_(const geometry_msgs::TwistStamped velocity, if(velocity_scale_factor->status.enabled_status == true) { - estimated_number_cur = velocity_scale_factor_parameter.estimated_number_max; + estimated_number_cur = estimated_buffer_number_max; } else { - estimated_number_cur = velocity_scale_factor_parameter.estimated_number_min; + estimated_number_cur = estimated_buffer_number_min; } if (velocity_scale_factor_status->estimated_number < estimated_number_cur) @@ -84,10 +88,10 @@ void velocity_scale_factor_estimate_(const geometry_msgs::TwistStamped velocity, std::vector index; std::vector velocity_scale_factor_buffer; - if (velocity_scale_factor_status->estimated_number >= velocity_scale_factor_parameter.estimated_number_min && + 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.estimated_velocity_threshold) + velocity_scale_factor_parameter.moving_judgment_threshold) { for (i = 0; i < velocity_scale_factor_status->estimated_number; i++) { @@ -95,7 +99,7 @@ void velocity_scale_factor_estimate_(const geometry_msgs::TwistStamped velocity, { gnss_index.push_back(i); } - if (velocity_scale_factor_status->velocity_buffer[i] > velocity_scale_factor_parameter.estimated_velocity_threshold) + if (velocity_scale_factor_status->velocity_buffer[i] > velocity_scale_factor_parameter.moving_judgment_threshold) { velocity_index.push_back(i); } @@ -106,7 +110,7 @@ void velocity_scale_factor_estimate_(const geometry_msgs::TwistStamped velocity, index_length = std::distance(index.begin(), index.end()); - if (index_length > velocity_scale_factor_status->estimated_number * velocity_scale_factor_parameter.estimated_coefficient) + if (index_length > velocity_scale_factor_status->estimated_number * enabled_data_ratio) { for (i = 0; i < index_length; i++) { diff --git a/eagleye_core/navigation/src/yawrate_offset.cpp b/eagleye_core/navigation/src/yawrate_offset.cpp old mode 100755 new mode 100644 index d446c836..b7da6151 --- a/eagleye_core/navigation/src/yawrate_offset.cpp +++ b/eagleye_core/navigation/src/yawrate_offset.cpp @@ -35,24 +35,21 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e const eagleye_msgs::Heading heading_interpolate,const sensor_msgs::Imu imu, const YawrateOffsetParameter yawrate_offset_parameter, YawrateOffsetStatus* yawrate_offset_status, eagleye_msgs::YawrateOffset* yawrate_offset) { - int i; - double yawrate = 0.0; - double sum_xy, sum_x, sum_y, sum_x2; - bool estimated_condition_status; - + double yawrate = imu.angular_velocity.z; std::size_t index_length; - std::size_t time_buffer_length; - std::size_t inversion_up_index_length; - std::size_t inversion_down_index_length; double estimated_number_cur; + double estimated_buffer_number_min = yawrate_offset_parameter.estimated_minimum_interval * yawrate_offset_parameter.imu_rate; + double estimated_buffer_number_max = yawrate_offset_parameter.estimated_maximum_interval * yawrate_offset_parameter.imu_rate; + double enabled_data_ratio = yawrate_offset_parameter.gnss_rate / yawrate_offset_parameter.imu_rate * yawrate_offset_parameter.gnss_receiving_threshold; + if(yawrate_offset->status.enabled_status == true) { - estimated_number_cur = yawrate_offset_parameter.estimated_number_max; + estimated_number_cur = estimated_buffer_number_max; } else { - estimated_number_cur = yawrate_offset_parameter.estimated_number_min; + estimated_number_cur = estimated_buffer_number_min; } if (yawrate_offset_status->estimated_number < estimated_number_cur) @@ -64,8 +61,6 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e yawrate_offset_status->estimated_number = estimated_number_cur; } - yawrate = imu.angular_velocity.z; - // data buffer generate yawrate_offset_status->time_buffer.push_back(imu.header.stamp.toSec()); yawrate_offset_status->yawrate_buffer.push_back(yawrate); @@ -74,7 +69,7 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e yawrate_offset_status->heading_estimate_status_buffer.push_back(heading_interpolate.status.estimate_status); yawrate_offset_status->yawrate_offset_stop_buffer.push_back(yawrate_offset_stop.yawrate_offset); - time_buffer_length = std::distance(yawrate_offset_status->time_buffer.begin(), yawrate_offset_status->time_buffer.end()); + std::size_t time_buffer_length = std::distance(yawrate_offset_status->time_buffer.begin(), yawrate_offset_status->time_buffer.end()); if (time_buffer_length >estimated_number_cur) { @@ -87,24 +82,25 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e } if (yawrate_offset_status->estimated_preparation_conditions == 0 && - yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number - 1] == true) + yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number - 1]) { yawrate_offset_status->estimated_preparation_conditions = 1; } else if (yawrate_offset_status->estimated_preparation_conditions == 1) { - if (yawrate_offset_status->heading_estimate_status_count < yawrate_offset_parameter.estimated_number_min) + if (yawrate_offset_status->heading_estimate_status_count < estimated_buffer_number_min) { ++yawrate_offset_status->heading_estimate_status_count; } - else if (yawrate_offset_status->heading_estimate_status_count == yawrate_offset_parameter.estimated_number_min) + else if (yawrate_offset_status->heading_estimate_status_count == estimated_buffer_number_min) { yawrate_offset_status->estimated_preparation_conditions = 2; } } + bool estimated_condition_status; if (yawrate_offset_status->estimated_preparation_conditions == 2 && - yawrate_offset_status->correction_velocity_buffer[yawrate_offset_status->estimated_number-1] > yawrate_offset_parameter.estimated_velocity_threshold && + yawrate_offset_status->correction_velocity_buffer[yawrate_offset_status->estimated_number-1] > yawrate_offset_parameter.moving_judgment_threshold && yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number-1] == true) { estimated_condition_status = true; @@ -118,15 +114,15 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e std::vector heading_estimate_status_index; std::vector index; - if (estimated_condition_status == true) + if (estimated_condition_status) { - for (i = 0; i < yawrate_offset_status->estimated_number; i++) + for (int i = 0; i < yawrate_offset_status->estimated_number; i++) { - if (yawrate_offset_status->correction_velocity_buffer[i] > yawrate_offset_parameter.estimated_velocity_threshold) + if (yawrate_offset_status->correction_velocity_buffer[i] > yawrate_offset_parameter.moving_judgment_threshold) { velocity_index.push_back(i); } - if (yawrate_offset_status->heading_estimate_status_buffer[i] == true) + if (yawrate_offset_status->heading_estimate_status_buffer[i]) { heading_estimate_status_index.push_back(i); } @@ -137,11 +133,11 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e index_length = std::distance(index.begin(), index.end()); - if (index_length > yawrate_offset_status->estimated_number * yawrate_offset_parameter.estimated_coefficient) + if (index_length > yawrate_offset_status->estimated_number * enabled_data_ratio) { std::vector provisional_heading_angle_buffer(yawrate_offset_status->estimated_number, 0); - for (i = 0; i < yawrate_offset_status->estimated_number; i++) + for (int i = 0; i < yawrate_offset_status->estimated_number; i++) { if (i > 0) { @@ -150,7 +146,6 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e } } - std::vector base_heading_angle_buffer; std::vector diff_buffer; std::vector time_buffer2; std::vector inversion_up_index; @@ -158,31 +153,23 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e index_length = std::distance(index.begin(), index.end()); - //base_heading_angle_buffer.clear(); - for (i = 0; i < yawrate_offset_status->estimated_number; i++) - { - base_heading_angle_buffer.push_back(yawrate_offset_status->heading_angle_buffer[index[index_length-1]] - - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[i]); - } - - //diff_buffer.clear(); - for (i = 0; i < index_length; i++) + for (int i = 0; i < index_length; i++) { - // diff_buffer.push_back(base_heading_angle_buffer[index[i]] - heading_angle_buffer[index[i]]); diff_buffer.push_back(yawrate_offset_status->heading_angle_buffer[index[index_length-1]] - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[index[i]] - yawrate_offset_status->heading_angle_buffer[index[i]]); } time_buffer2.clear(); - for (i = 0; i < index_length; i++) + for (int i = 0; i < index_length; i++) { time_buffer2.push_back(yawrate_offset_status->time_buffer[index[i]] - yawrate_offset_status->time_buffer[index[0]]); } // Least-square + double sum_xy, sum_x, sum_y, sum_x2; sum_xy = 0.0, sum_x = 0.0, sum_y = 0.0, sum_x2 = 0.0; - for (i = 0; i < index_length ; i++) + for (int i = 0; i < index_length ; i++) { sum_xy += time_buffer2[i] * diff_buffer[i]; sum_x += time_buffer2[i]; @@ -195,7 +182,7 @@ void yawrate_offset_estimate(const geometry_msgs::TwistStamped velocity, const e } } - if (yawrate_offset->status.enabled_status == false) + if (!yawrate_offset->status.enabled_status) { yawrate_offset->yawrate_offset = yawrate_offset_stop.yawrate_offset; } diff --git a/eagleye_core/navigation/src/yawrate_offset_stop.cpp b/eagleye_core/navigation/src/yawrate_offset_stop.cpp index c00585e1..13bb3346 100755 --- a/eagleye_core/navigation/src/yawrate_offset_stop.cpp +++ b/eagleye_core/navigation/src/yawrate_offset_stop.cpp @@ -39,9 +39,15 @@ void yawrate_offset_stop_estimate(const geometry_msgs::TwistStamped velocity, co int i; double tmp = 0.0; double initial_yawrate_offset_stop = 0.0; - double estimated_time_buffer_num = yawrate_offset_stop_parameter.estimated_number; std::size_t yawrate_buffer_length; + double estimated_buffer_number = yawrate_offset_stop_parameter.imu_rate * yawrate_offset_stop_parameter.estimated_interval; + double estimated_time_buffer_number = estimated_buffer_number; //TODO rename + + + // std::cout<< "estimated_number: " << _yawrate_offset_stop_parameter.estimated_number << std::endl; + + // data buffer generate if (yawrate_offset_stop_status->estimate_start_status == false) { @@ -55,12 +61,12 @@ void yawrate_offset_stop_estimate(const geometry_msgs::TwistStamped velocity, co yawrate_buffer_length = std::distance(yawrate_offset_stop_status->yawrate_buffer.begin(), yawrate_offset_stop_status->yawrate_buffer.end()); - if (yawrate_buffer_length > yawrate_offset_stop_parameter.estimated_number + estimated_time_buffer_num) + if (yawrate_buffer_length > estimated_buffer_number + estimated_time_buffer_number) { yawrate_offset_stop_status->yawrate_buffer.erase(yawrate_offset_stop_status->yawrate_buffer.begin()); } - if (velocity.twist.linear.x < yawrate_offset_stop_parameter.stop_judgment_velocity_threshold) + if (velocity.twist.linear.x < yawrate_offset_stop_parameter.stop_judgment_threshold) { ++yawrate_offset_stop_status->stop_count; } @@ -70,14 +76,14 @@ void yawrate_offset_stop_estimate(const geometry_msgs::TwistStamped velocity, co } // mean - if (yawrate_offset_stop_status->stop_count > yawrate_offset_stop_parameter.estimated_number + estimated_time_buffer_num) + if (yawrate_offset_stop_status->stop_count > estimated_buffer_number + estimated_time_buffer_number) { tmp = 0.0; - for (i = 0; i < yawrate_offset_stop_parameter.estimated_number; i++) + for (i = 0; i < estimated_buffer_number; i++) { tmp += yawrate_offset_stop_status->yawrate_buffer[i]; } - yawrate_offset_stop->yawrate_offset = -1 * tmp / yawrate_offset_stop_parameter.estimated_number; + yawrate_offset_stop->yawrate_offset = -1 * tmp / estimated_buffer_number; yawrate_offset_stop->status.enabled_status = true; yawrate_offset_stop->status.estimate_status = true; yawrate_offset_stop_status->estimate_start_status = true; diff --git a/eagleye_msgs/msg/Status.msg b/eagleye_msgs/msg/Status.msg index ecb6750a..bdd35828 100644 --- a/eagleye_msgs/msg/Status.msg +++ b/eagleye_msgs/msg/Status.msg @@ -1,2 +1,8 @@ +# error code +uint8 NAN_OR_INFINITE=0 +uint8 TOO_LARGE_OR_SMALL=1 + bool enabled_status # It's true in the IMU accumulation state, indicating available status. -bool estimate_status # true if the value is updated by GNSS, etc. \ No newline at end of file +bool estimate_status # true if the value is updated by GNSS, etc. +bool is_abnormal +uint8 error_code diff --git a/eagleye_pp/CMakeLists.txt b/eagleye_pp/CMakeLists.txt deleted file mode 100755 index ca03b955..00000000 --- a/eagleye_pp/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -add_subdirectory(eagleye_msgs) -add_subdirectory(eagleye_navigation) -add_subdirectory(eagleye_coordinate) -add_subdirectory(eagleye_util) -add_subdirectory(common/multi_rosbag_controller) -add_subdirectory(eagleye_rt) -add_subdirectory(eagleye_pp) diff --git a/eagleye_pp/common/multi_rosbag_controller b/eagleye_pp/common/multi_rosbag_controller index 242fd5a2..b7e32ffa 160000 --- a/eagleye_pp/common/multi_rosbag_controller +++ b/eagleye_pp/common/multi_rosbag_controller @@ -1 +1 @@ -Subproject commit 242fd5a2dc9403fa11920b35abca114269887bc3 +Subproject commit b7e32ffa03e34e948a1cd4b2d2a32fe4e5fd9790 diff --git a/eagleye_pp/eagleye_pp/config/README.md b/eagleye_pp/eagleye_pp/config/README.md new file mode 100644 index 00000000..a1c00134 --- /dev/null +++ b/eagleye_pp/eagleye_pp/config/README.md @@ -0,0 +1,247 @@ +# Parameter description + +The parameters for estimation in Eagleye can be set in the `config/eagleye_pp_config.yaml` file. + +## Topic + +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :---------------------------------------------------------------------- | :----------------------- | +| imu_topic | string | Topic name to be subscribed to in node (sensor_msgs/Imu.msg) | /imu/data_raw | +| twist_topic | string | Topic name to be subscribed to in node (geometry_msgs/TwistStamped.msg) | /can_twist | +| rtklib_nav_topic | string | Topic name to be subscribed to in node (rtklib_msgs/RtklibNav.msg) | /rtklib_nav_topic | +| nmea_sentence_topic | string | Topic name to be subscribed to in node (nmea_msgs/Sentence.msg) | /nmea_sentence_topic | +| gga_topic | string | Topic name to be subscribed to in node (nmea_msgs/Gpgga.msg) | /navsat/gga | +| rmc_topic | string | Topic name to be subscribed to in node (nmea_msgs/Gprmc.msg) | /navsat/rmc | +| localization_pose_topic | string | Topic name to be subscribed to in node (geometry_msgs/PoseStamped.msg) | /localization_pose_topic | + + +## TF + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| _tf_gnss_frame_ | | | | +| parent | string | | base_link | +| child | string | | gnss | + + +## Origin of GNSS coordinates (ECEF to ENU) + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| _ecef_base_pos_ | | | | +| x | double | x-coordinate | 0 | +| y | double | y-coordinate | 0 | +| z | double | z-coordinate | 0 | +| use_ecef_base_position | bool | Flag to use arbitrary origin coordinates | false | + + +## Common Parameters + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| imu_rate | double | IMU sampling cycle [Hz] | 50 | +| gnss_rate | double | GNSS sampling cycle [Hz] | 5 | +| stop_judgment_threshold | double | Stop judgment velocity [m/s] | 0.01 | +| slow_judgment_threshold | double | Slow judgment velocity [m/s] | 0.278 (1km/s) | +| moving_judgment_threshold | double | Movement judgment velocity [m/s] | 2.78 (10km/s) | + + + +## Eagleye Navigation Parameters + +### Basic Description + +The following parameters are used in most estimations. +Figure shows the relationship between these parameters. + +* _estimated_minimum_interval/estimated_maximum_interval_ +* _gnss_receiving_threshold_ +* _outlier_threshold_ +* _outlier_ratio_threshold_ + +![Parameter overview](../../docs/parameter_overview.png) + +### Basic Navigation Functions +### velocity_scale_factor + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------ | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 20 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 400 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| velocity_scale_factor_save_str | double | Scale factor value storage destination | /config/velocity_scale_factor.txt | +| save_velocity_scale_factor | bool | Flag to select preservation of estimated scale factor | false | +| velocity_scale_factor_save_duration | double | Scale factor save timing [s] | 100 | + + + +### yawrate_offset_stop + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------- | :------------------- | +| estimated_interval | double | Time of data buffering for estimation [s] | 4 | +| outlier_threshold | double | Allowable gap between current outlier estimate and previous estimate [rad] | 0.002 | + + +### yawrate_offset + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 30 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 300 (1st), 500 (2nd) | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Allowable gap between current estimate and _yawrate_offset_stop_ estimate [rad] | 0.002 | + + +### heading + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 10 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 30 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0524 (3 deg) | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | + + +### heading_interpolate + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| sync_search_period | double | Synchronous search time for delay interpolation [s] | 2 | + + +### slip_angle + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| manual_coefficient | double | Coefficients for slip angle estimation | 0.0 | + + +### slip_coefficient + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 2 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 100 | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0174 (1 deg/s) | +| lever_arm | double | Distance from GNSS antenna to center of rear axle [m] | 0.0 | + + +### rolling +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| filter_process_noise | double | Process noise for LPF [$(m/s^2)^2$] | 0.01 | +| filter_observation_noise | double | Observation noise for LPF [$(m/s^2)^2$] | 1 | + + +### trajectory + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0174 (1 deg/s) | +| timer_updata_rate | double | Self-diagnostic cycle [Hz] | 10 | +| deadlock_threshold | double | Allowable communication deadlock time for error output [s] | 1 | + + +### smoothing + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| moving_average_time | double | Moving average time of GNSS positioning solution [s] | 3 | +| moving_ratio_threshold | double | Ratio of movement within the interval (Value from 0~1) | 0.1 | + + +### height + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum distance of data buffering for estimation [m] | 200 | +| estimated_maximum_interval | double | Maximum distance of data buffering for estimation [m] | 2000 | +| update_distance | double | Distance of minimum movement to update the RTK-FIX solution [m] | 0.1 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.1 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [m] | 0.3 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| moving_average_time | double | Moving average time of longitudinal acceleration [s] | 1 | + + +### position + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_interval | double | Distance of data buffering for estimation [m] | 300 | +| update_distance | double | Distance of minimum movement to update data buffer [m] | 0.1 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [m] | 3 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | + + +### position_interpolate + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| sync_search_period | double | Synchronous search time for delay interpolation [s] | 2 | + + +### Optional Navigation Functions +### angular_velocity_offset_stop + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------- | :------------ | +| estimated_interval | double | Time of data buffering for estimation [s] | 4 | +| outlier_threshold | double | Allowable gap between current outlier estimate and previous estimate [rad] | 0.002 | + + +### rtk_heading + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| update_distance | double | Distance of minimum movement to update the RTK-FIX solution [m] | 0.3 | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 10 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 30 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0524 (3 deg) | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | + + +### enable_additional_rolling + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| update_distance | double | Distance of minimum movement to update pose [m] | 0.3 | +| moving_average_time | double | Moving average time of lateral acceleration [s] | 1 | +| sync_judgment_threshold | double | Synchronization judgment threshold [s] | 0.01 | +| sync_search_period | double | Synchronous search time for delay interpolation [s] | 1 | + + +### velocity_estimator + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| gga_downsample_time | double | Minimum time to update NMEA GGA [s] | 0.5 | +| stop_judgment_velocity_threshold | double | Stop judgment velocity [m/s] | 0.2 | +| stop_judgment_interval | double | Time to stop judgment [s] | 1 | +| variance_threshold | double | Angular velocity vibration variance for stop judgment [$(rad/s)^2$] | 0.000025 | +| | | | | +| _pitchrate_offset_ | | | | +| estimated_interval | double | Time of data buffering for estimation [s] | 8 | +| | | | | +| _pitching_ | | | | +| estimated_interval | double | Time of data buffering for estimation [s] | 5 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0174 (1 deg) | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.2 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| | | | | +| _acceleration_offset_ | | | | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 30 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 500 | +| filter_process_noise | double | Process noise for LPF [$(m/s^2)^2$] | 0.01 | +| filter_observation_noise | double | Observation noise for LPF [$(m/s^2)^2$] | 1 | +| | | | | +| _doppler_fusion_ | | | | +| estimated_interval | double | Time of data buffering for estimation [s] | 4 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.2 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [m/s] | 0.1 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | diff --git a/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml b/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml index 3f84998b..889e2aa8 100644 --- a/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml +++ b/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml @@ -32,6 +32,13 @@ gnss: ## The NMEA(nmea) mode eliminates the need to enter topics of type rtklib into eagleye, but may reduce accuracy. use_nmea_downsample: true nmea_downsample_freq: 5 + base_link2gnss: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0 # [rad] + pitch: 0 # [rad] + yaw: 0 # [rad] imu: base_link2imu: @@ -44,7 +51,7 @@ imu: use_canless_mode: false #Estimate mode -# Using Topic Name +# Topic twist_topic: /can_twist imu_topic: /imu/data_raw rtklib_nav_topic: /rtklib_nav @@ -52,141 +59,136 @@ rtklib_nav_topic: /rtklib_nav ## Forward/backward smoothing is not possible without nmea_msgs/Sentence. nmea_sentence_topic: /nmea_sentence gga_topic: /navsat/gga +rmc_topic: /navsat/rmc - -#Parameter settings for Eagleye - -ecef_base_pos: #The position in the ecef coordinate system to use when converting to the enu coordinate system. If 0, the position entered for the first time when starting the program will be used as a reference. +# Origin of GNSS coordinates (ECEF to ENU) +ecef_base_pos: x : 0.0 y : 0.0 z : 0.0 use_ecef_base_position : false -velocity_scale_factor: #Parameters for estimating vehicle speed scale factor. - estimated_number_min: 1000 #Minimum number of data used for estimation. (default:1000 = 20s) - estimated_number_max: 20000 #Maximum number of data used for estimation. (default:20000 = 400s) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_coefficient: 0.025 #A coefficient for determining the threshold for the number of valid data in the buffer to determine whether to make an estimate. (default:0.025 =2.5%) - -yawrate_offset_stop: #Parameters related to estimation of yaw rate offset due to temperature drift and noise of IMU during stoppage - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_number: 200 #Number of data used for estimation. (default:200 = 4s) - outlier_threshold: 0.002 - -angular_velocity_offset_stop: #Parameters related to estimation of yaw rate offset due to temperature drift and noise of IMU during stoppage - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_number: 200 #Number of data used for estimation. (default:200 = 4s) +# Eagleye Navigation Parameters +# Basic Navigation Functions +common: + imu_rate: 50 + gnss_rate: 5 + stop_judgment_threshold: 0.01 + slow_judgment_threshold: 0.278 + moving_judgment_threshold: 2.78 + +velocity_scale_factor: + estimated_minimum_interval: 20 + estimated_maximum_interval: 400 + gnss_receiving_threshold: 0.25 + velocity_scale_factor_save_str: /config/velocity_scale_factor.txt + save_velocity_scale_factor: false + velocity_scale_factor_save_duration: 100.0 + +yawrate_offset_stop: + estimated_interval: 4 outlier_threshold: 0.002 -yawrate_offset: #Parameters related to estimation of yaw rate offset due to temperature drift and noise of IMU while driving - estimated_number_min: 1500 #Minimum number of data used for estimation. (default:1500 = 30s) - estimated_coefficient: 0.01 #A coefficient for determining the threshold for the number of valid data in the buffer to determine whether to make an estimate. (default:0.01 =10%) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) +yawrate_offset: + estimated_minimum_interval: 30 + gnss_receiving_threshold: 0.25 outlier_threshold: 0.002 1st: - estimated_number_max: 14000 #Maximum number of data used for estimation. (default:14000 = 280s) + estimated_maximum_interval: 300 2nd: - estimated_number_max: 25000 #Maximum number of data used for estimation. (default:25000 = 500s) - -heading: #Parameters for estimating the azimuth of a car - estimated_number_min: 500 #Minimum number of data used for estimation. (default:500 = 10s) - estimated_number_max: 1500 #Maximum number of data used for estimation. (default:1500 = 30s) - estimated_gnss_coefficient: 0.025 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.025 = 2.5%) - estimated_heading_coefficient: 0.0125 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.0125 = 1.25%) - outlier_threshold: 0.0524 #Threshold for ending estimated outlier rejection. (default:0.0524 rad = 3 degree) - estimated_velocity_threshold: 0.278 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_yawrate_threshold: 0.0873 #Yaw rate threshold for curve judgment. (default:0.0873 rad/s = 5 degree/s) - -rtk_heading: #Parameters for estimating the azimuth of a car - estimated_distance: 0.3 #Distance to be used for heading angle estimation. - estimated_heading_buffer_min: 2 #Minimum number of RTK fix solutions required for heading angle estimation - estimated_number_min: 500 #Minimum number of data used for estimation. (default:500 = 10s) - estimated_number_max: 1500 #Maximum number of data used for estimation. (default:1500 = 30s) - estimated_gnss_coefficient: 0.025 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.025 = 2.5%) - estimated_heading_coefficient: 0.0125 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.0125 = 1.25%) - outlier_threshold: 0.0524 #Threshold for ending estimated outlier rejection. (default:0.0524 rad = 3 degree) - estimated_velocity_threshold: 0.278 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_yawrate_threshold: 0.0873 #Yaw rate threshold for curve judgment. (default:0.0873 rad/s = 5 degree/s) + estimated_maximum_interval: 500 + +heading: + estimated_minimum_interval: 10 + estimated_maximum_interval: 30 + gnss_receiving_threshold: 0.25 + outlier_threshold: 0.0524 + outlier_ratio_threshold: 0.5 + curve_judgment_threshold: 0.0873 heading_interpolate: - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - number_buffer_max: 100 + sync_search_period: 2 slip_angle: - manual_coefficient: 0.0 #If you do not want to correct slip angle, set slip angle estimate to false and set the coefficient here to 0 - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) + manual_coefficient: 0.0 slip_coefficient: - estimated_number_min: 100 #Minimum number of data used for estimation. (default:100) - estimated_number_max: 5000 #Maximum number of data used for estimation. (default:5000) - estimated_velocity_threshold: 3 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_yawrate_threshold: 0.017453 #Yaw rate threshold for curve judgment. (default:0.017453 rad/s = 1 degree/s) - lever_arm: 0.26 #Distance from center of rear wheel axle to GNSS antenna. (m) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) + estimated_minimum_interval: 2 + estimated_maximum_interval: 100 + curve_judgment_threshold: 0.017453 + lever_arm: 0.26 + +rolling: + filter_process_noise: 0.01 + filter_observation_noise: 1 trajectory: - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) + curve_judgment_threshold: 0.017453 + timer_updata_rate: 10 + deadlock_threshold: 1 + +smoothing: + moving_average_time: 3 + moving_ratio_threshold: 0.1 + +height: + estimated_minimum_interval: 200 + estimated_maximum_interval: 2000 + update_distance: 0.1 + gnss_receiving_threshold: 0.1 + outlier_threshold: 0.3 + outlier_ratio_threshold: 0.5 + moving_average_time: 1 position: - estimated_distance: 300 #Vehicle trajectory length used for estimation. (default:300 m) - separation_distance: 0.1 #Minimum travel distance to buffer. (0.1 m) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - outlier_threshold: 3.0 #Threshold for ending estimated outlier rejection. (default:3 m) - estimated_enu_vel_coefficient : 0.025 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.025 = 2.5%) - estimated_position_coefficient: 0.25 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.25 = 25%) - ecef_base_pos_x : 0.0 #The position in the ecef coordinate system to use when converting to the enu coordinate system. If 0, the position entered for the first time when starting the program will be used as a reference. - ecef_base_pos_y : 0.0 - ecef_base_pos_z : 0.0 + estimated_interval: 300 + update_distance: 0.1 + outlier_threshold: 3.0 + gnss_receiving_threshold: 0.25 + outlier_ratio_threshold: 0.5 position_interpolate: - number_buffer_max: 100 - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - -rtk_deadreckoning: - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - -smoothing: - estimated_number_max: 25 #GNSS output cycle　* Smoothing time - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_threshold: 0.1 + sync_search_period: 2 -height: - estimated_distance: 200 #Vehicle trajectory length used for acc x offset estimation. (default:200 m) - estimated_distance_max: 2000 #Vehicle trajectory length used for acc x scale factor estimation. (default:2000 m) - separation_distance: 0.1 #Minimum travel distance to buffer. (0.1 m) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_velocity_coefficient: 0.1 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.1 = 10%) - estimated_height_coefficient: 0.02 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.02 = 2%) - outlier_threshold: 0.3 #Threshold for ending estimated outlier rejection. (default:0.3 m) - average_num: 50 #Moving average parameter of the pitch angle. (default:50 = 1[s]) +# Optional Navigation Functions +angular_velocity_offset_stop: + estimated_interval: 4 + outlier_threshold: 0.002 -rolling: - stop_judgment_velocity_threshold: 0.01 - filter_process_noise: 0.01 - filter_observation_noise: 1 +rtk_heading: + update_distance: 0.3 + estimated_minimum_interval: 10 + estimated_maximum_interval: 30 + gnss_receiving_threshold: 0.25 + outlier_threshold: 0.0524 + outlier_ratio_threshold: 0.5 + curve_judgment_threshold: 0.0873 + +enable_additional_rolling: + update_distance: 0.3 + moving_average_time: 1 + sync_judgment_threshold: 0.01 + sync_search_period: 1 velocity_estimator: - gga_downsample_time: 1 - stop_judgment_velocity_threshold: 0.2 - stop_judgment_buffer_maxnum: 50 + gga_downsample_time: 0.5 + stop_judgment_velocity_threshold: 0.2 + stop_judgment_interval: 1 variance_threshold: 0.000025 pitchrate_offset: - buffer_count_max: 400 + estimated_interval: 8 pitching: - buffer_max: 250 + estimated_interval: 5 outlier_threshold: 0.0174 - estimated_velocity_threshold: 0.3 - estimated_gnss_coefficient: 0.02 - estimated_coefficient: 0.0125 + gnss_receiving_threshold: 0.2 + outlier_ratio_threshold: 0.5 acceleration_offset: - buffer_min: 1500 - buffer_max: 25000 + estimated_minimum_interval: 30 + estimated_maximum_interval: 500 filter_process_noise: 0.01 filter_observation_noise: 1 doppler_fusion: - buffer_max: 200 - estimated_gnss_coefficient: 0.02 - estimated_coefficient: 0.01 + estimated_interval: 4 + gnss_receiving_threshold: 0.2 + outlier_ratio_threshold: 0.5 outlier_threshold: 0.1 \ No newline at end of file diff --git a/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp b/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp index 7c9d4aeb..7d95731f 100644 --- a/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp +++ b/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp @@ -77,7 +77,7 @@ int main(int argc, char *argv[]) std::cout << "Estimate mode (GNSS) " << use_gnss_mode << std::endl; - if (rosbag_controller.setTopic(std::string(twist_topic))) + if (rosbag_controller.setTopic(std::string(twist_topic)) && !eagleye_pp.getUseCanlessMode()) { std::cout << "TwistStamped topic: " << twist_topic << std::endl; } diff --git a/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp b/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp index 72cc8537..c5db596e 100644 --- a/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp +++ b/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp @@ -126,6 +126,13 @@ void eagleye_pp::setParam(std::string arg_config_file, std::string *arg_twist_to base_link2imu_.transform.translation.y = y; base_link2imu_.transform.translation.z = z; base_link2imu_.transform.rotation = geometry_quat; + + tf2::Quaternion tf2_quat_gnss; + double gnss_roll = conf["gnss"]["base_link2gnss"]["roll"].as(); + double gnss_pitch = conf["gnss"]["base_link2gnss"]["pitch"].as(); + double gnss_yaw = conf["gnss"]["base_link2gnss"]["yaw"].as(); + tf2_quat_gnss.setRPY(gnss_roll, gnss_pitch, gnss_yaw); + geometry_msgs::Quaternion geometry_quat_gnss = tf2::toMsg(tf2_quat_gnss); // eagleye_rt params @@ -135,73 +142,101 @@ void eagleye_pp::setParam(std::string arg_config_file, std::string *arg_twist_to use_canless_mode_ = conf["use_canless_mode"].as(); - heading_interpolate_parameter_.stop_judgment_velocity_threshold = conf["heading_interpolate"]["stop_judgment_velocity_threshold"].as(); - heading_interpolate_parameter_.number_buffer_max = conf["heading_interpolate"]["number_buffer_max"].as(); - - heading_parameter_.estimated_number_min = conf["heading"]["estimated_number_min"].as(); - heading_parameter_.estimated_number_max = conf["heading"]["estimated_number_max"].as(); - heading_parameter_.estimated_gnss_coefficient = conf["heading"]["estimated_gnss_coefficient"].as(); - heading_parameter_.estimated_heading_coefficient = conf["heading"]["estimated_heading_coefficient"].as(); + heading_interpolate_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + heading_interpolate_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + heading_interpolate_parameter_.sync_search_period = conf["heading_interpolate"]["sync_search_period"].as(); + + heading_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + heading_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + heading_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + heading_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + heading_parameter_.estimated_minimum_interval = conf["heading"]["estimated_minimum_interval"].as(); + heading_parameter_.estimated_maximum_interval = conf["heading"]["estimated_maximum_interval"].as(); + heading_parameter_.gnss_receiving_threshold = conf["heading"]["gnss_receiving_threshold"].as(); heading_parameter_.outlier_threshold = conf["heading"]["outlier_threshold"].as(); - heading_parameter_.estimated_velocity_threshold = conf["heading"]["estimated_velocity_threshold"].as(); - heading_parameter_.stop_judgment_velocity_threshold = conf["heading"]["stop_judgment_velocity_threshold"].as(); - heading_parameter_.estimated_yawrate_threshold = conf["heading"]["estimated_yawrate_threshold"].as(); - - position_interpolate_parameter_.number_buffer_max = conf["position_interpolate"]["number_buffer_max"].as(); - - position_parameter_.estimated_distance = conf["position"]["estimated_distance"].as(); - position_parameter_.separation_distance = conf["position"]["separation_distance"].as(); - position_parameter_.estimated_velocity_threshold = conf["position"]["estimated_velocity_threshold"].as(); + heading_parameter_.outlier_ratio_threshold = conf["heading"]["outlier_ratio_threshold"].as(); + heading_parameter_.curve_judgment_threshold = conf["heading"]["curve_judgment_threshold"].as(); + + position_interpolate_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + position_interpolate_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + position_interpolate_parameter_.sync_search_period = conf["position_interpolate"]["sync_search_period"].as(); + + position_parameter_.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); + position_parameter_.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); + position_parameter_.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); + // position_parameter_.tf_gnss_parent_frame = conf["tf_gnss_frame"]["parent"].as(); + // position_parameter_.tf_gnss_child_frame = conf["tf_gnss_frame"]["child"].as(); + position_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + position_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + position_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + position_parameter_.estimated_interval = conf["position"]["estimated_interval"].as(); + position_parameter_.update_distance = conf["position"]["update_distance"].as(); position_parameter_.outlier_threshold = conf["position"]["outlier_threshold"].as(); - position_parameter_.estimated_enu_vel_coefficient = conf["position"]["estimated_enu_vel_coefficient"].as(); - position_parameter_.estimated_position_coefficient = conf["position"]["estimated_position_coefficient"].as();; - position_parameter_.ecef_base_pos_x = conf["position"]["ecef_base_pos_x"].as(); - position_parameter_.ecef_base_pos_y = conf["position"]["ecef_base_pos_y"].as(); - position_parameter_.ecef_base_pos_z = conf["position"]["ecef_base_pos_z"].as(); - + position_parameter_.gnss_receiving_threshold = conf["heading"]["gnss_receiving_threshold"].as(); + position_parameter_.outlier_ratio_threshold = conf["position"]["outlier_ratio_threshold"].as(); + + rtk_deadreckoning_parameter_.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); + rtk_deadreckoning_parameter_.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); + rtk_deadreckoning_parameter_.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); + rtk_deadreckoning_parameter_.use_ecef_base_position = conf["ecef_base_pos"]["use_ecef_base_position"].as(); + // rtk_deadreckoning_parameter_.tf_gnss_parent_frame = conf["tf_gnss_frame"]["parent"].as(); + // rtk_deadreckoning_parameter_.tf_gnss_child_frame = conf["tf_gnss_frame"]["child"].as(); + rtk_deadreckoning_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + + slip_angle_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); slip_angle_parameter_.manual_coefficient = conf["slip_angle"]["manual_coefficient"].as(); - slip_angle_parameter_.stop_judgment_velocity_threshold = conf["slip_angle"]["stop_judgment_velocity_threshold"].as(); - - smoothing_parameter_.ecef_base_pos_x = conf["position"]["ecef_base_pos_x"].as(); - smoothing_parameter_.ecef_base_pos_y = conf["position"]["ecef_base_pos_y"].as(); - smoothing_parameter_.ecef_base_pos_z = conf["position"]["ecef_base_pos_z"].as(); - smoothing_parameter_.estimated_number_max = conf["smoothing"]["estimated_number_max"].as(); - smoothing_parameter_.estimated_velocity_threshold = conf["smoothing"]["estimated_velocity_threshold"].as(); - smoothing_parameter_.estimated_threshold = conf["smoothing"]["estimated_threshold"].as(); - - trajectory_parameter_.stop_judgment_velocity_threshold = conf["trajectory"]["stop_judgment_velocity_threshold"].as(); - - velocity_scale_factor_parameter_.estimated_number_min = conf["velocity_scale_factor"]["estimated_number_min"].as(); - velocity_scale_factor_parameter_.estimated_number_max = conf["velocity_scale_factor"]["estimated_number_max"].as(); - velocity_scale_factor_parameter_.estimated_velocity_threshold = conf["velocity_scale_factor"]["estimated_velocity_threshold"].as(); - velocity_scale_factor_parameter_.estimated_coefficient = conf["velocity_scale_factor"]["estimated_coefficient"].as(); - - yawrate_offset_1st_parameter_.estimated_number_min = conf["yawrate_offset"]["estimated_number_min"].as(); - yawrate_offset_1st_parameter_.estimated_coefficient = conf["yawrate_offset"]["estimated_coefficient"].as(); - yawrate_offset_1st_parameter_.estimated_velocity_threshold = conf["yawrate_offset"]["estimated_velocity_threshold"].as(); - yawrate_offset_1st_parameter_.estimated_number_max = conf["yawrate_offset"]["1st"]["estimated_number_max"].as(); - yawrate_offset_1st_parameter_.outlier_threshold = conf["yawrate_offset"]["outlier_threshold"].as(); - - yawrate_offset_2nd_parameter_.estimated_number_min = conf["yawrate_offset"]["estimated_number_min"].as(); - yawrate_offset_2nd_parameter_.estimated_coefficient = conf["yawrate_offset"]["estimated_coefficient"].as(); - yawrate_offset_2nd_parameter_.estimated_velocity_threshold = conf["yawrate_offset"]["estimated_velocity_threshold"].as(); - yawrate_offset_2nd_parameter_.estimated_number_max = conf["yawrate_offset"]["2nd"]["estimated_number_max"].as(); - yawrate_offset_2nd_parameter_.outlier_threshold = conf["yawrate_offset"]["outlier_threshold"].as(); - - yawrate_offset_stop_parameter_.stop_judgment_velocity_threshold = conf["yawrate_offset_stop"]["stop_judgment_velocity_threshold"].as(); - yawrate_offset_stop_parameter_.estimated_number = conf["yawrate_offset_stop"]["estimated_number"].as(); + + smoothing_parameter_.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); + smoothing_parameter_.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); + smoothing_parameter_.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); + smoothing_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + smoothing_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + smoothing_parameter_.moving_average_time = conf["smoothing"]["moving_average_time"].as(); + smoothing_parameter_.moving_ratio_threshold = conf["smoothing"]["moving_ratio_threshold"].as(); + + trajectory_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + trajectory_parameter_.curve_judgment_threshold = conf["trajectory"]["curve_judgment_threshold"].as(); + + velocity_scale_factor_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + velocity_scale_factor_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + velocity_scale_factor_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + velocity_scale_factor_parameter_.estimated_minimum_interval = conf["velocity_scale_factor"]["estimated_minimum_interval"].as(); + velocity_scale_factor_parameter_.estimated_maximum_interval = conf["velocity_scale_factor"]["estimated_maximum_interval"].as(); + velocity_scale_factor_parameter_.gnss_receiving_threshold = conf["velocity_scale_factor"]["gnss_receiving_threshold"].as(); + + yawrate_offset_1st_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + yawrate_offset_1st_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + yawrate_offset_1st_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + yawrate_offset_1st_parameter_.estimated_minimum_interval = conf["yawrate_offset"]["estimated_minimum_interval"].as(); + yawrate_offset_1st_parameter_.estimated_maximum_interval = conf["yawrate_offset"]["1st"]["estimated_maximum_interval"].as(); + yawrate_offset_1st_parameter_.gnss_receiving_threshold = conf["yawrate_offset"]["gnss_receiving_threshold"].as(); + yawrate_offset_1st_parameter_.outlier_threshold = conf["yawrate_offset_stop"]["outlier_threshold"].as(); + + yawrate_offset_2nd_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + yawrate_offset_2nd_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + yawrate_offset_2nd_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + yawrate_offset_2nd_parameter_.estimated_minimum_interval = conf["yawrate_offset"]["estimated_minimum_interval"].as(); + yawrate_offset_2nd_parameter_.estimated_maximum_interval = conf["yawrate_offset"]["2nd"]["estimated_maximum_interval"].as(); + yawrate_offset_2nd_parameter_.gnss_receiving_threshold = conf["yawrate_offset"]["gnss_receiving_threshold"].as(); + yawrate_offset_2nd_parameter_.outlier_threshold = conf["yawrate_offset_stop"]["outlier_threshold"].as(); + + yawrate_offset_stop_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + yawrate_offset_stop_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + yawrate_offset_stop_parameter_.estimated_interval = conf["yawrate_offset_stop"]["estimated_interval"].as(); yawrate_offset_stop_parameter_.outlier_threshold = conf["yawrate_offset_stop"]["outlier_threshold"].as(); - height_parameter_.estimated_distance = conf["height"]["estimated_distance"].as(); - height_parameter_.estimated_distance_max = conf["height"]["estimated_distance_max"].as(); - height_parameter_.separation_distance = conf["height"]["separation_distance"].as(); - height_parameter_.estimated_velocity_threshold = conf["height"]["estimated_velocity_threshold"].as(); - height_parameter_.estimated_velocity_coefficient = conf["height"]["estimated_velocity_coefficient"].as(); - height_parameter_.estimated_height_coefficient = conf["height"]["estimated_height_coefficient"].as(); + height_parameter_.imu_rate = conf["common"]["imu_rate"].as(); + height_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); + height_parameter_.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + height_parameter_.estimated_minimum_interval = conf["height"]["estimated_minimum_interval"].as(); + height_parameter_.estimated_maximum_interval = conf["height"]["estimated_maximum_interval"].as(); + height_parameter_.update_distance = conf["height"]["update_distance"].as(); + height_parameter_.gnss_receiving_threshold = conf["height"]["gnss_receiving_threshold"].as(); height_parameter_.outlier_threshold = conf["height"]["outlier_threshold"].as(); - height_parameter_.average_num = conf["height"]["average_num"].as(); + height_parameter_.outlier_ratio_threshold = conf["height"]["outlier_ratio_threshold"].as(); + height_parameter_.moving_average_time = conf["height"]["moving_average_time"].as(); - rolling_parameter_.stop_judgment_velocity_threshold = conf["rolling"]["stop_judgment_velocity_threshold"].as(); + rolling_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); rolling_parameter_.filter_process_noise = conf["rolling"]["filter_process_noise"].as(); rolling_parameter_.filter_observation_noise = conf["rolling"]["filter_observation_noise"].as(); } @@ -612,6 +647,7 @@ void eagleye_pp::estimatingEagleye(bool arg_forward_flag) velocity_estimator.VelocityEstimate(imu_[i], rtklib_nav_[i], gga_[i], &_correction_velocity); _velocity_status.header = imu_[i].header; _velocity_status.status = velocity_estimator.getStatus(); + // std::cout << "_velocity_status: " << _velocity_status << std::flush; } else { diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index c647a35f..a929590e 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -9,6 +9,7 @@ pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) find_package(catkin REQUIRED COMPONENTS roscpp + roslib std_msgs geometry_msgs sensor_msgs @@ -53,95 +54,95 @@ include_directories( ) add_executable(velocity_scale_factor src/velocity_scale_factor_node.cpp) -target_link_libraries(velocity_scale_factor ${catkin_LIBRARIES}) +target_link_libraries(velocity_scale_factor ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(velocity_scale_factor ${catkin_EXPORTED_TARGETS}) add_executable(yawrate_offset_stop src/yawrate_offset_stop_node.cpp) -target_link_libraries(yawrate_offset_stop ${catkin_LIBRARIES}) +target_link_libraries(yawrate_offset_stop ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(yawrate_offset_stop ${catkin_EXPORTED_TARGETS}) add_executable(yawrate_offset src/yawrate_offset_node.cpp) -target_link_libraries(yawrate_offset ${catkin_LIBRARIES}) +target_link_libraries(yawrate_offset ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(yawrate_offset ${catkin_EXPORTED_TARGETS}) add_executable(heading src/heading_node.cpp) -target_link_libraries(heading ${catkin_LIBRARIES}) +target_link_libraries(heading ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(heading ${catkin_EXPORTED_TARGETS}) add_executable(position src/position_node.cpp) -target_link_libraries(position ${catkin_LIBRARIES}) +target_link_libraries(position ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(position ${catkin_EXPORTED_TARGETS}) add_executable(slip_angle src/slip_angle_node.cpp) -target_link_libraries(slip_angle ${catkin_LIBRARIES}) +target_link_libraries(slip_angle ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(slip_angle ${catkin_EXPORTED_TARGETS}) add_executable(smoothing src/smoothing_node.cpp) -target_link_libraries(smoothing ${catkin_LIBRARIES}) +target_link_libraries(smoothing ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(smoothing ${catkin_EXPORTED_TARGETS}) add_executable(trajectory src/trajectory_node.cpp) -target_link_libraries(trajectory ${catkin_LIBRARIES}) +target_link_libraries(trajectory ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(trajectory ${catkin_EXPORTED_TARGETS}) add_executable(heading_interpolate src/heading_interpolate_node.cpp) -target_link_libraries(heading_interpolate ${catkin_LIBRARIES}) +target_link_libraries(heading_interpolate ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(heading_interpolate ${catkin_EXPORTED_TARGETS}) add_executable(position_interpolate src/position_interpolate_node.cpp) -target_link_libraries(position_interpolate ${catkin_LIBRARIES}) +target_link_libraries(position_interpolate ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(position_interpolate ${catkin_EXPORTED_TARGETS}) add_executable(distance src/distance_node.cpp) -target_link_libraries(distance ${catkin_LIBRARIES}) +target_link_libraries(distance ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(distance ${catkin_EXPORTED_TARGETS}) add_executable(monitor src/monitor_node.cpp) -target_link_libraries(monitor ${catkin_LIBRARIES}) +target_link_libraries(monitor ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(monitor ${catkin_EXPORTED_TARGETS}) add_executable(height src/height_node.cpp) -target_link_libraries(height ${catkin_LIBRARIES}) +target_link_libraries(height ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(height ${catkin_EXPORTED_TARGETS}) add_executable(angular_velocity_offset_stop src/angular_velocity_offset_stop_node.cpp) -target_link_libraries(angular_velocity_offset_stop ${catkin_LIBRARIES}) +target_link_libraries(angular_velocity_offset_stop ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(angular_velocity_offset_stop ${catkin_EXPORTED_TARGETS}) add_executable(correction_imu src/correction_imu.cpp) -target_link_libraries(correction_imu ${catkin_LIBRARIES}) +target_link_libraries(correction_imu ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(correction_imu ${catkin_EXPORTED_TARGETS}) add_executable(rtk_deadreckoning src/rtk_deadreckoning_node.cpp) -target_link_libraries(rtk_deadreckoning ${catkin_LIBRARIES}) +target_link_libraries(rtk_deadreckoning ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(rtk_deadreckoning ${catkin_EXPORTED_TARGETS}) add_executable(rtk_heading src/rtk_heading_node.cpp) -target_link_libraries(rtk_heading ${catkin_LIBRARIES}) +target_link_libraries(rtk_heading ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(rtk_heading ${catkin_EXPORTED_TARGETS}) add_executable(slip_coefficient src/slip_coefficient_node.cpp) -target_link_libraries(slip_coefficient ${catkin_LIBRARIES}) +target_link_libraries(slip_coefficient ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(slip_coefficient ${catkin_EXPORTED_TARGETS}) add_executable(enable_additional_rolling src/enable_additional_rolling_node.cpp) -target_link_libraries(enable_additional_rolling ${catkin_LIBRARIES} ) +target_link_libraries(enable_additional_rolling ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(enable_additional_rolling ${catkin_EXPORTED_TARGETS}) add_executable(rolling src/rolling_node.cpp) -target_link_libraries(rolling ${catkin_LIBRARIES}) +target_link_libraries(rolling ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(rolling ${catkin_EXPORTED_TARGETS}) add_executable(navpvt2rtk src/navpvt2rtk_node.cpp) -target_link_libraries(navpvt2rtk ${catkin_LIBRARIES}) +target_link_libraries(navpvt2rtk ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(navpvt2rtk ${catkin_EXPORTED_TARGETS}) add_executable(tf_converted_imu src/tf_converted_imu.cpp) -target_link_libraries(tf_converted_imu ${catkin_LIBRARIES}) +target_link_libraries(tf_converted_imu ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(tf_converted_imu ${catkin_EXPORTED_TARGETS}) add_executable(velocity_estimator src/velocity_estimator_node.cpp) -target_link_libraries(velocity_estimator ${catkin_LIBRARIES}) +target_link_libraries(velocity_estimator ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(velocity_estimator ${catkin_EXPORTED_TARGETS}) install(TARGETS diff --git a/eagleye_rt/config/README.md b/eagleye_rt/config/README.md new file mode 100644 index 00000000..ae26fa3b --- /dev/null +++ b/eagleye_rt/config/README.md @@ -0,0 +1,271 @@ +# Parameter description + +The parameters for estimation in Eagleye can be set in the `config/eagleye_config.yaml` file. + +## Estimate mode + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| use_gnss_mode | string | Selecting the GNSS message type to use (RTKLIB/rtklib or NMEA/nmea) | RTKLIB | +| use_canless_mode | bool | Speed estimation as an alternative to speed sensors | false | + + +## Topic + +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :---------------------------------------------------------------------- | :----------------------- | +| imu_topic | string | Topic name to be subscribed to in node (sensor_msgs/Imu.msg) | /imu/data_raw | +| twist_topic | string | Topic name to be subscribed to in node (geometry_msgs/TwistStamped.msg) | /can_twist | +| rtklib_nav_topic | string | Topic name to be subscribed to in node (rtklib_msgs/RtklibNav.msg) | /rtklib_nav_topic | +| nmea_sentence_topic | string | Topic name to be subscribed to in node (nmea_msgs/Sentence.msg) | /nmea_sentence_topic | +| gga_topic | string | Topic name to be subscribed to in node (nmea_msgs/Gpgga.msg) | /navsat/gga | +| rmc_topic | string | Topic name to be subscribed to in node (nmea_msgs/Gprmc.msg) | /navsat/rmc | +| localization_pose_topic | string | Topic name to be subscribed to in node (geometry_msgs/PoseStamped.msg) | /localization_pose_topic | + + +## TF + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| _tf_gnss_frame_ | | | | +| parent | string | | base_link | +| child | string | | gnss | + + +## Origin of GNSS coordinates (ECEF to ENU) + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| _ecef_base_pos_ | | | | +| x | double | x-coordinate | 0 | +| y | double | y-coordinate | 0 | +| z | double | z-coordinate | 0 | +| use_ecef_base_position | bool | Flag to use arbitrary origin coordinates | false | + + +## Common Parameters + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| imu_rate | double | IMU sampling cycle [Hz] | 50 | +| gnss_rate | double | GNSS sampling cycle [Hz] | 5 | +| stop_judgment_threshold | double | Stop judgment velocity [m/s] | 0.01 | +| slow_judgment_threshold | double | Slow judgment velocity [m/s] | 0.278 (1km/s) | +| moving_judgment_threshold | double | Movement judgment velocity [m/s] | 2.78 (10km/s) | + + + +## Eagleye Navigation Parameters + +### Basic Description + +The following parameters are used in most estimations. +Figure shows the relationship between these parameters. + +* _estimated_minimum_interval/estimated_maximum_interval_ +* _gnss_receiving_threshold_ +* _outlier_threshold_ +* _outlier_ratio_threshold_ + +![Parameter overview](../../docs/parameter_overview.png) + +### Basic Navigation Functions +### velocity_scale_factor + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------ | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 20 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 400 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| velocity_scale_factor_save_str | double | Scale factor value storage destination | /config/velocity_scale_factor.txt | +| save_velocity_scale_factor | bool | Flag to select preservation of estimated scale factor | false | +| velocity_scale_factor_save_duration | double | Scale factor save timing [s] | 100 | + + + +### yawrate_offset_stop + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------- | :------------------- | +| estimated_interval | double | Time of data buffering for estimation [s] | 4 | +| outlier_threshold | double | Allowable gap between current outlier estimate and previous estimate [rad] | 0.002 | + + +### yawrate_offset + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 30 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 300 (1st), 500 (2nd) | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Allowable gap between current estimate and _yawrate_offset_stop_ estimate [rad] | 0.002 | + + +### heading + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 10 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 30 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0524 (3 deg) | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | + + +### heading_interpolate + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| sync_search_period | double | Synchronous search time for delay interpolation [s] | 2 | + + +### slip_angle + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| manual_coefficient | double | Coefficients for slip angle estimation | 0.0 | + + +### slip_coefficient + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 2 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 100 | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0174 (1 deg/s) | +| lever_arm | double | Distance from GNSS antenna to center of rear axle [m] | 0.0 | + + +### rolling +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| filter_process_noise | double | Process noise for LPF [$(m/s^2)^2$] | 0.01 | +| filter_observation_noise | double | Observation noise for LPF [$(m/s^2)^2$] | 1 | + + +### trajectory + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0174 (1 deg/s) | +| timer_updata_rate | double | Self-diagnostic cycle [Hz] | 10 | +| deadlock_threshold | double | Allowable communication deadlock time for error output [s] | 1 | + + +### smoothing + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| moving_average_time | double | Moving average time of GNSS positioning solution [s] | 3 | +| moving_ratio_threshold | double | Ratio of movement within the interval (Value from 0~1) | 0.1 | + + +### height + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_minimum_interval | double | Minimum distance of data buffering for estimation [m] | 200 | +| estimated_maximum_interval | double | Maximum distance of data buffering for estimation [m] | 2000 | +| update_distance | double | Distance of minimum movement to update the RTK-FIX solution [m] | 0.1 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.1 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [m] | 0.3 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| moving_average_time | double | Moving average time of longitudinal acceleration [s] | 1 | + + +### position + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| estimated_interval | double | Distance of data buffering for estimation [m] | 300 | +| update_distance | double | Distance of minimum movement to update data buffer [m] | 0.1 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [m] | 3 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | + + +### position_interpolate + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| sync_search_period | double | Synchronous search time for delay interpolation [s] | 2 | + + +### monitor + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| print_status | bool | Flag to output Eagleye estimates to console | true | +| log_output_status | bool | Flag to output Eagleye estimates to `log/eagleye_log_DATE.csv` | false | +| update_rate | double | Update cycle of Eagleye estimates [hz] | 10 | +| th_gnss_deadrock_time | double | Allowable communication deadlock time for error output [s] | 10 | +| th_velocity_scale_factor_ratio | double | Allowable ratio between current outlier estimate and previous estimate | 0.01 | +| use_compare_yawrate | bool | Flag for yaw rate value comparison diagnostics | false | +| comparison_twist_topic | string | Topic name for comparison (geometry_msgs/TwistStamped.msg) | /calculated_twist | +| th_diff_rad_per_sec | double | Allowable difference from comparables for error output [rad/s] | 0.174 (10 deg/s) | +| th_num_continuous_abnormal_yawrate | double | Number of times an abnormal value occurs for error output | 25 | +| th_dr_distance | double | Threshold for dead reckoning distance to error output [m] | 50 | + + +### Optional Navigation Functions +### angular_velocity_offset_stop + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------- | :------------ | +| estimated_interval | double | Time of data buffering for estimation [s] | 4 | +| outlier_threshold | double | Allowable gap between current outlier estimate and previous estimate [rad] | 0.002 | + + +### rtk_heading + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| update_distance | double | Distance of minimum movement to update the RTK-FIX solution [m] | 0.3 | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 10 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 30 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.25 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0524 (3 deg) | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | + + +### enable_additional_rolling + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| update_distance | double | Distance of minimum movement to update pose [m] | 0.3 | +| moving_average_time | double | Moving average time of lateral acceleration [s] | 1 | +| sync_judgment_threshold | double | Synchronization judgment threshold [s] | 0.01 | +| sync_search_period | double | Synchronous search time for delay interpolation [s] | 1 | + + +### velocity_estimator + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| gga_downsample_time | double | Minimum time to update NMEA GGA [s] | 0.5 | +| stop_judgment_velocity_threshold | double | Stop judgment velocity [m/s] | 0.2 | +| stop_judgment_interval | double | Time to stop judgment [s] | 1 | +| variance_threshold | double | Angular velocity vibration variance for stop judgment [$(rad/s)^2$] | 0.000025 | +| | | | | +| _pitchrate_offset_ | | | | +| estimated_interval | double | Time of data buffering for estimation [s] | 8 | +| | | | | +| _pitching_ | | | | +| estimated_interval | double | Time of data buffering for estimation [s] | 5 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0174 (1 deg) | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.2 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | +| | | | | +| _acceleration_offset_ | | | | +| estimated_minimum_interval | double | Minimum time of data buffering for estimation [s] | 30 | +| estimated_maximum_interval | double | Maximum time of data buffering for estimation [s] | 500 | +| filter_process_noise | double | Process noise for LPF [$(m/s^2)^2$] | 0.01 | +| filter_observation_noise | double | Observation noise for LPF [$(m/s^2)^2$] | 1 | +| | | | | +| _doppler_fusion_ | | | | +| estimated_interval | double | Time of data buffering for estimation [s] | 4 | +| gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.2 | +| outlier_threshold | double | Outlier threshold due to GNSS multipath [m/s] | 0.1 | +| outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml index 8e0614e8..8c18ba14 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -1,173 +1,162 @@ -#GNSS cycle 5Hz, IMU cycle 50Hz. -#Estimate mode +# Estimate mode use_gnss_mode: RTKLIB -## RTKLIB (rtklib) or NMEA (nmea) -## The NMEA(nmea) mode eliminates the need to enter topics of type rtklib into eagleye, but may reduce accuracy. +use_canless_mode: false +# Topic twist_topic: /can_twist imu_topic: /imu/data_raw rtklib_nav_topic: /rtklib_nav nmea_sentence_topic: /nmea_sentence gga_topic: /navsat/gga -localization_pose_topic: /localization/pose # This parameter is used to input external poses such as map matching to eagleye. (Normally it is not used.) - -#Parameter settings for Eagleye +rmc_topic: /navsat/rmc +localization_pose_topic: /localization/pose +# TF tf_gnss_frame: parent: "base_link" child: "gnss" -ecef_base_pos: #The position in the ecef coordinate system to use when converting to the enu coordinate system. If 0, the position entered for the first time when starting the program will be used as a reference. +# Origin of GNSS coordinates (ECEF to ENU) +ecef_base_pos: x : 0.0 y : 0.0 z : 0.0 use_ecef_base_position : false -velocity_scale_factor: #Parameters for estimating vehicle speed scale factor. - estimated_number_min: 1000 #Minimum number of data used for estimation. (default:1000 = 20s) - estimated_number_max: 20000 #Maximum number of data used for estimation. (default:20000 = 400s) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_coefficient: 0.025 #A coefficient for determining the threshold for the number of valid data in the buffer to determine whether to make an estimate. (default:0.025 =2.5%) - save_velocity_scale_factor: false #If this parameter is set to true, the velocitry_scale_factor saved at the last startup is used as the initial value at startup. - velocity_scale_factor_save_duration: 100.0 #[sec] - -yawrate_offset_stop: #Parameters related to estimation of yaw rate offset due to temperature drift and noise of IMU during stoppage - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_number: 200 #Number of data used for estimation. (default:200 = 4s) - outlier_threshold: 0.002 - -angular_velocity_offset_stop: #Parameters related to estimation of yaw rate offset due to temperature drift and noise of IMU during stoppage - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_number: 200 #Number of data used for estimation. (default:200 = 4s) +# Eagleye Navigation Parameters +# Basic Navigation Functions +common: + imu_rate: 50 + gnss_rate: 5 + stop_judgment_threshold: 0.01 + slow_judgment_threshold: 0.278 + moving_judgment_threshold: 2.78 + +velocity_scale_factor: + estimated_minimum_interval: 20 + estimated_maximum_interval: 400 + gnss_receiving_threshold: 0.25 + velocity_scale_factor_save_str: /config/velocity_scale_factor.txt + save_velocity_scale_factor: false + velocity_scale_factor_save_duration: 100.0 + th_velocity_scale_factor_percent: 10.0 + +yawrate_offset_stop: + estimated_interval: 4 outlier_threshold: 0.002 -yawrate_offset: #Parameters related to estimation of yaw rate offset due to temperature drift and noise of IMU while driving - estimated_number_min: 1500 #Minimum number of data used for estimation. (default:1500 = 30s) - estimated_coefficient: 0.01 #A coefficient for determining the threshold for the number of valid data in the buffer to determine whether to make an estimate. (default:0.01 =10%) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) +yawrate_offset: + estimated_minimum_interval: 30 + gnss_receiving_threshold: 0.25 outlier_threshold: 0.002 1st: - estimated_number_max: 14000 #Maximum number of data used for estimation. (default:14000 = 280s) + estimated_maximum_interval: 300 2nd: - estimated_number_max: 25000 #Maximum number of data used for estimation. (default:25000 = 500s) - -heading: #Parameters for estimating the azimuth of a car - estimated_number_min: 500 #Minimum number of data used for estimation. (default:500 = 10s) - estimated_number_max: 1500 #Maximum number of data used for estimation. (default:1500 = 30s) - estimated_gnss_coefficient: 0.025 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.025 = 2.5%) - estimated_heading_coefficient: 0.0125 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.0125 = 1.25%) - outlier_threshold: 0.0524 #Threshold for ending estimated outlier rejection. (default:0.0524 rad = 3 degree) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_yawrate_threshold: 0.0873 #Yaw rate threshold for curve judgment. (default:0.0873 rad/s = 5 degree/s) - -rtk_heading: #Parameters for estimating the azimuth of a car - estimated_distance: 0.3 #Distance to be used for heading angle estimation. - estimated_heading_buffer_min: 2 #Minimum number of RTK fix solutions required for heading angle estimation - estimated_number_min: 500 #Minimum number of data used for estimation. (default:500 = 10s) - estimated_number_max: 1500 #Maximum number of data used for estimation. (default:1500 = 30s) - estimated_gnss_coefficient: 0.025 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.025 = 2.5%) - estimated_heading_coefficient: 0.0125 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.0125 = 1.25%) - outlier_threshold: 0.0524 #Threshold for ending estimated outlier rejection. (default:0.0524 rad = 3 degree) - estimated_velocity_threshold: 0.278 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - estimated_yawrate_threshold: 0.0873 #Yaw rate threshold for curve judgment. (default:0.0873 rad/s = 5 degree/s) + estimated_maximum_interval: 500 + +heading: + estimated_minimum_interval: 10 + estimated_maximum_interval: 30 + gnss_receiving_threshold: 0.25 + outlier_threshold: 0.0524 + outlier_ratio_threshold: 0.5 + curve_judgment_threshold: 0.0873 heading_interpolate: - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - number_buffer_max: 100 + sync_search_period: 2 slip_angle: - manual_coefficient: 0.0 #If you do not want to correct slip angle, set slip angle estimate to false and set the coefficient here to 0 - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) + manual_coefficient: 0.0 slip_coefficient: - estimated_number_min: 100 #Minimum number of data used for estimation. (default:100) - estimated_number_max: 5000 #Maximum number of data used for estimation. (default:5000) - estimated_velocity_threshold: 3 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_yawrate_threshold: 0.017453 #Yaw rate threshold for curve judgment. (default:0.017453 rad/s = 1 degree/s) - lever_arm: 0.26 #Distance from center of rear wheel axle to GNSS antenna. (m) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - -trajectory: - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - timer_updata_rate: 10 #default:10 hz - th_deadlock_time: 1 #default:1s + estimated_minimum_interval: 2 + estimated_maximum_interval: 100 + curve_judgment_threshold: 0.017453 + lever_arm: 0.0 + +rolling: + filter_process_noise: 0.01 + filter_observation_noise: 1 + +trajectory: + curve_judgment_threshold: 0.017453 + timer_updata_rate: 10 + deadlock_threshold: 1 + +smoothing: + moving_average_time: 3 + moving_ratio_threshold: 0.1 + +height: + estimated_minimum_interval: 200 + estimated_maximum_interval: 2000 + update_distance: 0.1 + gnss_receiving_threshold: 0.1 + outlier_threshold: 0.3 + outlier_ratio_threshold: 0.5 + moving_average_time: 1 position: - estimated_distance: 300 #Vehicle trajectory length used for estimation. (default:300 m) - separation_distance: 0.1 #Minimum travel distance to buffer. (0.1 m) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - outlier_threshold: 3.0 #Threshold for ending estimated outlier rejection. (default:3 m) - estimated_enu_vel_coefficient : 0.025 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.025 = 2.5%) - estimated_position_coefficient: 0.25 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.25 = 25%) + estimated_interval: 300 + update_distance: 0.1 + outlier_threshold: 3.0 + gnss_receiving_threshold: 0.25 + outlier_ratio_threshold: 0.5 position_interpolate: - number_buffer_max: 100 - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) + sync_search_period: 2 -rtk_deadreckoning: - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) +monitor: + print_status: true + log_output_status: false + update_rate: 10.0 + th_gnss_deadrock_time: 10.0 + use_compare_yawrate: false + comparison_twist_topic: /calculated_twist + th_diff_rad_per_sec: 0.17453 + th_num_continuous_abnormal_yawrate: 25 + th_dr_distance: 50.0 -smoothing: - estimated_number_max: 25 #GNSS output cycle　* Smoothing time - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_threshold: 0.1 +# Optional Navigation Functions +angular_velocity_offset_stop: + estimated_interval: 4 + outlier_threshold: 0.002 -height: - estimated_distance: 200 #Vehicle trajectory length used for acc x offset estimation. (default:200 m) - estimated_distance_max: 2000 #Vehicle trajectory length used for acc x scale factor estimation. (default:2000 m) - separation_distance: 0.1 #Minimum travel distance to buffer. (0.1 m) - estimated_velocity_threshold: 2.78 #Velocity threshold at which to start estimation. (default:2.78 m/s = 10 km/h) - estimated_velocity_coefficient: 0.1 #A coefficient for determining the threshold for the number of valid data in the GNSS buffer to determine whether to make an estimate. (default:0.1 = 10%) - estimated_height_coefficient: 0.02 #A coefficient for determining the threshold for the number of valid data in the remainder buffer to determine whether to make an estimate. (default:0.02 = 2%) - outlier_threshold: 0.3 #Threshold for ending estimated outlier rejection. (default:0.3 m) - average_num: 50 #Moving average parameter of the pitch angle. (default:50 = 1[s]) +rtk_heading: + update_distance: 0.3 + estimated_minimum_interval: 10 + estimated_maximum_interval: 30 + gnss_receiving_threshold: 0.25 + outlier_threshold: 0.0524 + outlier_ratio_threshold: 0.5 + curve_judgment_threshold: 0.0873 enable_additional_rolling: - matching_update_distance: 0.3 #Distance to re-estimate from previous estimate. (default:0.3 m) - stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) - rolling_buffer_num: 50 #Number of data to take a moving average of roll angle (default:50 = 1s) - link_Time_stamp_parameter: 0.01 #Time considered to be in sync for time synchronization. (default:0.01 s) - imu_buffer_num: 10 #Number of IMU data to be stored for time correction. (default:10) - -rolling: - stop_judgment_velocity_threshold: 0.01 - filter_process_noise: 0.01 - filter_observation_noise: 1 + update_distance: 0.3 + moving_average_time: 1 + sync_judgment_threshold: 0.01 + sync_search_period: 1 velocity_estimator: - gga_downsample_time: 1 - stop_judgment_velocity_threshold: 0.2 - stop_judgment_buffer_maxnum: 50 + gga_downsample_time: 0.5 + stop_judgment_velocity_threshold: 0.2 + stop_judgment_interval: 1 variance_threshold: 0.000025 pitchrate_offset: - buffer_count_max: 400 + estimated_interval: 8 pitching: - buffer_max: 250 + estimated_interval: 5 outlier_threshold: 0.0174 - estimated_velocity_threshold: 0.3 - estimated_gnss_coefficient: 0.02 - estimated_coefficient: 0.0125 + gnss_receiving_threshold: 0.2 + outlier_ratio_threshold: 0.5 acceleration_offset: - buffer_min: 1500 - buffer_max: 25000 + estimated_minimum_interval: 30 + estimated_maximum_interval: 500 filter_process_noise: 0.01 filter_observation_noise: 1 doppler_fusion: - buffer_max: 200 - estimated_gnss_coefficient: 0.02 - estimated_coefficient: 0.01 + estimated_interval: 4 + gnss_receiving_threshold: 0.2 + outlier_ratio_threshold: 0.5 outlier_threshold: 0.1 - -monitor: - print_status: true - log_output_status: false - update_rate: 10.0 - th_gnss_deadrock_time: 10.0 - th_velocity_scale_factor_percent: 10.0 - use_compare_yawrate: false #Whether to compare the yaw rate with the yaw rate calculated from the vehicle speed model - comparison_twist_topic: /calculated_twist - th_diff_rad_per_sec: 0.17453 #Threshold of difference from comparison twist yaw rate(0.17453 rad/s = 10 degree/s) - th_num_continuous_abnormal_yawrate: 25 #Threshold for consecutive abnormal values in comparison twist diff --git a/eagleye_rt/launch/eagleye_calibration.launch b/eagleye_rt/launch/eagleye_calibration.launch index 6068b3b1..a6b0f9b1 100644 --- a/eagleye_rt/launch/eagleye_calibration.launch +++ b/eagleye_rt/launch/eagleye_calibration.launch @@ -10,6 +10,7 @@ + diff --git a/eagleye_rt/launch/eagleye_rt.launch b/eagleye_rt/launch/eagleye_rt.launch index 2e81950f..3a4d2cd3 100644 --- a/eagleye_rt/launch/eagleye_rt.launch +++ b/eagleye_rt/launch/eagleye_rt.launch @@ -6,11 +6,15 @@ - + + + - + + + @@ -44,10 +48,13 @@ - - + + + + + \ No newline at end of file diff --git a/eagleye_rt/launch/eagleye_rt_canless.launch b/eagleye_rt/launch/eagleye_rt_canless.launch index c3a02546..130ed7ea 100644 --- a/eagleye_rt/launch/eagleye_rt_canless.launch +++ b/eagleye_rt/launch/eagleye_rt_canless.launch @@ -14,6 +14,7 @@ + diff --git a/eagleye_rt/launch/eagleye_rt_dualantenna.launch b/eagleye_rt/launch/eagleye_rt_dualantenna.launch new file mode 100644 index 00000000..804a9c95 --- /dev/null +++ b/eagleye_rt/launch/eagleye_rt_dualantenna.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/eagleye_rt/package.xml b/eagleye_rt/package.xml index ffda8e1b..491ee531 100644 --- a/eagleye_rt/package.xml +++ b/eagleye_rt/package.xml @@ -60,6 +60,8 @@ tf2_sensor_msgs tf2_eigen ublox_msgs + roslib + yaml-cpp diff --git a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp index 60eee885..f13875fc 100644 --- a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp +++ b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp @@ -61,15 +61,32 @@ int main(int argc, char** argv) std::string subscribe_twist_topic_name = "/can_twist"; - nh.getParam("twist_topic", subscribe_twist_topic_name); - nh.getParam("angular_velocity_offset_stop/stop_judgment_velocity_threshold", _angular_velocity_offset_stop_parameter.stop_judgment_velocity_threshold); - nh.getParam("angular_velocity_offset_stop/estimated_number", _angular_velocity_offset_stop_parameter.estimated_number); - nh.getParam("angular_velocity_offset_stop/outlier_threshold", _angular_velocity_offset_stop_parameter.outlier_threshold); - - std::cout<< "subscribe_twist_topic_name: " << subscribe_twist_topic_name << std::endl; - std::cout<< "stop_judgment_velocity_threshold: " << _angular_velocity_offset_stop_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "estimated_number: " << _angular_velocity_offset_stop_parameter.estimated_number << std::endl; - std::cout<< "outlier_threshold: " << _angular_velocity_offset_stop_parameter.outlier_threshold << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + subscribe_twist_topic_name = conf["twist_topic"].as(); + + _angular_velocity_offset_stop_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _angular_velocity_offset_stop_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _angular_velocity_offset_stop_parameter.estimated_interval = conf["angular_velocity_offset_stop"]["estimated_interval"].as(); + _angular_velocity_offset_stop_parameter.outlier_threshold = conf["angular_velocity_offset_stop"]["outlier_threshold"].as(); + + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + std::cout << "imu_rate " << _angular_velocity_offset_stop_parameter.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << _angular_velocity_offset_stop_parameter.stop_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " << _angular_velocity_offset_stop_parameter.estimated_interval << std::endl; + std::cout << "outlier_threshold " << _angular_velocity_offset_stop_parameter.outlier_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe(subscribe_twist_topic_name, 1000, velocity_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/enable_additional_rolling_node.cpp b/eagleye_rt/src/enable_additional_rolling_node.cpp index 90a634b9..59ca5bf5 100644 --- a/eagleye_rt/src/enable_additional_rolling_node.cpp +++ b/eagleye_rt/src/enable_additional_rolling_node.cpp @@ -121,22 +121,41 @@ int main(int argc, char** argv) ros::init(argc, argv, "enable_additional_rolling"); ros::NodeHandle nh; - std::string subscribe_localization_pose_topic_name; - - nh.getParam("localization_pose_topic", subscribe_localization_pose_topic_name); - nh.getParam("enable_additional_rolling/matching_update_distance", _rolling_parameter.matching_update_distance); - nh.getParam("enable_additional_rolling/stop_judgment_velocity_threshold", _rolling_parameter.stop_judgment_velocity_threshold); - nh.getParam("enable_additional_rolling/rolling_buffer_num", _rolling_parameter.rolling_buffer_num); - nh.getParam("enable_additional_rolling/link_Time_stamp_parameter", _rolling_parameter.link_Time_stamp_parameter); - nh.getParam("enable_additional_rolling/imu_buffer_num", _rolling_parameter.imu_buffer_num); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_localization_pose_topic_name: " << subscribe_localization_pose_topic_name << std::endl; - std::cout<< "matching_update_distance: " << _rolling_parameter.matching_update_distance << std::endl; - std::cout<< "stop_judgment_velocity_threshold: " << _rolling_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "rolling_buffer_num: " << _rolling_parameter.rolling_buffer_num << std::endl; - std::cout<< "link_Time_stamp_parameter: " << _rolling_parameter.link_Time_stamp_parameter << std::endl; - std::cout<< "imu_buffer_num: " << _rolling_parameter.imu_buffer_num << std::endl; + std::string subscribe_localization_pose_topic_name = "/subscribe_pose_topic_name/invalid"; + + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + subscribe_localization_pose_topic_name = conf["localization_pose_topic"].as(); + + _rolling_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _rolling_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + + _rolling_parameter.update_distance = conf["enable_additional_rolling"]["update_distance"].as(); + _rolling_parameter.moving_average_time = conf["enable_additional_rolling"]["moving_average_time"].as(); + _rolling_parameter.sync_judgment_threshold = conf["enable_additional_rolling"]["sync_judgment_threshold"].as(); + _rolling_parameter.sync_search_period = conf["enable_additional_rolling"]["sync_search_period"].as(); + + std::cout<< "subscribe_localization_pose_topic_name " << subscribe_localization_pose_topic_name << std::endl; + + std::cout << "imu_rate " << _rolling_parameter.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << _rolling_parameter.stop_judgment_threshold << std::endl; + + std::cout << "update_distance " << _rolling_parameter.update_distance << std::endl; + std::cout << "moving_average_time " << _rolling_parameter.moving_average_time << std::endl; + std::cout << "sync_judgment_threshold " << _rolling_parameter.sync_judgment_threshold << std::endl; + std::cout << "sync_search_period " << _rolling_parameter.sync_search_period << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31menable_additional_rolling Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("velocity_scale_factor", 1000, velocity_scale_factor_callback , ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("yawrate_offset_2nd", 1000, yawrate_offset_2nd_callback , ros::TransportHints().tcpNoDelay()); @@ -145,6 +164,8 @@ int main(int argc, char** argv) ros::Subscriber sub5 = nh.subscribe(subscribe_localization_pose_topic_name, 1000, localization_pose_callback , ros::TransportHints().tcpNoDelay()); ros::Subscriber sub6 = nh.subscribe("angular_velocity_offset_stop", 1000, angular_velocity_offset_stop_callback , ros::TransportHints().tcpNoDelay()); ros::Subscriber sub7 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback , ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub8 = nh.subscribe("velocity", 1000, velocity_callback , ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub9 = nh.subscribe("velocity_status", 1000, velocity_status_callback , ros::TransportHints().tcpNoDelay()); _pub1 = nh.advertise("acc_y_offset_additional_rolling", 1000); _pub2 = nh.advertise("enable_additional_rolling", 1000); diff --git a/eagleye_rt/src/heading_interpolate_node.cpp b/eagleye_rt/src/heading_interpolate_node.cpp index 13b157c6..cd6bb9e0 100644 --- a/eagleye_rt/src/heading_interpolate_node.cpp +++ b/eagleye_rt/src/heading_interpolate_node.cpp @@ -96,10 +96,27 @@ int main(int argc, char** argv) ros::init(argc, argv, "heading_interpolate"); ros::NodeHandle nh; - nh.getParam("heading_interpolate/stop_judgment_velocity_threshold", _heading_interpolate_parameter.stop_judgment_velocity_threshold); - nh.getParam("heading_interpolate/number_buffer_max", _heading_interpolate_parameter.number_buffer_max); - std::cout<< "stop_judgment_velocity_threshold: " << _heading_interpolate_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "number_buffer_max: " << _heading_interpolate_parameter.number_buffer_max << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _heading_interpolate_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _heading_interpolate_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _heading_interpolate_parameter.sync_search_period = conf["heading_interpolate"]["sync_search_period"].as(); + + std::cout << "imu_rate " << _heading_interpolate_parameter.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << _heading_interpolate_parameter.stop_judgment_threshold << std::endl; + std::cout << "sync_search_period " << _heading_interpolate_parameter.sync_search_period << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mheading_interpolate Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } std::string publish_topic_name = "/publish_topic_name/invalid"; std::string subscribe_topic_name_1 = "/subscribe_topic_name/invalid_1"; diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index 36ce0f3a..ef5c6a3d 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -34,6 +34,7 @@ static rtklib_msgs::RtklibNav _rtklib_nav; static nmea_msgs::Gprmc _nmea_rmc; +static eagleye_msgs::Heading _multi_antenna_heading; static sensor_msgs::Imu _imu; static geometry_msgs::TwistStamped _velocity; static eagleye_msgs::StatusStamped _velocity_status; @@ -49,7 +50,8 @@ struct HeadingParameter _heading_parameter; struct HeadingStatus _heading_status; static std::string _use_gnss_mode; -static bool _use_canless_mode; +static bool _use_canless_mode = false; +static bool _use_multi_antenna_mode = false; bool _is_first_correction_velocity = false; @@ -63,10 +65,25 @@ void rmc_callback(const nmea_msgs::Gprmc::ConstPtr& msg) _nmea_rmc = *msg; } +void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) +{ + tf::Quaternion orientation; + tf::quaternionMsgToTF(msg->pose.orientation, orientation); + double roll, pitch, yaw; + tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + double heading = - yaw + (90* M_PI / 180); + + eagleye_msgs::Heading multi_antenna_heading; + multi_antenna_heading.header = msg->header; + multi_antenna_heading.heading_angle = heading; + + _multi_antenna_heading = multi_antenna_heading; +} + void velocity_callback(const geometry_msgs::TwistStamped::ConstPtr &msg) { _velocity = *msg; - if (!_is_first_correction_velocity && msg->twist.linear.x > _heading_parameter.estimated_velocity_threshold) + if (!_is_first_correction_velocity && msg->twist.linear.x > _heading_parameter.moving_judgment_threshold) { _is_first_correction_velocity = true; } @@ -101,19 +118,28 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) { if (!_is_first_correction_velocity) return; if(_use_canless_mode && !_velocity_status.status.enabled_status) return; + if(!_yawrate_offset_stop.status.enabled_status){ + ROS_WARN("Heading estimation is not started because the stop calibration is not yet completed"); + return; + } _imu = *msg; _heading.header = msg->header; _heading.header.frame_id = "base_link"; - if (_use_gnss_mode == "rtklib" || _use_gnss_mode == "RTKLIB") // use RTKLIB mode + bool use_rtklib_mode = _use_gnss_mode == "rtklib" || _use_gnss_mode == "RTKLIB"; + bool use_nmea_mode = _use_gnss_mode == "nmea" || _use_gnss_mode == "NMEA"; + if (use_rtklib_mode && !_use_multi_antenna_mode) // use RTKLIB mode heading_estimate(_rtklib_nav, _imu, _velocity, _yawrate_offset_stop, _yawrate_offset, _slip_angle, _heading_interpolate, _heading_parameter, &_heading_status, &_heading); - else if (_use_gnss_mode == "nmea" || _use_gnss_mode == "NMEA") // use NMEA mode + else if (use_nmea_mode && !_use_multi_antenna_mode) // use NMEA mode heading_estimate(_nmea_rmc, _imu, _velocity, _yawrate_offset_stop, _yawrate_offset, _slip_angle, _heading_interpolate, _heading_parameter, &_heading_status, &_heading); + else if (_use_multi_antenna_mode) + heading_estimate(_multi_antenna_heading, _imu, _velocity, _yawrate_offset_stop, _yawrate_offset, _slip_angle, + _heading_interpolate, _heading_parameter, &_heading_status, &_heading); - if (_heading.status.estimate_status) + if (_heading.status.estimate_status || _use_multi_antenna_mode) { _pub.publish(_heading); } @@ -126,32 +152,53 @@ int main(int argc, char** argv) ros::NodeHandle nh; std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_rmc_topic_name = "/mosaic/rmc"; - - nh.getParam("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - nh.getParam("rmc_topic",subscribe_rmc_topic_name); - nh.getParam("heading/estimated_number_min",_heading_parameter.estimated_number_min); - nh.getParam("heading/estimated_number_max",_heading_parameter.estimated_number_max); - nh.getParam("heading/estimated_gnss_coefficient",_heading_parameter.estimated_gnss_coefficient); - nh.getParam("heading/estimated_heading_coefficient",_heading_parameter.estimated_heading_coefficient); - nh.getParam("heading/outlier_threshold",_heading_parameter.outlier_threshold); - nh.getParam("heading/estimated_velocity_threshold",_heading_parameter.estimated_velocity_threshold); - nh.getParam("heading/stop_judgment_velocity_threshold",_heading_parameter.stop_judgment_velocity_threshold); - nh.getParam("heading/estimated_yawrate_threshold",_heading_parameter.estimated_yawrate_threshold); - nh.getParam("use_gnss_mode",_use_gnss_mode); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "subscribe_rmc_topic_name " << subscribe_rmc_topic_name << std::endl; - std::cout<< "estimated_number_min " << _heading_parameter.estimated_number_min << std::endl; - std::cout<< "estimated_number_max " << _heading_parameter.estimated_number_max << std::endl; - std::cout<< "estimated_gnss_coefficient " << _heading_parameter.estimated_gnss_coefficient << std::endl; - std::cout<< "estimated_heading_coefficient " << _heading_parameter.estimated_heading_coefficient << std::endl; - std::cout<< "outlier_threshold " << _heading_parameter.outlier_threshold << std::endl; - std::cout<< "estimated_velocity_threshold " << _heading_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "stop_judgment_velocity_threshold " << _heading_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "estimated_yawrate_threshold " << _heading_parameter.estimated_yawrate_threshold << std::endl; - std::cout<< "use_gnss_mode " << _use_gnss_mode << std::endl; + std::string subscribe_rmc_topic_name = "navsat/rmc"; + + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_gnss_mode = conf["use_gnss_mode"].as(); + + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + + _heading_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _heading_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _heading_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _heading_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + _heading_parameter.estimated_minimum_interval = conf["heading"]["estimated_minimum_interval"].as(); + _heading_parameter.estimated_maximum_interval = conf["heading"]["estimated_maximum_interval"].as(); + _heading_parameter.gnss_receiving_threshold = conf["heading"]["gnss_receiving_threshold"].as(); + _heading_parameter.outlier_threshold = conf["heading"]["outlier_threshold"].as(); + _heading_parameter.outlier_ratio_threshold = conf["heading"]["outlier_ratio_threshold"].as(); + _heading_parameter.curve_judgment_threshold = conf["heading"]["curve_judgment_threshold"].as(); + + std::cout<< "use_gnss_mode " << _use_gnss_mode << std::endl; + + std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + std::cout<< "subscribe_rmc_topic_name " << subscribe_rmc_topic_name << std::endl; + + std::cout << "imu_rate " << _heading_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _heading_parameter.gnss_rate << std::endl; + std::cout << "stop_judgment_threshold " << _heading_parameter.stop_judgment_threshold << std::endl; + std::cout << "moving_judgment_threshold " << _heading_parameter.moving_judgment_threshold << std::endl; + + std::cout << "estimated_minimum_interval " << _heading_parameter.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " << _heading_parameter.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " << _heading_parameter.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << _heading_parameter.outlier_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << _heading_parameter.outlier_ratio_threshold << std::endl; + std::cout << "curve_judgment_threshold " << _heading_parameter.curve_judgment_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mheading Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } std::string publish_topic_name = "/publish_topic_name/invalid"; std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; @@ -192,12 +239,13 @@ int main(int argc, char** argv) ros::Subscriber sub1 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub3 = nh.subscribe(subscribe_rmc_topic_name, 1000, rmc_callback, ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub4 = nh.subscribe("velocity", 1000, velocity_callback , ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub5 = nh.subscribe("velocity_status", 1000, velocity_status_callback, ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub6 = nh.subscribe("yawrate_offset_stop", 1000, yawrate_offset_stop_callback, ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub7 = nh.subscribe(subscribe_topic_name, 1000, yawrate_offset_callback, ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub8 = nh.subscribe("slip_angle", 1000, slip_angle_callback, ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub9 = nh.subscribe(subscribe_topic_name2, 1000, heading_interpolate_callback, ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub4 = nh.subscribe("gnss_compass_pose", 1000, pose_callback, ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub5 = nh.subscribe("velocity", 1000, velocity_callback , ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub6 = nh.subscribe("velocity_status", 1000, velocity_status_callback, ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub7 = nh.subscribe("yawrate_offset_stop", 1000, yawrate_offset_stop_callback, ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub8 = nh.subscribe(subscribe_topic_name, 1000, yawrate_offset_callback, ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub9 = nh.subscribe("slip_angle", 1000, slip_angle_callback, ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub10 = nh.subscribe(subscribe_topic_name2, 1000, heading_interpolate_callback, ros::TransportHints().tcpNoDelay()); _pub = nh.advertise(publish_topic_name, 1000); diff --git a/eagleye_rt/src/height_node.cpp b/eagleye_rt/src/height_node.cpp index 4f3ec4a2..aabc46f7 100644 --- a/eagleye_rt/src/height_node.cpp +++ b/eagleye_rt/src/height_node.cpp @@ -28,26 +28,26 @@ * Author MapIV Takanose */ - #include "ros/ros.h" - #include "coordinate/coordinate.hpp" - #include "navigation/navigation.hpp" +#include "ros/ros.h" +#include "coordinate/coordinate.hpp" +#include "navigation/navigation.hpp" - static sensor_msgs::Imu _imu; - static nmea_msgs::Gpgga _gga; - static geometry_msgs::TwistStamped _velocity; - static eagleye_msgs::StatusStamped _velocity_status; - static eagleye_msgs::Distance _distance; +static sensor_msgs::Imu _imu; +static nmea_msgs::Gpgga _gga; +static geometry_msgs::TwistStamped _velocity; +static eagleye_msgs::StatusStamped _velocity_status; +static eagleye_msgs::Distance _distance; - static ros::Publisher _pub1, _pub2, _pub3, _pub4, _pub5; - static eagleye_msgs::Height _height; - static eagleye_msgs::Pitching _pitching; - static eagleye_msgs::AccXOffset _acc_x_offset; - static eagleye_msgs::AccXScaleFactor _acc_x_scale_factor; +static ros::Publisher _pub1, _pub2, _pub3, _pub4, _pub5; +static eagleye_msgs::Height _height; +static eagleye_msgs::Pitching _pitching; +static eagleye_msgs::AccXOffset _acc_x_offset; +static eagleye_msgs::AccXScaleFactor _acc_x_scale_factor; - struct HeightParameter _height_parameter; - struct HeightStatus _height_status; +struct HeightParameter _height_parameter; +struct HeightStatus _height_status; - static bool _use_canless_mode; +static bool _use_canless_mode; void gga_callback(const nmea_msgs::Gpgga::ConstPtr& msg) { @@ -105,30 +105,44 @@ int main(int argc, char** argv) ros::NodeHandle nh; std::string subscribe_gga_topic_name = "/navsat/gga"; - std::string subscribe_imu_topic_name = "/imu/data_raw"; - - nh.getParam("gga_topic", subscribe_gga_topic_name); - nh.getParam("imu_topic", subscribe_imu_topic_name); - nh.getParam("height/estimated_distance", _height_parameter.estimated_distance); - nh.getParam("height/estimated_distance_max", _height_parameter.estimated_distance_max); - nh.getParam("height/separation_distance", _height_parameter.separation_distance); - nh.getParam("height/estimated_velocity_threshold", _height_parameter.estimated_velocity_threshold); - nh.getParam("height/estimated_velocity_coefficient", _height_parameter.estimated_velocity_coefficient); - nh.getParam("height/estimated_height_coefficient", _height_parameter.estimated_height_coefficient); - nh.getParam("height/outlier_threshold", _height_parameter.outlier_threshold); - nh.getParam("height/average_num", _height_parameter.average_num); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; - std::cout<< "subscribe_imu_topic_name " << subscribe_imu_topic_name << std::endl; - std::cout<< "estimated_distance " << _height_parameter.estimated_distance << std::endl; - std::cout<< "estimated_distance_max " << _height_parameter.estimated_distance_max << std::endl; - std::cout<< "separation_distance " << _height_parameter.separation_distance << std::endl; - std::cout<< "estimated_velocity_threshold " << _height_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "estimated_velocity_coefficient " << _height_parameter.estimated_velocity_coefficient << std::endl; - std::cout<< "estimated_height_coefficient " << _height_parameter.estimated_height_coefficient << std::endl; - std::cout<< "outlier_threshold " << _height_parameter.outlier_threshold << std::endl; - std::cout<< "average_num " << _height_parameter.average_num << std::endl; + + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _height_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _height_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _height_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + + _height_parameter.estimated_minimum_interval = conf["height"]["estimated_minimum_interval"].as(); + _height_parameter.estimated_maximum_interval = conf["height"]["estimated_maximum_interval"].as(); + _height_parameter.update_distance = conf["height"]["update_distance"].as(); + _height_parameter.gnss_receiving_threshold = conf["height"]["gnss_receiving_threshold"].as(); + _height_parameter.outlier_threshold = conf["height"]["outlier_threshold"].as(); + _height_parameter.outlier_ratio_threshold = conf["height"]["outlier_ratio_threshold"].as(); + _height_parameter.moving_average_time = conf["height"]["moving_average_time"].as(); + + std::cout << "imu_rate " << _height_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _height_parameter.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << _height_parameter.moving_judgment_threshold << std::endl; + + std::cout << "estimated_minimum_interval " << _height_parameter.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " << _height_parameter.estimated_maximum_interval << std::endl; + std::cout << "update_distance " << _height_parameter.update_distance << std::endl; + std::cout << "gnss_receiving_threshold " << _height_parameter.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << _height_parameter.outlier_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << _height_parameter.outlier_ratio_threshold << std::endl; + std::cout << "moving_average_time " << _height_parameter.moving_average_time << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mheight Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe(subscribe_gga_topic_name, 1000, gga_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/monitor_node.cpp b/eagleye_rt/src/monitor_node.cpp index b3a2d87e..f80720b6 100755 --- a/eagleye_rt/src/monitor_node.cpp +++ b/eagleye_rt/src/monitor_node.cpp @@ -39,11 +39,11 @@ static sensor_msgs::Imu _imu; static rtklib_msgs::RtklibNav _rtklib_nav; static sensor_msgs::NavSatFix _rtklib_fix; -static nmea_msgs::Gpgga _gga; +static nmea_msgs::Gpgga::ConstPtr _gga_ptr; static geometry_msgs::TwistStamped _velocity; static geometry_msgs::TwistStamped _correction_velocity; static eagleye_msgs::VelocityScaleFactor _velocity_scale_factor; -static eagleye_msgs::Distance _distance; +eagleye_msgs::Distance::ConstPtr _distance_ptr; static eagleye_msgs::Heading _heading_1st; static eagleye_msgs::Heading _heading_interpolate_1st; static eagleye_msgs::Heading _heading_2nd; @@ -97,11 +97,15 @@ static double _eagleye_twist_time_last; bool _use_compare_yawrate = false; double _update_rate = 10.0; double _th_gnss_deadrock_time = 10; -double _th_velocity_scale_factor_percent = 20; -double _th_diff_rad_per_sec = 0.17453; +double _th_diff_rad_per_sec = 0.17453; // [rad/sec] int _num_continuous_abnormal_yawrate = 0; int _th_num_continuous_abnormal_yawrate = 10; +bool _use_rtk_deadreckoning = false; +eagleye_msgs::Distance _previous_distance; +double _dr_distance = 0; // dead reckoning distance +double _th_dr_distance = 100.0; // [m] + void rtklib_nav_callback(const rtklib_msgs::RtklibNav::ConstPtr& msg) { _rtklib_nav = *msg; @@ -114,7 +118,7 @@ void rtklib_fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) void navsatfix_gga_callback(const nmea_msgs::Gpgga::ConstPtr& msg) { - _gga = *msg; + _gga_ptr = msg; _gga_sub_status = true; } @@ -135,7 +139,14 @@ void velocity_scale_factor_callback(const eagleye_msgs::VelocityScaleFactor::Con void distance_callback(const eagleye_msgs::Distance::ConstPtr& msg) { - _distance = *msg; + if (_distance_ptr != nullptr && _gga_ptr != nullptr){ + double delta_distance = _distance_ptr->distance - _previous_distance.distance; + _dr_distance += delta_distance; + if(int(_gga_ptr->gps_qual) == 4) { + _dr_distance = 0; + } + } + _distance_ptr = msg; } void heading_1st_callback(const eagleye_msgs::Heading::ConstPtr& msg) @@ -276,12 +287,12 @@ void navsat_fix_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat int8_t level = diagnostic_msgs::DiagnosticStatus::OK; std::string msg = "OK"; - if (_navsat_gga_time_last - _gga.header.stamp.toSec() > _th_gnss_deadrock_time || !_gga_sub_status) { + if (_gga_ptr == nullptr || _navsat_gga_time_last - _gga_ptr->header.stamp.toSec() > _th_gnss_deadrock_time || !_gga_sub_status) { level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - _navsat_gga_time_last = _gga.header.stamp.toSec(); + if (_gga_ptr != nullptr) _navsat_gga_time_last = _gga_ptr->header.stamp.toSec(); stat.summary(level, msg); } @@ -308,18 +319,25 @@ void velocity_scale_factor_topic_checker(diagnostic_updater::DiagnosticStatusWra level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(_velocity_scale_factor.scale_factor)) { - level = diagnostic_msgs::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_th_velocity_scale_factor_percent / 100 < std::abs(1.0 - _velocity_scale_factor.scale_factor)) { - level = diagnostic_msgs::DiagnosticStatus::ERROR; - msg = "Estimated velocity scale factor is too large or too small"; - } else if (!_velocity_scale_factor.status.enabled_status) { level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } + else if (_velocity_scale_factor.status.is_abnormal) { + level = diagnostic_msgs::DiagnosticStatus::ERROR; + if (_velocity_scale_factor.status.error_code == eagleye_msgs::Status::NAN_OR_INFINITE) + { + msg = "Estimated velocity scale factor is NaN or infinete"; + } + else if (_velocity_scale_factor.status.error_code == eagleye_msgs::Status::TOO_LARGE_OR_SMALL) + { + msg = "Estimated velocity scale factor is too large or too small"; + } + else + { + msg = "abnormal error of velocity_scale_factor"; + } + } _velocity_scale_factor_time_last = _velocity_scale_factor.header.stamp.toSec(); stat.summary(level, msg); @@ -330,20 +348,20 @@ void distance_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) int8_t level = diagnostic_msgs::DiagnosticStatus::OK; std::string msg = "OK"; - if (_distance_time_last == _distance.header.stamp.toSec()) { + if (_distance_ptr == nullptr || _distance_time_last == _distance_ptr->header.stamp.toSec()) { level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(_distance.distance)) { + else if (!std::isfinite(_distance_ptr->distance)) { level = diagnostic_msgs::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!_distance.status.enabled_status) { + else if (!_distance_ptr->status.enabled_status) { level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - _distance_time_last = _distance.header.stamp.toSec(); + if (_distance_ptr != nullptr) _distance_time_last = _distance_ptr->header.stamp.toSec(); stat.summary(level, msg); } @@ -487,14 +505,21 @@ void yawrate_offset_stop_topic_checker(diagnostic_updater::DiagnosticStatusWrapp level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(_yawrate_offset_stop.yawrate_offset)) { - level = diagnostic_msgs::DiagnosticStatus::ERROR; - msg = "invalid number"; - } else if (!_yawrate_offset_stop.status.enabled_status) { level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } + else if (!_yawrate_offset_stop.status.is_abnormal) { + level = diagnostic_msgs::DiagnosticStatus::ERROR; + if(_yawrate_offset_stop.status.error_code == eagleye_msgs::Status::NAN_OR_INFINITE) + { + msg = "estimate value is NaN or infinete"; + } + else + { + msg = "abnormal error of yawrate_offset_stop"; + } + } _yawrate_offset_stop_time_last = _yawrate_offset_stop.header.stamp.toSec(); stat.summary(level, msg); @@ -509,14 +534,21 @@ void yawrate_offset_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrappe level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(_yawrate_offset_1st.yawrate_offset)) { - level = diagnostic_msgs::DiagnosticStatus::ERROR; - msg = "invalid number"; - } else if (!_yawrate_offset_1st.status.enabled_status) { level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } + else if (!_yawrate_offset_1st.status.is_abnormal) { + level = diagnostic_msgs::DiagnosticStatus::ERROR; + if(_yawrate_offset_1st.status.error_code == eagleye_msgs::Status::NAN_OR_INFINITE) + { + msg = "estimate value is NaN or infinete"; + } + else + { + msg = "abnormal error of yawrate_offset_1st"; + } + } _yawrate_offset_1st_time_last = _yawrate_offset_1st.header.stamp.toSec(); stat.summary(level, msg); @@ -539,6 +571,17 @@ void yawrate_offset_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrappe level = diagnostic_msgs::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } + else if (!_yawrate_offset_2nd.status.is_abnormal) { + level = diagnostic_msgs::DiagnosticStatus::ERROR; + if(_yawrate_offset_2nd.status.error_code == eagleye_msgs::Status::NAN_OR_INFINITE) + { + msg = "estimate value is NaN or infinete"; + } + else + { + msg = "abnormal error of yawrate_offset_2nd"; + } + } _yawrate_offset_2nd_time_last = _yawrate_offset_2nd.header.stamp.toSec(); stat.summary(level, msg); @@ -722,6 +765,21 @@ void corrected_imu_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & s stat.summary(level, msg); } +void dr_distance_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::DiagnosticStatus::OK; + std::string msg = "OK"; + + if(_th_dr_distance < _dr_distance) + { + level = diagnostic_msgs::DiagnosticStatus::ERROR; + msg = "The dead reckoning interval is too large"; + } + + stat.summary(level, msg); + +} + void printStatus(void) { std::cout << std::endl; @@ -753,11 +811,11 @@ void printStatus(void) if(_gga_sub_status) { - std::cout<< "\033[1m rtk status \033[m "<gps_qual)<gps_qual)!=4 ? "\033[1;31mNo Fix\033[m" : "\033[1;32mFix\033[m")<lat<<" [deg]"<lon<<" [deg]"<alt + _gga_ptr->undulation<<" [m]"<::max_digits10) << _correction_velocity.twist.angular.z << ","; output_log_file << (_velocity_scale_factor.status.enabled_status ? "1" : "0") << ","; output_log_file << (_velocity_scale_factor.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _distance.distance << ","; - output_log_file << (_distance.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_distance.status.estimate_status ? "1" : "0") << ","; + if(_distance_ptr != nullptr) + { + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _distance_ptr->distance << ","; + output_log_file << (_distance_ptr->status.enabled_status ? "1" : "0") << ","; + output_log_file << (_distance_ptr->status.estimate_status ? "1" : "0") << ","; + } else + { + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; + output_log_file << 0 << ","; + output_log_file << 0 << ","; + } output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_1st.heading_angle << ","; output_log_file << (_heading_1st.status.enabled_status ? "1" : "0") << ","; output_log_file << (_heading_1st.status.estimate_status ? "1" : "0") << ","; @@ -963,11 +1029,21 @@ void outputLog(void) output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rolling.rolling_angle << ","; output_log_file << (_rolling.status.enabled_status ? "1" : "0") << ","; output_log_file << (_rolling.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.header.stamp.toNSec() << ","; //timestamp - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.lat << ","; //gga_llh.latitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.lon << ","; //gga_llh.longitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.alt + _gga.undulation<< ","; //gga_llh.altitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << int(_gga.gps_qual) << ","; //gga_llh.gps_qual + if(_gga_ptr != nullptr) + { + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga_ptr->header.stamp.toNSec() << ","; //timestamp + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga_ptr->lat << ","; //gga_llh.latitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga_ptr->lon << ","; //gga_llh.longitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga_ptr->alt + _gga_ptr->undulation<< ","; //gga_llh.altitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << int(_gga_ptr->gps_qual) << ","; //gga_llh.gps_qual + } else + { + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //timestamp + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //gga_llh.latitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //gga_llh.longitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0<< ","; //gga_llh.altitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //gga_llh.gps_qual + } output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.latitude << ","; //eagleye_pp_llh.latitude output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.longitude << ","; //eagleye_pp_llh.longitude output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.altitude << ","; //eagleye_pp_llh.altitude @@ -1022,35 +1098,53 @@ int main(int argc, char** argv) std::string subscribe_twist_topic_name = "/can_twist"; std::string subscribe_imu_topic_name = "/imu/data_raw"; std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_gga_topic_name = "navsat/gga"; std::string comparison_twist_topic_name = "/calculated_twist"; - n.getParam("twist_topic",subscribe_twist_topic_name); - n.getParam("imu_topic",subscribe_imu_topic_name); - n.getParam("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - n.getParam("gga_topic",subscribe_gga_topic_name); - n.getParam("monitor/print_status",_print_status); - n.getParam("monitor/log_output_status",_log_output_status); - n.getParam("monitor/update_rate",_update_rate); - n.getParam("monitor/th_gnss_deadrock_time",_th_gnss_deadrock_time); - n.getParam("monitor/th_velocity_scale_factor_percent",_th_velocity_scale_factor_percent); - n.getParam("monitor/use_compare_yawrate",_use_compare_yawrate); - n.getParam("monitor/comparison_twist_topic",comparison_twist_topic_name); - n.getParam("monitor/th_diff_rad_per_sec",_th_diff_rad_per_sec); - n.getParam("monitor/th_num_continuous_abnormal_yawrate",_th_num_continuous_abnormal_yawrate); - - std::cout<< "subscribe_twist_topic_name "<(); + subscribe_imu_topic_name = conf["imu_topic"].as(); + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + comparison_twist_topic_name = conf["monitor"]["comparison_twist_topic"].as(); + + _print_status = conf["monitor"]["print_status"].as(); + _log_output_status = conf["monitor"]["log_output_status"].as(); + _update_rate = conf["monitor"]["update_rate"].as(); + _th_gnss_deadrock_time = conf["monitor"]["th_gnss_deadrock_time"].as(); + _use_compare_yawrate = conf["monitor"]["use_compare_yawrate"].as(); + _th_diff_rad_per_sec = conf["monitor"]["th_diff_rad_per_sec"].as(); + _th_num_continuous_abnormal_yawrate = conf["monitor"]["th_num_continuous_abnormal_yawrate"].as(); + _th_dr_distance = conf["monitor"]["th_dr_distance"].as(); + + std::cout<< "subscribe_twist_topic_name "<(); + _position_interpolate_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _position_interpolate_parameter.sync_search_period = conf["position_interpolate"]["sync_search_period"].as(); + + std::cout << "imu_rate " << _position_interpolate_parameter.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << _position_interpolate_parameter.stop_judgment_threshold << std::endl; + std::cout << "sync_search_period " << _position_interpolate_parameter.sync_search_period << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mheading_interpolate Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("enu_vel", 1000, enu_vel_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("enu_absolute_pos", 1000, enu_absolute_pos_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/position_node.cpp b/eagleye_rt/src/position_node.cpp index 19196354..7619410b 100644 --- a/eagleye_rt/src/position_node.cpp +++ b/eagleye_rt/src/position_node.cpp @@ -148,35 +148,66 @@ int main(int argc, char** argv) ros::NodeHandle nh; std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_gga_topic_name = "/navsat/gga"; - - nh.getParam("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - nh.getParam("gga_topic",subscribe_gga_topic_name); - nh.getParam("position/estimated_distance",_position_parameter.estimated_distance); - nh.getParam("position/separation_distance",_position_parameter.separation_distance); - nh.getParam("position/estimated_velocity_threshold",_position_parameter.estimated_velocity_threshold); - nh.getParam("position/outlier_threshold",_position_parameter.outlier_threshold); - nh.getParam("position/estimated_enu_vel_coefficient",_position_parameter.estimated_enu_vel_coefficient); - nh.getParam("position/estimated_position_coefficient",_position_parameter.estimated_position_coefficient); - nh.getParam("ecef_base_pos/x",_position_parameter.ecef_base_pos_x); - nh.getParam("ecef_base_pos/y",_position_parameter.ecef_base_pos_y); - nh.getParam("ecef_base_pos/z",_position_parameter.ecef_base_pos_z); - nh.getParam("tf_gnss_frame/parent", _position_parameter.tf_gnss_parent_frame); - nh.getParam("tf_gnss_frame/child", _position_parameter.tf_gnss_child_frame); - nh.getParam("use_gnss_mode",_use_gnss_mode); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; - std::cout<< "estimated_distance " << _position_parameter.estimated_distance << std::endl; - std::cout<< "separation_distance " << _position_parameter.separation_distance << std::endl; - std::cout<< "estimated_velocity_threshold " << _position_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "outlier_threshold " << _position_parameter.outlier_threshold << std::endl; - std::cout<< "estimated_enu_vel_coefficient " << _position_parameter.estimated_enu_vel_coefficient << std::endl; - std::cout<< "estimated_position_coefficient " << _position_parameter.estimated_position_coefficient << std::endl; - std::cout<< "tf_gnss_frame/parent " << _position_parameter.tf_gnss_parent_frame << std::endl; - std::cout<< "tf_gnss_frame/child " << _position_parameter.tf_gnss_child_frame << std::endl; - std::cout<< "use_gnss_mode " << _use_gnss_mode << std::endl; + std::string subscribe_gga_topic_name = "navsat/gga"; + + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_gnss_mode = conf["use_gnss_mode"].as(); + _use_canless_mode = conf["use_canless_mode"].as(); + + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + + _position_parameter.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); + _position_parameter.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); + _position_parameter.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); + + _position_parameter.tf_gnss_parent_frame = conf["tf_gnss_frame"]["parent"].as(); + _position_parameter.tf_gnss_child_frame = conf["tf_gnss_frame"]["child"].as(); + + _position_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _position_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _position_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + + _position_parameter.estimated_interval = conf["position"]["estimated_interval"].as(); + _position_parameter.update_distance = conf["position"]["update_distance"].as(); + _position_parameter.outlier_threshold = conf["position"]["outlier_threshold"].as(); + + _position_parameter.gnss_receiving_threshold = conf["heading"]["gnss_receiving_threshold"].as(); + _position_parameter.outlier_ratio_threshold = conf["position"]["outlier_ratio_threshold"].as(); + + std::cout<< "use_gnss_mode " << _use_gnss_mode << std::endl; + std::cout<< "use_canless_mode " << _use_canless_mode << std::endl; + + std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + + std::cout<< "ecef_base_pos_x " << _position_parameter.ecef_base_pos_x << std::endl; + std::cout<< "ecef_base_pos_y " << _position_parameter.ecef_base_pos_y << std::endl; + std::cout<< "ecef_base_pos_z " << _position_parameter.ecef_base_pos_z << std::endl; + + std::cout<< "tf_gnss_frame/parent " << _position_parameter.tf_gnss_parent_frame << std::endl; + std::cout<< "tf_gnss_frame/child " << _position_parameter.tf_gnss_child_frame << std::endl; + + std::cout << "imu_rate " << _position_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _position_parameter.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << _position_parameter.moving_judgment_threshold << std::endl; + + std::cout << "estimated_interval " << _position_parameter.estimated_interval << std::endl; + std::cout << "update_distance " << _position_parameter.update_distance << std::endl; + std::cout << "outlier_threshold " << _position_parameter.outlier_threshold << std::endl; + std::cout << "gnss_receiving_threshold " << _position_parameter.gnss_receiving_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << _position_parameter.outlier_ratio_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mposition Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("enu_vel", 1000, enu_vel_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/rolling_node.cpp b/eagleye_rt/src/rolling_node.cpp index 03ab8f69..f8c06307 100755 --- a/eagleye_rt/src/rolling_node.cpp +++ b/eagleye_rt/src/rolling_node.cpp @@ -77,21 +77,37 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) _rolling_pub.publish(_rolling_msg); } -void setParam(ros::NodeHandle nh) +void setParam(std::string yaml_file) { - nh.getParam("rolling/stop_judgment_velocity_threshold", _rolling_parameter.stop_judgment_velocity_threshold); - nh.getParam("rolling/filter_process_noise", _rolling_parameter.filter_process_noise); - nh.getParam("rolling/filter_observation_noise", _rolling_parameter.filter_observation_noise); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout << "stop_judgment_velocity_threshold " << _rolling_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout << "filter_process_noise " << _rolling_parameter.filter_process_noise << std::endl; - std::cout << "filter_observation_noise " << _rolling_parameter.filter_observation_noise << std::endl; + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_canless_mode = conf["use_canless_mode"].as(); + _rolling_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _rolling_parameter.filter_process_noise = conf["rolling"]["filter_process_noise"].as(); + _rolling_parameter.filter_observation_noise = conf["rolling"]["filter_observation_noise"].as(); + + std::cout<< "use_canless_mode " << _use_canless_mode << std::endl; + std::cout << "stop_judgment_threshold " << _rolling_parameter.stop_judgment_threshold << std::endl; + std::cout << "filter_process_noise " << _rolling_parameter.filter_process_noise << std::endl; + std::cout << "filter_observation_noise " << _rolling_parameter.filter_observation_noise << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mrolling Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } } void rolling_node(ros::NodeHandle nh) { - setParam(nh); + + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + setParam(yaml_file); ros::Subscriber imu_sub = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/rtk_deadreckoning_node.cpp b/eagleye_rt/src/rtk_deadreckoning_node.cpp index 35405a9f..1b88e8fb 100644 --- a/eagleye_rt/src/rtk_deadreckoning_node.cpp +++ b/eagleye_rt/src/rtk_deadreckoning_node.cpp @@ -127,29 +127,43 @@ int main(int argc, char** argv) ros::NodeHandle nh; std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_gga_topic_name = "/navsat/gga"; - - nh.getParam("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - nh.getParam("gga_topic",subscribe_gga_topic_name); - nh.getParam("ecef_base_pos/x", _rtk_deadreckoning_parameter.ecef_base_pos_x); - nh.getParam("ecef_base_pos/y", _rtk_deadreckoning_parameter.ecef_base_pos_y); - nh.getParam("ecef_base_pos/z", _rtk_deadreckoning_parameter.ecef_base_pos_z); - nh.getParam("ecef_base_pos/use_ecef_base_position", _rtk_deadreckoning_parameter.use_ecef_base_position); - nh.getParam("rtk_deadreckoning/stop_judgment_velocity_threshold", _rtk_deadreckoning_parameter.stop_judgment_velocity_threshold); - nh.getParam("tf_gnss_frame/parent", _rtk_deadreckoning_parameter.tf_gnss_parent_frame); - nh.getParam("tf_gnss_frame/child", _rtk_deadreckoning_parameter.tf_gnss_child_frame); - nh.getParam("use_gnss_mode",_use_gnss_mode); - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; - std::cout<< "ecef_base_pos_x " << _rtk_deadreckoning_parameter.ecef_base_pos_x << std::endl; - std::cout<< "ecef_base_pos_y " << _rtk_deadreckoning_parameter.ecef_base_pos_y << std::endl; - std::cout<< "ecef_base_pos_z " << _rtk_deadreckoning_parameter.ecef_base_pos_z << std::endl; - std::cout<< "use_ecef_base_position " << _rtk_deadreckoning_parameter.use_ecef_base_position << std::endl; - std::cout<< "stop_judgment_velocity_threshold " << _rtk_deadreckoning_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "tf_gnss_frame/parent " << _rtk_deadreckoning_parameter.tf_gnss_parent_frame << std::endl; - std::cout<< "tf_gnss_frame/child " << _rtk_deadreckoning_parameter.tf_gnss_child_frame << std::endl; - std::cout<< "use_gnss_mode " << _use_gnss_mode << std::endl; + std::string subscribe_gga_topic_name = "navsat/gga"; + + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_gnss_mode = conf["use_gnss_mode"].as(); + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + + _rtk_deadreckoning_parameter.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); + _rtk_deadreckoning_parameter.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); + _rtk_deadreckoning_parameter.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); + _rtk_deadreckoning_parameter.use_ecef_base_position = conf["ecef_base_pos"]["use_ecef_base_position"].as(); + _rtk_deadreckoning_parameter.tf_gnss_parent_frame = conf["tf_gnss_frame"]["parent"].as(); + _rtk_deadreckoning_parameter.tf_gnss_child_frame = conf["tf_gnss_frame"]["child"].as(); + _rtk_deadreckoning_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + + std::cout << "use_gnss_mode " << _use_gnss_mode << std::endl; + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + + std::cout << "ecef_base_pos_x " << _rtk_deadreckoning_parameter.ecef_base_pos_x << std::endl; + std::cout << "ecef_base_pos_y " << _rtk_deadreckoning_parameter.ecef_base_pos_y << std::endl; + std::cout << "ecef_base_pos_z " << _rtk_deadreckoning_parameter.ecef_base_pos_z << std::endl; + std::cout << "use_ecef_base_position " << _rtk_deadreckoning_parameter.use_ecef_base_position << std::endl; + std::cout << "tf_gnss_frame/parent " << _rtk_deadreckoning_parameter.tf_gnss_parent_frame << std::endl; + std::cout << "tf_gnss_frame/child " << _rtk_deadreckoning_parameter.tf_gnss_child_frame << std::endl; + std::cout << "stop_judgment_threshold " << _rtk_deadreckoning_parameter.stop_judgment_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mrtk_deadreckoning Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("enu_vel", 1000, enu_vel_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/rtk_heading_node.cpp b/eagleye_rt/src/rtk_heading_node.cpp index fc777cf5..d45df6a8 100644 --- a/eagleye_rt/src/rtk_heading_node.cpp +++ b/eagleye_rt/src/rtk_heading_node.cpp @@ -114,30 +114,51 @@ int main(int argc, char** argv) std::string subscribe_gga_topic_name = "/navsat/gga"; - nh.getParam("gga_topic",subscribe_gga_topic_name); - nh.getParam("rtk_heading/estimated_distance",_heading_parameter.estimated_distance); - nh.getParam("rtk_heading/estimated_heading_buffer_min",_heading_parameter.estimated_heading_buffer_min); - nh.getParam("rtk_heading/estimated_number_min",_heading_parameter.estimated_number_min); - nh.getParam("rtk_heading/estimated_number_max",_heading_parameter.estimated_number_max); - nh.getParam("rtk_heading/estimated_gnss_coefficient",_heading_parameter.estimated_gnss_coefficient); - nh.getParam("rtk_heading/estimated_heading_coefficient",_heading_parameter.estimated_heading_coefficient); - nh.getParam("rtk_heading/outlier_threshold",_heading_parameter.outlier_threshold); - nh.getParam("rtk_heading/estimated_velocity_threshold",_heading_parameter.estimated_velocity_threshold); - nh.getParam("rtk_heading/stop_judgment_velocity_threshold",_heading_parameter.stop_judgment_velocity_threshold); - nh.getParam("rtk_heading/estimated_yawrate_threshold",_heading_parameter.estimated_yawrate_threshold); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; - std::cout<< "estimated_distance " << _heading_parameter.estimated_distance << std::endl; - std::cout<< "estimated_heading_buffer_min " << _heading_parameter.estimated_heading_buffer_min << std::endl; - std::cout<< "estimated_number_min " << _heading_parameter.estimated_number_min << std::endl; - std::cout<< "estimated_number_max " << _heading_parameter.estimated_number_max << std::endl; - std::cout<< "estimated_gnss_coefficient " << _heading_parameter.estimated_gnss_coefficient << std::endl; - std::cout<< "estimated_heading_coefficient " << _heading_parameter.estimated_heading_coefficient << std::endl; - std::cout<< "outlier_threshold " << _heading_parameter.outlier_threshold << std::endl; - std::cout<< "estimated_velocity_threshold " << _heading_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "stop_judgment_velocity_threshold " << _heading_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "estimated_yawrate_threshold " << _heading_parameter.estimated_yawrate_threshold << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_canless_mode = conf["use_canless_mode"].as(); + + _heading_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _heading_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _heading_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _heading_parameter.slow_judgment_threshold = conf["common"]["slow_judgment_threshold"].as(); + + _heading_parameter.update_distance = conf["rtk_heading"]["update_distance"].as(); + _heading_parameter.estimated_minimum_interval = conf["rtk_heading"]["estimated_minimum_interval"].as(); + _heading_parameter.estimated_maximum_interval = conf["rtk_heading"]["estimated_maximum_interval"].as(); + _heading_parameter.gnss_receiving_threshold = conf["rtk_heading"]["gnss_receiving_threshold"].as(); + _heading_parameter.outlier_threshold = conf["rtk_heading"]["outlier_threshold"].as(); + _heading_parameter.outlier_ratio_threshold = conf["rtk_heading"]["outlier_ratio_threshold"].as(); + _heading_parameter.curve_judgment_threshold = conf["rtk_heading"]["curve_judgment_threshold"].as(); + + std::cout<< "use_canless_mode " << _use_canless_mode << std::endl; + + std::cout<< "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; + + std::cout << "imu_rate " << _heading_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _heading_parameter.gnss_rate << std::endl; + std::cout << "stop_judgment_threshold " << _heading_parameter.stop_judgment_threshold << std::endl; + std::cout << "slow_judgment_threshold " << _heading_parameter.slow_judgment_threshold << std::endl; + + std::cout << "update_distance " << _heading_parameter.update_distance << std::endl; + std::cout << "estimated_minimum_interval " << _heading_parameter.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " << _heading_parameter.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " << _heading_parameter.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << _heading_parameter.outlier_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << _heading_parameter.outlier_ratio_threshold << std::endl; + std::cout << "curve_judgment_threshold " << _heading_parameter.curve_judgment_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mrtk_heading Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } std::string publish_topic_name = "/publish_topic_name/invalid"; std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; diff --git a/eagleye_rt/src/slip_angle_node.cpp b/eagleye_rt/src/slip_angle_node.cpp index ec753c3e..0ec5ec79 100644 --- a/eagleye_rt/src/slip_angle_node.cpp +++ b/eagleye_rt/src/slip_angle_node.cpp @@ -97,13 +97,27 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) int main(int argc, char** argv) { ros::init(argc, argv, "slip_angle"); - ros::NodeHandle nh; - nh.getParam("slip_angle/manual_coefficient", _slip_angle_parameter.manual_coefficient); - nh.getParam("slip_angle/stop_judgment_velocity_threshold", _slip_angle_parameter.stop_judgment_velocity_threshold); - std::cout<< "manual_coefficient " << _slip_angle_parameter.manual_coefficient << std::endl; - std::cout<< "stop_judgment_velocity_threshold " << _slip_angle_parameter.stop_judgment_velocity_threshold << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _slip_angle_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _slip_angle_parameter.manual_coefficient = conf["slip_angle"]["manual_coefficient"].as(); + + std::cout << "stop_judgment_threshold " << _slip_angle_parameter.stop_judgment_threshold << std::endl; + std::cout << "manual_coefficient " << _slip_angle_parameter.manual_coefficient << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mslip_angle Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("velocity_scale_factor", 1000, velocity_scale_factor_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/slip_coefficient_node.cpp b/eagleye_rt/src/slip_coefficient_node.cpp index e89b60dc..d6042a21 100644 --- a/eagleye_rt/src/slip_coefficient_node.cpp +++ b/eagleye_rt/src/slip_coefficient_node.cpp @@ -60,7 +60,7 @@ void rtklib_nav_callback(const rtklib_msgs::RtklibNav::ConstPtr& msg) void velocity_callback(const geometry_msgs::TwistStamped::ConstPtr &msg) { _velocity = *msg; - if (_is_first_correction_velocity == false && msg->twist.linear.x > _slip_coefficient_parameter.estimated_velocity_threshold) + if (_is_first_correction_velocity == false && msg->twist.linear.x > _slip_coefficient_parameter.moving_judgment_threshold) { _is_first_correction_velocity = true; } @@ -108,22 +108,45 @@ int main(int argc, char** argv) std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - nh.getParam("rtklib_nav_topic", subscribe_rtklib_nav_topic_name); - nh.getParam("slip_coefficient/estimated_number_min", _slip_coefficient_parameter.estimated_number_min); - nh.getParam("slip_coefficient/estimated_number_max", _slip_coefficient_parameter.estimated_number_max); - nh.getParam("slip_coefficient/estimated_velocity_threshold", _slip_coefficient_parameter.estimated_velocity_threshold); - nh.getParam("slip_coefficient/estimated_yawrate_threshold", _slip_coefficient_parameter.estimated_yawrate_threshold); - nh.getParam("slip_coefficient/lever_arm", _slip_coefficient_parameter.lever_arm); - nh.getParam("slip_coefficient/stop_judgment_velocity_threshold", _slip_coefficient_parameter.stop_judgment_velocity_threshold); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "estimated_number_min " << _slip_coefficient_parameter.estimated_number_min << std::endl; - std::cout<< "estimated_number_max " << _slip_coefficient_parameter.estimated_number_max << std::endl; - std::cout<< "estimated_velocity_threshold " << _slip_coefficient_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "estimated_yawrate_threshold " << _slip_coefficient_parameter.estimated_yawrate_threshold << std::endl; - std::cout<< "lever_arm " << _slip_coefficient_parameter.lever_arm << std::endl; - std::cout<< "stop_judgment_velocity_threshold " << _slip_coefficient_parameter.stop_judgment_velocity_threshold << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_canless_mode = conf["use_canless_mode"].as(); + + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + + _slip_coefficient_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _slip_coefficient_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _slip_coefficient_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + + _slip_coefficient_parameter.estimated_minimum_interval = conf["slip_coefficient"]["estimated_minimum_interval"].as(); + _slip_coefficient_parameter.estimated_maximum_interval = conf["slip_coefficient"]["estimated_maximum_interval"].as(); + _slip_coefficient_parameter.curve_judgment_threshold = conf["slip_coefficient"]["curve_judgment_threshold"].as(); + _slip_coefficient_parameter.lever_arm = conf["slip_coefficient"]["lever_arm"].as(); + + std::cout<< "use_canless_mode " << _use_canless_mode << std::endl; + + std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + + std::cout << "imu_rate " << _slip_coefficient_parameter.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << _slip_coefficient_parameter.stop_judgment_threshold << std::endl; + std::cout << "moving_judgment_threshold " << _slip_coefficient_parameter.moving_judgment_threshold << std::endl; + + std::cout << "estimated_minimum_interval " << _slip_coefficient_parameter.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " << _slip_coefficient_parameter.estimated_maximum_interval << std::endl; + std::cout << "curve_judgment_threshold " << _slip_coefficient_parameter.curve_judgment_threshold << std::endl; + std::cout << "lever_arm " << _slip_coefficient_parameter.lever_arm << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mslip_coefficient Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/smoothing_node.cpp b/eagleye_rt/src/smoothing_node.cpp index 62fb5061..7297d597 100644 --- a/eagleye_rt/src/smoothing_node.cpp +++ b/eagleye_rt/src/smoothing_node.cpp @@ -71,22 +71,45 @@ int main(int argc, char** argv) std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - nh.getParam("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - nh.getParam("ecef_base_pos/x",_smoothing_parameter.ecef_base_pos_x); - nh.getParam("ecef_base_pos/y",_smoothing_parameter.ecef_base_pos_y); - nh.getParam("ecef_base_pos/z",_smoothing_parameter.ecef_base_pos_z); - nh.getParam("smoothing/estimated_number_max",_smoothing_parameter.estimated_number_max); - nh.getParam("smoothing/estimated_velocity_threshold",_smoothing_parameter.estimated_velocity_threshold); - nh.getParam("smoothing/estimated_threshold",_smoothing_parameter.estimated_threshold); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "ecef_base_pos_x " << _smoothing_parameter.ecef_base_pos_x << std::endl; - std::cout<< "ecef_base_pos_y " << _smoothing_parameter.ecef_base_pos_y << std::endl; - std::cout<< "ecef_base_pos_z " << _smoothing_parameter.ecef_base_pos_z << std::endl; - std::cout<< "estimated_number_max " << _smoothing_parameter.estimated_number_max << std::endl; - std::cout<< "estimated_velocity_threshold " << _smoothing_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "estimated_threshold " << _smoothing_parameter.estimated_threshold << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_canless_mode = conf["use_canless_mode"].as(); + + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + + _smoothing_parameter.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); + _smoothing_parameter.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); + _smoothing_parameter.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); + + _smoothing_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _smoothing_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + _smoothing_parameter.moving_average_time = conf["smoothing"]["moving_average_time"].as(); + _smoothing_parameter.moving_ratio_threshold = conf["smoothing"]["moving_ratio_threshold"].as(); + + std::cout<< "use_canless_mode " << _use_canless_mode << std::endl; + + std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + + std::cout<< "ecef_base_pos_x " << _smoothing_parameter.ecef_base_pos_x << std::endl; + std::cout<< "ecef_base_pos_y " << _smoothing_parameter.ecef_base_pos_y << std::endl; + std::cout<< "ecef_base_pos_z " << _smoothing_parameter.ecef_base_pos_z << std::endl; + + std::cout << "gnss_rate " << _smoothing_parameter.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << _smoothing_parameter.moving_judgment_threshold << std::endl; + std::cout << "moving_average_time " << _smoothing_parameter.moving_average_time << std::endl; + std::cout << "moving_ratio_threshold " << _smoothing_parameter.moving_ratio_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31msmoothing Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe("velocity", 1000, velocity_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("velocity_status", 1000, velocity_status_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_rt/src/tf_converted_imu.cpp b/eagleye_rt/src/tf_converted_imu.cpp index f1cb00bd..00e9831f 100644 --- a/eagleye_rt/src/tf_converted_imu.cpp +++ b/eagleye_rt/src/tf_converted_imu.cpp @@ -68,12 +68,26 @@ TFConvertedIMU::TFConvertedIMU() : tflistener_(tfbuffer_) std::string subscribe_imu_topic_name = "/imu/data_raw"; std::string publish_imu_topic_name = "imu/data_tf_converted"; - nh_.getParam("imu_topic", subscribe_imu_topic_name); - nh_.getParam("publish_imu_topic", publish_imu_topic_name); - nh_.getParam("tf_gnss_frame/parent", tf_base_link_frame_); - std::cout<< "subscribe_imu_topic_name: " << subscribe_imu_topic_name << std::endl; - std::cout<< "publish_imu_topic_name: " << publish_imu_topic_name << std::endl; - std::cout<< "tf_base_link_frame: " << tf_base_link_frame_ << std::endl; + std::string yaml_file; + nh_.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + subscribe_imu_topic_name = conf["publish_imu_topic_name"].as(); + tf_base_link_frame_ = conf["tf_gnss_frame"]["parent"].as(); + std::cout<< "subscribe_imu_topic_name: " << subscribe_imu_topic_name << std::endl; + std::cout<< "publish_imu_topic_name: " << publish_imu_topic_name << std::endl; + std::cout<< "tf_base_link_frame: " << tf_base_link_frame_ << std::endl; + + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mtf_converted_imu Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } sub_ = nh_.subscribe(subscribe_imu_topic_name, 1000, &TFConvertedIMU::imu_callback, this, ros::TransportHints().tcpNoDelay()); pub_ = nh_.advertise("imu/data_tf_converted", 1000); diff --git a/eagleye_rt/src/trajectory_node.cpp b/eagleye_rt/src/trajectory_node.cpp index 380290e5..2e77211d 100644 --- a/eagleye_rt/src/trajectory_node.cpp +++ b/eagleye_rt/src/trajectory_node.cpp @@ -52,8 +52,8 @@ static ros::Publisher _pub3; struct TrajectoryParameter _trajectory_parameter; struct TrajectoryStatus _trajectory_status; -static double _update_rate = 10; -static double _th_deadlock_time = 1; +static double _timer_updata_rate = 10; +static double _deadlock_threshold = 1; static double _imu_time_last, _velocity_time_last; static bool _input_status; @@ -103,9 +103,9 @@ void velocity_callback(const geometry_msgs::TwistStamped::ConstPtr& msg) void timer_callback(const ros::TimerEvent& e) { - if (std::abs(_imu.header.stamp.toSec() - _imu_time_last) < _th_deadlock_time && - std::abs(_velocity.header.stamp.toSec() - _velocity_time_last) < _th_deadlock_time && - std::abs(_velocity.header.stamp.toSec() - _imu.header.stamp.toSec()) < _th_deadlock_time) + if (std::abs(_imu.header.stamp.toSec() - _imu_time_last) < _deadlock_threshold && + std::abs(_velocity.header.stamp.toSec() - _velocity_time_last) < _deadlock_threshold && + std::abs(_velocity.header.stamp.toSec() - _imu.header.stamp.toSec()) < _deadlock_threshold) { _input_status = true; } @@ -161,22 +161,42 @@ int main(int argc, char** argv) ros::NodeHandle nh; - std::string subscribe_imu_topic_name = "/imu/data_raw"; std::string subscribe_twist_topic_name = "/can_twist"; - nh.getParam("twist_topic",subscribe_twist_topic_name); - nh.getParam("trajectory/stop_judgment_velocity_threshold",_trajectory_parameter.stop_judgment_velocity_threshold); - nh.getParam("trajectory/stop_judgment_yawrate_threshold",_trajectory_parameter.stop_judgment_yawrate_threshold); - nh.getParam("trajectory/timer_updata_rate",_update_rate); - nh.getParam("trajectory/th_deadlock_time",_th_deadlock_time); - nh.getParam("use_canless_mode",_use_canless_mode); + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; - std::cout<< "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; - std::cout<< "stop_judgment_velocity_threshold " << _trajectory_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "stop_judgment_yawrate_threshold " << _trajectory_parameter.stop_judgment_yawrate_threshold << std::endl; - std::cout<< "timer_updata_rate " << _update_rate << std::endl; - std::cout<< "th_deadlock_time " << _th_deadlock_time << std::endl; + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_canless_mode = conf["use_canless_mode"].as(); + + subscribe_twist_topic_name = conf["twist_topic"].as(); + + _trajectory_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + + _trajectory_parameter.curve_judgment_threshold = conf["trajectory"]["curve_judgment_threshold"].as(); + _timer_updata_rate = conf["trajectory"]["timer_updata_rate"].as(); + _deadlock_threshold = conf["trajectory"]["deadlock_threshold"].as(); + + std::cout<< "use_canless_mode " << _use_canless_mode << std::endl; + std::cout<< "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + + std::cout << "stop_judgment_threshold " << _trajectory_parameter.stop_judgment_threshold << std::endl; + + std::cout << "curve_judgment_threshold " << _trajectory_parameter.curve_judgment_threshold << std::endl; + std::cout << "timer_updata_rate " << _timer_updata_rate << std::endl; + std::cout << "deadlock_threshold " << _deadlock_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mtrajectory Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + ros::Subscriber sub1 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe(subscribe_twist_topic_name, 1000, velocity_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub3 = nh.subscribe("velocity", 1000, correction_velocity_callback, ros::TransportHints().tcpNoDelay()); @@ -190,7 +210,7 @@ int main(int argc, char** argv) _pub2 = nh.advertise("enu_relative_pos", 1000); _pub3 = nh.advertise("twist", 1000); - ros::Timer timer = nh.createTimer(ros::Duration(1/_update_rate), timer_callback); + ros::Timer timer = nh.createTimer(ros::Duration(1/_timer_updata_rate), timer_callback); ros::spin(); diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index 17ceb940..58f26456 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -28,7 +28,10 @@ * Author MapIV Sekino */ -#include "ros/ros.h" +#include +#include +#include + #include "coordinate/coordinate.hpp" #include "navigation/navigation.hpp" @@ -52,6 +55,9 @@ bool _is_first_move = false; std::string _velocity_scale_factor_save_str; double _saved_vsf_estimater_number = 0; double _saved_velocity_scale_factor = 1.0; +double _previous_velocity_scale_factor = 1.0; + +double _th_velocity_scale_factor_percent = 20; void rtklib_nav_callback(const rtklib_msgs::RtklibNav::ConstPtr& msg) { @@ -67,7 +73,7 @@ void velocity_callback(const geometry_msgs::TwistStamped::ConstPtr& msg) { _velocity = *msg; - if (!_is_first_move && msg->twist.linear.x > _velocity_scale_factor_parameter.estimated_velocity_threshold) + if (!_is_first_move && msg->twist.linear.x > _velocity_scale_factor_parameter.moving_judgment_threshold) { _is_first_move = true; } @@ -95,8 +101,31 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) velocity_scale_factor_estimate(_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_status, &_correction_velocity, &_velocity_scale_factor); + + _velocity_scale_factor.status.is_abnormal = false; + if (!std::isfinite(_velocity_scale_factor.scale_factor)) { + _correction_velocity.twist.linear.x = _velocity.twist.linear.x * _previous_velocity_scale_factor; + _velocity_scale_factor.scale_factor = _previous_velocity_scale_factor; + ROS_WARN("Estimated velocity scale factor has NaN or infinity values."); + _velocity_scale_factor.status.is_abnormal = true; + _velocity_scale_factor.status.error_code = eagleye_msgs::Status::NAN_OR_INFINITE; + } + else if (_th_velocity_scale_factor_percent / 100 < std::abs(1.0 - _velocity_scale_factor.scale_factor)) + { + _correction_velocity.twist.linear.x = _velocity.twist.linear.x * _previous_velocity_scale_factor; + _velocity_scale_factor.scale_factor = _previous_velocity_scale_factor; + ROS_WARN("Estimated velocity scale factor is too large or too small."); + _velocity_scale_factor.status.is_abnormal = true; + _velocity_scale_factor.status.error_code = eagleye_msgs::Status::TOO_LARGE_OR_SMALL; + } + else + { + _previous_velocity_scale_factor = _velocity_scale_factor.scale_factor; + } + _pub1.publish(_correction_velocity); _pub2.publish(_velocity_scale_factor); + } void load_velocity_scale_factor(std::string txt_path) @@ -125,6 +154,7 @@ void load_velocity_scale_factor(std::string txt_path) _velocity_scale_factor.status.enabled_status = true; _velocity_scale_factor.scale_factor = _saved_velocity_scale_factor; std::cout<< "saved_velocity_scale_factor " << _saved_velocity_scale_factor << std::endl; + _previous_velocity_scale_factor = _saved_velocity_scale_factor; } count++; } @@ -163,35 +193,62 @@ int main(int argc, char** argv) std::string subscribe_twist_topic_name = "/can_twist"; std::string subscribe_imu_topic_name = "/imu/data_raw"; std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_rmc_topic_name = "/mosaic/rmc"; + std::string subscribe_rmc_topic_name = "navsat/rmc"; double velocity_scale_factor_save_duration; // [sec] - nh.getParam("twist_topic",subscribe_twist_topic_name); - nh.getParam("imu_topic" , subscribe_imu_topic_name); - nh.getParam("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - nh.getParam("rmc_topic",subscribe_rmc_topic_name); - nh.getParam("velocity_scale_factor/estimated_number_min",_velocity_scale_factor_parameter.estimated_number_min); - nh.getParam("velocity_scale_factor/estimated_number_max",_velocity_scale_factor_parameter.estimated_number_max); - nh.getParam("velocity_scale_factor/estimated_velocity_threshold",_velocity_scale_factor_parameter.estimated_velocity_threshold); - nh.getParam("velocity_scale_factor/estimated_coefficient",_velocity_scale_factor_parameter.estimated_coefficient); - nh.getParam("use_gnss_mode",_use_gnss_mode); - nh.getParam("velocity_scale_factor_save_str", _velocity_scale_factor_save_str); - nh.getParam("velocity_scale_factor/save_velocity_scale_factor", _velocity_scale_factor_parameter.save_velocity_scale_factor); - nh.getParam("velocity_scale_factor/velocity_scale_factor_save_duration", velocity_scale_factor_save_duration); - - std::cout<< "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; - std::cout<< "subscribe_imu_topic_name " << subscribe_imu_topic_name << std::endl; - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "subscribe_rmc_topic_name " << subscribe_rmc_topic_name << std::endl; - std::cout<< "estimated_number_min " << _velocity_scale_factor_parameter.estimated_number_min << std::endl; - std::cout<< "estimated_number_max " << _velocity_scale_factor_parameter.estimated_number_max << std::endl; - std::cout<< "estimated_velocity_threshold " << _velocity_scale_factor_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "estimated_coefficient " << _velocity_scale_factor_parameter.estimated_coefficient << std::endl; - std::cout<< "use_gnss_mode " << _use_gnss_mode << 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; - std::cout<< "velocity_scale_factor_save_duration " << velocity_scale_factor_save_duration << std::endl; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _use_gnss_mode = conf["use_gnss_mode"].as(); + + subscribe_imu_topic_name = conf["imu_topic"].as(); + subscribe_twist_topic_name = conf["twist_topic"].as(); + subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); + + _velocity_scale_factor_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _velocity_scale_factor_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _velocity_scale_factor_parameter.moving_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + + _velocity_scale_factor_parameter.estimated_minimum_interval = conf["velocity_scale_factor"]["estimated_minimum_interval"].as(); + _velocity_scale_factor_parameter.estimated_maximum_interval = conf["velocity_scale_factor"]["estimated_maximum_interval"].as(); + _velocity_scale_factor_parameter.gnss_receiving_threshold = conf["velocity_scale_factor"]["gnss_receiving_threshold"].as(); + + std::string package_path = ros::package::getPath("eagleye_rt"); + _velocity_scale_factor_save_str = package_path + conf["velocity_scale_factor"]["velocity_scale_factor_save_str"].as(); + velocity_scale_factor_save_duration = conf["velocity_scale_factor"]["velocity_scale_factor_save_duration"].as(); + _velocity_scale_factor_parameter.save_velocity_scale_factor = conf["velocity_scale_factor"]["save_velocity_scale_factor"].as(); + _th_velocity_scale_factor_percent = conf["velocity_scale_factor"]["th_velocity_scale_factor_percent"].as(); + + std::cout << "use_gnss_mode " << _use_gnss_mode << std::endl; + + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + std::cout << "subscribe_imu_topic_name " << subscribe_imu_topic_name << std::endl; + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + + std::cout << "imu_rate " << _velocity_scale_factor_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _velocity_scale_factor_parameter.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << _velocity_scale_factor_parameter.moving_judgment_threshold << std::endl; + + 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<< "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; + std::cout<< "velocity_scale_factor_save_duration " << velocity_scale_factor_save_duration << std::endl; + std::cout<< "th_velocity_scale_factor_percent "<<_th_velocity_scale_factor_percent<header; yawrate_offset_estimate(_velocity, _yawrate_offset_stop, _heading_interpolate, _imu, _yawrate_offset_parameter, &_yawrate_offset_status, &_yawrate_offset); + + _yawrate_offset.status.is_abnormal = false; + if (!std::isfinite(_yawrate_offset_stop.yawrate_offset)) { + ROS_WARN("Estimated velocity scale factor has NaN or infinity values."); + _yawrate_offset_stop.yawrate_offset =_previous_yawrate_offset; + _yawrate_offset.status.is_abnormal = true; + _yawrate_offset.status.error_code = eagleye_msgs::Status::NAN_OR_INFINITE; + } + else + { + _previous_yawrate_offset = _yawrate_offset_stop.yawrate_offset; + } _pub.publish(_yawrate_offset); _yawrate_offset.status.estimate_status = false; } @@ -88,35 +102,79 @@ int main(int argc, char** argv) ros::init(argc, argv, "yawrate_offset"); ros::NodeHandle nh; - nh.getParam("yawrate_offset/estimated_number_min", _yawrate_offset_parameter.estimated_number_min); - nh.getParam("yawrate_offset/estimated_coefficient", _yawrate_offset_parameter.estimated_coefficient); - nh.getParam("yawrate_offset/estimated_velocity_threshold" , _yawrate_offset_parameter.estimated_velocity_threshold); - nh.getParam("yawrate_offset/outlier_threshold", _yawrate_offset_parameter.outlier_threshold); - nh.getParam("use_canless_mode",_use_canless_mode); - - std::cout<< "estimated_number_min: " << _yawrate_offset_parameter.estimated_number_min << std::endl; - std::cout<< "estimated_coefficient: " << _yawrate_offset_parameter.estimated_coefficient << std::endl; - std::cout<< "estimated_velocity_threshold: " << _yawrate_offset_parameter.estimated_velocity_threshold << std::endl; - std::cout<< "outlier_threshold: " << _yawrate_offset_parameter.outlier_threshold << std::endl; - std::string publish_topic_name = "/publish_topic_name/invalid"; std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + if (argc == 2) { if (strcmp(argv[1], "1st") == 0) { publish_topic_name = "yawrate_offset_1st"; subscribe_topic_name = "heading_interpolate_1st"; - nh.getParam("yawrate_offset/1st/estimated_number_max", _yawrate_offset_parameter.estimated_number_max); - std::cout<< "estimated_number_max: " << _yawrate_offset_parameter.estimated_number_max << std::endl; + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _yawrate_offset_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _yawrate_offset_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _yawrate_offset_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + + _yawrate_offset_parameter.estimated_minimum_interval = conf["yawrate_offset"]["estimated_minimum_interval"].as(); + _yawrate_offset_parameter.estimated_maximum_interval = conf["yawrate_offset"]["1st"]["estimated_maximum_interval"].as(); + _yawrate_offset_parameter.gnss_receiving_threshold = conf["yawrate_offset"]["gnss_receiving_threshold"].as(); + _yawrate_offset_parameter.outlier_threshold = conf["yawrate_offset_stop"]["outlier_threshold"].as(); + + std::cout << "imu_rate " << _yawrate_offset_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _yawrate_offset_parameter.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << _yawrate_offset_parameter.moving_judgment_threshold << std::endl; + + std::cout << "estimated_minimum_interval " << _yawrate_offset_parameter.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " << _yawrate_offset_parameter.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " << _yawrate_offset_parameter.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << _yawrate_offset_parameter.outlier_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;yawrate_offset_1st Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } } else if (strcmp(argv[1], "2nd") == 0) { publish_topic_name = "yawrate_offset_2nd"; subscribe_topic_name = "heading_interpolate_2nd"; - nh.getParam("yawrate_offset/2nd/estimated_number_max", _yawrate_offset_parameter.estimated_number_max); - std::cout<< "estimated_number_max: " << _yawrate_offset_parameter.estimated_number_max << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + _yawrate_offset_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _yawrate_offset_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); + _yawrate_offset_parameter.moving_judgment_threshold = conf["common"]["moving_judgment_threshold"].as(); + + _yawrate_offset_parameter.estimated_minimum_interval = conf["yawrate_offset"]["estimated_minimum_interval"].as(); + _yawrate_offset_parameter.estimated_maximum_interval = conf["yawrate_offset"]["2nd"]["estimated_maximum_interval"].as(); + _yawrate_offset_parameter.gnss_receiving_threshold = conf["yawrate_offset"]["gnss_receiving_threshold"].as(); + _yawrate_offset_parameter.outlier_threshold = conf["yawrate_offset_stop"]["outlier_threshold"].as(); + + std::cout << "imu_rate " << _yawrate_offset_parameter.imu_rate << std::endl; + std::cout << "gnss_rate " << _yawrate_offset_parameter.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << _yawrate_offset_parameter.moving_judgment_threshold << std::endl; + + std::cout << "estimated_minimum_interval " << _yawrate_offset_parameter.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " << _yawrate_offset_parameter.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " << _yawrate_offset_parameter.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << _yawrate_offset_parameter.outlier_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;yawrate_offset_2nd Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } } else { diff --git a/eagleye_rt/src/yawrate_offset_stop_node.cpp b/eagleye_rt/src/yawrate_offset_stop_node.cpp index 491c3e24..eb8ac503 100644 --- a/eagleye_rt/src/yawrate_offset_stop_node.cpp +++ b/eagleye_rt/src/yawrate_offset_stop_node.cpp @@ -40,6 +40,8 @@ static sensor_msgs::Imu _imu; struct YawrateOffsetStopParameter _yawrate_offset_stop_parameter; struct YawrateOffsetStopStatus _yawrate_offset_stop_status; +double _previous_yawrate_offset_stop = 0.0; + void velocity_callback(const geometry_msgs::TwistStamped::ConstPtr& msg) { _velocity = *msg; @@ -50,6 +52,18 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) _imu = *msg; _yawrate_offset_stop.header = msg->header; yawrate_offset_stop_estimate(_velocity, _imu, _yawrate_offset_stop_parameter, &_yawrate_offset_stop_status, &_yawrate_offset_stop); + + _yawrate_offset_stop.status.is_abnormal = false; + if (!std::isfinite(_yawrate_offset_stop.yawrate_offset)) { + ROS_WARN("Estimated velocity scale factor has NaN or infinity values."); + _yawrate_offset_stop.yawrate_offset =_previous_yawrate_offset_stop; + _yawrate_offset_stop.status.is_abnormal = true; + _yawrate_offset_stop.status.error_code = eagleye_msgs::Status::NAN_OR_INFINITE; + } + else + { + _previous_yawrate_offset_stop = _yawrate_offset_stop.yawrate_offset; + } _pub.publish(_yawrate_offset_stop); } @@ -60,15 +74,35 @@ int main(int argc, char** argv) std::string subscribe_twist_topic_name = "/can_twist"; - nh.getParam("twist_topic", subscribe_twist_topic_name); - nh.getParam("yawrate_offset_stop/stop_judgment_velocity_threshold", _yawrate_offset_stop_parameter.stop_judgment_velocity_threshold); - nh.getParam("yawrate_offset_stop/estimated_number" , _yawrate_offset_stop_parameter.estimated_number); - nh.getParam("yawrate_offset_stop/outlier_threshold" , _yawrate_offset_stop_parameter.outlier_threshold); + std::string yaml_file; + nh.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + subscribe_twist_topic_name = conf["twist_topic"].as(); + + _yawrate_offset_stop_parameter.imu_rate = conf["common"]["imu_rate"].as(); + _yawrate_offset_stop_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + + _yawrate_offset_stop_parameter.estimated_interval = conf["yawrate_offset_stop"]["estimated_interval"].as(); + _yawrate_offset_stop_parameter.outlier_threshold = conf["yawrate_offset_stop"]["outlier_threshold"].as(); + + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + + std::cout << "imu_rate " << _yawrate_offset_stop_parameter.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << _yawrate_offset_stop_parameter.stop_judgment_threshold << std::endl; - std::cout<< "subscribe_twist_topic_name: " << subscribe_twist_topic_name << std::endl; - std::cout<< "stop_judgment_velocity_threshold: " << _yawrate_offset_stop_parameter.stop_judgment_velocity_threshold << std::endl; - std::cout<< "estimated_number: " << _yawrate_offset_stop_parameter.estimated_number << std::endl; - std::cout<< "outlier_threshold: " << _yawrate_offset_stop_parameter.outlier_threshold << std::endl; + std::cout << "estimated_minimum_interval " << _yawrate_offset_stop_parameter.estimated_interval << std::endl; + std::cout << "outlier_threshold " << _yawrate_offset_stop_parameter.outlier_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31myawrate_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } ros::Subscriber sub1 = nh.subscribe(subscribe_twist_topic_name, 1000, velocity_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub2 = nh.subscribe("imu/data_tf_converted", 1000, imu_callback, ros::TransportHints().tcpNoDelay()); diff --git a/eagleye_util/fix2pose/CMakeLists.txt b/eagleye_util/fix2pose/CMakeLists.txt index fa772894..fc2ec47a 100644 --- a/eagleye_util/fix2pose/CMakeLists.txt +++ b/eagleye_util/fix2pose/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs tf + tf2_ros eagleye_coordinate ) diff --git a/eagleye_util/fix2pose/launch/fix2pose.launch b/eagleye_util/fix2pose/launch/fix2pose.launch index f6431887..61c20f76 100644 --- a/eagleye_util/fix2pose/launch/fix2pose.launch +++ b/eagleye_util/fix2pose/launch/fix2pose.launch @@ -10,17 +10,24 @@ + + - + + + + + - + + diff --git a/eagleye_util/fix2pose/package.xml b/eagleye_util/fix2pose/package.xml index 82afd1b7..a2adbc36 100644 --- a/eagleye_util/fix2pose/package.xml +++ b/eagleye_util/fix2pose/package.xml @@ -17,6 +17,7 @@ eagleye_navigation tf eagleye_coordinate + tf2_ros roscpp rospy std_msgs @@ -25,6 +26,7 @@ eagleye_navigation tf eagleye_coordinate + tf2_ros roscpp rospy std_msgs @@ -33,6 +35,7 @@ eagleye_navigation tf eagleye_coordinate + tf2_ros diff --git a/eagleye_util/fix2pose/src/fix2pose.cpp b/eagleye_util/fix2pose/src/fix2pose.cpp index 6dc6f091..b07fe2f7 100644 --- a/eagleye_util/fix2pose/src/fix2pose.cpp +++ b/eagleye_util/fix2pose/src/fix2pose.cpp @@ -39,9 +39,13 @@ #include "tf/transform_broadcaster.h" #include "coordinate/coordinate.hpp" +#include +#include +#include + static eagleye_msgs::Rolling _eagleye_rolling; static eagleye_msgs::Pitching _eagleye_pitching; -static eagleye_msgs::Heading _eagleye_heading; +eagleye_msgs::Heading::ConstPtr _eagleye_heading_ptr; static geometry_msgs::Quaternion _quat; static ros::Publisher _pose_pub, _pose_with_covariance_pub; @@ -51,13 +55,15 @@ static geometry_msgs::PoseWithCovarianceStamped _pose_with_covariance; static int _convert_height_num = 0; static int _plane = 7; static int _tf_num = 1; -static std::string _parent_frame_id, _child_frame_id; +static std::string _parent_frame_id, _child_frame_id, _base_link_frame_id, _gnss_frame_id; static ConvertHeight _convert_height; +bool _fix_only_publish = false; + void heading_callback(const eagleye_msgs::Heading::ConstPtr& msg) { - _eagleye_heading = *msg; + _eagleye_heading_ptr = msg; } void rolling_callback(const eagleye_msgs::Rolling::ConstPtr& msg) @@ -70,8 +76,12 @@ void pitching_callback(const eagleye_msgs::Pitching::ConstPtr& msg) _eagleye_pitching = *msg; } -void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) +void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg, tf2_ros::TransformListener* tf_listener, tf2_ros::Buffer* tf_buffer) { + if(_fix_only_publish && msg->status.status != 0) + { + return; + } double llh[3] = {0}; double xyz[3] = {0}; @@ -101,11 +111,12 @@ void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) ll2xy_mgrs(llh, xyz); } - if (_eagleye_heading.status.enabled_status == true) + double eagleye_heading = 0; + if (_eagleye_heading_ptr != nullptr) { - _eagleye_heading.heading_angle = fmod(_eagleye_heading.heading_angle, 2*M_PI); + eagleye_heading = fmod(_eagleye_heading_ptr->heading_angle, 2*M_PI); tf::Quaternion tf_quat = tf::createQuaternionFromRPY(_eagleye_rolling.rolling_angle, - _eagleye_pitching.pitching_angle, (90* M_PI / 180) - _eagleye_heading.heading_angle); + _eagleye_pitching.pitching_angle, (90* M_PI / 180) - eagleye_heading); quaternionTFToMsg(tf_quat, _quat); } else @@ -119,6 +130,30 @@ void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) _pose.pose.position.y = xyz[0]; _pose.pose.position.z = xyz[2]; _pose.pose.orientation = _quat; + + geometry_msgs::PoseStamped::Ptr transformed_pose_msg_ptr( + new geometry_msgs::PoseStamped); + + if(_fix_only_publish) + { + geometry_msgs::TransformStamped::Ptr TF_sensor_to_base_ptr(new geometry_msgs::TransformStamped); + try + { + *TF_sensor_to_base_ptr = tf_buffer->lookupTransform(_base_link_frame_id, _gnss_frame_id, ros::Time(0)); + + tf2::doTransform(_pose, *transformed_pose_msg_ptr, *TF_sensor_to_base_ptr); + std::string map_frame = "map"; + transformed_pose_msg_ptr->header = _pose.header; + transformed_pose_msg_ptr->header.frame_id = _parent_frame_id; + _pose = *transformed_pose_msg_ptr; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + } + _pose_pub.publish(_pose); _pose_with_covariance.header = _pose.header; @@ -129,7 +164,7 @@ void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) double std_dev_yaw = 100; // [rad] if(_eagleye_rolling.status.enabled_status) std_dev_roll = 0.5 / 180 * M_PI; if(_eagleye_pitching.status.enabled_status) std_dev_pitch = 0.5 / 180 * M_PI; - if(_eagleye_heading.status.enabled_status) std_dev_yaw = 0.2 / 180 * M_PI; + if(_eagleye_heading_ptr != nullptr && _eagleye_heading_ptr->status.enabled_status) std_dev_yaw = 0.2 / 180 * M_PI; _pose_with_covariance.pose.covariance[0] = msg->position_covariance[0]; _pose_with_covariance.pose.covariance[7] = msg->position_covariance[4]; _pose_with_covariance.pose.covariance[14] = msg->position_covariance[8]; @@ -141,8 +176,9 @@ void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; + if(_eagleye_heading_ptr != nullptr) eagleye_heading = _eagleye_heading_ptr->heading_angle; transform.setOrigin(tf::Vector3(_pose.pose.position.x, _pose.pose.position.y, _pose.pose.position.z)); - q.setRPY(0, 0, (90* M_PI / 180) - _eagleye_heading.heading_angle); + q.setRPY(0, 0, (90* M_PI / 180) - eagleye_heading); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, msg->header.stamp, _parent_frame_id, _child_frame_id)); } @@ -152,24 +188,42 @@ int main(int argc, char** argv) ros::init(argc, argv, "fix2pose"); ros::NodeHandle nh; + nh.getParam("fix2pose_node/plane", _plane); nh.getParam("fix2pose_node/plane", _plane); nh.getParam("fix2pose_node/tf_num", _tf_num); nh.getParam("fix2pose_node/convert_height_num", _convert_height_num); nh.getParam("fix2pose_node/parent_frame_id", _parent_frame_id); nh.getParam("fix2pose_node/child_frame_id", _child_frame_id); + nh.getParam("fix2pose_node/fix_only_publish", _fix_only_publish); + nh.getParam("fix2pose_node/base_link_frame_id", _base_link_frame_id); + nh.getParam("fix2pose_node/gnss_frame_id", _gnss_frame_id); std::cout<< "plane " << _plane << std::endl; std::cout<< "tf_num " << _tf_num << std::endl; std::cout<< "convert_height_num " << _convert_height_num << std::endl; std::cout<< "parent_frame_id " << _parent_frame_id << std::endl; std::cout<< "child_frame_id " << _child_frame_id << std::endl; + std::cout<< "fix_only_publish " << _fix_only_publish << std::endl; + std::cout<< "base_link_frame_id " << _base_link_frame_id << std::endl; + std::cout<< "gnss_frame_id " << _gnss_frame_id << std::endl; + + std::string fix_name; + if(!_fix_only_publish) + { + fix_name = "eagleye/fix"; + } + else { + fix_name = "navsat/fix"; + } + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener(tf_buffer); ros::Subscriber sub1 = nh.subscribe("eagleye/heading_interpolate_3rd", 1000, heading_callback); - ros::Subscriber sub2 = nh.subscribe("eagleye/fix", 1000, fix_callback); + ros::Subscriber sub2 = nh.subscribe(fix_name, 1000, boost::bind(fix_callback,_1, &tf_listener, &tf_buffer)); ros::Subscriber sub3 = nh.subscribe("eagleye/rolling", 1000, rolling_callback); ros::Subscriber sub4 = nh.subscribe("eagleye/pitching", 1000, pitching_callback); - _pose_pub = nh.advertise("/eagleye/pose", 1000); - _pose_with_covariance_pub = nh.advertise("/eagleye/pose_with_covariance", 1000); + _pose_pub = nh.advertise("eagleye/pose", 1000); + _pose_with_covariance_pub = nh.advertise("eagleye/pose_with_covariance", 1000); ros::spin(); return 0; diff --git a/eagleye_util/kml_generator b/eagleye_util/kml_generator index 2edbdef0..4f765529 160000 --- a/eagleye_util/kml_generator +++ b/eagleye_util/kml_generator @@ -1 +1 @@ -Subproject commit 2edbdef0a98415ad8f27f9c6c8690b81b7954fa3 +Subproject commit 4f76552986bee00c0a3456c5010187681dada7eb diff --git a/eagleye_util/nmea2fix/CMakeLists.txt b/eagleye_util/nmea2fix/CMakeLists.txt index 6d25b599..5762a1d8 100644 --- a/eagleye_util/nmea2fix/CMakeLists.txt +++ b/eagleye_util/nmea2fix/CMakeLists.txt @@ -24,8 +24,8 @@ add_library(nmea2fix src/nmea2fix_core.cpp ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/nmea2fix/ + DESTINATION include/nmea2fix/ FILES_MATCHING PATTERN "*.hpp" ) @@ -44,7 +44,7 @@ find_package(catkin REQUIRED COMPONENTS ) add_executable(nmea2fix_node src/nmea2fix_node.cpp) -target_link_libraries(nmea2fix_node ${catkin_LIBRARIES} ) +target_link_libraries(nmea2fix_node ${catkin_LIBRARIES}) add_dependencies(nmea2fix_node ${catkin_EXPORTED_TARGETS}) install(TARGETS diff --git a/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp b/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp index 02acf10c..be1adbae 100644 --- a/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp +++ b/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp @@ -32,8 +32,10 @@ #include "nmea_msgs/Gprmc.h" #include +#include #include #include +#include extern double stringToGPSTime(std::string&, double); extern void nmea2fix_converter(const nmea_msgs::Sentence, sensor_msgs::NavSatFix*, nmea_msgs::Gpgga*, nmea_msgs::Gprmc*); diff --git a/eagleye_util/nmea2fix/launch/nmea2fix.launch b/eagleye_util/nmea2fix/launch/nmea2fix.launch index 344870f3..25910e18 100644 --- a/eagleye_util/nmea2fix/launch/nmea2fix.launch +++ b/eagleye_util/nmea2fix/launch/nmea2fix.launch @@ -8,7 +8,7 @@ - + diff --git a/eagleye_util/nmea2fix/launch/nmea2fix_dualantenna.launch b/eagleye_util/nmea2fix/launch/nmea2fix_dualantenna.launch new file mode 100644 index 00000000..c490b9dd --- /dev/null +++ b/eagleye_util/nmea2fix/launch/nmea2fix_dualantenna.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/eagleye_util/nmea2fix/src/nmea2fix_node.cpp b/eagleye_util/nmea2fix/src/nmea2fix_node.cpp index ab8e65a0..a92e341a 100644 --- a/eagleye_util/nmea2fix/src/nmea2fix_node.cpp +++ b/eagleye_util/nmea2fix/src/nmea2fix_node.cpp @@ -37,12 +37,17 @@ int main(int argc, char** argv) std::string use_gnss_mode; - n.getParam("nmea_sentence_topic",sub_topic_name); - n.getParam("nmea2fix_node/pub_fix_topic_name",pub_fix_topic_name); - n.getParam("nmea2fix_node/pub_gga_topic_name",pub_gga_topic_name); - n.getParam("nmea2fix_node/pub_rmc_topic_name",pub_rmc_topic_name); - n.getParam("nmea2fix_node/output_gga",output_gga); - n.getParam("nmea2fix_node/output_rmc",output_rmc); + std::string node_name = ros::this_node::getName(); + + if(!n.getParam("nmea_sentence_topic",sub_topic_name)) + { + n.getParam(node_name + "/nmea_sentence_topic",sub_topic_name); + } + n.getParam(node_name + "/pub_fix_topic_name",pub_fix_topic_name); + n.getParam(node_name + "/pub_gga_topic_name",pub_gga_topic_name); + n.getParam(node_name + "/pub_rmc_topic_name",pub_rmc_topic_name); + n.getParam(node_name + "/output_gga",output_gga); + n.getParam(node_name + "/output_rmc",output_rmc); n.getParam("eagleye/use_gnss_mode",use_gnss_mode); if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode