Merge branch 'master' into add-simple-joint-limiter

.github/workflows/ci-check-docs.yml

@@ -0,0 +1,12 @@
+name: Check Docs
+
+on:
+  workflow_dispatch:
+  pull_request:
+
+jobs:
+  check-docs:
+    name: Check Docs
+    uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master
+    with:
+      ROS2_CONTROL_PR: ${{ github.ref }}

controller_interface/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.16.0 (2023-07-09)
+-------------------
+
 3.15.0 (2023-06-23)
 -------------------
 

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>3.15.0</version>
+  <version>3.16.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.16.0 (2023-07-09)
+-------------------
+* added controller manager runner to have update cycles (`#1075 <https://github.com/ros-controls/ros2_control/issues/1075>`_)
+* [CM] Make error message after trying to active controller more informative. (`#1066 <https://github.com/ros-controls/ros2_control/issues/1066>`_)
+* Fix equal and higher controller update rate (`#1070 <https://github.com/ros-controls/ros2_control/issues/1070>`_)
+* Create doc file for chained controllers (`#985 <https://github.com/ros-controls/ros2_control/issues/985>`_)
+* Contributors: Dr. Denis, Sai Kishor Kothakota
+
 3.15.0 (2023-06-23)
 -------------------
 * Enable setting of initial state in HW compoments (`#1046 <https://github.com/ros-controls/ros2_control/issues/1046>`_)

controller_manager/doc/controller_chaining.rst

@@ -0,0 +1,119 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/doc/controller_chaining.rst
+
+.. _controller_chaining:
+
+Controller Chaining / Cascade Control
+======================================
+
+This document proposes a minimal-viable-implementation of serial controller chaining as described in `Chaining Controllers design document <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md>`__.
+Cascade control is a specific type of controller chaining.
+
+
+Scope of the Document and Background Knowledge
+-------------------------------------------------------
+
+This approach focuses only on serial chaining of controllers and tries to reuse existing mechanisms for it.
+It focuses on `inputs and outputs of a controller <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#input--outputs-of-a-controller>`__ and their management in the controller manager.
+The concept of `controller groups <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#controller-group>`__ will be introduced only for clarity reasons, and its only meaning is that controllers in that group can be updated in arbitrary order.
+This doesn't mean that the controller groups as described `in the controller chaining document <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#controller-group>`__ will not be introduced and used in the future.
+Nevertheless, the author is convinced that this would add only unnecessary complexity at this stage, although in the long term they *could* provide clearer structure and interfaces.
+
+Motivation, Purpose and Use
+---------------------------------
+
+To describe the intent of this document, lets focus on the simple yet sufficient example `Example 2 from 'controllers_chaining' design docs  <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#example-2>`__:
+
+.. image:: images/chaining_example2.png
+  :alt: Example2
+
+
+In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers.
+Let's now imagine a use-case where we don't only want to run all those controllers as a group, but also flexibly add preceding steps.
+This means the following:
+
+  1. When a robot is started, we want to check if motor velocity control is working properly and therefore only PID controllers are activated.
+     At this stage we can control the input of PID controller also externally using topics.
+     However, these controllers also provide virtual interfaces, so we can chain them.
+  2. Then "diff_drive_controller" is activated and attaches itself to the virtual input interfaces of PID controllers.
+     PID controllers also get informed that they are working in chained mode and therefore disable their external interface through subscriber.
+     Now we check if kinematics of differential robot is running properly.
+  3. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces.
+  4. If any of the controllers is deactivated, also all preceding controllers are deactivated.
+
+
+Implementation
+--------------
+
+A Controller Base-Class: ChainableController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual InterfaceConfiguration input_interface_configuration() const = 0`` method.
+This method should implement for each controller that **can be preceded** by another controller exporting all the input interfaces.
+For simplicity reasons, it is assumed for now that controller's all input interfaces are used.
+Therefore, do not try to implement any exclusive combinations of input interfaces, but rather write multiple controllers if you need exclusivity.
+
+The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained modes is activated or deactivated, e.g., deactivating subscribers.
+
+As an example, PID controllers export one virtual interface ``pid_reference`` and stop their subscriber ``<controller_name>/pid_reference`` when used in chained mode.  'diff_drive_controller' controller exports list of virtual interfaces  ``<controller_name>/v_x``, ``<controller_name>/v_y``, and ``<controller_name>/w_z``, and stops subscribers from topics ``<controller_name>/cmd_vel`` and ``<controller_name>/cmd_vel_unstamped``. Its publishers can continue running.
+
+Inner Resource Management
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+After configuring a chainable controller, controller manager calls ``input_interface_configuration`` method and takes ownership over controller's input interfaces.
+This is the same process as done by ``ResourceManager`` and hardware interfaces.
+Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``).
+
+Activation and Deactivation Chained Controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Controller Manager has an additional parameter that describes how controllers are chained.
+In the first version, the parameter-structure would have some semantic meaning embedded into it, as follows:
+
+
+.. code-block:: yaml
+
+  controller_manager:
+    ros__parameters:
+      chained_controllers:
+
+        - parallel_group_1:
+          - controller1_1
+          - controller1_2
+
+        - parallel_group_2:
+          - controller2_1
+
+        - parallel_group_3:
+          - controller3_1
+          - controller3_2
+          - controller3_3
+
+        ...
+
+        - parallel_group_N:
+          - controllerN_1
+          - ...
+          - controllerN_M
+
+
+
+This structure is motivated by ``filter_chain`` structure from `ros/filters repository <https://github.com/ros/filters/tree/ros2>`__, see `this file for implementation <https://github.com/ros/filters/blob/ros2/include/filters/filter_chain.hpp>`__.
+
+This structure is stored internally by controller manager into an ordered map (``std::map<std::string, std::vector<std::string>>``) with group name as key.
+When a controller should be deactivated, the controller manager deactivates all the controllers in the preceding groups first.
+All other controllers from the group stay active, as well as all controllers in the following groups.
+NOTE: In the future this could be done more intelligently, i.e., deactivate only controllers in the preceding groups that actually precede the controller that should be deactivated.
+
+On the other hand, the controller should be manually activated in the reverse order, i.e., from the those closer to the hardware toward those preceding them.
+
+
+Debugging outputs
+----------------------------
+
+Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage.
+
+
+Closing remarks
+----------------------------
+
+- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``.

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>3.15.0</version>
+  <version>3.16.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/src/controller_manager.cpp

