[Feature] enable display errors (autowarefoundation#1970)

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;
 }

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
 )

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 <autoware_system_msgs/DiagnosticStatus.h>
 
-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
+#endif // CONSTANTS_H_INCLUDED

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 <autoware_health_checker/constants.h>
 #include <autoware_system_msgs/DiagnosticStatusArray.h>
 
-//headers in STL
-#include <vector>
-#include <string>
+// headers in STL
 #include <map>
 #include <mutex>
+#include <string>
+#include <vector>
 
-//headers in ROS
+// headers in ROS
 #include <ros/ros.h>
 
-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<uint8_t,autoware_system_msgs::DiagnosticStatusArray > 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<uint8_t, autoware_system_msgs::DiagnosticStatusArray> 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
+#endif // DIAG_BUFFER_H_INCLUDED

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 <ros/ros.h>
+/*
+ * 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 <diagnostic_msgs/DiagnosticArray.h>
+#include <jsk_rviz_plugins/OverlayText.h>
+#include <ros/ros.h>
 
-//headers in Autoware
+// headers in Autoware
 #include <autoware_health_checker/constants.h>
 #include <autoware_system_msgs/NodeStatus.h>
 #include <autoware_system_msgs/SystemStatus.h>
 
-//headers in boost
+// headers in boost
+#include <boost/foreach.hpp>
 #include <boost/optional.hpp>
-#include <boost/property_tree/ptree.hpp>
 #include <boost/property_tree/json_parser.hpp>
-#include <boost/foreach.hpp>
+#include <boost/property_tree/ptree.hpp>
 #include <boost/thread.hpp>
 
-//headers in STL
-#include <mutex>
+// headers in STL
 #include <map>
+#include <mutex>
 
-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<autoware_system_msgs::HardwareStatus> 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<uint8_t, ros::Publisher> 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<autoware_system_msgs::DiagnosticStatus> status);
+  jsk_rviz_plugins::OverlayText
+  generateOverlayText(autoware_system_msgs::SystemStatus status, uint8_t level);
+  std::vector<autoware_system_msgs::DiagnosticStatus>
+  filterNodeStatus(autoware_system_msgs::SystemStatus status, uint8_t level);
+  boost::optional<autoware_system_msgs::HardwareStatus>
+  convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg);
+  autoware_system_msgs::SystemStatus system_status_;
+  std::mutex mtx_;
+  void updateConnectionStatus();
 };
-#endif  //HEALTH_AGGREGATOR_H_INCLUDED
+#endif // HEALTH_AGGREGATOR_H_INCLUDED