diff --git a/diagnostic_updater/CMakeLists.txt b/diagnostic_updater/CMakeLists.txt index cd723d2e6..dd4261e72 100644 --- a/diagnostic_updater/CMakeLists.txt +++ b/diagnostic_updater/CMakeLists.txt @@ -11,22 +11,25 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) find_package(std_msgs REQUIRED) -add_library(${PROJECT_NAME} INTERFACE) +add_library(${PROJECT_NAME} + src/diagnostic_updater.cpp +) target_include_directories( ${PROJECT_NAME} - INTERFACE + PUBLIC $ $ ) ament_target_dependencies( ${PROJECT_NAME} - INTERFACE + PUBLIC diagnostic_msgs rclcpp ) @@ -63,12 +66,10 @@ if(BUILD_TESTING) $ $ ) + target_link_libraries(diagnostic_updater_test ${PROJECT_NAME}) ament_target_dependencies( diagnostic_updater_test - "diagnostic_msgs" - "rclcpp" "rclcpp_lifecycle" - "std_msgs" ) ament_add_gtest(diagnostic_status_wrapper_test test/diagnostic_status_wrapper_test.cpp) @@ -90,11 +91,7 @@ if(BUILD_TESTING) $ $ ) - ament_target_dependencies( - status_msg_test - "diagnostic_msgs" - "rclcpp" - ) + target_link_libraries(status_msg_test ${PROJECT_NAME}) find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test(diagnostic_updater_test.py "test/diagnostic_updater_test.py") diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp index 904ca6e5c..8d4795669 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp @@ -47,7 +47,8 @@ #include "diagnostic_msgs/msg/diagnostic_status.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" namespace diagnostic_updater { diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index a301ada59..635ead06a 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -50,8 +50,12 @@ #include "rcl/time.h" -#include "rclcpp/create_timer.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" namespace diagnostic_updater { @@ -381,43 +385,7 @@ class Updater : public DiagnosticTaskVector std::shared_ptr parameters_interface, std::shared_ptr timers_interface, std::shared_ptr topics_interface, - double period = 1.0) - : verbose_(false), - base_interface_(base_interface), - timers_interface_(timers_interface), - clock_(clock_interface->get_clock()), - period_(rclcpp::Duration::from_seconds(period)), - publisher_( - rclcpp::create_publisher( - topics_interface, "/diagnostics", 1)), - logger_(logging_interface->get_logger()), - node_name_(base_interface->get_name()), - warn_nohwid_done_(false) - { - constexpr const char * period_param_name = "diagnostic_updater.period"; - rclcpp::ParameterValue period_param; - if (parameters_interface->has_parameter(period_param_name)) { - period_param = parameters_interface->get_parameter(period_param_name).get_parameter_value(); - } else { - period_param = parameters_interface->declare_parameter( - period_param_name, rclcpp::ParameterValue(period)); - } - period = period_param.get(); - period_ = rclcpp::Duration::from_seconds(period); - - reset_timer(); - - constexpr const char * use_fqn_param_name = "diagnostic_updater.use_fqn"; - rclcpp::ParameterValue use_fqn_param; - if (parameters_interface->has_parameter(use_fqn_param_name)) { - use_fqn_param = parameters_interface->get_parameter(use_fqn_param_name).get_parameter_value(); - } else { - use_fqn_param = parameters_interface->declare_parameter( - use_fqn_param_name, rclcpp::ParameterValue(false)); - } - node_name_ = use_fqn_param.get() ? - base_interface->get_fully_qualified_name() : base_interface->get_name(); - } + double period = 1.0); /** * \brief Returns the interval between updates. @@ -459,105 +427,20 @@ class Updater : public DiagnosticTaskVector * * \param msg Status message to output. */ - void broadcast(unsigned char lvl, const std::string msg) - { - std::vector status_vec; - - const std::vector & tasks = getTasks(); - for (std::vector::const_iterator iter = - tasks.begin(); - iter != tasks.end(); iter++) - { - diagnostic_updater::DiagnosticStatusWrapper status; - - status.name = iter->getName(); - status.summary(lvl, msg); - - status_vec.push_back(status); - } + void broadcast(unsigned char lvl, const std::string msg); - publish(status_vec); - } - - void setHardwareIDf(const char * format, ...) - { - va_list va; - const int kBufferSize = 1000; - char buff[kBufferSize]; // @todo This could be done more elegantly. - va_start(va, format); - if (vsnprintf(buff, kBufferSize, format, va) >= kBufferSize) { - RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf."); - } - hwid_ = std::string(buff); - va_end(va); - } + void setHardwareIDf(const char * format, ...); void setHardwareID(const std::string & hwid) {hwid_ = hwid;} private: - void reset_timer() - { - update_timer_ = rclcpp::create_timer( - base_interface_, - timers_interface_, - clock_, - period_, - std::bind(&Updater::update, this)); - } + void reset_timer(); /** * \brief Causes the diagnostics to update if the inter-update interval * has been exceeded. */ - void update() - { - if (rclcpp::ok()) { - bool warn_nohwid = hwid_.empty(); - - std::vector status_vec; - - std::unique_lock lock( - lock_); // Make sure no adds happen while we are processing here. - const std::vector & tasks = getTasks(); - for (std::vector::const_iterator iter = - tasks.begin(); - iter != tasks.end(); iter++) - { - diagnostic_updater::DiagnosticStatusWrapper status; - - status.name = iter->getName(); - status.level = 2; - status.message = "No message was set"; - status.hardware_id = hwid_; - - iter->run(status); - - status_vec.push_back(status); - - if (status.level) { - warn_nohwid = false; - } - - if (verbose_ && status.level) { - RCLCPP_WARN( - logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'", - status.name.c_str(), status.level, status.message.c_str()); - } - } - - if (warn_nohwid && !warn_nohwid_done_) { - std::string error_msg = "diagnostic_updater: No HW_ID was set."; - error_msg += " This is probably a bug. Please report it."; - error_msg += " For devices that do not have a HW_ID, set this value to 'none'."; - error_msg += " This warning only occurs once all diagnostics are OK."; - error_msg += " It is okay to wait until the device is open before calling setHardwareID."; - RCLCPP_WARN(logger_, "%s", error_msg.c_str()); - warn_nohwid_done_ = true; - } - - publish(status_vec); - } - } + void update(); /** * Recheck the diagnostic_period on the parameter server. (Cached) @@ -574,41 +457,18 @@ class Updater : public DiagnosticTaskVector /** * Publishes a single diagnostic status. */ - void publish(diagnostic_msgs::msg::DiagnosticStatus & stat) - { - std::vector status_vec; - status_vec.push_back(stat); - publish(status_vec); - } + void publish(diagnostic_msgs::msg::DiagnosticStatus & stat); /** * Publishes a vector of diagnostic statuses. */ - void publish(std::vector & status_vec) - { - for (std::vector::iterator iter = - status_vec.begin(); - iter != status_vec.end(); iter++) - { - iter->name = node_name_ + std::string(": ") + iter->name; - } - diagnostic_msgs::msg::DiagnosticArray msg; - msg.status = status_vec; - msg.header.stamp = clock_->now(); - publisher_->publish(msg); - } + void publish(std::vector & status_vec); /** * Causes a placeholder DiagnosticStatus to be published as soon as a * diagnostic task is added to the Updater. */ - virtual void addedTaskCallback(DiagnosticTaskInternal & task) - { - DiagnosticStatusWrapper stat; - stat.name = task.getName(); - stat.summary(0, "Node starting up"); - publish(stat); - } + virtual void addedTaskCallback(DiagnosticTaskInternal & task); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface_; rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface_; diff --git a/diagnostic_updater/src/diagnostic_updater.cpp b/diagnostic_updater/src/diagnostic_updater.cpp new file mode 100644 index 000000000..10ddb1573 --- /dev/null +++ b/diagnostic_updater/src/diagnostic_updater.cpp @@ -0,0 +1,198 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include "rclcpp/rclcpp.hpp" + +namespace diagnostic_updater +{ +Updater::Updater( + std::shared_ptr base_interface, + std::shared_ptr clock_interface, + std::shared_ptr logging_interface, + std::shared_ptr parameters_interface, + std::shared_ptr timers_interface, + std::shared_ptr topics_interface, double period) +: verbose_(false), + base_interface_(base_interface), + timers_interface_(timers_interface), + clock_(clock_interface->get_clock()), + period_(rclcpp::Duration::from_seconds(period)), + publisher_(rclcpp::create_publisher( + topics_interface, "/diagnostics", 1)), + logger_(logging_interface->get_logger()), + node_name_(base_interface->get_name()), + warn_nohwid_done_(false) +{ + constexpr const char * period_param_name = "diagnostic_updater.period"; + rclcpp::ParameterValue period_param; + if (parameters_interface->has_parameter(period_param_name)) { + period_param = parameters_interface->get_parameter(period_param_name).get_parameter_value(); + } else { + period_param = + parameters_interface->declare_parameter(period_param_name, rclcpp::ParameterValue(period)); + } + period = period_param.get(); + period_ = rclcpp::Duration::from_seconds(period); + + reset_timer(); + + constexpr const char * use_fqn_param_name = "diagnostic_updater.use_fqn"; + rclcpp::ParameterValue use_fqn_param; + if (parameters_interface->has_parameter(use_fqn_param_name)) { + use_fqn_param = parameters_interface->get_parameter(use_fqn_param_name).get_parameter_value(); + } else { + use_fqn_param = + parameters_interface->declare_parameter(use_fqn_param_name, rclcpp::ParameterValue(false)); + } + node_name_ = use_fqn_param.get() ? base_interface->get_fully_qualified_name() : + base_interface->get_name(); +} + +void Updater::broadcast(unsigned char lvl, const std::string msg) +{ + std::vector status_vec; + + const std::vector & tasks = getTasks(); + for (std::vector::const_iterator iter = tasks.begin(); + iter != tasks.end(); iter++) + { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = iter->getName(); + status.summary(lvl, msg); + + status_vec.push_back(status); + } + + publish(status_vec); +} + +void Updater::setHardwareIDf(const char * format, ...) +{ + va_list va; + const int kBufferSize = 1000; + char buff[kBufferSize]; // @todo This could be done more elegantly. + va_start(va, format); + if (vsnprintf(buff, kBufferSize, format, va) >= kBufferSize) { + RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf."); + } + hwid_ = std::string(buff); + va_end(va); +} + +void Updater::reset_timer() +{ + update_timer_ = rclcpp::create_timer( + base_interface_, timers_interface_, clock_, period_, std::bind(&Updater::update, this)); +} + +void Updater::update() +{ + if (rclcpp::ok()) { + bool warn_nohwid = hwid_.empty(); + + std::vector status_vec; + + std::unique_lock lock( + lock_); // Make sure no adds happen while we are processing here. + const std::vector & tasks = getTasks(); + for (std::vector::const_iterator iter = tasks.begin(); + iter != tasks.end(); iter++) + { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = iter->getName(); + status.level = 2; + status.message = "No message was set"; + status.hardware_id = hwid_; + + iter->run(status); + + status_vec.push_back(status); + + if (status.level) { + warn_nohwid = false; + } + + if (verbose_ && status.level) { + RCLCPP_WARN( + logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), + status.level, status.message.c_str()); + } + } + + if (warn_nohwid && !warn_nohwid_done_) { + std::string error_msg = "diagnostic_updater: No HW_ID was set."; + error_msg += " This is probably a bug. Please report it."; + error_msg += " For devices that do not have a HW_ID, set this value to 'none'."; + error_msg += " This warning only occurs once all diagnostics are OK."; + error_msg += " It is okay to wait until the device is open before calling setHardwareID."; + RCLCPP_WARN(logger_, "%s", error_msg.c_str()); + warn_nohwid_done_ = true; + } + + publish(status_vec); + } +} + +void Updater::publish(diagnostic_msgs::msg::DiagnosticStatus & stat) +{ + std::vector status_vec; + status_vec.push_back(stat); + publish(status_vec); +} + +void Updater::publish(std::vector & status_vec) +{ + for (std::vector::iterator iter = status_vec.begin(); + iter != status_vec.end(); iter++) + { + iter->name = node_name_ + std::string(": ") + iter->name; + } + diagnostic_msgs::msg::DiagnosticArray msg; + msg.status = status_vec; + msg.header.stamp = clock_->now(); + publisher_->publish(msg); +} + +void Updater::addedTaskCallback(DiagnosticTaskInternal & task) +{ + DiagnosticStatusWrapper stat; + stat.name = task.getName(); + stat.summary(0, "Node starting up"); + publish(stat); +} +} // namespace diagnostic_updater diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp index fb03c98dc..328c2f717 100644 --- a/diagnostic_updater/src/example.cpp +++ b/diagnostic_updater/src/example.cpp @@ -34,9 +34,10 @@ #include #include - #include +#include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; double time_to_launch;