Fix/health checker (#2012)

autowarefoundation · Feb 27, 2019 · e4187a7 · e4187a7
1 parent 460cfbc
commit e4187a7
Show file tree

Hide file tree

Showing 37 changed files with 928 additions and 454 deletions.
diff --git a/ros/run b/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; roslaunch autoware_health_checker health_checker.launch'"&
+${TERMINAL} ${MASTER_DISPLAY_OPTION} ${OPTION_WORKING_DIR}=${MY_PATH} ${OPTION_COMMAND}="bash -c 'source ./devel/setup.bash; roscore'"&
 
 # 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'"
diff --git a/ros/src/.config/rviz/default.rviz b/ros/src/.config/rviz/default.rviz
@@ -67,24 +67,23 @@ 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:
-        map:
-          base_link:
-            velodyne:
-              {}
-          gps:
-            {}
+        world:
+          map:
+            base_link:
+              velodyne:
+                {}
       Update Interval: 0
       Value: true
     - Alpha: 0.0500000007
@@ -122,9 +121,7 @@ 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
@@ -179,8 +176,8 @@ Visualization Manager:
     - Alpha: 0.300000012
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 11.8534737
-        Min Value: 9.73868942
+        Max Value: 11.6879101
+        Min Value: -11.4952602
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -271,26 +268,23 @@ 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
@@ -357,19 +351,15 @@ 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
@@ -522,15 +512,15 @@ Visualization Manager:
       Unreliable: false
       Value: false
     - Class: jsk_rviz_plugin/OverlayImage
-      Enabled: false
+      Enabled: true
       Name: OverlayImage
       Topic: /image_raw
-      Value: false
+      Value: true
       alpha: 0.800000012
       height: 128
       keep aspect ratio: true
       left: 128
-      top: 128
+      top: 720
       width: 640
     - Background Alpha: 0.800000012
       Background Color: 0; 0; 0
@@ -646,11 +636,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 19.7382278
-      Target Frame: base_link
+      Scale: 3.93164873
+      Target Frame: world
       Value: TopDownOrtho (rviz)
-      X: 0
-      Y: 0
+      X: 1027.31726
+      Y: 636.823425
     Saved: ~
 Window Geometry:
   Camera:

diff --git a/...ting/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp b/...ting/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,6 @@ 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();
@@ -1354,7 +1353,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 +1374,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 +1392,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 +1522,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);

diff --git a/...computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp b/...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;
 }
 

diff --git a/...c/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h b/...c/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_;

diff --git a/...computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp b/...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,13 @@ 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;
 }
 

diff --git a/...c/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h b/...c/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();

diff --git a/...uting/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp b/...uting/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);
 }
 

diff --git a/...mputing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h b/...mputing/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_;