From 0885751e8c7d73553bc3e139631c7897f00b754f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 19 Jun 2019 15:49:39 -0700 Subject: [PATCH 01/11] support for node interfaces Signed-off-by: Karsten Knese --- diagnostic_updater/CMakeLists.txt | 2 + .../diagnostic_updater/_diagnostic_updater.py | 15 ++--- .../diagnostic_updater/diagnostic_updater.hpp | 66 +++++++++---------- diagnostic_updater/package.xml | 1 + diagnostic_updater/src/example.cpp | 2 +- .../test/diagnostic_updater_test.cpp | 54 ++++++++++----- 6 files changed, 78 insertions(+), 62 deletions(-) diff --git a/diagnostic_updater/CMakeLists.txt b/diagnostic_updater/CMakeLists.txt index 451b70f96..0e5c11ef8 100644 --- 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 index 7cd3ffb92..49f7d1e92 100644 --- 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,17 @@ 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) + # unable to separate namespace with '.': https://github.com/ros2/rclpy/issues/373 + # workaround so far is to specify with '__'. + self.period_parameter = node.get_name() + '__diagnostics_update' + self.period = self.node.declare_parameter(self.period_parameter, 1.0).value self.verbose = False self.hwid = '' @@ -332,7 +329,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/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 1f54a10e9..2ee51a304 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -370,11 +370,34 @@ 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 + explicit Updater(std::shared_ptr 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 base_interface, + std::shared_ptr topics_interface, + std::shared_ptr logging_interface, + std::shared_ptr parameters_interface) + : verbose_(false), + publisher_( + rclcpp::create_publisher( + 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( + node_name_ + ".diagnostics_updater_period", + rclcpp::ParameterValue(1.0), + rcl_interfaces::msg::ParameterDescriptor()).get(); + period_ = static_cast(period * 1e9); + next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); } /** @@ -434,8 +457,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 +468,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; } @@ -498,8 +520,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 +536,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,7 +559,7 @@ class Updater : public DiagnosticTaskVector status_vec.begin(); iter != status_vec.end(); iter++) { - iter->name = node_name_.substr(1) + std::string(": ") + iter->name; + iter->name = node_name_ + std::string(": ") + iter->name; } diagnostic_msgs::msg::DiagnosticArray msg; msg.status = status_vec; @@ -551,24 +567,6 @@ class Updater : public DiagnosticTaskVector publisher_->publish(msg); } - /** - * Publishes on /diagnostics and reads the diagnostic_period parameter. - */ - void setup() - { - publisher_ = - node_->create_publisher( - "/diagnostics", 1); - - period_ = static_cast(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 +579,8 @@ class Updater : public DiagnosticTaskVector publish(stat); } - rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Logger logger_; rclcpp::Time next_time_; diff --git a/diagnostic_updater/package.xml b/diagnostic_updater/package.xml index 545d2d7d8..9660c5afc 100644 --- a/diagnostic_updater/package.xml +++ b/diagnostic_updater/package.xml @@ -27,6 +27,7 @@ ament_cmake_pytest ament_lint_auto ament_lint_common + rclcpp_lifecycle ament_cmake diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp index 787acd63f..fc57d25f6 100644 --- 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 diff --git a/diagnostic_updater/test/diagnostic_updater_test.cpp b/diagnostic_updater/test/diagnostic_updater_test.cpp index 67389e52d..793f791bd 100644 --- a/diagnostic_updater/test/diagnostic_updater_test.cpp +++ b/diagnostic_updater/test/diagnostic_updater_test.cpp @@ -35,12 +35,16 @@ #include #include +#include #include #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,28 +61,42 @@ 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) { + TestClass c; + + auto node = std::make_shared("test_diagnostics_updater"); + diagnostic_updater::Updater updater(node); + + updater.add("wrapped", &c, &TestClass::wrapped); + + classFunction cf; + updater.add(cf); +} +TEST(DiagnosticUpdater, testLifecycleDiagnosticUpdater) { TestClass c; - diagnostic_updater::Updater updater; + auto node = + std::make_shared("test_lifeycycle_diagnostics_updater"); + diagnostic_updater::Updater updater(node); updater.add("wrapped", &c, &TestClass::wrapped); From eb1421e4e663d7e5468812f6ca033b74f984432b Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 20 Jun 2019 11:51:25 -0700 Subject: [PATCH 02/11] use default parameter descriptor Signed-off-by: Karsten Knese --- .../include/diagnostic_updater/diagnostic_updater.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 2ee51a304..b4dbc46f1 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -394,8 +394,7 @@ class Updater : public DiagnosticTaskVector { double period = parameters_interface->declare_parameter( node_name_ + ".diagnostics_updater_period", - rclcpp::ParameterValue(1.0), - rcl_interfaces::msg::ParameterDescriptor()).get(); + rclcpp::ParameterValue(1.0)).get(); period_ = static_cast(period * 1e9); next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); } From 15fb3f87373048b28745a51dbabf54d4c4763719 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 31 Jul 2019 18:03:35 -0700 Subject: [PATCH 03/11] rclpy 373 resolved: use namespace for parameter Signed-off-by: Karsten Knese --- diagnostic_updater/diagnostic_updater/_diagnostic_updater.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index 49f7d1e92..ab323aa54 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -240,9 +240,7 @@ def __init__(self, node): self.last_time = now self.last_time_period_checked = self.last_time - # unable to separate namespace with '.': https://github.com/ros2/rclpy/issues/373 - # workaround so far is to specify with '__'. - self.period_parameter = node.get_name() + '__diagnostics_update' + self.period_parameter = node.get_name() + '.diagnostics_update' self.period = self.node.declare_parameter(self.period_parameter, 1.0).value self.verbose = False From b134ed8b585bc7c5fdca899783b6a0822aca6b38 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 31 Jul 2019 18:17:59 -0700 Subject: [PATCH 04/11] set qos profile for publisher Signed-off-by: Karsten Knese --- diagnostic_updater/diagnostic_updater/example.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diagnostic_updater/diagnostic_updater/example.py b/diagnostic_updater/diagnostic_updater/example.py index bc7d3bc57..9dd09a94f 100755 --- 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 From f1c9fc037f7ed65c9335887a076e4e1296275eb7 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 31 Jul 2019 18:19:15 -0700 Subject: [PATCH 05/11] make c++ and py example behave similar Signed-off-by: Karsten Knese --- diagnostic_updater/src/example.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp index fc57d25f6..7f585fd3a 100644 --- a/diagnostic_updater/src/example.cpp +++ b/diagnostic_updater/src/example.cpp @@ -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("topic1", 1); + auto pub1 = node->create_publisher("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. From df74e3cecfab89d39e0d3a00098997d5809cce3f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 31 Jul 2019 18:25:10 -0700 Subject: [PATCH 06/11] set period as either rcl_duration_value_t or double Signed-off-by: Karsten Knese --- .../diagnostic_updater/diagnostic_updater.hpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index b4dbc46f1..d989e3fe7 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -479,7 +479,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. From 7d68ddd53a3db3e6175be062952a290cb36aab0e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 31 Jul 2019 18:31:43 -0700 Subject: [PATCH 07/11] revert changes to leading slashes in favor of pr 109 Signed-off-by: Karsten Knese --- .../include/diagnostic_updater/diagnostic_updater.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index d989e3fe7..39abcb3b7 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -574,7 +574,8 @@ class Updater : public DiagnosticTaskVector status_vec.begin(); iter != status_vec.end(); iter++) { - iter->name = node_name_ + std::string(": ") + iter->name; + // see https://github.com/ros/diagnostics/pull/109 + iter->name = node_name_.substr(1) + std::string(": ") + iter->name; } diagnostic_msgs::msg::DiagnosticArray msg; msg.status = status_vec; From 633350c553b2e1170025261bf32efad2a3be9c3e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Aug 2019 00:04:08 -0700 Subject: [PATCH 08/11] accept also raw pointer to node Signed-off-by: Karsten Knese --- .../include/diagnostic_updater/diagnostic_updater.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 39abcb3b7..fb6876d1a 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -371,7 +371,7 @@ class Updater : public DiagnosticTaskVector * parameter. */ template - explicit Updater(std::shared_ptr node) + explicit Updater(NodeT node) : Updater( node->get_node_base_interface(), node->get_node_topics_interface(), From 35aa44a471db03912d90a71bc8a8d7de6bc0d65a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Aug 2019 00:04:19 -0700 Subject: [PATCH 09/11] make tests compile Signed-off-by: Karsten Knese --- .../test/diagnostic_updater_test.cpp | 36 +++++++++++++++---- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/diagnostic_updater/test/diagnostic_updater_test.cpp b/diagnostic_updater/test/diagnostic_updater_test.cpp index 793f791bd..88ff6421e 100644 --- a/diagnostic_updater/test/diagnostic_updater_test.cpp +++ b/diagnostic_updater/test/diagnostic_updater_test.cpp @@ -80,6 +80,8 @@ class classFunction : public diagnostic_updater::DiagnosticTask }; TEST(DiagnosticUpdater, testDiagnosticUpdater) { + rclcpp::init(0, nullptr); + TestClass c; auto node = std::make_shared("test_diagnostics_updater"); @@ -89,9 +91,14 @@ TEST(DiagnosticUpdater, testDiagnosticUpdater) { classFunction cf; updater.add(cf); + + rclcpp::shutdown(); + SUCCEED(); } TEST(DiagnosticUpdater, testLifecycleDiagnosticUpdater) { + rclcpp::init(0, nullptr); + TestClass c; auto node = @@ -102,6 +109,28 @@ TEST(DiagnosticUpdater, testLifecycleDiagnosticUpdater) { 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) { @@ -241,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(); -} From a98acb04e3c6b2a44db0f9f69a34b0d006d285ac Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Aug 2019 00:10:09 -0700 Subject: [PATCH 10/11] make parameter names equal between c++ and py Signed-off-by: Karsten Knese --- diagnostic_updater/diagnostic_updater/_diagnostic_updater.py | 2 +- .../include/diagnostic_updater/diagnostic_updater.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index ab323aa54..9f7d8a6be 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -240,7 +240,7 @@ def __init__(self, node): self.last_time = now self.last_time_period_checked = self.last_time - self.period_parameter = node.get_name() + '.diagnostics_update' + self.period_parameter = node.get_name() + '.diagnostic_period' self.period = self.node.declare_parameter(self.period_parameter, 1.0).value self.verbose = False diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index fb6876d1a..44da81e49 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -393,7 +393,7 @@ class Updater : public DiagnosticTaskVector warn_nohwid_done_(false) { double period = parameters_interface->declare_parameter( - node_name_ + ".diagnostics_updater_period", + node_name_ + ".diagnostic_period", rclcpp::ParameterValue(1.0)).get(); period_ = static_cast(period * 1e9); next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); From 9eb75019bd8d1042defd35ee87ab1efe29ecdf43 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 22 Aug 2019 23:59:59 -0700 Subject: [PATCH 11/11] prefix parameter with diagnostic_updater Signed-off-by: Karsten Knese --- diagnostic_updater/diagnostic_updater/_diagnostic_updater.py | 2 +- .../include/diagnostic_updater/diagnostic_updater.hpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index 9f7d8a6be..abcafb76b 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -240,7 +240,7 @@ def __init__(self, node): self.last_time = now self.last_time_period_checked = self.last_time - self.period_parameter = node.get_name() + '.diagnostic_period' + self.period_parameter = 'diagnostic_updater.period' self.period = self.node.declare_parameter(self.period_parameter, 1.0).value self.verbose = False diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 44da81e49..aaf5b0b63 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -393,8 +393,7 @@ class Updater : public DiagnosticTaskVector warn_nohwid_done_(false) { double period = parameters_interface->declare_parameter( - node_name_ + ".diagnostic_period", - rclcpp::ParameterValue(1.0)).get(); + "diagnostic_updater.period", rclcpp::ParameterValue(1.0)).get(); period_ = static_cast(period * 1e9); next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); }