From c93beb5d164162fd0806641c7032b834b37de935 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Wed, 12 Dec 2018 11:52:54 -0800 Subject: [PATCH] Resolve startup race condition for sim time (#608) Resolves #595 * Separate the Node Time Source from the Node Clock * Implement initial value checking of use_sim_time parameter parameter * Be sure to update all newly attached clocks * Homogenizing the behavior to use the last received value otherwise zero time when enabling sim time. * Add virtual destructors to interface classes --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/node.hpp | 7 ++ .../node_interfaces/node_base_interface.hpp | 4 ++ .../rclcpp/node_interfaces/node_clock.hpp | 1 - .../node_interfaces/node_clock_interface.hpp | 4 ++ .../node_interfaces/node_graph_interface.hpp | 4 ++ .../node_logging_interface.hpp | 4 ++ .../node_parameters_interface.hpp | 4 ++ .../node_services_interface.hpp | 4 ++ .../node_interfaces/node_time_source.hpp | 72 +++++++++++++++++++ .../node_time_source_interface.hpp | 42 +++++++++++ .../node_interfaces/node_timers_interface.hpp | 4 ++ .../node_interfaces/node_topics_interface.hpp | 4 ++ .../node_waitables_interface.hpp | 4 ++ rclcpp/include/rclcpp/time_source.hpp | 6 +- rclcpp/src/rclcpp/node.cpp | 16 +++++ .../src/rclcpp/node_interfaces/node_clock.cpp | 10 +-- .../node_interfaces/node_time_source.cpp | 50 +++++++++++++ rclcpp/src/rclcpp/time_source.cpp | 52 +++++++++++--- rclcpp/test/test_time_source.cpp | 33 ++++++++- 20 files changed, 303 insertions(+), 23 deletions(-) create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp create mode 100644 rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 76901f0..6c3bf88 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -53,6 +53,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_logging.cpp src/rclcpp/node_interfaces/node_parameters.cpp src/rclcpp/node_interfaces/node_services.cpp + src/rclcpp/node_interfaces/node_time_source.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp src/rclcpp/node_interfaces/node_waitables.cpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3084277..7b98597 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -47,6 +47,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" @@ -458,6 +459,11 @@ public: rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + private: RCLCPP_DISABLE_COPY(Node) @@ -473,6 +479,7 @@ private: 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/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index 7869cde..356f69c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -38,6 +38,10 @@ class NodeBaseInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + RCLCPP_PUBLIC + virtual + ~NodeBaseInterface() = default; + /// Return the name of the node. /** \return The name of the node. */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp index 66c74cc..e0e7059 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -64,7 +64,6 @@ private: rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; rclcpp::Clock::SharedPtr ros_clock_; - rclcpp::TimeSource time_source_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp index ef06b0c..055d475 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -31,6 +31,10 @@ class NodeClockInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface) + RCLCPP_PUBLIC + virtual + ~NodeClockInterface() = default; + /// Get a ROS clock which will be kept up to date by the node. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index e93db94..022984d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -38,6 +38,10 @@ class NodeGraphInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface) + RCLCPP_PUBLIC + virtual + ~NodeGraphInterface() = default; + /// Return a map of existing topic names to list of topic types. /** * A topic is considered to exist when at least one publisher or subscriber diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index 16790db..ace0afc 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -33,6 +33,10 @@ class NodeLoggingInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) + RCLCPP_PUBLIC + virtual + ~NodeLoggingInterface() = default; + /// Return the logger of the node. /** \return The logger of the node. */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index aac3b71..a9ee5c3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -37,6 +37,10 @@ class NodeParametersInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface) + RCLCPP_PUBLIC + virtual + ~NodeParametersInterface() = default; + RCLCPP_PUBLIC virtual std::vector diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index 7f591ce..c75fec8 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -32,6 +32,10 @@ class NodeServicesInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface) + RCLCPP_PUBLIC + virtual + ~NodeServicesInterface() = default; + RCLCPP_PUBLIC virtual void diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp new file mode 100644 index 0000000..6ba37b7 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -0,0 +1,72 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/time_source.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTimeSource part of the Node API. +class NodeTimeSource : public NodeTimeSourceInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource) + + RCLCPP_PUBLIC + explicit NodeTimeSource( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters + ); + + RCLCPP_PUBLIC + virtual + ~NodeTimeSource(); + +private: + RCLCPP_DISABLE_COPY(NodeTimeSource) + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + rclcpp::TimeSource time_source_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp new file mode 100644 index 0000000..0f2df14 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -0,0 +1,42 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTimeSource part of the Node API. +class NodeTimeSourceInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTimeSourceInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 09edbad..422409a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -31,6 +31,10 @@ class NodeTimersInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface) + RCLCPP_PUBLIC + virtual + ~NodeTimersInterface() = default; + /// Add a timer to the node. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 2a2e628..b3d46ff 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -40,6 +40,10 @@ class NodeTopicsInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) + RCLCPP_PUBLIC + virtual + ~NodeTopicsInterface() = default; + RCLCPP_PUBLIC virtual rclcpp::PublisherBase::SharedPtr diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index ef78f52..e848cea 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -31,6 +31,10 @@ class NodeWaitablesInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface) + RCLCPP_PUBLIC + virtual + ~NodeWaitablesInterface() = default; + RCLCPP_PUBLIC virtual void diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 436abf6..09a8340 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -51,7 +51,9 @@ public: rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface); + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface); RCLCPP_PUBLIC void detachNode(); @@ -76,6 +78,8 @@ private: rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; // Store (and update on node attach) logger for logging. Logger logger_; diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c40f3f3..15311ff 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -29,6 +29,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" @@ -83,6 +84,15 @@ Node::Node( use_intra_process_comms, start_parameter_services )), + 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_(use_intra_process_comms) { @@ -263,6 +273,12 @@ Node::get_node_logging_interface() return node_logging_; } +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +Node::get_node_time_source_interface() +{ + return node_time_source_; +} + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr Node::get_node_timers_interface() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp index cb6335d..8de8c3c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -31,15 +31,7 @@ NodeClock::NodeClock( node_services_(node_services), node_logging_(node_logging), ros_clock_(std::make_shared(RCL_ROS_TIME)) -{ - time_source_.attachNode( - node_base_, - node_topics_, - node_graph_, - node_services_, - node_logging_); - time_source_.attachClock(ros_clock_); -} +{} NodeClock::~NodeClock() {} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp new file mode 100644 index 0000000..7c678b7 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp @@ -0,0 +1,50 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/node_interfaces/node_time_source.hpp" + +#include +#include + +using rclcpp::node_interfaces::NodeTimeSource; + +NodeTimeSource::NodeTimeSource( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters) +: node_base_(node_base), + node_topics_(node_topics), + node_graph_(node_graph), + node_services_(node_services), + node_logging_(node_logging), + node_clock_(node_clock), + node_parameters_(node_parameters) +{ + time_source_.attachNode( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_); + time_source_.attachClock(node_clock_->get_clock()); +} + +NodeTimeSource::~NodeTimeSource() +{} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 38e7074..173ccf0 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -53,7 +53,9 @@ void TimeSource::attachNode(rclcpp::Node::SharedPtr node) node->get_node_topics_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), - node->get_node_logging_interface()); + node->get_node_logging_interface(), + node->get_node_clock_interface(), + node->get_node_parameters_interface()); } void TimeSource::attachNode( @@ -61,17 +63,38 @@ void TimeSource::attachNode( rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface) + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { node_base_ = node_base_interface; node_topics_ = node_topics_interface; node_graph_ = node_graph_interface; node_services_ = node_services_interface; node_logging_ = node_logging_interface; + node_clock_ = node_clock_interface; + node_parameters_ = node_parameters_interface; // TODO(tfoote): Update QOS logger_ = node_logging_->get_logger(); + rclcpp::Parameter use_sim_time_param; + if (node_parameters_->get_parameter("use_sim_time", use_sim_time_param)) { + if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { + if (use_sim_time_param.get_value() == true) { + parameter_state_ = SET_TRUE; + enable_ros_time(); + create_clock_sub(); + } + } else { + RCLCPP_ERROR(logger_, "Invalid type for parameter 'use_sim_time' %s should be bool", + use_sim_time_param.get_type_name().c_str()); + } + } else { + RCLCPP_DEBUG(logger_, "'use_sim_time' parameter not set, using wall time by default."); + } + + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 parameter_client_ = std::make_shared( node_base_, node_topics_, @@ -92,6 +115,9 @@ void TimeSource::detachNode() node_topics_.reset(); node_graph_.reset(); node_services_.reset(); + node_logging_.reset(); + node_clock_.reset(); + node_parameters_.reset(); disable_ros_time(); } @@ -103,11 +129,12 @@ void TimeSource::attachClock(std::shared_ptr clock) std::lock_guard guard(clock_list_lock_); associated_clocks_.push_back(clock); - // Set the clock if there's already data for it + // Set the clock to zero unless there's a recently received message + auto time_msg = std::make_shared(); if (last_msg_set_) { - auto time_msg = std::make_shared(last_msg_set_->clock); - set_clock(time_msg, ros_time_active_, clock); + time_msg = std::make_shared(last_msg_set_->clock); } + set_clock(time_msg, ros_time_active_, clock); } void TimeSource::detachClock(std::shared_ptr clock) @@ -123,7 +150,9 @@ void TimeSource::detachClock(std::shared_ptr clock) TimeSource::~TimeSource() { - if (node_base_ || node_topics_ || node_graph_ || node_services_) { + if (node_base_ || node_topics_ || node_graph_ || node_services_ || + node_logging_ || node_clock_ || node_parameters_) + { this->detachNode(); } } @@ -257,13 +286,14 @@ void TimeSource::enable_ros_time() // Local storage ros_time_active_ = true; - // Update all attached clocks + // Update all attached clocks to zero or last recorded time std::lock_guard guard(clock_list_lock_); + auto time_msg = std::make_shared(); + if (last_msg_set_) { + time_msg = std::make_shared(last_msg_set_->clock); + } for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - auto msg = std::make_shared(); - msg->sec = 0; - msg->nanosec = 0; - set_clock(msg, true, *it); + set_clock(time_msg, true, *it); } } diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index 54c0e69..99d39eb 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -112,7 +112,7 @@ TEST_F(TestTimeSource, reattach) { ASSERT_NO_THROW(ts.attachNode(node)); } -TEST_F(TestTimeSource, ROS_time_valid) { +TEST_F(TestTimeSource, ROS_time_valid_attach_detach) { rclcpp::TimeSource ts; auto ros_clock = std::make_shared(RCL_ROS_TIME); @@ -131,6 +131,37 @@ TEST_F(TestTimeSource, ROS_time_valid) { EXPECT_FALSE(ros_clock->ros_time_is_active()); } +TEST_F(TestTimeSource, ROS_time_valid_wall_time) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + auto ros_clock2 = std::make_shared(RCL_ROS_TIME); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock2); + EXPECT_FALSE(ros_clock2->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_time_valid_sim_time) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + auto ros_clock2 = std::make_shared(RCL_ROS_TIME); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + node->set_parameter_if_not_set("use_sim_time", true); + ts.attachNode(node); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock2); + EXPECT_TRUE(ros_clock2->ros_time_is_active()); +} + TEST_F(TestTimeSource, clock) { rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME);