Make test codes work. Create a threshold structure so that other packages can use the methods in pose_instability_detector

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

Author: TaikiYamada4

localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp

@@ -24,6 +24,7 @@
 
 #include <deque>
 #include <vector>
+#include <tuple>
 
 class PoseInstabilityDetector : public rclcpp::Node
 {
@@ -38,17 +39,26 @@ class PoseInstabilityDetector : public rclcpp::Node
   using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
 
 public:
+  struct ThresholdValues {
+    double position_x;
+    double position_y;
+    double position_z;
+    double angle_x;
+    double angle_y;
+    double angle_z;
+  };
+  
   explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ThresholdValues calculate_threshold(double interval_sec);
+  void dead_reckon(
+    PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time,
+    const std::deque<TwistWithCovarianceStamped> & twist_deque, Pose::SharedPtr & estimated_pose);
 
 private:
   void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr);
   void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr);
   void callback_timer();
 
-  void calculate_threshold(double interval_sec);
-  void dead_reckon(
-    PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time,
-    const std::deque<TwistWithCovarianceStamped> & twist_deque, Pose::SharedPtr & estimated_pose);
   std::deque<TwistWithCovarianceStamped> clip_out_necessary_twist(
     const std::deque<TwistWithCovarianceStamped> & twist_buffer, const rclcpp::Time & start_time,
     const rclcpp::Time & end_time);
@@ -65,12 +75,7 @@ class PoseInstabilityDetector : public rclcpp::Node
   // parameters
   const double timer_period_;  // [sec]
 
-  double threshold_diff_position_x_;  // longitudinal
-  double threshold_diff_position_y_;  // lateral
-  double threshold_diff_position_z_;  // vertical
-  double threshold_diff_angle_x_;     // roll
-  double threshold_diff_angle_y_;     // pitch
-  double threshold_diff_angle_z_;     // yaw
+  ThresholdValues threshold_values_;
 
   const double heading_velocity_maximum_;                 // [m/s]
   const double heading_velocity_scale_factor_tolerance_;  // [%]

localization/pose_instability_detector/src/pose_instability_detector.cpp

@@ -136,11 +136,11 @@ void PoseInstabilityDetector::callback_timer()
   diff_pose_pub_->publish(diff_pose);
 
   // publish diagnostics
-  calculate_threshold((latest_odometry_time - prev_odometry_time).seconds());
+  ThresholdValues threshold_values = calculate_threshold((latest_odometry_time - prev_odometry_time).seconds());
 
-  const std::vector<double> thresholds = {threshold_diff_position_x_, threshold_diff_position_y_,
-                                          threshold_diff_position_z_, threshold_diff_angle_x_,
-                                          threshold_diff_angle_y_,    threshold_diff_angle_z_};
+  const std::vector<double> thresholds = {threshold_values.position_x, threshold_values.position_y,
+                                          threshold_values.position_z, threshold_values.angle_x,
+                                          threshold_values.angle_y,    threshold_values.angle_z};
 
   const std::vector<std::string> labels = {"diff_position_x", "diff_position_y", "diff_position_z",
                                            "diff_angle_x",    "diff_angle_y",    "diff_angle_z"};
@@ -176,7 +176,7 @@ void PoseInstabilityDetector::callback_timer()
   prev_odometry_ = latest_odometry_;
 }
 
