From 284dc1791874c8d6141dc83f3817405a95006cae Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Thu, 30 Nov 2017 14:07:23 -0800 Subject: [PATCH] Add a clock interface to the Node API (#407) node clock interface lower level abstraction Update node and node interface to expose get_clock and now. add unit tests to cover node clock API --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/node.hpp | 15 ++++ .../rclcpp/node_interfaces/node_clock.hpp | 71 +++++++++++++++++++ .../node_interfaces/node_clock_interface.hpp | 44 ++++++++++++ rclcpp/include/rclcpp/time_source.hpp | 13 +++- rclcpp/src/rclcpp/node.cpp | 27 +++++++ .../src/rclcpp/node_interfaces/node_clock.cpp | 48 +++++++++++++ rclcpp/src/rclcpp/time_source.cpp | 59 ++++++++++++--- rclcpp/test/test_node.cpp | 16 +++++ 9 files changed, 284 insertions(+), 10 deletions(-) create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_clock.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp create mode 100644 rclcpp/src/rclcpp/node_interfaces/node_clock.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9344a6e..357a96c 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,6 +49,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/memory_strategy.cpp src/rclcpp/node.cpp src/rclcpp/node_interfaces/node_base.cpp + src/rclcpp/node_interfaces/node_clock.cpp src/rclcpp/node_interfaces/node_graph.cpp src/rclcpp/node_interfaces/node_parameters.cpp src/rclcpp/node_interfaces/node_services.cpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index aa12398..bec9954 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -40,6 +40,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" @@ -359,11 +360,24 @@ public: rclcpp::event::Event::SharedPtr event, std::chrono::nanoseconds timeout); + RCLCPP_PUBLIC + rclcpp::Clock::SharedPtr + get_clock(); + + RCLCPP_PUBLIC + Time + now(); + /// Return the Node's internal NodeBaseInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); + /// Return the Node's internal NodeClockInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeClockInterface::SharedPtr + get_node_clock_interface(); + /// Return the Node's internal NodeGraphInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeGraphInterface::SharedPtr @@ -402,6 +416,7 @@ private: 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_; bool use_intra_process_comms_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp new file mode 100644 index 0000000..3bdc95e --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -0,0 +1,71 @@ +// Copyright 2017 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_CLOCK_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_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_topics_interface.hpp" +#include "rclcpp/time_source.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeClock part of the Node API. +class NodeClock : public NodeClockInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClock) + + RCLCPP_PUBLIC + explicit NodeClock( + 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_PUBLIC + virtual + ~NodeClock(); + + /// Get a clock which will be kept up to date by the node. + RCLCPP_PUBLIC + virtual + rclcpp::Clock::SharedPtr + get_clock(); + +private: + RCLCPP_DISABLE_COPY(NodeClock) + + 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::Clock::SharedPtr ros_clock_; + rclcpp::TimeSource time_source_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp new file mode 100644 index 0000000..ef06b0c --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -0,0 +1,44 @@ +// Copyright 2017 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_CLOCK_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_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 NodeClock part of the Node API. +class NodeClockInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface) + + /// Get a ROS clock which will be kept up to date by the node. + RCLCPP_PUBLIC + virtual + rclcpp::Clock::SharedPtr + get_clock() = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 40b93dc..34c6424 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -20,6 +20,7 @@ #include "rcl/time.h" +#include "builtin_interfaces/msg/time.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" #include "rclcpp/node.hpp" @@ -43,6 +44,13 @@ public: RCLCPP_PUBLIC void attachNode(rclcpp::node::Node::SharedPtr node); + RCLCPP_PUBLIC + void attachNode( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface); + RCLCPP_PUBLIC void detachNode(); @@ -61,7 +69,10 @@ public: private: // Preserve the node reference - rclcpp::node::Node::SharedPtr node_; + 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_; // The subscription for the clock callback using MessageT = builtin_interfaces::msg::Time; diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 6d8163c..82c807e 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -24,6 +24,7 @@ #include "rclcpp/graph_listener.hpp" #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_clock.hpp" #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" @@ -58,6 +59,12 @@ Node::Node( node_topics_.get(), use_intra_process_comms )), + node_clock_(new rclcpp::node_interfaces::NodeClock( + node_base_, + node_topics_, + node_graph_, + node_services_ + )), use_intra_process_comms_(use_intra_process_comms) { } @@ -189,12 +196,32 @@ Node::wait_for_graph_change( node_graph_->wait_for_graph_change(event, timeout); } +rclcpp::Clock::SharedPtr +Node::get_clock() +{ + return node_clock_->get_clock(); +} + +rclcpp::Time +Node::now() +{ + return node_clock_->get_clock()->now(); +} + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Node::get_node_base_interface() { return node_base_; } +rclcpp::node_interfaces::NodeClockInterface::SharedPtr +Node::get_node_clock_interface() +{ + return node_clock_; +} + + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr Node::get_node_graph_interface() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp new file mode 100644 index 0000000..c4d78ca --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -0,0 +1,48 @@ +// Copyright 2017 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_clock.hpp" + +#include +#include + +using rclcpp::node_interfaces::NodeClock; + +NodeClock::NodeClock( + 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) +: node_base_(node_base), + node_topics_(node_topics), + node_graph_(node_graph), + node_services_(node_services), + ros_clock_(std::make_shared(RCL_ROS_TIME)) +{ + time_source_.attachNode( + node_base_, + node_topics_, + node_graph_, + node_services_); + time_source_.attachClock(ros_clock_); +} + +NodeClock::~NodeClock() +{} + +std::shared_ptr +NodeClock::get_clock() +{ + return ros_clock_; +} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 6ea9a0a..38a0bce 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -44,15 +44,53 @@ TimeSource::TimeSource() { } -void TimeSource::attachNode(std::shared_ptr node) +void TimeSource::attachNode(rclcpp::node::Node::SharedPtr node) { - node_ = node; - // TODO(tfoote): Update QOS - clock_subscription_ = node_->create_subscription( - "clock", std::bind(&TimeSource::clock_cb, this, std::placeholders::_1), - rmw_qos_profile_default); + attachNode( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); +} - parameter_client_ = std::make_shared(node); +void TimeSource::attachNode( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface) +{ + node_base_ = node_base_interface; + node_topics_ = node_topics_interface; + node_graph_ = node_graph_interface; + node_services_ = node_services_interface; + // TODO(tfoote): Update QOS + + const std::string & topic_name = "/clock"; + + rclcpp::callback_group::CallbackGroup::SharedPtr group; + using rclcpp::message_memory_strategy::MessageMemoryStrategy; + auto msg_mem_strat = MessageMemoryStrategy::create_default(); + auto allocator = std::make_shared(); + + auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); + + clock_subscription_ = rclcpp::create_subscription( + node_topics_.get(), + topic_name, + std::move(cb), + rmw_qos_profile_default, + group, + false, + false, + msg_mem_strat, + allocator); + + parameter_client_ = std::make_shared( + node_base_, + node_topics_, + node_graph_, + node_services_ + ); parameter_subscription_ = parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); @@ -63,7 +101,10 @@ void TimeSource::detachNode() this->ros_time_active_ = false; clock_subscription_.reset(); parameter_client_.reset(); - node_.reset(); + node_base_.reset(); + node_topics_.reset(); + node_graph_.reset(); + node_services_.reset(); disable_ros_time(); } @@ -94,7 +135,7 @@ void TimeSource::detachClock(std::shared_ptr clock) TimeSource::~TimeSource() { - if (node_) { + if (node_base_ || node_topics_ || node_graph_ || node_services_) { this->detachNode(); } } diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 42d59f8..7230aa5 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -73,3 +73,19 @@ TEST_F(TestNode, get_name_and_namespace) { EXPECT_STREQ("/my/ns", node->get_namespace()); } } + +TEST_F(TestNode, get_clock) { + auto node = std::make_shared("my_node", "/ns"); + auto ros_clock = node->get_clock(); + EXPECT_TRUE(ros_clock != nullptr); + EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME); +} + +TEST_F(TestNode, now) { + auto node = std::make_shared("my_node", "/ns"); + auto clock = node->get_clock(); + auto now_builtin = node->now().nanoseconds(); + auto now_external = clock->now().nanoseconds(); + EXPECT_GE(now_external, now_builtin); + EXPECT_LT(now_external - now_builtin, 50000ul); +}