feat: add dummy_diag_publisher package

system/dummy_diag_publisher/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(dummy_diag_publisher)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+### Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+### Target executable
+set(DUMMY_DIAG_PUBLISHER_SRC
+  src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp)
+
+ament_auto_add_executable(${PROJECT_NAME}
+  src/dummy_diag_publisher_node/main.cpp
+  ${DUMMY_DIAG_PUBLISHER_SRC}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

system/dummy_diag_publisher/README.md

@@ -0,0 +1,33 @@
+# dummy_diag_publisher
+
+## Purpose
+
+This package outputs a dummy diagnostic data for debugging and developing.
+
+## Inputs / Outputs
+
+### Outputs
+
+| Name           | Type                                     | Description         |
+| -------------- | ---------------------------------------- | ------------------- |
+| `/diagnostics` | `diagnostic_msgs::msgs::DiagnosticArray` | Diagnostics outputs |
+
+## Parameters
+
+### Node Parameters
+
+| Name          | Type   | Default Value | Explanation                           |
+| ------------- | ------ | ------------- | ------------------------------------- |
+| `update_rate` | int    | `10`          | Timer callback period [Hz]            |
+| `diag_name`   | string | `diag_name`   | Diag_name set by dummy diag publisher |
+| `is_active`   | bool   | `true`        | Force update or not                   |
+
+## Assumptions / Known limits
+
+TBD.
+
+## Usage
+
+```sh
+ros2 launch dummy_diag_publisher dummy_diag_publisher.launch.xml
+```

system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_node.hpp

@@ -0,0 +1,74 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// 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.
+
+#ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_
+#define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#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
+{
+public:
+  DummyDiagPublisherNode();
+
+private:
+  enum Status {
+    OK,
+    WARN,
+    ERROR,
+    STALE,
+  };
+
+  struct DummyDiagPublisherConfig
+  {
+    Status status;
+    bool is_active;
+  };
+
+  // Parameter
+  double update_rate_;
+  DiagConfig diag_config_;
+  DummyDiagPublisherConfig config_;
+
+  // Dynamic Reconfigure
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult paramCallback(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  // Diagnostic Updater
+  // Set long period to reduce automatic update
+  diagnostic_updater::Updater updater_{this, 1000.0 /* sec */};
+
+  void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  // Timer
+  void onTimer();
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+#endif  // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_

system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml

@@ -0,0 +1,67 @@
+<launch>
+  <arg name="launch_rqt_reconfigure" default="true"/>
+  <arg name="launch_rqt_runtime_monitor" default="true"/>
+  <arg name="launch_rqt_robot_monitor" default="true"/>
+  <arg name="launch_rqt_runtime_monitor_err" default="true"/>
+
+  <group if="$(var launch_rqt_reconfigure)">
+    <node pkg="rqt_reconfigure" exec="rqt_reconfigure" name="rqt_reconfigure"/>
+  </group>
+
+  <group if="$(var launch_rqt_runtime_monitor)">
+    <node pkg="rqt_runtime_monitor" exec="rqt_runtime_monitor" name="rqt_runtime_monitor"/>
+  </group>
+
+  <group if="$(var launch_rqt_robot_monitor)">
+    <node pkg="rqt_robot_monitor" exec="rqt_robot_monitor" name="rqt_robot_monitor"/>
+  </group>
+
+  <group if="$(var launch_rqt_runtime_monitor_err)">
+    <node pkg="rqt_runtime_monitor" exec="rqt_runtime_monitor" name="rqt_runtime_monitor_err">
+      <remap from="/diagnostics" to="/diagnostics_err" />
+    </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"/>
+  </include>
+</launch>

system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="diag_name" />
+  <arg name="update_rate" default="10.0" />
+  <arg name="is_active" default="true" />
+
+  <group>
+    <push-ros-namespace namespace="dummy_diag_publisher"/>
+
+    <node pkg="dummy_diag_publisher" exec="dummy_diag_publisher" name="$(var diag_name)" output="screen">
+      <param name="diag_name" value="$(var diag_name)" />
+      <param name="update_rate" value="$(var update_rate)" />
+      <param name="is_active" value="$(var is_active)" />
+    </node>
+  </group>
+</launch>

system/dummy_diag_publisher/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dummy_diag_publisher</name>
+  <version>0.1.0</version>
+  <description>The dummy_diag_publisher ROS2 package</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_utils</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>rclcpp</depend>
+
+  <exec_depend>rqt_reconfigure</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp

@@ -0,0 +1,106 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// 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.
+
+#include "dummy_diag_publisher/dummy_diag_publisher_node.hpp"
+
+#include <autoware_utils/ros/update_param.hpp>
+#include <rclcpp/create_timer.hpp>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+rcl_interfaces::msg::SetParametersResult DummyDiagPublisherNode::paramCallback(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+
+  DummyDiagPublisherConfig config = config_;
+  try {
+    int status = static_cast<int>(config.status);
+    autoware_utils::updateParam(parameters, "status", status);
+    config.status = Status(status);
+    autoware_utils::updateParam(parameters, "is_active", config.is_active);
+    config_ = config;
+  } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+    result.successful = false;
+    result.reason = e.what();
+  }
+
+  return result;
+}
+
+void DummyDiagPublisherNode::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  diagnostic_msgs::msg::DiagnosticStatus status;
+
+  if (config_.status == Status::OK) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    status.message = diag_config_.msg_ok;
+  } else if (config_.status == Status::WARN) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    status.message = diag_config_.msg_warn;
+  } else if (config_.status == Status::ERROR) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    status.message = diag_config_.msg_error;
+  } else if (config_.status == Status::STALE) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
+    status.message = diag_config_.msg_stale;
+  } else {
+    throw std::runtime_error("invalid status");
+  }
+
+  stat.summary(status.level, status.message);
+}
+
+void DummyDiagPublisherNode::onTimer()
+{
+  if (config_.is_active) {
+    updater_.force_update();
+  }
+}
+
+DummyDiagPublisherNode::DummyDiagPublisherNode() : Node("dummy_diag_publisher")
+{
+  // Parameter
+  update_rate_ = declare_parameter("update_rate", 10.0);
+  const std::string diag_name = this->declare_parameter<std::string>("diag_name");
+  const std::string hardware_id = "dummy_diag_" + diag_name;
+  diag_config_ = DiagConfig{
+    diag_name, hardware_id, "OK", "Warn", "Error", "Stale",
+  };
+
+  // Set parameter callback
+  config_.status = static_cast<Status>(this->declare_parameter("status", 0));
+  config_.is_active = this->declare_parameter("is_active", true);
+  set_param_res_ = this->add_on_set_parameters_callback(
+    std::bind(&DummyDiagPublisherNode::paramCallback, this, std::placeholders::_1));
+
+  // Diagnostic Updater
+  updater_.setHardwareID(diag_config_.hardware_id);
+  updater_.add(diag_config_.name, this, &DummyDiagPublisherNode::produceDiagnostics);
+
+  // Timer
+  auto timer_callback = std::bind(&DummyDiagPublisherNode::onTimer, this);
+  auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    std::chrono::duration<double>(1 / update_rate_));
+
+  timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
+    this->get_clock(), period, std::move(timer_callback),
+    this->get_node_base_interface()->get_context());
+  this->get_node_timers_interface()->add_timer(timer_, nullptr);
+}

system/dummy_diag_publisher/src/dummy_diag_publisher_node/main.cpp

@@ -0,0 +1,29 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// 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.
+
+#include "dummy_diag_publisher/dummy_diag_publisher_node.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<DummyDiagPublisherNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+
+  return 0;
+}