forked from ros2/rclcpp
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathclock.cpp
More file actions
172 lines (148 loc) · 4.32 KB
/
clock.cpp
File metadata and controls
172 lines (148 loc) · 4.32 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
// 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/clock.hpp"
#include <memory>
#include <thread>
#include "rclcpp/exceptions.hpp"
#include "rcutils/logging_macros.h"
namespace rclcpp
{
class Clock::Impl
{
public:
explicit Impl(rcl_clock_type_t clock_type)
: allocator_{rcl_get_default_allocator()}
{
rcl_ret_t ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
if (ret != RCL_RET_OK) {
exceptions::throw_from_rcl_error(ret, "failed to initialize rcl clock");
}
}
~Impl()
{
rcl_ret_t ret = rcl_clock_fini(&rcl_clock_);
if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
}
}
rcl_clock_t rcl_clock_;
rcl_allocator_t allocator_;
std::mutex clock_mutex_;
};
JumpHandler::JumpHandler(
pre_callback_t pre_callback,
post_callback_t post_callback,
const rcl_jump_threshold_t & threshold)
: pre_callback(pre_callback),
post_callback(post_callback),
notice_threshold(threshold)
{}
Clock::Clock(rcl_clock_type_t clock_type)
: impl_(new Clock::Impl(clock_type)) {}
Clock::~Clock() {}
Time
Clock::now()
{
Time now(0, 0, impl_->rcl_clock_.type);
auto ret = rcl_clock_get_now(&impl_->rcl_clock_, &now.rcl_time_.nanoseconds);
if (ret != RCL_RET_OK) {
exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
}
return now;
}
bool
Clock::ros_time_is_active()
{
if (!rcl_clock_valid(&impl_->rcl_clock_)) {
RCUTILS_LOG_ERROR("ROS time not valid!");
return false;
}
bool is_enabled = false;
auto ret = rcl_is_enabled_ros_time_override(&impl_->rcl_clock_, &is_enabled);
if (ret != RCL_RET_OK) {
exceptions::throw_from_rcl_error(
ret, "Failed to check ros_time_override_status");
}
return is_enabled;
}
rcl_clock_t *
Clock::get_clock_handle() noexcept
{
return &impl_->rcl_clock_;
}
rcl_clock_type_t
Clock::get_clock_type() const noexcept
{
return impl_->rcl_clock_.type;
}
std::mutex &
Clock::get_clock_mutex() noexcept
{
return impl_->clock_mutex_;
}
void
Clock::on_time_jump(
const rcl_time_jump_t * time_jump,
bool before_jump,
void * user_data)
{
const auto * handler = static_cast<JumpHandler *>(user_data);
if (nullptr == handler) {
return;
}
if (before_jump && handler->pre_callback) {
handler->pre_callback();
} else if (!before_jump && handler->post_callback) {
handler->post_callback(*time_jump);
}
}
JumpHandler::SharedPtr
Clock::create_jump_callback(
JumpHandler::pre_callback_t pre_callback,
JumpHandler::post_callback_t post_callback,
const rcl_jump_threshold_t & threshold)
{
// Allocate a new jump handler
JumpHandler::UniquePtr handler(new JumpHandler(pre_callback, post_callback, threshold));
if (nullptr == handler) {
throw std::bad_alloc{};
}
{
std::lock_guard<std::mutex> clock_guard(impl_->clock_mutex_);
// Try to add the jump callback to the clock
rcl_ret_t ret = rcl_clock_add_jump_callback(
&impl_->rcl_clock_, threshold, Clock::on_time_jump,
handler.get());
if (RCL_RET_OK != ret) {
exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
}
}
std::weak_ptr<Clock::Impl> weak_impl = impl_;
// *INDENT-OFF*
// create shared_ptr that removes the callback automatically when all copies are destructed
return JumpHandler::SharedPtr(handler.release(), [weak_impl](JumpHandler * handler) noexcept {
auto shared_impl = weak_impl.lock();
if (shared_impl) {
std::lock_guard<std::mutex> clock_guard(shared_impl->clock_mutex_);
rcl_ret_t ret = rcl_clock_remove_jump_callback(&shared_impl->rcl_clock_,
Clock::on_time_jump, handler);
if (RCL_RET_OK != ret) {
RCUTILS_LOG_ERROR("Failed to remove time jump callback");
}
}
delete handler;
});
// *INDENT-ON*
}
} // namespace rclcpp