Merge branch 'master' into fix/exclusive_hw_interface_switching

Author: saikishor

.github/workflows/stale.yml

@@ -0,0 +1,27 @@
+name: 'Stale issues and PRs'
+on:
+  workflow_dispatch:
+  schedule:
+    # UTC noon every workday
+    - cron: '0 12 * * MON-FRI'
+
+jobs:
+  stale:
+    runs-on: ubuntu-latest
+    permissions:
+      issues: write
+      pull-requests: write
+    steps:
+      - uses: actions/stale@v9
+        with:
+          stale-issue-label: 'stale'
+          stale-pr-label: 'stale'
+          stale-issue-message: 'This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.'
+          close-issue-message: 'This issue was closed because it has been stalled for 45 days with no activity.'
+          stale-pr-message: 'This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.'
+          days-before-stale: 45
+          days-before-close: 45
+          days-before-pr-close: -1
+          exempt-all-milestones: true
+          exempt-issue-labels: good first issue,good second issue,persistent,release,roadmap,Epic
+          operations-per-run: 100

.pre-commit-config.yaml

@@ -56,14 +56,14 @@ repos:
         args: ["--line-length=99"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.1.2
+    rev: 7.2.0
     hooks:
     - id: flake8
       args: ["--extend-ignore=E501"]
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.7
+    rev: v20.1.0
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.31.2
+    rev: 0.32.1
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

MIGRATION.md

@@ -1,6 +1,6 @@
 # ros2_control
 
-[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
 [![codecov](https://codecov.io/gh/ros-controls/ros2_control/graph/badge.svg?token=idvm1zJXOf)](https://codecov.io/gh/ros-controls/ros2_control)
 
 This package is a part of the ros2_control framework.

README.md

@@ -1,14 +1,9 @@
 cmake_minimum_required(VERSION 3.16)
 project(controller_interface LANGUAGES CXX)
 
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
-  add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow
-          -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   hardware_interface

controller_interface/CMakeLists.txt

@@ -306,6 +306,19 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    */
   void wait_for_trigger_update_to_finish();
 
+  /**
+   * Method to prepare the controller for deactivation. This method is called by the controller
+   * manager before deactivating the controller. The method is used to prepare the controller for
+   * deactivation, e.g., to stop triggering the update cycles further. This method is especially
+   * needed for controllers running in async mode and different frequency than the control manager.
+   *
+   * \note **The method is not real-time safe and shouldn't be called in the RT control loop.**
+   *
+   * If the controller is running in async mode, the method will stop the async update cycles. If
+   * the controller is not running in async mode, the method will do nothing.
+   */
+  void prepare_for_deactivation();
+
   std::string get_name() const;
 
   /// Enable or disable introspection of the controller.
@@ -319,11 +332,18 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
 
 private:
+  /**
+   * Method to stop the async handler thread. This method is called before the controller cleanup,
+   * error and shutdown lifecycle transitions.
+   */
+  void stop_async_handler_thread();
+
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
   unsigned int update_rate_ = 0;
   bool is_async_ = false;
   std::string urdf_ = "";
+  std::atomic_bool skip_async_triggers_ = false;
   ControllerUpdateStats trigger_stats_;
 
 protected:

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -65,9 +65,9 @@ class LedRgbDevice : public SemanticComponentCommandInterface<std_msgs::msg::Col
       return false;
     }
     bool all_set = true;
-    all_set &= command_interfaces_[0].get().set_value(message.r);
-    all_set &= command_interfaces_[1].get().set_value(message.g);
-    all_set &= command_interfaces_[2].get().set_value(message.b);
+    all_set &= command_interfaces_[0].get().set_value(static_cast<double>(message.r));
+    all_set &= command_interfaces_[1].get().set_value(static_cast<double>(message.g));
+    all_set &= command_interfaces_[2].get().set_value(static_cast<double>(message.b));
     return all_set;
   }
 };

controller_interface/include/semantic_components/led_rgb_device.hpp

@@ -3,17 +3,20 @@
 <package format="2">
   <name>controller_interface</name>
   <version>4.27.0</version>
-  <description>Description of controller_interface</description>
+  <description>Base classes for controllers and syntax cookies for supporting common sensor types in controllers and broadcasters</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>
+  <maintainer email="[email protected]">Christoph Froehlich</maintainer>
+  <maintainer email="[email protected]">Sai Kishor Kothakota</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>
 
   <build_depend>hardware_interface</build_depend>
-  <build_depend>realtime_tools</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>ros2_control_cmake</build_depend>
   <build_depend>sensor_msgs</build_depend>
 
   <build_export_depend>hardware_interface</build_export_depend>

controller_interface/package.xml

@@ -247,8 +247,9 @@ ChainableControllerInterface::on_export_reference_interfaces()
   std::vector<hardware_interface::CommandInterface> reference_interfaces;
   for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i)
   {
-    reference_interfaces.emplace_back(hardware_interface::CommandInterface(
-      get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i]));
+    reference_interfaces.emplace_back(
+      hardware_interface::CommandInterface(
+        get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i]));
   }
   return reference_interfaces;
 }

