Merge branch 'master' into pass-urdf-to-controllers

.github/workflows/ci-format.yml

@@ -13,7 +13,7 @@ jobs:
     runs-on: ubuntu-latest
     steps:
     - uses: actions/checkout@v4
-    - uses: actions/[email protected].0
+    - uses: actions/[email protected].1
       with:
         python-version: '3.10'
     - name: Install system hooks

controller_interface/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.19.1 (2023-10-04)
+-------------------
+
+3.19.0 (2023-10-03)
+-------------------
+* Enable services for setting the log-level in controller per default (`#1102 <https://github.com/ros-controls/ros2_control/issues/1102>`_)
+* Contributors: Dr. Denis
+
 3.18.0 (2023-08-17)
 -------------------
 * add a broadcaster for range sensor (`#1091 <https://github.com/ros-controls/ros2_control/issues/1091>`_)

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.18.0</version>
+  <version>3.19.1</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,18 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.19.1 (2023-10-04)
+-------------------
+* Fix next controller update cycle time clock (`#1127 <https://github.com/ros-controls/ros2_control/issues/1127>`_)
+* Contributors: Sai Kishor Kothakota
+
+3.19.0 (2023-10-03)
+-------------------
+* Proper controller update rate (`#1105 <https://github.com/ros-controls/ros2_control/issues/1105>`_)
+* Fix multiple calls to export reference interfaces (`#1108 <https://github.com/ros-controls/ros2_control/issues/1108>`_)
+* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 <https://github.com/ros-controls/ros2_control/issues/1104>`_)
+* Contributors: Dr. Denis, Sai Kishor Kothakota
+
 3.18.0 (2023-08-17)
 -------------------
 * Controller sorting and proper execution in a chain (Fixes `#853 <https://github.com/ros-controls/ros2_control/issues/853>`_) (`#1063 <https://github.com/ros-controls/ros2_control/issues/1063>`_)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -58,7 +58,7 @@ namespace controller_manager
 {
 using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
 
-rclcpp::NodeOptions get_cm_node_options();
+CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options();
 
 class ControllerManager : public rclcpp::Node
 {
@@ -122,6 +122,7 @@ class ControllerManager : public rclcpp::Node
     controller_spec.c = controller;
     controller_spec.info.name = controller_name;
     controller_spec.info.type = controller_type;
+    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     return add_controller_impl(controller_spec);
   }
 

controller_manager/include/controller_manager/controller_spec.hpp

@@ -20,6 +20,7 @@
 #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 #include "controller_interface/controller_interface.hpp"
@@ -37,6 +38,7 @@ struct ControllerSpec
 {
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
+  std::shared_ptr<rclcpp::Time> next_update_cycle_time;
 };
 
 }  // namespace controller_manager

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.18.0</version>
+  <version>3.19.1</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

@@ -566,6 +566,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
+  controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
+    0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
   return add_controller_impl(controller_spec);
 }
@@ -741,16 +743,13 @@ controller_interface::return_type ControllerManager::configure_controller(
   }
   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);
+      "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the "
+      "controller's update rate : %d Hz is not a perfect divisor of the controller manager's "
+      "update rate : %d Hz!.",
+      controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate,
+      cm_update_rate);
   }
 
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
@@ -1459,6 +1458,9 @@ void ControllerManager::activate_controllers(
     }
     auto controller = found_it->c;
     auto controller_name = found_it->info.name;
+    // reset the next update cycle time for newly activated controllers
+    *found_it->next_update_cycle_time =
+      rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
     bool assignment_successful = true;
     // assign command interfaces to the controller
@@ -2031,30 +2033,40 @@ controller_interface::return_type ControllerManager::update(
   ++update_loop_counter_;
   update_loop_counter_ %= update_rate_;
 
-  for (auto loaded_controller : rt_controller_list)
+  for (const auto & loaded_controller : rt_controller_list)
   {
     // TODO(v-lopez) we could cache this information
     // https://github.com/ros-controls/ros2_control/issues/153
     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;
+      const bool run_controller_at_cm_rate =
+        (controller_update_rate == 0) || (controller_update_rate >= update_rate_);
+      const auto controller_period =
+        run_controller_at_cm_rate ? period
+                                  : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
+
+      bool controller_go =
+        (time ==
+         rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
+        (time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
 
-      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",
         loaded_controller.info.name.c_str());
 
       if (controller_go)
       {
-        auto controller_ret = loaded_controller.c->update(
-          time, (controller_update_factor != 1u)
-                  ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
-                  : period);
+        auto controller_ret = loaded_controller.c->update(time, controller_period);
+
+        if (
+          *loaded_controller.next_update_cycle_time ==
+          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+        {
+          *loaded_controller.next_update_cycle_time = time;
+        }
+        *loaded_controller.next_update_cycle_time += controller_period;
 
         if (controller_ret != controller_interface::return_type::OK)
         {

controller_manager/test/controller_manager_test_common.hpp

@@ -41,7 +41,6 @@
 
 namespace
 {
-const auto TIME = rclcpp::Time(0);
 const auto PERIOD = rclcpp::Duration::from_seconds(0.01);
 const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT;
 const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
@@ -101,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test
         cm_->robot_description_callback(msg);
       }
     }
+    time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type());
   }
 
   static void SetUpTestCase() { rclcpp::init(0, nullptr); }
@@ -119,7 +119,7 @@ class ControllerManagerFixture : public ::testing::Test
       {
         while (run_updater_)
         {
-          cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+          cm_->update(time_, rclcpp::Duration::from_seconds(0.01));
           std::this_thread::sleep_for(std::chrono::milliseconds(10));
         }
       });
@@ -157,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test
   bool run_updater_;
   const std::string robot_description_;
   const bool pass_urdf_as_parameter_;
+  rclcpp::Time time_;
 };
 
 class TestControllerManagerSrvs
@@ -177,9 +178,9 @@ class TestControllerManagerSrvs
       std::chrono::milliseconds(10),
       [&]()
       {
-        cm_->read(TIME, PERIOD);
-        cm_->update(TIME, PERIOD);
-        cm_->write(TIME, PERIOD);
+        cm_->read(time_, PERIOD);
+        cm_->update(time_, PERIOD);
+        cm_->write(time_, PERIOD);
       });
 
     executor_->add_node(cm_);
@@ -206,7 +207,7 @@ class TestControllerManagerSrvs
       while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) !=
              rclcpp::FutureReturnCode::SUCCESS)
       {
-        cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+        cm_->update(time_, rclcpp::Duration::from_seconds(0.01));
       }
     }
     else