Merge branch 'master' into feat/interface/remapping

controller_interface/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.1 (2024-08-24)
+-------------------
+
+4.16.0 (2024-08-22)
+-------------------
+* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 <https://github.com/ros-controls/ros2_control/issues/1701>`_)
+* Avoid using the global arguments for internal controller nodes (`#1694 <https://github.com/ros-controls/ros2_control/issues/1694>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.15.0 (2024-08-05)
 -------------------
 

controller_interface/include/controller_interface/async_controller.hpp

@@ -82,7 +82,8 @@ class AsyncControllerThread
       TimePoint next_iteration_time =
         TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));
 
-      if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+      if (
+        controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
       {
         auto const current_time = controller_->get_node()->now();
         auto const measured_period = current_time - previous_time;

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -148,7 +148,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;
 
   CONTROLLER_INTERFACE_PUBLIC
-  const rclcpp_lifecycle::State & get_state() const;
+  const rclcpp_lifecycle::State & get_lifecycle_state() const;
 
   CONTROLLER_INTERFACE_PUBLIC
   unsigned int get_update_rate() const;

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

controller_interface/src/chainable_controller_interface.cpp

@@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
 {
   bool result = false;
 
-  if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     result = on_set_chained_mode(chained_mode);
 
@@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
       get_node()->get_logger(),
       "Can not change controller's chained mode because it is no in '%s' state. "
       "Current state is '%s'.",
-      hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str());
+      hardware_interface::lifecycle_state_names::UNCONFIGURED,
+      get_lifecycle_state().label().c_str());
   }
 
   return result;

controller_interface/src/controller_interface_base.cpp

@@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
   // Then we don't need to do state-machine related checks.
   //
   // Other solution is to add check into the LifecycleNode if a transition is valid to trigger
-  if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  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());
     is_async_ = get_node()->get_parameter("is_async").as_bool();
@@ -169,7 +169,7 @@ void ControllerInterfaceBase::release_interfaces()
   state_interfaces_.clear();
 }
 
-const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const
+const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
 {
   return node_->get_current_state();
 }

controller_manager/CHANGELOG.rst

@@ -2,6 +2,26 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.1 (2024-08-24)
+-------------------
+* propage a portion of global args to the controller nodes (`#1712 <https://github.com/ros-controls/ros2_control/issues/1712>`_)
+* Contributors: Sai Kishor Kothakota
+
+4.16.0 (2024-08-22)
+-------------------
+* inform user what reason is for not setting rt policy, inform is policy (`#1705 <https://github.com/ros-controls/ros2_control/issues/1705>`_)
+* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 <https://github.com/ros-controls/ros2_control/issues/1701>`_)
+* Fix spawner tests timeout (`#1692 <https://github.com/ros-controls/ros2_control/issues/1692>`_)
+* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 <https://github.com/ros-controls/ros2_control/issues/1661>`_)
+* Robustify controller spawner and add integration test with many controllers (`#1501 <https://github.com/ros-controls/ros2_control/issues/1501>`_)
+* Handle waiting in Spawner and align Hardware Spawner functionality (`#1562 <https://github.com/ros-controls/ros2_control/issues/1562>`_)
+* Make list controller and list hardware components immediately visualize the state. (`#1606 <https://github.com/ros-controls/ros2_control/issues/1606>`_)
+* [CI] Add coveragepy and remove ignore: test (`#1668 <https://github.com/ros-controls/ros2_control/issues/1668>`_)
+* Fix spawner unload on kill test (`#1675 <https://github.com/ros-controls/ros2_control/issues/1675>`_)
+* [CM] Add more logging for easier debugging (`#1645 <https://github.com/ros-controls/ros2_control/issues/1645>`_)
+* refactor SwitchParams to fix the incosistencies in the spawner tests (`#1638 <https://github.com/ros-controls/ros2_control/issues/1638>`_)
+* Contributors: Bence Magyar, Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota
+
 4.15.0 (2024-08-05)
 -------------------
 * Add missing include for executors (`#1653 <https://github.com/ros-controls/ros2_control/issues/1653>`_)

controller_manager/controller_manager/spawner.py

@@ -234,7 +234,7 @@ def main(args=None):
 
             node.get_logger().info(
                 bcolors.OKGREEN
-                + "Configured and activated all the parsed controllers list!"
+                + f"Configured and activated all the parsed controllers list : {controller_names}!"
                 + bcolors.ENDC
             )
 
@@ -258,16 +258,25 @@ def main(args=None):
                     )
                     return 1
 