controller_interface/src/chainable_controller_interface.cpp

@@ -86,16 +86,14 @@ return_type ControllerInterfaceBase::init(
       // make sure introspection is disabled on controller cleanup as users may manually enable
       // it in `on_configure` and `on_deactivate` - see the docs for details
       enable_introspection(false);
-      if (is_async() && async_handler_ && async_handler_->is_running())
-      {
-        async_handler_->stop_thread();
-      }
+      this->stop_async_handler_thread();
       return on_cleanup(previous_state);
     });
 
   node_->register_on_activate(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
+      skip_async_triggers_.store(false);
       enable_introspection(true);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
@@ -113,10 +111,22 @@ return_type ControllerInterfaceBase::init(
     });
 
   node_->register_on_shutdown(
-    std::bind(&ControllerInterfaceBase::on_shutdown, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      this->stop_async_handler_thread();
+      auto transition_state_status = on_shutdown(previous_state);
+      this->release_interfaces();
+      return transition_state_status;
+    });
 
   node_->register_on_error(
-    std::bind(&ControllerInterfaceBase::on_error, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      this->stop_async_handler_thread();
+      auto transition_state_status = on_error(previous_state);
+      this->release_interfaces();
+      return transition_state_status;
+    });
 
   return return_type::OK;
 }
@@ -204,6 +214,13 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
   trigger_stats_.total_triggers++;
   if (is_async())
   {
+    if (skip_async_triggers_.load())
+    {
+      // Skip further async triggers if the controller is being deactivated
+      status.successful = false;
+      status.result = return_type::OK;
+      return status;
+    }
     const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time();
     const auto result = async_handler_->trigger_async_callback(time, period);
     if (!result.first)
@@ -270,6 +287,20 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
   }
 }
 
+void ControllerInterfaceBase::prepare_for_deactivation()
+{
+  skip_async_triggers_.store(true);
+  this->wait_for_trigger_update_to_finish();
+}
+
+void ControllerInterfaceBase::stop_async_handler_thread()
+{
+  if (is_async() && async_handler_ && async_handler_->is_running())
+  {
+    async_handler_->stop_thread();
+  }
+}
+
 std::string ControllerInterfaceBase::get_name() const { return get_node()->get_name(); }
 
 void ControllerInterfaceBase::enable_introspection(bool enable)

controller_interface/src/controller_interface_base.cpp

@@ -14,9 +14,10 @@
 
 #include "test_chainable_controller_interface.hpp"
 
-#include <gmock/gmock.h>
 #include <memory>
 
+#include "gmock/gmock.h"
+
 using ::testing::IsEmpty;
 using ::testing::SizeIs;
 

controller_interface/test/test_chainable_controller_interface.cpp

@@ -15,12 +15,11 @@
 #ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
 #define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
 
-#include <gmock/gmock.h>
-
 #include <string>
 #include <vector>
 
 #include "controller_interface/chainable_controller_interface.hpp"
