Skip to content

Commit

Permalink
Update controller_manager/src/ros2_control_node.cpp
Browse files Browse the repository at this point in the history 
Co-authored-by: G.A. vd. Hoorn <g.a.vanderhoorn@tudelft.nl>
Signed-off-by: Tyler Weaver <tylerjw@gmail.com>
  • Loading branch information
@tylerjw @gavanderhoorn
tylerjw and gavanderhoorn committed Jun 2, 2022
1 parent 6afb3eb commit a19eb9e
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 49 deletions.
44 changes: 20 additions & 24 deletions controller_manager/src/ros2_control_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,33 +44,29 @@ int main(int argc, char ** argv)
// the executor (see issue #260).
// When the MutliThreadedExecutor issues are fixed (ros2/rclcpp#1168), this loop should be
// converted back to a timer.
std::thread cm_thread(
[cm]()
{
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
std::thread cm_thread([cm]() {
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());

// Use nanoseconds to avoid chrono's rounding
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
// Use nanoseconds to avoid chrono's rounding
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());

// Functions for control loop
auto const now = [&cm]() { return cm->now(); };
auto const sleep_until = [](rclcpp::Time time)
{
std::this_thread::sleep_until(
std::chrono::system_clock::time_point(std::chrono::nanoseconds(time.nanoseconds())));
};
auto const ok = []() { return rclcpp::ok(); };
auto const do_work = [&cm](rclcpp::Time current_time, rclcpp::Duration period)
{
// Write is called first as the consistent rate or writing to hardware
// should not be affected by the time it takes to call update and read
cm->write(current_time, period);
cm->update(current_time, period);
cm->read(current_time, period);
};
// Functions for control loop
auto const now = [&cm]() { return cm->now(); };
auto const sleep_until = [](rclcpp::Time time) {
std::this_thread::sleep_until(
std::chrono::system_clock::time_point(std::chrono::nanoseconds(time.nanoseconds())));
};
auto const ok = []() { return rclcpp::ok(); };
auto const do_work = [&cm](rclcpp::Time current_time, rclcpp::Duration period) {
// Write is called first as the consistent rate of writing to hardware
// should not be affected by the time it takes to call update and read
cm->write(current_time, period);
cm->update(current_time, period);
cm->read(current_time, period);
};

ControlLoop(period, now, ok, do_work, sleep_until);
});
ControlLoop(period, now, ok, do_work, sleep_until);
});

