Merge branch 'master' into cm/service_timeout

controller_interface/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+* reset the async variables upon activation to work post exceptions (`#1860 <https://github.com/ros-controls/ros2_control/issues/1860>`_)
+* [CM] Fix controller missing update cycles in a real setup (`#1774 <https://github.com/ros-controls/ros2_control/issues/1774>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.19.0 (2024-10-26)
 -------------------
 * [CM] Async Function Handler for Controllers (`#1489 <https://github.com/ros-controls/ros2_control/issues/1489>`_)

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

controller_interface/src/controller_interface_base.cpp

@@ -67,7 +67,15 @@ return_type ControllerInterfaceBase::init(
     });
 
   node_->register_on_activate(
-    std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      if (is_async() && async_handler_ && async_handler_->is_running())
+      {
+        // This is needed if it is disabled due to a thrown exception in the async callback thread
+        async_handler_->reset_variables();
+      }
+      return on_activate(previous_state);
+    });
 
   node_->register_on_deactivate(
     std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1));

controller_manager/CHANGELOG.rst

@@ -2,6 +2,20 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+* change from thread_priority.hpp to realtime_helpers.hpp (`#1829 <https://github.com/ros-controls/ros2_control/issues/1829>`_)
+* Use Clock instead of Rate for backward compatibility of rolling (`#1864 <https://github.com/ros-controls/ros2_control/issues/1864>`_)
+* [ros2_control_node] Handle simulation environment clocks (`#1810 <https://github.com/ros-controls/ros2_control/issues/1810>`_)
+* [CM] Fix controller missing update cycles in a real setup (`#1774 <https://github.com/ros-controls/ros2_control/issues/1774>`_)
+* [ros2_control_node] Add option to set the CPU affinity  (`#1852 <https://github.com/ros-controls/ros2_control/issues/1852>`_)
+* [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (`#1822 <https://github.com/ros-controls/ros2_control/issues/1822>`_)
+* Fix CMP0115 (`#1830 <https://github.com/ros-controls/ros2_control/issues/1830>`_)
+* fix: typo use thread_priority (`#1844 <https://github.com/ros-controls/ros2_control/issues/1844>`_)
+* Fix Hardware spawner and add tests for it (`#1759 <https://github.com/ros-controls/ros2_control/issues/1759>`_)
+* add thread_priority option to the ros2_control_node (`#1820 <https://github.com/ros-controls/ros2_control/issues/1820>`_)
+* Contributors: Baris Yazici, Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota
+
 4.19.0 (2024-10-26)
 -------------------
 * Fix timeout value in std output (`#1807 <https://github.com/ros-controls/ros2_control/issues/1807>`_)

controller_manager/controller_manager/hardware_spawner.py

@@ -67,18 +67,15 @@ def handle_set_component_state_service_call(
     response = set_hardware_component_state(node, controller_manager_name, component, target_state)
     if response.ok and response.state == target_state:
         node.get_logger().info(
-            bcolors.OKGREEN
-            + f"{action} component '{component}'. Hardware now in state: {response.state}."
+            f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}"
         )
     elif response.ok and not response.state == target_state:
         node.get_logger().warn(
-            bcolors.WARNING
-            + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'."
+            f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}"
         )
     else:
         node.get_logger().warn(
-            bcolors.WARNING
-            + f"Could not {action} component '{component}'. Service call failed. Wrong component name?"
+            f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}"
         )
 
 
@@ -160,24 +157,22 @@ def main(args=None):
                 node, controller_manager_name, hardware_component, controller_manager_timeout
             ):
                 node.get_logger().warn(
-                    bcolors.WARNING
-                    + "Hardware Component is not loaded - state can not be changed."
-                    + bcolors.ENDC
+                    f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}"
                 )
             elif activate:
                 activate_component(node, controller_manager_name, hardware_component)
             elif configure:
                 configure_component(node, controller_manager_name, hardware_component)
             else:
                 node.get_logger().error(
-                    'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
+                    f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}'
                 )
                 parser.print_help()
                 return 0
     except KeyboardInterrupt:
         pass
     except ServiceNotFoundError as err:
-        node.get_logger().fatal(str(err))
+        node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}")
         return 1
     finally:
         rclpy.shutdown()

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.19.0</version>
+  <version>4.20.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/ros2_control_node.cpp

@@ -20,7 +20,7 @@
 
 #include "controller_manager/controller_manager.hpp"
 #include "rclcpp/executors.hpp"
-#include "realtime_tools/thread_priority.hpp"
+#include "realtime_tools/realtime_helpers.hpp"
 
 using namespace std::chrono_literals;
 
@@ -58,7 +58,6 @@ int main(int argc, char ** argv)
     executor, manager_node_name, "", cm_node_options);
 
   const bool use_sim_time = cm->get_parameter_or("use_sim_time", false);
-  rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock());
 
   const bool lock_memory = cm->get_parameter_or<bool>("lock_memory", true);
   std::string message;
