Revert "Fix/health checker (autowarefoundation#2012)" (autowarefounda…

…tion#2037)

This reverts commit e4187a7.

Reverts autowarefoundation#2012

Merged without adequate description of the bug or fixes made

ros/run

@@ -43,7 +43,7 @@ if [ "$?" == "0" ]; then
 fi
 
 # boot ros-master & health_checker
-${TERMINAL} ${MASTER_DISPLAY_OPTION} ${OPTION_WORKING_DIR}=${MY_PATH} ${OPTION_COMMAND}="bash -c 'source ./devel/setup.bash; roscore'"&
+${TERMINAL} ${MASTER_DISPLAY_OPTION} ${OPTION_WORKING_DIR}=${MY_PATH} ${OPTION_COMMAND}="bash -c 'source ./devel/setup.bash; roslaunch autoware_health_checker health_checker.launch'"&
 
 # boot runtime_manager
 ${TERMINAL} ${RUNMGR_DISPLAY_OPTION} ${OPTION_WORKING_DIR}=${MY_PATH} ${OPTION_COMMAND}="bash -c 'source ./devel/setup.bash; rosrun runtime_manager runtime_manager_dialog.py'"

ros/src/.config/rviz/default.rviz

@@ -67,23 +67,24 @@ Visualization Manager:
         All Enabled: true
         base_link:
           Value: true
+        gps:
+          Value: true
         map:
           Value: true
         velodyne:
           Value: true
-        world:
-          Value: true
       Marker Scale: 5
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        world:
-          map:
-            base_link:
-              velodyne:
-                {}
+        map:
+          base_link:
+            velodyne:
+              {}
+          gps:
+            {}
       Update Interval: 0
       Value: true
     - Alpha: 0.0500000007
@@ -121,7 +122,9 @@ Visualization Manager:
       Marker Topic: /vector_map
       Name: Vector Map
       Namespaces:
-        {}
+        signal: true
+        stop_line: true
+        white_line: true
       Queue Size: 100
       Value: true
     - Class: rviz/Camera
@@ -176,8 +179,8 @@ Visualization Manager:
     - Alpha: 0.300000012
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 11.6879101
-        Min Value: -11.4952602
+        Max Value: 11.8534737
+        Min Value: 9.73868942
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -268,23 +271,26 @@ Visualization Manager:
       Marker Topic: /detection_range
       Name: Detection Range
       Namespaces:
-        {}
+        Crosswalk Detection: true
+        Decelerate Detection: true
+        Stop Detection: true
+        Stop Line: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: /next_target_mark
       Name: Next Waypoint Mark
       Namespaces:
-        {}
+        next_target_marker: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: /trajectory_circle_mark
       Name: PP Trajectory Mark
       Namespaces:
-        {}
+        trajectory_circle_marker: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
@@ -351,15 +357,19 @@ Visualization Manager:
       Marker Topic: /local_waypoints_mark
       Name: Local Waypoints
       Namespaces:
-        {}
+        local_path_marker: true
+        local_point_marker: true
+        local_waypoint_velocity: true
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /global_waypoints_mark
       Name: Global Waypoints
       Namespaces:
-        {}
+        global_change_flag_lane_1: true
+        global_lane_waypoint_orientation_marker_1: true
+        global_velocity_lane_1: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
@@ -512,15 +522,15 @@ Visualization Manager:
       Unreliable: false
       Value: false
     - Class: jsk_rviz_plugin/OverlayImage
-      Enabled: true
+      Enabled: false
       Name: OverlayImage
       Topic: /image_raw
-      Value: true
+      Value: false
       alpha: 0.800000012
       height: 128
       keep aspect ratio: true
       left: 128
-      top: 720
+      top: 128
       width: 640
     - Background Alpha: 0.800000012
       Background Color: 0; 0; 0
@@ -636,11 +646,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 3.93164873
-      Target Frame: world
+      Scale: 19.7382278
+      Target Frame: base_link
       Value: TopDownOrtho (rviz)
-      X: 1027.31726
-      Y: 636.823425
+      X: 0
+      Y: 0
     Saved: ~
 Window Geometry:
   Camera:

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/health_checker/health_checker.h>
+#include <autoware_health_checker/node_status_publisher.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::HealthChecker> health_checker_ptr_;
+static std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_ptr_;
 
 struct pose
 {
@@ -921,6 +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.");
   if (map_loaded == 1 && init_pos_set == 1)
   {
     matching_start = std::chrono::system_clock::now();
@@ -1353,6 +1354,7 @@ 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);
@@ -1374,7 +1376,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;
-    health_checker_ptr_->CHECK_MAX_VALUE("time_ndt_matching",time_ndt_matching.data,50,70,100,"value time_ndt_matching is too high.");
+    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.");
     time_ndt_matching_pub.publish(time_ndt_matching);
 
     // Set values for /estimate_twist
@@ -1392,8 +1394,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;
-    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.");
+    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.");
     estimated_vel_pub.publish(estimate_vel_msg);
 
     // Set values for /ndt_stat
@@ -1522,9 +1524,9 @@ int main(int argc, char** argv)
 
   ros::NodeHandle nh;
   ros::NodeHandle private_nh("~");
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh);
-  health_checker_ptr_->ENABLE();
-  health_checker_ptr_->NODE_ACTIVATE();
+  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();
 
   // 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_("~");
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh_);
-  health_checker_ptr_->ENABLE();
+  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,private_nh_);
+  node_status_publisher_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,13 +98,14 @@ void SearchInfo::mapCallback(const nav_msgs::OccupancyGridConstPtr &msg)
 void SearchInfo::currentPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
 {
   current_pose_ = *msg;
-  health_checker_ptr_->NODE_ACTIVATE();
+  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.");
   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));
-    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.");
+    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.");
   }
   return;
 }
@@ -181,6 +182,7 @@ 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/health_checker/health_checker.h>
+#include <autoware_health_checker/node_status_publisher.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::HealthChecker> health_checker_ptr_;
+  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_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);
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh_);
-  health_checker_ptr_->ENABLE();
+  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,private_nh_);
+  node_status_publisher_ptr_->ENABLE();
 }
 
 VelocitySetInfo::~VelocitySetInfo()
@@ -73,6 +73,7 @@ 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);
 
@@ -107,13 +108,15 @@ void VelocitySetInfo::detectionCallback(const std_msgs::Int32 &msg)
 void VelocitySetInfo::controlPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
 {
   control_pose_ = *msg;
-  health_checker_ptr_->NODE_ACTIVATE();
+
   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.");
   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/health_checker/health_checker.h>
+#include <autoware_health_checker/node_status_publisher.h>
 #include <memory>
 
 class VelocitySetInfo
@@ -54,7 +54,7 @@ class VelocitySetInfo
   bool set_pose_;
   bool use_obstacle_sim_;
 
-  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
+  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_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();
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh_,private_nh_);
-  health_checker_ptr_->ENABLE();
+  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);
+  node_status_publisher_ptr_->ENABLE();
   // initialize for PurePursuit
   pp_.setLinearInterpolationParameter(is_linear_interpolation_);
 }
@@ -96,7 +96,8 @@ void PurePursuitNode::run()
 
     publishTwistStamped(can_get_curvature, kappa);
     publishControlCommandStamped(can_get_curvature, kappa);
-    health_checker_ptr_->NODE_ACTIVATE();
+    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.");
     // for visualization with Rviz
     pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint()));
     pub13_.publish(displaySearchRadius(pp_.getCurrentPose().position, pp_.getLookaheadDistance()));
@@ -123,7 +124,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;
-  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");
+  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");
   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/health_checker/health_checker.h>
+#include <autoware_health_checker/node_status_publisher.h>
 
 #include <memory>
 
@@ -64,7 +64,7 @@ class PurePursuitNode
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
 
-  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
+  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_publisher_ptr_;
 
   // class
   PurePursuit pp_;