Merge branch 'master' into async_controllers

codecov.yml

@@ -14,8 +14,6 @@ fixes:
 comment:
  layout: "diff, flags, files"
  behavior: default
-ignore:
- - "**/test"
 flags:
  unittests:
  paths:

controller_manager/include/controller_manager/controller_manager.hpp

@@ -582,14 +582,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/package.xml

@@ -31,6 +31,7 @@
 
  <test_depend>ament_cmake_gmock</test_depend>
  <test_depend>ament_cmake_pytest</test_depend>
+ <test_depend>python3-coverage</test_depend>
  <test_depend>hardware_interface_testing</test_depend>
  <test_depend>ros2_control_test_assets</test_depend>
 

controller_manager/src/controller_manager.cpp

@@ -530,12 +530,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
  get_logger(), "The 'type' param was not defined for '%s'.", controller_name.c_str());
  return nullptr;
  }
+ RCLCPP_INFO(
+ get_logger(), "Loading controller : '%s' of type '%s'", controller_name.c_str(),
+ controller_type.c_str());
  return load_controller(controller_name, controller_type);
 }
 
 controller_interface::return_type ControllerManager::unload_controller(
  const std::string & controller_name)
 {
+ RCLCPP_INFO(get_logger(), "Unloading controller: '%s'", controller_name.c_str());
  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);
  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);
@@ -626,7 +630,7 @@ std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const
 controller_interface::return_type ControllerManager::configure_controller(
  const std::string & controller_name)
 {
- RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str());
+ RCLCPP_INFO(get_logger(), "Configuring controller: '%s'", controller_name.c_str());
 
  const auto & controllers = get_loaded_controllers();
 
@@ -834,6 +838,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
@@ -863,7 +868,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())
  {
@@ -902,16 +908,24 @@ controller_interface::return_type ControllerManager::switch_controller(
  strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
  }
 
- RCLCPP_DEBUG(get_logger(), "Activating controllers:");
+ std::string activate_list, deactivate_list;
+ activate_list.reserve(500);
+ deactivate_list.reserve(500);
  for (const auto & controller : activate_controllers)
  {
- RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str());
+ activate_list.append(controller);
+ activate_list.append(" ");
  }
- RCLCPP_DEBUG(get_logger(), "Deactivating controllers:");
  for (const auto & controller : deactivate_controllers)
  {
- RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str());
+ deactivate_list.append(controller);
+ deactivate_list.append(" ");
  }
