Merge branch 'feature/vel_pose_diff_checker/apply_health_checker' int…

…o 'master'

Apply health checker to vel_pose_diff_checker

See merge request autowarefoundation/autoware.ai/core_perception!69

vel_pose_diff_checker/CMakeLists.txt

@@ -8,7 +8,9 @@ endif()
 
 find_package(catkin REQUIRED COMPONENTS
   amathutils_lib
+  autoware_health_checker
   autoware_msgs
+  autoware_system_msgs
   geometry_msgs
   roscpp
   roslint
@@ -39,7 +41,7 @@ add_dependencies(${PROJECT_NAME}
 )
 
 target_link_libraries(${PROJECT_NAME}
- ${catkin_LIBRARIES}
+  ${catkin_LIBRARIES}
 )
 
 install(TARGETS ${PROJECT_NAME}
@@ -53,6 +55,11 @@ install(DIRECTORY launch/
   PATTERN ".svn" EXCLUDE
 )
 
+install(DIRECTORY config/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+  PATTERN ".svn" EXCLUDE
+)
+
 set(ROSLINT_CPP_OPTS "--filter=-build/c++14")
 roslint_cpp()
 
@@ -62,8 +69,8 @@ if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest_gtest(
     vel_pose_diff_checker-test
-    test/test_vel_pose_diff_checker.test 
-    test/src/test_vel_pose_diff_checker.cpp 
+    test/test_vel_pose_diff_checker.test
+    test/src/test_vel_pose_diff_checker.cpp
     src/vel_pose_diff_checker_core.cpp
     src/value_time_queue.cpp
   )

vel_pose_diff_checker/config/health_checker.yaml

@@ -0,0 +1,13 @@
+health_checker:
+  vel_pose_diff_checker_diff_pose_topic_time:
+    default
+  vel_pose_diff_checker_diff_twist_topic_time:
+    default
+  vel_pose_diff_checker_diff_position:
+    default
+  vel_pose_diff_checker_diff_position_median:
+    default
+  vel_pose_diff_checker_diff_angle:
+    default
+  vel_pose_diff_checker_diff_angle_median:
+    default

vel_pose_diff_checker/include/vel_pose_diff_checker/value_time_queue.h

@@ -25,26 +25,24 @@ class ValueTimeQueue
 {
   struct ValueTime
   {
-    ValueTime(const double a_value, const ros::Time &a_stamp)
-      : value(a_value)
-      , stamp(a_stamp)
+    ValueTime(const double a_value, const ros::Time& a_stamp) : value(a_value), stamp(a_stamp)
     {
     }
 
     double value;
     ros::Time stamp;
   };
 
-  public:
-    ValueTimeQueue();
-    explicit ValueTimeQueue(const double window_size_sec);
-    void addValueTime(const double value, const ros::Time &a_stamp);
-    double getMedianValue() const;
-    void setWindowSizeSec(const double time_window_size_sec);
+public:
+  ValueTimeQueue();
+  explicit ValueTimeQueue(const double window_size_sec);
+  void addValueTime(const double value, const ros::Time& a_stamp);
+  double getMedianValue() const;
+  void setWindowSizeSec(const double time_window_size_sec);
 
-  private:
-    double window_size_sec_;
-    std::deque<ValueTime> queue_;
+private:
+  double window_size_sec_;
+  std::deque<ValueTime> queue_;
 };
 
 #endif  // VEL_POSE_DIFF_CHECKER_VALUE_TIME_QUEUE_H

vel_pose_diff_checker/include/vel_pose_diff_checker/vel_pose_diff_checker_core.h

@@ -17,61 +17,92 @@
 #ifndef VEL_POSE_DIFF_CHECKER_VEL_POSE_DIFF_CHECKER_CORE_H
 #define VEL_POSE_DIFF_CHECKER_VEL_POSE_DIFF_CHECKER_CORE_H
 
+#include <string>
 #include <deque>
+#include <utility>
 
 #include <ros/ros.h>
+#include <tf/tf.h>
 
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 
-#include "vel_pose_diff_checker/value_time_queue.h"
+#include <autoware_health_checker/health_checker/health_checker.h>
 
+#include "vel_pose_diff_checker/value_time_queue.h"
 
 class VelPoseDiffChecker
 {
   friend class VelPoseDiffCheckerTestSuite;
-
-  public:
-    enum class Status { ERROR = 0, OK = 1, INVALID = 2 };
-
-    VelPoseDiffChecker(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
-    void run();
-
-  private:
-    void callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg);
-    void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& msg);
-    VelPoseDiffChecker::Status checkDiff();
-
-    ros::NodeHandle nh_;
-    ros::NodeHandle private_nh_;
-
-    ros::Subscriber pose_sub_;
-    ros::Subscriber twist_sub_;
-
-    ros::Publisher diff_position_pub_;
-    ros::Publisher diff_position_median_pub_;
-    ros::Publisher diff_angle_rad_pub_;
-    ros::Publisher diff_angle_rad_median_pub_;
-    ros::Publisher diff_angle_deg_pub_;
-    ros::Publisher diff_angle_deg_median_pub_;
-    ros::Publisher remote_cmd_pub_;
-
-    ValueTimeQueue dist_time_queue_;
-    ValueTimeQueue angle_time_queue_;
-    std::deque< boost::shared_ptr<const geometry_msgs::PoseStamped> > pose_ptr_queue_;
-    std::deque< boost::shared_ptr<const geometry_msgs::TwistStamped> > twist_ptr_queue_;
-
-    double loop_rate_hz_;
-    double comparison_window_size_sec_;
-    double topic_timeout_sec_;
-    double moving_median_window_size_sec_;
-
-    double diff_position_threshold_meter_;
-    double diff_position_median_threshold_meter_;
-    double diff_angle_threshold_rad_;
-    double diff_angle_median_threshold_rad_;
-
-    bool enable_emergency_to_twist_gate_;
+  struct CheckValues
+  {
+    double diff_pose_topic_time = 0.0;
+    double diff_twist_topic_time = 0.0;
+    double diff_position = 0.0;
+    double diff_position_median = 0.0;
+    double diff_angle = 0.0;
+    double diff_angle_median = 0.0;
+  };
+
+  struct CheckValueThresholds
+  {
+    std::string key;
+    double warn_value;
+    double error_value;
+    double fatal_value;
+    std::string error_description;
+  };
+
+public:
+  VelPoseDiffChecker(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
+
+private:
+  void callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg);
+  void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& msg);
+  void callbackTimer(const ros::TimerEvent& e);
+  CheckValues checkDiff();
+  uint8_t getErrorLevelWithHealthChecker(const CheckValues& check_values);
+  void publishCheckValues(const CheckValues& check_values);
+  std::pair<tf::Transform, ros::Time> calcDeltaTfPoseAndTime(
+    const boost::shared_ptr<const geometry_msgs::PoseStamped>& current_pose_ptr,
+    const ros::Time& ros_time_now, const ros::Time& ros_time_past);
+  std::deque<boost::shared_ptr<const geometry_msgs::TwistStamped>>::iterator
+    searchCurrentTwistPtrItr(const ros::Time& pose_time_current);
+  tf::Transform calcDeltaTfOdom(
+    const std::deque<boost::shared_ptr<const geometry_msgs::TwistStamped>>::iterator& current_twist_ptr_it,
+    const ros::Time& pose_time_past);
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  ros::Subscriber pose_sub_;
+  ros::Subscriber twist_sub_;
+  ros::Timer timer_;
+
+  ros::Publisher diff_position_pub_;
+  ros::Publisher diff_position_median_pub_;
+  ros::Publisher diff_angle_rad_pub_;
+  ros::Publisher diff_angle_rad_median_pub_;
+  ros::Publisher diff_angle_deg_pub_;
+  ros::Publisher diff_angle_deg_median_pub_;
+
+  autoware_health_checker::HealthChecker health_checker_;
+
+  ValueTimeQueue dist_time_queue_;
+  ValueTimeQueue angle_time_queue_;
+  std::deque<boost::shared_ptr<const geometry_msgs::PoseStamped> > pose_ptr_queue_;
+  std::deque<boost::shared_ptr<const geometry_msgs::TwistStamped> > twist_ptr_queue_;
+
+  double loop_rate_hz_;
+  double comparison_window_size_sec_;
+  double moving_median_window_size_sec_;
+
+  CheckValueThresholds diff_pose_topic_time_thresholds_;
+  CheckValueThresholds diff_twist_topic_time_thresholds_;
+  CheckValueThresholds diff_position_thresholds_;
+  CheckValueThresholds diff_position_median_thresholds_;
+  CheckValueThresholds diff_angle_thresholds_;
+  CheckValueThresholds diff_angle_median_thresholds_;
 };
 
 #endif  // VEL_POSE_DIFF_CHECKER_VEL_POSE_DIFF_CHECKER_CORE_H