executor->add_node(cm);
executor->spin();
Expand Down
43 changes: 18 additions & 25 deletions controller_manager/test/test_control_loop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,16 +60,16 @@ TEST(TestControlLoop, ExitsWhenNotOk)
TEST(TestControlLoop, DurationIsPeriod)
{
auto loop_iterations = 0;
auto ok = [&loop_iterations]()
{
auto ok = [&loop_iterations]() {
loop_iterations++;
return loop_iterations < 10;
};

// GIVEN do_work function that records duration values
std::vector<rclcpp::Duration> durations;
auto do_work = [&durations](rclcpp::Time, rclcpp::Duration duration)
{ durations.push_back(duration); };
auto do_work = [&durations](rclcpp::Time, rclcpp::Duration duration) {
durations.push_back(duration);
};

// WHEN we run the ControlLoop
ControlLoop(10'000'000ns, now, ok, do_work, sleep_until);
Expand All @@ -85,16 +85,16 @@ TEST(TestControlLoop, DurationIsPeriod)
TEST(TestControlLoop, FirstDurationIsSmall)
{
auto loop_iterations = 0;
auto ok = [&loop_iterations]()
{
auto ok = [&loop_iterations]() {
loop_iterations++;
return loop_iterations < 2;
};

// GIVEN do_work function that records duration values
std::vector<rclcpp::Duration> durations;
auto do_work = [&durations](rclcpp::Time, rclcpp::Duration duration)
{ durations.push_back(duration); };
auto do_work = [&durations](rclcpp::Time, rclcpp::Duration duration) {
durations.push_back(duration);
};

// WHEN we run the ControlLoop
ControlLoop(10'000'000ns, now, ok, do_work, sleep_until);
Expand All @@ -106,16 +106,14 @@ TEST(TestControlLoop, FirstDurationIsSmall)
TEST(TestControlLoop, DoWorkDelayInDuration)
{
auto loop_iterations = 0;
auto ok = [&loop_iterations]()
{
auto ok = [&loop_iterations]() {
loop_iterations++;
return loop_iterations < 3;
};

// GIVEN do_work function takes longer than the period
std::vector<rclcpp::Duration> durations;
auto do_work = [&durations](rclcpp::Time, rclcpp::Duration duration)
{
auto do_work = [&durations](rclcpp::Time, rclcpp::Duration duration) {
durations.push_back(duration);
std::this_thread::sleep_for(20'000'000ns);
};
Expand All @@ -124,23 +122,21 @@ TEST(TestControlLoop, DoWorkDelayInDuration)
ControlLoop(10'000'000ns, now, ok, do_work, sleep_until);

// THEN we expect the second duration to be near the time the work function took
EXPECT_NEAR(durations.at(1).nanoseconds(), 20'000'000, 100'000);
EXPECT_NEAR(durations.at(1).nanoseconds(), 20'000'000, 1'000'000);
}

TEST(TestControlLoop, LoopTimeBeforeWorkTime)
{
auto loop_iterations = 0;
auto ok = [&loop_iterations]()
{
auto ok = [&loop_iterations]() {
loop_iterations++;
return loop_iterations < 3;
};

// GIVEN do_work function takes longer than the period
std::vector<rclcpp::Time> loop_times;
std::vector<rclcpp::Time> work_times;
auto do_work = [&loop_times, &work_times](rclcpp::Time time, rclcpp::Duration)
{
auto do_work = [&loop_times, &work_times](rclcpp::Time time, rclcpp::Duration) {
loop_times.push_back(time);
work_times.push_back(now());
};
Expand All @@ -156,8 +152,7 @@ TEST(TestControlLoop, LoopTimeBeforeWorkTime)
TEST(TestControlLoop, LoopTimeSeparatedByPeriod)
{
auto loop_iterations = 0;
auto ok = [&loop_iterations]()
{
auto ok = [&loop_iterations]() {
loop_iterations++;
return loop_iterations < 3;
};
Expand All @@ -167,27 +162,25 @@ TEST(TestControlLoop, LoopTimeSeparatedByPeriod)
auto do_work = [&times](rclcpp::Time time, rclcpp::Duration) { times.push_back(time); };

// WHEN we run the ControlLoop
ControlLoop(10'000'000ns, now, ok, do_work, sleep_until);
ControlLoop(100'000'000ns, now, ok, do_work, sleep_until);

// THEN we expect the time between the loop times to be near the period
auto const duration = times.at(1) - times.at(0);
EXPECT_NEAR(duration.nanoseconds(), 10'000'000, 100'000);
EXPECT_NEAR(duration.nanoseconds(), 100'000'000, 1'000'000);
}

TEST(TestControlLoop, LoopTimeCloseToReportedDuration)
{
auto loop_iterations = 0;
auto ok = [&loop_iterations]()
{
auto ok = [&loop_iterations]() {
loop_iterations++;
return loop_iterations < 3;
};

// GIVEN do_work function takes longer than the period
std::vector<rclcpp::Time> times;
std::vector<rclcpp::Duration> durations;
auto do_work = [&times, &durations](rclcpp::Time time, rclcpp::Duration duration)
{
auto do_work = [&times, &durations](rclcpp::Time time, rclcpp::Duration duration) {
times.push_back(time);
durations.push_back(duration);
};
Expand Down

0 comments on commit a19eb9e

Please sign in to comment.