+#include "gmock/gmock.h"
 #include "hardware_interface/handle.hpp"
 
 constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller";
@@ -71,8 +70,9 @@ class TestableChainableControllerInterface
   {
     std::vector<hardware_interface::StateInterface> state_interfaces;
 
-    state_interfaces.push_back(hardware_interface::StateInterface(
-      name_prefix_of_interfaces_, "test_state", &state_interfaces_values_[0]));
+    state_interfaces.push_back(
+      hardware_interface::StateInterface(
+        name_prefix_of_interfaces_, "test_state", &state_interfaces_values_[0]));
 
     return state_interfaces;
   }
@@ -82,8 +82,9 @@ class TestableChainableControllerInterface
   {
     std::vector<hardware_interface::CommandInterface> command_interfaces;
 
-    command_interfaces.push_back(hardware_interface::CommandInterface(
-      name_prefix_of_interfaces_, "test_itf", &reference_interfaces_[0]));
+    command_interfaces.push_back(
+      hardware_interface::CommandInterface(
+        name_prefix_of_interfaces_, "test_itf", &reference_interfaces_[0]));
 
     return command_interfaces;
   }

controller_interface/test/test_chainable_controller_interface.hpp

@@ -14,11 +14,11 @@
 
 #include "test_controller_interface.hpp"
 
-#include <gmock/gmock.h>
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "gmock/gmock.h"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/executor_options.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"

controller_interface/test/test_controller_interface.cpp

@@ -14,9 +14,10 @@
 
 #include "test_controller_with_options.hpp"
 
-#include <gtest/gtest.h>
 #include <string>
 
+#include "gmock/gmock.h"
+
 class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions
 {
   FRIEND_TEST(ControllerWithOption, init_with_overrides);

controller_interface/test/test_controller_with_options.cpp

@@ -39,9 +39,10 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names)
   ASSERT_EQ(force_torque_sensor_->state_interfaces_.capacity(), size_);
 
   // validate the default interface_names_
-  ASSERT_TRUE(std::equal(
-    force_torque_sensor_->interface_names_.begin(), force_torque_sensor_->interface_names_.end(),
-    full_interface_names_.begin(), full_interface_names_.end()));
+  ASSERT_TRUE(
+    std::equal(
+      force_torque_sensor_->interface_names_.begin(), force_torque_sensor_->interface_names_.end(),
+      full_interface_names_.begin(), full_interface_names_.end()));
 
   // get the interface names
   std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();

controller_interface/test/test_force_torque_sensor.cpp

@@ -19,12 +19,11 @@
 #ifndef TEST_FORCE_TORQUE_SENSOR_HPP_
 #define TEST_FORCE_TORQUE_SENSOR_HPP_
 
-#include <gmock/gmock.h>
-
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "gmock/gmock.h"
 #include "semantic_components/force_torque_sensor.hpp"
 
 // implementing and friending so we can access member variables

controller_interface/test/test_force_torque_sensor.hpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <gmock/gmock.h>
-#include <gtest/gtest.h>
 #include <algorithm>
 #include <array>
 #include <string>
 #include <vector>
+
+#include "gmock/gmock.h"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "semantic_components/gps_sensor.hpp"

controller_interface/test/test_gps_sensor.cpp

@@ -38,9 +38,10 @@ TEST_F(IMUSensorTest, validate_all)
   ASSERT_EQ(imu_sensor_->state_interfaces_.capacity(), size_);
 
   // validate the default interface_names_
-  ASSERT_TRUE(std::equal(
-    imu_sensor_->interface_names_.begin(), imu_sensor_->interface_names_.end(),
-    full_interface_names_.begin(), full_interface_names_.end()));
+  ASSERT_TRUE(
+    std::equal(
+      imu_sensor_->interface_names_.begin(), imu_sensor_->interface_names_.end(),
+      full_interface_names_.begin(), full_interface_names_.end()));
 
   // get the interface names
   std::vector<std::string> interface_names = imu_sensor_->get_state_interface_names();