Merge branch 'master' into cm/service_timeout

controller_interface/src/controller_interface_base.cpp

@@ -28,13 +28,14 @@ return_type ControllerInterfaceBase::init(
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
   urdf_ = urdf;
+  update_rate_ = cm_update_rate;
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
     controller_name, node_namespace, node_options,
     false);  // disable LifecycleNode service interfaces
 
   try
   {
-    auto_declare<int>("update_rate", cm_update_rate);
+    auto_declare<int>("update_rate", update_rate_);
     auto_declare<bool>("is_async", false);
   }
   catch (const std::exception & e)
@@ -85,7 +86,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
   // Other solution is to add check into the LifecycleNode if a transition is valid to trigger
   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
-    update_rate_ = static_cast<unsigned int>(get_node()->get_parameter("update_rate").as_int());
+    const auto update_rate = get_node()->get_parameter("update_rate").as_int();
+    if (update_rate < 0)
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!");
+      return get_lifecycle_state();
+    }
+    if (update_rate_ != 0u && update_rate > update_rate_)
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(),
+        "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the "
+        "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.",
+        update_rate, update_rate_);
+    }
+    else
+    {
+      update_rate_ = static_cast<unsigned int>(update_rate);
+    }
     is_async_ = get_node()->get_parameter("is_async").as_bool();
   }
 

controller_interface/test/test_controller_interface.cpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/executor_options.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 
@@ -46,8 +47,8 @@ TEST(TestableControllerInterface, init)
     controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
-  // update_rate is set to default 0
-  ASSERT_EQ(controller.get_update_rate(), 0u);
+  // update_rate is set to controller_manager's rate
+  ASSERT_EQ(controller.get_update_rate(), 10u);
 
   // Even after configure is 10
   controller.configure();
@@ -56,6 +57,32 @@ TEST(TestableControllerInterface, init)
   rclcpp::shutdown();
 }
 
+TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
+{
+  // mocks the declaration of overrides parameters in a yaml file
+  rclcpp::init(0, nullptr);
+
+  TestableControllerInterface controller;
+  // initialize, create node
+  auto node_options = controller.define_custom_node_options();
+  std::vector<std::string> node_options_arguments = node_options.arguments();
+  node_options_arguments.push_back("--ros-args");
+  node_options_arguments.push_back("-p");
+  node_options_arguments.push_back("update_rate:=-100");
+  node_options = node_options.arguments(node_options_arguments);
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options),
+    controller_interface::return_type::OK);
+
+  // update_rate is set to controller_manager's rate
+  ASSERT_EQ(controller.get_update_rate(), 1000u);
+
+  // The configure should fail and the update rate should stay the same
+  ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  ASSERT_EQ(controller.get_update_rate(), 1000u);
+  rclcpp::shutdown();
+}
+
 TEST(TestableControllerInterface, setting_update_rate_in_configure)
 {
   // mocks the declaration of overrides parameters in a yaml file
@@ -70,7 +97,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   node_options_arguments.push_back("update_rate:=2812");
   node_options = node_options.arguments(node_options_arguments);
   ASSERT_EQ(
-    controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options),
+    controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options),
     controller_interface::return_type::OK);
 
   // initialize executor to be able to get parameter update
@@ -85,8 +112,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   using namespace std::chrono_literals;
   std::this_thread::sleep_for(50ms);
 
-  // update_rate is set to default 0 because it is set on configure
-  ASSERT_EQ(controller.get_update_rate(), 0u);
+  // update_rate is set to controller_manager's rate
+  ASSERT_EQ(controller.get_update_rate(), 5000u);
 
   // Even after configure is 0
   controller.configure();

controller_manager/CMakeLists.txt

@@ -91,6 +91,7 @@ if(BUILD_TESTING)
   target_link_libraries(test_controller_manager
     controller_manager
     test_controller
+    test_chainable_controller
     ros2_control_test_assets::ros2_control_test_assets
   )
 

controller_manager/controller_manager/controller_manager_services.py

@@ -253,24 +253,57 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
     )
 
 
-def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
+def get_parameter_from_param_file(
+    node, controller_name, namespace, parameter_file, parameter_name
+):
     with open(parameter_file) as f:
         namespaced_controller = (
-            controller_name if namespace == "/" else f"{namespace}/{controller_name}"
+            f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
         )
+        WILDCARD_KEY = "/**"
+        ROS_PARAMS_KEY = "ros__parameters"
         parameters = yaml.safe_load(f)
