From b352d450319e84dd28aed78b800ab40938cec5c2 Mon Sep 17 00:00:00 2001 From: Vinnam Kim Date: Wed, 27 Mar 2019 08:24:20 +0900 Subject: [PATCH] Fix use_sim_time issue on LifeCycleNode (#651) Signed-off-by: vinnamkim --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 7 +++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 16 ++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 2e368dac8c..c93837ab9e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -43,6 +43,7 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" @@ -383,6 +384,11 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr @@ -513,6 +519,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; 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 63d1dfbcba..b9a1665f4b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -33,6 +33,7 @@ #include "rclcpp/node_interfaces/node_logging.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" @@ -81,6 +82,15 @@ LifecycleNode::LifecycleNode( options.start_parameter_event_publisher(), options.parameter_event_qos_profile() )), + node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), use_intra_process_comms_(options.use_intra_process_comms()), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) @@ -274,6 +284,12 @@ LifecycleNode::get_node_logging_interface() return node_logging_; } +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +LifecycleNode::get_node_time_source_interface() +{ + return node_time_source_; +} + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr LifecycleNode::get_node_timers_interface() {