Skip to content
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.

Sign up for GitHub

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

Jump to bottom

Fix clock thread issue (#1266) #1267

Merged
merged 4 commits into from
Aug 13, 2020
Merged

Fix clock thread issue (#1266) #1267

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4a2579f
lock before rcl_set_ros_time_override
daisukes Aug 9, 2020
bf64c5f
additional locks for possible thread problem
daisukes Aug 10, 2020
eb3e599
merge three methods accessing rcl clock
daisukes Aug 10, 2020
f5ad9f3
fix error comment
daisukes Aug 10, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions rclcpp/include/rclcpp/time_source.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,8 +148,6 @@ class TimeSource
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,
Expand Down
32 changes: 12 additions & 20 deletions rclcpp/src/rclcpp/time_source.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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_) {
Expand Down