niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Use a PIMPL for rclcpp::Clock implementation

Browse source 
Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
This commit is contained in:
Michel Hidalgo 2020-02-21 10:53:44 -03:00
parent 09bde58ba7
commit b100b39353
2 changed files with 43 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

7
rclcpp/include/rclcpp/clock.hpp
View file

@ -133,9 +133,10 @@ private:
bool before_jump,
void * user_data);
/// Internal storage backed by rcl
std::shared_ptr<rcl_clock_t> rcl_clock_;
rcl_allocator_t allocator_;
/// Private internal storage
class Impl;
std::shared_ptr<Impl> impl_;
};
} // namespace rclcpp

70
rclcpp/src/rclcpp/clock.cpp
View file

@ -23,6 +23,30 @@
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, "could not get current time stamp");
}
}
~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_;
};
JumpHandler::JumpHandler(
pre_callback_t pre_callback,
post_callback_t post_callback,
@ -32,35 +56,17 @@ JumpHandler::JumpHandler(
notice_threshold(threshold)
{}
static
void
rcl_clock_deleter(rcl_clock_t * rcl_clock)
{
auto ret = rcl_clock_fini(rcl_clock);
if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
}
delete rcl_clock;
}
Clock::Clock(rcl_clock_type_t clock_type)
{
allocator_ = rcl_get_default_allocator();
rcl_clock_ = std::shared_ptr<rcl_clock_t>(new rcl_clock_t(), rcl_clock_deleter);
auto ret = rcl_clock_init(clock_type, rcl_clock_.get(), &allocator_);
if (ret != RCL_RET_OK) {
exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
}
}
: impl_(new Clock::Impl(clock_type)) {}
Clock::~Clock() {}
Time
Clock::now()
{
Time now(0, 0, rcl_clock_->type);
Time now(0, 0, impl_->rcl_clock_.type);
auto ret = rcl_clock_get_now(rcl_clock_.get(), &now.rcl_time_.nanoseconds);
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");
}
@ -71,13 +77,13 @@ Clock::now()
bool
Clock::ros_time_is_active()
{
if (!rcl_clock_valid(rcl_clock_.get())) {
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(rcl_clock_.get(), &is_enabled);
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");
@ -88,13 +94,13 @@ Clock::ros_time_is_active()
rcl_clock_t *
Clock::get_clock_handle() noexcept
{
return rcl_clock_.get();
return &impl_->rcl_clock_;
}
rcl_clock_type_t
Clock::get_clock_type() const noexcept
{
return rcl_clock_->type;
return impl_->rcl_clock_.type;
}
void
@ -128,20 +134,20 @@ Clock::create_jump_callback(
// Try to add the jump callback to the clock
rcl_ret_t ret = rcl_clock_add_jump_callback(
rcl_clock_.get(), threshold, Clock::on_time_jump,
&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<rcl_clock_t> weak_clock = rcl_clock_;
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_clock](JumpHandler * handler) noexcept {
auto shared_clock = weak_clock.lock();
if (shared_clock) {
rcl_ret_t ret = rcl_clock_remove_jump_callback(shared_clock.get(), Clock::on_time_jump,
handler);
return JumpHandler::SharedPtr(handler.release(), [weak_impl](JumpHandler * handler) noexcept {
auto shared_impl = weak_impl.lock();
if (shared_impl) {
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");
}