From 9d7b50e4f7ad48846eccafd22a0d0ea4fe885172 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Tue, 4 Dec 2018 14:24:48 -0800 Subject: [PATCH] adding node path and time stamp to parameter event message (#584) modify adding clock for rclcpp_lifestyle --- rclcpp/include/rclcpp/node.hpp | 2 +- .../rclcpp/node_interfaces/node_parameters.hpp | 5 +++++ rclcpp/src/rclcpp/node.cpp | 17 +++++++++-------- .../rclcpp/node_interfaces/node_parameters.cpp | 14 +++++++++----- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 2 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 17 +++++++++-------- 6 files changed, 34 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 4fdd981..3084277 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -471,8 +471,8 @@ private: rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; bool use_intra_process_comms_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index b872de0..1beab95 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -51,6 +51,7 @@ public: const node_interfaces::NodeBaseInterface::SharedPtr node_base, const node_interfaces::NodeTopicsInterface::SharedPtr node_topics, const node_interfaces::NodeServicesInterface::SharedPtr node_services, + const node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::vector & initial_parameters, bool use_intra_process, bool start_parameter_services); @@ -120,6 +121,10 @@ private: Publisher::SharedPtr events_publisher_; std::shared_ptr parameter_service_; + + std::string combined_name_; + + node_interfaces::NodeClockInterface::SharedPtr node_clock_; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 9355ff9..c40f3f3 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -67,14 +67,6 @@ Node::Node( node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), - node_parameters_(new rclcpp::node_interfaces::NodeParameters( - node_base_, - node_topics_, - node_services_, - initial_parameters, - use_intra_process_comms, - start_parameter_services - )), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, node_topics_, @@ -82,6 +74,15 @@ Node::Node( node_services_, node_logging_ )), + node_parameters_(new rclcpp::node_interfaces::NodeParameters( + node_base_, + node_topics_, + node_services_, + node_clock_, + initial_parameters, + use_intra_process_comms, + start_parameter_services + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), use_intra_process_comms_(use_intra_process_comms) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 9f37aaa..205b096 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -36,9 +36,11 @@ NodeParameters::NodeParameters( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::vector & initial_parameters, bool use_intra_process, bool start_parameter_services) +: node_clock_(node_clock) { using MessageT = rcl_interfaces::msg::ParameterEvent; using PublisherT = rclcpp::Publisher; @@ -105,11 +107,11 @@ NodeParameters::NodeParameters( // Should never happen throw std::runtime_error("Node name and namespace were not set"); } - std::string combined_name; + if ('/' == node_namespace.at(node_namespace.size() - 1)) { - combined_name = node_namespace + node_name; + combined_name_ = node_namespace + node_name; } else { - combined_name = node_namespace + '/' + node_name; + combined_name_ = node_namespace + '/' + node_name; } std::map parameters; @@ -131,7 +133,7 @@ NodeParameters::NodeParameters( rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params); rcl_yaml_node_struct_fini(yaml_params); - auto iter = initial_map.find(combined_name); + auto iter = initial_map.find(combined_name_); if (initial_map.end() == iter) { continue; } @@ -186,6 +188,8 @@ NodeParameters::set_parameters_atomically( std::map tmp_map; auto parameter_event = std::make_shared(); + parameter_event->node = combined_name_; + // TODO(jacquelinekay): handle parameter constraints rcl_interfaces::msg::SetParametersResult result; if (parameters_callback_) { @@ -228,7 +232,7 @@ NodeParameters::set_parameters_atomically( } std::swap(tmp_map, parameters_); - + parameter_event->stamp = node_clock_->get_clock()->now(); events_publisher_->publish(parameter_event); return result; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index bb5c275..be0817b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -519,8 +519,8 @@ private: rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; bool use_intra_process_comms_; diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index ec03c1a..43cff74 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -74,20 +74,21 @@ LifecycleNode::LifecycleNode( node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), - node_parameters_(new rclcpp::node_interfaces::NodeParameters( - node_base_, - node_topics_, - node_services_, - initial_parameters, - use_intra_process_comms, - start_parameter_services - )), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, node_topics_, node_graph_, node_services_, node_logging_ + )), + node_parameters_(new rclcpp::node_interfaces::NodeParameters( + node_base_, + node_topics_, + node_services_, + node_clock_, + initial_parameters, + use_intra_process_comms, + start_parameter_services )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), use_intra_process_comms_(use_intra_process_comms),