refactor SwitchParams to fix the incosistencies in the spawner tests (r…

…os-controls#1638)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -583,14 +583,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_;

controller_manager/src/controller_manager.cpp

@@ -849,6 +849,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
@@ -878,7 +879,8 @@ controller_interface::return_type ControllerManager::switch_controller(
  return controller_interface::return_type::ERROR;
  }
 
- switch_params_ = SwitchParams();
+ // reset the switch param internal variables
+ switch_params_.reset();
 
  if (!deactivate_request_.empty() || !activate_request_.empty())
  {
@@ -1299,19 +1301,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
@@ -2147,6 +2157,12 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
 
 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_))
@@ -2175,6 +2191,7 @@ void ControllerManager::manage_switch()
 
  // All controllers switched --> switching done
  switch_params_.do_switch = false;
+ switch_params_.cv.notify_all();
 }
 
 controller_interface::return_type ControllerManager::update(

controller_manager/test/test_controller_manager_srvs.cpp

@@ -168,9 +168,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());