-void PoseInstabilityDetector::calculate_threshold(double interval_sec)
+PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold(double interval_sec)
 {
   // Calculate maximum longitudinal difference
   const double longitudinal_difference =
@@ -227,12 +227,15 @@ void PoseInstabilityDetector::calculate_threshold(double interval_sec)
   const double yaw_difference = roll_difference;
 
   // Set thresholds
-  threshold_diff_position_x_ = longitudinal_difference + pose_estimator_longitudinal_tolerance_;
-  threshold_diff_position_y_ = lateral_difference + pose_estimator_lateral_tolerance_;
-  threshold_diff_position_z_ = vertical_difference + pose_estimator_vertical_tolerance_;
-  threshold_diff_angle_x_ = roll_difference + pose_estimator_angular_tolerance_;
-  threshold_diff_angle_y_ = pitch_difference + pose_estimator_angular_tolerance_;
-  threshold_diff_angle_z_ = yaw_difference + pose_estimator_angular_tolerance_;
+  ThresholdValues result_values;
+  result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_;
+  result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_;
+  result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_;
+  result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_;
+  result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_;
+  result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_;
+
+  return result_values;
 }
 
 void PoseInstabilityDetector::dead_reckon(
@@ -302,9 +305,25 @@ PoseInstabilityDetector::clip_out_necessary_twist(
   const std::deque<TwistWithCovarianceStamped> & twist_buffer, const rclcpp::Time & start_time,
   const rclcpp::Time & end_time)
 {
+  // If there is only one element in the twist_buffer, return a deque that has the same twist
+  // from the start till the end
+  if (twist_buffer.size() == 1) {
+    TwistWithCovarianceStamped twist = twist_buffer.front();
+    std::deque<TwistWithCovarianceStamped> simple_twist_deque;
+
+    twist.header.stamp = start_time;
+    simple_twist_deque.push_back(twist);
+
+    twist.header.stamp = end_time;
+    simple_twist_deque.push_back(twist);
+
+    return simple_twist_deque;
+  }
+
   // get iterator to the element that is right before start_time (if it does not exist, start_it =
   // twist_buffer.begin())
   auto start_it = twist_buffer.begin();
+  
   for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) {
     if (rclcpp::Time(it->header.stamp) > start_time) {
       break;
@@ -326,21 +345,6 @@ PoseInstabilityDetector::clip_out_necessary_twist(
   // Create result deque
   std::deque<TwistWithCovarianceStamped> result_deque(start_it, end_it);
 
-  // If the result deque has only one element, return a deque that starts and ends with the same
-  // element
-  if (result_deque.size() == 1) {
-    TwistWithCovarianceStamped twist = *start_it;
-    result_deque.clear();
-
-    twist.header.stamp = start_time;
-    result_deque.push_back(twist);
-
-    twist.header.stamp = end_time;
-    result_deque.push_back(twist);
-
-    return result_deque;
-  }
-
   // If the first element is later than start_time, add the first element to the front of the
   // result_deque
   if (rclcpp::Time(result_deque.front().header.stamp) > start_time) {
@@ -395,7 +399,6 @@ PoseInstabilityDetector::clip_out_necessary_twist(
 
     result_deque[result_deque.size() - 1].header.stamp = end_time;
   }
-
   return result_deque;
 }
 

localization/pose_instability_detector/test/test.cpp

@@ -81,18 +81,18 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry)  // N
   timestamp.nanosec = 0;
   helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0);
 
+  // send the twist message1 (move 1m in x direction)
+  timestamp.sec = 0;
+  timestamp.nanosec = 5e8;
+  helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0);
+
   // process the above message (by timer_callback)
   helper_->received_diagnostic_array_flag = false;
   while (!helper_->received_diagnostic_array_flag) {
     executor_.spin_some();
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
   }
 
-  // send the twist message1 (move 1m in x direction)
-  timestamp.sec = 0;
-  timestamp.nanosec = 5e8;
-  helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0);
-
   // send the twist message2 (move 1m in x direction)
   timestamp.sec = 1;
   timestamp.nanosec = 0;
@@ -101,7 +101,9 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry)  // N
   // send the second odometry message (finish x = 12)
   timestamp.sec = 2;
   timestamp.nanosec = 0;
-  helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0);
+  helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0);
+
+  executor_.spin_some();
 
   // process the above messages (by timer_callback)
   helper_->received_diagnostic_array_flag = false;
@@ -124,18 +126,18 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small)  // NOL
   timestamp.nanosec = 0;
   helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0);
 
+  // send the twist message1 (move 0.1m in x direction)
+  timestamp.sec = 0;
+  timestamp.nanosec = 5e8;
+  helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0);
+
   // process the above message (by timer_callback)
   helper_->received_diagnostic_array_flag = false;
   while (!helper_->received_diagnostic_array_flag) {
     executor_.spin_some();
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
   }
 
-  // send the twist message1 (move 0.1m in x direction)
-  timestamp.sec = 0;
-  timestamp.nanosec = 5e8;
-  helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0);
-
   // send the twist message2 (move 0.1m in x direction)
   timestamp.sec = 1;
   timestamp.nanosec = 0;
@@ -144,7 +146,9 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small)  // NOL
   // send the second odometry message (finish x = 12)
   timestamp.sec = 2;
   timestamp.nanosec = 0;
-  helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0);
+  helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0);
+
+  executor_.spin_some();
 
   // process the above messages (by timer_callback)
   helper_->received_diagnostic_array_flag = false;