From 4df2c39821253e4077b7dfc679db1ea4aaa10c70 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 14 Feb 2019 16:53:32 +0900 Subject: [PATCH] [Feature] enable display errors (#1970) --- ros/src/.config/rviz/default.rviz | 215 ++++++--- .../nodes/velocity_set/velocity_set_info.cpp | 1 + .../autoware_health_checker/CMakeLists.txt | 2 +- .../autoware_health_checker/constants.h | 48 +- .../autoware_health_checker/diag_buffer.h | 77 ++-- .../health_aggregator.h | 81 ++-- .../node_status_publisher.h | 162 ++++--- .../autoware_health_checker/rate_checker.h | 75 ++-- .../system_status_subscriber.h | 65 ++- .../autoware_health_checker/package.xml | 6 +- .../src/diag_buffer.cpp | 188 ++++---- .../src/health_aggregator.cpp | 284 ++++++++---- .../src/health_aggregator_node.cpp | 36 +- .../src/node_status_publisher.cpp | 409 +++++++++--------- .../src/rate_checker.cpp | 181 ++++---- .../src/system_status_subscriber.cpp | 91 ++-- .../test/src/test_autoware_health_checker.cpp | 205 +++++---- 17 files changed, 1263 insertions(+), 863 deletions(-) diff --git a/ros/src/.config/rviz/default.rviz b/ros/src/.config/rviz/default.rviz index dba3124f5bf..76dca5787ca 100644 --- a/ros/src/.config/rviz/default.rviz +++ b/ros/src/.config/rviz/default.rviz @@ -6,14 +6,19 @@ Panels: Expanded: - /Global Options1 - /Camera1/Visibility1 + - /Points Raw1 - /Points Raw1/Status1 - /Local Waypoints1/Status1 - /Vector Map CenterLines1/Namespaces1 - /Global Path1/Namespaces1 - /Local Rollouts1/Namespaces1 - /GlobalPathAnimation1/Status1 + - /OverlayImage1 + - /OK1 + - /ERROR1 + - /FATAL1 Splitter Ratio: 0.695804 - Tree Height: 667 + Tree Height: 1903 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -22,7 +27,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 @@ -32,7 +37,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Points Map + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -42,7 +47,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: false Line Style: - Line Width: 0.03 + Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 @@ -59,36 +64,16 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - base_link: - Value: true - gps: - Value: true - map: - Value: true - mobility: - Value: true - velodyne: - Value: true - world: - Value: true Marker Scale: 5 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - world: - map: - base_link: - velodyne: - {} - gps: - {} - mobility: - {} + {} Update Interval: 0 Value: true - - Alpha: 0.05 + - Alpha: 0.0500000007 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -111,7 +96,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 2 - Size (m): 0.01 + Size (m): 0.00999999978 Style: Points Topic: /points_map Unreliable: false @@ -123,19 +108,7 @@ Visualization Manager: Marker Topic: /vector_map Name: Vector Map Namespaces: - cross_walk: true - curb: true - gutter: true - road_edge: true - road_mark: true - road_pole: true - road_sign: true - signal: true - stop_line: true - street_light: true - utility_pole: true - white_line: true - zebra_zone: true + {} Queue Size: 100 Value: true - Class: rviz/Camera @@ -143,13 +116,12 @@ Visualization Manager: Image Rendering: overlay Image Topic: /image_raw Name: Camera - Overlay Alpha: 0.4 + Overlay Alpha: 0.400000006 Queue Size: 10 Transport Hint: raw Unreliable: false Value: false Visibility: - "": true A* Sim Obstacle: true Behavior State: true Control Pose: true @@ -159,10 +131,13 @@ Visualization Manager: Global Waypoints: true GlobalPathAnimation: true Grid: true + Image: true Local Rollouts: true Local Waypoints: true Next Waypoint Mark: true Occupancy Grid Map: true + OverlayImage: true + OverlayText: true PP Trajectory Mark: true Points Cluster: true Points Map: true @@ -180,17 +155,17 @@ Visualization Manager: Vscan Points: true Waypoint Guide: true Zoom Factor: 1 - - Alpha: 0.5 + - Alpha: 0.300000012 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 + Max Value: 30.3039417 + Min Value: 9.71042252 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 - Color Transformer: Intensity + Color Transformer: AxisColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -202,8 +177,8 @@ Visualization Manager: Position Transformer: XYZ Queue Size: 10 Selectable: true - Size (Pixels): 1 - Size (m): 0.01 + Size (Pixels): 5 + Size (m): 0.00999999978 Style: Points Topic: /points_raw Unreliable: false @@ -233,7 +208,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.01 + Size (m): 0.00999999978 Style: Points Topic: /vscan_points Unreliable: false @@ -242,7 +217,7 @@ Visualization Manager: Value: false - Alpha: 1 Axes Length: 1 - Axes Radius: 0.1 + Axes Radius: 0.100000001 Class: rviz/Pose Color: 255; 25; 0 Enabled: false @@ -257,7 +232,7 @@ Visualization Manager: Value: false - Alpha: 1 Axes Length: 1 - Axes Radius: 0.1 + Axes Radius: 0.100000001 Class: rviz/Pose Color: 255; 170; 255 Enabled: false @@ -341,7 +316,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 5 - Size (m): 0.01 + Size (m): 0.00999999978 Style: Points Topic: /points_cluster Unreliable: false @@ -380,7 +355,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Alpha: 0.7 + - Alpha: 0.699999988 Class: rviz/Map Color Scheme: map Draw Behind: true @@ -388,6 +363,7 @@ Visualization Manager: Name: Occupancy Grid Map Topic: /grid_map_visualization/distance_transform Unreliable: false + Use Timestamp: false Value: true - Class: rviz/MarkerArray Enabled: true @@ -451,10 +427,10 @@ Visualization Manager: Topic: /dp_planner_tracked_boxes Unreliable: false Value: true - alpha: 0.8 + alpha: 0.800000012 color: 25; 255; 0 coloring: Value - line width: 0.01 + line width: 0.00999999978 only edge: false show coords: false - Buffer length: 100 @@ -469,7 +445,7 @@ Visualization Manager: background color: 0; 0; 0 backround alpha: 0 border: true - foreground alpha: 0.7 + foreground alpha: 0.699999988 foreground color: 85; 255; 0 height: 80 left: 40 @@ -480,13 +456,13 @@ Visualization Manager: show caption: true text size: 8 top: 30 - update interval: 0.04 + update interval: 0.0399999991 width: 80 - - Background Alpha: 0.8 + - Background Alpha: 0.800000012 Background Color: 0; 0; 0 Class: jsk_rviz_plugin/OverlayText Enabled: true - Foreground Alpha: 0.8 + Foreground Alpha: 0.800000012 Foreground Color: 25; 255; 240 Name: OverlayText Overtake Color Properties: false @@ -500,10 +476,106 @@ Visualization Manager: text size: 12 top: 0 width: 128 + - Class: rviz/Image + Enabled: false + Image Topic: /image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: jsk_rviz_plugin/OverlayImage + Enabled: false + Name: OverlayImage + Topic: /image_raw + Value: false + alpha: 0.800000012 + height: 128 + keep aspect ratio: true + left: 128 + top: 128 + width: 640 + - Background Alpha: 0.800000012 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000012 + Foreground Color: 25; 255; 240 + Name: OK + Overtake Color Properties: false + Overtake Position Properties: false + Topic: /health_aggreator/ok_text + Value: true + font: DejaVu Sans Mono + height: 128 + left: 0 + line width: 2 + text size: 12 + top: 0 + width: 128 + - Background Alpha: 0.800000012 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000012 + Foreground Color: 25; 255; 240 + Name: WARN + Overtake Color Properties: false + Overtake Position Properties: false + Topic: /health_aggreator/warn_text + Value: true + font: DejaVu Sans Mono + height: 128 + left: 0 + line width: 2 + text size: 12 + top: 0 + width: 128 + - Background Alpha: 0.800000012 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000012 + Foreground Color: 25; 255; 240 + Name: ERROR + Overtake Color Properties: false + Overtake Position Properties: false + Topic: /health_aggreator/error_text + Value: true + font: DejaVu Sans Mono + height: 128 + left: 0 + line width: 2 + text size: 12 + top: 0 + width: 128 + - Background Alpha: 0.800000012 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000012 + Foreground Color: 25; 255; 240 + Name: FATAL + Overtake Color Properties: false + Overtake Position Properties: false + Topic: /health_aggreator/fatal_text + Value: true + font: DejaVu Sans Mono + height: 128 + left: 0 + line width: 2 + text size: 12 + top: 0 + width: 128 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -526,27 +598,30 @@ Visualization Manager: Angle: 0 Class: rviz/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 3.97317 + Near Clip Distance: 0.00999999978 + Scale: 10 Target Frame: Value: TopDownOrtho (rviz) - X: -27.0383 - Y: 47.1504 + X: 0 + Y: 0 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 900 + Height: 2136 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002dcfc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000028000002dc000000dd00fffffffa000000020100000003fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000c00430061006d0065007200610000000000ffffffff0000006500fffffffb000000100044006900730070006c0061007900730100000000000001360000016a00fffffffb0000000a0049006d006100670065010000028e000000d20000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120049006d006100670065005f0072006100770000000000ffffffff0000000000000000fb0000000c00430061006d006500720061000000024e000001710000000000000000fb000000120049006d00610067006500200052006100770100000421000000160000000000000000fb0000000a0049006d00610067006501000002f4000000cb00000000000000000000000100000163000002dcfc0200000003fb0000000a005600690065007700730100000028000002dc000000b000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f000000a8fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000005cfc0100000002fb0000000800540069006d0065010000000000000640000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000367000002dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 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 Selection: collapsed: false Time: @@ -555,6 +630,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1600 - X: 43 - Y: 174 + Width: 3775 + X: 65 + Y: 24 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 42c54190486..32119763400 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 @@ -115,6 +115,7 @@ void VelocitySetInfo::controlPoseCallback(const geometry_msgs::PoseStampedConstP 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/system/autoware_health_checker/CMakeLists.txt b/ros/src/system/autoware_health_checker/CMakeLists.txt index e5ec6f64e46..497837d170a 100644 --- a/ros/src/system/autoware_health_checker/CMakeLists.txt +++ b/ros/src/system/autoware_health_checker/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS autoware_system_msgs roscpp diagnostic_msgs - topic_tools + jsk_rviz_plugins rostest rosunit ) 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 0ddbe362d8c..de37415ee81 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 @@ -1,22 +1,44 @@ #ifndef CONSTANTS_H_INCLUDED #define CONSTANTS_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 + */ + #include -namespace autoware_health_checker -{ - constexpr uint8_t LEVEL_UNDEFINED = autoware_system_msgs::DiagnosticStatus::UNDEFINED; - constexpr uint8_t LEVEL_OK = autoware_system_msgs::DiagnosticStatus::OK; - 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; +namespace autoware_health_checker { +constexpr uint8_t LEVEL_UNDEFINED = + autoware_system_msgs::DiagnosticStatus::UNDEFINED; +constexpr uint8_t LEVEL_OK = autoware_system_msgs::DiagnosticStatus::OK; +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 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; +constexpr double BUFFER_LENGTH = 5.0; +constexpr double UPDATE_RATE = 10.0; } -#endif //CONSTANTS_H_INCLUDED \ No newline at end of file +#endif // CONSTANTS_H_INCLUDED \ No newline at end of file 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/diag_buffer.h index 962739945b8..390ce5528b2 100644 --- 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/diag_buffer.h @@ -1,41 +1,62 @@ #ifndef DIAG_BUFFER_H_INCLUDED #define DIAG_BUFFER_H_INCLUDED -//headers in Autoare +/* + * 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 Autoare #include #include -//headers in STL -#include -#include +// headers in STL #include #include +#include +#include -//headers in ROS +// headers in ROS #include -namespace autoware_health_checker -{ - class DiagBuffer - { - public: - DiagBuffer(std::string key,uint8_t type,std::string description,double buffer_length); - ~DiagBuffer(); - void addDiag(autoware_system_msgs::DiagnosticStatus status); - autoware_system_msgs::DiagnosticStatusArray getAndClearData(); - const uint8_t type; - const std::string description; - private: - std::mutex mtx_; - uint8_t getErrorLevel(); - void updateBuffer(); - std::string key_; - ros::Duration buffer_length_; - std::map buffer_; - autoware_system_msgs::DiagnosticStatusArray filterBuffer(ros::Time now, uint8_t level); - ros::Publisher status_pub_; - bool isOlderTimestamp(const autoware_system_msgs::DiagnosticStatus &a, const autoware_system_msgs::DiagnosticStatus &b); - }; +namespace autoware_health_checker { +class DiagBuffer { +public: + DiagBuffer(std::string key, uint8_t type, std::string description, + double buffer_length); + ~DiagBuffer(); + void addDiag(autoware_system_msgs::DiagnosticStatus status); + autoware_system_msgs::DiagnosticStatusArray getAndClearData(); + const uint8_t type; + const std::string description; + +private: + std::mutex mtx_; + uint8_t getErrorLevel(); + void updateBuffer(); + std::string key_; + ros::Duration buffer_length_; + std::map buffer_; + autoware_system_msgs::DiagnosticStatusArray filterBuffer(ros::Time now, + uint8_t level); + ros::Publisher status_pub_; + bool isOlderTimestamp(const autoware_system_msgs::DiagnosticStatus &a, + const autoware_system_msgs::DiagnosticStatus &b); +}; } -#endif //DIAG_BUFFER_H_INCLUDED \ No newline at end of file +#endif // DIAG_BUFFER_H_INCLUDED \ No newline at end of file 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.h index 089cc638822..ba66bd23637 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.h @@ -1,44 +1,73 @@ #ifndef HEALTH_AGGREGATOR_H_INCLUDED #define HEALTH_AGGREGATOR_H_INCLUDED -//headers in ROS -#include +/* + * 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 +#include +#include -//headers in Autoware +// headers in Autoware #include #include #include -//headers in boost +// headers in boost +#include #include -#include #include -#include +#include #include -//headers in STL -#include +// headers in STL #include +#include -class HealthAggregator -{ +class HealthAggregator { public: - HealthAggregator(ros::NodeHandle nh,ros::NodeHandle pnh); - ~HealthAggregator(); - void run(); + HealthAggregator(ros::NodeHandle nh, ros::NodeHandle pnh); + ~HealthAggregator(); + void run(); + private: - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - ros::Publisher system_status_pub_; - ros::Subscriber node_status_sub_; - ros::Subscriber diagnostic_array_sub_; - void publishSystemStatus(); - void nodeStatusCallback(const autoware_system_msgs::NodeStatus::ConstPtr msg); - void diagnosticArrayCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); - boost::optional convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); - autoware_system_msgs::SystemStatus system_status_; - std::mutex mtx_; - void updateConnectionStatus(); + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + ros::Publisher system_status_pub_; + std::map text_pub_; + ros::Subscriber node_status_sub_; + ros::Subscriber diagnostic_array_sub_; + void publishSystemStatus(); + void nodeStatusCallback(const autoware_system_msgs::NodeStatus::ConstPtr msg); + void + diagnosticArrayCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); + std::string + generateText(std::vector status); + jsk_rviz_plugins::OverlayText + generateOverlayText(autoware_system_msgs::SystemStatus status, uint8_t level); + std::vector + filterNodeStatus(autoware_system_msgs::SystemStatus status, uint8_t level); + boost::optional + convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); + autoware_system_msgs::SystemStatus system_status_; + std::mutex mtx_; + void updateConnectionStatus(); }; -#endif //HEALTH_AGGREGATOR_H_INCLUDED \ No newline at end of file +#endif // HEALTH_AGGREGATOR_H_INCLUDED \ No newline at end of file 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/node_status_publisher.h index 7a4f30e78b2..8c1b3fde1b2 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/node_status_publisher.h @@ -1,87 +1,113 @@ #ifndef NODE_STATUS_PUBLISHER_H_INCLUDED #define NODE_STATUS_PUBLISHER_H_INCLUDED -//headers in ROS +/* + * 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 +// headers in Autoware #include #include #include #include -//headers in STL +// headers in STL +#include #include #include -#include #include -//headers in boost -#include -#include +// headers in boost +#include #include #include +#include +#include #include -#include -namespace autoware_health_checker -{ - class NodeStatusPublisher - { - public: - NodeStatusPublisher(ros::NodeHandle nh,ros::NodeHandle pnh); - ~NodeStatusPublisher(); - void ENABLE(); - uint8_t CHECK_MIN_VALUE(std::string key,double value,double warn_value,double error_value,double fatal_value, std::string description); - uint8_t CHECK_MAX_VALUE(std::string key,double value,double warn_value,double error_value,double fatal_value, std::string description); - // std::pair first value is min value and second value is max value - uint8_t CHECK_RANGE(std::string key,double value,std::pair warn_value,std::pair error_value,std::pair fatal_value,std::string description); - template - uint8_t CHECK_VALUE(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,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.level = check_result; - new_status.description = description; - new_status.description = ss.str(); - new_status.header.stamp = ros::Time::now(); - diag_buffers_[key]->addDiag(new_status); - return new_status.level; - } - void CHECK_RATE(std::string key,double warn_rate,double error_rate,double fatal_rate,std::string description); - void NODE_ACTIVATE() - { - std::lock_guard lock(mtx_); - node_activated_ = true; - }; - void NODE_DEACTIVATE() - { - std::lock_guard lock(mtx_); - node_activated_ = false; - }; - bool getNodeStatus() - { - return node_activated_; - }; - private: - std::vector getKeys(); - std::vector getRateCheckerKeys(); - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - 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); - void publishStatus(); - bool node_activated_; - std::mutex mtx_; - }; +namespace autoware_health_checker { +class NodeStatusPublisher { +public: + NodeStatusPublisher(ros::NodeHandle nh, ros::NodeHandle pnh); + ~NodeStatusPublisher(); + void ENABLE(); + uint8_t CHECK_MIN_VALUE(std::string key, double value, double warn_value, + double error_value, double fatal_value, + std::string description); + uint8_t CHECK_MAX_VALUE(std::string key, double value, double warn_value, + double error_value, double fatal_value, + std::string description); + // std::pair first value is min value and second value is max + // value + uint8_t CHECK_RANGE(std::string key, double value, + std::pair warn_value, + std::pair error_value, + std::pair fatal_value, + std::string description); + template + uint8_t CHECK_VALUE( + 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, + 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.level = check_result; + new_status.description = description; + new_status.description = ss.str(); + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; + } + void CHECK_RATE(std::string key, double warn_rate, double error_rate, + double fatal_rate, std::string description); + void NODE_ACTIVATE() { + std::lock_guard lock(mtx_); + node_activated_ = true; + }; + void NODE_DEACTIVATE() { + std::lock_guard lock(mtx_); + node_activated_ = false; + }; + bool getNodeStatus() { return node_activated_; }; + +private: + std::vector getKeys(); + std::vector getRateCheckerKeys(); + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + 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); + void publishStatus(); + bool node_activated_; + std::mutex mtx_; +}; } -#endif //NODE_STATUS_PUBLISHER_H_INCLUDED \ No newline at end of file +#endif // NODE_STATUS_PUBLISHER_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/rate_checker.h index c16447d99b5..71707a91974 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/rate_checker.h @@ -1,40 +1,59 @@ #ifndef RATE_CHECKER_H_INCLUDED #define RATE_CHECKER_H_INCLUDED -//headers in ROS +/* + * 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 STL -#include +// headers in STL #include +#include -//headers in Boost +// headers in Boost #include -//headers in Autoware +// headers in Autoware #include -namespace autoware_health_checker -{ - class RateChecker - { - public: - RateChecker(double buffer_length,double warn_rate,double error_rate,double fatal_rate,std::string description); - ~RateChecker(); - void check(); - std::pair getErrorLevelAndRate(); - uint8_t getErrorLevel(); - boost::optional getRate(); - const std::string description; - 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_; - std::mutex mtx_; - }; +namespace autoware_health_checker { +class RateChecker { +public: + RateChecker(double buffer_length, double warn_rate, double error_rate, + double fatal_rate, std::string description); + ~RateChecker(); + void check(); + std::pair getErrorLevelAndRate(); + uint8_t getErrorLevel(); + boost::optional getRate(); + const std::string description; + +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_; + std::mutex mtx_; +}; } -#endif //RATE_CHECKER_H_INCLUDED \ No newline at end of file +#endif // RATE_CHECKER_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.h index 12fe8b3a4af..b00f85a4991 100644 --- 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.h @@ -1,34 +1,55 @@ #ifndef SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED #define SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED -//headers in Autoware +/* + * 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 Autoware #include #include -//headers in ROS +// headers in ROS #include -//headers in STL -#include +// headers in STL #include +#include + +namespace autoware_health_checker { +class SystemStatusSubscriber { +public: + SystemStatusSubscriber(ros::NodeHandle nh, ros::NodeHandle pnh); + ~SystemStatusSubscriber(); + void enable(); + void + addCallback(std::function func); -namespace autoware_health_checker -{ - class SystemStatusSubscriber - { - public: - SystemStatusSubscriber(ros::NodeHandle nh,ros::NodeHandle pnh); - ~SystemStatusSubscriber(); - void enable(); - void addCallback(std::function func); - private: - void systemStatusCallback(const autoware_system_msgs::SystemStatus::ConstPtr msg); - std::mutex mtx_; - ros::Subscriber status_sub_; - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - std::vector > functions_; - }; +private: + void + systemStatusCallback(const autoware_system_msgs::SystemStatus::ConstPtr msg); + std::mutex mtx_; + ros::Subscriber status_sub_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + std::vector> + functions_; +}; } -#endif //SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED \ No newline at end of file +#endif // SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/package.xml b/ros/src/system/autoware_health_checker/package.xml index 21de50da288..1dcadbb5f67 100644 --- a/ros/src/system/autoware_health_checker/package.xml +++ b/ros/src/system/autoware_health_checker/package.xml @@ -12,17 +12,17 @@ autoware_system_msgs roscpp diagnostic_msgs - topic_tools rostest rosunit + jsk_rviz_plugins autoware_system_msgs roscpp diagnostic_msgs - topic_tools + jsk_rviz_plugins autoware_system_msgs diagnostic_msgs roscpp - topic_tools + jsk_rviz_plugins diff --git a/ros/src/system/autoware_health_checker/src/diag_buffer.cpp b/ros/src/system/autoware_health_checker/src/diag_buffer.cpp index 040117342d6..a7100139760 100644 --- a/ros/src/system/autoware_health_checker/src/diag_buffer.cpp +++ b/ros/src/system/autoware_health_checker/src/diag_buffer.cpp @@ -1,94 +1,116 @@ +/* + * 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 -{ - DiagBuffer::DiagBuffer(std::string key, uint8_t type, std::string description, double buffer_length) : type(type), description(description) - { - key_ = key; - buffer_length_ = ros::Duration(buffer_length); - } +namespace autoware_health_checker { +DiagBuffer::DiagBuffer(std::string key, uint8_t type, std::string description, + double buffer_length) + : type(type), description(description) { + key_ = key; + buffer_length_ = ros::Duration(buffer_length); +} - DiagBuffer::~DiagBuffer() - { - - } +DiagBuffer::~DiagBuffer() {} - void DiagBuffer::addDiag(autoware_system_msgs::DiagnosticStatus status) - { - std::lock_guard lock(mtx_); - buffer_[status.level].status.emplace_back(status); - updateBuffer(); - return; - } +void DiagBuffer::addDiag(autoware_system_msgs::DiagnosticStatus status) { + std::lock_guard lock(mtx_); + buffer_[status.level].status.emplace_back(status); + updateBuffer(); + return; +} - autoware_system_msgs::DiagnosticStatusArray DiagBuffer::getAndClearData() - { - std::lock_guard lock(mtx_); - autoware_system_msgs::DiagnosticStatusArray data; - data = buffer_[autoware_health_checker::LEVEL_FATAL]; - data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_ERROR].status.begin(),buffer_[autoware_health_checker::LEVEL_ERROR].status.end()); - data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_WARN].status.begin(),buffer_[autoware_health_checker::LEVEL_WARN].status.end()); - data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_OK].status.begin(),buffer_[autoware_health_checker::LEVEL_OK].status.end()); - data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_UNDEFINED].status.begin(),buffer_[autoware_health_checker::LEVEL_UNDEFINED].status.end()); - std::sort(data.status.begin(), data.status.end(), std::bind(&DiagBuffer::isOlderTimestamp, this, std::placeholders::_1, std::placeholders::_2)); - buffer_.clear(); - return data; - } +autoware_system_msgs::DiagnosticStatusArray DiagBuffer::getAndClearData() { + std::lock_guard lock(mtx_); + autoware_system_msgs::DiagnosticStatusArray data; + data = buffer_[autoware_health_checker::LEVEL_FATAL]; + data.status.insert( + data.status.end(), + buffer_[autoware_health_checker::LEVEL_ERROR].status.begin(), + buffer_[autoware_health_checker::LEVEL_ERROR].status.end()); + data.status.insert( + data.status.end(), + buffer_[autoware_health_checker::LEVEL_WARN].status.begin(), + buffer_[autoware_health_checker::LEVEL_WARN].status.end()); + data.status.insert(data.status.end(), + buffer_[autoware_health_checker::LEVEL_OK].status.begin(), + buffer_[autoware_health_checker::LEVEL_OK].status.end()); + data.status.insert( + data.status.end(), + buffer_[autoware_health_checker::LEVEL_UNDEFINED].status.begin(), + buffer_[autoware_health_checker::LEVEL_UNDEFINED].status.end()); + std::sort(data.status.begin(), data.status.end(), + std::bind(&DiagBuffer::isOlderTimestamp, this, + std::placeholders::_1, std::placeholders::_2)); + buffer_.clear(); + return data; +} - uint8_t DiagBuffer::getErrorLevel() - { - std::lock_guard lock(mtx_); - updateBuffer(); - if(buffer_[autoware_health_checker::LEVEL_FATAL].status.size() != 0) - { - return autoware_health_checker::LEVEL_FATAL; - } - else if(buffer_[autoware_health_checker::LEVEL_ERROR].status.size() != 0) - { - return autoware_health_checker::LEVEL_ERROR; - } - else if(buffer_[autoware_health_checker::LEVEL_WARN].status.size() != 0) - { - return autoware_health_checker::LEVEL_WARN; - } - else - { - return autoware_health_checker::LEVEL_OK; - } - } +uint8_t DiagBuffer::getErrorLevel() { + std::lock_guard lock(mtx_); + updateBuffer(); + if (buffer_[autoware_health_checker::LEVEL_FATAL].status.size() != 0) { + return autoware_health_checker::LEVEL_FATAL; + } else if (buffer_[autoware_health_checker::LEVEL_ERROR].status.size() != 0) { + return autoware_health_checker::LEVEL_ERROR; + } else if (buffer_[autoware_health_checker::LEVEL_WARN].status.size() != 0) { + return autoware_health_checker::LEVEL_WARN; + } else { + return autoware_health_checker::LEVEL_OK; + } +} - // filter data from timestamp and level - autoware_system_msgs::DiagnosticStatusArray DiagBuffer::filterBuffer(ros::Time now, uint8_t level) - { - autoware_system_msgs::DiagnosticStatusArray filterd_data; - autoware_system_msgs::DiagnosticStatusArray ret; - if(buffer_.count(level) != 0) - { - filterd_data = buffer_[level]; - } - for(auto data_itr = filterd_data.status.begin(); data_itr != filterd_data.status.end(); data_itr++) - { - if(data_itr->header.stamp> (now - buffer_length_)) - { - ret.status.push_back(*data_itr); - } - } - return ret; +// filter data from timestamp and level +autoware_system_msgs::DiagnosticStatusArray +DiagBuffer::filterBuffer(ros::Time now, uint8_t level) { + autoware_system_msgs::DiagnosticStatusArray filterd_data; + autoware_system_msgs::DiagnosticStatusArray ret; + if (buffer_.count(level) != 0) { + filterd_data = buffer_[level]; + } + for (auto data_itr = filterd_data.status.begin(); + data_itr != filterd_data.status.end(); data_itr++) { + if (data_itr->header.stamp > (now - buffer_length_)) { + ret.status.push_back(*data_itr); } + } + return ret; +} - void DiagBuffer::updateBuffer() - { - ros::Time now = ros::Time::now(); - buffer_[autoware_health_checker::LEVEL_FATAL] = filterBuffer(now, autoware_health_checker::LEVEL_FATAL); - buffer_[autoware_health_checker::LEVEL_ERROR] = filterBuffer(now, autoware_health_checker::LEVEL_ERROR); - buffer_[autoware_health_checker::LEVEL_WARN] = filterBuffer(now, autoware_health_checker::LEVEL_WARN); - buffer_[autoware_health_checker::LEVEL_OK] = filterBuffer(now, autoware_health_checker::LEVEL_OK); - buffer_[autoware_health_checker::LEVEL_UNDEFINED] = filterBuffer(now, autoware_health_checker::LEVEL_UNDEFINED); - return; - } +void DiagBuffer::updateBuffer() { + ros::Time now = ros::Time::now(); + buffer_[autoware_health_checker::LEVEL_FATAL] = + filterBuffer(now, autoware_health_checker::LEVEL_FATAL); + buffer_[autoware_health_checker::LEVEL_ERROR] = + filterBuffer(now, autoware_health_checker::LEVEL_ERROR); + buffer_[autoware_health_checker::LEVEL_WARN] = + filterBuffer(now, autoware_health_checker::LEVEL_WARN); + buffer_[autoware_health_checker::LEVEL_OK] = + filterBuffer(now, autoware_health_checker::LEVEL_OK); + buffer_[autoware_health_checker::LEVEL_UNDEFINED] = + filterBuffer(now, autoware_health_checker::LEVEL_UNDEFINED); + return; +} - bool DiagBuffer::isOlderTimestamp(const autoware_system_msgs::DiagnosticStatus &a, const autoware_system_msgs::DiagnosticStatus &b) - { - return a.header.stamp < b.header.stamp; - } +bool DiagBuffer::isOlderTimestamp( + const autoware_system_msgs::DiagnosticStatus &a, + const autoware_system_msgs::DiagnosticStatus &b) { + return a.header.stamp < b.header.stamp; +} } \ 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.cpp index b006279d85c..c26fb0c9916 100644 --- a/ros/src/system/autoware_health_checker/src/health_aggregator.cpp +++ b/ros/src/system/autoware_health_checker/src/health_aggregator.cpp @@ -1,114 +1,212 @@ +/* + * 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 -HealthAggregator::HealthAggregator(ros::NodeHandle nh,ros::NodeHandle pnh) -{ - nh_ = nh; - pnh_ = pnh; +HealthAggregator::HealthAggregator(ros::NodeHandle nh, ros::NodeHandle pnh) { + nh_ = nh; + pnh_ = pnh; } -HealthAggregator::~HealthAggregator() -{ +HealthAggregator::~HealthAggregator() {} +void HealthAggregator::run() { + system_status_pub_ = + nh_.advertise("/system_status", 10); + text_pub_[autoware_health_checker::LEVEL_OK] = + pnh_.advertise("ok_text", 1); + text_pub_[autoware_health_checker::LEVEL_WARN] = + pnh_.advertise("warn_text", 1); + text_pub_[autoware_health_checker::LEVEL_ERROR] = + pnh_.advertise("error_text", 1); + text_pub_[autoware_health_checker::LEVEL_FATAL] = + pnh_.advertise("fatal_text", 1); + node_status_sub_ = nh_.subscribe("/node_status", 10, + &HealthAggregator::nodeStatusCallback, this); + diagnostic_array_sub_ = nh_.subscribe( + "/diagnostic_agg", 10, &HealthAggregator::diagnosticArrayCallback, this); + boost::thread publish_thread( + boost::bind(&HealthAggregator::publishSystemStatus, this)); + return; } -void HealthAggregator::run() -{ - system_status_pub_ = nh_.advertise("/system_status",10); - node_status_sub_ = nh_.subscribe("/node_status",10,&HealthAggregator::nodeStatusCallback,this); - diagnostic_array_sub_ = nh_.subscribe("/diagnostic_agg",10,&HealthAggregator::diagnosticArrayCallback,this); - boost::thread publish_thread(boost::bind(&HealthAggregator::publishSystemStatus, this)); - return; +void HealthAggregator::publishSystemStatus() { + ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); + while (ros::ok()) { + mtx_.lock(); + system_status_.header.stamp = ros::Time::now(); + updateConnectionStatus(); + system_status_pub_.publish(system_status_); + text_pub_[autoware_health_checker::LEVEL_OK].publish( + generateOverlayText(system_status_, autoware_health_checker::LEVEL_OK)); + text_pub_[autoware_health_checker::LEVEL_WARN].publish(generateOverlayText( + system_status_, autoware_health_checker::LEVEL_WARN)); + text_pub_[autoware_health_checker::LEVEL_ERROR].publish(generateOverlayText( + system_status_, autoware_health_checker::LEVEL_ERROR)); + text_pub_[autoware_health_checker::LEVEL_FATAL].publish(generateOverlayText( + system_status_, autoware_health_checker::LEVEL_FATAL)); + system_status_.node_status.clear(); + system_status_.hardware_status.clear(); + mtx_.unlock(); + rate.sleep(); + } + return; } -void HealthAggregator::publishSystemStatus() -{ - ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); - while(ros::ok()) - { - mtx_.lock(); - system_status_.header.stamp = ros::Time::now(); - updateConnectionStatus(); - system_status_pub_.publish(system_status_); - system_status_.node_status.clear(); - system_status_.hardware_status.clear(); - mtx_.unlock(); - rate.sleep(); - } - return; +void HealthAggregator::updateConnectionStatus() { + std::vector detected_nodes; + ros::master::getNodes(detected_nodes); + system_status_.available_nodes = detected_nodes; + return; } -void HealthAggregator::updateConnectionStatus() -{ - std::vector detected_nodes; - ros::master::getNodes(detected_nodes); - system_status_.available_nodes = detected_nodes; - return; +void HealthAggregator::nodeStatusCallback( + const autoware_system_msgs::NodeStatus::ConstPtr msg) { + mtx_.lock(); + system_status_.node_status.push_back(*msg); + mtx_.unlock(); + return; } -void HealthAggregator::nodeStatusCallback(const autoware_system_msgs::NodeStatus::ConstPtr msg) -{ - mtx_.lock(); - system_status_.node_status.push_back(*msg); - mtx_.unlock(); - return; +void HealthAggregator::diagnosticArrayCallback( + const diagnostic_msgs::DiagnosticArray::ConstPtr msg) { + mtx_.lock(); + boost::optional status = convert(msg); + if (status) { + system_status_.hardware_status.push_back(*status); + } + mtx_.unlock(); + return; } -void HealthAggregator::diagnosticArrayCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr msg) -{ - mtx_.lock(); - boost::optional status = convert(msg); - if(status) - { - system_status_.hardware_status.push_back(*status); +std::string HealthAggregator::generateText( + std::vector status) { + std::string text; + for (auto itr = status.begin(); itr != status.end(); itr++) { + text = text + itr->description + "\n"; + // text = itr->key + " : " + itr->description + "\n"; + } + return text; +} + +jsk_rviz_plugins::OverlayText +HealthAggregator::generateOverlayText(autoware_system_msgs::SystemStatus status, + uint8_t level) { + jsk_rviz_plugins::OverlayText text; + text.action = text.ADD; + text.width = 640; + text.height = 640; + text.top = 0; + text.bg_color.r = 0; + text.bg_color.g = 0; + text.bg_color.b = 0; + text.bg_color.a = 0.7; + text.text_size = 20.0; + if (level == autoware_health_checker::LEVEL_OK) { + text.left = 0; + text.fg_color.r = 0.0; + text.fg_color.g = 0.0; + text.fg_color.b = 1.0; + text.fg_color.a = 1.0; + text.text = generateText(filterNodeStatus(status, level)); + } else if (level == autoware_health_checker::LEVEL_WARN) { + text.left = 640 * 1; + text.fg_color.r = 1.0; + text.fg_color.g = 1.0; + text.fg_color.b = 0.0; + text.fg_color.a = 1.0; + text.text = generateText(filterNodeStatus(status, level)); + } else if (level == autoware_health_checker::LEVEL_ERROR) { + text.left = 640 * 2; + text.fg_color.r = 1.0; + text.fg_color.g = 0.0; + text.fg_color.b = 0.0; + text.fg_color.a = 1.0; + text.text = generateText(filterNodeStatus(status, level)); + } else if (level == autoware_health_checker::LEVEL_FATAL) { + text.left = 640 * 3; + text.fg_color.r = 1.0; + text.fg_color.g = 1.0; + text.fg_color.b = 1.0; + text.fg_color.a = 1.0; + text.text = generateText(filterNodeStatus(status, level)); + } + return text; +} + +std::vector +HealthAggregator::filterNodeStatus(autoware_system_msgs::SystemStatus status, + uint8_t level) { + std::vector ret; + for (auto node_status_itr = status.node_status.begin(); + node_status_itr != status.node_status.end(); node_status_itr++) { + if (node_status_itr->node_activated) { + for (auto array_itr = node_status_itr->status.begin(); + array_itr != node_status_itr->status.end(); array_itr++) { + for (auto itr = array_itr->status.begin(); + itr != array_itr->status.end(); itr++) { + if (itr->level == level) { + ret.push_back(*itr); + } + } + } } - mtx_.unlock(); - return; + } + return ret; } -boost::optional HealthAggregator::convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg) -{ - autoware_system_msgs::HardwareStatus status; - if(msg->status.size() == 0) - { - return boost::none; +boost::optional HealthAggregator::convert( + const diagnostic_msgs::DiagnosticArray::ConstPtr msg) { + autoware_system_msgs::HardwareStatus status; + if (msg->status.size() == 0) { + return boost::none; + } + status.header = msg->header; + for (auto diag_itr = msg->status.begin(); diag_itr != msg->status.end(); + diag_itr++) { + status.hardware_name = diag_itr->hardware_id; + autoware_system_msgs::DiagnosticStatus diag; + autoware_system_msgs::DiagnosticStatusArray diag_array; + diag.header = msg->header; + diag.key = diag_itr->hardware_id; + diag.description = diag_itr->message; + diag.type = autoware_system_msgs::DiagnosticStatus::HARDWARE; + if (diag_itr->level == diagnostic_msgs::DiagnosticStatus::OK) { + diag.level = autoware_health_checker::LEVEL_OK; + } else if (diag_itr->level == diagnostic_msgs::DiagnosticStatus::WARN) { + diag.level = autoware_health_checker::LEVEL_WARN; + } else if (diag_itr->level == diagnostic_msgs::DiagnosticStatus::ERROR) { + diag.level = autoware_health_checker::LEVEL_ERROR; + } else if (diag_itr->level == diagnostic_msgs::DiagnosticStatus::STALE) { + diag.level = autoware_health_checker::LEVEL_FATAL; } - status.header = msg->header; - for(auto diag_itr = msg->status.begin(); diag_itr != msg->status.end(); diag_itr++) - { - status.hardware_name = diag_itr->hardware_id; - autoware_system_msgs::DiagnosticStatus diag; - autoware_system_msgs::DiagnosticStatusArray diag_array; - diag.header = msg->header; - diag.key = diag_itr->hardware_id; - diag.description = diag_itr->message; - diag.type = autoware_system_msgs::DiagnosticStatus::HARDWARE; - if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::OK) - { - diag.level = autoware_health_checker::LEVEL_OK; - } - else if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::WARN) - { - diag.level = autoware_health_checker::LEVEL_WARN; - } - else if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::ERROR) - { - diag.level = autoware_health_checker::LEVEL_ERROR; - } - else if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::STALE) - { - diag.level = autoware_health_checker::LEVEL_FATAL; - } - using namespace boost::property_tree; - std::stringstream ss; - ptree pt; - for(auto value_itr = diag_itr->values.begin(); value_itr != diag_itr->values.end(); value_itr++) - { - pt.put(value_itr->key+".string", value_itr->value); - } - write_json(ss, pt); - diag.value = ss.str(); - diag_array.status.push_back(diag); - status.status.push_back(diag_array); + using namespace boost::property_tree; + std::stringstream ss; + ptree pt; + for (auto value_itr = diag_itr->values.begin(); + value_itr != diag_itr->values.end(); value_itr++) { + pt.put(value_itr->key + ".string", value_itr->value); } - return status; + write_json(ss, pt); + diag.value = ss.str(); + diag_array.status.push_back(diag); + status.status.push_back(diag_array); + } + return status; } \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp b/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp index bea7288d2f0..29a19330a64 100644 --- a/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp +++ b/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp @@ -1,14 +1,32 @@ +/* + * 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 #include -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "health_aggreator"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - HealthAggregator agg(nh,pnh); - agg.run(); - ros::spin(); - return 0; +int main(int argc, char *argv[]) { + ros::init(argc, argv, "health_aggreator"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + HealthAggregator agg(nh, pnh); + agg.run(); + ros::spin(); + return 0; } \ 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 index d7a6521ab7c..a620e754d4c 100644 --- a/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp +++ b/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp @@ -1,222 +1,221 @@ +/* + * 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); - } +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() - { - - } +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::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); } - - void NodeStatusPublisher::ENABLE() - { - boost::thread publish_thread(boost::bind(&NodeStatusPublisher::publishStatus, this)); - return; + // 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; +} - 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; - } +void NodeStatusPublisher::ENABLE() { + boost::thread publish_thread( + boost::bind(&NodeStatusPublisher::publishStatus, this)); + return; +} - 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; +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; + } } - - bool NodeStatusPublisher::keyExist(std::string key) - { - if(diag_buffers_.count(key) == 0) - { - return false; - } - return true; + if (!matched) { + keys.push_back(buf_itr.first); } + } + return keys; +} - // 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; - } +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; +} - 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); - diag_buffers_[key]->addDiag(new_status); - return new_status.level; - } +bool NodeStatusPublisher::keyExist(std::string key) { + if (diag_buffers_.count(key) == 0) { + return false; + } + return true; +} - 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; - } +// 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_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; - } +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); + 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; - } +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; +} - 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(); - } +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/rate_checker.cpp b/ros/src/system/autoware_health_checker/src/rate_checker.cpp index fdc4ed51bb2..9a7e22e8d1e 100644 --- a/ros/src/system/autoware_health_checker/src/rate_checker.cpp +++ b/ros/src/system/autoware_health_checker/src/rate_checker.cpp @@ -1,102 +1,99 @@ -#include +/* + * 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 + */ -namespace autoware_health_checker -{ - RateChecker::RateChecker(double buffer_length,double warn_rate,double error_rate,double fatal_rate,std::string description) - : buffer_length_(buffer_length), warn_rate_(warn_rate), error_rate_(error_rate), fatal_rate_(fatal_rate), description(description) - { - start_time_ = ros::Time::now(); - } +#include - RateChecker::~RateChecker() - { +namespace autoware_health_checker { +RateChecker::RateChecker(double buffer_length, double warn_rate, + double error_rate, double fatal_rate, + std::string description) + : buffer_length_(buffer_length), warn_rate_(warn_rate), + error_rate_(error_rate), fatal_rate_(fatal_rate), + description(description) { + start_time_ = ros::Time::now(); +} - } +RateChecker::~RateChecker() {} - std::pair RateChecker::getErrorLevelAndRate() - { - std::pair ret; - boost::optional rate = getRate(); - if(!rate) - { - ret = std::make_pair(autoware_health_checker::LEVEL_ERROR,0); - } - else if(rate.get() < fatal_rate_) - { - ret = std::make_pair(autoware_health_checker::LEVEL_FATAL,rate.get()); - } - else if(rate.get() < error_rate_) - { - ret = std::make_pair(autoware_health_checker::LEVEL_ERROR,rate.get()); - } - else if(rate.get() < warn_rate_) - { - ret = std::make_pair(autoware_health_checker::LEVEL_WARN,rate.get()); - } - else - { - ret = std::make_pair(autoware_health_checker::LEVEL_OK,rate.get()); - } - return ret; - } +std::pair RateChecker::getErrorLevelAndRate() { + std::pair ret; + boost::optional rate = getRate(); + if (!rate) { + ret = std::make_pair(autoware_health_checker::LEVEL_ERROR, 0); + } else if (rate.get() < fatal_rate_) { + ret = std::make_pair(autoware_health_checker::LEVEL_FATAL, rate.get()); + } else if (rate.get() < error_rate_) { + ret = std::make_pair(autoware_health_checker::LEVEL_ERROR, rate.get()); + } else if (rate.get() < warn_rate_) { + ret = std::make_pair(autoware_health_checker::LEVEL_WARN, rate.get()); + } else { + ret = std::make_pair(autoware_health_checker::LEVEL_OK, rate.get()); + } + return ret; +} - uint8_t RateChecker::getErrorLevel() - { - boost::optional rate = getRate(); - if(!rate) - { - return autoware_health_checker::LEVEL_ERROR; - } - if(rate.get() < fatal_rate_) - { - return autoware_health_checker::LEVEL_FATAL; - } - if(rate.get() < error_rate_) - { - return autoware_health_checker::LEVEL_ERROR; - } - if(rate.get() < warn_rate_) - { - return autoware_health_checker::LEVEL_WARN; - } - return autoware_health_checker::LEVEL_OK; - } +uint8_t RateChecker::getErrorLevel() { + boost::optional rate = getRate(); + if (!rate) { + return autoware_health_checker::LEVEL_ERROR; + } + if (rate.get() < fatal_rate_) { + return autoware_health_checker::LEVEL_FATAL; + } + if (rate.get() < error_rate_) { + return autoware_health_checker::LEVEL_ERROR; + } + if (rate.get() < warn_rate_) { + return autoware_health_checker::LEVEL_WARN; + } + return autoware_health_checker::LEVEL_OK; +} - void RateChecker::check() - { - update(); - mtx_.lock(); - data_.push_back(ros::Time::now()); - mtx_.unlock(); - } +void RateChecker::check() { + update(); + mtx_.lock(); + data_.push_back(ros::Time::now()); + mtx_.unlock(); +} - void RateChecker::update() - { - mtx_.lock(); - std::vector buffer; - for(auto data_itr = data_.begin(); data_itr != data_.end(); data_itr++) - { - if(*data_itr > ros::Time::now()-ros::Duration(buffer_length_)) - { - buffer.push_back(*data_itr); - } - } - data_ = buffer; - mtx_.unlock(); - return; +void RateChecker::update() { + mtx_.lock(); + std::vector buffer; + for (auto data_itr = data_.begin(); data_itr != data_.end(); data_itr++) { + if (*data_itr > ros::Time::now() - ros::Duration(buffer_length_)) { + buffer.push_back(*data_itr); } + } + data_ = buffer; + mtx_.unlock(); + return; +} - boost::optional RateChecker::getRate() - { - boost::optional rate; - if(ros::Time::now() - start_time_ < ros::Duration(buffer_length_)) - { - return boost::none; - } - update(); - mtx_.lock(); - rate = data_.size()/buffer_length_; - mtx_.unlock(); - return rate; - } +boost::optional RateChecker::getRate() { + boost::optional rate; + if (ros::Time::now() - start_time_ < ros::Duration(buffer_length_)) { + return boost::none; + } + update(); + mtx_.lock(); + rate = data_.size() / buffer_length_; + mtx_.unlock(); + return rate; +} } \ 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.cpp index 692de2f3b24..8139c9f2c8a 100644 --- a/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp +++ b/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp @@ -1,45 +1,60 @@ -#include +/* + * 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 + */ -namespace autoware_health_checker -{ - SystemStatusSubscriber::SystemStatusSubscriber(ros::NodeHandle nh,ros::NodeHandle pnh) - { - nh_ = nh; - pnh_ = pnh; - } +#include - SystemStatusSubscriber::~SystemStatusSubscriber() - { +namespace autoware_health_checker { +SystemStatusSubscriber::SystemStatusSubscriber(ros::NodeHandle nh, + ros::NodeHandle pnh) { + nh_ = nh; + pnh_ = pnh; +} - } +SystemStatusSubscriber::~SystemStatusSubscriber() {} - void SystemStatusSubscriber::enable() - { - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::Rate rate(1); - status_sub_ = nh_.subscribe("system_status",10,&SystemStatusSubscriber::systemStatusCallback,this); - while(ros::ok()) - { - rate.sleep(); - } - spinner.stop(); - return; - } +void SystemStatusSubscriber::enable() { + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::Rate rate(1); + status_sub_ = nh_.subscribe( + "system_status", 10, &SystemStatusSubscriber::systemStatusCallback, this); + while (ros::ok()) { + rate.sleep(); + } + spinner.stop(); + return; +} - void SystemStatusSubscriber::systemStatusCallback(const autoware_system_msgs::SystemStatus::ConstPtr msg) - { - for(auto function_itr = functions_.begin(); function_itr != functions_.end(); function_itr++) - { - std::function func = *function_itr; - func(*msg); - } - return; - } +void SystemStatusSubscriber::systemStatusCallback( + const autoware_system_msgs::SystemStatus::ConstPtr msg) { + for (auto function_itr = functions_.begin(); function_itr != functions_.end(); + function_itr++) { + std::function func = + *function_itr; + func(*msg); + } + return; +} - void SystemStatusSubscriber::addCallback(std::function func) - { - functions_.push_back(func); - return; - } +void SystemStatusSubscriber::addCallback( + std::function func) { + functions_.push_back(func); + return; +} } \ No newline at end of file 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 352f188e02a..be73533a9df 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,127 +16,164 @@ * * v1.0 Masaya Kataoka */ -#include -#include #include +#include +#include -class AutowareHealthCheckerTestSuite : public ::testing::Test -{ +class AutowareHealthCheckerTestSuite : public ::testing::Test { public: - AutowareHealthCheckerTestSuite() - { - } + AutowareHealthCheckerTestSuite() {} - ~AutowareHealthCheckerTestSuite() - { - } + ~AutowareHealthCheckerTestSuite() {} }; -class AutowareHealthCheckerTestClass -{ +class AutowareHealthCheckerTestClass { public: - AutowareHealthCheckerTestClass() - { + AutowareHealthCheckerTestClass() { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - node_status_publisher_ptr = std::make_shared(nh,pnh); + node_status_publisher_ptr = + std::make_shared(nh, pnh); }; - std::shared_ptr node_status_publisher_ptr; + std::shared_ptr + node_status_publisher_ptr; }; -TEST(TestSuite, CHECK_MIN_VALUE) -{ +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"; + 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) -{ +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"; + 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) -{ +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 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) -{ +uint8_t test_function(double value) { + if (value == 0.0) { return autoware_health_checker::LEVEL_FATAL; -} -if(value == 1.0) -{ + } + if (value == 1.0) { return autoware_health_checker::LEVEL_ERROR; -} -if(value == 2.0) -{ + } + if (value == 2.0) { return autoware_health_checker::LEVEL_WARN; -} -return autoware_health_checker::LEVEL_OK; + } + return autoware_health_checker::LEVEL_OK; }; -boost::property_tree::ptree test_value_json_func(double value) -{ -boost::property_tree::ptree tree; -tree.put("value", value); -return tree; +boost::property_tree::ptree test_value_json_func(double value) { + boost::property_tree::ptree tree; + tree.put("value", value); + return tree; }; -TEST(TestSuite, CHECK_VALUE) -{ +TEST(TestSuite, CHECK_VALUE) { AutowareHealthCheckerTestClass test_autoware_health_checker; 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("test",0.0,check_func,check_value_json_func,"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_VALUE("test",1.0,check_func,check_value_json_func,"test"); - ASSERT_EQ(ret_error, 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("test",2.0,check_func,check_value_json_func,"test"); - ASSERT_EQ(ret_warn, 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("test",-1.0,check_func,check_value_json_func,"test"); - ASSERT_EQ(ret_ok, 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"; + std::function + check_value_json_func = test_value_json_func; + uint8_t ret_fatal = + test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE( + "test", 0.0, check_func, check_value_json_func, "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_VALUE( + "test", 1.0, check_func, check_value_json_func, "test"); + ASSERT_EQ(ret_error, 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( + "test", 2.0, check_func, check_value_json_func, "test"); + ASSERT_EQ(ret_warn, 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( + "test", -1.0, check_func, check_value_json_func, "test"); + ASSERT_EQ(ret_ok, 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(TestSuite, NODE_STATUS) -{ +TEST(TestSuite, NODE_STATUS) { AutowareHealthCheckerTestClass test_autoware_health_checker; test_autoware_health_checker.node_status_publisher_ptr->NODE_ACTIVATE(); - uint8_t ret_active = test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); - ASSERT_EQ(ret_active, true) << "The value must be true"; + uint8_t ret_active = + test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); + ASSERT_EQ(ret_active, true) << "The value must be true"; test_autoware_health_checker.node_status_publisher_ptr->NODE_DEACTIVATE(); - uint8_t ret_inactive = test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); - ASSERT_EQ(ret_inactive, false) << "The value must be true"; + uint8_t ret_inactive = + test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); + ASSERT_EQ(ret_inactive, false) << "The value must be true"; } -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "AutowareHealthCheckerTestNode"); return RUN_ALL_TESTS();