forked from ros2/rclcpp
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathclock.hpp
More file actions
151 lines (127 loc) · 3.95 KB
/
clock.hpp
File metadata and controls
151 lines (127 loc) · 3.95 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
// 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__CLOCK_HPP_
#define RCLCPP__CLOCK_HPP_
#include <functional>
#include <memory>
#include <mutex>
#include "rclcpp/macros.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rcl/time.h"
#include "rcutils/time.h"
#include "rcutils/types/rcutils_ret.h"
namespace rclcpp
{
class TimeSource;
class JumpHandler
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
using pre_callback_t = std::function<void ()>;
using post_callback_t = std::function<void (const rcl_time_jump_t &)>;
JumpHandler(
pre_callback_t pre_callback,
post_callback_t post_callback,
const rcl_jump_threshold_t & threshold);
pre_callback_t pre_callback;
post_callback_t post_callback;
rcl_jump_threshold_t notice_threshold;
};
class Clock
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(Clock)
/// Default c'tor
/**
* Initializes the clock instance with the given clock_type.
*
* \param clock_type type of the clock.
* \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
*/
RCLCPP_PUBLIC
explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
RCLCPP_PUBLIC
~Clock();
/**
* Returns current time from the time source specified by clock_type.
*
* \return current time.
* \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
*/
RCLCPP_PUBLIC
Time
now();
/**
* Returns the clock of the type `RCL_ROS_TIME` is active.
*
* \return true if the clock is active
* \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if
* the current clock does not have the clock_type `RCL_ROS_TIME`.
*/
RCLCPP_PUBLIC
bool
ros_time_is_active();
/// Return the rcl_clock_t clock handle
RCLCPP_PUBLIC
rcl_clock_t *
get_clock_handle() noexcept;
RCLCPP_PUBLIC
rcl_clock_type_t
get_clock_type() const noexcept;
/// Get the clock's mutex
RCLCPP_PUBLIC
std::mutex &
get_clock_mutex() noexcept;
// Add a callback to invoke if the jump threshold is exceeded.
/**
* These callback functions must remain valid as long as the
* returned shared pointer is valid.
*
* Function will register callbacks to the callback queue. On time jump all
* callbacks will be executed whose threshold is greater then the time jump;
* The logic will first call selected pre_callbacks and then all selected
* post_callbacks.
*
* Function is only applicable if the clock_type is `RCL_ROS_TIME`
*
* \param pre_callback Must be non-throwing
* \param post_callback Must be non-throwing.
* \param threshold Callbacks will be triggered if the time jump is greater
* then the threshold.
* \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
* \throws std::bad_alloc if the allocation of the JumpHandler fails.
* \warning the instance of the clock must remain valid as long as any created
* JumpHandler.
*/
RCLCPP_PUBLIC
JumpHandler::SharedPtr
create_jump_callback(
JumpHandler::pre_callback_t pre_callback,
JumpHandler::post_callback_t post_callback,
const rcl_jump_threshold_t & threshold);
private:
// Invoke time jump callback
RCLCPP_PUBLIC
static void
on_time_jump(
const rcl_time_jump_t * time_jump,
bool before_jump,
void * user_data);
/// Private internal storage
class Impl;
std::shared_ptr<Impl> impl_;
};
} // namespace rclcpp
#endif // RCLCPP__CLOCK_HPP_