Skip to content

Commit

Permalink
feat: add dummy_diag_publisher package (autowarefoundation#18)
Browse files Browse the repository at this point in the history
* release v0.4.0

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit 6ab6bcca1dea5065fcb06aeec107538dad1f62af.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* ROS2 Porting: dummy_diag_publisher (autowarefoundation#69)

* Fix CMake, package.xml and remove COLCON_IGNORE

* First pass
 - Remove ROS references: dynamic_configuration
 - Can compile

* Fix references to dynamic_reconfigure
 - Clean up comments in cmake and package.xml
 - Add timer callback

* Modify config yamls and remove dynamic reconfigure file
 - Fix launch files

* Fix declaration of parameters using get parameters
 - Add rqt_reconfigure to package deps

* Add comment in launch file

* Remove fmt dependency

* Address PR comment:

- Ensure that status value is initialised properly

Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>

* Address PR comment:

- Ensure config are declared to ensure it can be set in the parameter callback

Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>

* Address PR comment:
 - Remove headers from executable generation in cmake

* Address PR comment:
 - Remove headers specification

Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* Add linters (autowarefoundation#208)

* Rename ROS-related .yaml to .param.yaml (autowarefoundation#352)

* Rename ROS-related .yaml to .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove prefix 'default_' of yaml files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename vehicle_info.yaml to vehicle_info.param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename diagnostic_aggregator's param files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix overlooked parameters

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add use_sim-time option (autowarefoundation#454)

* Fix for rolling (autowarefoundation#1226)

* Replace doc by description

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Replace ns by push-ros-namespace

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Cleanup dummy_diag_publisher (autowarefoundation#1392)

* Cleanup dummy_diag_publisher

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix typo

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Make double and write comment

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Set hardware_id from diag_name

* Add const to daig_name and hardware_id

Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>

* suppress warnings for declare parameters (autowarefoundation#1724)

* fix for lanelet2_extension

* fix for traffic light ssd fine detector

* fix for topic_state_monitor

* fix for dummy diag publisher

* fix for remote cmd converter

* fix for vehicle_info_util

* fix for multi object tracker

* fix for freespace planner

* fix for autoware_error_monitor

* add Werror for multi object tracker

* fix for multi object tracker

* add Werror for liraffic light ssd fine detector

* add Werror for topic state monitor

* add Werror

* add Werror

* add Werror

* add Werror

* fix style

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* Fix typo `obstacle_crush` to `obstacle_crash` (autowarefoundation#2031)

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* Refactor dummy_diag_publisher (autowarefoundation#2151)

* Refactor dummy_diag_publisher

* fix depend order

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* remove COLCON_IGNORE in dummy_diag_publisher (autowarefoundation#528)

* add README in dummy diag publisher (autowarefoundation#627)

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
  • Loading branch information
11 people authored Dec 3, 2021
1 parent 0fdbc9d commit 597ff72
Show file tree
Hide file tree
Showing 8 changed files with 382 additions and 0 deletions.
34 changes: 34 additions & 0 deletions system/dummy_diag_publisher/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
)
33 changes: 33 additions & 0 deletions system/dummy_diag_publisher/README.md
Original file line number Diff line number Diff line change
@@ -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
```
Original file line number Diff line number Diff line change
@@ -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_
67 changes: 67 additions & 0 deletions system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
@@ -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>
24 changes: 24 additions & 0 deletions system/dummy_diag_publisher/package.xml
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
@@ -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);
}
29 changes: 29 additions & 0 deletions system/dummy_diag_publisher/src/dummy_diag_publisher_node/main.cpp
Original file line number Diff line number Diff line change
@@ -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;
}

0 comments on commit 597ff72

Please sign in to comment.