Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(dummy_diag_publisher): not use diagnostic_updater and param callback for v0.29.0 #1414

Merged
merged 2 commits into from
Jul 17, 2024
Merged

fix(dummy_diag_publisher): not use diagnostic_updater and param callback for v0.29.0 #1414

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
86d6ebb
fix(dummy_diag_publisher): not use diagnostic_updater and param callback
h-ohta Apr 24, 2024
d746ac2
style(pre-commit): autofix
pre-commit-ci[bot] Apr 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 4 additions & 19 deletions system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,10 @@
#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 <diagnostic_msgs/msg/diagnostic_array.hpp>

#include <optional>
#include <string>
#include <vector>
Expand Down Expand Up @@ -64,27 +64,12 @@ class DummyDiagPublisher : public rclcpp::Node

std::optional<Status> convertStrToStatus(std::string & status_str);
std::string convertStatusToStr(const Status & status);
diagnostic_msgs::msg::DiagnosticStatus::_level_type convertStatusToLevel(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 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_;
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_;
};

#endif // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_
183 changes: 32 additions & 151 deletions system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,22 +14,9 @@

#include "dummy_diag_publisher/dummy_diag_publisher_core.hpp"

#include <rcl_interfaces/msg/detail/set_parameters_result__struct.hpp>

#define FMT_HEADER_ONLY
#include <autoware/universe_utils/ros/update_param.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/create_timer.hpp>

#include <fmt/format.h>

#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

namespace
{
std::vector<std::string> split(const std::string & str, const char delim)
Expand All @@ -44,82 +31,6 @@ std::vector<std::string> split(const std::string & str, const char delim)
}
} // namespace

rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::paramCallback(
const std::vector<rclcpp::Parameter> & parameters)
{
rcl_interfaces::msg::SetParametersResult result;

for (const auto & param : parameters) {
const auto param_name = param.get_name();
const auto split_names = split(param_name, '.');
const auto & diag_name = split_names.at(0);
auto it = std::find_if(
std::begin(required_diags_), std::end(required_diags_),
[&diag_name](DummyDiagConfig config) { return config.name == diag_name; });
if (it == std::end(required_diags_)) { // diag name not found
result.successful = false;
result.reason = "no matching diag name";
} else { // diag name found
const auto status_prefix_str = diag_name + std::string(".status");
const auto is_active_prefix_str = diag_name + std::string(".is_active");
auto status_str = convertStatusToStr(it->status);
auto prev_status_str = status_str;
auto is_active = true;
try {
autoware::universe_utils::updateParam(parameters, status_prefix_str, status_str);
autoware::universe_utils::updateParam(parameters, is_active_prefix_str, is_active);
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
result.successful = false;
result.reason = e.what();
return result;
}
const auto status = convertStrToStatus(status_str);
if (!status) {
result.successful = false;
result.reason = "invalid status";
return result;
}
result = updateDiag(diag_name, *it, is_active, *status);
} // end diag name found
}
return result;
}

// update diag with new param
rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::updateDiag(
const std::string diag_name, DummyDiagConfig & config, bool is_active, const Status status)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "success";

if (is_active == config.is_active && config.status == status) { // diag config not changed
result.successful = true;
result.reason = "config not changed";
} else if (is_active == true && config.is_active == false) { // newly activated
config.is_active = true;
if (config.status == status) { // status not changed
addDiagByStatus(diag_name, config.status);
} else { // status changed
config.status = status;
addDiagByStatus(diag_name, status);
}
} else { // deactivated or status changed
if (!updater_.removeByName(diag_name)) {
result.successful = false;
result.reason = "updater removal failed";
return result;
}
if (is_active == false) { // deactivated
config.is_active = false;
} else { // status changed
config.status = status;
addDiagByStatus(diag_name, status);
}
}
return result;
}

std::optional<DummyDiagPublisher::Status> DummyDiagPublisher::convertStrToStatus(
std::string & status_str)
{
Expand Down Expand Up @@ -147,6 +58,21 @@ std::string DummyDiagPublisher::convertStatusToStr(const Status & status)
}
}