@@ -85,7 +84,7 @@ int main(int argc, char ** argv)
     thread_priority);
 
   std::thread cm_thread(
-    [cm, thread_priority, use_sim_time, &rate]()
+    [cm, thread_priority, use_sim_time]()
     {
       if (!realtime_tools::configure_sched_fifo(thread_priority))
       {
@@ -128,7 +127,7 @@ int main(int argc, char ** argv)
         next_iteration_time += period;
         if (use_sim_time)
         {
-          rate.sleep();
+          cm->get_clock()->sleep_until(current_time + period);
         }
         else
         {

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+
 4.19.0 (2024-10-26)
 -------------------
 

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>4.19.0</version>
+  <version>4.20.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>

doc/release_notes.rst

@@ -114,6 +114,7 @@ hardware_interface
 * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 <https://github.com/ros-controls/ros2_control/pull/1643>`_)
 * With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
 * With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
+* With (`#1763 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.
 
 joint_limits
 ************

hardware_interface/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+* Add Support for SDF (`#1763 <https://github.com/ros-controls/ros2_control/issues/1763>`_)
+* [HW_IF] Prepare the handles for async operations (`#1750 <https://github.com/ros-controls/ros2_control/issues/1750>`_)
+* Contributors: Aarav Gupta, Sai Kishor Kothakota
+
 4.19.0 (2024-10-26)
 -------------------
 * [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 <https://github.com/ros-controls/ros2_control/issues/1767>`_)

hardware_interface/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface</name>
-  <version>4.19.0</version>
+  <version>4.20.0</version>
   <description>ros2_control hardware interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>
@@ -18,6 +18,7 @@
   <depend>tinyxml2_vendor</depend>
   <depend>joint_limits</depend>
   <depend>urdf</depend>
+  <depend>sdformat_urdf</depend>
 
   <build_depend>rcutils</build_depend>
   <exec_depend>rcutils</exec_depend>

hardware_interface/src/component_parser.cpp

@@ -31,6 +31,8 @@
 namespace
 {
 constexpr const auto kRobotTag = "robot";
+constexpr const auto kSDFTag = "sdf";
+constexpr const auto kModelTag = "model";
 constexpr const auto kROS2ControlTag = "ros2_control";
 constexpr const auto kHardwareTag = "hardware";
 constexpr const auto kPluginNameTag = "plugin";
@@ -812,15 +814,26 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
       "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr()));
   }
 
-  // Find robot tag
+  // Find robot or sdf tag
   const tinyxml2::XMLElement * robot_it = doc.RootElement();
+  const tinyxml2::XMLElement * ros2_control_it;
 
-  if (std::string(kRobotTag) != robot_it->Name())
+  if (std::string(kRobotTag) == robot_it->Name())
   {
-    throw std::runtime_error("the robot tag is not root element in URDF");
+    ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag);
+  }
+  else if (std::string(kSDFTag) == robot_it->Name())
+  {
+    // find model tag in sdf tag
+    const tinyxml2::XMLElement * model_it = robot_it->FirstChildElement(kModelTag);
+    ros2_control_it = model_it->FirstChildElement(kROS2ControlTag);
+  }
+  else
+  {
+    throw std::runtime_error(
+      "the robot tag is not root element in URDF or sdf tag is not root element in SDF");
   }
 
-  const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag);
   if (!ros2_control_it)
   {
     throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag");

hardware_interface/test/test_component_parser.cpp

@@ -1616,3 +1616,38 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw
   EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum");
   EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
 }
+
+TEST_F(TestComponentParser, successfully_parse_valid_sdf)
+{
+  std::string sdf_to_test = ros2_control_test_assets::diff_drive_robot_sdf;
+  const auto control_hardware = parse_control_resources_from_urdf(sdf_to_test);
+  ASSERT_THAT(control_hardware, SizeIs(1));
+  const auto hardware_info = control_hardware.front();
+
+  EXPECT_EQ(hardware_info.name, "GazeboSimSystem");
+  EXPECT_EQ(hardware_info.type, "system");
+  ASSERT_THAT(hardware_info.group, IsEmpty());
+  EXPECT_EQ(hardware_info.hardware_plugin_name, "gz_ros2_control/GazeboSimSystem");
+
+  ASSERT_THAT(hardware_info.joints, SizeIs(2));
+
+  EXPECT_EQ(hardware_info.joints[0].name, "left_wheel_joint");
+  EXPECT_EQ(hardware_info.joints[0].type, "joint");
+  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-10");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "10");
+  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(2));
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_POSITION);
+
+  EXPECT_EQ(hardware_info.joints[1].name, "right_wheel_joint");
+  EXPECT_EQ(hardware_info.joints[1].type, "joint");
+  ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-10");
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "10");
+  ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(2));
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_POSITION);
+}

hardware_interface_testing/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package hardware_interface_testing
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+
 4.19.0 (2024-10-26)
 -------------------
 

hardware_interface_testing/package.xml

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

joint_limits/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package joint_limits
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+
 4.19.0 (2024-10-26)
 -------------------
 

joint_limits/package.xml

@@ -1,6 +1,6 @@
 <package format="3">
   <name>joint_limits</name>
-  <version>4.19.0</version>
+  <version>4.20.0</version>
   <description>Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

ros2_control/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ros2_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+
 4.19.0 (2024-10-26)
 -------------------
 

ros2_control/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ros2_control</name>
-  <version>4.19.0</version>
+  <version>4.20.0</version>
   <description>Metapackage for ROS2 control related packages</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

ros2_control_test_assets/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package ros2_control_test_assets
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+* Add Support for SDF (`#1763 <https://github.com/ros-controls/ros2_control/issues/1763>`_)
+* Contributors: Aarav Gupta
+
 4.19.0 (2024-10-26)
 -------------------