From 078d5ff662e82e095d153437b0c7966d3b5623e8 Mon Sep 17 00:00:00 2001 From: Dima Dorezyuk Date: Wed, 1 May 2019 02:22:26 +0200 Subject: [PATCH] Fixup clock (#696) * Fix uninitialized bool in clock.cpp Signed-off-by: Dmitrij Dorezyuk * Fixup includes of clock.hpp/cpp Signed-off-by: Dmitrij Dorezyuk * Add documentation for exceptions to clock.hpp Signed-off-by: Dmitrij Dorezyuk * Adjust function signature of getters of clock.hpp/cpp Signed-off-by: Dmitrij Dorezyuk * Remove raw pointers Clock::create_jump_callback Signed-off-by: Dmitrij Dorezyuk * Remove unnecessary rclcpp namespace reference from clock.cpp Signed-off-by: Dmitrij Dorezyuk * Change exception to bad_alloc on JumpHandler allocation failure Signed-off-by: Dmitrij Dorezyuk * Fix missing nullptr check in Clock::on_time_jump Signed-off-by: Dmitrij Dorezyuk * Add JumpHandler::callback types Signed-off-by: Dmitrij Dorezyuk * Add warning for lifetime of Clock and JumpHandler Signed-off-by: Dmitrij Dorezyuk * Incorporate review Signed-off-by: Dmitrij Dorezyuk * Incorporate review Signed-off-by: Dmitrij Dorezyuk --- rclcpp/include/rclcpp/clock.hpp | 59 +++++++++++++++++++++++++++------ rclcpp/src/rclcpp/clock.cpp | 57 ++++++++++++------------------- 2 files changed, 70 insertions(+), 46 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 2bcd257..b9bbe2d 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -16,9 +16,6 @@ #define RCLCPP__CLOCK_HPP_ #include -#include -#include -#include #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; + using post_callback_t = std::function; + JumpHandler( - std::function pre_callback, - std::function post_callback, + pre_callback_t pre_callback, + post_callback_t post_callback, const rcl_jump_threshold_t & threshold); - std::function pre_callback; - std::function 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 pre_callback, - std::function post_callback, + JumpHandler::pre_callback_t pre_callback, + JumpHandler::post_callback_t post_callback, const rcl_jump_threshold_t & threshold); private: diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 0aaa0c8..0117732 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,14 +14,6 @@ #include "rclcpp/clock.hpp" -#include -#include -#include - -#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 pre_callback, - std::function 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(user_data); + const auto * handler = static_cast(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 pre_callback, - std::function 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;