tier4 · 0x126 · Apr 20, 2023 · Feb 16, 2023
@@ -1,12 +1,19 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
+  <arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
   <arg name="run_mode" default="online" description="options: online, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
   <arg name="tier4_system_launch_param_path" default="$(find-pkg-share tier4_system_launch)/config" description="tier4_system_launch parameter path"/>
 
   <let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
 
+  <!-- Dummy Diag Publisher -->
+  <arg name="launch_rqt_reconfigure" default="false"/>
+  <arg name="launch_rqt_runtime_monitor" default="false"/>
+  <arg name="launch_rqt_robot_monitor" default="false"/>
+  <arg name="launch_rqt_runtime_monitor_err" default="false"/>
+
   <group>
     <push-ros-namespace namespace="/system"/>
 
@@ -69,4 +76,16 @@
       </include>
     </group>
   </group>
+
+  <!-- Dummy Diag Publisher -->
+  <group if="$(var launch_dummy_diag_publisher)">
+    <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher.launch.xml">
+      <arg name="config_file" value="$(var dummy_diag_publisher_param_path)"/>
+      <arg name="extra_config_file_sensor" value="$(var sensor_launch_pkg)/config/dummy_diag_publisher/sensor_kit.param.yaml"/>
+      <arg name="launch_rqt_reconfigure" value="$(var launch_rqt_reconfigure)"/>
+      <arg name="launch_rqt_runtime_monitor" value="$(var launch_rqt_runtime_monitor)"/>
+      <arg name="launch_rqt_robot_monitor" value="$(var launch_rqt_robot_monitor)"/>
+      <arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
+    </include>
+  </group>
 </launch>
diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/dummy_diag_publisher/CMakeLists.txt
@@ -4,15 +4,12 @@ project(dummy_diag_publisher)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(dummy_diag_publisher SHARED
-  src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp
-)
-
-rclcpp_components_register_node(dummy_diag_publisher
-  PLUGIN "DummyDiagPublisherNode"
-  EXECUTABLE dummy_diag_publisher_node
+ament_auto_add_executable(${PROJECT_NAME}
+  src/dummy_diag_publisher_node.cpp
+  src/dummy_diag_publisher_core.cpp
 )
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/system/dummy_diag_publisher/README.md b/system/dummy_diag_publisher/README.md
@@ -16,19 +16,38 @@ This package outputs a dummy diagnostic data for debugging and developing.
 
 ### Node Parameters
 
-| Name          | Type                           | Default Value | Explanation                             | Reconfigurable |
-| ------------- | ------------------------------ | ------------- | --------------------------------------- | -------------- |
-| `update_rate` | int                            | `10`          | Timer callback period [Hz]              | false          |
-| `diag_name`   | string                         | `diag_name`   | Diag_name set by dummy diag publisher   | false          |
-| `is_active`   | bool                           | `true`        | Force update or not                     | true           |
-| `status`      | DummyDiagPublisherNode::Status | `0 (OK)`      | diag status set by dummy diag publisher | true           |
+The parameter `DIAGNOSTIC_NAME` must be a name that exists in the parameter YAML file. If the parameter `status` is given from a command line, the parameter `is_active` is automatically set to `true`.
+
+| Name                        | Type   | Default Value | Explanation                             | Reconfigurable |
+| --------------------------- | ------ | ------------- | --------------------------------------- | -------------- |
+| `update_rate`               | int    | `10`          | Timer callback period [Hz]              | false          |
+| `DIAGNOSTIC_NAME.is_active` | bool   | `true`        | Force update or not                     | true           |
+| `DIAGNOSTIC_NAME.status`    | string | `"OK"`        | diag status set by dummy diag publisher | true           |
+
+### YAML format for dummy_diag_publisher
+
+If the value is `default`, the default value will be set.
+
+| Key                                        | Type   | Default Value | Explanation                             |
+| ------------------------------------------ | ------ | ------------- | --------------------------------------- |
+| `required_diags.DIAGNOSTIC_NAME.is_active` | bool   | `true`        | Force update or not                     |
+| `required_diags.DIAGNOSTIC_NAME.status`    | string | `"OK"`        | diag status set by dummy diag publisher |
 
 ## Assumptions / Known limits
 
 TBD.
 
 ## Usage
 
+### launch
+
 ```sh
 ros2 launch dummy_diag_publisher dummy_diag_publisher.launch.xml
 ```
+
+### reconfigure
+
+```sh
+ros2 param set /dummy_diag_publisher velodyne_connection.status "Warn"
+ros2 param set /dummy_diag_publisher velodyne_connection.is_active true
+```
diff --git a/system/dummy_diag_publisher/config/_empty.param.yaml b/system/dummy_diag_publisher/config/_empty.param.yaml
@@ -0,0 +1,15 @@
+# Description:
+#   name: diag name
+#   is_active: Force update or not
+#   status: diag status set by dummy diag publisher "OK, Warn, Error, Stale"
+#
+# Note:
+#
+# default values are:
+#   is_active: "true"
+#   status: "OK"
+---
+/**:
+  ros__parameters:
+    required_diags:
+      dummy_diag_empty: default
diff --git a/system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml b/system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml
@@ -0,0 +1,32 @@
+# Description:
+#   name: diag name
+#   is_active: Force update or not
+#   status: diag status set by dummy diag publisher "OK, Warn, Error, Stale"
+#
+# Note:
+#
+# default values are:
+#   is_active: "true"
+#   status: "OK"
+---
+/**:
+  ros__parameters:
+    required_diags:
+      velodyne_connection: {is_active: "false", status: "Error"}
+
+      velodyne_temperature: {is_active: "true", status: "Warn"}
+
+      livox_ptp_signal: default
+
+      camera_connection: {is_active: "true", status: "Stale"}
+
+
+      lane_departure: default
+
+      obstacle_crash: default
+
+      vehicle_errors: default
+
+      can_bus_connection: default
+
+      speaker: default
diff --git a/system/dummy_diag_publisher/config/extra.param.yaml b/system/dummy_diag_publisher/config/extra.param.yaml
@@ -0,0 +1,16 @@
+# Description:
+#   name: diag name
+#   is_active: Force update or not
+#   status: diag status set by dummy diag publisher "OK, Warn, Error, Stale"
+#
+# Note:
+#
+# default values are:
+#   is_active: "true"
+#   status: "OK"
+---
+/**:
+  ros__parameters:
+    required_diags:
+      camera_connection: {is_active: "true", status: "Warn"}
+      extra: default
diff --git a/...g_publisher/dummy_diag_publisher_node.hpp → ...g_publisher/dummy_diag_publisher_core.hpp b/...g_publisher/dummy_diag_publisher_node.hpp → ...g_publisher/dummy_diag_publisher_core.hpp
@@ -12,29 +12,30 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_
-#define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_
+#ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_
+#define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rcl_interfaces/msg/detail/set_parameters_result__struct.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <optional>
 #include <string>
 #include <vector>
 
 struct DiagConfig
 {
-  std::string name;
   std::string hardware_id;
   std::string msg_ok;
   std::string msg_warn;
   std::string msg_error;
   std::string msg_stale;
 };
 
-class DummyDiagPublisherNode : public rclcpp::Node
+class DummyDiagPublisher : public rclcpp::Node
 {
 public:
-  explicit DummyDiagPublisherNode(const rclcpp::NodeOptions & node_options);
+  DummyDiagPublisher();
 
 private:
   enum Status {
@@ -44,31 +45,46 @@ class DummyDiagPublisherNode : public rclcpp::Node
     STALE,
   };
 
-  struct DummyDiagPublisherConfig
+  struct DummyDiagConfig
   {
-    Status status;
+    std::string name;
     bool is_active;
+    Status status;
   };
 
+  using RequiredDiags = std::vector<DummyDiagConfig>;
+
   // Parameter
   double update_rate_;
   DiagConfig diag_config_;
-  DummyDiagPublisherConfig config_;
+  DummyDiagConfig config_;
+
+  RequiredDiags required_diags_;
+  void loadRequiredDiags();
+
+  std::optional<Status> convertStrToStatus(std::string & status_str);
+  std::string convertStatusToStr(const Status & status);
 
   // Dynamic Reconfigure
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
   rcl_interfaces::msg::SetParametersResult paramCallback(
     const std::vector<rclcpp::Parameter> & parameters);
 
+  rcl_interfaces::msg::SetParametersResult updateDiag(
+    const std::string diag_name, DummyDiagConfig & config, bool is_active, const Status status);
+
   // Diagnostic Updater
   // Set long period to reduce automatic update
   diagnostic_updater::Updater updater_{this, 1000.0 /* sec */};
 