+ RCLCPP_INFO_EXPRESSION(
+ get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str());
+ RCLCPP_INFO_EXPRESSION(
+ get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]",
+ deactivate_list.c_str());
 
  const auto list_controllers = [this, strictness](
  const std::vector<std::string> & controller_list,
@@ -1284,19 +1298,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
@@ -2113,13 +2135,32 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
  if (!ok)
  {
  std::vector<std::string> stop_request = {};
+ std::string failed_hardware_string;
+ failed_hardware_string.reserve(500);
  // Determine controllers to stop
  for (const auto & hardware_name : failed_hardware_names)
  {
+ failed_hardware_string.append(hardware_name);
+ failed_hardware_string.append(" ");
  auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
  stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
  }
-
+ std::string stop_request_string;
+ stop_request_string.reserve(500);
+ for (const auto & controller : stop_request)
+ {
+ stop_request_string.append(controller);
+ stop_request_string.append(" ");
+ }
+ RCLCPP_ERROR(
+ get_logger(),
+ "Deactivating following hardware components as their read cycle resulted in an error: [ %s]",
+ failed_hardware_string.c_str());
+ RCLCPP_ERROR_EXPRESSION(
+ get_logger(), !stop_request_string.empty(),
+ "Deactivating following controllers as their hardware components read cycle resulted in an "
+ "error: [ %s]",
+ stop_request_string.c_str());
  std::vector<ControllerSpec> & rt_controller_list =
  rt_controllers_wrapper_.update_and_get_used_by_rt_list();
  deactivate_controllers(rt_controller_list, stop_request);
@@ -2129,6 +2170,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_))
@@ -2157,6 +2204,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(
@@ -2279,13 +2327,33 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
  if (!ok)
  {
  std::vector<std::string> stop_request = {};
+ std::string failed_hardware_string;
+ failed_hardware_string.reserve(500);
  // Determine controllers to stop
  for (const auto & hardware_name : failed_hardware_names)
  {
+ failed_hardware_string.append(hardware_name);
+ failed_hardware_string.append(" ");
  auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
  stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
  }
-
+ std::string stop_request_string;
+ stop_request_string.reserve(500);
+ for (const auto & controller : stop_request)
+ {
+ stop_request_string.append(controller);
+ stop_request_string.append(" ");
+ }
+ RCLCPP_ERROR(
+ get_logger(),
+ "Deactivating following hardware components as their write cycle resulted in an error: [ "
+ "%s]",
+ failed_hardware_string.c_str());
+ RCLCPP_ERROR_EXPRESSION(
+ get_logger(), !stop_request_string.empty(),
+ "Deactivating following controllers as their hardware components write cycle resulted in an "
+ "error: [ %s]",
+ stop_request_string.c_str());
  std::vector<ControllerSpec> & rt_controller_list =
  rt_controllers_wrapper_.update_and_get_used_by_rt_list();
  deactivate_controllers(rt_controller_list, stop_request);

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

controller_manager/test/test_spawner_unspawner.cpp

@@ -27,6 +27,7 @@
 
 using ::testing::_;
 using ::testing::Return;
+const char coveragepy_script[] = "python3 -m coverage run --append --branch";
 
 using namespace std::chrono_literals;
 class TestLoadController : public ControllerManagerFixture<controller_manager::ControllerManager>
@@ -68,14 +69,18 @@ class TestLoadController : public ControllerManagerFixture<controller_manager::C
 
 int call_spawner(const std::string extra_args)
 {
- std::string spawner_script = "ros2 run controller_manager spawner ";
+ std::string spawner_script =
+ std::string(coveragepy_script) +
+ " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner ";
  return std::system((spawner_script + extra_args).c_str());
 }
 
 int call_unspawner(const std::string extra_args)
 {
- std::string spawner_script = "ros2 run controller_manager unspawner ";
- return std::system((spawner_script + extra_args).c_str());
+ std::string unspawner_script =
+ std::string(coveragepy_script) +
+ " $(ros2 pkg prefix controller_manager)/lib/controller_manager/unspawner ";
+ return std::system((unspawner_script + extra_args).c_str());
 }
 
 TEST_F(TestLoadController, spawner_with_no_arguments_errors)
@@ -298,11 +303,12 @@ TEST_F(TestLoadController, unload_on_kill)
 {
  // Launch spawner with unload on kill
  // timeout command will kill it after the specified time with signal SIGINT
+ cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
  std::stringstream ss;
  ss << "timeout --signal=INT 5 "
- << "ros2 run controller_manager spawner "
- << "ctrl_3 -c test_controller_manager -t "
- << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill";
+ << std::string(coveragepy_script) +
+  " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner "
+ << "ctrl_3 -c test_controller_manager --unload-on-kill";
 
  EXPECT_NE(std::system(ss.str().c_str()), 0)
  << "timeout should have killed spawner and returned non 0 code";

hardware_interface/include/hardware_interface/actuator.hpp

@@ -42,7 +42,8 @@ class Actuator final
  HARDWARE_INTERFACE_PUBLIC
  explicit Actuator(std::unique_ptr<ActuatorInterface> impl);
 
- Actuator(Actuator && other) = default;
+ HARDWARE_INTERFACE_PUBLIC
+ explicit Actuator(Actuator && other) noexcept;
 
  ~Actuator() = default;
 
@@ -102,6 +103,7 @@ class Actuator final
 
 private:
  std::unique_ptr<ActuatorInterface> impl_;
+ mutable std::recursive_mutex actuators_mutex_;
 };
 
 } // namespace hardware_interface

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -228,7 +228,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  */
  void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
-protected:
  /// Get the logger of the ActuatorInterface.
  /**
  * \return logger of the ActuatorInterface.
@@ -247,6 +246,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  */
  const HardwareInfo & get_hardware_info() const { return info_; }
 
+protected:
  HardwareInfo info_;
  rclcpp_lifecycle::State lifecycle_state_;
 

hardware_interface/include/hardware_interface/sensor.hpp

@@ -42,7 +42,8 @@ class Sensor final
  HARDWARE_INTERFACE_PUBLIC
  explicit Sensor(std::unique_ptr<SensorInterface> impl);
 
- Sensor(Sensor && other) = default;
+ HARDWARE_INTERFACE_PUBLIC
+ explicit Sensor(Sensor && other) noexcept;
 
  ~Sensor() = default;
 
@@ -89,6 +90,7 @@ class Sensor final
 
 private:
  std::unique_ptr<SensorInterface> impl_;
+ mutable std::recursive_mutex sensors_mutex_;
 };
 
 } // namespace hardware_interface