support for node interfaces (ros#112)

* support for node interfaces Signed-off-by: Karsten Knese <[email protected]> * use default parameter descriptor Signed-off-by: Karsten Knese <[email protected]> * rclpy 373 resolved: use namespace for parameter Signed-off-by: Karsten Knese <[email protected]> * set qos profile for publisher Signed-off-by: Karsten Knese <[email protected]> * make c++ and py example behave similar Signed-off-by: Karsten Knese <[email protected]> * set period as either rcl_duration_value_t or double Signed-off-by: Karsten Knese <[email protected]> * revert changes to leading slashes in favor of pr 109 Signed-off-by: Karsten Knese <[email protected]> * accept also raw pointer to node Signed-off-by: Karsten Knese <[email protected]> * make tests compile Signed-off-by: Karsten Knese <[email protected]> * make parameter names equal between c++ and py Signed-off-by: Karsten Knese <[email protected]> * prefix parameter with diagnostic_updater Signed-off-by: Karsten Knese <[email protected]>
boschresearch · Aug 27, 2019 · 7672656 · 7672656
1 parent 53f0083
commit 7672656
Show file tree

Hide file tree

Showing 7 changed files with 123 additions and 72 deletions.
diff --git a/diagnostic_updater/CMakeLists.txt b/diagnostic_updater/CMakeLists.txt
@@ -36,6 +36,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(rclcpp_lifecycle REQUIRED)
   ament_add_gtest(diagnostic_updater_test test/diagnostic_updater_test.cpp)
   target_include_directories(diagnostic_updater_test
     PUBLIC
@@ -46,6 +47,7 @@ if(BUILD_TESTING)
     diagnostic_updater_test
     "diagnostic_msgs"
     "rclcpp"
+    "rclcpp_lifecycle"
     "std_msgs"
   )
 

diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py
@@ -43,8 +43,6 @@
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
 
 from rclpy.clock import Clock
-from rclpy.parameter import Parameter
-from rclpy.parameter_service import ParameterService
 
 from ._diagnostic_status_wrapper import DiagnosticStatusWrapper
 
@@ -235,18 +233,15 @@ def __init__(self, node):
         """Construct an updater class."""
         DiagnosticTaskVector.__init__(self)
         self.node = node
-        self.publisher = self.node.create_publisher(DiagnosticArray, '/diagnostics')
+        self.publisher = self.node.create_publisher(DiagnosticArray, '/diagnostics', 1)
         self.clock = Clock()
         now = self.clock.now()
 
         self.last_time = now
 
         self.last_time_period_checked = self.last_time
-        self.period = 1
-        self.node.set_parameters_atomically(
-            [Parameter('diagnostics_update', Parameter.Type.INTEGER, 1)]
-        )
-        self.parameter_server = ParameterService(self.node)
+        self.period_parameter = 'diagnostic_updater.period'
+        self.period = self.node.declare_parameter(self.period_parameter, 1.0).value
 
         self.verbose = False
         self.hwid = ''
@@ -332,7 +327,7 @@ def _check_diagnostic_period(self):
         # parameter server using a standard timeout mechanism (4Hz)
         now = self.clock.now()
         if now >= self.last_time_period_checked:
-            self.period = self.node.get_parameter('diagnostics_update').value
+            self.period = self.node.get_parameter(self.period_parameter).value
             self.last_time_period_checked = now
 
     def publish(self, msg):

diff --git a/diagnostic_updater/diagnostic_updater/example.py b/diagnostic_updater/diagnostic_updater/example.py
@@ -192,7 +192,7 @@ def main():
     # is in a special state.
     updater.broadcast(b'0', 'Doing important initialization stuff.')
 
-    pub1 = node.create_publisher(std_msgs.msg.Bool, 'topic1')
+    pub1 = node.create_publisher(std_msgs.msg.Bool, 'topic1', 10)
     sleep(2)  # It isn't important if it doesn't take time.
 
     # Some diagnostic tasks are very common, such as checking the rate

diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
@@ -370,11 +370,32 @@ class Updater : public DiagnosticTaskVector
    * \param h Node handle from which to get the diagnostic_period
    * parameter.
    */
-  explicit Updater(rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("diagnostics_updater"))
-  : node_(node), node_name_(node_->get_name())
+  template<class NodeT>
+  explicit Updater(NodeT node)
+  : Updater(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_logging_interface(),
+      node->get_node_parameters_interface())
+  {}
+
+  Updater(
+    std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> base_interface,
+    std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> topics_interface,
+    std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> logging_interface,
+    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface)
+  : verbose_(false),
+    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)
   {
-    // @todo: how to deal with default node?
-    setup();
+    double period = parameters_interface->declare_parameter(
+      "diagnostic_updater.period", rclcpp::ParameterValue(1.0)).get<double>();
+    period_ = static_cast<rcl_duration_value_t>(period * 1e9);
+    next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_);
   }
 
   /**
@@ -434,8 +455,7 @@ class Updater : public DiagnosticTaskVector
 
         if (verbose_ && status.level) {
           RCLCPP_WARN(
-            node_->get_logger(),
-            "Non-zero diagnostic status. Name: '%s', status %i: '%s'",
+            logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'",
             status.name.c_str(), status.level, status.message.c_str());
         }
       }
@@ -446,7 +466,7 @@ class Updater : public DiagnosticTaskVector
         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(node_->get_logger(), error_msg);
+        RCLCPP_WARN(logger_, error_msg);
         warn_nohwid_done_ = true;
       }
 
@@ -458,7 +478,23 @@ class Updater : public DiagnosticTaskVector
    * \brief Returns the interval between updates.
    */
 
-  rcl_duration_value_t getPeriod() {return period_;}
+  rcl_duration_value_t getPeriod() const {return period_;}
+
+  /**
+   * \brief Sets the period as a rcl_duration_value_t
+   */
+  void setPeriod(rcl_duration_value_t period)
+  {
+    period_ = period;
+  }
+
+  /**
+   * \brief Sets the period given a value in seconds
+   */
+  void setPeriod(double period)
+  {
+    period_ = rcl_duration_value_t(period * 1e9);
+  }
 
   /**
    * \brief Output a message on all the known DiagnosticStatus.
@@ -498,8 +534,7 @@ class Updater : public DiagnosticTaskVector
     char buff[kBufferSize];  // @todo This could be done more elegantly.
     va_start(va, format);
     if (vsnprintf(buff, kBufferSize, format, va) >= kBufferSize) {
-      RCLCPP_DEBUG(
-        node_->get_logger(), "Really long string in diagnostic_updater::setHardwareIDf.");
+      RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf.");
     }
     hwid_ = std::string(buff);
     va_end(va);
@@ -515,11 +550,6 @@ class Updater : public DiagnosticTaskVector
   void update_diagnostic_period()
   {
     rcl_duration_value_t old_period = period_;
-#if 0  // @todo: nodes don't automatically have a parameter service yet...disable
-       // for now
-    rclcpp::parameter_client::SyncParametersClient client(private_node_handle_);
-    period_ = client.get_parameter("diagnostic_period", period_);
-#endif
     next_time_ = next_time_ +
       rclcpp::Duration(period_ - old_period);             // Update next_time_
   }
@@ -543,6 +573,7 @@ class Updater : public DiagnosticTaskVector
       status_vec.begin();
       iter != status_vec.end(); iter++)
     {
+      // see https://github.com/ros/diagnostics/pull/109
       iter->name = node_name_.substr(1) + std::string(": ") + iter->name;
     }
     diagnostic_msgs::msg::DiagnosticArray msg;
@@ -551,24 +582,6 @@ class Updater : public DiagnosticTaskVector
     publisher_->publish(msg);
   }
 
-  /**
-   * Publishes on /diagnostics and reads the diagnostic_period parameter.
-   */
-  void setup()
-  {
-    publisher_ =
-      node_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>(
-      "/diagnostics", 1);
-
-    period_ = static_cast<rcl_duration_value_t>(1e9);  // 1.0s
-
-    next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_);
-    update_diagnostic_period();
-
-    verbose_ = false;
-    warn_nohwid_done_ = false;
-  }
-
   /**
    * Causes a placeholder DiagnosticStatus to be published as soon as a
    * diagnostic task is added to the Updater.
@@ -581,8 +594,8 @@ class Updater : public DiagnosticTaskVector
     publish(stat);
   }
 
-  rclcpp::Node::SharedPtr node_;
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr publisher_;
+  rclcpp::Logger logger_;
 
   rclcpp::Time next_time_;
 

diff --git a/diagnostic_updater/package.xml b/diagnostic_updater/package.xml
@@ -27,6 +27,7 @@
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>rclcpp_lifecycle</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp
@@ -128,7 +128,7 @@ int main(int argc, char ** argv)
   // The Updater class advertises to /diagnostics, and has a
   // ~diagnostic_period parameter that says how often the diagnostics
   // should be published.
-  diagnostic_updater::Updater updater;
+  diagnostic_updater::Updater updater(node);
 
   // The diagnostic_updater::Updater class will fill out the hardware_id
   // field of the diagnostic_msgs::DiagnosticStatus message. You need to
@@ -186,7 +186,7 @@ int main(int argc, char ** argv)
   // is in a special state.
   updater.broadcast(0, "Doing important initialization stuff.");
 
-  auto pub1 = node->create_publisher<std_msgs::msg::Bool>("topic1", 1);
+  auto pub1 = node->create_publisher<std_msgs::msg::Bool>("topic1", 10);
   rclcpp::Rate(2).sleep();  // It isn't important if it doesn't take time.
 
   // Some diagnostic tasks are very common, such as checking the rate
@@ -233,7 +233,7 @@ int main(int argc, char ** argv)
 
   while (rclcpp::ok()) {
     std_msgs::msg::Bool msg;
-    rclcpp::Rate(0.1).sleep();
+    rclcpp::Rate(10).sleep();
 
     // Calls to pub1 have to be accompanied by calls to pub1_freq to keep
     // the statistics up to date.

diff --git a/diagnostic_updater/test/diagnostic_updater_test.cpp b/diagnostic_updater/test/diagnostic_updater_test.cpp
@@ -35,12 +35,16 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
+#include <memory>
 #include <thread>
 
 #include "diagnostic_updater/DiagnosticStatusWrapper.hpp"
 #include "diagnostic_updater/diagnostic_updater.hpp"
 #include "diagnostic_updater/update_functions.hpp"
 
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
 using namespace std::chrono_literals;
 
 class TestClass
@@ -57,33 +61,76 @@ class TestClass
   }
 };
 
-TEST(DiagnosticUpdater, testDiagnosticUpdater) {
-  class classFunction : public diagnostic_updater::DiagnosticTask
-  {
+class classFunction : public diagnostic_updater::DiagnosticTask
+{
 public:
-    classFunction()
-    : DiagnosticTask("classFunction") {}
-
-    void run(diagnostic_updater::DiagnosticStatusWrapper & s)
-    {
-      s.summary(0, "Test is running");
-      s.addf("Value", "%f", 5);
-      s.add("String", "Toto");
-      s.add("Floating", 5.55);
-      s.add("Integer", 5);
-      s.addf("Formatted %s %i", "Hello", 5);
-      s.add("Bool", true);
-    }
-  };
+  classFunction()
+  : DiagnosticTask("classFunction") {}
+
+  void run(diagnostic_updater::DiagnosticStatusWrapper & s)
+  {
+    s.summary(0, "Test is running");
+    s.addf("Value", "%f", 5);
+    s.add("String", "Toto");
+    s.add("Floating", 5.55);
+    s.add("Integer", 5);
+    s.addf("Formatted %s %i", "Hello", 5);
+    s.add("Bool", true);
+  }
+};
+
+TEST(DiagnosticUpdater, testDiagnosticUpdater) {
+  rclcpp::init(0, nullptr);
+
+  TestClass c;
+
+  auto node = std::make_shared<rclcpp::Node>("test_diagnostics_updater");
+  diagnostic_updater::Updater updater(node);
+
+  updater.add("wrapped", &c, &TestClass::wrapped);
+
+  classFunction cf;
+  updater.add(cf);
+
+  rclcpp::shutdown();
+  SUCCEED();
+}
+
+TEST(DiagnosticUpdater, testLifecycleDiagnosticUpdater) {
+  rclcpp::init(0, nullptr);
 
   TestClass c;
 
-  diagnostic_updater::Updater updater;
+  auto node =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_lifeycycle_diagnostics_updater");
+  diagnostic_updater::Updater updater(node);
 
   updater.add("wrapped", &c, &TestClass::wrapped);
 
   classFunction cf;
   updater.add(cf);
+
+  rclcpp::shutdown();
+  SUCCEED();
+}
+
+class DiagnosticWrapperNode : public rclcpp::Node
+{
+public:
+  DiagnosticWrapperNode()
+  : Node("DiagnosticUpdaterTestNode"),
+    updater_(this)
+  {}
+
+private:
+  diagnostic_updater::Updater updater_;
+};
+
+TEST(DiagnosticUpdater, testUpdaterAsNodeClassMember) {
+  rclcpp::init(0, nullptr);
+  DiagnosticWrapperNode node;
+  rclcpp::shutdown();
+  SUCCEED();
 }
 
 TEST(DiagnosticUpdater, testDiagnosticStatusWrapperKeyValuePairs) {
@@ -223,10 +270,3 @@ TEST(DiagnosticUpdater, testTimeStampStatus) {
   EXPECT_STREQ("Timestamp Status", ts.getName().c_str()) <<
     "Name should be \"Timestamp Status\"";
 }
-
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}