forked from ros2/rclcpp
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtime_source.hpp
More file actions
206 lines (174 loc) · 7.17 KB
/
time_source.hpp
File metadata and controls
206 lines (174 loc) · 7.17 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
// 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__TIME_SOURCE_HPP_
#define RCLCPP__TIME_SOURCE_HPP_
#include <memory>
#include <vector>
#include "rcl/time.h"
#include "builtin_interfaces/msg/time.hpp"
#include "rosgraph_msgs/msg/clock.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/executors.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
namespace rclcpp
{
class Clock;
/**
* Time source that will drive the attached clocks.
*
* If the attached node `use_sim_time` parameter is `true`, the attached clocks will
* be updated based on messages received.
*
* The subscription to the clock topic created by the time source can have it's qos reconfigured
* using parameter overrides, particularly the following ones are accepted:
*
* - qos_overrides./clock.depth
* - qos_overrides./clock.durability
* - qos_overrides./clock.history
* - qos_overrides./clock.reliability
*/
class TimeSource
{
public:
/// Constructor
/**
* The node will be attached to the time source.
*
* \param node std::shared pointer to a initialized node
* \param qos QoS that will be used when creating a `/clock` subscription.
*/
RCLCPP_PUBLIC
explicit TimeSource(
rclcpp::Node::SharedPtr node,
const rclcpp::QoS & qos = rclcpp::ClockQoS(),
bool use_clock_thread = true);
/// Empty constructor
/**
* An Empty TimeSource class
*
* \param qos QoS that will be used when creating a `/clock` subscription.
*/
RCLCPP_PUBLIC
explicit TimeSource(
const rclcpp::QoS & qos = rclcpp::ClockQoS(),
bool use_clock_thread = true);
/// Attach node to the time source.
/**
* \param node std::shared pointer to a initialized node
*/
RCLCPP_PUBLIC
void attachNode(rclcpp::Node::SharedPtr node);
/// Attach node to the time source.
/**
* If the parameter `use_sim_time` is `true` then the source time is the simulation time,
* otherwise the source time is defined by the system.
*
* \param node_base_interface Node base interface.
* \param node_topics_interface Node topic base interface.
* \param node_graph_interface Node graph interface.
* \param node_services_interface Node service interface.
* \param node_logging_interface Node logging interface.
* \param node_clock_interface Node clock interface.
* \param node_parameters_interface Node parameters interface.
*/
RCLCPP_PUBLIC
void attachNode(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
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::NodeClockInterface::SharedPtr node_clock_interface,
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface);
/// Detach the node from the time source
RCLCPP_PUBLIC
void detachNode();
/// Attach a clock to the time source to be updated
/**
* \param[in] clock to attach to the time source
* \throws std::invalid_argument the time source must be a RCL_ROS_TIME otherwise throws an exception
*/
RCLCPP_PUBLIC
void attachClock(rclcpp::Clock::SharedPtr clock);
/// Detach a clock to the time source
RCLCPP_PUBLIC
void detachClock(rclcpp::Clock::SharedPtr clock);
/// TimeSource Destructor
RCLCPP_PUBLIC
~TimeSource();
protected:
// Dedicated thread for clock subscription.
bool use_clock_thread_;
std::thread clock_executor_thread_;
private:
// Preserve the node reference
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_;
// Store (and update on node attach) logger for logging.
Logger logger_;
// QoS of the clock subscription.
rclcpp::QoS qos_;
// The subscription for the clock callback
using MessageT = rosgraph_msgs::msg::Clock;
using Alloc = std::allocator<void>;
using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
std::mutex clock_sub_lock_;
rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_;
std::promise<void> cancel_clock_executor_promise_;
// The clock callback itself
void clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg);
// Create the subscription for the clock topic
void create_clock_sub();
// Destroy the subscription for the clock topic
void destroy_clock_sub();
// Parameter Event subscription
using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
// Callback for parameter updates
void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event);
// An enum to hold the parameter state
enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
UseSimTimeParameterState parameter_state_;
// An internal method to use in the clock callback that iterates and enables all clocks
void enable_ros_time();
// An internal method to use in the clock callback that iterates and disables all clocks
void disable_ros_time();
// Internal helper functions used inside iterators
static void set_clock(
const builtin_interfaces::msg::Time::SharedPtr msg,
bool set_ros_time_enabled,
rclcpp::Clock::SharedPtr clock);
// Local storage of validity of ROS time
// This is needed when new clocks are added.
bool ros_time_active_{false};
// Last set message to be passed to newly registered clocks
std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
// A lock to protect iterating the associated_clocks_ field.
std::mutex clock_list_lock_;
// A vector to store references to associated clocks.
std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
// A handler for the use_sim_time parameter callback.
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
};
} // namespace rclcpp
#endif // RCLCPP__TIME_SOURCE_HPP_