-        if namespaced_controller in parameters:
-            value = parameters[namespaced_controller]
-            if not isinstance(value, dict) or "ros__parameters" not in value:
+        controller_param_dict = None
+        # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
+        for key in [
+            controller_name,
+            namespaced_controller,
+            f"{WILDCARD_KEY}/{controller_name}",
+            f"{WILDCARD_KEY}{namespaced_controller}",
+        ]:
+            if key in parameters:
+                if key == controller_name and namespace != "/":
+                    node.get_logger().fatal(
+                        f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
+                    )
+                    break
+                controller_param_dict = parameters[key]
+
+            if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
+                controller_param_dict = parameters[WILDCARD_KEY][key]
+
+            if controller_param_dict and (
+                not isinstance(controller_param_dict, dict)
+                or ROS_PARAMS_KEY not in controller_param_dict
+            ):
                 raise RuntimeError(
-                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
+                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}"
                 )
-            if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
-                return parameters[namespaced_controller]["ros__parameters"][parameter_name]
-            else:
-                return None
-        else:
-            return None
+            if (
+                controller_param_dict
+                and ROS_PARAMS_KEY in controller_param_dict
+                and parameter_name in controller_param_dict[ROS_PARAMS_KEY]
+            ):
+                break
+
+        if controller_param_dict is None:
+            node.get_logger().fatal(
+                f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}"
+            )
+        if parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
+            return controller_param_dict[ROS_PARAMS_KEY][parameter_name]
+
+        return None
 
 
 def set_controller_parameters(
@@ -324,7 +357,7 @@ def set_controller_parameters_from_param_file(
         )
 
         controller_type = get_parameter_from_param_file(
-            controller_name, spawner_namespace, parameter_file, "type"
+            node, controller_name, spawner_namespace, parameter_file, "type"
         )
         if controller_type:
             if not set_controller_parameters(
@@ -333,7 +366,7 @@ def set_controller_parameters_from_param_file(
                 return False
 
         fallback_controllers = get_parameter_from_param_file(
-            controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+            node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
         )
         if fallback_controllers:
             if not set_controller_parameters(

controller_manager/doc/userdoc.rst

@@ -87,6 +87,18 @@ update_rate (mandatory; integer)
   Name of a plugin exported using ``pluginlib`` for a controller.
   This is a class from which controller's instance with name "``controller_name``" is created.
 
+<controller_name>.params_file
+  The absolute path to the YAML file with parameters for the controller.
+  The file should contain the parameters for the controller in the standard ROS 2 YAML format.
+
+<controller_name>.fallback_controllers
+  List of controllers that are activated as a fallback strategy, when the spawned controllers fail by returning ``return_type::ERROR`` during the ``update`` cycle.
+  It is recommended to add all the controllers needed for the fallback strategy to the list, including the chainable controllers whose interfaces are used by the main fallback controllers.
+
+.. warning::
+  The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation.
+  It is recommended to test the fallback strategy in simulation before deploying it on the real robot.
+
 Handling Multiple Controller Managers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
@@ -171,6 +183,60 @@ There are two scripts to interact with controller manager from launch files:
       --activate-as-group   Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
 
 
+The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files:
+
+ .. code-block:: yaml
+
+    /**/position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
+ .. code-block:: yaml
+
+    /position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
+ .. code-block:: yaml
+
+    position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
+ .. code-block:: yaml
+
+    /rrbot_1/position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
 ``unspawner``
 ^^^^^^^^^^^^^^^^
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -128,6 +128,12 @@ class ControllerManager : public rclcpp::Node
     return add_controller_impl(controller_spec);
   }
 
+  controller_interface::ControllerInterfaceBaseSharedPtr add_controller(
+    const ControllerSpec & controller_spec)
+  {
+    return add_controller_impl(controller_spec);
+  }
+
   /// configure_controller Configure controller by name calling their "configure" method.
   /**
    * \param[in] controller_name as a string.
@@ -418,6 +424,18 @@ class ControllerManager : public rclcpp::Node
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it);
 
+  /// Checks if the fallback controllers of the given controllers are in the right
+  /// state, so they can be activated immediately
+  /**
+   * \param[in] controllers is a list of controllers to activate.
+   * \param[in] controller_it is the iterator pointing to the controller to be activated.
+   * \return return_type::OK if all fallback controllers are in the right state, otherwise
+   * return_type::ERROR.
+   */
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type check_fallback_controllers_state_pre_activation(
+    const std::vector<ControllerSpec> & controllers, const ControllersListIterator controller_it);
+
   /**
    * @brief Inserts a controller into an ordered list based on dependencies to compute the
    * controller chain.