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

…1638)

(cherry picked from commit a1ad523)

# Conflicts:
#	controller_manager/src/controller_manager.cpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -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_;

controller_manager/src/controller_manager.cpp

@@ -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
@@ -840,7 +841,21 @@ controller_interface::return_type ControllerManager::switch_controller(
   const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
   const rclcpp::Duration & timeout)
 {
+<<<<<<< HEAD
   switch_params_ = SwitchParams();
+=======
+  if (!is_resource_manager_initialized())
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Resource Manager is not initialized yet! Please provide robot description on "
+      "'robot_description' topic before trying to switch controllers.");
+    return controller_interface::return_type::ERROR;
+  }
+
+  // reset the switch param internal variables
+  switch_params_.reset();
+>>>>>>> a1ad523 (refactor SwitchParams to fix the incosistencies in the spawner tests (#1638))
 
   if (!deactivate_request_.empty() || !activate_request_.empty())
   {
@@ -1201,19 +1216,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
@@ -1999,7 +2022,66 @@ std::vector<std::string> ControllerManager::get_controller_names()
 
 void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+<<<<<<< HEAD
   resource_manager_->read(time, period);
+=======
+  auto [ok, failed_hardware_names] = resource_manager_->read(time, period);
+
+  if (!ok)
+  {
+    std::vector<std::string> stop_request = {};
+    // Determine controllers to stop
+    for (const auto & hardware_name : failed_hardware_names)
+    {
+      auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
+      stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
+    }
+
+    std::vector<ControllerSpec> & rt_controller_list =
+      rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+    deactivate_controllers(rt_controller_list, stop_request);
+    // TODO(destogl): do auto-start of broadcasters
+  }
+}
+
+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_))
+  {
+    RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
+  }
+
+  std::vector<ControllerSpec> & rt_controller_list =
+    rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+
+  deactivate_controllers(rt_controller_list, deactivate_request_);
+
+  switch_chained_mode(to_chained_mode_request_, true);
+  switch_chained_mode(from_chained_mode_request_, false);
+
+  // activate controllers once the switch is fully complete
+  if (!switch_params_.activate_asap)
+  {
+    activate_controllers(rt_controller_list, activate_request_);
+  }
+  else
+  {
+    // activate controllers as soon as their required joints are done switching
+    activate_controllers_asap(rt_controller_list, activate_request_);
+  }
+
+  // All controllers switched --> switching done
+  switch_params_.do_switch = false;
+  switch_params_.cv.notify_all();
+>>>>>>> a1ad523 (refactor SwitchParams to fix the incosistencies in the spawner tests (#1638))
 }
 
 controller_interface::return_type ControllerManager::update(

controller_manager/test/test_controller_manager_srvs.cpp

@@ -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());