Minimize header includes by moving impl to .cpp files (#331) and Fix usage of rclcpp::ok with a non-default context (#352) (#390)

* Minimize header includes by moving impl to .cpp files (#331)

* Minimize header includes by moving impl to .cpp files
* Make sure to build a shared library

Signed-off-by: Christian Henkel <[email protected]>

* Fix usage of rclcpp::ok with a non-default context (#352)

* Fix usage of rclcpp::ok with a non-default context

The current implementation calls `rclcpp::ok` without any arguments,
which amounts to verifying that the global default context is valid. In
the case where a node is added to a custom context, and the global
context is not used, `rclcpp::ok` without any arguments will always
return `false` since the global context has never been initialized. To
fix it, pass to rclcpp the context that's available via the node's base
interface.

* Add a test for custom context

Signed-off-by: Christian Henkel <[email protected]>

---------

Signed-off-by: Christian Henkel <[email protected]>
Co-authored-by: Ramon Wijnands <[email protected]>
Co-authored-by: Hervé Audren <[email protected]>

Author: 3 people

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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
 ament_target_dependencies(
   ${PROJECT_NAME}
-  INTERFACE
+  PUBLIC
   diagnostic_msgs
   rclcpp
 )
@@ -63,12 +66,10 @@ if(BUILD_TESTING)
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
   )
+  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)
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
   )
-  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")

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
 {

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<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
     std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> timers_interface,
     std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> 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<diagnostic_msgs::msg::DiagnosticArray>(
-        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<double>();
-    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<bool>() ?
-      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<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
-
-    const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
-    for (std::vector<DiagnosticTaskInternal>::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(base_interface_->get_context())) {
-      bool warn_nohwid = hwid_.empty();
-
-      std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
-
-      std::unique_lock<std::mutex> lock(
-        lock_);    // Make sure no adds happen while we are processing here.
-      const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
-      for (std::vector<DiagnosticTaskInternal>::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<diagnostic_msgs::msg::DiagnosticStatus> 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<diagnostic_msgs::msg::DiagnosticStatus> & status_vec)
-  {
-    for (std::vector<diagnostic_msgs::msg::DiagnosticStatus>::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<diagnostic_msgs::msg::DiagnosticStatus> & 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_;