Fixup clock (#696)
* Fix uninitialized bool in clock.cpp Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Fixup includes of clock.hpp/cpp Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Add documentation for exceptions to clock.hpp Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Adjust function signature of getters of clock.hpp/cpp Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Remove raw pointers Clock::create_jump_callback Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Remove unnecessary rclcpp namespace reference from clock.cpp Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Change exception to bad_alloc on JumpHandler allocation failure Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Fix missing nullptr check in Clock::on_time_jump Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Add JumpHandler::callback types Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Add warning for lifetime of Clock and JumpHandler Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Incorporate review Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de> * Incorporate review Signed-off-by: Dmitrij Dorezyuk <dmitrij.dorezyuk@hotmail.de>
This commit is contained in:
Dima Dorezyuk
Karsten Knese
parent
dc05a2e755
commit
078d5ff662
2 changed files with 70 additions and 46 deletions
|
|
@ -16,9 +16,6 @@
|
|||
#define RCLCPP__CLOCK_HPP_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/time.hpp"
|
||||
|
|
@ -35,13 +32,17 @@ 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(
|
||||
std::function<void()> pre_callback,
|
||||
std::function<void(const rcl_time_jump_t &)> post_callback,
|
||||
pre_callback_t pre_callback,
|
||||
post_callback_t post_callback,
|
||||
const rcl_jump_threshold_t & threshold);
|
||||
|
||||
std::function<void()> pre_callback;
|
||||
std::function<void(const rcl_time_jump_t &)> post_callback;
|
||||
pre_callback_t pre_callback;
|
||||
post_callback_t post_callback;
|
||||
rcl_jump_threshold_t notice_threshold;
|
||||
};
|
||||
|
||||
|
|
@ -50,38 +51,74 @@ 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();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rcl_clock_t *
|
||||
get_clock_handle();
|
||||
get_clock_handle() noexcept;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rcl_clock_type_t
|
||||
get_clock_type();
|
||||
get_clock_type() const 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(
|
||||
std::function<void()> pre_callback,
|
||||
std::function<void(const rcl_time_jump_t &)> post_callback,
|
||||
JumpHandler::pre_callback_t pre_callback,
|
||||
JumpHandler::post_callback_t post_callback,
|
||||
const rcl_jump_threshold_t & threshold);
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -14,14 +14,6 @@
|
|||
|
||||
#include "rclcpp/clock.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
|
||||
#include "rcl/time.h"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
|
@ -30,8 +22,8 @@ namespace rclcpp
|
|||
{
|
||||
|
||||
JumpHandler::JumpHandler(
|
||||
std::function<void()> pre_callback,
|
||||
std::function<void(const rcl_time_jump_t &)> post_callback,
|
||||
pre_callback_t pre_callback,
|
||||
post_callback_t post_callback,
|
||||
const rcl_jump_threshold_t & threshold)
|
||||
: pre_callback(pre_callback),
|
||||
post_callback(post_callback),
|
||||
|
|
@ -43,8 +35,7 @@ Clock::Clock(rcl_clock_type_t clock_type)
|
|||
allocator_ = rcl_get_default_allocator();
|
||||
auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not get current time stamp");
|
||||
exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -63,8 +54,7 @@ Clock::now()
|
|||
|
||||
auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds);
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not get current time stamp");
|
||||
exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
|
||||
}
|
||||
|
||||
return now;
|
||||
|
|
@ -78,23 +68,23 @@ Clock::ros_time_is_active()
|
|||
return false;
|
||||
}
|
||||
|
||||
bool is_enabled;
|
||||
bool is_enabled = false;
|
||||
auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
exceptions::throw_from_rcl_error(
|
||||
ret, "Failed to check ros_time_override_status");
|
||||
}
|
||||
return is_enabled;
|
||||
}
|
||||
|
||||
rcl_clock_t *
|
||||
Clock::get_clock_handle()
|
||||
Clock::get_clock_handle() noexcept
|
||||
{
|
||||
return &rcl_clock_;
|
||||
}
|
||||
|
||||
rcl_clock_type_t
|
||||
Clock::get_clock_type()
|
||||
Clock::get_clock_type() const noexcept
|
||||
{
|
||||
return rcl_clock_.type;
|
||||
}
|
||||
|
|
@ -105,7 +95,10 @@ Clock::on_time_jump(
|
|||
bool before_jump,
|
||||
void * user_data)
|
||||
{
|
||||
rclcpp::JumpHandler * handler = static_cast<rclcpp::JumpHandler *>(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) {
|
||||
|
|
@ -113,36 +106,30 @@ Clock::on_time_jump(
|
|||
}
|
||||
}
|
||||
|
||||
rclcpp::JumpHandler::SharedPtr
|
||||
JumpHandler::SharedPtr
|
||||
Clock::create_jump_callback(
|
||||
std::function<void()> pre_callback,
|
||||
std::function<void(const rcl_time_jump_t &)> post_callback,
|
||||
JumpHandler::pre_callback_t pre_callback,
|
||||
JumpHandler::post_callback_t post_callback,
|
||||
const rcl_jump_threshold_t & threshold)
|
||||
{
|
||||
// Allocate a new jump handler
|
||||
auto handler = new rclcpp::JumpHandler(pre_callback, post_callback, threshold);
|
||||
JumpHandler::UniquePtr handler(new JumpHandler(pre_callback, post_callback, threshold));
|
||||
if (nullptr == handler) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, "Failed to allocate jump handler");
|
||||
throw std::bad_alloc{};
|
||||
}
|
||||
|
||||
// Try to add the jump callback to the clock
|
||||
rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold,
|
||||
rclcpp::Clock::on_time_jump, handler);
|
||||
Clock::on_time_jump, handler.get());
|
||||
if (RCL_RET_OK != ret) {
|
||||
delete handler;
|
||||
handler = nullptr;
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
|
||||
}
|
||||
|
||||
if (nullptr == handler) {
|
||||
// imposible to reach here; added to make cppcheck happy
|
||||
return nullptr;
|
||||
exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
|
||||
}
|
||||
|
||||
// *INDENT-OFF*
|
||||
// create shared_ptr that removes the callback automatically when all copies are destructed
|
||||
return rclcpp::JumpHandler::SharedPtr(handler, [this](rclcpp::JumpHandler * handler) noexcept {
|
||||
rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, rclcpp::Clock::on_time_jump,
|
||||
// TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler
|
||||
return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept {
|
||||
rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump,
|
||||
handler);
|
||||
delete handler;
|
||||
handler = NULL;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue