Skip to content

Commit

Permalink
Fix next controller update cycle time clock (#1127)
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor authored Oct 4, 2023
1 parent 77229e0 commit 52218f8
Show file tree
Hide file tree
Showing 6 changed files with 58 additions and 52 deletions.
16 changes: 11 additions & 5 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -565,7 +565,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());

return add_controller_impl(controller_spec);
}
Expand Down Expand Up @@ -1457,7 +1458,8 @@ void ControllerManager::activate_controllers(
auto controller = found_it->c;
auto controller_name = found_it->info.name;
// reset the next update cycle time for newly activated controllers
*found_it->next_update_cycle_time = rclcpp::Time(0);
*found_it->next_update_cycle_time =
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());

bool assignment_successful = true;
// assign command interfaces to the controller
Expand Down Expand Up @@ -2043,8 +2045,10 @@ controller_interface::return_type ControllerManager::update(
run_controller_at_cm_rate ? period
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));

bool controller_go = (time == rclcpp::Time(0)) ||
(time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
bool controller_go =
(time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
(time.seconds() >= loaded_controller.next_update_cycle_time->seconds());

RCLCPP_DEBUG(
get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
Expand All @@ -2055,7 +2059,9 @@ controller_interface::return_type ControllerManager::update(
{
auto controller_ret = loaded_controller.c->update(time, controller_period);

if (*loaded_controller.next_update_cycle_time == rclcpp::Time(0))
if (
*loaded_controller.next_update_cycle_time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
{
*loaded_controller.next_update_cycle_time = time;
}
Expand Down
13 changes: 7 additions & 6 deletions controller_manager/test/controller_manager_test_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@

namespace
{
const auto TIME = rclcpp::Time(0);
const auto PERIOD = rclcpp::Duration::from_seconds(0.01);
const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT;
const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
Expand Down Expand Up @@ -101,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test
cm_->robot_description_callback(msg);
}
}
time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type());
}

static void SetUpTestCase() { rclcpp::init(0, nullptr); }
Expand All @@ -119,7 +119,7 @@ class ControllerManagerFixture : public ::testing::Test
{
while (run_updater_)
{
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01));
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
});
Expand Down Expand Up @@ -157,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test
bool run_updater_;
const std::string robot_description_;
const bool pass_urdf_as_parameter_;
rclcpp::Time time_;
};

class TestControllerManagerSrvs
Expand All @@ -177,9 +178,9 @@ class TestControllerManagerSrvs
std::chrono::milliseconds(10),
[&]()
{
cm_->read(TIME, PERIOD);
cm_->update(TIME, PERIOD);
cm_->write(TIME, PERIOD);
cm_->read(time_, PERIOD);
cm_->update(time_, PERIOD);
cm_->write(time_, PERIOD);
});

executor_->add_node(cm_);
Expand All @@ -206,7 +207,7 @@ class TestControllerManagerSrvs
while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) !=
rclcpp::FutureReturnCode::SUCCESS)
{
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01));
}
}
else
Expand Down
39 changes: 19 additions & 20 deletions controller_manager/test/test_controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter)
<< "Update should not reach an unconfigured controller";

Expand All @@ -108,7 +108,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
cm_->configure_controller(TEST_CONTROLLER2_NAME);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";

Expand All @@ -123,7 +123,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
Expand All @@ -132,7 +132,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter);

// Start the real test controller, will take effect at the end of the update function
Expand All @@ -147,7 +147,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
Expand All @@ -157,7 +157,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_GE(test_controller->internal_counter, 1u);
size_t last_internal_counter = test_controller->internal_counter;

Expand All @@ -173,7 +173,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter)
<< "Controller is stopped at the end of update, so it should have done one more update";
{
Expand Down Expand Up @@ -208,7 +208,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter)
<< "Update should not reach an unconfigured controller";

Expand All @@ -220,7 +220,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
Expand All @@ -237,7 +237,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
Expand All @@ -251,7 +251,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
{
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
}
EXPECT_GE(test_controller->internal_counter, 1u);
EXPECT_EQ(test_controller->get_update_rate(), 4u);
Expand Down Expand Up @@ -286,7 +286,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
EXPECT_EQ(2, test_controller.use_count());
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
<< "Update should not reach an unconfigured controller";

Expand All @@ -299,7 +299,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
<< "Controller is not started";
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
Expand All @@ -316,7 +316,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
<< "Controller is started at the end of update";
{
Expand All @@ -332,7 +332,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
{
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
loop_rate.sleep();
}
// if we do 2 times of the controller_manager update rate, the internal counter should be
Expand All @@ -348,8 +348,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
<< "switch_controller should be blocking until next update cycle";

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)))
controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01)))
<< "Controller is stopped at the end of update, so it should have done one more update";
{
ControllerManagerRunner cm_runner(this);
Expand Down Expand Up @@ -393,7 +392,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
Expand All @@ -411,7 +410,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
Expand All @@ -425,7 +424,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
const auto controller_update_rate = test_controller->get_update_rate();

const auto initial_counter = test_controller->internal_counter;
rclcpp::Time time = rclcpp::Time(0);
rclcpp::Time time = time_;
for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
{
EXPECT_EQ(
Expand Down
Loading

0 comments on commit 52218f8

Please sign in to comment.