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

Fix clock thread issue (#1266) (#1267) (#1685)

Browse source 
Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com>
This commit is contained in:
Tomoya Fujita 2021-06-09 06:07:15 +09:00 • committed by GitHub
parent 1c92622516
commit 1fff1b7cba
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 12 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rclcpp/include/rclcpp/time_source.hpp
View file

@ -148,8 +148,6 @@ private:
void disable_ros_time();
// Internal helper functions used inside iterators
static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
static void set_clock(
const builtin_interfaces::msg::Time::SharedPtr msg,
bool set_ros_time_enabled,

32
rclcpp/src/rclcpp/time_source.cpp
View file

@ -185,11 +185,21 @@ void TimeSource::set_clock(
const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
std::shared_ptr<rclcpp::Clock> clock)
{
std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
// Do change
if (!set_ros_time_enabled && clock->ros_time_is_active()) {
disable_ros_time(clock);
auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to disable ros_time_override_status");
}
} else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
enable_ros_time(clock);
auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to enable ros_time_override_status");
}
}
auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds());
@ -273,24 +283,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
}
}
void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to enable ros_time_override_status");
}
}
void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to enable ros_time_override_status");
}
}
void TimeSource::enable_ros_time()
{
if (ros_time_active_) {