diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 03ece2e58b..cbedd048fe 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -122,7 +122,7 @@ LifecycleNode::LifecycleNode( )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), - impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_)) + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_, node_clock_)) { impl_->init(enable_communication_interface); diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 1e8030b4d3..fe991a8d1a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -31,6 +31,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -52,10 +53,12 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, std::shared_ptr node_services_interface, - std::shared_ptr node_logging_interface) + std::shared_ptr node_logging_interface, + std::shared_ptr node_clock_interface) : node_base_interface_(node_base_interface), node_services_interface_(node_services_interface), - node_logging_interface_(node_logging_interface) + node_logging_interface_(node_logging_interface), + node_clock_interface_(node_clock_interface) { } @@ -86,6 +89,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf state_machine_options.enable_com_interface = enable_communication_interface; state_machine_options.allocator = node_options->allocator; + rcl_clock_t * clock = node_clock_interface_->get_clock()->get_clock_handle(); + // The call to initialize the state machine takes // currently five different typesupports for all publishers/services // created within the RCL_LIFECYCLE structure. @@ -97,6 +102,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf rcl_ret_t ret = rcl_lifecycle_state_machine_init( &state_machine_, node_handle, + clock, ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 5cf5bdaacf..ee318bd134 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -34,6 +34,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -54,7 +55,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, std::shared_ptr node_services_interface, - std::shared_ptr node_logging_interface); + std::shared_ptr node_logging_interface, + std::shared_ptr node_clock_interface); ~LifecycleNodeInterfaceImpl(); @@ -155,6 +157,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; using NodeLoggingPtr = std::shared_ptr; + using NodeClockPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; using GetAvailableStatesSrvPtr = @@ -167,6 +170,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; NodeLoggingPtr node_logging_interface_; + NodeClockPtr node_clock_interface_; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_;