-  void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
-
+  void produceOKDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void produceErrorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void produceWarnDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void produceStaleDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void addDiagByStatus(const std::string & diag_name, const Status status);
   // Timer
   void onTimer();
   rclcpp::TimerBase::SharedPtr timer_;
 };
 
-#endif  // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_
+#endif  // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_
diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml
@@ -1,4 +1,8 @@
 <launch>
+  <arg name="config_file" default="$(find-pkg-share dummy_diag_publisher)/config/_empty.param.yaml"/>
+  <arg name="extra_config_file_sensor" default="$(find-pkg-share dummy_diag_publisher)/config/_empty.param.yaml"/>
+  <arg name="update_rate" default="10.0"/>
+
   <arg name="launch_rqt_reconfigure" default="true"/>
   <arg name="launch_rqt_runtime_monitor" default="true"/>
   <arg name="launch_rqt_robot_monitor" default="true"/>
@@ -22,46 +26,9 @@
     </node>
   </group>
 
-  <!-- velodyne -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="velodyne_connection"/>
-  </include>
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="velodyne_temperature"/>
-  </include>
-
-  <!-- livox -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="livox_ptp_signal"/>
-  </include>
-
-  <!-- camera -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="camera_connection"/>
-  </include>
-
-  <!-- lane departure -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="lane_departure"/>
-  </include>
-
-  <!-- obstacle crash -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="obstacle_crash"/>
-  </include>
-
-  <!-- vehicle -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="vehicle_errors"/>
-  </include>
-
-  <!-- CAN -->
-  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="can_bus_connection"/>
-  </include>
-
-  <!-- speaker (in order to check if correctly ignored) -->
   <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
-    <arg name="diag_name" value="speaker"/>
+    <arg name="config_file" value="$(var config_file)"/>
+    <arg name="extra_config_file_sensor" value="$(var extra_config_file_sensor)"/>
+    <arg name="update_rate" value="$(var update_rate)"/>
   </include>
 </launch>
diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml
@@ -1,15 +1,13 @@
 <launch>
-  <arg name="diag_name"/>
+  <arg name="config_file" default="$(find-pkg-share dummy_diag_publisher)/config/dummy_diag_publisher.param.yaml"/>
+  <arg name="extra_config_file_sensor" default="$(find-pkg-share dummy_diag_publisher)/config/extra.param.yaml"/>
   <arg name="update_rate" default="10.0"/>
-  <arg name="is_active" default="true"/>
-  <arg name="status" default="0"/>
 
   <group>
-    <node pkg="dummy_diag_publisher" exec="dummy_diag_publisher_node" name="$(anon dummy_diag_publisher)" output="screen">
-      <param name="diag_name" value="$(var diag_name)"/>
+    <node pkg="dummy_diag_publisher" exec="dummy_diag_publisher" name="dummy_diag_publisher" output="screen">
+      <param from="$(var config_file)"/>
+      <param from="$(var extra_config_file_sensor)"/>
       <param name="update_rate" value="$(var update_rate)"/>
-      <param name="is_active" value="$(var is_active)"/>
-      <param name="status" value="$(var status)"/>
     </node>
   </group>
 </launch>
diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml
@@ -5,13 +5,16 @@
   <version>0.1.0</version>
   <description>The dummy_diag_publisher ROS2 package</description>
   <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <maintainer email="akihiro.sakurai@tier4.jp">Akihiro Sakurai</maintainer>
+  <maintainer email="fumihito.ito@tier4.jp">Fumihito Ito</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <build_depend>autoware_cmake</build_depend>
 
   <depend>diagnostic_updater</depend>
+  <depend>fmt</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tier4_autoware_utils</depend>