-                node.get_logger().info("Deactivated controller")
-
-            ret = unload_controller(node, controller_manager_name, controller_name)
-            if not ret.ok:
-                node.get_logger().error(
-                    bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
+                node.get_logger().info(
+                    f"Successfully deactivated controllers : {controller_names}"
                 )
-                return 1
 
-            node.get_logger().info("Unloaded controller")
+            unload_status = True
+            for controller_name in controller_names:
+                ret = unload_controller(node, controller_manager_name, controller_name)
+                if not ret.ok:
+                    unload_status = False
+                    node.get_logger().error(
+                        bcolors.FAIL
+                        + f"Failed to unload controller : {controller_name}"
+                        + bcolors.ENDC
+                    )
+
+            if unload_status:
+                node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
+            else:
+                return 1
         return 0
     except KeyboardInterrupt:
         pass

controller_manager/include/controller_manager/controller_manager.hpp

@@ -577,6 +577,7 @@ class ControllerManager : public rclcpp::Node
   std::map<std::string, std::vector<std::string>> controller_chained_reference_interfaces_cache_;
   std::map<std::string, std::vector<std::string>> controller_chained_state_interfaces_cache_;
 
+  rclcpp::NodeOptions cm_node_options_;
   std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;

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>4.15.0</version>
+  <version>4.16.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

@@ -24,6 +24,7 @@
 #include "controller_manager_msgs/msg/hardware_component_state.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "rcl/arguments.h"
 #include "rclcpp/version.h"
 #include "rclcpp_lifecycle/state.hpp"
 
@@ -57,7 +58,8 @@ static const rmw_qos_profile_t qos_services = {
 
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
+  return controller.get_lifecycle_state().id() ==
+         lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
 }
 
 inline bool is_controller_inactive(
@@ -68,7 +70,7 @@ inline bool is_controller_inactive(
 
 inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
+  return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
 }
 
 inline bool is_controller_active(
@@ -197,7 +199,8 @@ ControllerManager::ControllerManager(
     kControllerInterfaceNamespace, kControllerInterfaceClassName)),
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
+  cm_node_options_(options)
 {
   init_controller_manager();
 }
@@ -217,7 +220,8 @@ ControllerManager::ControllerManager(
     kControllerInterfaceNamespace, kControllerInterfaceClassName)),
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
+  cm_node_options_(options)
 {
   init_controller_manager();
 }
@@ -234,7 +238,8 @@ ControllerManager::ControllerManager(
     kControllerInterfaceNamespace, kControllerInterfaceClassName)),
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
+  cm_node_options_(options)
 {
   init_controller_manager();
 }
@@ -655,7 +660,7 @@ controller_interface::return_type ControllerManager::configure_controller(
   }
   auto controller = found_it->c;
 
-  auto state = controller->get_state();
+  auto state = controller->get_lifecycle_state();
   if (
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -666,7 +671,7 @@ controller_interface::return_type ControllerManager::configure_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  auto new_state = controller->get_state();
+  auto new_state = controller->get_lifecycle_state();
   if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     RCLCPP_DEBUG(
@@ -1752,7 +1757,7 @@ void ControllerManager::list_controllers_srv_cb(
     controller_state.name = controllers[i].info.name;
     controller_state.type = controllers[i].info.type;
     controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces;
-    controller_state.state = controllers[i].c->get_state().label();
+    controller_state.state = controllers[i].c->get_lifecycle_state().label();
     controller_state.is_chainable = controllers[i].c->is_chainable();
     controller_state.is_chained = controllers[i].c->is_in_chained_mode();
 
@@ -2736,7 +2741,7 @@ void ControllerManager::controller_activity_diagnostic_callback(
     {
       all_active = false;
     }
-    stat.add(controllers[i].info.name, controllers[i].c->get_state().label());
+    stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label());
   }
 
   if (all_active)
@@ -2831,29 +2836,56 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
 
   rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options();
   std::vector<std::string> node_options_arguments = controller_node_options.arguments();
-  const std::string ros_args_arg = "--ros-args";
+
+  for (const std::string & arg : cm_node_options_.arguments())
+  {
+    if (arg.find("__ns") != std::string::npos || arg.find("__node") != std::string::npos)
+    {
+      if (
+        node_options_arguments.back() == RCL_REMAP_FLAG ||
+        node_options_arguments.back() == RCL_SHORT_REMAP_FLAG)
+      {
+        node_options_arguments.pop_back();
+      }
+      continue;
+    }
+
+    node_options_arguments.push_back(arg);
+  }
+
   if (controller.info.parameters_file.has_value())
   {
-    if (!check_for_element(node_options_arguments, ros_args_arg))
+    if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG))
     {
-      node_options_arguments.push_back(ros_args_arg);
+      node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
     }
-    node_options_arguments.push_back("--params-file");
+    node_options_arguments.push_back(RCL_PARAM_FILE_FLAG);
     node_options_arguments.push_back(controller.info.parameters_file.value());
   }
 
   // ensure controller's `use_sim_time` parameter matches controller_manager's
   const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time");
   if (use_sim_time.as_bool())
   {
-    if (!check_for_element(node_options_arguments, ros_args_arg))
+    if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG))
     {
-      node_options_arguments.push_back(ros_args_arg);
+      node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
     }
-    node_options_arguments.push_back("-p");
+    node_options_arguments.push_back(RCL_PARAM_FLAG);
     node_options_arguments.push_back("use_sim_time:=true");
   }
 
+  std::string arguments;
+  arguments.reserve(1000);
+  for (const auto & arg : node_options_arguments)
+  {
+    arguments.append(arg);
+    arguments.append(" ");
+  }
+  RCLCPP_INFO(
+    get_logger(), "Controller '%s' node arguments: %s", controller.info.name.c_str(),
+    arguments.c_str());
+
   controller_node_options = controller_node_options.arguments(node_options_arguments);
   controller_node_options.use_global_arguments(false);
   return controller_node_options;

controller_manager/src/ros2_control_node.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <errno.h>
 #include <chrono>
 #include <memory>
 #include <string>
@@ -40,7 +41,22 @@ int main(int argc, char ** argv)
     std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   std::string manager_node_name = "controller_manager";
 
-  auto cm = std::make_shared<controller_manager::ControllerManager>(executor, manager_node_name);
+  rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options();
+  std::vector<std::string> node_arguments = cm_node_options.arguments();
+  for (int i = 1; i < argc; ++i)
+  {
+    if (node_arguments.empty() && std::string(argv[i]) != "--ros-args")
+    {
+      // A simple way to reject non ros args
+      continue;
+    }
+    RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]);
+    node_arguments.push_back(argv[i]);
+  }
+  cm_node_options.arguments(node_arguments);
+
+  auto cm = std::make_shared<controller_manager::ControllerManager>(
+    executor, manager_node_name, "", cm_node_options);
 
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
 
@@ -51,10 +67,16 @@ int main(int argc, char ** argv)
       {
         RCLCPP_WARN(
           cm->get_logger(),
-          "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT "
-          "scheduling. See "
+          "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See "
           "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
-          "for details.");
+          "for details on how to enable realtime scheduling.",
+          errno, strerror(errno));
+      }
+      else
+      {
+        RCLCPP_INFO(
+          cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.",
+          kSchedPriority);
       }
 
       // for calculating sleep time

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -33,8 +33,8 @@ controller_interface::InterfaceConfiguration
 TestChainableController::command_interface_configuration() const
 {
   if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     return cmd_iface_cfg_;
   }
@@ -49,8 +49,8 @@ controller_interface::InterfaceConfiguration
 TestChainableController::state_interface_configuration() const
 {
   if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     auto state_iface_cfg = state_iface_cfg_;
     if (imu_sensor_)