-
Notifications
You must be signed in to change notification settings - Fork 240
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add callbacks for PlayerClock::jump(time_point) API with CI fix #779
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -16,6 +16,7 @@ | |
#include <memory> | ||
#include <mutex> | ||
#include <thread> | ||
#include <vector> | ||
|
||
#include "rcpputils/thread_safety_annotations.hpp" | ||
#include "rosbag2_cpp/clocks/time_controller_clock.hpp" | ||
|
@@ -114,6 +115,72 @@ class TimeControllerClockImpl | |
snapshot(ros_now()); | ||
} | ||
|
||
/** | ||
* \brief Adjust internal clock to the specified timestamp. | ||
* \details It will change the current internally maintained offset so that next published time | ||
* is different. | ||
* \note Will trigger any registered JumpHandler callbacks. | ||
* \param ros_time Time point in ROS playback timeline. | ||
*/ | ||
void jump(rcutils_time_point_value_t ros_time) | ||
{ | ||
rcl_duration_t time_jump_delta; | ||
{ | ||
std::lock_guard<std::mutex> lock(state_mutex); | ||
time_jump_delta.nanoseconds = ros_time - steady_to_ros(now_fn()); | ||
} | ||
|
||
rcl_time_jump_t time_jump{}; | ||
time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE; | ||
time_jump.delta = time_jump_delta; | ||
|
||
process_callbacks_before_jump(time_jump); | ||
{ | ||
std::lock_guard<std::mutex> lock(state_mutex); | ||
snapshot(ros_time); | ||
} | ||
process_callbacks_after_jump(time_jump); | ||
cv.notify_all(); | ||
} | ||
|
||
/** | ||
* \brief Add callbacks to be called when a time jump exceeds a threshold. | ||
* \param handler Shared pointer to the JumpHandler object returned from JumpHandler::create(..) | ||
* \throws std::invalid argument if jump threshold has invalid value. | ||
*/ | ||
void add_jump_callbacks(PlayerClock::JumpHandler::SharedPtr handler) | ||
{ | ||
if (handler->notice_threshold.min_forward.nanoseconds < 0) { | ||
throw std::invalid_argument("forward jump threshold must be positive or zero"); | ||
} | ||
if (handler->notice_threshold.min_backward.nanoseconds > 0) { | ||
throw std::invalid_argument("backward jump threshold must be negative or zero"); | ||
} | ||
|
||
std::lock_guard<std::mutex> lock(callback_list_mutex_); | ||
for (auto const & registered_handler : callback_list_) { | ||
if (*registered_handler == *handler) { | ||
return; // Already have this callback in the list. | ||
} | ||
} | ||
callback_list_.push_back(handler); | ||
} | ||
|
||
/** | ||
* \brief remove jump callbacks from processing list. | ||
* \param handler Shared pointer to the JumpHandler object returned from JumpHandler::create(..) | ||
*/ | ||
void remove_jump_callbacks(PlayerClock::JumpHandler::SharedPtr handler) | ||
{ | ||
std::lock_guard<std::mutex> lock(callback_list_mutex_); | ||
for (auto it = callback_list_.begin(); it != callback_list_.end(); ++it) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The function name and comment make it sound like this will remove multiple jump callbacks from the processing list. However, the code only removes one. Should the comment/function name change to make it more clear that we're removing one callback? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. A |
||
if (**it == *handler) { | ||
callback_list_.erase(it); | ||
return; | ||
} | ||
} | ||
} | ||
|
||
const PlayerClock::NowFunction now_fn; | ||
const std::chrono::milliseconds sleep_time_while_paused; | ||
|
||
|
@@ -122,6 +189,49 @@ class TimeControllerClockImpl | |
double rate RCPPUTILS_TSA_GUARDED_BY(state_mutex) = 1.0; | ||
bool paused RCPPUTILS_TSA_GUARDED_BY(state_mutex) = false; | ||
TimeReference reference RCPPUTILS_TSA_GUARDED_BY(state_mutex); | ||
|
||
private: | ||
std::mutex callback_list_mutex_; | ||
std::vector<PlayerClock::JumpHandler::SharedPtr> callback_list_ | ||
RCPPUTILS_TSA_GUARDED_BY(callback_list_mutex_); | ||
|
||
void process_callbacks_before_jump(const rcl_time_jump_t & time_jump) | ||
{ | ||
std::lock_guard<std::mutex> lock(callback_list_mutex_); | ||
for (auto const & handler : callback_list_) { | ||
process_callback(handler, time_jump, true); | ||
} | ||
} | ||
|
||
void process_callbacks_after_jump(const rcl_time_jump_t & time_jump) | ||
{ | ||
std::lock_guard<std::mutex> lock(callback_list_mutex_); | ||
for (auto const & handler : callback_list_) { | ||
process_callback(handler, time_jump, false); | ||
} | ||
} | ||
|
||
static void process_callback( | ||
PlayerClock::JumpHandler::SharedPtr handler, const rcl_time_jump_t & time_jump, | ||
bool before_jump) RCPPUTILS_TSA_REQUIRES(callback_list_mutex_) | ||
{ | ||
bool is_clock_change = time_jump.clock_change == RCL_ROS_TIME_ACTIVATED || | ||
time_jump.clock_change == RCL_ROS_TIME_DEACTIVATED; | ||
if ((is_clock_change && handler->notice_threshold.on_clock_change) || | ||
(handler->notice_threshold.min_backward.nanoseconds != 0 && | ||
time_jump.delta.nanoseconds < 0 && | ||
time_jump.delta.nanoseconds <= handler->notice_threshold.min_backward.nanoseconds) || | ||
(handler->notice_threshold.min_forward.nanoseconds != 0 && | ||
time_jump.delta.nanoseconds > 0 && | ||
time_jump.delta.nanoseconds >= handler->notice_threshold.min_forward.nanoseconds)) | ||
{ | ||
if (before_jump && handler->pre_callback) { | ||
handler->pre_callback(); | ||
} else if (!before_jump && handler->post_callback) { | ||
handler->post_callback(time_jump); | ||
} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. There is no |
||
} | ||
} | ||
}; | ||
|
||
TimeControllerClock::TimeControllerClock( | ||
|
@@ -186,6 +296,16 @@ bool TimeControllerClock::set_rate(double rate) | |
return true; | ||
} | ||
|
||
void TimeControllerClock::add_jump_calbacks(PlayerClock::JumpHandler::SharedPtr handler) | ||
{ | ||
impl_->add_jump_callbacks(handler); | ||
} | ||
|
||
void TimeControllerClock::remove_jump_callbacks(PlayerClock::JumpHandler::SharedPtr handler) | ||
{ | ||
impl_->remove_jump_callbacks(handler); | ||
} | ||
|
||
double TimeControllerClock::get_rate() const | ||
{ | ||
std::lock_guard<std::mutex> lock(impl_->state_mutex); | ||
|
@@ -198,7 +318,7 @@ void TimeControllerClock::pause() | |
if (impl_->paused) { | ||
return; | ||
} | ||
// Take snaphot before changing state | ||
// Take snapshot before changing state | ||
impl_->snapshot(); | ||
impl_->paused = true; | ||
impl_->cv.notify_all(); | ||
|
@@ -210,7 +330,7 @@ void TimeControllerClock::resume() | |
if (!impl_->paused) { | ||
return; | ||
} | ||
// Take snaphot before changing state | ||
// Take snapshot before changing state | ||
impl_->snapshot(); | ||
impl_->paused = false; | ||
impl_->cv.notify_all(); | ||
|
@@ -224,9 +344,7 @@ bool TimeControllerClock::is_paused() const | |
|
||
void TimeControllerClock::jump(rcutils_time_point_value_t ros_time) | ||
{ | ||
std::lock_guard<std::mutex> lock(impl_->state_mutex); | ||
impl_->snapshot(ros_time); | ||
impl_->cv.notify_all(); | ||
impl_->jump(ros_time); | ||
} | ||
|
||
void TimeControllerClock::jump(rclcpp::Time ros_time) | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Since ROS 2 supports C++ 17, we now have access to the
std::scoped_lock
class. Should we use that here?There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We haven't yet changed rosbag2 to target C++17 - this is an upgrade that could be done in a new PR if we're inclined