Merge branch 'master' into support/wilcard_entries/param_files

controller_interface/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-09-11)
+-------------------
+* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 <https://github.com/ros-controls/ros2_control/issues/1683>`_)
+* Contributors: Manuel Muth
+
 4.16.1 (2024-08-24)
 -------------------
 

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -104,13 +104,26 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
  CONTROLLER_INTERFACE_PUBLIC
  virtual InterfaceConfiguration state_interface_configuration() const = 0;
 
+ /// Method that assigns the Loaned interfaces to the controller.
+ /**
+ * Method used by the controller_manager to assign the interfaces to the controller.
+ * \note When this method is overridden, the user has to also implement the `release_interfaces`
+ * method by overriding it to release the interfaces.
+ *
+ * \param[in] command_interfaces vector of command interfaces to be assigned to the controller.
+ * \param[in] state_interfaces vector of state interfaces to be assigned to the controller.
+ */
  CONTROLLER_INTERFACE_PUBLIC
- void assign_interfaces(
+ virtual void assign_interfaces(
  std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
  std::vector<hardware_interface::LoanedStateInterface> && state_interfaces);
 
+ /// Method that releases the Loaned interfaces from the controller.
+ /**
+ * Method used by the controller_manager to release the interfaces from the controller.
+ */
  CONTROLLER_INTERFACE_PUBLIC
- void release_interfaces();
+ virtual void release_interfaces();
 
  CONTROLLER_INTERFACE_PUBLIC
  return_type init(

controller_interface/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
  <name>controller_interface</name>
- <version>4.16.1</version>
+ <version>4.17.0</version>
  <description>Description of controller_interface</description>
  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
  <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-09-11)
+-------------------
+* Log exception type when catching the exception (`#1749 <https://github.com/ros-controls/ros2_control/issues/1749>`_)
+* [CM] Handle other exceptions while loading the controller plugin (`#1731 <https://github.com/ros-controls/ros2_control/issues/1731>`_)
+* remove unnecessary log of the CM args (`#1720 <https://github.com/ros-controls/ros2_control/issues/1720>`_)
+* Fix unload of controllers when spawned with `--unload-on-kill` (`#1717 <https://github.com/ros-controls/ros2_control/issues/1717>`_)
+* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 <https://github.com/ros-controls/ros2_control/issues/1683>`_)
+* Contributors: Manuel Muth, Sai Kishor Kothakota
+
 4.16.1 (2024-08-24)
 -------------------
 * propage a portion of global args to the controller nodes (`#1712 <https://github.com/ros-controls/ros2_control/issues/1712>`_)

controller_manager/controller_manager/controller_manager_services.py

@@ -88,15 +88,23 @@ def service_caller(
  @return The service response
 
  """
- cli = node.create_client(service_type, service_name)
+ namespace = "" if node.get_namespace() == "/" else node.get_namespace()
+ fully_qualified_service_name = (
+ f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name
+ )
+ cli = node.create_client(service_type, fully_qualified_service_name)
 
  while not cli.service_is_ready():
- node.get_logger().info(f"waiting for service {service_name} to become available...")
+ node.get_logger().info(
+ f"waiting for service {fully_qualified_service_name} to become available..."
+ )
  if service_timeout:
  if not cli.wait_for_service(service_timeout):
- raise ServiceNotFoundError(f"Could not contact service {service_name}")
+ raise ServiceNotFoundError(
+ f"Could not contact service {fully_qualified_service_name}"
+ )
  elif not cli.wait_for_service(10.0):
- node.get_logger().warn(f"Could not contact service {service_name}")
+ node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}")
 
  node.get_logger().debug(f"requester: making request: {request}\n")
  future = None
@@ -105,13 +113,13 @@ def service_caller(
  rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout)
  if future.result() is None:
  node.get_logger().warning(
- f"Failed getting a result from calling {service_name} in "
+ f"Failed getting a result from calling {fully_qualified_service_name} in "
  f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)"
  )
  else:
  return future.result()
  raise RuntimeError(
- f"Could not successfully call service {service_name} after {max_attempts} attempts."
+ f"Could not successfully call service {fully_qualified_service_name} after {max_attempts} attempts."
  )
 
 

controller_manager/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
  <name>controller_manager</name>
- <version>4.16.1</version>
+ <version>4.17.0</version>
  <description>Description of controller_manager</description>
  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
  <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

controller_manager/src/controller_manager.cpp

@@ -470,8 +470,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s",
- controller_name.c_str(), controller_type.c_str(), e.what());
+ get_logger(),
+ "Caught exception of type : %s while loading the controller '%s' of plugin type '%s':\n%s",
+ typeid(e).name(), controller_name.c_str(), controller_type.c_str(), e.what());
  return nullptr;
  }
  catch (...)
@@ -615,8 +616,9 @@ controller_interface::return_type ControllerManager::unload_controller(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Failed to clean-up the controller '%s' before unloading: %s",
- controller_name.c_str(), e.what());
+ get_logger(),
+ "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s",
+ typeid(e).name(), controller_name.c_str(), e.what());
  }
  catch (...)
  {
@@ -720,8 +722,8 @@ controller_interface::return_type ControllerManager::configure_controller(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Caught exception while configuring controller '%s': %s",
- controller_name.c_str(), e.what());
+ get_logger(), "Caught exception of type : %s while configuring controller '%s': %s",
+ typeid(e).name(), controller_name.c_str(), e.what());
  return controller_interface::return_type::ERROR;
  }
  catch (...)
@@ -1432,8 +1434,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
  {
  to.clear();
  RCLCPP_ERROR(
- get_logger(), "Caught exception while initializing controller '%s': %s",
- controller.info.name.c_str(), e.what());
+ get_logger(), "Caught exception of type : %s while initializing controller '%s': %s",
+ typeid(e).name(), controller.info.name.c_str(), e.what());
  return nullptr;
  }
  catch (...)
@@ -1508,8 +1510,8 @@ void ControllerManager::deactivate_controllers(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Caught exception while deactivating the controller '%s': %s",
- controller_name.c_str(), e.what());
+ get_logger(), "Caught exception of type : %s while deactivating the controller '%s': %s",
+ typeid(e).name(), controller_name.c_str(), e.what());
  continue;
  }
  catch (...)
@@ -1626,7 +1628,10 @@ void ControllerManager::activate_controllers(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+ get_logger(),
+ "Caught exception of type : %s while claiming the command interfaces. Can't activate "
+ "controller '%s': %s",
+ typeid(e).name(), controller_name.c_str(), e.what());
  assignment_successful = false;
  break;
  }
@@ -1661,7 +1666,10 @@ void ControllerManager::activate_controllers(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+ get_logger(),
+ "Caught exception of type : %s while claiming the state interfaces. Can't activate "
+ "controller '%s': %s",
+ typeid(e).name(), controller_name.c_str(), e.what());
  assignment_successful = false;
  break;
  }
@@ -1689,8 +1697,8 @@ void ControllerManager::activate_controllers(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Caught exception while activating the controller '%s': %s",
- controller_name.c_str(), e.what());
+ get_logger(), "Caught exception of type : %s while activating the controller '%s': %s",
+ typeid(e).name(), controller_name.c_str(), e.what());
  continue;
  }
  catch (...)
@@ -2295,8 +2303,8 @@ controller_interface::return_type ControllerManager::update(
  catch (const std::exception & e)
  {
  RCLCPP_ERROR(
- get_logger(), "Caught exception while updating controller '%s': %s",
- loaded_controller.info.name.c_str(), e.what());
+ get_logger(), "Caught exception of type : %s while updating controller '%s': %s",
+ typeid(e).name(), loaded_controller.info.name.c_str(), e.what());
  controller_ret = controller_interface::return_type::ERROR;
  }
  catch (...)

controller_manager/test/test_spawner_unspawner.cpp

@@ -277,6 +277,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
  ASSERT_EQ(
  ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
 
  auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(
@@ -287,6 +289,9 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
  ASSERT_EQ(
  chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ test_file_path);
 
  EXPECT_EQ(
  call_spawner(
@@ -302,12 +307,17 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
  ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_EQ(
  ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
 
  auto ctrl_2 = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
  ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_EQ(
  ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ test_file_path);
 }
 
 TEST_F(TestLoadController, unload_on_kill)
@@ -367,6 +377,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
  ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty());
  ASSERT_EQ(
  ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path);
  }
 
  // Try to spawn now the controller with fallback controllers inside the yaml
@@ -381,6 +392,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
  ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty());
  ASSERT_EQ(
  ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path);
 
  auto ctrl_2 = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
@@ -389,13 +401,15 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
  ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
  ASSERT_EQ(
  ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string(), test_file_path);
 
  auto ctrl_3 = cm_->get_loaded_controllers()[2];
  ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
  ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
  ASSERT_EQ(
  ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string(), test_file_path);
  }
 }
 
@@ -680,6 +694,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
  ASSERT_EQ(
  ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
 
  auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(
@@ -690,6 +706,9 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
  ASSERT_EQ(
  chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ test_file_path);
 
  EXPECT_EQ(
  call_spawner(
@@ -705,12 +724,17 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
  ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_EQ(
  ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
 
  auto ctrl_2 = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type");
  ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_EQ(
  ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ test_file_path);
 }
 
 TEST_F(
@@ -758,6 +782,8 @@ TEST_F(
  ASSERT_EQ(
  ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
 
  auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(
@@ -768,6 +794,9 @@ TEST_F(
  ASSERT_EQ(
  chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ test_file_path);
 
  EXPECT_EQ(
  call_spawner(
@@ -784,12 +813,17 @@ TEST_F(
  ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_EQ(
  ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
 
  auto ctrl_2 = cm_->get_loaded_controllers()[1];
  ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type");
  ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
  ASSERT_EQ(
  ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ test_file_path);
 }
 
 TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file)

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-09-11)
+-------------------
+
 4.16.1 (2024-08-24)
 -------------------
 

controller_manager_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>controller_manager_msgs</name>
- <version>4.16.1</version>
+ <version>4.17.0</version>
  <description>Messages and services for the controller manager.</description>
  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
  <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>