Skip to content

Commit

Permalink
refactor SwitchParams to fix the incosistencies in the spawner tests …
Browse files Browse the repository at this point in the history
…(backport #1638) (#1659)
  • Loading branch information
mergify[bot] authored Aug 14, 2024
1 parent 8a3ac66 commit 8779757
Show file tree
Hide file tree
Showing 3 changed files with 76 additions and 17 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -509,14 +509,25 @@ class ControllerManager : public rclcpp::Node

struct SwitchParams
{
bool do_switch = {false};
bool started = {false};
rclcpp::Time init_time = {rclcpp::Time::max()};
void reset()
{
do_switch = false;
started = false;
strictness = 0;
activate_asap = false;
}

bool do_switch;
bool started;

// Switch options
int strictness = {0};
bool activate_asap = {false};
rclcpp::Duration timeout = rclcpp::Duration{0, 0};
int strictness;
bool activate_asap;
std::chrono::nanoseconds timeout;

// conditional variable and mutex to wait for the switch to complete
std::condition_variable cv;
std::mutex mutex;
};

SwitchParams switch_params_;
Expand Down
39 changes: 29 additions & 10 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -821,6 +821,7 @@ controller_interface::return_type ControllerManager::configure_controller(

void ControllerManager::clear_requests()
{
switch_params_.do_switch = false;
deactivate_request_.clear();
activate_request_.clear();
// Set these interfaces as unavailable when clearing requests to avoid leaving them in available
Expand All @@ -840,7 +841,8 @@ controller_interface::return_type ControllerManager::switch_controller(
const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
const rclcpp::Duration & timeout)
{
switch_params_ = SwitchParams();
// reset the switch param internal variables
switch_params_.reset();

if (!deactivate_request_.empty() || !activate_request_.empty())
{
Expand Down Expand Up @@ -1201,19 +1203,27 @@ controller_interface::return_type ControllerManager::switch_controller(
// start the atomic controller switching
switch_params_.strictness = strictness;
switch_params_.activate_asap = activate_asap;
switch_params_.init_time = rclcpp::Clock().now();
switch_params_.timeout = timeout;
if (timeout == rclcpp::Duration{0, 0})
{
RCLCPP_INFO_ONCE(get_logger(), "Switch controller timeout is set to 0, using default 1s!");
switch_params_.timeout = std::chrono::nanoseconds(1'000'000'000);
}
else
{
switch_params_.timeout = timeout.to_chrono<std::chrono::nanoseconds>();
}
switch_params_.do_switch = true;

// wait until switch is finished
RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop");
while (rclcpp::ok() && switch_params_.do_switch)
std::unique_lock<std::mutex> switch_params_guard(switch_params_.mutex, std::defer_lock);
if (!switch_params_.cv.wait_for(
switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; }))
{
if (!rclcpp::ok())
{
return controller_interface::return_type::ERROR;
}
std::this_thread::sleep_for(std::chrono::microseconds(100));
RCLCPP_ERROR(
get_logger(), "Switch controller timed out after %f seconds!",
static_cast<double>(switch_params_.timeout.count()) / 1e9);
clear_requests();
return controller_interface::return_type::ERROR;
}

// copy the controllers spec from the used to the unused list
Expand Down Expand Up @@ -1306,6 +1316,12 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co

void ControllerManager::manage_switch()
{
std::unique_lock<std::mutex> guard(switch_params_.mutex, std::try_to_lock);
if (!guard.owns_lock())
{
RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle.");
return;
}
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
Expand All @@ -1330,6 +1346,9 @@ void ControllerManager::manage_switch()
}

// TODO(destogl): move here "do_switch = false"

switch_params_.do_switch = false;
switch_params_.cv.notify_all();
}

void ControllerManager::deactivate_controllers()
Expand Down
31 changes: 30 additions & 1 deletion controller_manager/test/test_controller_manager_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -166,9 +166,38 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
"joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
"joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity"));

// Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch
ASSERT_EQ(
controller_interface::return_type::ERROR,
cm_->switch_controller(
{}, {test_controller::TEST_CONTROLLER_NAME},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 1)));

result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(1u, result->controller.size());
ASSERT_EQ("active", result->controller[0].state);
ASSERT_THAT(
result->controller[0].claimed_interfaces,
UnorderedElementsAre(
"joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk",
"joint1/position", "joint1/max_velocity"));
ASSERT_THAT(
result->controller[0].required_command_interfaces,
UnorderedElementsAre(
"configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position",
"joint2/max_acceleration", "joint2/velocity", "joint3/velocity"));
ASSERT_THAT(
result->controller[0].required_state_interfaces,
UnorderedElementsAre(
"configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface",
"joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
"joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity"));

// Try again with higher timeout
cm_->switch_controller(
{}, {test_controller::TEST_CONTROLLER_NAME},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(3, 0));

result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(1u, result->controller.size());
Expand Down

0 comments on commit 8779757

Please sign in to comment.