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 #9257

Merged
merged 2 commits into from
Nov 8, 2024
Merged

fix(dummy_diag_publisher): not use diagnostic_updater and param callback #9257

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
bfaf996
fix(dummy_diag_publisher): not use diagnostic_updater and param callb…
saka1-s Jul 17, 2024
2c04db1
fix: resolve build error of dummy diag publisher (#1415)
saka1-s Jul 17, 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_
185 changes: 33 additions & 152 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 @@ -43,83 +30,7 @@
return elems;
}
} // namespace

Check notice on line 33 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

DummyDiagPublisher::paramCallback is no longer above the threshold for logical blocks with deeply nested code

Check notice on line 33 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

DummyDiagPublisher::updateDiag is no longer above the threshold for logical blocks with deeply nested code
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 @@
}
}

diagnostic_msgs::msg::DiagnosticStatus::_level_type DummyDiagPublisher::convertStatusToLevel(

Check warning on line 61 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp#L61 

Added line #L61 was not covered by tests
const Status & status)
{
switch (status) {

Check warning on line 64 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp#L64 

Added line #L64 was not covered by tests
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::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);
}

Check warning on line 130 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp#L124-L130 

Added lines #L124 - L130 were not covered by tests
}
array.header.stamp = this->now();
pub_->publish(array);

Check warning on line 133 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp#L132-L133 

Added lines #L132 - L133 were not covered by tests
}

rclcpp::NodeOptions override_options(rclcpp::NodeOptions options)
Expand All @@ -252,32 +140,25 @@
}

DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options)
: Node("dummy_diag_publisher", override_options(options)), updater_(this)
: Node("dummy_diag_publisher", override_options(options))

Check warning on line 143 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp#L143 

Added line #L143 was not covered by tests

{
// 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));

Check warning on line 161 in system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp#L161 

Added line #L161 was not covered by tests
}

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