diff --git a/ros/run b/ros/run index 0520ed08826..eb1211e13f4 100755 --- 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'" \ No newline at end of file diff --git a/ros/src/.config/rviz/default.rviz b/ros/src/.config/rviz/default.rviz index 87ad3fc1e05..5bb37e99691 100644 --- 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,10 +268,7 @@ 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 @@ -282,7 +276,7 @@ Visualization Manager: Marker Topic: /next_target_mark Name: Next Waypoint Mark Namespaces: - next_target_marker: true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -290,7 +284,7 @@ Visualization Manager: Marker Topic: /trajectory_circle_mark Name: PP Trajectory Mark Namespaces: - trajectory_circle_marker: true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -357,9 +351,7 @@ 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 @@ -367,9 +359,7 @@ Visualization Manager: 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/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp b/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp index 4ae31227324..137068fc729 100644 --- a/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp +++ b/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp @@ -70,7 +70,7 @@ #include //headers in Autoware Health Checker -#include +#include #define PREDICT_POSE_THRESHOLD 0.5 @@ -78,7 +78,7 @@ #define Wb 0.3 #define Wc 0.3 -static std::shared_ptr node_status_publisher_ptr_; +static std::shared_ptr 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(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(nh,private_nh); - node_status_publisher_ptr_->ENABLE(); - node_status_publisher_ptr_->NODE_ACTIVATE(); + health_checker_ptr_ = std::make_shared(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/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp index 3e85174f357..24b06a08bbb 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp +++ b/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(nh,private_nh_); - node_status_publisher_ptr_->ENABLE(); + health_checker_ptr_ = std::make_shared(nh,private_nh_); + health_checker_ptr_->ENABLE(); private_nh_.param("map_frame", map_frame_, "map"); private_nh_.param("obstacle_detect_count", obstacle_detect_count_, 10); private_nh_.param("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/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h index 4a0d0af0c4a..03424257ce3 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h +++ b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h @@ -27,7 +27,7 @@ #include #include -#include +#include #include 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 node_status_publisher_ptr_; + std::shared_ptr health_checker_ptr_; nav_msgs::OccupancyGrid map_; geometry_msgs::PoseStamped start_pose_global_; geometry_msgs::PoseStamped goal_pose_global_; diff --git a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp b/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp index 32119763400..79d2195203a 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp +++ b/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("remove_points_upto", remove_points_upto_, 2.3); - node_status_publisher_ptr_ = std::make_shared(nh,private_nh_); - node_status_publisher_ptr_->ENABLE(); + health_checker_ptr_ = std::make_shared(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 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/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h b/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h index 13afcc35c76..2b103329ee3 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h +++ b/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 +#include #include class VelocitySetInfo @@ -54,7 +54,7 @@ class VelocitySetInfo bool set_pose_; bool use_obstacle_sim_; - std::shared_ptr node_status_publisher_ptr_; + std::shared_ptr health_checker_ptr_; public: VelocitySetInfo(); diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp index edbdfc1e8fc..12f26e6dc3d 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp +++ b/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(nh_,private_nh_); - node_status_publisher_ptr_->ENABLE(); + health_checker_ptr_ = std::make_shared(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/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h index 627851457c9..dee6a1bd47e 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h +++ b/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 +#include #include @@ -64,7 +64,7 @@ class PurePursuitNode ros::NodeHandle nh_; ros::NodeHandle private_nh_; - std::shared_ptr node_status_publisher_ptr_; + std::shared_ptr health_checker_ptr_; // class PurePursuit pp_; diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp index 341bd4d1731..326c3be13ee 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp +++ b/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 +#include class TwistGate { @@ -76,7 +76,7 @@ class TwistGate ros::NodeHandle nh_; ros::NodeHandle private_nh_; - std::shared_ptr node_status_pub_ptr_; + std::shared_ptr node_status_pub_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(nh_,private_nh_); + node_status_pub_ptr_ = std::make_shared(nh_,private_nh_); emergency_stop_pub_ = nh_.advertise("/emergency_stop", 1, true); control_command_pub_ = nh_.advertise("/ctrl_mode", 1); vehicle_cmd_pub_ = nh_.advertise("/vehicle_cmd", 1, true); @@ -234,7 +234,6 @@ 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."); if(command_mode_ == CommandMode::AUTO) { twist_gate_msg_.header.frame_id = input_msg->header.frame_id; diff --git a/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg b/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg index 4abe2e1489d..2b0905bd30e 100755 --- a/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg +++ b/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg @@ -10,7 +10,11 @@ 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 INVALID_LOGICAL_VALUE = 6 uint8 HARDWARE = 255 uint8 level diff --git a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h index ca195bac9d6..f8e9a5bcc3e 100644 --- a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h +++ b/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 +#include #include #if (CV_MAJOR_VERSION == 3) @@ -46,7 +46,7 @@ class RayGroundFilter { private: - std::shared_ptr node_status_pub_ptr_; + std::shared_ptr node_status_pub_ptr_; ros::NodeHandle node_handle_; ros::Subscriber points_node_sub_; ros::Subscriber config_node_sub_; diff --git a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp index 3bca79e5793..f3bce8f1a40 100644 --- a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +++ b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp @@ -284,7 +284,6 @@ void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud::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."); pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr); @@ -331,7 +330,7 @@ RayGroundFilter::RayGroundFilter():node_handle_("~") { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - node_status_pub_ptr_ = std::make_shared(nh,pnh); + node_status_pub_ptr_ = std::make_shared(nh,pnh); node_status_pub_ptr_->ENABLE(); } diff --git a/ros/src/system/autoware_health_checker/CMakeLists.txt b/ros/src/system/autoware_health_checker/CMakeLists.txt index b4c0f5051de..d7056fd43ea 100644 --- a/ros/src/system/autoware_health_checker/CMakeLists.txt +++ b/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 @@ -79,10 +80,10 @@ install(TARGETS node_status_publisher system_status_subscriber if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest_gtest(test-autoware_health_checker + add_rostest_gtest(test_autoware_health_checker test/test_autoware_health_checker.test test/src/test_autoware_health_checker.cpp - ${NODE_STATUS_PUBLISHER_SRC}) - target_link_libraries(test-autoware_health_checker + ${HEALTH_CHECKER_SRC}) + target_link_libraries(test_autoware_health_checker ${catkin_LIBRARIES}) endif () \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/config/default.yaml b/ros/src/system/autoware_health_checker/config/default.yaml new file mode 100644 index 00000000000..abb25748abf --- /dev/null +++ b/ros/src/system/autoware_health_checker/config/default.yaml @@ -0,0 +1,112 @@ +health_checker: + estimate_twist_angular: + max: + warn: 0.3 + error: 0.6 + fatal : 0.9 + estimate_twist_linear: + max: + warn: 10.0 + error: 20.0 + fatal: 30.0 + time_ndt_matching: + max: + warn: 50.0 + error: 75.0 + fatal : 100.0 + closest_waypoint_distance: + max: + warn: 1.0 + error: 2.0 + fatal: 3.0 + twist_cmd_linear_high: + max: + warn: 15.0 + error: 18.0 + fatal: 20.0 + topic_rate: + topic_points_raw_slow_in_ndt_matching_topic_rate: + pub_node: /velodyne_driver + sub_node: /ndt_matching + topic_name: /points_raw + warn: 8.0 + error: 4.0 + fatal: 2.0 + topic_twist_cmd_in_twist_gate_topic_rate: + pub_node: /twist_filter + sub_node: /twist_gate + topic_name: /twist_cmd + warn: 8.0 + error: 4.0 + fatal: 2.0 + topic_points_no_ground_slow_in_ndt_matching_topic_rate: + pub_node: /ray_ground_filter + sub_node: /velocity_set + topic_name: /points_no_ground + warn: 8.0 + error: 4.0 + fatal: 2.0 + topic_points_no_ground_slow_in_ndt_matching_topic_rate: + pub_node: /ray_ground_filter + sub_node: /velocity_set + topic_name: /points_no_ground + warn: 8.0 + error: 4.0 + fatal: 2.0 + topic_ndt_pose_in_obstacle_avoid_topic_rate: + pub_node: /ndt_matching + sub_node: /obstacle_avoid + topic_name: /ndt_pose + warn: 8.0 + error: 4.0 + fatal: 2.0 + topic_ndt_pose_in_velocity_set_topic_rate: + pub_node: /ndt_matching + sub_node: /velocity_set + topic_name: /ndt_pose + warn: 8.0 + error: 4.0 + fatal: 2.0 + drop_rate: + topic_points_raw_slow_in_ndt_matching_drop_rate: + pub_node: /velodyne_driver + sub_node: /ndt_matching + topic_name: /points_raw + warn : 0.05 + error: 0.1 + fatal: 0.2 + topic_twist_cmd_in_twist_gate_drop_rate: + pub_node: /twist_filter + sub_node: /twist_gate + topic_name: /twist_cmd + warn : 0.05 + error: 0.1 + fatal: 0.2 + topic_points_no_ground_slow_in_ndt_matching_drop_rate: + pub_node: /ray_ground_filter + sub_node: /velocity_set + topic_name: /points_no_ground + warn : 0.05 + error: 0.1 + fatal: 0.2 + topic_points_no_ground_slow_in_ndt_matching_drop_rate: + pub_node: /ray_ground_filter + sub_node: /velocity_set + topic_name: /points_no_ground + warn : 0.05 + error: 0.1 + fatal: 0.2 + topic_ndt_pose_in_obstacle_avoid_drop_rate: + pub_node: /ndt_matching + sub_node: /obstacle_avoid + topic_name: /ndt_pose + warn : 0.05 + error: 0.1 + fatal: 0.2 + topic_ndt_pose_in_velocity_set_drop_rate: + pub_node: /ndt_matching + sub_node: /velocity_set + topic_name: /ndt_pose + warn : 0.05 + error: 0.1 + fatal: 0.2 \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/data/generate_pdf.sh b/ros/src/system/autoware_health_checker/data/generate_pdf.sh deleted file mode 100644 index 56b33905d5a..00000000000 --- a/ros/src/system/autoware_health_checker/data/generate_pdf.sh +++ /dev/null @@ -1 +0,0 @@ -dot -T pdf node_depends.dot -o node_depends.pdf \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h index de37415ee81..7c0a748b4e8 100644 --- a/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h @@ -30,13 +30,6 @@ constexpr uint8_t LEVEL_WARN = autoware_system_msgs::DiagnosticStatus::WARN; constexpr uint8_t LEVEL_ERROR = autoware_system_msgs::DiagnosticStatus::ERROR; constexpr uint8_t LEVEL_FATAL = autoware_system_msgs::DiagnosticStatus::FATAL; -constexpr uint8_t TYPE_UNDEFINED = - autoware_system_msgs::DiagnosticStatus::UNDEFINED; -constexpr uint8_t TYPE_OUT_OF_RANGE = - autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; -constexpr uint8_t TYPE_RATE_IS_SLOW = - autoware_system_msgs::DiagnosticStatus::RATE_IS_SLOW; - constexpr double BUFFER_LENGTH = 5.0; constexpr double UPDATE_RATE = 10.0; } diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator/health_aggregator.h similarity index 89% rename from ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator.h rename to ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator/health_aggregator.h index 14d420686f2..059fda09199 100644 --- a/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator.h +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator/health_aggregator.h @@ -56,6 +56,9 @@ class HealthAggregator { ros::Subscriber node_status_sub_; ros::Subscriber diagnostic_array_sub_; ros::Subscriber topic_statistics_sub_; + XmlRpc::XmlRpcValue topic_rate_params_; + XmlRpc::XmlRpcValue drop_rate_params_; + void addTopicDiag(); void publishSystemStatus(); void nodeStatusCallback(const autoware_system_msgs::NodeStatus::ConstPtr msg); void @@ -72,10 +75,19 @@ class HealthAggregator { convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); autoware_system_msgs::SystemStatus system_status_; std::mutex mtx_; + bool ros_ok_; void updateConnectionStatus(); // key topic_name,publisher_node,subscriber_node std::map, rosgraph_msgs::TopicStatistics> topic_status_; void updateTopicStatus(); + template std::string valueToJson(T value) { + using namespace boost::property_tree; + std::stringstream ss; + ptree pt; + pt.put("value", value); + write_json(ss, pt); + return ss.str(); + } }; #endif // HEALTH_AGGREGATOR_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_analyzer.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_analyzer/health_analyzer.h similarity index 98% rename from ros/src/system/autoware_health_checker/include/autoware_health_checker/health_analyzer.h rename to ros/src/system/autoware_health_checker/include/autoware_health_checker/health_analyzer/health_analyzer.h index cc1355d7ea9..acf8ae90dc2 100644 --- a/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_analyzer.h +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_analyzer/health_analyzer.h @@ -84,7 +84,7 @@ class HealthAnalyzer { int countWarn(autoware_system_msgs::SystemStatus msg); void writeDot(); graph_t depend_graph_; - int warn_nodes_count_threshold_; + int warn_count_threshold_; template bool isAlreadyExist(std::vector vector, T target) { for (auto itr = vector.begin(); itr != vector.end(); itr++) { if (target == *itr) { diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/diag_buffer.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/diag_buffer.h similarity index 100% rename from ros/src/system/autoware_health_checker/include/autoware_health_checker/diag_buffer.h rename to ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/diag_buffer.h diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/health_checker.h similarity index 73% rename from ros/src/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h rename to ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/health_checker.h index a1f4a7583b5..4cef890214e 100644 --- a/ros/src/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/health_checker.h @@ -1,5 +1,5 @@ -#ifndef NODE_STATUS_PUBLISHER_H_INCLUDED -#define NODE_STATUS_PUBLISHER_H_INCLUDED +#ifndef HEALTH_CHECKER_H_INCLUDED +#define HEALTH_CHECKER_H_INCLUDED /* * Copyright 2019 Autoware Foundation. All rights reserved. @@ -25,9 +25,10 @@ // headers in Autoware #include -#include -#include #include +#include +#include +#include // headers in STL #include @@ -44,10 +45,10 @@ #include namespace autoware_health_checker { -class NodeStatusPublisher { +class HealthChecker { public: - NodeStatusPublisher(ros::NodeHandle nh, ros::NodeHandle pnh); - ~NodeStatusPublisher(); + HealthChecker(ros::NodeHandle nh, ros::NodeHandle pnh); + ~HealthChecker(); void ENABLE(); uint8_t CHECK_MIN_VALUE(std::string key, double value, double warn_value, double error_value, double fatal_value, @@ -67,14 +68,14 @@ class NodeStatusPublisher { std::string key, T value, std::function check_func, std::function value_json_func, std::string description) { - addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, + addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::INVALID_VALUE, description); uint8_t check_result = check_func(value); boost::property_tree::ptree pt = value_json_func(value); std::stringstream ss; write_json(ss, pt); autoware_system_msgs::DiagnosticStatus new_status; - new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + new_status.type = autoware_system_msgs::DiagnosticStatus::INVALID_VALUE; new_status.level = check_result; new_status.description = description; new_status.value = ss.str(); @@ -84,6 +85,9 @@ class NodeStatusPublisher { } void CHECK_RATE(std::string key, double warn_rate, double error_rate, double fatal_rate, std::string description); + uint8_t CHECK_TRUE(std::string key, bool value, uint8_t level, + std::string description); + uint8_t SET_DIAG_STATUS(autoware_system_msgs::DiagnosticStatus status); void NODE_ACTIVATE() { std::lock_guard lock(mtx_); node_activated_ = true; @@ -95,19 +99,28 @@ class NodeStatusPublisher { bool getNodeStatus() { return node_activated_; }; private: + ValueManager value_manager_; std::vector getKeys(); std::vector getRateCheckerKeys(); ros::NodeHandle nh_; ros::NodeHandle pnh_; - std::map> diag_buffers_; - std::map> rate_checkers_; + std::map> diag_buffers_; + std::map> rate_checkers_; ros::Publisher status_pub_; bool keyExist(std::string key); - void addNewBuffer(std::string key, uint8_t type, std::string description); - std::string doubeToJson(double value); + bool addNewBuffer(std::string key, uint8_t type, std::string description); + template std::string valueToJson(T value) { + using namespace boost::property_tree; + std::stringstream ss; + ptree pt; + pt.put("value", value); + write_json(ss, pt); + return ss.str(); + } void publishStatus(); bool node_activated_; + bool ros_ok_; std::mutex mtx_; }; } -#endif // NODE_STATUS_PUBLISHER_H_INCLUDED \ No newline at end of file +#endif // HEALTH_CHECKER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/rate_checker.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/rate_checker.h similarity index 89% rename from ros/src/system/autoware_health_checker/include/autoware_health_checker/rate_checker.h rename to ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/rate_checker.h index 71707a91974..1da68ec0bf9 100644 --- a/ros/src/system/autoware_health_checker/include/autoware_health_checker/rate_checker.h +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/rate_checker.h @@ -44,15 +44,16 @@ class RateChecker { uint8_t getErrorLevel(); boost::optional getRate(); const std::string description; + void setRate(double warn_rate, double error_rate, double fatal_rate); private: ros::Time start_time_; void update(); std::vector data_; - const double buffer_length_; - const double warn_rate_; - const double error_rate_; - const double fatal_rate_; + double buffer_length_; + double warn_rate_; + double error_rate_; + double fatal_rate_; std::mutex mtx_; }; } diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/value_manager.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/value_manager.h new file mode 100644 index 00000000000..d693b85b693 --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_checker/value_manager.h @@ -0,0 +1,59 @@ +#ifndef VALUE_MANAGER_H_INCLUDED +#define VALUE_MANAGER_H_INCLUDED + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * + * v1.0 Masaya Kataoka + */ + +// headers in ROS +#include + +// headers in Autoware +#include + +// headers in STL +#include +#include + +// headers in Boost +#include +#include + +class ValueManager { +public: + ValueManager(ros::NodeHandle nh, ros::NodeHandle pnh); + ~ValueManager(); + void run(); + void stop() { ros_ok_ = false; }; + void setDefaultValue(std::string key, std::string type, double warn_value, + double error_value, double fatal_value); + double getValue(std::string key, std::string type, uint8_t level); + +private: + std::map, uint8_t>, double> + data_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + XmlRpc::XmlRpcValue diag_params_; + void updateParams(); + bool ros_ok_; + bool foundParamKey(std::string key, std::string type, uint8_t level, + double &value); + std::mutex mtx_; +}; +#endif // VALUE_MANAGER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber/system_status_subscriber.h similarity index 100% rename from ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber.h rename to ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber/system_status_subscriber.h diff --git a/ros/src/system/autoware_health_checker/launch/health_checker.launch b/ros/src/system/autoware_health_checker/launch/health_checker.launch index 6c69cd9b362..251a170c1cd 100644 --- a/ros/src/system/autoware_health_checker/launch/health_checker.launch +++ b/ros/src/system/autoware_health_checker/launch/health_checker.launch @@ -1,8 +1,9 @@ + - + \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/health_aggregator.cpp b/ros/src/system/autoware_health_checker/src/health_aggregator/health_aggregator.cpp similarity index 63% rename from ros/src/system/autoware_health_checker/src/health_aggregator.cpp rename to ros/src/system/autoware_health_checker/src/health_aggregator/health_aggregator.cpp index 682fc0098a3..0066177bf09 100644 --- a/ros/src/system/autoware_health_checker/src/health_aggregator.cpp +++ b/ros/src/system/autoware_health_checker/src/health_aggregator/health_aggregator.cpp @@ -17,14 +17,15 @@ * v1.0 Masaya Kataoka */ -#include +#include HealthAggregator::HealthAggregator(ros::NodeHandle nh, ros::NodeHandle pnh) { nh_ = nh; pnh_ = pnh; + ros_ok_ = true; } -HealthAggregator::~HealthAggregator() {} +HealthAggregator::~HealthAggregator() { ros_ok_ = false; } void HealthAggregator::run() { system_status_pub_ = @@ -40,7 +41,7 @@ void HealthAggregator::run() { node_status_sub_ = nh_.subscribe("/node_status", 10, &HealthAggregator::nodeStatusCallback, this); diagnostic_array_sub_ = nh_.subscribe( - "/diagnostic_agg", 10, &HealthAggregator::diagnosticArrayCallback, this); + "/diagnostics_agg", 10, &HealthAggregator::diagnosticArrayCallback, this); topic_statistics_sub_ = nh_.subscribe( "/statistics", 1, &HealthAggregator::topicStatisticsCallback, this); boost::thread publish_thread( @@ -48,6 +49,102 @@ void HealthAggregator::run() { return; } +void HealthAggregator::addTopicDiag() { + for (auto itr = topic_rate_params_.begin(); itr != topic_rate_params_.end(); + itr++) { + std::string key = itr->first; + XmlRpc::XmlRpcValue data = itr->second["sub_node"]; + std::string sub_node = data; + data = itr->second["pub_node"]; + std::string pub_node = data; + data = itr->second["topic_name"]; + std::string topic_name = data; + data = itr->second["topic_rate"]["warn"]; + double warn_topic_rate = data; + data = itr->second["topic_rate"]["error"]; + double error_topic_rate = data; + data = itr->second["topic_rate"]["fatal"]; + double fatal_topic_rate = data; + autoware_system_msgs::DiagnosticStatus diag_topic_rate; + diag_topic_rate.key = key; + diag_topic_rate.description = topic_name + " in " + sub_node + " from " + + pub_node + " topic rate is slow"; + diag_topic_rate.type = diag_topic_rate.TOPIC_RATE_IS_SLOW; + std::array query_key = {topic_name, pub_node, sub_node}; + if (topic_status_.count(query_key) != 0) { + rosgraph_msgs::TopicStatistics stat = topic_status_[query_key]; + double topic_rate = + (double)stat.delivered_msgs / + (double)(stat.window_stop - stat.window_start).toSec(); + diag_topic_rate.header.stamp = stat.window_stop; + diag_topic_rate.key = key; + diag_topic_rate.value = valueToJson(topic_rate); + if (topic_rate < fatal_topic_rate) { + diag_topic_rate.level = autoware_health_checker::LEVEL_FATAL; + } else if (topic_rate < error_topic_rate) { + diag_topic_rate.level = autoware_health_checker::LEVEL_ERROR; + } else if (topic_rate < error_topic_rate) { + diag_topic_rate.level = autoware_health_checker::LEVEL_WARN; + } else { + diag_topic_rate.level = autoware_health_checker::LEVEL_OK; + } + for (auto node_status_itr = system_status_.node_status.begin(); + node_status_itr != system_status_.node_status.end(); + node_status_itr++) { + autoware_system_msgs::DiagnosticStatusArray diag; + diag.status.push_back(diag_topic_rate); + node_status_itr->status.push_back(diag); + } + } + } + for (auto itr = drop_rate_params_.begin(); itr != drop_rate_params_.end(); + itr++) { + std::string key = itr->first; + XmlRpc::XmlRpcValue data = itr->second["sub_node"]; + std::string sub_node = data; + data = itr->second["pub_node"]; + std::string pub_node = data; + data = itr->second["topic_name"]; + std::string topic_name = data; + data = itr->second["drop_rate"]["warn"]; + double warn_drop_rate = data; + data = itr->second["drop_rate"]["error"]; + double error_drop_rate = data; + data = itr->second["drop_rate"]["fatal"]; + double fatal_drop_rate = data; + autoware_system_msgs::DiagnosticStatus diag_drop_rate; + diag_drop_rate.key = key; + diag_drop_rate.description = topic_name + " in " + sub_node + " from " + + pub_node + " message deop rate is too high"; + diag_drop_rate.type = diag_drop_rate.TOPIC_DROP_RATE_IS_HIGH; + std::array query_key = {topic_name, pub_node, sub_node}; + if (topic_status_.count(query_key) != 0) { + rosgraph_msgs::TopicStatistics stat = topic_status_[query_key]; + double drop_rate = (double)stat.dropped_msgs / + (double)(stat.dropped_msgs + stat.delivered_msgs); + diag_drop_rate.header.stamp = stat.window_stop; + diag_drop_rate.key = key; + diag_drop_rate.value = valueToJson(drop_rate); + if (drop_rate > fatal_drop_rate) { + diag_drop_rate.level = autoware_health_checker::LEVEL_FATAL; + } else if (drop_rate > error_drop_rate) { + diag_drop_rate.level = autoware_health_checker::LEVEL_ERROR; + } else if (drop_rate > error_drop_rate) { + diag_drop_rate.level = autoware_health_checker::LEVEL_WARN; + } else { + diag_drop_rate.level = autoware_health_checker::LEVEL_OK; + } + for (auto node_status_itr = system_status_.node_status.begin(); + node_status_itr != system_status_.node_status.end(); + node_status_itr++) { + autoware_system_msgs::DiagnosticStatusArray diag; + diag.status.push_back(diag_drop_rate); + node_status_itr->status.push_back(diag); + } + } + } +} + void HealthAggregator::topicStatisticsCallback( const rosgraph_msgs::TopicStatistics::ConstPtr msg) { std::array key = {msg->topic, msg->node_pub, msg->node_sub}; @@ -64,13 +161,16 @@ void HealthAggregator::updateTopicStatus() { system_status_.topic_statistics.push_back(pair.second); } } + addTopicDiag(); return; } void HealthAggregator::publishSystemStatus() { ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); - while (ros::ok()) { + while (ros_ok_) { mtx_.lock(); + nh_.getParam("/health_checker/topic_rate", topic_rate_params_); + nh_.getParam("/health_checker/drop_rate", drop_rate_params_); system_status_.header.stamp = ros::Time::now(); updateConnectionStatus(); updateTopicStatus(); diff --git a/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp b/ros/src/system/autoware_health_checker/src/health_aggregator/health_aggregator_node.cpp similarity index 92% rename from ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp rename to ros/src/system/autoware_health_checker/src/health_aggregator/health_aggregator_node.cpp index 802dcaeab9f..4f69074ada7 100644 --- a/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp +++ b/ros/src/system/autoware_health_checker/src/health_aggregator/health_aggregator_node.cpp @@ -19,7 +19,7 @@ #include -#include +#include int main(int argc, char *argv[]) { ros::init(argc, argv, "health_aggregator"); diff --git a/ros/src/system/autoware_health_checker/src/health_analyzer.cpp b/ros/src/system/autoware_health_checker/src/health_analyzer/health_analyzer.cpp similarity index 97% rename from ros/src/system/autoware_health_checker/src/health_analyzer.cpp rename to ros/src/system/autoware_health_checker/src/health_analyzer/health_analyzer.cpp index c4ed5e4546f..9278f3e2eb9 100644 --- a/ros/src/system/autoware_health_checker/src/health_analyzer.cpp +++ b/ros/src/system/autoware_health_checker/src/health_analyzer/health_analyzer.cpp @@ -17,12 +17,12 @@ * v1.0 Masaya Kataoka */ -#include +#include HealthAnalyzer::HealthAnalyzer(ros::NodeHandle nh, ros::NodeHandle pnh) { nh_ = nh; pnh_ = pnh; - pnh_.param("warn_nodes_count_threshold", warn_nodes_count_threshold_, 30); + pnh_.param("warn_count_threshold", warn_count_threshold_, 30); system_status_summary_pub_ = nh_.advertise( "/system_status/summary", 1); @@ -144,7 +144,7 @@ HealthAnalyzer::filterSystemStatus(autoware_system_msgs::SystemStatus status) { int warn_count = countWarn(status); autoware_system_msgs::SystemStatus filtered_status; std::vector target_nodes; - if (warn_count >= warn_nodes_count_threshold_) { + if (warn_count >= warn_count_threshold_) { filtered_status.detect_too_match_warning = true; target_nodes = findWarningNodes(status); } else { diff --git a/ros/src/system/autoware_health_checker/src/health_analyzer_node.cpp b/ros/src/system/autoware_health_checker/src/health_analyzer/health_analyzer_node.cpp similarity index 92% rename from ros/src/system/autoware_health_checker/src/health_analyzer_node.cpp rename to ros/src/system/autoware_health_checker/src/health_analyzer/health_analyzer_node.cpp index 3c66c7e8a74..b26d8aeae76 100644 --- a/ros/src/system/autoware_health_checker/src/health_analyzer_node.cpp +++ b/ros/src/system/autoware_health_checker/src/health_analyzer/health_analyzer_node.cpp @@ -19,7 +19,7 @@ #include -#include +#include int main(int argc, char *argv[]) { ros::init(argc, argv, "health_analyzer"); diff --git a/ros/src/system/autoware_health_checker/src/diag_buffer.cpp b/ros/src/system/autoware_health_checker/src/health_checker/diag_buffer.cpp similarity index 98% rename from ros/src/system/autoware_health_checker/src/diag_buffer.cpp rename to ros/src/system/autoware_health_checker/src/health_checker/diag_buffer.cpp index e93d38aa7ba..abbbf8b5cff 100644 --- a/ros/src/system/autoware_health_checker/src/diag_buffer.cpp +++ b/ros/src/system/autoware_health_checker/src/health_checker/diag_buffer.cpp @@ -17,7 +17,7 @@ * v1.0 Masaya Kataoka */ -#include +#include namespace autoware_health_checker { DiagBuffer::DiagBuffer(std::string key, uint8_t type, std::string description, diff --git a/ros/src/system/autoware_health_checker/src/health_checker/health_checker.cpp b/ros/src/system/autoware_health_checker/src/health_checker/health_checker.cpp new file mode 100644 index 00000000000..c0496cfe206 --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/health_checker/health_checker.cpp @@ -0,0 +1,286 @@ +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * + * v1.0 Masaya Kataoka + */ + +#include + +namespace autoware_health_checker { +HealthChecker::HealthChecker(ros::NodeHandle nh, ros::NodeHandle pnh) + : value_manager_(nh, pnh) { + node_activated_ = false; + ros_ok_ = true; + nh_ = nh; + pnh_ = pnh; + value_manager_.run(); + status_pub_ = + nh_.advertise("node_status", 10); +} + +HealthChecker::~HealthChecker() { + ros_ok_ = false; + value_manager_.stop(); +} + +void HealthChecker::publishStatus() { + ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); + while (ros_ok_) { + mtx_.lock(); + autoware_system_msgs::NodeStatus status; + status.node_activated = node_activated_; + ros::Time now = ros::Time::now(); + status.header.stamp = now; + status.node_name = ros::this_node::getName(); + std::vector checker_keys = getRateCheckerKeys(); + // iterate Rate checker and publish rate_check result + for (auto key_itr = checker_keys.begin(); key_itr != checker_keys.end(); + key_itr++) { + autoware_system_msgs::DiagnosticStatusArray diag_array; + autoware_system_msgs::DiagnosticStatus diag; + diag.type = autoware_system_msgs::DiagnosticStatus::LOOP_RATE_IS_SLOW; + std::pair result = + rate_checkers_[*key_itr]->getErrorLevelAndRate(); + diag.level = result.first; + diag.key = *key_itr; + diag.value = valueToJson(result.second); + diag.description = rate_checkers_[*key_itr]->description; + diag.header.stamp = now; + diag_array.status.push_back(diag); + status.status.push_back(diag_array); + } + // iterate Diagnostic Buffer and publish all diagnostic data + std::vector keys = getKeys(); + for (auto key_itr = keys.begin(); key_itr != keys.end(); key_itr++) { + status.status.push_back(diag_buffers_[*key_itr]->getAndClearData()); + } + status_pub_.publish(status); + mtx_.unlock(); + rate.sleep(); + } + return; +} + +uint8_t +HealthChecker::SET_DIAG_STATUS(autoware_system_msgs::DiagnosticStatus status) { + using namespace autoware_health_checker; + if (status.level == LEVEL_OK || status.level == LEVEL_WARN || + status.level == LEVEL_ERROR || status.level == LEVEL_FATAL) { + addNewBuffer(status.key, status.type, status.description); + diag_buffers_[status.key]->addDiag(status); + return status.level; + } else { + return LEVEL_UNDEFINED; + } +} + +uint8_t HealthChecker::CHECK_TRUE(std::string key, bool value, uint8_t level, + std::string description) { + using namespace autoware_health_checker; + if (level == LEVEL_OK || level == LEVEL_WARN || level == LEVEL_ERROR || + level == LEVEL_FATAL) { + autoware_system_msgs::DiagnosticStatus status; + status.key = key; + status.value = valueToJson(value); + status.level = level; + status.header.stamp = ros::Time::now(); + addNewBuffer(key, status.INVALID_LOGICAL_VALUE, description); + diag_buffers_[status.key]->addDiag(status); + return level; + } else { + return LEVEL_UNDEFINED; + } +} + +void HealthChecker::ENABLE() { + boost::thread publish_thread( + boost::bind(&HealthChecker::publishStatus, this)); + return; +} + +std::vector HealthChecker::getKeys() { + std::vector keys; + std::vector checker_keys = getRateCheckerKeys(); + std::pair> buf_itr; + for (auto buf_itr = diag_buffers_.begin(); buf_itr != diag_buffers_.end(); + buf_itr++) { + bool matched = false; + for (auto checker_key_itr = checker_keys.begin(); + checker_key_itr != checker_keys.end(); checker_key_itr++) { + if (*checker_key_itr == buf_itr->first) { + matched = true; + } + } + if (!matched) { + keys.push_back(buf_itr->first); + } + } + return keys; +} + +std::vector HealthChecker::getRateCheckerKeys() { + std::vector keys; + std::pair> checker_itr; + for (auto itr = rate_checkers_.begin(); itr != rate_checkers_.end(); itr++) { + keys.push_back(itr->first); + } + return keys; +} + +bool HealthChecker::keyExist(std::string key) { + if (diag_buffers_.count(key) == 0) { + return false; + } + return true; +} + +// add New Diagnostic Buffer if the key does not exist +bool HealthChecker::addNewBuffer(std::string key, uint8_t type, + std::string description) { + mtx_.lock(); + if (!keyExist(key)) { + std::unique_ptr buf_ptr(new DiagBuffer( + key, type, description, autoware_health_checker::BUFFER_LENGTH)); + diag_buffers_[key] = std::move(buf_ptr); + mtx_.unlock(); + return true; + } + mtx_.unlock(); + return false; +} + +uint8_t HealthChecker::CHECK_MIN_VALUE(std::string key, double value, + double warn_value, double error_value, + double fatal_value, + std::string description) { + value_manager_.setDefaultValue(key, "min", warn_value, error_value, + fatal_value); + addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, + description); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + if (value < value_manager_.getValue(key, "min", + autoware_health_checker::LEVEL_FATAL)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; + } else if (value < value_manager_.getValue( + key, "min", autoware_health_checker::LEVEL_ERROR)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; + } else if (value < value_manager_.getValue( + key, "min", autoware_health_checker::LEVEL_WARN)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; + } else { + new_status.level = autoware_system_msgs::DiagnosticStatus::OK; + } + new_status.key = key; + new_status.description = description; + new_status.value = valueToJson(value); + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; +} + +uint8_t HealthChecker::CHECK_MAX_VALUE(std::string key, double value, + double warn_value, double error_value, + double fatal_value, + std::string description) { + value_manager_.setDefaultValue(key, "max", warn_value, error_value, + fatal_value); + addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, + description); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + if (value > value_manager_.getValue(key, "max", + autoware_health_checker::LEVEL_FATAL)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; + } else if (value > value_manager_.getValue( + key, "max", autoware_health_checker::LEVEL_ERROR)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; + } else if (value > value_manager_.getValue( + key, "max", autoware_health_checker::LEVEL_WARN)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; + } else { + new_status.level = autoware_system_msgs::DiagnosticStatus::OK; + } + new_status.key = key; + new_status.description = description; + new_status.value = valueToJson(value); + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; +} + +uint8_t HealthChecker::CHECK_RANGE(std::string key, double value, + std::pair warn_value, + std::pair error_value, + std::pair fatal_value, + std::string description) { + value_manager_.setDefaultValue(key, "min", warn_value.first, + error_value.first, fatal_value.first); + value_manager_.setDefaultValue(key, "max", warn_value.second, + error_value.second, fatal_value.second); + addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, + description); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + if (value < value_manager_.getValue(key, "min", + autoware_health_checker::LEVEL_FATAL) || + value > value_manager_.getValue(key, "max", + autoware_health_checker::LEVEL_FATAL)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; + } else if (value < value_manager_.getValue( + key, "min", autoware_health_checker::LEVEL_ERROR) || + value > value_manager_.getValue( + key, "max", autoware_health_checker::LEVEL_ERROR)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; + } else if (value < value_manager_.getValue( + key, "min", autoware_health_checker::LEVEL_WARN) || + value > value_manager_.getValue( + key, "max", autoware_health_checker::LEVEL_WARN)) { + new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; + } else { + new_status.level = autoware_system_msgs::DiagnosticStatus::OK; + } + new_status.key = key; + new_status.value = valueToJson(value); + new_status.description = description; + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; +} + +void HealthChecker::CHECK_RATE(std::string key, double warn_rate, + double error_rate, double fatal_rate, + std::string description) { + if (!keyExist(key)) { + value_manager_.setDefaultValue(key, "rate", warn_rate, error_rate, + fatal_rate); + std::unique_ptr checker_ptr( + new RateChecker(autoware_health_checker::BUFFER_LENGTH, warn_rate, + error_rate, fatal_rate, description)); + rate_checkers_[key] = std::move(checker_ptr); + } + addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::LOOP_RATE_IS_SLOW, + description); + rate_checkers_[key]->setRate( + value_manager_.getValue(key, "rate", autoware_health_checker::LEVEL_WARN), + value_manager_.getValue(key, "rate", + autoware_health_checker::LEVEL_ERROR), + value_manager_.getValue(key, "rate", + autoware_health_checker::LEVEL_FATAL)); + rate_checkers_[key]->check(); + return; +} +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/rate_checker.cpp b/ros/src/system/autoware_health_checker/src/health_checker/rate_checker.cpp similarity index 90% rename from ros/src/system/autoware_health_checker/src/rate_checker.cpp rename to ros/src/system/autoware_health_checker/src/health_checker/rate_checker.cpp index 9a7e22e8d1e..64ee759063a 100644 --- a/ros/src/system/autoware_health_checker/src/rate_checker.cpp +++ b/ros/src/system/autoware_health_checker/src/health_checker/rate_checker.cpp @@ -17,7 +17,7 @@ * v1.0 Masaya Kataoka */ -#include +#include namespace autoware_health_checker { RateChecker::RateChecker(double buffer_length, double warn_rate, @@ -65,11 +65,23 @@ uint8_t RateChecker::getErrorLevel() { return autoware_health_checker::LEVEL_OK; } +void RateChecker::setRate(double warn_rate, double error_rate, + double fatal_rate) { + update(); + mtx_.lock(); + warn_rate_ = warn_rate; + error_rate_ = error_rate; + fatal_rate_ = fatal_rate; + mtx_.unlock(); + return; +} + void RateChecker::check() { update(); mtx_.lock(); data_.push_back(ros::Time::now()); mtx_.unlock(); + return; } void RateChecker::update() { diff --git a/ros/src/system/autoware_health_checker/src/health_checker/value_manager.cpp b/ros/src/system/autoware_health_checker/src/health_checker/value_manager.cpp new file mode 100644 index 00000000000..be2a650ce1c --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/health_checker/value_manager.cpp @@ -0,0 +1,90 @@ +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * + * v1.0 Masaya Kataoka + */ + +#include + +ValueManager::ValueManager(ros::NodeHandle nh, ros::NodeHandle pnh) { + nh_ = nh; + pnh_ = pnh; + nh_.getParam("/health_checker", diag_params_); + ros_ok_ = true; +} + +ValueManager::~ValueManager() { ros_ok_ = false; } + +void ValueManager::run() { + boost::thread update_thread = + boost::thread(boost::bind(&ValueManager::updateParams, this)); +} + +void ValueManager::setDefaultValue(std::string key, std::string type, + double warn_value, double error_value, + double fatal_value) { + data_[{{key, type}, autoware_health_checker::LEVEL_WARN}] = warn_value; + data_[{{key, type}, autoware_health_checker::LEVEL_ERROR}] = error_value; + data_[{{key, type}, autoware_health_checker::LEVEL_FATAL}] = fatal_value; + return; +} + +bool ValueManager::foundParamKey(std::string key, std::string type, + uint8_t level, double &value) { + for (auto itr = diag_params_.begin(); itr != diag_params_.end(); itr++) { + if (itr->first == key) { + if (level == autoware_health_checker::LEVEL_WARN) { + XmlRpc::XmlRpcValue data = itr->second[type]["warn"]; + value = data; + return true; + } + if (level == autoware_health_checker::LEVEL_ERROR) { + XmlRpc::XmlRpcValue data = itr->second[type]["error"]; + value = data; + return true; + } + if (level == autoware_health_checker::LEVEL_FATAL) { + XmlRpc::XmlRpcValue data = itr->second[type]["fatal"]; + value = data; + return true; + } + } + } + return false; +} + +double ValueManager::getValue(std::string key, std::string type, + uint8_t level) { + double ret; + mtx_.lock(); + if (foundParamKey(key, type, level, ret)) { + } else { + ret = data_[{{key, type}, level}]; + } + mtx_.unlock(); + return ret; +} + +void ValueManager::updateParams() { + ros::Rate rate(1); + while (ros_ok_) { + mtx_.lock(); + nh_.getParam("health_checker", diag_params_); + mtx_.unlock(); + rate.sleep(); + } + return; +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp b/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp deleted file mode 100644 index a088ee6bd6b..00000000000 --- a/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp +++ /dev/null @@ -1,222 +0,0 @@ -/* - * Copyright 2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - * - * v1.0 Masaya Kataoka - */ - -#include - -namespace autoware_health_checker { -NodeStatusPublisher::NodeStatusPublisher(ros::NodeHandle nh, - ros::NodeHandle pnh) { - node_activated_ = false; - nh_ = nh; - pnh_ = pnh; - status_pub_ = - nh_.advertise("node_status", 10); -} - -NodeStatusPublisher::~NodeStatusPublisher() {} - -void NodeStatusPublisher::publishStatus() { - ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); - while (ros::ok()) { - mtx_.lock(); - autoware_system_msgs::NodeStatus status; - status.node_activated = node_activated_; - ros::Time now = ros::Time::now(); - status.header.stamp = now; - status.node_name = ros::this_node::getName(); - std::vector checker_keys = getRateCheckerKeys(); - // iterate Rate checker and publish rate_check result - for (auto key_itr = checker_keys.begin(); key_itr != checker_keys.end(); - key_itr++) { - autoware_system_msgs::DiagnosticStatusArray diag_array; - autoware_system_msgs::DiagnosticStatus diag; - diag.type = autoware_system_msgs::DiagnosticStatus::RATE_IS_SLOW; - std::pair result = - rate_checkers_[*key_itr]->getErrorLevelAndRate(); - diag.level = result.first; - diag.key = *key_itr; - diag.value = doubeToJson(result.second); - diag.description = rate_checkers_[*key_itr]->description; - diag.header.stamp = now; - diag_array.status.push_back(diag); - status.status.push_back(diag_array); - } - // iterate Diagnostic Buffer and publish all diagnostic data - std::vector keys = getKeys(); - for (auto key_itr = keys.begin(); key_itr != keys.end(); key_itr++) { - status.status.push_back(diag_buffers_[*key_itr]->getAndClearData()); - } - status_pub_.publish(status); - mtx_.unlock(); - rate.sleep(); - } - return; -} - -void NodeStatusPublisher::ENABLE() { - boost::thread publish_thread( - boost::bind(&NodeStatusPublisher::publishStatus, this)); - return; -} - -std::vector NodeStatusPublisher::getKeys() { - std::vector keys; - std::vector checker_keys = getRateCheckerKeys(); - std::pair> buf_itr; - BOOST_FOREACH (buf_itr, diag_buffers_) { - bool matched = false; - for (auto checker_key_itr = checker_keys.begin(); - checker_key_itr != checker_keys.end(); checker_key_itr++) { - if (*checker_key_itr == buf_itr.first) { - matched = true; - } - } - if (!matched) { - keys.push_back(buf_itr.first); - } - } - return keys; -} - -std::vector NodeStatusPublisher::getRateCheckerKeys() { - std::vector keys; - std::pair> checker_itr; - BOOST_FOREACH (checker_itr, rate_checkers_) { - keys.push_back(checker_itr.first); - } - return keys; -} - -bool NodeStatusPublisher::keyExist(std::string key) { - if (diag_buffers_.count(key) == 0) { - return false; - } - return true; -} - -// add New Diagnostic Buffer if the key does not exist -void NodeStatusPublisher::addNewBuffer(std::string key, uint8_t type, - std::string description) { - if (!keyExist(key)) { - std::shared_ptr buf_ptr = std::make_shared( - key, type, description, autoware_health_checker::BUFFER_LENGTH); - diag_buffers_[key] = buf_ptr; - } - return; -} - -uint8_t NodeStatusPublisher::CHECK_MIN_VALUE(std::string key, double value, - double warn_value, - double error_value, - double fatal_value, - std::string description) { - addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, - description); - autoware_system_msgs::DiagnosticStatus new_status; - new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; - if (value < fatal_value) { - new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; - } else if (value < error_value) { - new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; - } else if (value < warn_value) { - new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; - } else { - new_status.level = autoware_system_msgs::DiagnosticStatus::OK; - } - new_status.description = description; - new_status.value = doubeToJson(value); - new_status.header.stamp = ros::Time::now(); - diag_buffers_[key]->addDiag(new_status); - return new_status.level; -} - -uint8_t NodeStatusPublisher::CHECK_MAX_VALUE(std::string key, double value, - double warn_value, - double error_value, - double fatal_value, - std::string description) { - addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, - description); - autoware_system_msgs::DiagnosticStatus new_status; - new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; - if (value > fatal_value) { - new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; - } else if (value > error_value) { - new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; - } else if (value > warn_value) { - new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; - } else { - new_status.level = autoware_system_msgs::DiagnosticStatus::OK; - } - new_status.description = description; - new_status.value = doubeToJson(value); - new_status.header.stamp = ros::Time::now(); - diag_buffers_[key]->addDiag(new_status); - return new_status.level; -} - -uint8_t NodeStatusPublisher::CHECK_RANGE(std::string key, double value, - std::pair warn_value, - std::pair error_value, - std::pair fatal_value, - std::string description) { - addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE, - description); - autoware_system_msgs::DiagnosticStatus new_status; - new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; - if (value < fatal_value.first || value > fatal_value.second) { - new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; - } else if (value < error_value.first || value > error_value.second) { - new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; - } else if (value < warn_value.first || value > warn_value.second) { - new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; - } else { - new_status.level = autoware_system_msgs::DiagnosticStatus::OK; - } - new_status.value = doubeToJson(value); - new_status.description = description; - new_status.header.stamp = ros::Time::now(); - diag_buffers_[key]->addDiag(new_status); - return new_status.level; -} - -void NodeStatusPublisher::CHECK_RATE(std::string key, double warn_rate, - double error_rate, double fatal_rate, - std::string description) { - if (!keyExist(key)) { - std::shared_ptr checker_ptr = std::make_shared( - autoware_health_checker::BUFFER_LENGTH, warn_rate, error_rate, - fatal_rate, description); - rate_checkers_[key] = checker_ptr; - } - addNewBuffer(key, autoware_system_msgs::DiagnosticStatus::RATE_IS_SLOW, - description); - rate_checkers_[key]->check(); - return; -} - -std::string NodeStatusPublisher::doubeToJson(double value) { - using namespace boost::property_tree; - std::stringstream ss; - ptree pt; - pt.put("value.double", value); - write_json(ss, pt); - return ss.str(); -} -} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp b/ros/src/system/autoware_health_checker/src/system_status_subscriber/system_status_subscriber.cpp similarity index 95% rename from ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp rename to ros/src/system/autoware_health_checker/src/system_status_subscriber/system_status_subscriber.cpp index 8139c9f2c8a..d2c41dfa9a0 100644 --- a/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp +++ b/ros/src/system/autoware_health_checker/src/system_status_subscriber/system_status_subscriber.cpp @@ -17,7 +17,7 @@ * v1.0 Masaya Kataoka */ -#include +#include namespace autoware_health_checker { SystemStatusSubscriber::SystemStatusSubscriber(ros::NodeHandle nh, diff --git a/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp b/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp index be73533a9df..af20badbdcd 100644 --- a/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp +++ b/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp @@ -16,8 +16,8 @@ * * v1.0 Masaya Kataoka */ -#include #include +#include #include class AutowareHealthCheckerTestSuite : public ::testing::Test { @@ -29,88 +29,16 @@ class AutowareHealthCheckerTestSuite : public ::testing::Test { class AutowareHealthCheckerTestClass { public: - AutowareHealthCheckerTestClass() { - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - node_status_publisher_ptr = - std::make_shared(nh, pnh); + AutowareHealthCheckerTestClass() : pnh("~") { + health_checker_ptr = + std::make_shared(nh, pnh); }; - std::shared_ptr - node_status_publisher_ptr; + std::shared_ptr health_checker_ptr; + ros::NodeHandle pnh; + ros::NodeHandle nh; + ~AutowareHealthCheckerTestClass(){}; }; -TEST(TestSuite, CHECK_MIN_VALUE) { - AutowareHealthCheckerTestClass test_autoware_health_checker; - uint8_t ret_fatal = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE( - "test", 1, 6, 4, 2, "test"); - ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) - << "The value was self-diagnosed as fatal"; - uint8_t ret_error = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE( - "test", 3, 6, 4, 2, "test"); - ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) - << "The value was self-diagnosed as error"; - uint8_t ret_warn = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE( - "test", 5, 6, 4, 2, "test"); - ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) - << "The value was self-diagnosed as warn"; - uint8_t ret_ok = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE( - "test", 7, 6, 4, 2, "test"); - ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) - << "The value was self-diagnosed as ok"; -} - -TEST(TestSuite, CHECK_MAX_VALUE) { - AutowareHealthCheckerTestClass test_autoware_health_checker; - uint8_t ret_fatal = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE( - "test", 7, 2, 4, 6, "test"); - ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) - << "The value was self-diagnosed as fatal"; - uint8_t ret_error = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE( - "test", 5, 2, 4, 6, "test"); - ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) - << "The value was self-diagnosed as error"; - uint8_t ret_warn = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE( - "test", 3, 2, 4, 6, "test"); - ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) - << "The value was self-diagnosed as warn"; - uint8_t ret_ok = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE( - "test", 1, 2, 4, 6, "test"); - ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) - << "The value was self-diagnosed as ok"; -} - -TEST(TestSuite, CHECK_RANGE) { - AutowareHealthCheckerTestClass test_autoware_health_checker; - uint8_t ret_fatal = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE( - "test", 7.0, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); - ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) - << "The value was self-diagnosed as fatal"; - uint8_t ret_error = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE( - "test", 5.5, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); - ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) - << "The value was self-diagnosed as error"; - uint8_t ret_warn = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE( - "test", 4.5, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); - ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) - << "The value was self-diagnosed as warn"; - uint8_t ret_ok = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE( - "test", 3.0, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); - ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) - << "The value was self-diagnosed as ok"; -} - uint8_t test_function(double value) { if (value == 0.0) { return autoware_health_checker::LEVEL_FATAL; @@ -130,51 +58,156 @@ boost::property_tree::ptree test_value_json_func(double value) { return tree; }; -TEST(TestSuite, CHECK_VALUE) { +TEST(TestSuite, CHECK_HEALTH_CHECK_FUNCS) { AutowareHealthCheckerTestClass test_autoware_health_checker; + /* + test for value check function + */ + std::function check_func = test_function; std::function check_value_json_func = test_value_json_func; - uint8_t ret_fatal = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE( + uint8_t ret_fatal_value = + test_autoware_health_checker.health_checker_ptr->CHECK_VALUE( "test", 0.0, check_func, check_value_json_func, "test"); - ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) + ASSERT_EQ(ret_fatal_value, autoware_health_checker::LEVEL_FATAL) << "The value was self-diagnosed as fatal"; - uint8_t ret_error = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE( + uint8_t ret_error_value = + test_autoware_health_checker.health_checker_ptr->CHECK_VALUE( "test", 1.0, check_func, check_value_json_func, "test"); - ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) + ASSERT_EQ(ret_error_value, autoware_health_checker::LEVEL_ERROR) << "The value was self-diagnosed as fatal"; - uint8_t ret_warn = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE( + uint8_t ret_warn_value = + test_autoware_health_checker.health_checker_ptr->CHECK_VALUE( "test", 2.0, check_func, check_value_json_func, "test"); - ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) + ASSERT_EQ(ret_warn_value, autoware_health_checker::LEVEL_WARN) << "The value was self-diagnosed as fatal"; - uint8_t ret_ok = - test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE( + uint8_t ret_ok_value = + test_autoware_health_checker.health_checker_ptr->CHECK_VALUE( "test", -1.0, check_func, check_value_json_func, "test"); - ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) + ASSERT_EQ(ret_ok_value, autoware_health_checker::LEVEL_OK) << "The value was self-diagnosed as fatal"; boost::optional value = check_value_json_func(0.0).get_optional("value"); ASSERT_EQ(value.get(), 0.0) << "The value must be true, failed to get json value"; + + /* + test for minimum value check function + */ + uint8_t ret_fatal_min = + test_autoware_health_checker.health_checker_ptr->CHECK_MIN_VALUE( + "test", 1, 6, 4, 2, "test"); + ASSERT_EQ(ret_fatal_min, autoware_health_checker::LEVEL_FATAL) + << "The value was self-diagnosed as fatal"; + uint8_t ret_error_min = + test_autoware_health_checker.health_checker_ptr->CHECK_MIN_VALUE( + "test", 3, 6, 4, 2, "test"); + ASSERT_EQ(ret_error_min, autoware_health_checker::LEVEL_ERROR) + << "The value was self-diagnosed as error"; + uint8_t ret_warn_min = + test_autoware_health_checker.health_checker_ptr->CHECK_MIN_VALUE( + "test", 5, 6, 4, 2, "test"); + ASSERT_EQ(ret_warn_min, autoware_health_checker::LEVEL_WARN) + << "The value was self-diagnosed as warn"; + uint8_t ret_ok_min = + test_autoware_health_checker.health_checker_ptr->CHECK_MIN_VALUE( + "test", 7, 6, 4, 2, "test"); + ASSERT_EQ(ret_ok_min, autoware_health_checker::LEVEL_OK) + << "The value was self-diagnosed as ok"; + + /* + test for maximum value check function + */ + uint8_t ret_fatal_max = + test_autoware_health_checker.health_checker_ptr->CHECK_MAX_VALUE( + "test", 7, 2, 4, 6, "test"); + ASSERT_EQ(ret_fatal_max, autoware_health_checker::LEVEL_FATAL) + << "The value was self-diagnosed as fatal"; + uint8_t ret_error_max = + test_autoware_health_checker.health_checker_ptr->CHECK_MAX_VALUE( + "test", 5, 2, 4, 6, "test"); + ASSERT_EQ(ret_error_max, autoware_health_checker::LEVEL_ERROR) + << "The value was self-diagnosed as error"; + uint8_t ret_warn_max = + test_autoware_health_checker.health_checker_ptr->CHECK_MAX_VALUE( + "test", 3, 2, 4, 6, "test"); + ASSERT_EQ(ret_warn_max, autoware_health_checker::LEVEL_WARN) + << "The value was self-diagnosed as warn"; + uint8_t ret_ok_max = + test_autoware_health_checker.health_checker_ptr->CHECK_MAX_VALUE( + "test", 1, 2, 4, 6, "test"); + ASSERT_EQ(ret_ok_max, autoware_health_checker::LEVEL_OK) + << "The value was self-diagnosed as ok"; + + /* + test for range check functions + */ + uint8_t ret_fatal_range = + test_autoware_health_checker.health_checker_ptr->CHECK_RANGE( + "test", 7.0, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); + ASSERT_EQ(ret_fatal_range, autoware_health_checker::LEVEL_FATAL) + << "The value was self-diagnosed as fatal"; + uint8_t ret_error_range = + test_autoware_health_checker.health_checker_ptr->CHECK_RANGE( + "test", 5.5, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); + ASSERT_EQ(ret_error_range, autoware_health_checker::LEVEL_ERROR) + << "The value was self-diagnosed as error"; + uint8_t ret_warn_range = + test_autoware_health_checker.health_checker_ptr->CHECK_RANGE( + "test", 4.5, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); + ASSERT_EQ(ret_warn_range, autoware_health_checker::LEVEL_WARN) + << "The value was self-diagnosed as warn"; + uint8_t ret_ok_range = + test_autoware_health_checker.health_checker_ptr->CHECK_RANGE( + "test", 3.0, {2.0, 4.0}, {1.0, 5.0}, {0.0, 6.0}, "test"); + ASSERT_EQ(ret_ok_range, autoware_health_checker::LEVEL_OK) + << "The value was self-diagnosed as ok"; + + /* + test for set diag function + */ + autoware_system_msgs::DiagnosticStatus status; + status.level = status.FATAL; + uint8_t ret_diag_fatal = + test_autoware_health_checker.health_checker_ptr->SET_DIAG_STATUS(status); + ASSERT_EQ(ret_diag_fatal, autoware_health_checker::LEVEL_FATAL) + << "The value was self-diagnosed as fatal"; + status.level = status.ERROR; + uint8_t ret_diag_error = + test_autoware_health_checker.health_checker_ptr->SET_DIAG_STATUS(status); + ASSERT_EQ(ret_diag_error, autoware_health_checker::LEVEL_ERROR) + << "The value was self-diagnosed as error"; + status.level = status.WARN; + uint8_t ret_diag_warn = + test_autoware_health_checker.health_checker_ptr->SET_DIAG_STATUS(status); + ASSERT_EQ(ret_diag_warn, autoware_health_checker::LEVEL_WARN) + << "The value was self-diagnosed as warn"; + status.level = status.OK; + uint8_t ret_diag_ok = + test_autoware_health_checker.health_checker_ptr->SET_DIAG_STATUS(status); + ASSERT_EQ(ret_diag_ok, autoware_health_checker::LEVEL_OK) + << "The value was self-diagnosed as ok"; } +/* + test for node status +*/ TEST(TestSuite, NODE_STATUS) { AutowareHealthCheckerTestClass test_autoware_health_checker; - test_autoware_health_checker.node_status_publisher_ptr->NODE_ACTIVATE(); + test_autoware_health_checker.health_checker_ptr->NODE_ACTIVATE(); uint8_t ret_active = - test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); + test_autoware_health_checker.health_checker_ptr->getNodeStatus(); ASSERT_EQ(ret_active, true) << "The value must be true"; - test_autoware_health_checker.node_status_publisher_ptr->NODE_DEACTIVATE(); + test_autoware_health_checker.health_checker_ptr->NODE_DEACTIVATE(); uint8_t ret_inactive = - test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); + test_autoware_health_checker.health_checker_ptr->getNodeStatus(); ASSERT_EQ(ret_inactive, false) << "The value must be true"; } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "AutowareHealthCheckerTestNode"); - return RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); + return result; } \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test b/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test index d87a24411dd..d322cb8b78c 100644 --- a/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test +++ b/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test @@ -1,3 +1,3 @@ - + \ No newline at end of file