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>