vel_pose_diff_checker/launch/vel_pose_diff_checker.launch

@@ -5,14 +5,15 @@
 
   <arg name="loop_rate_hz" default="10.0"/>
   <arg name="comparison_window_size_sec" default="1.0"/>
-  <arg name="topic_timeout_sec" default="0.3"/>
   <arg name="moving_median_window_size_sec" default="2.0"/>
+  <arg name="topic_timeout_sec" default="0.3"/>
   <arg name="diff_position_threshold_meter" default="1.0"/>
   <arg name="diff_position_median_threshold_meter" default="0.5"/>
   <arg name="diff_angle_threshold_rad" default="0.1"/>
   <arg name="diff_angle_median_threshold_rad" default="0.05"/>
-  <arg name="enable_emergency_to_twist_gate" default="true"/>
-
+
+  <rosparam command="load" file="$(find vel_pose_diff_checker)/config/health_checker.yaml" />
+
   <node pkg="vel_pose_diff_checker" type="vel_pose_diff_checker" name="vel_pose_diff_checker" output="log">
     <!-- Input Topics -->
     <remap from="current_pose" to="$(arg input_pose_name)" />
@@ -27,7 +28,6 @@
     <param name="diff_position_median_threshold_meter" value="$(arg diff_position_median_threshold_meter)" />
     <param name="diff_angle_threshold_rad" value="$(arg diff_angle_threshold_rad)" />
     <param name="diff_angle_median_threshold_rad" value="$(arg diff_angle_median_threshold_rad)" />
-    <param name="enable_emergency_to_twist_gate" value="$(arg enable_emergency_to_twist_gate)" />
-</node>
+  </node>
 
 </launch>

vel_pose_diff_checker/package.xml

@@ -14,7 +14,9 @@
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>autoware_msgs</depend>
+  <depend>autoware_system_msgs</depend>
   <depend>amathutils_lib</depend>
+  <depend>autoware_health_checker</depend>
   <depend>roslint</depend>
 
 </package>

vel_pose_diff_checker/src/value_time_queue.cpp

@@ -20,18 +20,15 @@
 #include <deque>
 #include <algorithm>
 
-
-ValueTimeQueue::ValueTimeQueue()
-  : window_size_sec_(1.0)
+ValueTimeQueue::ValueTimeQueue() : window_size_sec_(1.0)
 {
 }
 
-ValueTimeQueue::ValueTimeQueue(const double window_size_sec)
-  : window_size_sec_(window_size_sec)
+ValueTimeQueue::ValueTimeQueue(const double window_size_sec) : window_size_sec_(window_size_sec)
 {
 }
 
-void ValueTimeQueue::addValueTime(const double value, const ros::Time &a_stamp)
+void ValueTimeQueue::addValueTime(const double value, const ros::Time& a_stamp)
 {
   const auto ros_time_now = ros::Time::now();
   queue_.push_back(ValueTime(value, a_stamp));
@@ -58,10 +55,7 @@ double ValueTimeQueue::getMedianValue() const
   std::deque<ValueTime> queue_tmp = queue_;
   const size_t median_index = queue_tmp.size() / 2;
   std::nth_element(std::begin(queue_tmp), std::begin(queue_tmp) + median_index, std::end(queue_tmp),
-    [](const ValueTime &lhs, const ValueTime &rhs)
-    {
-      return lhs.value < rhs.value;
-    });  // NOLINT
+                   [](const ValueTime& lhs, const ValueTime& rhs) { return lhs.value < rhs.value; });  // NOLINT
 
   return queue_tmp.at(median_index).value;
 }