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

@@ -31,15 +31,80 @@
 #include "twist_gate.h"
 
 #include <chrono>
-
-TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh)
-  : nh_(nh)
-  , private_nh_(private_nh)
-  , timeout_period_(1.0)
-  , command_mode_(CommandMode::AUTO)
-  , previous_command_mode_(CommandMode::AUTO)
+#include <map>
+#include <memory>
+
+#include <ros/ros.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/TwistStamped.h>
+
+#include "autoware_msgs/RemoteCmd.h"
+#include "autoware_msgs/VehicleCmd.h"
+#include "tablet_socket_msgs/mode_cmd.h"
+#include "tablet_socket_msgs/gear_cmd.h"
+#include "autoware_msgs/AccelCmd.h"
+#include "autoware_msgs/BrakeCmd.h"
+#include "autoware_msgs/SteerCmd.h"
+#include "autoware_msgs/ControlCommandStamped.h"
+
+//headers in Autowae Health Checker
+#include <autoware_health_checker/health_checker/health_checker.h>
+
+class TwistGate
+{
+  using remote_msgs_t = autoware_msgs::RemoteCmd;
+  using vehicle_cmd_msg_t = autoware_msgs::VehicleCmd;
+
+  public:
+    TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
+    ~TwistGate();
+  private:
+    void watchdog_timer();
+    void remote_cmd_callback(const remote_msgs_t::ConstPtr& input_msg);
+    void auto_cmd_twist_cmd_callback(const geometry_msgs::TwistStamped::ConstPtr& input_msg);
+    void mode_cmd_callback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg);
+    void gear_cmd_callback(const tablet_socket_msgs::gear_cmd::ConstPtr& input_msg);
+    void accel_cmd_callback(const autoware_msgs::AccelCmd::ConstPtr& input_msg);
+    void steer_cmd_callback(const autoware_msgs::SteerCmd::ConstPtr& input_msg);
+    void brake_cmd_callback(const autoware_msgs::BrakeCmd::ConstPtr& input_msg);
+    void lamp_cmd_callback(const autoware_msgs::LampCmd::ConstPtr& input_msg);
+    void ctrl_cmd_callback(const autoware_msgs::ControlCommandStamped::ConstPtr& input_msg);
+
+    void reset_vehicle_cmd_msg();
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+    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_;
+    ros::Publisher state_cmd_pub_;
+    ros::Subscriber remote_cmd_sub_;
+    std::map<std::string , ros::Subscriber> auto_cmd_sub_stdmap_;
+
+    vehicle_cmd_msg_t twist_gate_msg_;
+    std_msgs::Bool emergency_stop_msg_;
+    ros::Time remote_cmd_time_;
+    ros::Duration timeout_period_;
+
+    std::thread watchdog_timer_thread_;
+    enum class CommandMode{AUTO=1, REMOTE=2} command_mode_, previous_command_mode_;
+    std_msgs::String command_mode_topic_;
+
+    // still send is true
+    bool send_emergency_cmd = false;
+};
+
+TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh) :
+     nh_(nh)
+    ,private_nh_(private_nh)
+    ,timeout_period_(1.0)
+    ,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);
@@ -60,7 +125,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);
@@ -208,8 +273,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()