diagnostic_msgs::msg::DiagnosticStatus::_level_type DummyDiagPublisher::convertStatusToLevel(
const Status & status)
{
switch (status) {
case Status::OK:
return diagnostic_msgs::msg::DiagnosticStatus::OK;
case Status::WARN:
return diagnostic_msgs::msg::DiagnosticStatus::WARN;
case Status::ERROR:
return diagnostic_msgs::msg::DiagnosticStatus::ERROR;
default:
return diagnostic_msgs::msg::DiagnosticStatus::STALE;
}
}

void DummyDiagPublisher::loadRequiredDiags()
{
const auto param_key = std::string("required_diags");
Expand Down Expand Up @@ -189,60 +115,22 @@ void DummyDiagPublisher::loadRequiredDiags()
}
}

void DummyDiagPublisher::produceOKDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
diagnostic_msgs::msg::DiagnosticStatus status;

status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
status.message = diag_config_.msg_ok;

stat.summary(status.level, status.message);
}
void DummyDiagPublisher::produceWarnDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
diagnostic_msgs::msg::DiagnosticStatus status;

status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
status.message = diag_config_.msg_warn;

stat.summary(status.level, status.message);
}
void DummyDiagPublisher::produceErrorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
diagnostic_msgs::msg::DiagnosticStatus status;

status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
status.message = diag_config_.msg_error;

stat.summary(status.level, status.message);
}
void DummyDiagPublisher::produceStaleDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
diagnostic_msgs::msg::DiagnosticStatus status;

status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
status.message = diag_config_.msg_stale;

stat.summary(status.level, status.message);
}
void DummyDiagPublisher::addDiagByStatus(const std::string & diag_name, const Status status)
{
if (status == Status::OK) {
updater_.add(diag_name, this, &DummyDiagPublisher::produceOKDiagnostics);
} else if (status == Status::WARN) {
updater_.add(diag_name, this, &DummyDiagPublisher::produceWarnDiagnostics);
} else if (status == Status::ERROR) {
updater_.add(diag_name, this, &DummyDiagPublisher::produceErrorDiagnostics);
} else if (status == Status::STALE) {
updater_.add(diag_name, this, &DummyDiagPublisher::produceStaleDiagnostics);
} else {
throw std::runtime_error("invalid status");
}
}

void DummyDiagPublisher::onTimer()
{
updater_.force_update();
diagnostic_msgs::msg::DiagnosticArray array;

for (const auto & e : required_diags_) {
if (e.is_active) {
diagnostic_msgs::msg::DiagnosticStatus status;
status.hardware_id = diag_config_.hardware_id;
status.name = e.name;
status.message = convertStatusToStr(e.status);
status.level = convertStatusToLevel(e.status);
array.status.push_back(status);
}
}
array.header.stamp = this->now();
pub_->publish(array);
}

rclcpp::NodeOptions override_options(rclcpp::NodeOptions options)
Expand All @@ -258,26 +146,19 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options)
// Parameter
update_rate_ = this->get_parameter_or("update_rate", 10.0);

// Set parameter callback
set_param_res_ = this->add_on_set_parameters_callback(
std::bind(&DummyDiagPublisher::paramCallback, this, std::placeholders::_1));

// Diagnostic Updater
loadRequiredDiags();

const std::string hardware_id = "dummy_diag";
updater_.setHardwareID(hardware_id);
diag_config_ = DiagConfig{hardware_id, "OK", "Warn", "Error", "Stale"};
for (const auto & config : required_diags_) {
if (config.is_active) {
addDiagByStatus(config.name, config.status);
}
}

// Timer
const auto period_ns = rclcpp::Rate(update_rate_).period();
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&DummyDiagPublisher::onTimer, this));

// Publisher
pub_ = create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", rclcpp::QoS(1));
}

#include <rclcpp_components/register_node_macro.hpp>
Expand Down
Loading