@@ -610,6 +610,18 @@ controller_interface::return_type ControllerManager::configure_controller(
       controller_name, std::make_unique<ControllerThreadWrapper>(controller, update_rate_));
   }
 
+  const auto controller_update_rate = controller->get_update_rate();
+  const auto cm_update_rate = get_update_rate();
+  if (controller_update_rate > cm_update_rate)
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "The controller : %s update rate : %d Hz should be less than or equal to controller "
+      "manager's update rate : %d Hz!. The controller will be updated at controller_manager's "
+      "update rate.",
+      controller_name.c_str(), controller_update_rate, cm_update_rate);
+  }
+
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
   if (controller->is_chainable())
   {
@@ -1387,8 +1399,11 @@ void ControllerManager::activate_controllers(
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
     {
       RCLCPP_ERROR(
-        get_logger(), "After activating, controller '%s' is in state '%s', expected Active",
-        controller->get_node()->get_name(), new_state.label().c_str());
+        get_logger(),
+        "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
+        controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
+        hardware_interface::lifecycle_state_names::ACTIVE,
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
     }
 
     if (controller->is_async())
@@ -1873,8 +1888,9 @@ controller_interface::return_type ControllerManager::update(
     {
       const auto controller_update_rate = loaded_controller.c->get_update_rate();
 
-      bool controller_go =
-        controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0);
+      bool controller_go = controller_update_rate == 0 ||
+                           ((update_loop_counter_ % controller_update_rate) == 0) ||
+                           (controller_update_rate >= update_rate_);
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
         update_loop_counter_, controller_go ? "True" : "False",

controller_manager/test/test_controller/test_controller.hpp

@@ -65,7 +65,7 @@ class TestController : public controller_interface::ControllerInterface
   CONTROLLER_MANAGER_PUBLIC
   void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg);
 
-  size_t internal_counter = 0;
+  unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller
   // is usually destroyed after cleanup

controller_manager/test/test_controller_manager.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <gtest/gtest.h>
+#include <gmock/gmock.h>
 #include <memory>
 #include <string>
 #include <utility>
@@ -27,13 +27,13 @@
 using ::testing::_;
 using ::testing::Return;
 
-class TestControllerManager
+class TestControllerManagerWithStrictness
 : public ControllerManagerFixture<controller_manager::ControllerManager>,
   public testing::WithParamInterface<Strictness>
 {
 };
 
-TEST_P(TestControllerManager, controller_lifecycle)
+TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
 {
   const auto test_param = GetParam();
   auto test_controller = std::make_shared<test_controller::TestController>();
@@ -159,7 +159,7 @@ TEST_P(TestControllerManager, controller_lifecycle)
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_GE(test_controller->internal_counter, 1u);
-  auto last_internal_counter = test_controller->internal_counter;
+  size_t last_internal_counter = test_controller->internal_counter;
 
   // Stop controller, will take effect at the end of the update function
   start_controllers = {};
@@ -196,7 +196,7 @@ TEST_P(TestControllerManager, controller_lifecycle)
   EXPECT_EQ(1, test_controller.use_count());
 }
 
-TEST_P(TestControllerManager, per_controller_update_rate)
+TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 {
   auto strictness = GetParam().strictness;
   auto test_controller = std::make_shared<test_controller::TestController>();
@@ -254,4 +254,113 @@ TEST_P(TestControllerManager, per_controller_update_rate)
 }
 
 INSTANTIATE_TEST_SUITE_P(
-  test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort));
+  test_strict_best_effort, TestControllerManagerWithStrictness,
+  testing::Values(strict, best_effort));
+
+class TestControllerManagerWithUpdateRates
+: public ControllerManagerFixture<controller_manager::ControllerManager>,
+  public testing::WithParamInterface<unsigned int>
+{
+};
+
+TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_update_rate)
+{
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  const unsigned int ctrl_update_rate = GetParam();
+  auto test_controller = std::make_shared<test_controller::TestController>();
+
+  auto last_internal_counter = 0u;
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controller_manager"), "Testing update rate : %u Hz", ctrl_update_rate);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->add_controller(
+      test_controller, test_controller::TEST_CONTROLLER_NAME,
+      test_controller::TEST_CONTROLLER_CLASS_NAME);
+  }
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+    << "Update should not reach an unconfigured controller";
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+
+  rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
+  test_controller->get_node()->set_parameter(update_rate_parameter);
+  // configure controller
+  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+    << "Controller is not started";
+  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+
+  // Start controller, will take effect at the end of the update function
+  std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
+  std::vector<std::string> stop_controllers = {};
+  auto switch_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+  ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+    << "switch_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+    << "Controller is started at the end of update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+  }
+
+  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+
+  const auto pre_internal_counter = test_controller->internal_counter;
+  rclcpp::Rate loop_rate(cm_->get_update_rate());
+  for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++)
+  {
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+    loop_rate.sleep();
+  }
+  // if we do 2 times of the controller_manager update rate, the internal counter should be
+  // similarly incremented
+  EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate()));
+  EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
+
+  auto deactivate_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+  ASSERT_EQ(std::future_status::timeout, deactivate_future.wait_for(std::chrono::milliseconds(100)))
+    << "switch_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)))
+    << "Controller is stopped at the end of update, so it should have done one more update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, deactivate_future.get());
+  }
+  auto unload_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
+    test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100)))
+    << "unload_controller should be blocking until next update cycle";
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
+  last_internal_counter = test_controller->internal_counter;
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates,
+  testing::Values(100, 232, 400));

controller_manager/test/test_spawner_unspawner.cpp

@@ -96,6 +96,7 @@ TEST_F(TestLoadController, spawner_test_type_in_param)
 {
   cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
 
+  ControllerManagerRunner cm_runner(this);
   EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0);
 
   ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);