Merge branch 'master' into controller_sorting

.docker/README.md

@@ -3,7 +3,7 @@
 Meant to provide basic docker containers for CI or development purposes. To use them, make sure you have [Docker](https://docs.docker.com/get-docker/) installed. You then can pull the latest source or build image or build any version and run the image by following tag specific commands as described below:
 
 ## Source folder and tag
-Downloads and builds ros2_controls into a workspace for development use exactly as is found [here](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). This is primarily used for development and CI of ros2_control and related packages.
+Downloads and builds ros2_controls into a workspace for development use exactly as is found [here](https://control.ros.org/master/doc/getting_started/getting_started.html#building-from-source). This is primarily used for development and CI of ros2_control and related packages.
 
 You can pull a prebuilt image with this docker tag: `ghcr.io/ros-controls/ros2_control_source`.
 
@@ -30,7 +30,7 @@ docker run -it --mount type=volume,source=name-of-volume,destination=/root/ws_ro
 Note: this is probably the preferred solution as changes persist across rebuilds of container and doesn't require you to pull all repositories that you want to edit as they'll already be in the container.
 
 ## Release folder and tag
-Installs ros2_control in the base ros2 docker container as mentioned [here](https://ros-controls.github.io/control.ros.org/getting_started.html#getting-started). This is mainly intended for development of external packages that rely on ros2_control.
+Installs ros2_control in the base ros2 docker container as mentioned [here](https://control.ros.org/master/doc/getting_started/getting_started.html#building-from-source). This is mainly intended for development of external packages that rely on ros2_control.
 
 You can pull a prebuilt image using the following tag: `ghcr.io/ros2-controls/ros2_control_release`.
 

.github/ISSUE_TEMPLATE/good-first-issue.md

@@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa
 
 - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along!
 
-- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling).
+- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step 3 use "Download Source Code" section with [these instructions](https://control.ros.org/master/doc/getting_started/getting_started.html#building-from-source).
 
 - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_control`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_control` folder before cloning your own fork)
 
@@ -53,7 +53,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa
 
 Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below!
 Furthermore, you find helpful resources here:
-* [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html)
+* [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html)
 * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html)
 * [ROS Answers](https://answers.ros.org/questions/)
 

.github/workflows/ci-coverage-build.yml

@@ -16,11 +16,11 @@ jobs:
     env:
       ROS_DISTRO: rolling
     steps:
-      - uses: ros-tooling/setup-ros@0.6.2
+      - uses: ros-tooling/setup-ros@0.7.0
         with:
           required-ros-distributions: ${{ env.ROS_DISTRO }}
       - uses: actions/checkout@v3
-      - uses: ros-tooling/[email protected].2
+      - uses: ros-tooling/[email protected].3
         with:
           target-ros2-distro: ${{ env.ROS_DISTRO }}
           import-token: ${{ secrets.GITHUB_TOKEN }}

.github/workflows/ci-format.yml

@@ -13,7 +13,7 @@ jobs:
     runs-on: ubuntu-latest
     steps:
     - uses: actions/checkout@v3
-    - uses: actions/setup-python@v4.6.1
+    - uses: actions/setup-python@v4.7.0
       with:
         python-version: '3.10'
     - name: Install system hooks

.github/workflows/ci-ros-lint.yml

@@ -5,14 +5,16 @@ on:
 jobs:
   ament_lint:
     name: ament_${{ matrix.linter }}
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
           linter: [cppcheck, copyright, lint_cmake]
+    env:
+      AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true
     steps:
     - uses: actions/checkout@v3
-    - uses: ros-tooling/setup-ros@0.6.2
+    - uses: ros-tooling/setup-ros@0.7.0
     - uses: ros-tooling/[email protected]
       with:
         distribution: rolling
@@ -29,14 +31,14 @@ jobs:
 
   ament_lint_100:
     name: ament_${{ matrix.linter }}
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
           linter: [cpplint]
     steps:
     - uses: actions/checkout@v3
-    - uses: ros-tooling/setup-ros@0.6.2
+    - uses: ros-tooling/setup-ros@0.7.0
     - uses: ros-tooling/[email protected]
       with:
         distribution: rolling

.github/workflows/reusable-ros-tooling-source-build.yml

@@ -26,13 +26,13 @@ jobs:
     strategy:
       fail-fast: false
     steps:
-      - uses: ros-tooling/setup-ros@0.6.2
+      - uses: ros-tooling/setup-ros@0.7.0
         with:
           required-ros-distributions: ${{ inputs.ros_distro }}
       - uses: actions/checkout@v3
         with:
           ref: ${{ inputs.ref }}
-      - uses: ros-tooling/[email protected].2
+      - uses: ros-tooling/[email protected].3
         with:
           target-ros2-distro: ${{ inputs.ros_distro }}
           # build all packages listed in the meta package

CONTRIBUTING.md

@@ -21,7 +21,7 @@ Please try to include as much information as you can. Details like these are inc
 
 
 ## Contributing via Pull Requests
-The following guidance should be up-to-date, but the documentation as found [here](https://ros-controls.github.io/control.ros.org/contributing.html#pull-requests) should prove as the final say.
+The following guidance should be up-to-date, but the documentation as found [here](https://control.ros.org/master/doc/contributing/contributing.html#pull-requests) should prove as the final say.
 
 Contributions via pull requests are much appreciated.
 Before sending us a pull request, please ensure that:

README.md

@@ -3,7 +3,7 @@
 [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
 
 This package is a part of the ros2_control framework.
-For more, please check the [documentation](https://ros-controls.github.io/control.ros.org/).
+For more, please check the [documentation](https://control.ros.org/).
 
 
 ## Build status

controller_interface/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.17.0 (2023-08-07)
+-------------------
+
+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.17.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,20 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.17.0 (2023-08-07)
+-------------------
+* [CM] Fixes the issue with individual controller's update rate (`#1082 <https://github.com/ros-controls/ros2_control/issues/1082>`_)
+* Fix deprecation warning (`#1093 <https://github.com/ros-controls/ros2_control/issues/1093>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
+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/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.17.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

@@ -412,9 +412,9 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
     RCLCPP_WARN(
       get_logger(),
       "Parameter 'configure_components_on_start' is deprecated. "
-      "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial "
+      "Use 'hardware_components_initial_state.inactive' instead, to set component's initial "
       "state to 'inactive'. Don't use this parameters in combination with the new "
-      "'hardware_interface_state_after_start' parameter structure.");
+      "'hardware_components_initial_state' parameter structure.");
     set_components_to_state(
       "configure_components_on_start",
       rclcpp_lifecycle::State(
@@ -440,7 +440,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       get_logger(),
       "Parameter 'activate_components_on_start' is deprecated. "
       "Components are activated per default. Don't use this parameters in combination with the new "
-      "'hardware_interface_state_after_start' parameter structure.");
+      "'hardware_components_initial_state' parameter structure.");
     rclcpp_lifecycle::State active_state(
       State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
     for (const auto & component : activate_components_on_start)
@@ -738,6 +738,19 @@ controller_interface::return_type ControllerManager::configure_controller(
       "update rate.",
       controller_name.c_str(), controller_update_rate, cm_update_rate);
   }
+  else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0)
+  {
+    // NOTE: The following computation is done to compute the approx controller update that can be
+    // achieved w.r.t to the CM's update rate. This is done this way to take into account the
+    // unsigned integer division.
+    const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate);
+    RCLCPP_WARN(
+      get_logger(),
+      "The controller : %s update rate : %d Hz is not a perfect divisor of the controller "
+      "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's "
+      "update rate which is : %d Hz.",
+      controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate);
+  }
 
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
   if (controller->is_chainable())
@@ -2018,10 +2031,12 @@ controller_interface::return_type ControllerManager::update(
     if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c))
     {
       const auto controller_update_rate = loaded_controller.c->get_update_rate();
+      const auto controller_update_factor =
+        (controller_update_rate == 0) || (controller_update_rate >= update_rate_)
+          ? 1u
+          : update_rate_ / controller_update_rate;
 
-      bool controller_go = controller_update_rate == 0 ||
-                           ((update_loop_counter_ % controller_update_rate) == 0) ||
-                           (controller_update_rate >= update_rate_);
+      bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0);
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
         update_loop_counter_, controller_go ? "True" : "False",
@@ -2030,7 +2045,7 @@ controller_interface::return_type ControllerManager::update(
       if (controller_go)
       {
         auto controller_ret = loaded_controller.c->update(
-          time, (controller_update_rate != update_rate_ && controller_update_rate != 0)
+          time, (controller_update_factor != 1u)
                   ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
                   : period);
 

controller_manager/test/test_controller_manager.cpp

@@ -246,9 +246,13 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
 
-  EXPECT_EQ(
-    controller_interface::return_type::OK,
-    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle
+  for (size_t i = 0; i < 25; i++)
+  {
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  }
   EXPECT_GE(test_controller->internal_counter, 1u);
   EXPECT_EQ(test_controller->get_update_rate(), 4u);
 }
@@ -364,3 +368,82 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
 INSTANTIATE_TEST_SUITE_P(
   per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates,
   testing::Values(100, 232, 400));
+
+class TestControllerUpdateRates
+: public ControllerManagerFixture<controller_manager::ControllerManager>,
+  public testing::WithParamInterface<unsigned int>
+{
+};
+
+TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
+{
+  const unsigned int ctrl_update_rate = GetParam();
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  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(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+
+  test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
+  // 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(0u, 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
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  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(0u, 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());
+
+  EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
+  const auto cm_update_rate = cm_->get_update_rate();
+  const auto controller_update_rate = test_controller->get_update_rate();
+  const auto controller_factor = (cm_update_rate / controller_update_rate);
+  const auto expected_controller_update_rate =
+    static_cast<unsigned int>(std::round(cm_update_rate / static_cast<double>(controller_factor)));
+
+  const auto initial_counter = test_controller->internal_counter;
+  for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter)
+  {
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+
+    if (update_counter % cm_update_rate == 0)
+    {
+      const auto no_of_secs_passed = update_counter / cm_update_rate;
+      EXPECT_EQ(
+        test_controller->internal_counter - initial_counter,
+        (expected_controller_update_rate * no_of_secs_passed));
+    }
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  per_controller_update_rate_check, TestControllerUpdateRates,
+  testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.17.0 (2023-08-07)
+-------------------
+
+3.16.0 (2023-07-09)
+-------------------
+
 3.15.0 (2023-06-23)
 -------------------
 

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>3.15.0</version>
+  <version>3.17.0</version>
   <description>Messages and services for the controller manager.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

hardware_interface/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.17.0 (2023-08-07)
+-------------------
+* Add checks if hardware is initialized. (`#1054 <https://github.com/ros-controls/ros2_control/issues/1054>`_)
+* Contributors: Dr. Denis
+
+3.16.0 (2023-07-09)
+-------------------
+
 3.15.0 (2023-06-23)
 -------------------
 * Enable setting of initial state in HW compoments (`#1046 <https://github.com/ros-controls/ros2_control/issues/1046>`_)

hardware_interface/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface</name>
-  <version>3.15.0</version>
+  <version>3.17.0</version>
   <description>ros2_control hardware interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>