rclcpp  master
C++ ROS Client Library API
time_source.hpp
Go to the documentation of this file.
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef RCLCPP__TIME_SOURCE_HPP_
16 #define RCLCPP__TIME_SOURCE_HPP_
17 
18 #include <memory>
19 #include <vector>
20 
21 #include "rcl/time.h"
22 
23 #include "builtin_interfaces/msg/time.hpp"
24 #include "rosgraph_msgs/msg/clock.hpp"
25 #include "rcl_interfaces/msg/parameter_event.hpp"
26 
27 #include "rclcpp/node.hpp"
30 
31 
32 namespace rclcpp
33 {
34 class Clock;
35 
37 {
38 public:
40  explicit TimeSource(rclcpp::Node::SharedPtr node);
41 
43  TimeSource();
44 
46  void attachNode(rclcpp::Node::SharedPtr node);
47 
49  void attachNode(
50  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
51  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
52  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
53  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface);
54 
56  void detachNode();
57 
59 
63  void attachClock(rclcpp::Clock::SharedPtr clock);
64 
66  void detachClock(rclcpp::Clock::SharedPtr clock);
67 
69  ~TimeSource();
70 
71 private:
72  // Preserve the node reference
73  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
74  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
75  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
76  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
77 
78  // The subscription for the clock callback
79  using MessageT = rosgraph_msgs::msg::Clock;
82  std::shared_ptr<SubscriptionT> clock_subscription_;
83 
84  // The clock callback itself
85  void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
86 
87  // Parameter Client pointer
89 
90  // Parameter Event subscription
91  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
93  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
94 
95  // Callback for parameter updates
96  void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
97 
98  // An enum to hold the parameter state
99  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
100  UseSimTimeParameterState parameter_state_;
101 
102  // An internal method to use in the clock callback that iterates and enables all clocks
103  void enable_ros_time();
104  // An internal method to use in the clock callback that iterates and disables all clocks
105  void disable_ros_time();
106 
107  // Internal helper functions used inside iterators
108  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
109  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
110  static void set_clock(
111  const builtin_interfaces::msg::Time::SharedPtr msg,
112  bool set_ros_time_enabled,
113  rclcpp::Clock::SharedPtr clock);
114 
115  // Local storage of validity of ROS time
116  // This is needed when new clocks are added.
117  bool ros_time_active_;
118  // Last set message to be passed to newly registered clocks
119  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
120 
121  // A lock to protect iterating the associated_clocks_ field.
122  std::mutex clock_list_lock_;
123  // A vector to store references to associated clocks.
124  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
125 };
126 
127 } // namespace rclcpp
128 
129 #endif // RCLCPP__TIME_SOURCE_HPP_
Definition: time_source.hpp:36
Subscription implementation, templated on the type of message this subscription receives.
Definition: subscription.hpp:147
Definition: allocator_common.hpp:24
void attachNode(rclcpp::Node::SharedPtr node)
void detachClock(rclcpp::Clock::SharedPtr clock)
#define RCLCPP_PUBLIC
Definition: visibility_control.hpp:50
void attachClock(rclcpp::Clock::SharedPtr clock)
Attach a clock to the time source to be updated.