Skip to content

Commit

Permalink
Revert "Add Clock::sleep_until method (ros2#1748)" (ros2#1793)
Browse files Browse the repository at this point in the history 
This reverts commit 81df584.

Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>
  • Loading branch information
@Blast545
Blast545 committed Oct 5, 2021
1 parent d5ec258 commit d04319a
Show file tree
Hide file tree
Showing 4 changed files with 2 additions and 300 deletions.
19 changes: 0 additions & 19 deletions rclcpp/include/rclcpp/clock.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,25 +78,6 @@ class Clock
Time
now();

/**
* Sleep until a specified Time, according to clock type.
*
* Notes for RCL_ROS_TIME clock type:
* - Can sleep forever if ros time is active and received clock never reaches `until`
* - If ROS time enabled state changes during the sleep, this method will immediately return
* false. There is not a consistent choice of sleeping time when the time source changes,
* so this is up to the caller to call again if needed.
*
* \param until absolute time according to current clock type to sleep until.
* \return true immediately if `until` is in the past
* \return true when the time `until` is reached
* \return false if time cannot be reached reliably, for example from shutdown or a change
* of time source.
*/
RCLCPP_PUBLIC
bool
sleep_until(Time until);

/**
* Returns the clock of the type `RCL_ROS_TIME` is active.
*
Expand Down
97 changes: 2 additions & 95 deletions rclcpp/src/rclcpp/clock.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,10 @@

#include "rclcpp/clock.hpp"

#include <condition_variable>
#include <memory>
#include <thread>

#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/utilities.hpp"

#include "rcutils/logging_macros.h"

Expand Down Expand Up @@ -50,8 +47,6 @@ class Clock::Impl
rcl_clock_t rcl_clock_;
rcl_allocator_t allocator_;
std::mutex clock_mutex_;
std::condition_variable cv_;
rclcpp::OnShutdownCallbackHandle shutdown_cb_;
};

JumpHandler::JumpHandler(
Expand All @@ -64,18 +59,9 @@ JumpHandler::JumpHandler(
{}

Clock::Clock(rcl_clock_type_t clock_type)
: impl_(new Clock::Impl(clock_type))
{
impl_->shutdown_cb_ = rclcpp::contexts::get_global_default_context()->add_on_shutdown_callback(
[this]() {
impl_->cv_.notify_all();
});
}
: impl_(new Clock::Impl(clock_type)) {}

Clock::~Clock()
{
rclcpp::contexts::get_global_default_context()->remove_on_shutdown_callback(impl_->shutdown_cb_);
}
Clock::~Clock() {}

Time
Clock::now()
Expand All @@ -90,85 +76,6 @@ Clock::now()
return now;
}

bool
Clock::sleep_until(Time until)
{
const auto this_clock_type = get_clock_type();
if (until.get_clock_type() != this_clock_type) {
RCUTILS_LOG_ERROR(
"sleep_until Time clock type %d does not match this clock's type %d.",
until.get_clock_type(), this_clock_type);
return false;
}
bool time_source_changed = false;

if (this_clock_type == RCL_STEADY_TIME) {
auto steady_time = std::chrono::steady_clock::time_point(
std::chrono::nanoseconds(until.nanoseconds()));

// loop over spurious wakeups but notice shutdown
std::unique_lock lock(impl_->clock_mutex_);
while (now() < until && rclcpp::ok()) {
impl_->cv_.wait_until(lock, steady_time);
}
} else if (this_clock_type == RCL_SYSTEM_TIME) {
auto system_time = std::chrono::time_point<
std::chrono::system_clock, std::chrono::nanoseconds>(
std::chrono::nanoseconds(until.nanoseconds()));

// loop over spurious wakeups but notice shutdown
std::unique_lock lock(impl_->clock_mutex_);
while (now() < until && rclcpp::ok()) {
impl_->cv_.wait_until(lock, system_time);
}
} else if (this_clock_type == RCL_ROS_TIME) {
// Install jump handler for any amount of time change, for two purposes:
// - if ROS time is active, check if time reached on each new clock sample
// - Trigger via on_clock_change to detect if time source changes, to invalidate sleep
rcl_jump_threshold_t threshold;
threshold.on_clock_change = true;
threshold.min_backward.nanoseconds = 0;
threshold.min_forward.nanoseconds = 0;
auto clock_handler = create_jump_callback(
[]() {},
[this](const rcl_time_jump_t &) {impl_->cv_.notify_all();},
threshold);

try {
if (!ros_time_is_active()) {
auto system_time = std::chrono::time_point<
std::chrono::system_clock, std::chrono::nanoseconds>(
std::chrono::nanoseconds(until.nanoseconds()));

// loop over spurious wakeups but notice shutdown or time source change
std::unique_lock lock(impl_->clock_mutex_);
while (now() < until && rclcpp::ok() && !ros_time_is_active()) {
impl_->cv_.wait_until(lock, system_time);
}
time_source_changed = ros_time_is_active();
} else {
// RCL_ROS_TIME with ros_time_is_active.
// Just wait without "until" because installed
// jump callbacks wake the cv on every new sample.
std::unique_lock lock(impl_->clock_mutex_);
while (now() < until && rclcpp::ok() && ros_time_is_active()) {
impl_->cv_.wait(lock);
}
time_source_changed = !ros_time_is_active();
}
} catch (...) {
RCUTILS_LOG_ERROR("Unexpected exception from ros_time_is_active()");
return false;
}
}

if (!rclcpp::ok() || time_source_changed) {
return false;
}

return now() >= until;
}

bool
Clock::ros_time_is_active()
{
Expand Down
153 changes: 0 additions & 153 deletions rclcpp/test/rclcpp/test_time.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,15 +17,13 @@
#include <algorithm>
#include <chrono>
#include <limits>
#include <memory>
#include <string>

#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/time_source.hpp"
#include "rclcpp/utilities.hpp"

#include "../utils/rclcpp_gtest_macros.hpp"
Expand Down Expand Up @@ -449,154 +447,3 @@ TEST_F(TestTime, test_overflow_underflow_throws) {
test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1),
std::underflow_error("addition leads to int64_t underflow"));
}

class TestClockSleep : public ::testing::Test
{
protected:
void SetUp()
{
// Shutdown in case there was a dangling global context from other test fixtures
rclcpp::shutdown();
rclcpp::init(0, nullptr);
node = std::make_shared<rclcpp::Node>("clock_sleep_node");
param_client = std::make_shared<rclcpp::SyncParametersClient>(node);
ASSERT_TRUE(param_client->wait_for_service(5s));
}

void TearDown()
{
node.reset();
rclcpp::shutdown();
}

rclcpp::Node::SharedPtr node;
rclcpp::SyncParametersClient::SharedPtr param_client;
};

TEST_F(TestClockSleep, bad_clock_type) {
rclcpp::Clock clock(RCL_SYSTEM_TIME);
rclcpp::Time steady_until(12345, 0, RCL_STEADY_TIME);
ASSERT_FALSE(clock.sleep_until(steady_until));

rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME);
ASSERT_FALSE(clock.sleep_until(ros_until));
}

TEST_F(TestClockSleep, sleep_until_basic_system) {
static const auto MILLION = 1000L * 1000L;
const auto milliseconds = 300;
rclcpp::Clock clock(RCL_SYSTEM_TIME);
auto delay = rclcpp::Duration(0, milliseconds * MILLION);
auto sleep_until = clock.now() + delay;

auto start = std::chrono::system_clock::now();
ASSERT_TRUE(clock.sleep_until(sleep_until));
auto end = std::chrono::system_clock::now();

EXPECT_GE(clock.now(), sleep_until);
EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds));
}

TEST_F(TestClockSleep, sleep_until_basic_steady) {
static const auto MILLION = 1000L * 1000L;
const auto milliseconds = 300;
rclcpp::Clock clock(RCL_STEADY_TIME);
auto delay = rclcpp::Duration(0, milliseconds * MILLION);
auto sleep_until = clock.now() + delay;

auto steady_start = std::chrono::steady_clock::now();
ASSERT_TRUE(clock.sleep_until(sleep_until));
auto steady_end = std::chrono::steady_clock::now();

EXPECT_GE(clock.now(), sleep_until);
EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds));
}

TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) {
rclcpp::Clock clock(RCL_STEADY_TIME);
auto until = clock.now() - rclcpp::Duration(1000, 0);
// This should return immediately, other possible behavior might be sleep forever and timeout
ASSERT_TRUE(clock.sleep_until(until));
}

TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) {
rclcpp::Clock clock(RCL_SYSTEM_TIME);
auto until = clock.now() - rclcpp::Duration(1000, 0);
// This should return immediately, other possible behavior might be sleep forever and timeout
ASSERT_TRUE(clock.sleep_until(until));
}

TEST_F(TestClockSleep, sleep_until_ros_time_enable_interrupt) {
auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::TimeSource time_source;
time_source.attachNode(node);
time_source.attachClock(clock);

// 5 second timeout, but it should be interrupted right away
const auto until = clock->now() + rclcpp::Duration(5, 0);

// Try sleeping with ROS time off, then turn it on to interrupt
bool sleep_succeeded = true;
auto sleep_thread = std::thread(
[clock, until, &sleep_succeeded]() {
sleep_succeeded = clock->sleep_until(until);
});
// yield execution long enough to let the sleep thread get to waiting on the condition variable
std::this_thread::sleep_for(std::chrono::milliseconds(200));
auto set_parameters_results = param_client->set_parameters(
{rclcpp::Parameter("use_sim_time", true)});
for (auto & result : set_parameters_results) {
ASSERT_TRUE(result.successful);
}
sleep_thread.join();
EXPECT_FALSE(sleep_succeeded);
}

TEST_F(TestClockSleep, sleep_until_ros_time_disable_interrupt) {
param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)});
auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::TimeSource time_source;
time_source.attachNode(node);
time_source.attachClock(clock);

// /clock shouldn't be publishing, shouldn't be possible to reach timeout
const auto until = clock->now() + rclcpp::Duration(600, 0);

// Try sleeping with ROS time off, then turn it on to interrupt
bool sleep_succeeded = true;
auto sleep_thread = std::thread(
[clock, until, &sleep_succeeded]() {
sleep_succeeded = clock->sleep_until(until);
});
// yield execution long enough to let the sleep thread get to waiting on the condition variable
std::this_thread::sleep_for(std::chrono::milliseconds(200));
auto set_parameters_results = param_client->set_parameters(
{rclcpp::Parameter("use_sim_time", false)});
for (auto & result : set_parameters_results) {
ASSERT_TRUE(result.successful);
}
sleep_thread.join();
EXPECT_FALSE(sleep_succeeded);
}

TEST_F(TestClockSleep, sleep_until_shutdown_interrupt) {
param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)});
auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::TimeSource time_source;
time_source.attachNode(node);
time_source.attachClock(clock);

// the timeout doesn't matter here - no /clock is being published, so it should never wake
const auto until = clock->now() + rclcpp::Duration(600, 0);

bool sleep_succeeded = true;
auto sleep_thread = std::thread(
[clock, until, &sleep_succeeded]() {
sleep_succeeded = clock->sleep_until(until);
});
// yield execution long enough to let the sleep thread get to waiting on the condition variable
std::this_thread::sleep_for(std::chrono::milliseconds(200));
rclcpp::shutdown();
sleep_thread.join();
EXPECT_FALSE(sleep_succeeded);
}
33 changes: 0 additions & 33 deletions rclcpp/test/rclcpp/test_time_source.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -742,36 +742,3 @@ TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) {
// Node should have get out of timer callback
ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen());
}

TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) {
SimClockPublisherNode pub_node;
pub_node.SpinNode();

node->set_parameter({"use_sim_time", true});
auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::TimeSource time_source;
time_source.attachNode(node);
time_source.attachClock(clock);

// Wait until time source has definitely received a first ROS time from the pub node
{
rcl_jump_threshold_t threshold;
threshold.on_clock_change = false;
threshold.min_backward.nanoseconds = 0;
threshold.min_forward.nanoseconds = 0;

std::condition_variable cv;
std::mutex mutex;
auto handler = clock->create_jump_callback(
[]() {},
[&cv](const rcl_time_jump_t &) {cv.notify_all();},
threshold);
std::unique_lock lock(mutex);
cv.wait(lock);
}

auto now = clock->now();
// Any amount of time will do, just need to make sure that we awake and return true
auto until = now + rclcpp::Duration(0, 500);
EXPECT_TRUE(clock->sleep_until(until));
}

0 comments on commit d04319a

Please sign in to comment.