diff --git a/gnss_compass/config/gnss_compass.yaml b/gnss_compass/config/gnss_compass.yaml index f97ae8b..5b75e7d 100644 --- a/gnss_compass/config/gnss_compass.yaml +++ b/gnss_compass/config/gnss_compass.yaml @@ -9,7 +9,7 @@ plane_num: 7 # Whether sensor_frame name is specified in yaml or not use_change_of_sensor_frame: true # If use_change_of_sensor_frame is true, the following is used for sensor_frame to base_link tf conversion. -sensor_frame: "gnss_link" +sensor_frame: "gnss" # Main gnss antenna data acquisition cycle # If the time difference between main and sub is 1/gnss_frequency[sec], no calculation is performed. gnss_frequency : 50.0 @@ -22,9 +22,10 @@ time_thresshold : 0.03 yaw_bias: 0.0 # Minimize number of ROS_WARN use_simple_roswarn : true +use_beseline_outlier_detection : false # Distance between antennas in three dimensions [m] beseline_length: 1.30 -# If the calculated baseline length has an error greater than or equal to this threshold, +# If the calculated baseline length has an error greater than or equal to this threshold, # it is determined to be a missed FIX [m] allowable_beseline_length_error: 0.05 # If the number of times that pose is not calculated consecutively exceeds this value, an error is output by diagnostics. diff --git a/gnss_compass/include/gnss_compass_core/gnss_compass_core.h b/gnss_compass/include/gnss_compass_core/gnss_compass_core.h index 4ace9a9..e3f9695 100644 --- a/gnss_compass/include/gnss_compass_core/gnss_compass_core.h +++ b/gnss_compass/include/gnss_compass_core/gnss_compass_core.h @@ -89,6 +89,7 @@ class GnssCompass double yaw_bias_; bool use_simple_roswarn_; double beseline_length_; + bool use_beseline_outlier_detection_; double allowable_beseline_length_error_; int max_skipping_publish_num_; }; diff --git a/gnss_compass/src/gnss_compass_core.cpp b/gnss_compass/src/gnss_compass_core.cpp index 1cef006..d6a1127 100644 --- a/gnss_compass/src/gnss_compass_core.cpp +++ b/gnss_compass/src/gnss_compass_core.cpp @@ -17,6 +17,7 @@ GnssCompass::GnssCompass(ros::NodeHandle nh, ros::NodeHandle private_nh) private_nh_.getParam("time_thresshold", time_thresshold_); private_nh_.getParam("yaw_bias", yaw_bias_); private_nh_.getParam("use_simple_roswarn", use_simple_roswarn_); + private_nh_.getParam("use_beseline_outlier_detection", use_beseline_outlier_detection_); private_nh_.getParam("beseline_length", beseline_length_); private_nh_.getParam("allowable_beseline_length_error", allowable_beseline_length_error_); private_nh_.getParam("max_skipping_publish_num", max_skipping_publish_num_); @@ -209,16 +210,19 @@ void GnssCompass::callbackSubGga(const nmea_msgs::Gpgga::ConstPtr & subgga_msg_p double baseline_length = std::sqrt(pow(diff_x, 2) + pow(diff_y, 2) + pow(diff_z, 2)); bool is_beseline_ok = (beseline_length_ - allowable_beseline_length_error_ <= baseline_length && baseline_length <= beseline_length_ + allowable_beseline_length_error_); - if(!is_beseline_ok) + if(use_beseline_outlier_detection_) { - ROS_WARN("mayby mis-FIX:l %lf, yaw,%lf, dt %lf", baseline_length, theta * 180 / M_PI, dt_ms); - illigal_odom_pub_.publish(odom_msg_); - return; - } - else { - ROS_INFO("normal :l %lf, yaw %lf, dt %lf", baseline_length, theta * 180 / M_PI, dt_ms); + if(!is_beseline_ok) + { + ROS_WARN("mayby mis-FIX:l %lf, yaw,%lf, dt %lf", baseline_length, theta * 180 / M_PI, dt_ms); + illigal_odom_pub_.publish(odom_msg_); + return; + } + else { + ROS_INFO("normal :l %lf, yaw %lf, dt %lf", baseline_length, theta * 180 / M_PI, dt_ms); + } } - + pose_pub_.publish(transformed_pose_msg_ptr); odom_pub_.publish(odom_msg_);