refactor SwitchParams to fix the incosistencies running at higher fre…

…quencies

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

@@ -877,7 +877,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())
  {
@@ -1296,21 +1297,30 @@ controller_interface::return_type ControllerManager::switch_controller(
  }
 
  // start the atomic controller switching
+ std::unique_lock<std::mutex> switch_params_guard(switch_params_.mutex);
  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)
+ 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
@@ -2146,6 +2156,7 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
 
 void ControllerManager::manage_switch()
 {
+ std::lock_guard<std::mutex> guard(switch_params_.mutex);
  // Ask hardware interfaces to change mode
  if (!resource_manager_->perform_command_mode_switch(
  activate_command_interface_request_, deactivate_command_interface_request_))
@@ -2174,6 +2185,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(