diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/dummy_diag_publisher/CMakeLists.txt new file mode 100644 index 0000000000000..1878f725320a0 --- /dev/null +++ b/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 +) diff --git a/system/dummy_diag_publisher/README.md b/system/dummy_diag_publisher/README.md new file mode 100644 index 0000000000000..d553885f8ca70 --- /dev/null +++ b/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 +``` diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_node.hpp b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_node.hpp new file mode 100644 index 0000000000000..0a2a10ca832dc --- /dev/null +++ b/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 +#include + +#include +#include + +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 & 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_ diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml new file mode 100644 index 0000000000000..1fe7e8fe26434 --- /dev/null +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000000000..26fab9cf3ebd5 --- /dev/null +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml new file mode 100644 index 0000000000000..1e29f7fdc6689 --- /dev/null +++ b/system/dummy_diag_publisher/package.xml @@ -0,0 +1,24 @@ + + + + dummy_diag_publisher + 0.1.0 + The dummy_diag_publisher ROS2 package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_utils + diagnostic_updater + rclcpp + + rqt_reconfigure + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp new file mode 100644 index 0000000000000..43d8e9e10b44d --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include + +rcl_interfaces::msg::SetParametersResult DummyDiagPublisherNode::paramCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + DummyDiagPublisherConfig config = config_; + try { + int status = static_cast(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("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(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::duration(1 / update_rate_)); + + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/main.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/main.cpp new file mode 100644 index 0000000000000..6e72d8511db68 --- /dev/null +++ b/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 + +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}