diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 560ac37cff..96938467f9 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 30a83e5367..c2d137bde0 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [iron] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 9634732cf9..12769eb740 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [jazzy] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..b55091521e --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,44 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..621ffb6a26 --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,44 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 4cdb7ab585..492da45ab9 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [rolling] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index be2de417d6..665b0175f6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/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 `_) +* [CM] Fix controller missing update cycles in a real setup (`#1774 `_) +* Contributors: Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * [CM] Async Function Handler for Controllers (`#1489 `_) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 55c47473f0..ca3fc2af49 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.19.0 + 4.20.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a6e0da988f..0713ec3c25 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/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)); diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 7d9fb12a0d..376b15014d 100644 --- a/controller_manager/CHANGELOG.rst +++ b/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 `_) +* Use Clock instead of Rate for backward compatibility of rolling (`#1864 `_) +* [ros2_control_node] Handle simulation environment clocks (`#1810 `_) +* [CM] Fix controller missing update cycles in a real setup (`#1774 `_) +* [ros2_control_node] Add option to set the CPU affinity (`#1852 `_) +* [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (`#1822 `_) +* Fix CMP0115 (`#1830 `_) +* fix: typo use thread_priority (`#1844 `_) +* Fix Hardware spawner and add tests for it (`#1759 `_) +* add thread_priority option to the ros2_control_node (`#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 `_) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 323e02584a..4f7afe714c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/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,9 +157,7 @@ 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) @@ -170,14 +165,14 @@ def main(args=None): 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() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index c024a071f6..81bfbd7fc1 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -115,7 +115,16 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=0, + default=0.0, + type=float, + ) + parser.add_argument( + "--switch-timeout", + help="Time to wait for a successful state switch of controllers." + " Useful when switching cannot be performed immediately, e.g.," + " paused simulations at startup", + required=False, + default=5.0, type=float, ) parser.add_argument( @@ -132,6 +141,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_files = args.param_file controller_manager_timeout = args.controller_manager_timeout + switch_timeout = args.switch_timeout if param_files: for param_file in param_files: @@ -211,7 +221,13 @@ def main(args=None): if not args.inactive and not args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], [controller_name], True, True, 5.0 + node, + controller_manager_name, + [], + [controller_name], + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( @@ -229,7 +245,13 @@ def main(args=None): if not args.inactive and args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], controller_names, True, True, 5.0 + node, + controller_manager_name, + [], + controller_names, + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( @@ -255,7 +277,13 @@ def main(args=None): node.get_logger().info("Interrupt captured, deactivating and unloading controller") # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index e42d85aee9..9e380f5086 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -36,17 +36,33 @@ def main(args=None): default="/controller_manager", required=False, ) + parser.add_argument( + "--switch-timeout", + help="Time to wait for a successful state switch of controllers." + " Useful when switching cannot be performed immediately, e.g.," + " paused simulations at startup", + required=False, + default=5.0, + type=float, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager + switch_timeout = args.switch_timeout node = Node("unspawner_" + controller_names[0]) try: # Ignore returncode, because message is already printed and we'll try to unload anyway ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + switch_timeout, ) node.get_logger().info("Deactivated controller") diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 057d3f01fd..36b9c10c45 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -157,7 +157,8 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h - usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--activate-as-group] + usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] controller_names [controller_names ...] positional arguments: @@ -176,6 +177,9 @@ There are two scripts to interact with controller manager from launch files: -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused + simulations at startup --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether @@ -239,15 +243,18 @@ The parsed controller config file can follow the same conventions as the typical .. code-block:: console $ ros2 run controller_manager unspawner -h - usage: unspawner [-h] [-c CONTROLLER_MANAGER] controller_name + usage: unspawner [-h] [-c CONTROLLER_MANAGER] [--switch-timeout SWITCH_TIMEOUT] controller_names [controller_names ...] positional arguments: - controller_name Name of the controller + controller_names Name of the controller - optional arguments: + options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused + simulations at startup ``hardware_spawner`` ^^^^^^^^^^^^^^^^^^^^^^ diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 69f5f6e4ce..18189d5d16 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.19.0 + 4.20.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 0ed68d9765..f8347df952 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/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("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 { diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 48802d740a..46ecc7ab6e 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/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) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index fbd4859323..78a9f99591 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.19.0 + 4.20.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a9c611f480..0920e2934a 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -77,6 +77,7 @@ controller_manager * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). +* ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). @@ -114,6 +115,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 `_) * With (`#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 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#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 `_) parsing for SDF published to ``robot_description`` topic is now also supported. joint_limits ************ diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 913d4cc36b..17e276bc93 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.20.0 (2024-11-08) +------------------- +* Add Support for SDF (`#1763 `_) +* [HW_IF] Prepare the handles for async operations (`#1750 `_) +* Contributors: Aarav Gupta, Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 4cdd81b60f..8aa214e728 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -123,39 +123,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); - import_command_interface_descriptions(info_); + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and store them. - */ - virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_state_interface_descriptions) - { - joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - - /** - * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and store them. - */ - virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_command_interface_descriptions) - { - joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 2d0c067606..fc195d0a65 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -41,6 +42,16 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_state_interface_descriptions( const std::vector & component_info); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \param[out] state_interfaces_map unordered_map filled with information about hardware's + * StateInterfaces for the component which are exported + */ +HARDWARE_INTERFACE_PUBLIC +void parse_state_interface_descriptions( + const std::vector & component_info, + std::unordered_map & state_interfaces_map); + /** * \param[in] component_info information about a component (gpio, joint, sensor) * \return vector filled with information about hardware's CommandInterfaces for the component @@ -50,5 +61,14 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_command_interface_descriptions( const std::vector & component_info); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \param[out] command_interfaces_map unordered_map filled with information about hardware's + * CommandInterfaces for the component which are exported + */ +HARDWARE_INTERFACE_PUBLIC +void parse_command_interface_descriptions( + const std::vector & component_info, + std::unordered_map & command_interfaces_map); } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 50d79d1a45..74c1d04bd7 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -123,24 +123,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Sensor and store them. - */ - virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto sensor_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.sensors); - for (const auto & description : sensor_state_interface_descriptions) - { - sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 6b0652d3fe..18da0e4012 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -126,57 +126,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); - import_command_interface_descriptions(info_); + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint, GPIO, Sensor and store them. - */ - void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_state_interface_descriptions) - { - joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto sensor_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.sensors); - for (const auto & description : sensor_state_interface_descriptions) - { - sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto gpio_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.gpios); - for (const auto & description : gpio_state_interface_descriptions) - { - gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - - /** - * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and GPIO and store them. - */ - void import_command_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_command_interface_descriptions) - { - joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto gpio_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.gpios); - for (const auto & description : gpio_command_interface_descriptions) - { - gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 96d593e8f6..dbe9ad5750 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.19.0 + 4.20.0 ros2_control hardware interface Bence Magyar Denis Štogl @@ -18,6 +18,7 @@ tinyxml2_vendor joint_limits urdf + sdformat_urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d2ec0f9d53..0f186531e9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/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 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"); @@ -929,6 +942,22 @@ std::vector parse_state_interface_descriptions( return component_state_interface_descriptions; } +void parse_state_interface_descriptions( + const std::vector & component_info, + std::unordered_map & state_interfaces_map) +{ + state_interfaces_map.reserve(state_interfaces_map.size() + component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + InterfaceDescription description(component.name, state_interface); + state_interfaces_map.insert(std::make_pair(description.get_name(), description)); + } + } +} + std::vector parse_command_interface_descriptions( const std::vector & component_info) { @@ -946,4 +975,20 @@ std::vector parse_command_interface_descriptions( return component_command_interface_descriptions; } +void parse_command_interface_descriptions( + const std::vector & component_info, + std::unordered_map & command_interfaces_map) +{ + command_interfaces_map.reserve(command_interfaces_map.size() + component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + InterfaceDescription description(component.name, command_interface); + command_interfaces_map.insert(std::make_pair(description.get_name(), description)); + } + } +} + } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 20e098b62e..3c24b0cc2a 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/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); +} diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 8cab703114..8daf6bab93 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/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) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 1c4f2f1b73..3966cbc993 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.19.0 + 4.20.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 7c1e88b2f9..6b00437dd1 100644 --- a/joint_limits/CHANGELOG.rst +++ b/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) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index a945aa4710..c7fca5f2af 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.19.0 + 4.20.0 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. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 771506022e..5b57ddc2e2 100644 --- a/ros2_control/CHANGELOG.rst +++ b/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) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 71ec807544..925ad6e23f 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.19.0 + 4.20.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 8a2406ca4e..cad5325b2e 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/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 `_) +* Contributors: Aarav Gupta + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index cc2b1798d4..e94d4e6736 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -1715,6 +1715,225 @@ const auto gripper_hardware_resources_mimic_false_command_if = )"; +const auto diff_drive_robot_sdf = + R"( + + + + + + true + + + + base_link + chassis_link + 0 0 0.075 0 0 0 + + + + + + + + 0.3 0.3 0.15 + + + + + 1 1 1 1 + 1 1 1 1 + + + + + + + 0.3 0.3 0.15 + + + + + + 0.5 + + 0.0046875 + 0.0 + 0.0 + 0.0046875 + 0.0 + 0.0075 + + + + + + chassis_link + left_wheel_link + 0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + right_wheel_link + 0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + caster_link + -0.09 0 -0.075 0 0 0 + + 1 1 1 + + -inf + inf + + + + + + + + + 0.05 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + + + + + 0.1 + + 0.00010000000000000002 + 0.0 + 0.0 + 0.00010000000000000002 + 0.0 + 0.00010000000000000002 + + + + + + gz_ros2_control/GazeboSimSystem + + + + -10 + 10 + + + + + + + -10 + 10 + + + + + + + /path/to/config.yml + + + +)"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 9752b87fca..bb818063ab 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.19.0 + 4.20.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 546356417a..894bbb8ea3 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.20.0 (2024-11-08) +------------------- + 4.19.0 (2024-10-26) ------------------- * [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 67b651e3ee..ddb904c432 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.19.0 + 4.20.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 244359601e..949445ec59 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index da13697e81..1f9ad52f22 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.20.0 (2024-11-08) +------------------- + 4.19.0 (2024-10-26) ------------------- * fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index ca6e74373c..1611ce05a3 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.19.0 + 4.20.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 70cf0f8984..76d8c706c2 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 68b6ec5054..842cc76488 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.20.0 (2024-11-08) +------------------- + 4.19.0 (2024-10-26) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 22d0c90ba1..7ae2f2eda7 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.19.0 + 4.20.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl