Feature/update health checker threashold by rosparam

ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp

@@ -70,15 +70,15 @@
 #include <autoware_msgs/NDTStat.h>
 
 //headers in Autoware Health Checker
-#include <autoware_health_checker/node_status_publisher.h>
+#include <autoware_health_checker/health_checker/health_checker.h>
 
 #define PREDICT_POSE_THRESHOLD 0.5
 
 #define Wa 0.4
 #define Wb 0.3
 #define Wc 0.3
 
-static std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_ptr_;
+static std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
 
 struct pose
 {
@@ -921,7 +921,7 @@ static void imu_callback(const sensor_msgs::Imu::Ptr& input)
 
 static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 {
-  node_status_publisher_ptr_->CHECK_RATE("/topic/rate/points_raw/slow",8,5,1,"topic points_raw subscribe rate low.");
+  health_checker_ptr_->CHECK_RATE("topic_points_filtered_slow_in_ndt_matching",8.0,4.0,2.0,"points_filtered topic rate in ndt_mathing node is slow.");
   if (map_loaded == 1 && init_pos_set == 1)
   {
     matching_start = std::chrono::system_clock::now();
@@ -1354,7 +1354,6 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
     }
 
     predict_pose_pub.publish(predict_pose_msg);
-    node_status_publisher_ptr_->CHECK_RATE("/topic/rate/ndt_pose/slow",8,5,1,"topic points_raw publish rate low.");
     ndt_pose_pub.publish(ndt_pose_msg);
     // current_pose is published by vel_pose_mux
     //    current_pose_pub.publish(current_pose_msg);
@@ -1376,7 +1375,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
     matching_end = std::chrono::system_clock::now();
     exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end - matching_start).count() / 1000.0;
     time_ndt_matching.data = exe_time;
-    node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/time_ndt_matching",time_ndt_matching.data,50,70,100,"value time_ndt_matching is too high.");
+    health_checker_ptr_->CHECK_MAX_VALUE("time_ndt_matching",time_ndt_matching.data,50,70,100,"value time_ndt_matching is too high.");
     time_ndt_matching_pub.publish(time_ndt_matching);
 
     // Set values for /estimate_twist
@@ -1394,8 +1393,8 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
     geometry_msgs::Vector3Stamped estimate_vel_msg;
     estimate_vel_msg.header.stamp = current_scan_time;
     estimate_vel_msg.vector.x = current_velocity;
-    node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/estimate_twist/linear",current_velocity,5,10,15,"value linear estimated twist is too high.");
-    node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/estimate_twist/angular",angular_velocity,5,10,15,"value linear angular twist is too high.");
+    health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_linear",current_velocity,5,10,15,"value linear estimated twist is too high.");
+    health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_angular",angular_velocity,5,10,15,"value linear angular twist is too high.");
     estimated_vel_pub.publish(estimate_vel_msg);
 
     // Set values for /ndt_stat
@@ -1524,9 +1523,9 @@ int main(int argc, char** argv)
 
   ros::NodeHandle nh;
   ros::NodeHandle private_nh("~");
-  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,private_nh);
-  node_status_publisher_ptr_->ENABLE();
-  node_status_publisher_ptr_->NODE_ACTIVATE();
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh);
+  health_checker_ptr_->ENABLE();
+  health_checker_ptr_->NODE_ACTIVATE();
 
   // Set log file name.
   private_nh.getParam("output_log_data", _output_log_data);

ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp

@@ -32,8 +32,8 @@ SearchInfo::SearchInfo()
 {
   ros::NodeHandle nh;
   ros::NodeHandle private_nh_("~");
-  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,private_nh_);
-  node_status_publisher_ptr_->ENABLE();
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh_);
+  health_checker_ptr_->ENABLE();
   private_nh_.param<std::string>("map_frame", map_frame_, "map");
   private_nh_.param<int>("obstacle_detect_count", obstacle_detect_count_, 10);
   private_nh_.param<int>("avoid_distance", avoid_distance_, 13);
@@ -98,14 +98,13 @@ void SearchInfo::mapCallback(const nav_msgs::OccupancyGridConstPtr &msg)
 void SearchInfo::currentPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
 {
   current_pose_ = *msg;
-  node_status_publisher_ptr_->NODE_ACTIVATE();
-  node_status_publisher_ptr_->CHECK_RATE("/topic/rate/current_pose/slow",8,5,1,"topic current_pose subscribe rate low.");
+  health_checker_ptr_->NODE_ACTIVATE();
   if(closest_waypoint_index_!=-1 && path_set_)
   {
     autoware_msgs::Waypoint closest_waypoint = subscribed_waypoints_.waypoints[closest_waypoint_index_];
     double dist = std::sqrt(std::pow(closest_waypoint.pose.pose.position.x-current_pose_.pose.position.x,2)
       +std::pow(closest_waypoint.pose.pose.position.y-current_pose_.pose.position.y,2));
-    node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/range/closest_waypoint_distance",dist,0.5,1.0,2.0,"distance between closest_waypoint and current_pose is too long.");
+    health_checker_ptr_->CHECK_MAX_VALUE("closest_waypoint_distance",dist,0.5,1.0,2.0,"distance between closest_waypoint and current_pose is too long.");
   }
   return;
 }
@@ -182,7 +181,6 @@ void SearchInfo::waypointsCallback(const autoware_msgs::LaneConstPtr &msg)
 
 void SearchInfo::closestWaypointCallback(const std_msgs::Int32ConstPtr &msg)
 {
-  node_status_publisher_ptr_->CHECK_RATE("/topic/rate/closest_waypoint/slow",8,5,1,"topic closest_waypoint subscribe rate low.");
   closest_waypoint_index_ = msg->data;
 }
 

ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h

@@ -27,7 +27,7 @@
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
 
-#include <autoware_health_checker/node_status_publisher.h>
+#include <autoware_health_checker/health_checker/health_checker.h>
 #include <memory>
 
 namespace astar_planner
@@ -143,7 +143,7 @@ class SearchInfo
 private:
   double calcPathLength(const autoware_msgs::Lane &lane, const int start_waypoint_index,
                         const int goal_waypoint_index) const;
-  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_ptr_;
+  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
   nav_msgs::OccupancyGrid map_;
   geometry_msgs::PoseStamped start_pose_global_;
   geometry_msgs::PoseStamped goal_pose_global_;

ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp

@@ -43,8 +43,8 @@ VelocitySetInfo::VelocitySetInfo()
   ros::NodeHandle private_nh_("~");
   ros::NodeHandle nh;
   private_nh_.param<double>("remove_points_upto", remove_points_upto_, 2.3);
-  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,private_nh_);
-  node_status_publisher_ptr_->ENABLE();
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh_);
+  health_checker_ptr_->ENABLE();
 }
 
 VelocitySetInfo::~VelocitySetInfo()
@@ -73,7 +73,6 @@ void VelocitySetInfo::configCallback(const autoware_config_msgs::ConfigVelocityS
 
 void VelocitySetInfo::pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
 {
-  node_status_publisher_ptr_->CHECK_RATE("/topic/rate/points_no_ground/slow",8,5,1,"topic points_no_ground subscribe rate low.");
   pcl::PointCloud<pcl::PointXYZ> sub_points;
   pcl::fromROSMsg(*msg, sub_points);
 
@@ -108,15 +107,14 @@ void VelocitySetInfo::detectionCallback(const std_msgs::Int32 &msg)
 void VelocitySetInfo::controlPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
 {
   control_pose_ = *msg;
-
   if (!set_pose_)
     set_pose_ = true;
 }
 
 void VelocitySetInfo::localizerPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
 {
-  node_status_publisher_ptr_->NODE_ACTIVATE();
-  node_status_publisher_ptr_->CHECK_RATE("/topic/rate/current_pose/slow",8,5,1,"topic current_pose subscribe rate low.");
+  health_checker_ptr_->NODE_ACTIVATE();
+  health_checker_ptr_->CHECK_RATE("topic_current_pose_slow_in_velocity_set",8,5,1,"topic current_pose subscribe rate in velocity_set node is slow.");
   localizer_pose_ = *msg;
 }
 

ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h

@@ -24,7 +24,7 @@
 
 #include "autoware_config_msgs/ConfigVelocitySet.h"
 
-#include <autoware_health_checker/node_status_publisher.h>
+#include <autoware_health_checker/health_checker/health_checker.h>
 #include <memory>
 
 class VelocitySetInfo
@@ -54,7 +54,7 @@ class VelocitySetInfo
   bool set_pose_;
   bool use_obstacle_sim_;
 
-  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_ptr_;
+  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
 
  public:
   VelocitySetInfo();

ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp

@@ -36,8 +36,8 @@ PurePursuitNode::PurePursuitNode()
   , minimum_lookahead_distance_(6.0)
 {
   initForROS();
-  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);
-  node_status_publisher_ptr_->ENABLE();
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh_,private_nh_);
+  health_checker_ptr_->ENABLE();
   // initialize for PurePursuit
   pp_.setLinearInterpolationParameter(is_linear_interpolation_);
 }
@@ -96,8 +96,7 @@ void PurePursuitNode::run()
 
     publishTwistStamped(can_get_curvature, kappa);
     publishControlCommandStamped(can_get_curvature, kappa);
-    node_status_publisher_ptr_->NODE_ACTIVATE();
-    node_status_publisher_ptr_->CHECK_RATE("/topic/rate/vehicle/slow",8,5,1,"topic vehicle_cmd publish rate low.");
+    health_checker_ptr_->NODE_ACTIVATE();
     // for visualization with Rviz
     pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint()));
     pub13_.publish(displaySearchRadius(pp_.getCurrentPose().position, pp_.getLookaheadDistance()));
@@ -124,7 +123,7 @@ void PurePursuitNode::publishTwistStamped(const bool &can_get_curvature, const d
   ts.header.stamp = ros::Time::now();
   ts.twist.linear.x = can_get_curvature ? computeCommandVelocity() : 0;
   ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0;
-  node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/twist",ts.twist.linear.x,2.2,3.3,4.4,"linear twist_cmd is too high");
+  health_checker_ptr_->CHECK_MAX_VALUE("twist_cmd_linear_high",ts.twist.linear.x,2.2,3.3,4.4,"linear twist_cmd is too high");
   pub1_.publish(ts);
 }
 

ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h

@@ -31,7 +31,7 @@
 #include "pure_pursuit.h"
 #include "pure_pursuit_viz.h"
 
-#include <autoware_health_checker/node_status_publisher.h>
+#include <autoware_health_checker/health_checker/health_checker.h>
 
 #include <memory>
 
@@ -64,7 +64,7 @@ class PurePursuitNode
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
 
-  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_ptr_;
+  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
 
   // class
   PurePursuit pp_;

ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp

@@ -50,7 +50,7 @@
 #include "autoware_msgs/ControlCommandStamped.h"
 
 //headers in Autowae Health Checker
-#include <autoware_health_checker/node_status_publisher.h>
+#include <autoware_health_checker/health_checker/health_checker.h>
 
 class TwistGate
 {
@@ -76,7 +76,7 @@ class TwistGate
 
     ros::NodeHandle nh_;
     ros::NodeHandle private_nh_;
-    std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_pub_ptr_;
+    std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
     ros::Publisher emergency_stop_pub_;
     ros::Publisher control_command_pub_;
     ros::Publisher vehicle_cmd_pub_;
@@ -104,7 +104,7 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n
     ,command_mode_(CommandMode::AUTO)
     ,previous_command_mode_(CommandMode::AUTO)
 {
-  node_status_pub_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh_,private_nh_);
   emergency_stop_pub_ = nh_.advertise<std_msgs::Bool>("/emergency_stop", 1, true);
   control_command_pub_ = nh_.advertise<std_msgs::String>("/ctrl_mode", 1);
   vehicle_cmd_pub_ = nh_.advertise<vehicle_cmd_msg_t>("/vehicle_cmd", 1, true);
@@ -124,7 +124,7 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n
   twist_gate_msg_.header.seq = 0;
   emergency_stop_msg_.data = false;
   send_emergency_cmd = false;
-  node_status_pub_ptr_->ENABLE();
+  health_checker_ptr_->ENABLE();
 
   remote_cmd_time_ = ros::Time::now();
   watchdog_timer_thread_ = std::thread(&TwistGate::watchdog_timer, this);
@@ -233,8 +233,8 @@ void TwistGate::remote_cmd_callback(const remote_msgs_t::ConstPtr& input_msg)
 
 void TwistGate::auto_cmd_twist_cmd_callback(const geometry_msgs::TwistStamped::ConstPtr& input_msg)
 {
-  node_status_pub_ptr_->NODE_ACTIVATE();
-  node_status_pub_ptr_->CHECK_RATE("/topic/rate/twist_cmd/slow",8,5,1,"topic twist_cmd subscribe rate low.");
+  health_checker_ptr_->NODE_ACTIVATE();
+  health_checker_ptr_->CHECK_RATE("topic_rate_twist_cmd_in_twist_gate_slow",8,5,1,"topic twist_cmd subscribe rate in twist_get node is slow.");
   if(command_mode_ == CommandMode::AUTO)
   {
     twist_gate_msg_.header.frame_id = input_msg->header.frame_id;

ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg

@@ -10,7 +10,10 @@ uint8 UNDEFINED = 0
 
 uint8 type
 uint8 OUT_OF_RANGE = 1
-uint8 RATE_IS_SLOW = 2
+uint8 LOOP_RATE_IS_SLOW = 2
+uint8 INVALID_VALUE = 3
+uint8 TOPIC_RATE_IS_SLOW = 4
+uint8 TOPIC_DROP_RATE_IS_HIGH = 5
 uint8 HARDWARE = 255
 
 uint8 level

ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h

@@ -34,7 +34,7 @@
 #include "autoware_config_msgs/ConfigRayGroundFilter.h"
 
 //headers in Autoware Health Checker
-#include <autoware_health_checker/node_status_publisher.h>
+#include <autoware_health_checker/health_checker/health_checker.h>
 
 #include <opencv2/core/version.hpp>
 #if (CV_MAJOR_VERSION == 3)
@@ -46,7 +46,7 @@
 class RayGroundFilter
 {
 private:
-	std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_pub_ptr_;
+	std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
 	ros::NodeHandle     node_handle_;
 	ros::Subscriber     points_node_sub_;
 	ros::Subscriber     config_node_sub_;

ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp

@@ -283,8 +283,8 @@ void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Pt
 
 void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr &in_sensor_cloud)
 {
-  node_status_pub_ptr_->NODE_ACTIVATE();
-  node_status_pub_ptr_->CHECK_RATE("/topic/rate/points_raw/slow",8,5,1,"topic points_raw subscribe rate low.");
+  health_checker_ptr_->NODE_ACTIVATE();
+  health_checker_ptr_->CHECK_RATE("topic_rate_points_raw_in_ray_ground_filter_slow",8,5,1,"topic points_raw subscribe rate in ray_ground_filter is slow.");
   pcl::PointCloud<pcl::PointXYZI>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
   pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr);
 
@@ -331,8 +331,8 @@ RayGroundFilter::RayGroundFilter():node_handle_("~")
 {
   ros::NodeHandle nh;
   ros::NodeHandle pnh("~");
-  node_status_pub_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,pnh);
-  node_status_pub_ptr_->ENABLE();
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,pnh);
+  health_checker_ptr_->ENABLE();
 }
 
 void RayGroundFilter::Run()

ros/src/system/autoware_health_checker/CMakeLists.txt

@@ -15,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES node_status_publisher
+  LIBRARIES health_checker
   CATKIN_DEPENDS autoware_system_msgs roscpp rosgraph_msgs
 #  DEPENDS system_lib
 )
@@ -26,33 +26,34 @@ include_directories(
   ${autoware_system_msgs_INCLUDE_DIRS}
 )
 
-set(NODE_STATUS_PUBLISHER_SRC
-  src/node_status_publisher.cpp
-  src/diag_buffer.cpp
-  src/rate_checker.cpp
+set(HEALTH_CHECKER_SRC
+  src/health_checker/health_checker.cpp
+  src/health_checker/diag_buffer.cpp
+  src/health_checker/rate_checker.cpp
+  src/health_checker/value_manager.cpp
 )
-add_library(node_status_publisher
-  ${NODE_STATUS_PUBLISHER_SRC}
+add_library(health_checker
+  ${HEALTH_CHECKER_SRC}
 )
-target_link_libraries(node_status_publisher ${catkin_LIBRARIES})
-add_dependencies(node_status_publisher ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp)
+target_link_libraries(health_checker ${catkin_LIBRARIES})
+add_dependencies(health_checker ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp)
 
 add_library(system_status_subscriber
-  src/system_status_subscriber.cpp
+  src/system_status_subscriber/system_status_subscriber.cpp
 )
 target_link_libraries(system_status_subscriber ${catkin_LIBRARIES})
 add_dependencies(system_status_subscriber ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp)
 
 add_executable(health_aggregator
-  src/health_aggregator_node.cpp
-  src/health_aggregator.cpp
+  src/health_aggregator/health_aggregator_node.cpp
+  src/health_aggregator/health_aggregator.cpp
 )
 target_link_libraries(health_aggregator ${catkin_LIBRARIES})
 add_dependencies(health_aggregator ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp)
 
 add_executable(health_analyzer
-  src/health_analyzer_node.cpp
-  src/health_analyzer.cpp
+  src/health_analyzer/health_analyzer_node.cpp
+  src/health_analyzer/health_analyzer.cpp
 )
 target_link_libraries(health_analyzer ${catkin_LIBRARIES})
 add_dependencies(health_analyzer ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp)
@@ -71,7 +72,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
 )
 
 # Install library
-install(TARGETS node_status_publisher system_status_subscriber
+install(TARGETS health_checker system_status_subscriber
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -82,7 +83,7 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest_gtest(test-autoware_health_checker
   test/test_autoware_health_checker.test
   test/src/test_autoware_health_checker.cpp
-  ${NODE_STATUS_PUBLISHER_SRC})
+  ${HEALTH_CHECKER_SRC})
   target_link_libraries(test-autoware_health_checker
   ${catkin_LIBRARIES})
 endif ()