Add use_beseline_outlier_detection_ param

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.

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_;
 };

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_);