├── gz_ros2_control_tests ├── rolling.ignored ├── CMakeLists.txt ├── tests │ ├── CMakeLists.txt │ ├── effort_test.py │ ├── position_test.py │ ├── velocity_custom_plugin_test.py │ ├── velocity_test.py │ └── ft_sensor_test.py ├── package.xml └── CHANGELOG.rst ├── doc └── img │ ├── diff_drive.gif │ ├── gz_gripper.gif │ ├── gz_ros2_control.gif │ └── ign_ros2_control.gif ├── Dockerfile ├── entrypoint.sh └── Dockerfile ├── .github ├── CODEOWNERS ├── workflows │ ├── update-pre-commit.yml │ ├── triage.yml │ ├── ci-format.yaml │ ├── jazzy-check-docs.yml │ ├── humble-check-docs.yml │ ├── kilted-check-docs.yml │ ├── rolling-check-docs.yml │ ├── stale.yml │ ├── ci-rolling.yaml │ ├── ci-jazzy.yaml │ ├── ci-kilted.yaml │ └── ci-humble.yaml └── dependabot.yml ├── gz_ros2_control ├── gz_hardware_plugins.xml ├── package.xml ├── include │ └── gz_ros2_control │ │ ├── gz_ros2_control_plugin.hpp │ │ ├── gz_system.hpp │ │ └── gz_system_interface.hpp └── CMakeLists.txt ├── gz_ros2_control_demos ├── config │ ├── gripper_controller_effort.yaml │ ├── gripper_controller_position.yaml │ ├── cart_controller_position.yaml │ ├── cart_controller_effort.yaml │ ├── cart_controller_ft_sensor.yaml │ ├── cart_controller_velocity.yaml │ ├── mecanum_drive_controller.yaml │ ├── ackermann_drive_controller.yaml │ ├── diff_drive_controller.yaml │ └── tricycle_drive_controller.yaml ├── gz_custom_hardware_plugins.xml ├── examples │ ├── example_mobile_robots.cpp │ └── example_gripper.cpp ├── package.xml ├── urdf │ ├── test_cart_effort.xacro.urdf │ ├── test_cart_position.xacro.urdf │ ├── test_cart_velocity_custom_plugin.xacro.urdf │ ├── test_gripper_mimic_joint_effort.xacro.urdf │ ├── test_gripper_mimic_joint_position.xacro.urdf │ ├── test_cart_ft_sensor.xacro.urdf │ ├── test_pendulum_effort.xacro.urdf │ ├── test_pendulum_position.xacro.urdf │ ├── test_diff_drive.xacro.urdf │ ├── test_tricycle_drive.xacro.urdf │ └── test_cart_velocity.xacro.urdf ├── launch │ ├── gripper_mimic_joint_example_effort.launch.xml │ ├── gripper_mimic_joint_example_position.launch.xml │ ├── pendulum_example_effort.launch.py │ ├── pendulum_example_position.launch.py │ ├── cart_example_effort.launch.py │ ├── gripper_mimic_joint_example_effort.launch.py │ ├── gripper_mimic_joint_example_position.launch.py │ ├── cart_example_position.launch.py │ ├── cart_example_velocity_custom_plugin.launch.py │ ├── tricycle_drive_example.launch.py │ ├── mecanum_drive_example.launch.py │ ├── diff_drive_example_namespaced.launch.py │ ├── cart_example_velocity.launch.py │ ├── diff_drive_example.launch.py │ ├── ackermann_drive_example.launch.py │ └── cart_example_ft_sensor.launch.py ├── include │ └── gz_ros2_control_demos │ │ └── gz_custom_system.hpp ├── CMakeLists.txt └── worlds │ └── empty_ft_sensor.sdf ├── gz_ros2_control.humble.repos ├── gz_ros2_control.jazzy.repos ├── gz_ros2_control.kilted.repos ├── gz_ros2_control.rolling.repos ├── .pre-commit-config.yaml └── README.md /gz_ros2_control_tests/rolling.ignored: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /doc/img/diff_drive.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/gz_ros2_control/HEAD/doc/img/diff_drive.gif -------------------------------------------------------------------------------- /doc/img/gz_gripper.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/gz_ros2_control/HEAD/doc/img/gz_gripper.gif -------------------------------------------------------------------------------- /doc/img/gz_ros2_control.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/gz_ros2_control/HEAD/doc/img/gz_ros2_control.gif -------------------------------------------------------------------------------- /doc/img/ign_ros2_control.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/gz_ros2_control/HEAD/doc/img/ign_ros2_control.gif -------------------------------------------------------------------------------- /Dockerfile/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | . /opt/ros/"${ROS_DISTRO}"/setup.sh 4 | . /home/ros2_ws/install/setup.sh 5 | exec "$@" 6 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | # More info: 2 | # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners 3 | 4 | * @ahcorde 5 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(gz_ros2_control_tests) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | if(BUILD_TESTING) 7 | find_package(ament_lint_auto REQUIRED) 8 | ament_lint_auto_find_test_dependencies() 9 | 10 | add_subdirectory(tests) 11 | endif() 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /gz_ros2_control/gz_hardware_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | ros2_control hardware component to be loaded by the gazebo plugin. 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(launch_testing_ament_cmake REQUIRED) 2 | 3 | add_launch_test(position_test.py 4 | TIMEOUT 50 5 | ) 6 | add_launch_test(velocity_test.py 7 | TIMEOUT 50 8 | ) 9 | add_launch_test(velocity_custom_plugin_test.py 10 | TIMEOUT 50 11 | ) 12 | add_launch_test(effort_test.py 13 | TIMEOUT 50 14 | ) 15 | 16 | add_launch_test(ft_sensor_test.py 17 | TIMEOUT 50 18 | ) 19 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/gripper_controller_effort.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | gripper_controller: 9 | ros__parameters: 10 | type: forward_command_controller/ForwardCommandController 11 | joints: 12 | - right_finger_joint 13 | interface_name: effort 14 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/gripper_controller_position.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | gripper_controller: 9 | ros__parameters: 10 | type: forward_command_controller/ForwardCommandController 11 | joints: 12 | - right_finger_joint 13 | interface_name: position 14 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/gz_custom_hardware_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | Custom ros2_control hardware component to be loaded by the gazebo plugin. 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /.github/workflows/update-pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Auto Update pre-commit 2 | # Update pre-commit config and create PR if changes are detected 3 | # author: Christoph Fröhlich 4 | 5 | on: 6 | workflow_dispatch: 7 | schedule: 8 | - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month 9 | 10 | jobs: 11 | auto_update_and_create_pr: 12 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master 13 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/cart_controller_position.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 1000 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | ros__parameters: 10 | type: joint_trajectory_controller/JointTrajectoryController 11 | joints: 12 | - slider_to_cart 13 | command_interfaces: 14 | - position 15 | state_interfaces: 16 | - position 17 | - velocity 18 | -------------------------------------------------------------------------------- /.github/workflows/triage.yml: -------------------------------------------------------------------------------- 1 | on: 2 | issues: 3 | types: [opened] 4 | pull_request_target: 5 | types: [opened] 6 | name: Ticket opened 7 | jobs: 8 | assign: 9 | name: Add ticket to inbox 10 | runs-on: ubuntu-latest 11 | steps: 12 | - name: Add ticket to inbox 13 | uses: technote-space/create-project-card-action@v1 14 | with: 15 | PROJECT: Core development 16 | COLUMN: Inbox 17 | GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} 18 | CHECK_ORG_PROJECT: true 19 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/cart_controller_effort.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 1000 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | ros__parameters: 10 | type: joint_trajectory_controller/JointTrajectoryController 11 | joints: 12 | - slider_to_cart 13 | command_interfaces: 14 | - effort 15 | state_interfaces: 16 | - position 17 | - velocity 18 | gains: 19 | slider_to_cart: 20 | p: 100.0 21 | i: 0.0 22 | d: 10.0 23 | i_clamp: 0.0 24 | antiwindup: false 25 | -------------------------------------------------------------------------------- /.github/workflows/ci-format.yaml: -------------------------------------------------------------------------------- 1 | # This is a format job. Pre-commit has a first-party GitHub action, so we use 2 | # that: https://github.com/pre-commit/action 3 | 4 | name: Format 5 | 6 | on: 7 | workflow_dispatch: 8 | pull_request: 9 | 10 | jobs: 11 | pre-commit: 12 | name: Format 13 | runs-on: ubuntu-22.04 14 | steps: 15 | - uses: actions/checkout@v6 16 | - uses: actions/setup-python@v6 17 | with: 18 | python-version: 3.10.6 19 | - name: Install system hooks 20 | run: sudo apt install -qq cppcheck ament-cmake-uncrustify ament-cmake-pep257 21 | - uses: pre-commit/action@v3.0.1 22 | with: 23 | extra_args: --all-files --hook-stage manual 24 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 1000 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | ros__parameters: 10 | type: joint_trajectory_controller/JointTrajectoryController 11 | joints: 12 | - slider_to_cart 13 | command_interfaces: 14 | - position 15 | state_interfaces: 16 | - position 17 | - velocity 18 | 19 | force_torque_sensor_broadcaster: 20 | ros__parameters: 21 | type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster 22 | sensor_name: "force_torque_sensor" 23 | frame_id: "slider_to_cart" 24 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-check-docs.yml: -------------------------------------------------------------------------------- 1 | name: Check Docs - Jazzy 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - jazzy 8 | paths: 9 | - '**.rst' 10 | - '**.md' 11 | - '**.jpg' 12 | - '**.jpeg' 13 | - '**.png' 14 | - '**.svg' 15 | - '**.yml' 16 | - '**.yaml' 17 | - '!.github/**' # exclude yaml files in .github directory 18 | - '.github/workflows/jazzy-check-docs.yml' 19 | 20 | concurrency: 21 | group: ${{ github.workflow }}-${{ github.ref }} 22 | cancel-in-progress: true 23 | 24 | jobs: 25 | check-docs: 26 | name: Check Docs 27 | uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@jazzy 28 | with: 29 | GZ_ROS2_CONTROL_PR: ${{ github.ref }} 30 | -------------------------------------------------------------------------------- /.github/workflows/humble-check-docs.yml: -------------------------------------------------------------------------------- 1 | name: Check Docs - Humble 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - humble 8 | paths: 9 | - '**.rst' 10 | - '**.md' 11 | - '**.jpg' 12 | - '**.jpeg' 13 | - '**.png' 14 | - '**.svg' 15 | - '**.yml' 16 | - '**.yaml' 17 | - '!.github/**' # exclude yaml files in .github directory 18 | - '.github/workflows/humble-check-docs.yml' 19 | 20 | concurrency: 21 | group: ${{ github.workflow }}-${{ github.ref }} 22 | cancel-in-progress: true 23 | 24 | jobs: 25 | check-docs: 26 | name: Check Docs 27 | uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@humble 28 | with: 29 | GZ_ROS2_CONTROL_PR: ${{ github.ref }} 30 | -------------------------------------------------------------------------------- /.github/workflows/kilted-check-docs.yml: -------------------------------------------------------------------------------- 1 | name: Check Docs - Kilted 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - kilted 8 | paths: 9 | - '**.rst' 10 | - '**.md' 11 | - '**.jpg' 12 | - '**.jpeg' 13 | - '**.png' 14 | - '**.svg' 15 | - '**.yml' 16 | - '**.yaml' 17 | - '!.github/**' # exclude yaml files in .github directory 18 | - '.github/workflows/kilted-check-docs.yml' 19 | 20 | concurrency: 21 | group: ${{ github.workflow }}-${{ github.ref }} 22 | cancel-in-progress: true 23 | 24 | jobs: 25 | check-docs: 26 | name: Check Docs 27 | uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@kilted 28 | with: 29 | GZ_ROS2_CONTROL_PR: ${{ github.ref }} 30 | -------------------------------------------------------------------------------- /.github/workflows/rolling-check-docs.yml: -------------------------------------------------------------------------------- 1 | name: Check Docs - Rolling 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - rolling 8 | paths: 9 | - '**.rst' 10 | - '**.md' 11 | - '**.jpg' 12 | - '**.jpeg' 13 | - '**.png' 14 | - '**.svg' 15 | - '**.yml' 16 | - '**.yaml' 17 | - '!.github/**' # exclude yaml files in .github directory 18 | - '.github/workflows/rolling-check-docs.yml' 19 | 20 | concurrency: 21 | group: ${{ github.workflow }}-${{ github.ref }} 22 | cancel-in-progress: true 23 | 24 | jobs: 25 | check-docs: 26 | name: Check Docs 27 | uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling 28 | with: 29 | GZ_ROS2_CONTROL_PR: ${{ github.ref }} 30 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/cart_controller_velocity.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 1000 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | ros__parameters: 10 | type: joint_trajectory_controller/JointTrajectoryController 11 | joints: 12 | - slider_to_cart 13 | command_interfaces: 14 | - velocity 15 | state_interfaces: 16 | - position 17 | - velocity 18 | gains: 19 | slider_to_cart: 20 | p: 100.0 21 | i: 0.0 22 | d: 0.0 23 | i_clamp: 0.0 24 | antiwindup: false 25 | 26 | imu_sensor_broadcaster: 27 | ros__parameters: 28 | type: imu_sensor_broadcaster/IMUSensorBroadcaster 29 | sensor_name: cart_imu_sensor 30 | frame_id: imu 31 | -------------------------------------------------------------------------------- /gz_ros2_control.humble.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: humble 6 | ros-controls/control_toolbox: 7 | type: git 8 | url: https://github.com/ros-controls/control_toolbox.git 9 | version: humble 10 | ros-controls/kinematics_interface: 11 | type: git 12 | url: https://github.com/ros-controls/kinematics_interface.git 13 | version: humble 14 | ros-controls/realtime_tools: 15 | type: git 16 | url: https://github.com/ros-controls/realtime_tools.git 17 | version: humble 18 | ros-controls/ros2_control: 19 | type: git 20 | url: https://github.com/ros-controls/ros2_control.git 21 | version: humble 22 | ros-controls/ros2_controllers: 23 | type: git 24 | url: https://github.com/ros-controls/ros2_controllers.git 25 | version: humble 26 | -------------------------------------------------------------------------------- /gz_ros2_control.jazzy.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: jazzy 6 | ros-controls/control_toolbox: 7 | type: git 8 | url: https://github.com/ros-controls/control_toolbox.git 9 | version: jazzy 10 | ros-controls/kinematics_interface: 11 | type: git 12 | url: https://github.com/ros-controls/kinematics_interface.git 13 | version: jazzy 14 | ros-controls/realtime_tools: 15 | type: git 16 | url: https://github.com/ros-controls/realtime_tools.git 17 | version: jazzy 18 | ros-controls/ros2_control: 19 | type: git 20 | url: https://github.com/ros-controls/ros2_control.git 21 | version: jazzy 22 | ros-controls/ros2_controllers: 23 | type: git 24 | url: https://github.com/ros-controls/ros2_controllers.git 25 | version: jazzy 26 | ros-controls/ros2_control_cmake: 27 | type: git 28 | url: https://github.com/ros-controls/ros2_control_cmake.git 29 | version: master 30 | -------------------------------------------------------------------------------- /gz_ros2_control.kilted.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: master 6 | ros-controls/control_toolbox: 7 | type: git 8 | url: https://github.com/ros-controls/control_toolbox.git 9 | version: kilted 10 | ros-controls/kinematics_interface: 11 | type: git 12 | url: https://github.com/ros-controls/kinematics_interface.git 13 | version: master 14 | ros-controls/realtime_tools: 15 | type: git 16 | url: https://github.com/ros-controls/realtime_tools.git 17 | version: kilted 18 | ros-controls/ros2_control: 19 | type: git 20 | url: https://github.com/ros-controls/ros2_control.git 21 | version: kilted 22 | ros-controls/ros2_controllers: 23 | type: git 24 | url: https://github.com/ros-controls/ros2_controllers.git 25 | version: kilted 26 | ros-controls/ros2_control_cmake: 27 | type: git 28 | url: https://github.com/ros-controls/ros2_control_cmake.git 29 | version: master 30 | -------------------------------------------------------------------------------- /gz_ros2_control.rolling.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: master 6 | ros-controls/control_toolbox: 7 | type: git 8 | url: https://github.com/ros-controls/control_toolbox.git 9 | version: master 10 | ros-controls/kinematics_interface: 11 | type: git 12 | url: https://github.com/ros-controls/kinematics_interface.git 13 | version: master 14 | ros-controls/realtime_tools: 15 | type: git 16 | url: https://github.com/ros-controls/realtime_tools.git 17 | version: master 18 | ros-controls/ros2_control: 19 | type: git 20 | url: https://github.com/ros-controls/ros2_control.git 21 | version: master 22 | ros-controls/ros2_control_demos: 23 | type: git 24 | url: https://github.com/ros-controls/ros2_control_demos.git 25 | version: master 26 | ros-controls/ros2_control_cmake: 27 | type: git 28 | url: https://github.com/ros-controls/ros2_control_cmake.git 29 | version: master 30 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | # To get started with Dependabot version updates, you'll need to specify which 2 | # package ecosystems to update and where the package manifests are located. 3 | # Please see the documentation for all configuration options: 4 | # https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates 5 | 6 | version: 2 7 | updates: 8 | - package-ecosystem: "github-actions" 9 | # Workflow files stored in the 10 | # default location of `.github/workflows` 11 | directory: "/" 12 | schedule: 13 | interval: "weekly" 14 | - package-ecosystem: "github-actions" 15 | # Workflow files stored in the 16 | # default location of `.github/workflows` 17 | directory: "/" 18 | schedule: 19 | interval: "weekly" 20 | target-branch: "humble" 21 | - package-ecosystem: "github-actions" 22 | # Workflow files stored in the 23 | # default location of `.github/workflows` 24 | directory: "/" 25 | schedule: 26 | interval: "weekly" 27 | target-branch: "jazzy" 28 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/mecanum_drive_controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | use_sim_time: true 5 | 6 | joint_state_broadcaster: 7 | type: joint_state_broadcaster/JointStateBroadcaster 8 | 9 | mecanum_drive_controller: 10 | type: mecanum_drive_controller/MecanumDriveController 11 | 12 | mecanum_drive_controller: 13 | ros__parameters: 14 | reference_timeout: 1.0 # max time between command messages before the vehicle should stop 15 | front_left_wheel_command_joint_name: "front_left_wheel_joint" 16 | front_right_wheel_command_joint_name: "front_right_wheel_joint" 17 | rear_right_wheel_command_joint_name: "rear_right_wheel_joint" 18 | rear_left_wheel_command_joint_name: "rear_left_wheel_joint" 19 | kinematics: 20 | base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } 21 | wheels_radius: 0.3 22 | sum_of_robot_center_projection_on_X_Y_axis: 1.1 23 | base_frame_id: "base_link" 24 | odom_frame_id: "odom" 25 | enable_odom_tf: true 26 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/ackermann_drive_controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | ackermann_steering_controller: 9 | ros__parameters: 10 | type: 'ackermann_steering_controller/AckermannSteeringController' 11 | wheelbase: 1.7 12 | traction_track_width: 1.0 13 | steering_track_width: 1.0 14 | traction_wheels_radius: 0.3 15 | steering_wheels_radius: 0.3 16 | reference_timeout: 2.0 # In s. Timeout to stop if no cmd_vel is received 17 | traction_joints_names: ['rear_right_wheel_joint', 'rear_left_wheel_joint'] 18 | steering_joints_names: ['right_wheel_steering_joint', 'left_wheel_steering_joint'] 19 | open_loop: false 20 | velocity_rolling_window_size: 10 21 | base_frame_id: base_link 22 | odom_frame_id: odom 23 | enable_odom_tf: true 24 | twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] 25 | pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] 26 | position_feedback: false 27 | -------------------------------------------------------------------------------- /gz_ros2_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gz_ros2_control 4 | 3.0.5 5 | Gazebo ros2_control package allows to control simulated robots using ros2_control framework. 6 | Alejandro Hernández 7 | Bence Magyar 8 | Alejandro Hernández 9 | Apache 2 10 | 11 | ament_cmake 12 | 13 | ament_index_cpp 14 | gz_sim_vendor 15 | gz_plugin_vendor 16 | pluginlib 17 | rclcpp 18 | yaml_cpp_vendor 19 | rclcpp_lifecycle 20 | 21 | controller_manager 22 | hardware_interface 23 | ros2_control_cmake 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /.github/workflows/stale.yml: -------------------------------------------------------------------------------- 1 | name: 'Stale issues and PRs' 2 | on: 3 | workflow_dispatch: 4 | schedule: 5 | # UTC noon every workday 6 | - cron: '0 12 * * MON-FRI' 7 | 8 | jobs: 9 | stale: 10 | runs-on: ubuntu-latest 11 | permissions: 12 | issues: write 13 | pull-requests: write 14 | steps: 15 | - uses: actions/stale@v10 16 | with: 17 | stale-issue-label: 'stale' 18 | stale-pr-label: 'stale' 19 | 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.' 20 | close-issue-message: 'This issue was closed because it has been stalled for 45 days with no activity.' 21 | 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.' 22 | days-before-stale: 45 23 | days-before-close: 45 24 | days-before-pr-close: -1 25 | exempt-all-milestones: true 26 | exempt-issue-labels: good first issue,good second issue,persistent,release,roadmap,Epic 27 | operations-per-run: 100 28 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gz_ros2_control_tests 5 | 3.0.5 6 | Gazebo ros2 control tests 7 | Alejandro Hernández 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | ament_index_python 13 | ament_lint_auto 14 | ament_lint_common 15 | gz_ros2_control_demos 16 | launch_ros 17 | launch_testing_ament_cmake 18 | launch_testing_ros 19 | launch 20 | python3-psutil 21 | python3-pytest 22 | rclpy 23 | controller_manager 24 | ros2launch 25 | rosgraph_msgs 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /Dockerfile/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:24.04 2 | 3 | ENV DEBIAN_FRONTEND noninteractive 4 | ENV ROS_DISTRO rolling 5 | 6 | # Make sure everything is up to date before building from source 7 | RUN apt-get update \ 8 | && apt-get upgrade -y \ 9 | && apt-get update && apt-get install -q -y --no-install-recommends \ 10 | dirmngr \ 11 | gnupg \ 12 | lsb-release \ 13 | wget \ 14 | curl \ 15 | git \ 16 | ca-certificates \ 17 | build-essential \ 18 | cmake \ 19 | && apt-get clean 20 | 21 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ 22 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ 23 | apt-get update && apt-get install -q -y --no-install-recommends \ 24 | python3-colcon-ros \ 25 | python3-colcon-common-extensions \ 26 | python3-rosdep \ 27 | && apt-get clean 28 | 29 | RUN mkdir -p /home/ros2_ws/src \ 30 | && cd /home/ros2_ws/src \ 31 | && if [ "${ROS_DISTRO}" = "rolling" ] ; then \ 32 | git clone https://github.com/ros-controls/gz_ros2_control/; \ 33 | else \ 34 | git clone https://github.com/ros-controls/gz_ros2_control/ -b ${ROS_DISTRO}; \ 35 | fi \ 36 | && rosdep init && rosdep update \ 37 | && rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} 38 | 39 | RUN cd /home/ros2_ws/ \ 40 | && . /opt/ros/"${ROS_DISTRO}"/setup.sh \ 41 | && colcon build --merge-install 42 | 43 | COPY entrypoint.sh /entrypoint.sh 44 | ENTRYPOINT ["/entrypoint.sh"] 45 | 46 | CMD ros2 launch gz_ros2_control_demos cart_example_position.launch.py 47 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/diff_drive_controller.yaml: -------------------------------------------------------------------------------- 1 | /**/controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | /**/diff_drive_base_controller: 9 | ros__parameters: 10 | type: diff_drive_controller/DiffDriveController 11 | left_wheel_names: ["left_wheel_joint"] 12 | right_wheel_names: ["right_wheel_joint"] 13 | 14 | wheel_separation: 1.25 15 | #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal 16 | wheel_radius: 0.3 17 | 18 | wheel_separation_multiplier: 1.0 19 | left_wheel_radius_multiplier: 1.0 20 | right_wheel_radius_multiplier: 1.0 21 | 22 | publish_rate: 50.0 23 | odom_frame_id: odom 24 | base_frame_id: chassis 25 | pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] 26 | twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] 27 | 28 | open_loop: false 29 | enable_odom_tf: true 30 | 31 | cmd_vel_timeout: 0.5 32 | #publish_limited_velocity: true 33 | #velocity_rolling_window_size: 10 34 | 35 | # Velocity and acceleration limits 36 | # Whenever a min_* is unspecified, default to -max_* 37 | linear.x.has_velocity_limits: true 38 | linear.x.has_acceleration_limits: true 39 | linear.x.max_velocity: 1.0 40 | linear.x.min_velocity: -1.0 41 | linear.x.max_acceleration: 1.0 42 | linear.x.max_jerk: .NAN 43 | linear.x.min_jerk: .NAN 44 | 45 | angular.z.has_velocity_limits: true 46 | angular.z.has_acceleration_limits: true 47 | angular.z.max_velocity: 1.0 48 | angular.z.min_velocity: -1.0 49 | angular.z.max_acceleration: 1.0 50 | angular.z.min_acceleration: -1.0 51 | angular.z.max_jerk: .NAN 52 | angular.z.min_jerk: .NAN 53 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/examples/example_mobile_robots.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | using namespace std::chrono_literals; 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | std::shared_ptr node = 28 | std::make_shared("example_mobile_robots_node"); 29 | 30 | rclcpp::executors::SingleThreadedExecutor executor; 31 | executor.add_node(node); 32 | 33 | auto publisher = node->create_publisher( 34 | "/cmd_vel", 10); 35 | 36 | RCLCPP_INFO(node->get_logger(), "node created"); 37 | 38 | geometry_msgs::msg::TwistStamped command; 39 | 40 | command.twist.linear.x = 0.1; 41 | command.twist.linear.y = 0.0; 42 | command.twist.linear.z = 0.0; 43 | 44 | command.twist.angular.x = 0.0; 45 | command.twist.angular.y = 0.0; 46 | command.twist.angular.z = 0.1; 47 | 48 | while (1) { 49 | command.header.stamp = node->now(); 50 | publisher->publish(command); 51 | std::this_thread::sleep_for(50ms); 52 | executor.spin_some(); 53 | } 54 | rclcpp::shutdown(); 55 | 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/examples/example_gripper.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: Denis Štogl (Stogl Robotics Consulting) 16 | // 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "rclcpp/rclcpp.hpp" 23 | 24 | #include "std_msgs/msg/float64_multi_array.hpp" 25 | 26 | int main(int argc, char * argv[]) 27 | { 28 | rclcpp::init(argc, argv); 29 | 30 | std::shared_ptr node = std::make_shared("gripper_test_node"); 31 | 32 | auto publisher = node->create_publisher( 33 | "/gripper_controller/commands", 10); 34 | 35 | RCLCPP_INFO(node->get_logger(), "node created"); 36 | 37 | std_msgs::msg::Float64MultiArray commands; 38 | 39 | using namespace std::chrono_literals; 40 | 41 | commands.data.push_back(0); 42 | publisher->publish(commands); 43 | std::this_thread::sleep_for(1s); 44 | 45 | commands.data[0] = 0.38; 46 | publisher->publish(commands); 47 | std::this_thread::sleep_for(1s); 48 | 49 | commands.data[0] = 0.19; 50 | publisher->publish(commands); 51 | std::this_thread::sleep_for(1s); 52 | 53 | commands.data[0] = 0; 54 | publisher->publish(commands); 55 | std::this_thread::sleep_for(1s); 56 | rclcpp::shutdown(); 57 | 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /.github/workflows/ci-rolling.yaml: -------------------------------------------------------------------------------- 1 | name: gz_ros2_control CI - Rolling 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: [ rolling ] 7 | push: 8 | branches: [ rolling ] 9 | schedule: 10 | # Run every morning to detect flakiness and broken dependencies 11 | - cron: '03 5 * * MON-FRI' 12 | 13 | jobs: 14 | build: 15 | runs-on: ubuntu-latest 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | include: 20 | - ros-distro: "rolling" 21 | ros-repo-packages: "-testing" 22 | upstream-repos: "gz_ros2_control.rolling.repos" 23 | - ros-distro: "rolling" 24 | ros-repo-packages: "-testing" 25 | upstream-repos: "" 26 | - ros-distro: "rolling" 27 | ros-repo-packages: "" 28 | upstream-repos: "" 29 | env: 30 | ROS_DISTRO: ${{ matrix.ros-distro }} 31 | container: 32 | image: ghcr.io/ros-controls/ros:${{ matrix.ros-distro }}-ubuntu${{ matrix.ros-repo-packages }} 33 | steps: 34 | - uses: actions/checkout@v6 35 | - name: Checkout ros2_control framework for semi-binary builds 36 | if: ${{ matrix.upstream-repos != '' }} 37 | run: vcs import --input ${{ matrix.upstream-repos }} 38 | - name: Setup colcon workspace 39 | id: configure 40 | shell: bash 41 | run: | 42 | apt-get update 43 | rosdep update 44 | rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} 45 | - name: Build project 46 | id: build 47 | run: | 48 | . /opt/ros/${ROS_DISTRO}/local_setup.sh 49 | colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests 50 | - name: Run tests 51 | id: test 52 | run: | 53 | . /opt/ros/${ROS_DISTRO}/local_setup.sh 54 | colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests 55 | colcon test-result 56 | -------------------------------------------------------------------------------- /.github/workflows/ci-jazzy.yaml: -------------------------------------------------------------------------------- 1 | name: gz_ros2_control CI - Jazzy 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: [ jazzy ] 7 | push: 8 | branches: [ jazzy ] 9 | schedule: 10 | # Run every morning to detect flakiness and broken dependencies 11 | - cron: '03 4 * * MON-FRI' 12 | 13 | jobs: 14 | build: 15 | runs-on: ubuntu-latest 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | include: 20 | - ros-repo-packages: "-testing" 21 | upstream-repos: "" 22 | - ros-repo-packages: "" 23 | upstream-repos: "" 24 | - ros-repo-packages: "-testing" 25 | upstream-repos: "gz_ros2_control.jazzy.repos" 26 | env: 27 | ROS_DISTRO: jazzy 28 | container: 29 | image: ghcr.io/ros-controls/ros:jazzy-ubuntu${{ matrix.ros-repo-packages }} 30 | steps: 31 | - name: Checkout code 32 | if: github.event_name != 'schedule' 33 | uses: actions/checkout@v6 34 | - name: Checkout code for scheduled workflow 35 | if: github.event_name == 'schedule' 36 | uses: actions/checkout@v6 37 | with: 38 | ref: jazzy 39 | - name: Checkout ros2_control framework for semi-binary builds 40 | if: ${{ matrix.upstream-repos != '' }} 41 | run: vcs import --input ${{ matrix.upstream-repos }} 42 | - name: Setup colcon workspace 43 | id: configure 44 | shell: bash 45 | run: | 46 | apt-get update 47 | rosdep update 48 | rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} 49 | - name: Build project 50 | id: build 51 | run: | 52 | . /opt/ros/${ROS_DISTRO}/local_setup.sh 53 | colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests 54 | - name: Run tests 55 | id: test 56 | run: | 57 | . /opt/ros/${ROS_DISTRO}/local_setup.sh 58 | colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests 59 | colcon test-result 60 | -------------------------------------------------------------------------------- /.github/workflows/ci-kilted.yaml: -------------------------------------------------------------------------------- 1 | name: gz_ros2_control CI - Kilted 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: [ kilted ] 7 | push: 8 | branches: [ kilted ] 9 | schedule: 10 | # Run every morning to detect flakiness and broken dependencies 11 | - cron: '03 5 * * MON-FRI' 12 | 13 | jobs: 14 | build: 15 | runs-on: ubuntu-latest 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | include: 20 | - ros-repo-packages: "-testing" 21 | upstream-repos: "" 22 | - ros-repo-packages: "" 23 | upstream-repos: "" 24 | - ros-repo-packages: "-testing" 25 | upstream-repos: "gz_ros2_control.kilted.repos" 26 | env: 27 | ROS_DISTRO: kilted 28 | container: 29 | image: ghcr.io/ros-controls/ros:kilted-ubuntu${{ matrix.ros-repo-packages }} 30 | steps: 31 | - name: Checkout code 32 | if: github.event_name != 'schedule' 33 | uses: actions/checkout@v6 34 | - name: Checkout code for scheduled workflow 35 | if: github.event_name == 'schedule' 36 | uses: actions/checkout@v6 37 | with: 38 | ref: kilted 39 | - name: Checkout ros2_control framework for semi-binary builds 40 | if: ${{ matrix.upstream-repos != '' }} 41 | run: vcs import --input ${{ matrix.upstream-repos }} 42 | - name: Setup colcon workspace 43 | id: configure 44 | shell: bash 45 | run: | 46 | apt-get update 47 | rosdep update 48 | rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} 49 | - name: Build project 50 | id: build 51 | run: | 52 | . /opt/ros/${ROS_DISTRO}/local_setup.sh 53 | colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests 54 | - name: Run tests 55 | id: test 56 | run: | 57 | . /opt/ros/${ROS_DISTRO}/local_setup.sh 58 | colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests 59 | colcon test-result 60 | -------------------------------------------------------------------------------- /gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef GZ_ROS2_CONTROL__GZ_ROS2_CONTROL_PLUGIN_HPP_ 16 | #define GZ_ROS2_CONTROL__GZ_ROS2_CONTROL_PLUGIN_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | namespace sim = gz::sim; 22 | 23 | namespace gz_ros2_control 24 | { 25 | // Forward declarations. 26 | class GazeboSimROS2ControlPluginPrivate; 27 | 28 | class GazeboSimROS2ControlPlugin 29 | : public sim::System, 30 | public sim::ISystemConfigure, 31 | public sim::ISystemPreUpdate, 32 | public sim::ISystemPostUpdate 33 | { 34 | public: 35 | /// \brief Constructor 36 | GazeboSimROS2ControlPlugin(); 37 | 38 | /// \brief Destructor 39 | ~GazeboSimROS2ControlPlugin() override; 40 | 41 | // Documentation inherited 42 | void Configure( 43 | const sim::Entity & _entity, 44 | const std::shared_ptr & _sdf, 45 | sim::EntityComponentManager & _ecm, 46 | sim::EventManager & _eventMgr) override; 47 | 48 | // Documentation inherited 49 | void PreUpdate( 50 | const sim::UpdateInfo & _info, 51 | sim::EntityComponentManager & _ecm) override; 52 | 53 | void PostUpdate( 54 | const sim::UpdateInfo & _info, 55 | const sim::EntityComponentManager & _ecm) override; 56 | 57 | private: 58 | /// \brief Private data pointer. 59 | std::unique_ptr dataPtr; 60 | }; 61 | } // namespace gz_ros2_control 62 | 63 | #endif // GZ_ROS2_CONTROL__GZ_ROS2_CONTROL_PLUGIN_HPP_ 64 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/config/tricycle_drive_controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 50 # Hz 4 | 5 | joint_state_broadcaster: 6 | ros__parameters: 7 | type: joint_state_broadcaster/JointStateBroadcaster 8 | extra_joints: ["right_wheel_joint", "left_wheel_joint"] 9 | 10 | tricycle_controller: 11 | ros__parameters: 12 | type: tricycle_controller/TricycleController 13 | 14 | # Model 15 | traction_joint_name: traction_joint # Name of traction joint in URDF 16 | steering_joint_name: steering_joint # Name of steering joint in URDF 17 | wheel_radius: 0.3 # Radius of front wheel 18 | wheelbase: 1.7 # Distance between center of back wheels and front wheel 19 | 20 | # Odometry 21 | odom_frame_id: odom 22 | base_frame_id: base_link 23 | open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry 24 | enable_odom_tf: true # If True, publishes odom<-base_link TF 25 | odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf 26 | pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source 27 | twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source 28 | velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom 29 | 30 | # Rate Limiting 31 | traction: # All values should be positive 32 | # min_velocity: 0.0 33 | # max_velocity: 1000.0 34 | # min_acceleration: 0.0 35 | max_acceleration: 5.0 36 | # min_deceleration: 0.0 37 | max_deceleration: 8.0 38 | # min_jerk: 0.0 39 | # max_jerk: 1000.0 40 | steering: 41 | min_position: -1.57 42 | max_position: 1.57 43 | # min_velocity: 0.0 44 | max_velocity: 1.0 45 | # min_acceleration: 0.0 46 | # max_acceleration: 1000.0 47 | 48 | # cmd_vel input 49 | cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received 50 | 51 | # Debug 52 | publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. 53 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gz_ros2_control_demos 4 | 3.0.5 5 | gz_ros2_control_demos 6 | 7 | Alejandro Hernandez 8 | 9 | Apache License 2.0 10 | 11 | https://github.com/ros-controls/gz_ros2_control/blob/master/README.md 12 | https://github.com/ros-controls/gz_ros2_control/issues 13 | https://github.com/ros-controls/gz_ros2_control 14 | 15 | Alejandro Hernández 16 | 17 | ament_cmake 18 | 19 | geometry_msgs 20 | pluginlib 21 | rclcpp 22 | rclcpp_action 23 | std_msgs 24 | 25 | gz_sim_vendor 26 | rclcpp_lifecycle 27 | 28 | ament_index_python 29 | geometry_msgs 30 | launch_ros 31 | launch 32 | rclcpp 33 | robot_state_publisher 34 | ros_gz_bridge 35 | ros_gz_sim 36 | ros2launch 37 | std_msgs 38 | xacro 39 | 40 | 41 | control_msgs 42 | control_toolbox 43 | gz_ros2_control 44 | hardware_interface 45 | ros2_control_cmake 46 | ackermann_steering_controller 47 | diff_drive_controller 48 | effort_controllers 49 | force_torque_sensor_broadcaster 50 | imu_sensor_broadcaster 51 | joint_state_broadcaster 52 | joint_trajectory_controller 53 | mecanum_drive_controller 54 | ros2controlcli 55 | tricycle_steering_controller 56 | velocity_controllers 57 | 58 | ament_lint_auto 59 | ament_lint_common 60 | 61 | 62 | ament_cmake 63 | 64 | 65 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_cart_effort.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | gz_ros2_control/GazeboSimSystem 45 | 46 | 47 | 48 | -15 49 | 15 50 | 51 | 52 | 1.0 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 0 0.8 0 1 63 | 0 0.8 0 1 64 | 0 0.8 0 1 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 0 0 0.8 1 73 | 0 0 0.8 1 74 | 0 0 0.8 1 75 | 76 | 77 | 78 | 79 | 80 | 81 | $(find gz_ros2_control_demos)/config/cart_controller_effort.yaml 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml: -------------------------------------------------------------------------------- 1 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml: -------------------------------------------------------------------------------- 1 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_cart_position.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | gz_ros2_control/GazeboSimSystem 57 | 58 | 59 | 60 | -15 61 | 15 62 | 63 | 64 | 1.0 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 0 0.8 0 1 75 | 0 0.8 0 1 76 | 0 0.8 0 1 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 0 0 0.8 1 85 | 0 0 0.8 1 86 | 0 0 0.8 1 87 | 88 | 89 | 90 | 91 | 92 | 93 | $(find gz_ros2_control_demos)/config/cart_controller_position.yaml 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_cart_velocity_custom_plugin.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | gz_ros2_control_demos/GazeboCustomSimSystem 57 | 58 | 59 | 60 | 0.5 61 | 1.0 62 | 63 | 64 | 1.0 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 0 0.8 0 1 75 | 0 0.8 0 1 76 | 0 0.8 0 1 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 0 0 0.8 1 85 | 0 0 0.8 1 86 | 0 0 0.8 1 87 | 88 | 89 | 90 | 91 | 92 | 93 | $(find gz_ros2_control_demos)/config/cart_controller_velocity.yaml 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /gz_ros2_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(gz_ros2_control) 3 | 4 | find_package(ros2_control_cmake REQUIRED) 5 | set_compiler_options() 6 | export_windows_symbols() 7 | 8 | # Compiler options 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # Find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(ament_index_cpp REQUIRED) 16 | find_package(controller_manager REQUIRED) 17 | find_package(hardware_interface REQUIRED) 18 | find_package(pluginlib REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rclcpp_lifecycle REQUIRED) 21 | find_package(yaml_cpp_vendor REQUIRED) 22 | 23 | find_package(gz_sim_vendor REQUIRED) 24 | find_package(gz-sim REQUIRED) 25 | 26 | find_package(gz_plugin_vendor REQUIRED) 27 | find_package(gz-plugin REQUIRED) 28 | 29 | include_directories(include) 30 | 31 | add_library(${PROJECT_NAME}-system SHARED 32 | src/gz_ros2_control_plugin.cpp 33 | ) 34 | add_library(${PROJECT_NAME}-system::${PROJECT_NAME}-system ALIAS ${PROJECT_NAME}-system) 35 | 36 | target_link_libraries(${PROJECT_NAME}-system 37 | PUBLIC 38 | gz-sim::gz-sim 39 | gz-plugin::register 40 | ament_index_cpp::ament_index_cpp 41 | controller_manager::controller_manager 42 | hardware_interface::hardware_interface 43 | ${pluginlib_TARGETS} 44 | rclcpp::rclcpp 45 | rclcpp_lifecycle::rclcpp_lifecycle 46 | ) 47 | target_include_directories(${PROJECT_NAME}-system PUBLIC 48 | $ 49 | $ 50 | ) 51 | 52 | ######### 53 | 54 | add_library(gz_hardware_plugins SHARED 55 | src/gz_system.cpp 56 | ) 57 | target_link_libraries(gz_hardware_plugins 58 | PUBLIC 59 | gz-sim::gz-sim 60 | hardware_interface::hardware_interface 61 | rclcpp::rclcpp 62 | rclcpp_lifecycle::rclcpp_lifecycle 63 | ) 64 | 65 | # Install headers 66 | install(DIRECTORY include/ 67 | DESTINATION include/${PROJECT_NAME} 68 | ) 69 | 70 | # Install library and export targets 71 | install(TARGETS 72 | ${PROJECT_NAME}-system 73 | gz_hardware_plugins 74 | EXPORT export_${PROJECT_NAME} 75 | ARCHIVE DESTINATION lib 76 | LIBRARY DESTINATION lib 77 | RUNTIME DESTINATION bin 78 | ) 79 | 80 | install(EXPORT export_${PROJECT_NAME} 81 | NAMESPACE ${PROJECT_NAME}:: 82 | DESTINATION share/${PROJECT_NAME}/cmake 83 | ) 84 | 85 | # Testing and linting 86 | if(BUILD_TESTING) 87 | find_package(ament_lint_auto REQUIRED) 88 | ament_lint_auto_find_test_dependencies() 89 | endif() 90 | 91 | # Export old-style CMake variables 92 | ament_export_include_directories(include) 93 | ament_export_libraries(${PROJECT_NAME}-system gz_hardware_plugins) 94 | 95 | # Export modern CMake targets 96 | ament_export_targets( 97 | export_${PROJECT_NAME} 98 | ) 99 | 100 | ament_export_dependencies( 101 | controller_manager 102 | hardware_interface 103 | ) 104 | 105 | pluginlib_export_plugin_description_file(gz_ros2_control gz_hardware_plugins.xml) 106 | 107 | # Setup the project 108 | ament_package() 109 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/include/gz_ros2_control_demos/gz_custom_system.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 AIT Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef GZ_ROS2_CONTROL_DEMOS__GZ_CUSTOM_SYSTEM_HPP_ 17 | #define GZ_ROS2_CONTROL_DEMOS__GZ_CUSTOM_SYSTEM_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "gz_ros2_control/gz_system_interface.hpp" 25 | #include "rclcpp_lifecycle/state.hpp" 26 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 27 | 28 | namespace gz_ros2_control_demos 29 | { 30 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 31 | 32 | // Forward declaration 33 | class GazeboSimSystemPrivate; 34 | 35 | // These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a 36 | // simulated `ros2_control` `hardware_interface::SystemInterface`. 37 | 38 | class GazeboCustomSimSystem : public gz_ros2_control::GazeboSimSystemInterface 39 | { 40 | public: 41 | // Documentation Inherited 42 | CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams & params) 43 | override; 44 | 45 | CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; 46 | 47 | // Documentation Inherited 48 | std::vector export_state_interfaces() override; 49 | 50 | // Documentation Inherited 51 | std::vector export_command_interfaces() override; 52 | 53 | // Documentation Inherited 54 | CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 55 | 56 | // Documentation Inherited 57 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 58 | 59 | // Documentation Inherited 60 | hardware_interface::return_type read( 61 | const rclcpp::Time & time, 62 | const rclcpp::Duration & period) override; 63 | 64 | // Documentation Inherited 65 | hardware_interface::return_type write( 66 | const rclcpp::Time & time, 67 | const rclcpp::Duration & period) override; 68 | 69 | // Documentation Inherited 70 | bool initSim( 71 | rclcpp::Node::SharedPtr & model_nh, 72 | std::map & joints, 73 | const hardware_interface::HardwareInfo & hardware_info, 74 | sim::EntityComponentManager & _ecm, 75 | unsigned int update_rate) override; 76 | 77 | private: 78 | /// \brief Private data class 79 | std::unique_ptr dataPtr; 80 | }; 81 | 82 | } // namespace gz_ros2_control_demos 83 | 84 | #endif // GZ_ROS2_CONTROL_DEMOS__GZ_CUSTOM_SYSTEM_HPP_ 85 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.0) 2 | project(gz_ros2_control_demos) 3 | 4 | find_package(ros2_control_cmake REQUIRED) 5 | set_compiler_options() 6 | export_windows_symbols() 7 | 8 | if(NOT WIN32) 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(gz_sim_vendor REQUIRED) 15 | find_package(gz-sim REQUIRED) 16 | find_package(pluginlib REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(rclcpp_action REQUIRED) 19 | find_package(rclcpp_lifecycle REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | 22 | find_package(control_msgs REQUIRED) 23 | find_package(control_toolbox REQUIRED) 24 | find_package(gz_ros2_control REQUIRED) 25 | find_package(hardware_interface REQUIRED) 26 | 27 | install(DIRECTORY 28 | launch 29 | config 30 | sdf 31 | urdf 32 | worlds 33 | DESTINATION share/${PROJECT_NAME}/ 34 | ) 35 | 36 | add_executable(example_position examples/example_position.cpp) 37 | target_link_libraries(example_position PUBLIC 38 | ${control_msgs_TARGETS} 39 | rclcpp::rclcpp 40 | rclcpp_action::rclcpp_action 41 | ) 42 | 43 | # use the same example_position.cpp for example_velocity 44 | add_executable(example_velocity examples/example_position.cpp) 45 | target_link_libraries(example_velocity PUBLIC 46 | ${control_msgs_TARGETS} 47 | rclcpp::rclcpp 48 | rclcpp_action::rclcpp_action 49 | ) 50 | 51 | # use the same example_position.cpp for example_effort 52 | add_executable(example_effort examples/example_position.cpp) 53 | target_link_libraries(example_effort PUBLIC 54 | ${control_msgs_TARGETS} 55 | rclcpp::rclcpp 56 | rclcpp_action::rclcpp_action 57 | ) 58 | 59 | add_executable(example_mobile_robots examples/example_mobile_robots.cpp) 60 | target_link_libraries(example_mobile_robots 61 | rclcpp::rclcpp 62 | ${geometry_msgs_TARGETS} 63 | ) 64 | 65 | add_executable(example_gripper examples/example_gripper.cpp) 66 | target_link_libraries(example_gripper PUBLIC 67 | ${std_msgs_TARGETS} 68 | rclcpp::rclcpp 69 | ) 70 | 71 | ######### 72 | 73 | add_library(gz_custom_hardware_plugins SHARED 74 | src/gz_custom_system.cpp 75 | ) 76 | target_include_directories(gz_custom_hardware_plugins PUBLIC 77 | $ 78 | $ 79 | ) 80 | target_link_libraries(gz_custom_hardware_plugins 81 | PUBLIC 82 | gz-sim::gz-sim 83 | control_toolbox::control_toolbox 84 | gz_ros2_control::gz_ros2_control-system 85 | hardware_interface::hardware_interface 86 | rclcpp::rclcpp 87 | rclcpp_lifecycle::rclcpp_lifecycle 88 | ) 89 | 90 | ######### 91 | 92 | if(BUILD_TESTING) 93 | find_package(ament_lint_auto REQUIRED) 94 | 95 | ament_lint_auto_find_test_dependencies() 96 | endif() 97 | 98 | ## Install 99 | install( 100 | TARGETS 101 | example_position 102 | example_velocity 103 | example_effort 104 | example_mobile_robots 105 | example_gripper 106 | DESTINATION 107 | lib/${PROJECT_NAME} 108 | ) 109 | 110 | install(TARGETS 111 | gz_custom_hardware_plugins 112 | EXPORT export_gz_custom_hardware_plugins 113 | ARCHIVE DESTINATION lib 114 | LIBRARY DESTINATION lib 115 | RUNTIME DESTINATION bin 116 | ) 117 | 118 | ament_export_targets(export_gz_custom_hardware_plugins HAS_LIBRARY_TARGET) 119 | pluginlib_export_plugin_description_file(gz_ros2_control gz_custom_hardware_plugins.xml) 120 | 121 | ament_package() 122 | -------------------------------------------------------------------------------- /gz_ros2_control/include/gz_ros2_control/gz_system.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef GZ_ROS2_CONTROL__GZ_SYSTEM_HPP_ 17 | #define GZ_ROS2_CONTROL__GZ_SYSTEM_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "gz_ros2_control/gz_system_interface.hpp" 25 | #include "rclcpp_lifecycle/state.hpp" 26 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 27 | 28 | namespace gz_ros2_control 29 | { 30 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 31 | 32 | // Forward declaration 33 | class GazeboSimSystemPrivate; 34 | 35 | // These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a 36 | // simulated `ros2_control` `hardware_interface::SystemInterface`. 37 | 38 | class GazeboSimSystem : public GazeboSimSystemInterface 39 | { 40 | public: 41 | // Documentation Inherited 42 | CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams & params) 43 | override; 44 | 45 | CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; 46 | 47 | // Documentation Inherited 48 | std::vector export_state_interfaces() override; 49 | 50 | // Documentation Inherited 51 | std::vector export_command_interfaces() override; 52 | 53 | // Documentation Inherited 54 | CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 55 | 56 | // Documentation Inherited 57 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 58 | 59 | // Documentation Inherited 60 | hardware_interface::return_type perform_command_mode_switch( 61 | const std::vector & start_interfaces, 62 | const std::vector & stop_interfaces) override; 63 | 64 | // Documentation Inherited 65 | hardware_interface::return_type read( 66 | const rclcpp::Time & time, 67 | const rclcpp::Duration & period) override; 68 | 69 | // Documentation Inherited 70 | hardware_interface::return_type write( 71 | const rclcpp::Time & time, 72 | const rclcpp::Duration & period) override; 73 | 74 | // Documentation Inherited 75 | bool initSim( 76 | rclcpp::Node::SharedPtr & model_nh, 77 | std::map & joints, 78 | const hardware_interface::HardwareInfo & hardware_info, 79 | sim::EntityComponentManager & _ecm, 80 | unsigned int update_rate) override; 81 | 82 | private: 83 | // Register a sensor (for now just IMUs) 84 | // \param[in] hardware_info hardware information where the data of 85 | // the sensors is extract. 86 | void registerSensors( 87 | const hardware_interface::HardwareInfo & hardware_info); 88 | 89 | /// \brief Private data class 90 | std::unique_ptr dataPtr; 91 | }; 92 | 93 | } // namespace gz_ros2_control 94 | 95 | #endif // GZ_ROS2_CONTROL__GZ_SYSTEM_HPP_ 96 | -------------------------------------------------------------------------------- /.github/workflows/ci-humble.yaml: -------------------------------------------------------------------------------- 1 | name: gz_ros2_control CI - Humble 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: [ humble ] 7 | push: 8 | branches: [ humble ] 9 | schedule: 10 | # Run every morning to detect flakiness and broken dependencies 11 | - cron: '33 5 * * MON-FRI' 12 | 13 | jobs: 14 | build: 15 | runs-on: ubuntu-latest 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | include: 20 | - gz-version: fortress 21 | ros-repo-packages: "" 22 | upstream-repos: "" 23 | - gz-version: fortress 24 | ros-repo-packages: "-testing" 25 | upstream-repos: "" 26 | - gz-version: harmonic 27 | ros-repo-packages: "" 28 | upstream-repos: "" 29 | - gz-version: harmonic 30 | ros-repo-packages: "-testing" 31 | upstream-repos: "" 32 | - gz-version: fortress 33 | ros-repo-packages: "-testing" 34 | upstream-repos: "gz_ros2_control.humble.repos" 35 | - gz-version: harmonic 36 | ros-repo-packages: "-testing" 37 | upstream-repos: "gz_ros2_control.humble.repos" 38 | env: 39 | GZ_VERSION: ${{ matrix.gz-version }} 40 | container: 41 | image: ghcr.io/ros-controls/ros:humble-ubuntu${{ matrix.ros-repo-packages }} 42 | steps: 43 | - name: Checkout code 44 | if: github.event_name != 'schedule' 45 | uses: actions/checkout@v6 46 | - name: Checkout code for scheduled workflow 47 | if: github.event_name == 'schedule' 48 | uses: actions/checkout@v6 49 | with: 50 | ref: humble 51 | - name: Checkout ros2_control framework for semi-binary builds 52 | if: ${{ matrix.upstream-repos != '' }} 53 | run: vcs import --input ${{ matrix.upstream-repos }} 54 | - name: Setup colcon workspace 55 | id: configure 56 | shell: bash 57 | run: | 58 | if [ "$GZ_VERSION" == "harmonic" ]; then 59 | # https://gazebosim.org/docs/harmonic/install_ubuntu/#binary-installation-on-ubuntu 60 | wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 61 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 62 | apt-get update && apt-get install --no-install-recommends -qq -y gz-harmonic ros-humble-ros-gzharmonic ros-humble-ros-gzharmonic-bridge ros-humble-ros-gzharmonic-sim 63 | export GZ_DEPS_ROSDEP="ros_gz ros_gz_bridge ros_gz_sim" 64 | # https://github.com/osrf/osrf-rosdep#installing-rosdep-rules-to-resolve-gazebo-harmonic-libraries 65 | wget https://raw.githubusercontent.com/osrf/osrf-rosdep/master/gz/00-gazebo.list -O /etc/ros/rosdep/sources.list.d/00-gazebo.list 66 | else 67 | apt-get update 68 | fi 69 | rosdep update --rosdistro humble 70 | rosdep install --from-paths ./ -i -y --rosdistro humble --skip-keys="${GZ_DEPS_ROSDEP}" 71 | - name: Build project 72 | id: build 73 | run: | 74 | . /opt/ros/humble/local_setup.sh 75 | colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests 76 | - name: Run tests 77 | id: test 78 | run: | 79 | . /opt/ros/humble/local_setup.sh 80 | colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests 81 | colcon test-result 82 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | gz_ros2_control/GazeboSimSystem 81 | 82 | 83 | 84 | 85 | 0.15 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | $(find gz_ros2_control_demos)/config/gripper_controller_effort.yaml 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | gz_ros2_control/GazeboSimSystem 81 | 82 | 83 | 84 | 85 | 0.15 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | $(find gz_ros2_control_demos)/config/gripper_controller_position.yaml 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | gz_ros2_control/GazeboSimSystem 57 | 58 | 59 | 60 | -15 61 | 15 62 | 63 | 64 | 1.0 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 0 0.8 0 1 84 | 0 0.8 0 1 85 | 0 0.8 0 1 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 0 0 0.8 1 94 | 0 0 0.8 1 95 | 0 0 0.8 1 96 | 97 | 98 | 99 | 100 | 101 | 102 | 10.0 103 | true 104 | true 105 | force_torque_sensor 106 | 107 | 108 | 109 | 110 | 111 | $(find gz_ros2_control_demos)/config/cart_controller_ft_sensor.yaml 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package gz_ros2_control_tests 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.5 (2025-10-16) 6 | ------------------ 7 | 8 | 3.0.4 (2025-09-29) 9 | ------------------ 10 | * Add a custom plugin for simulating actuator dynamics to the demos (`#693 `_) 11 | * Ensure shutdown of gz jetty (`#682 `_) 12 | * Contributors: Christoph Fröhlich 13 | 14 | 3.0.3 (2025-08-18) 15 | ------------------ 16 | 17 | 3.0.2 (2025-07-09) 18 | ------------------ 19 | 20 | 3.0.1 (2025-07-02) 21 | ------------------ 22 | * Provide force-torque sensor data through gz_system to controller_manager - fixes to original PR (`#610 `_) 23 | * Bump CMake minimum version (`#601 `_) 24 | * Contributors: Bartłomiej Krajewski, Christoph Fröhlich 25 | 26 | 3.0.0 (2025-05-26) 27 | ------------------ 28 | 29 | 2.0.8 (2025-05-23) 30 | ------------------ 31 | 32 | 2.0.7 (2025-04-04) 33 | ------------------ 34 | 35 | 2.0.6 (2025-02-19) 36 | ------------------ 37 | * Change the order of tests to fix timing issues (`#498 `_) 38 | * Update cart demos to use joint_trajectory_controller (`#486 `_) 39 | Co-authored-by: Alejandro Hernández Cordero 40 | * Contributors: Christoph Fröhlich 41 | 42 | 2.0.5 (2025-01-30) 43 | ------------------ 44 | * Use files from demos for testing (`#485 `_) 45 | * Fix the test criterion (`#481 `_) 46 | Co-authored-by: Alejandro Hernández Cordero 47 | * Contributors: Christoph Fröhlich 48 | 49 | 2.0.4 (2025-01-15) 50 | ------------------ 51 | 52 | 2.0.3 (2024-12-11) 53 | ------------------ 54 | * Add missing bridge for simulation time (`#443 `_) 55 | * Contributors: Christoph Fröhlich 56 | 57 | 2.0.2 (2024-10-28) 58 | ------------------ 59 | 60 | 2.0.1 (2024-08-26) 61 | ------------------ 62 | * Use spawner with `--params-file` argument instead of cli verbs (`#399 `_) 63 | Co-authored-by: Alejandro Hernández Cordero 64 | * Contributors: Christoph Fröhlich 65 | 66 | 2.0.0 (2024-07-09) 67 | ------------------ 68 | 69 | 1.3.1 (2024-07-02) 70 | ------------------ 71 | 72 | 1.3.0 (2024-05-14) 73 | ------------------ 74 | * Use Gazebo ROS vendor packages (`#277 `_) 75 | * Contributors: Addisu Z. Taddese 76 | 77 | 1.2.2 (2024-03-21) 78 | ------------------ 79 | 80 | 1.2.1 (2024-01-24) 81 | ------------------ 82 | * Include testing packages on CI (`#223 `_) 83 | * Contributors: Alejandro Hernández Cordero 84 | 85 | 1.2.0 (2024-01-04) 86 | ------------------ 87 | * Rename cartpole with cart (`#214 `_) 88 | Co-authored-by: Christoph Fröhlich 89 | * Support Harmonic (`#185 `_) 90 | * Contributors: Alejandro Hernández Cordero 91 | 92 | 1.1.1 (2023-07-13) 93 | ------------------ 94 | * Run end to end test in CI (`#152 `_) 95 | * Add test to check position controller (`#134 `_) 96 | * Contributors: Alejandro Hernández Cordero 97 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | gz_ros2_control/GazeboSimSystem 81 | 82 | 83 | 84 | -1000 85 | 1000 86 | 87 | 88 | 1.0 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | $(find gz_ros2_control_demos)/config/cart_controller_effort.yaml 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | gz_ros2_control/GazeboSimSystem 81 | 82 | 83 | 84 | -15 85 | 15 86 | 87 | 88 | 1.0 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | $(find gz_ros2_control_demos)/config/cart_controller_position.yaml 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_ 17 | #define GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | namespace sim = gz::sim; 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | namespace gz_ros2_control 33 | { 34 | 35 | /// \brief This class allows us to handle flags easily, instead of using strings 36 | /// 37 | /// For example 38 | /// enum ControlMethod_ 39 | /// { 40 | /// NONE = 0, 41 | /// POSITION = (1 << 0), 42 | /// VELOCITY = (1 << 1), 43 | /// EFFORT = (1 << 2), 44 | /// }; 45 | /// typedef SafeEnum ControlMethod; 46 | /// 47 | /// ControlMethod foo; 48 | /// foo |= POSITION // Foo has the position flag active 49 | /// foo & POSITION -> True // Check if position is active in the flag 50 | /// foo & VELOCITY -> False // Check if velocity is active in the flag 51 | 52 | template::type> 53 | class SafeEnum 54 | { 55 | public: 56 | SafeEnum() 57 | : mFlags(0) {} 58 | explicit SafeEnum(ENUM singleFlag) 59 | : mFlags(singleFlag) {} 60 | SafeEnum(const SafeEnum & original) 61 | : mFlags(original.mFlags) {} 62 | 63 | SafeEnum & operator|=(ENUM addValue) {mFlags |= addValue; return *this;} 64 | SafeEnum operator|(ENUM addValue) {SafeEnum result(*this); result |= addValue; return result;} 65 | SafeEnum & operator&=(ENUM maskValue) {mFlags &= maskValue; return *this;} 66 | SafeEnum operator&(ENUM maskValue) {SafeEnum result(*this); result &= maskValue; return result;} 67 | SafeEnum operator~() {SafeEnum result(*this); result.mFlags = ~result.mFlags; return result;} 68 | explicit operator bool() {return mFlags != 0;} 69 | 70 | protected: 71 | UNDERLYING mFlags; 72 | }; 73 | 74 | // SystemInterface provides API-level access to read and command joint properties. 75 | class GazeboSimSystemInterface 76 | : public hardware_interface::SystemInterface 77 | { 78 | public: 79 | /// \brief Initialize the system interface 80 | /// param[in] model_nh Pointer to the ros2 node 81 | /// param[in] joints Map with the name of the joint as the key and the value is 82 | /// related with the entity in Gazebo 83 | /// param[in] hardware_info structure with data from URDF. 84 | /// param[in] _ecm Entity-component manager. 85 | /// param[in] update_rate controller update rate 86 | virtual bool initSim( 87 | rclcpp::Node::SharedPtr & model_nh, 88 | std::map & joints, 89 | const hardware_interface::HardwareInfo & hardware_info, 90 | sim::EntityComponentManager & _ecm, 91 | unsigned int update_rate) = 0; 92 | 93 | // Methods used to control a joint. 94 | enum ControlMethod_ 95 | { 96 | NONE = 0, 97 | POSITION = (1 << 0), 98 | VELOCITY = (1 << 1), 99 | EFFORT = (1 << 2), 100 | }; 101 | 102 | typedef SafeEnum ControlMethod; 103 | 104 | protected: 105 | rclcpp::Node::SharedPtr nh_; 106 | }; 107 | 108 | } // namespace gz_ros2_control 109 | 110 | #endif // GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_ 111 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/worlds/empty_ft_sensor.sdf: -------------------------------------------------------------------------------- 1 | 2 | 62 | 63 | 64 | 65 | 0.001 66 | 1.0 67 | 68 | 71 | 72 | 75 | 76 | 79 | 80 | 83 | 84 | 87 | 88 | 89 | 90 | true 91 | 0 0 10 0 0 0 92 | 0.8 0.8 0.8 1 93 | 0.2 0.2 0.2 1 94 | 95 | 1000 96 | 0.9 97 | 0.01 98 | 0.001 99 | 100 | -0.5 0.1 -0.9 101 | 102 | 103 | 104 | true 105 | 106 | 107 | 108 | 109 | 0 0 1 110 | 100 100 111 | 112 | 113 | 114 | 115 | 116 | 117 | 0 0 1 118 | 100 100 119 | 120 | 121 | 122 | 0.8 0.8 0.8 1 123 | 0.8 0.8 0.8 1 124 | 0.8 0.8 0.8 1 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/tests/effort_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2025 ros2_control Maintainers 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | import unittest 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | from controller_manager.test_utils import ( 21 | check_controllers_running, 22 | check_if_js_published, 23 | check_node_running 24 | ) 25 | from launch import LaunchDescription 26 | from launch.actions import IncludeLaunchDescription 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | import launch_testing 30 | from launch_testing.actions import ReadyToTest 31 | from launch_testing.util import KeepAliveProc 32 | from launch_testing_ros import WaitForTopics 33 | import psutil 34 | import pytest 35 | import rclpy 36 | from rosgraph_msgs.msg import Clock 37 | 38 | 39 | # This function specifies the processes to be run for our test 40 | @pytest.mark.rostest 41 | def generate_test_description(): 42 | # This is necessary to get unbuffered output from the process under test 43 | proc_env = os.environ.copy() 44 | proc_env['PYTHONUNBUFFERED'] = '1' 45 | launch_include = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join( 48 | get_package_share_directory('gz_ros2_control_demos'), 49 | 'launch/cart_example_effort.launch.py', 50 | ) 51 | ), 52 | launch_arguments={'gz_args': '--headless-rendering -s'}.items(), 53 | ) 54 | 55 | return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) 56 | 57 | 58 | class TestFixture(unittest.TestCase): 59 | 60 | @classmethod 61 | def setUpClass(cls): 62 | rclpy.init() 63 | 64 | @classmethod 65 | def tearDownClass(cls): 66 | for proc in psutil.process_iter(): 67 | # check whether the process name matches 68 | if proc.name() == 'ruby' or 'gz sim' in proc.name(): 69 | # up to version 9 of gz-sim 70 | proc.kill() 71 | if 'gz-sim' in proc.name(): 72 | # from version 10 of gz-sim 73 | proc.kill() 74 | rclpy.shutdown() 75 | 76 | def setUp(self): 77 | self.node = rclpy.create_node('test_node') 78 | 79 | def tearDown(self): 80 | self.node.destroy_node() 81 | 82 | def test_node_start(self, proc_output): 83 | check_node_running(self.node, 'robot_state_publisher') 84 | 85 | def test_clock(self): 86 | topic_list = [('/clock', Clock)] 87 | with WaitForTopics(topic_list, timeout=10.0): 88 | print('/clock is receiving messages!') 89 | 90 | def test_check_if_msgs_published(self): 91 | check_if_js_published( 92 | '/joint_states', 93 | [ 94 | 'slider_to_cart', 95 | ], 96 | ) 97 | 98 | def test_arm(self, launch_service, proc_info, proc_output): 99 | 100 | # Check if the controllers are running 101 | cnames = ['joint_trajectory_controller', 'joint_state_broadcaster'] 102 | check_controllers_running(self.node, cnames) 103 | 104 | proc_action = Node( 105 | package='gz_ros2_control_demos', 106 | executable='example_effort', 107 | output='screen', 108 | ) 109 | 110 | with launch_testing.tools.launch_process( 111 | launch_service, proc_action, proc_info, proc_output 112 | ): 113 | proc_info.assertWaitForShutdown(process=proc_action, timeout=300) 114 | launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, 115 | allowable_exit_codes=[0]) 116 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/tests/position_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | import unittest 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | from controller_manager.test_utils import ( 21 | check_controllers_running, 22 | check_if_js_published, 23 | check_node_running 24 | ) 25 | from launch import LaunchDescription 26 | from launch.actions import IncludeLaunchDescription 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | import launch_testing 30 | from launch_testing.actions import ReadyToTest 31 | from launch_testing.util import KeepAliveProc 32 | from launch_testing_ros import WaitForTopics 33 | import psutil 34 | import pytest 35 | import rclpy 36 | from rosgraph_msgs.msg import Clock 37 | 38 | 39 | # This function specifies the processes to be run for our test 40 | @pytest.mark.rostest 41 | def generate_test_description(): 42 | # This is necessary to get unbuffered output from the process under test 43 | proc_env = os.environ.copy() 44 | proc_env['PYTHONUNBUFFERED'] = '1' 45 | launch_include = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join( 48 | get_package_share_directory('gz_ros2_control_demos'), 49 | 'launch/cart_example_position.launch.py', 50 | ) 51 | ), 52 | launch_arguments={'gz_args': '--headless-rendering -s'}.items(), 53 | ) 54 | 55 | return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) 56 | 57 | 58 | class TestFixture(unittest.TestCase): 59 | 60 | @classmethod 61 | def setUpClass(cls): 62 | rclpy.init() 63 | 64 | @classmethod 65 | def tearDownClass(cls): 66 | for proc in psutil.process_iter(): 67 | # check whether the process name matches 68 | if proc.name() == 'ruby' or 'gz sim' in proc.name(): 69 | # up to version 9 of gz-sim 70 | proc.kill() 71 | if 'gz-sim' in proc.name(): 72 | # from version 10 of gz-sim 73 | proc.kill() 74 | rclpy.shutdown() 75 | 76 | def setUp(self): 77 | self.node = rclpy.create_node('test_node') 78 | 79 | def tearDown(self): 80 | self.node.destroy_node() 81 | 82 | def test_node_start(self, proc_output): 83 | check_node_running(self.node, 'robot_state_publisher') 84 | 85 | def test_clock(self): 86 | topic_list = [('/clock', Clock)] 87 | with WaitForTopics(topic_list, timeout=10.0): 88 | print('/clock is receiving messages!') 89 | 90 | def test_check_if_msgs_published(self): 91 | check_if_js_published( 92 | '/joint_states', 93 | [ 94 | 'slider_to_cart', 95 | ], 96 | ) 97 | 98 | def test_arm(self, launch_service, proc_info, proc_output): 99 | 100 | # Check if the controllers are running 101 | cnames = ['joint_trajectory_controller', 'joint_state_broadcaster'] 102 | check_controllers_running(self.node, cnames) 103 | 104 | proc_action = Node( 105 | package='gz_ros2_control_demos', 106 | executable='example_position', 107 | output='screen', 108 | ) 109 | 110 | with launch_testing.tools.launch_process( 111 | launch_service, proc_action, proc_info, proc_output 112 | ): 113 | proc_info.assertWaitForShutdown(process=proc_action, timeout=300) 114 | launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, 115 | allowable_exit_codes=[0]) 116 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/tests/velocity_custom_plugin_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2025 ros2_control Maintainers 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | import unittest 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | from controller_manager.test_utils import ( 21 | check_controllers_running, 22 | check_if_js_published, 23 | check_node_running 24 | ) 25 | from launch import LaunchDescription 26 | from launch.actions import IncludeLaunchDescription 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | import launch_testing 30 | from launch_testing.actions import ReadyToTest 31 | from launch_testing.util import KeepAliveProc 32 | from launch_testing_ros import WaitForTopics 33 | import psutil 34 | import pytest 35 | import rclpy 36 | from rosgraph_msgs.msg import Clock 37 | 38 | 39 | # This function specifies the processes to be run for our test 40 | @pytest.mark.rostest 41 | def generate_test_description(): 42 | # This is necessary to get unbuffered output from the process under test 43 | proc_env = os.environ.copy() 44 | proc_env['PYTHONUNBUFFERED'] = '1' 45 | launch_include = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join( 48 | get_package_share_directory('gz_ros2_control_demos'), 49 | 'launch', 'cart_example_velocity_custom_plugin.launch.py', 50 | ) 51 | ), 52 | launch_arguments={'gz_args': '--headless-rendering -s'}.items(), 53 | ) 54 | 55 | return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) 56 | 57 | 58 | class TestFixture(unittest.TestCase): 59 | 60 | @classmethod 61 | def setUpClass(cls): 62 | rclpy.init() 63 | 64 | @classmethod 65 | def tearDownClass(cls): 66 | for proc in psutil.process_iter(): 67 | # check whether the process name matches 68 | if proc.name() == 'ruby' or 'gz sim' in proc.name(): 69 | # up to version 9 of gz-sim 70 | proc.kill() 71 | if 'gz-sim' in proc.name(): 72 | # from version 10 of gz-sim 73 | proc.kill() 74 | rclpy.shutdown() 75 | 76 | def setUp(self): 77 | self.node = rclpy.create_node('test_node') 78 | 79 | def tearDown(self): 80 | self.node.destroy_node() 81 | 82 | def test_node_start(self, proc_output): 83 | check_node_running(self.node, 'robot_state_publisher') 84 | 85 | def test_clock(self): 86 | topic_list = [('/clock', Clock)] 87 | with WaitForTopics(topic_list, timeout=10.0): 88 | print('/clock is receiving messages!') 89 | 90 | def test_check_if_msgs_published(self): 91 | check_if_js_published( 92 | '/joint_states', 93 | [ 94 | 'slider_to_cart', 95 | ], 96 | ) 97 | 98 | def test_arm(self, launch_service, proc_info, proc_output): 99 | 100 | # Check if the controllers are running 101 | cnames = [ 102 | 'joint_trajectory_controller', 103 | 'joint_state_broadcaster' 104 | ] 105 | check_controllers_running(self.node, cnames) 106 | 107 | proc_action = Node( 108 | package='gz_ros2_control_demos', 109 | executable='example_velocity', 110 | output='screen', 111 | ) 112 | 113 | with launch_testing.tools.launch_process( 114 | launch_service, proc_action, proc_info, proc_output 115 | ): 116 | proc_info.assertWaitForShutdown(process=proc_action, timeout=300) 117 | launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, 118 | allowable_exit_codes=[0]) 119 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/tests/velocity_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2025 ros2_control Maintainers 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | import unittest 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | from controller_manager.test_utils import ( 21 | check_controllers_running, 22 | check_if_js_published, 23 | check_node_running 24 | ) 25 | from launch import LaunchDescription 26 | from launch.actions import IncludeLaunchDescription 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | import launch_testing 30 | from launch_testing.actions import ReadyToTest 31 | from launch_testing.util import KeepAliveProc 32 | from launch_testing_ros import WaitForTopics 33 | import psutil 34 | import pytest 35 | import rclpy 36 | from rosgraph_msgs.msg import Clock 37 | 38 | 39 | # This function specifies the processes to be run for our test 40 | @pytest.mark.rostest 41 | def generate_test_description(): 42 | # This is necessary to get unbuffered output from the process under test 43 | proc_env = os.environ.copy() 44 | proc_env['PYTHONUNBUFFERED'] = '1' 45 | launch_include = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join( 48 | get_package_share_directory('gz_ros2_control_demos'), 49 | 'launch/cart_example_velocity.launch.py', 50 | ) 51 | ), 52 | launch_arguments={'gz_args': '--headless-rendering -s'}.items(), 53 | ) 54 | 55 | return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) 56 | 57 | 58 | class TestFixture(unittest.TestCase): 59 | 60 | @classmethod 61 | def setUpClass(cls): 62 | rclpy.init() 63 | 64 | @classmethod 65 | def tearDownClass(cls): 66 | for proc in psutil.process_iter(): 67 | # check whether the process name matches 68 | if proc.name() == 'ruby' or 'gz sim' in proc.name(): 69 | # up to version 9 of gz-sim 70 | proc.kill() 71 | if 'gz-sim' in proc.name(): 72 | # from version 10 of gz-sim 73 | proc.kill() 74 | rclpy.shutdown() 75 | 76 | def setUp(self): 77 | self.node = rclpy.create_node('test_node') 78 | 79 | def tearDown(self): 80 | self.node.destroy_node() 81 | 82 | def test_node_start(self, proc_output): 83 | check_node_running(self.node, 'robot_state_publisher') 84 | 85 | def test_clock(self): 86 | topic_list = [('/clock', Clock)] 87 | with WaitForTopics(topic_list, timeout=10.0): 88 | print('/clock is receiving messages!') 89 | 90 | def test_check_if_msgs_published(self): 91 | check_if_js_published( 92 | '/joint_states', 93 | [ 94 | 'slider_to_cart', 95 | ], 96 | ) 97 | 98 | def test_arm(self, launch_service, proc_info, proc_output): 99 | 100 | # Check if the controllers are running 101 | cnames = [ 102 | 'joint_trajectory_controller', 103 | 'joint_state_broadcaster', 104 | 'imu_sensor_broadcaster' 105 | ] 106 | check_controllers_running(self.node, cnames) 107 | 108 | proc_action = Node( 109 | package='gz_ros2_control_demos', 110 | executable='example_velocity', 111 | output='screen', 112 | ) 113 | 114 | with launch_testing.tools.launch_process( 115 | launch_service, proc_action, proc_info, proc_output 116 | ): 117 | proc_info.assertWaitForShutdown(process=proc_action, timeout=300) 118 | launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, 119 | allowable_exit_codes=[0]) 120 | -------------------------------------------------------------------------------- /gz_ros2_control_tests/tests/ft_sensor_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2025 ros2_control Maintainers 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | import unittest 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | from controller_manager.test_utils import ( 21 | check_controllers_running, 22 | check_if_js_published, 23 | check_node_running 24 | ) 25 | from launch import LaunchDescription 26 | from launch.actions import IncludeLaunchDescription 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | import launch_testing 30 | from launch_testing.actions import ReadyToTest 31 | from launch_testing.util import KeepAliveProc 32 | from launch_testing_ros import WaitForTopics 33 | import psutil 34 | import pytest 35 | import rclpy 36 | from rosgraph_msgs.msg import Clock 37 | 38 | 39 | # This function specifies the processes to be run for our test 40 | @pytest.mark.rostest 41 | def generate_test_description(): 42 | # This is necessary to get unbuffered output from the process under test 43 | proc_env = os.environ.copy() 44 | proc_env['PYTHONUNBUFFERED'] = '1' 45 | launch_include = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join( 48 | get_package_share_directory('gz_ros2_control_demos'), 49 | 'launch/cart_example_ft_sensor.launch.py', 50 | ) 51 | ), 52 | launch_arguments={'gz_args': '--headless-rendering -s'}.items(), 53 | ) 54 | 55 | return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) 56 | 57 | 58 | class TestFixture(unittest.TestCase): 59 | 60 | @classmethod 61 | def setUpClass(cls): 62 | rclpy.init() 63 | 64 | @classmethod 65 | def tearDownClass(cls): 66 | for proc in psutil.process_iter(): 67 | # check whether the process name matches 68 | if proc.name() == 'ruby' or 'gz sim' in proc.name(): 69 | # up to version 9 of gz-sim 70 | proc.kill() 71 | if 'gz-sim' in proc.name(): 72 | # from version 10 of gz-sim 73 | proc.kill() 74 | rclpy.shutdown() 75 | 76 | def setUp(self): 77 | self.node = rclpy.create_node('test_node') 78 | 79 | def tearDown(self): 80 | self.node.destroy_node() 81 | 82 | def test_node_start(self, proc_output): 83 | check_node_running(self.node, 'robot_state_publisher') 84 | 85 | def test_clock(self): 86 | topic_list = [('/clock', Clock)] 87 | with WaitForTopics(topic_list, timeout=10.0): 88 | print('/clock is receiving messages!') 89 | 90 | def test_check_if_msgs_published(self): 91 | check_if_js_published( 92 | '/joint_states', 93 | [ 94 | 'slider_to_cart', 95 | ], 96 | ) 97 | 98 | def test_arm(self, launch_service, proc_info, proc_output): 99 | 100 | # Check if the controllers are running 101 | cnames = [ 102 | 'joint_trajectory_controller', 103 | 'joint_state_broadcaster', 104 | 'force_torque_sensor_broadcaster' 105 | ] 106 | check_controllers_running(self.node, cnames) 107 | 108 | proc_action = Node( 109 | package='gz_ros2_control_demos', 110 | executable='example_position', 111 | output='screen', 112 | ) 113 | 114 | with launch_testing.tools.launch_process( 115 | launch_service, proc_action, proc_info, proc_output 116 | ): 117 | proc_info.assertWaitForShutdown(process=proc_action, timeout=300) 118 | launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, 119 | allowable_exit_codes=[0]) 120 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/pendulum_example_effort.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 ros2_control Development Team 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | # Get URDF via xacro 31 | robot_description_content = Command( 32 | [ 33 | PathJoinSubstitution([FindExecutable(name='xacro')]), 34 | ' ', 35 | PathJoinSubstitution( 36 | [FindPackageShare('gz_ros2_control_demos'), 37 | 'urdf', 'test_pendulum_effort.xacro.urdf'] 38 | ), 39 | ] 40 | ) 41 | robot_description = {'robot_description': robot_description_content} 42 | robot_controllers = PathJoinSubstitution( 43 | [ 44 | FindPackageShare('gz_ros2_control_demos'), 45 | 'config', 46 | 'cart_controller_effort.yaml', 47 | ] 48 | ) 49 | 50 | node_robot_state_publisher = Node( 51 | package='robot_state_publisher', 52 | executable='robot_state_publisher', 53 | output='screen', 54 | parameters=[robot_description] 55 | ) 56 | 57 | gz_spawn_entity = Node( 58 | package='ros_gz_sim', 59 | executable='create', 60 | output='screen', 61 | arguments=['-topic', 'robot_description', 62 | '-name', 'cart', '-allow_renaming', 'true'], 63 | ) 64 | 65 | joint_state_broadcaster_spawner = Node( 66 | package='controller_manager', 67 | executable='spawner', 68 | arguments=['joint_state_broadcaster'], 69 | ) 70 | joint_trajectory_controller_spawner = Node( 71 | package='controller_manager', 72 | executable='spawner', 73 | arguments=[ 74 | 'joint_trajectory_controller', 75 | '--param-file', 76 | robot_controllers, 77 | ], 78 | ) 79 | 80 | # Bridge 81 | bridge = Node( 82 | package='ros_gz_bridge', 83 | executable='parameter_bridge', 84 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 85 | output='screen' 86 | ) 87 | 88 | return LaunchDescription([ 89 | # Launch gazebo environment 90 | IncludeLaunchDescription( 91 | PythonLaunchDescriptionSource( 92 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 93 | 'launch', 94 | 'gz_sim.launch.py'])]), 95 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 96 | RegisterEventHandler( 97 | event_handler=OnProcessExit( 98 | target_action=gz_spawn_entity, 99 | on_exit=[joint_state_broadcaster_spawner], 100 | ) 101 | ), 102 | RegisterEventHandler( 103 | event_handler=OnProcessExit( 104 | target_action=joint_state_broadcaster_spawner, 105 | on_exit=[joint_trajectory_controller_spawner], 106 | ) 107 | ), 108 | bridge, 109 | node_robot_state_publisher, 110 | gz_spawn_entity, 111 | # Launch Arguments 112 | DeclareLaunchArgument( 113 | 'use_sim_time', 114 | default_value=use_sim_time, 115 | description='If true, use simulated clock'), 116 | ]) 117 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | repos: 16 | # Standard hooks 17 | - repo: https://github.com/pre-commit/pre-commit-hooks 18 | rev: v6.0.0 19 | hooks: 20 | - id: check-added-large-files 21 | - id: check-ast 22 | - id: check-case-conflict 23 | - id: check-docstring-first 24 | - id: check-merge-conflict 25 | - id: check-symlinks 26 | - id: check-xml 27 | - id: check-yaml 28 | - id: debug-statements 29 | - id: end-of-file-fixer 30 | - id: mixed-line-ending 31 | - id: trailing-whitespace 32 | exclude_types: [rst] 33 | - id: fix-byte-order-marker 34 | 35 | 36 | # Python hooks 37 | - repo: https://github.com/asottile/pyupgrade 38 | rev: v3.21.2 39 | hooks: 40 | - id: pyupgrade 41 | args: [--py36-plus] 42 | 43 | # # PEP 257 44 | - repo: local 45 | hooks: 46 | - id: ament_pep257 47 | name: ament_pep257 48 | description: Format files with pep257. 49 | entry: ament_pep257 50 | language: system 51 | args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] 52 | 53 | - repo: https://github.com/pycqa/flake8 54 | rev: 7.3.0 55 | hooks: 56 | - id: flake8 57 | args: ["--extend-ignore=E501"] 58 | 59 | # CPP hooks 60 | - repo: local 61 | hooks: 62 | - id: ament_uncrustify 63 | name: ament_uncrustify 64 | description: Format files with uncrustify. 65 | entry: ament_uncrustify 66 | language: system 67 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 68 | args: ["--reformat"] 69 | 70 | - repo: local 71 | hooks: 72 | - id: ament_cppcheck 73 | name: ament_cppcheck 74 | description: Static code analysis of C/C++ files. 75 | stages: [pre-commit] 76 | entry: ament_cppcheck 77 | language: system 78 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 79 | 80 | # Maybe use https://github.com/cpplint/cpplint instead 81 | - repo: local 82 | hooks: 83 | - id: ament_cpplint 84 | name: ament_cpplint 85 | description: Static code analysis of C/C++ files. 86 | stages: [pre-commit] 87 | entry: ament_cpplint 88 | language: system 89 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 90 | args: ["--linelength=100", "--filter=-whitespace/newline"] 91 | 92 | # Cmake hooks 93 | - repo: local 94 | hooks: 95 | - id: ament_lint_cmake 96 | name: ament_lint_cmake 97 | description: Check format of CMakeLists.txt files. 98 | stages: [pre-commit] 99 | entry: ament_lint_cmake 100 | language: system 101 | files: CMakeLists\.txt$ 102 | 103 | # Copyright 104 | - repo: local 105 | hooks: 106 | - id: ament_copyright 107 | name: ament_copyright 108 | description: Check if copyright notice is available in all files. 109 | stages: [pre-commit] 110 | entry: ament_copyright 111 | language: system 112 | 113 | # Docs - RestructuredText hooks 114 | - repo: https://github.com/PyCQA/doc8 115 | rev: v2.0.0 116 | hooks: 117 | - id: doc8 118 | args: ['--max-line-length=100', '--ignore=D001'] 119 | exclude: CHANGELOG\.rst$ 120 | 121 | - repo: https://github.com/pre-commit/pygrep-hooks 122 | rev: v1.10.0 123 | hooks: 124 | - id: rst-backticks 125 | exclude: CHANGELOG\.rst$ 126 | - id: rst-directive-colons 127 | - id: rst-inline-touching-normal 128 | 129 | # Spellcheck in comments and docs 130 | # skipping of *.svg files is not working... 131 | - repo: https://github.com/codespell-project/codespell 132 | rev: v2.4.1 133 | hooks: 134 | - id: codespell 135 | args: ['--write-changes'] 136 | exclude: CHANGELOG\.rst|\.(svg|pyc)$ 137 | 138 | - repo: https://github.com/python-jsonschema/check-jsonschema 139 | rev: 0.35.0 140 | hooks: 141 | - id: check-github-workflows 142 | args: ["--verbose"] 143 | - id: check-github-actions 144 | args: ["--verbose"] 145 | - id: check-dependabot 146 | args: ["--verbose"] 147 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/pendulum_example_position.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 ros2_control Development Team 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | # Get URDF via xacro 31 | robot_description_content = Command( 32 | [ 33 | PathJoinSubstitution([FindExecutable(name='xacro')]), 34 | ' ', 35 | PathJoinSubstitution( 36 | [FindPackageShare('gz_ros2_control_demos'), 37 | 'urdf', 'test_pendulum_position.xacro.urdf'] 38 | ), 39 | ] 40 | ) 41 | robot_description = {'robot_description': robot_description_content} 42 | robot_controllers = PathJoinSubstitution( 43 | [ 44 | FindPackageShare('gz_ros2_control_demos'), 45 | 'config', 46 | 'cart_controller_position.yaml', 47 | ] 48 | ) 49 | 50 | node_robot_state_publisher = Node( 51 | package='robot_state_publisher', 52 | executable='robot_state_publisher', 53 | output='screen', 54 | parameters=[robot_description] 55 | ) 56 | 57 | gz_spawn_entity = Node( 58 | package='ros_gz_sim', 59 | executable='create', 60 | output='screen', 61 | arguments=['-topic', 'robot_description', 62 | '-name', 'cart', '-allow_renaming', 'true'], 63 | ) 64 | 65 | joint_state_broadcaster_spawner = Node( 66 | package='controller_manager', 67 | executable='spawner', 68 | arguments=['joint_state_broadcaster'], 69 | ) 70 | joint_trajectory_controller_spawner = Node( 71 | package='controller_manager', 72 | executable='spawner', 73 | arguments=[ 74 | 'joint_trajectory_controller', 75 | '--param-file', 76 | robot_controllers, 77 | ], 78 | ) 79 | 80 | # Bridge 81 | bridge = Node( 82 | package='ros_gz_bridge', 83 | executable='parameter_bridge', 84 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 85 | output='screen' 86 | ) 87 | 88 | return LaunchDescription([ 89 | # Launch gazebo environment 90 | IncludeLaunchDescription( 91 | PythonLaunchDescriptionSource( 92 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 93 | 'launch', 94 | 'gz_sim.launch.py'])]), 95 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 96 | RegisterEventHandler( 97 | event_handler=OnProcessExit( 98 | target_action=gz_spawn_entity, 99 | on_exit=[joint_state_broadcaster_spawner], 100 | ) 101 | ), 102 | RegisterEventHandler( 103 | event_handler=OnProcessExit( 104 | target_action=joint_state_broadcaster_spawner, 105 | on_exit=[joint_trajectory_controller_spawner], 106 | ) 107 | ), 108 | bridge, 109 | node_robot_state_publisher, 110 | gz_spawn_entity, 111 | # Launch Arguments 112 | DeclareLaunchArgument( 113 | 'use_sim_time', 114 | default_value=use_sim_time, 115 | description='If true, use simulated clock'), 116 | ]) 117 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/cart_example_effort.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | gz_args = LaunchConfiguration('gz_args', default='') 30 | 31 | # Get URDF via xacro 32 | robot_description_content = Command( 33 | [ 34 | PathJoinSubstitution([FindExecutable(name='xacro')]), 35 | ' ', 36 | PathJoinSubstitution( 37 | [FindPackageShare('gz_ros2_control_demos'), 38 | 'urdf', 'test_cart_effort.xacro.urdf'] 39 | ), 40 | ] 41 | ) 42 | robot_description = {'robot_description': robot_description_content} 43 | robot_controllers = PathJoinSubstitution( 44 | [ 45 | FindPackageShare('gz_ros2_control_demos'), 46 | 'config', 47 | 'cart_controller_effort.yaml', 48 | ] 49 | ) 50 | 51 | node_robot_state_publisher = Node( 52 | package='robot_state_publisher', 53 | executable='robot_state_publisher', 54 | output='screen', 55 | parameters=[robot_description] 56 | ) 57 | 58 | gz_spawn_entity = Node( 59 | package='ros_gz_sim', 60 | executable='create', 61 | output='screen', 62 | arguments=['-topic', 'robot_description', 63 | '-name', 'cart', '-allow_renaming', 'true'], 64 | ) 65 | 66 | joint_state_broadcaster_spawner = Node( 67 | package='controller_manager', 68 | executable='spawner', 69 | arguments=['joint_state_broadcaster'], 70 | ) 71 | joint_trajectory_controller_spawner = Node( 72 | package='controller_manager', 73 | executable='spawner', 74 | arguments=[ 75 | 'joint_trajectory_controller', 76 | '--param-file', 77 | robot_controllers, 78 | ], 79 | ) 80 | 81 | # Bridge 82 | bridge = Node( 83 | package='ros_gz_bridge', 84 | executable='parameter_bridge', 85 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 86 | output='screen' 87 | ) 88 | 89 | return LaunchDescription([ 90 | # Launch gazebo environment 91 | IncludeLaunchDescription( 92 | PythonLaunchDescriptionSource( 93 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 94 | 'launch', 95 | 'gz_sim.launch.py'])]), 96 | launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]), 97 | RegisterEventHandler( 98 | event_handler=OnProcessExit( 99 | target_action=gz_spawn_entity, 100 | on_exit=[joint_state_broadcaster_spawner], 101 | ) 102 | ), 103 | RegisterEventHandler( 104 | event_handler=OnProcessExit( 105 | target_action=joint_state_broadcaster_spawner, 106 | on_exit=[joint_trajectory_controller_spawner], 107 | ) 108 | ), 109 | bridge, 110 | node_robot_state_publisher, 111 | gz_spawn_entity, 112 | # Launch Arguments 113 | DeclareLaunchArgument( 114 | 'use_sim_time', 115 | default_value=use_sim_time, 116 | description='If true, use simulated clock'), 117 | ]) 118 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Denis Stogl (Stogl Robotics Consulting) 16 | # 17 | 18 | from launch import LaunchDescription 19 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 20 | from launch.actions import RegisterEventHandler 21 | from launch.event_handlers import OnProcessExit 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 24 | 25 | from launch_ros.actions import Node 26 | from launch_ros.substitutions import FindPackageShare 27 | 28 | 29 | def generate_launch_description(): 30 | # Launch Arguments 31 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 32 | 33 | # Get URDF via xacro 34 | robot_description_content = Command( 35 | [ 36 | PathJoinSubstitution([FindExecutable(name='xacro')]), 37 | ' ', 38 | PathJoinSubstitution( 39 | [FindPackageShare('gz_ros2_control_demos'), 40 | 'urdf', 'test_gripper_mimic_joint_effort.xacro.urdf'] 41 | ), 42 | ] 43 | ) 44 | robot_description = {'robot_description': robot_description_content} 45 | robot_controllers = PathJoinSubstitution( 46 | [ 47 | FindPackageShare('gz_ros2_control_demos'), 48 | 'config', 49 | 'gripper_controller_effort.yaml', 50 | ] 51 | ) 52 | 53 | node_robot_state_publisher = Node( 54 | package='robot_state_publisher', 55 | executable='robot_state_publisher', 56 | output='screen', 57 | parameters=[robot_description] 58 | ) 59 | 60 | gz_spawn_entity = Node( 61 | package='ros_gz_sim', 62 | executable='create', 63 | output='screen', 64 | arguments=['-topic', 'robot_description', '-name', 65 | 'gripper', '-allow_renaming', 'true'], 66 | ) 67 | 68 | joint_state_broadcaster_spawner = Node( 69 | package='controller_manager', 70 | executable='spawner', 71 | arguments=['joint_state_broadcaster'], 72 | ) 73 | gripper_controller_spawner = Node( 74 | package='controller_manager', 75 | executable='spawner', 76 | arguments=[ 77 | 'gripper_controller', 78 | '--param-file', 79 | robot_controllers, 80 | ], 81 | ) 82 | 83 | # Bridge 84 | bridge = Node( 85 | package='ros_gz_bridge', 86 | executable='parameter_bridge', 87 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 88 | output='screen' 89 | ) 90 | 91 | return LaunchDescription([ 92 | # Launch gazebo environment 93 | IncludeLaunchDescription( 94 | PythonLaunchDescriptionSource( 95 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 96 | 'launch', 97 | 'gz_sim.launch.py'])]), 98 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 99 | RegisterEventHandler( 100 | event_handler=OnProcessExit( 101 | target_action=gz_spawn_entity, 102 | on_exit=[joint_state_broadcaster_spawner], 103 | ) 104 | ), 105 | RegisterEventHandler( 106 | event_handler=OnProcessExit( 107 | target_action=joint_state_broadcaster_spawner, 108 | on_exit=[gripper_controller_spawner], 109 | ) 110 | ), 111 | bridge, 112 | node_robot_state_publisher, 113 | gz_spawn_entity, 114 | # Launch Arguments 115 | DeclareLaunchArgument( 116 | 'use_sim_time', 117 | default_value=use_sim_time, 118 | description='If true, use simulated clock'), 119 | ]) 120 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Denis Stogl (Stogl Robotics Consulting) 16 | # 17 | 18 | from launch import LaunchDescription 19 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 20 | from launch.actions import RegisterEventHandler 21 | from launch.event_handlers import OnProcessExit 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 24 | 25 | from launch_ros.actions import Node 26 | from launch_ros.substitutions import FindPackageShare 27 | 28 | 29 | def generate_launch_description(): 30 | # Launch Arguments 31 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 32 | 33 | # Get URDF via xacro 34 | robot_description_content = Command( 35 | [ 36 | PathJoinSubstitution([FindExecutable(name='xacro')]), 37 | ' ', 38 | PathJoinSubstitution( 39 | [FindPackageShare('gz_ros2_control_demos'), 40 | 'urdf', 'test_gripper_mimic_joint_position.xacro.urdf'] 41 | ), 42 | ] 43 | ) 44 | robot_description = {'robot_description': robot_description_content} 45 | robot_controllers = PathJoinSubstitution( 46 | [ 47 | FindPackageShare('gz_ros2_control_demos'), 48 | 'config', 49 | 'gripper_controller_position.yaml', 50 | ] 51 | ) 52 | 53 | node_robot_state_publisher = Node( 54 | package='robot_state_publisher', 55 | executable='robot_state_publisher', 56 | output='screen', 57 | parameters=[robot_description] 58 | ) 59 | 60 | gz_spawn_entity = Node( 61 | package='ros_gz_sim', 62 | executable='create', 63 | output='screen', 64 | arguments=['-topic', 'robot_description', '-name', 65 | 'gripper', '-allow_renaming', 'true'], 66 | ) 67 | 68 | joint_state_broadcaster_spawner = Node( 69 | package='controller_manager', 70 | executable='spawner', 71 | arguments=['joint_state_broadcaster'], 72 | ) 73 | gripper_controller_spawner = Node( 74 | package='controller_manager', 75 | executable='spawner', 76 | arguments=[ 77 | 'gripper_controller', 78 | '--param-file', 79 | robot_controllers, 80 | ], 81 | ) 82 | 83 | # Bridge 84 | bridge = Node( 85 | package='ros_gz_bridge', 86 | executable='parameter_bridge', 87 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 88 | output='screen' 89 | ) 90 | 91 | return LaunchDescription([ 92 | # Launch gazebo environment 93 | IncludeLaunchDescription( 94 | PythonLaunchDescriptionSource( 95 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 96 | 'launch', 97 | 'gz_sim.launch.py'])]), 98 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 99 | RegisterEventHandler( 100 | event_handler=OnProcessExit( 101 | target_action=gz_spawn_entity, 102 | on_exit=[joint_state_broadcaster_spawner], 103 | ) 104 | ), 105 | RegisterEventHandler( 106 | event_handler=OnProcessExit( 107 | target_action=joint_state_broadcaster_spawner, 108 | on_exit=[gripper_controller_spawner], 109 | ) 110 | ), 111 | bridge, 112 | node_robot_state_publisher, 113 | gz_spawn_entity, 114 | # Launch Arguments 115 | DeclareLaunchArgument( 116 | 'use_sim_time', 117 | default_value=use_sim_time, 118 | description='If true, use simulated clock'), 119 | ]) 120 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/cart_example_position.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | gz_args = LaunchConfiguration('gz_args', default='') 30 | 31 | # Get URDF via xacro 32 | robot_description_content = Command( 33 | [ 34 | PathJoinSubstitution([FindExecutable(name='xacro')]), 35 | ' ', 36 | PathJoinSubstitution( 37 | [FindPackageShare('gz_ros2_control_demos'), 38 | 'urdf', 'test_cart_position.xacro.urdf'] 39 | ), 40 | ] 41 | ) 42 | robot_description = {'robot_description': robot_description_content} 43 | robot_controllers = PathJoinSubstitution( 44 | [ 45 | FindPackageShare('gz_ros2_control_demos'), 46 | 'config', 47 | 'cart_controller_position.yaml', 48 | ] 49 | ) 50 | 51 | node_robot_state_publisher = Node( 52 | package='robot_state_publisher', 53 | executable='robot_state_publisher', 54 | output='screen', 55 | parameters=[robot_description] 56 | ) 57 | 58 | gz_spawn_entity = Node( 59 | package='ros_gz_sim', 60 | executable='create', 61 | output='screen', 62 | arguments=['-topic', 'robot_description', 63 | '-name', 'cart', '-allow_renaming', 'true'], 64 | ) 65 | 66 | joint_state_broadcaster_spawner = Node( 67 | package='controller_manager', 68 | executable='spawner', 69 | arguments=['joint_state_broadcaster', 70 | ], 71 | ) 72 | joint_trajectory_controller_spawner = Node( 73 | package='controller_manager', 74 | executable='spawner', 75 | arguments=[ 76 | 'joint_trajectory_controller', 77 | '--param-file', 78 | robot_controllers, 79 | ], 80 | ) 81 | 82 | # Bridge 83 | bridge = Node( 84 | package='ros_gz_bridge', 85 | executable='parameter_bridge', 86 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 87 | output='screen' 88 | ) 89 | 90 | return LaunchDescription([ 91 | # Launch gazebo environment 92 | IncludeLaunchDescription( 93 | PythonLaunchDescriptionSource( 94 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 95 | 'launch', 96 | 'gz_sim.launch.py'])]), 97 | launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]), 98 | RegisterEventHandler( 99 | event_handler=OnProcessExit( 100 | target_action=gz_spawn_entity, 101 | on_exit=[joint_state_broadcaster_spawner], 102 | ) 103 | ), 104 | RegisterEventHandler( 105 | event_handler=OnProcessExit( 106 | target_action=joint_state_broadcaster_spawner, 107 | on_exit=[joint_trajectory_controller_spawner], 108 | ) 109 | ), 110 | bridge, 111 | node_robot_state_publisher, 112 | gz_spawn_entity, 113 | # Launch Arguments 114 | DeclareLaunchArgument( 115 | 'use_sim_time', 116 | default_value=use_sim_time, 117 | description='If true, use simulated clock'), 118 | ]) 119 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/cart_example_velocity_custom_plugin.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 AIT Austrian Institute of Technology GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | gz_args = LaunchConfiguration('gz_args', default='') 30 | 31 | # Get URDF via xacro 32 | robot_description_content = Command( 33 | [ 34 | PathJoinSubstitution([FindExecutable(name='xacro')]), 35 | ' ', 36 | PathJoinSubstitution( 37 | [FindPackageShare('gz_ros2_control_demos'), 38 | 'urdf', 'test_cart_velocity_custom_plugin.xacro.urdf'] 39 | ), 40 | ] 41 | ) 42 | robot_description = {'robot_description': robot_description_content} 43 | robot_controllers = PathJoinSubstitution( 44 | [ 45 | FindPackageShare('gz_ros2_control_demos'), 46 | 'config', 47 | 'cart_controller_velocity.yaml', 48 | ] 49 | ) 50 | 51 | node_robot_state_publisher = Node( 52 | package='robot_state_publisher', 53 | executable='robot_state_publisher', 54 | output='screen', 55 | parameters=[robot_description] 56 | ) 57 | 58 | gz_spawn_entity = Node( 59 | package='ros_gz_sim', 60 | executable='create', 61 | output='screen', 62 | arguments=['-topic', 'robot_description', 63 | '-name', 'cart', '-allow_renaming', 'true'], 64 | ) 65 | 66 | joint_state_broadcaster_spawner = Node( 67 | package='controller_manager', 68 | executable='spawner', 69 | arguments=['joint_state_broadcaster'], 70 | ) 71 | joint_trajectory_controller_spawner = Node( 72 | package='controller_manager', 73 | executable='spawner', 74 | arguments=[ 75 | 'joint_trajectory_controller', 76 | '--param-file', 77 | robot_controllers, 78 | ], 79 | ) 80 | 81 | # Bridge 82 | bridge = Node( 83 | package='ros_gz_bridge', 84 | executable='parameter_bridge', 85 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 86 | output='screen' 87 | ) 88 | 89 | return LaunchDescription([ 90 | # Launch gazebo environment 91 | IncludeLaunchDescription( 92 | PythonLaunchDescriptionSource( 93 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 94 | 'launch', 95 | 'gz_sim.launch.py'])]), 96 | launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]), 97 | RegisterEventHandler( 98 | event_handler=OnProcessExit( 99 | target_action=gz_spawn_entity, 100 | on_exit=[joint_state_broadcaster_spawner], 101 | ) 102 | ), 103 | RegisterEventHandler( 104 | event_handler=OnProcessExit( 105 | target_action=joint_state_broadcaster_spawner, 106 | on_exit=[joint_trajectory_controller_spawner], 107 | ) 108 | ), 109 | bridge, 110 | node_robot_state_publisher, 111 | gz_spawn_entity, 112 | # Launch Arguments 113 | DeclareLaunchArgument( 114 | 'use_sim_time', 115 | default_value=use_sim_time, 116 | description='If true, use simulated clock'), 117 | ]) 118 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/tricycle_drive_example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | # Get URDF via xacro 31 | robot_description_content = Command( 32 | [ 33 | PathJoinSubstitution([FindExecutable(name='xacro')]), 34 | ' ', 35 | PathJoinSubstitution( 36 | [FindPackageShare('gz_ros2_control_demos'), 37 | 'urdf', 'test_tricycle_drive.xacro.urdf'] 38 | ), 39 | ] 40 | ) 41 | robot_description = {'robot_description': robot_description_content} 42 | robot_controllers = PathJoinSubstitution( 43 | [ 44 | FindPackageShare('gz_ros2_control_demos'), 45 | 'config', 46 | 'tricycle_drive_controller.yaml', 47 | ] 48 | ) 49 | 50 | node_robot_state_publisher = Node( 51 | package='robot_state_publisher', 52 | executable='robot_state_publisher', 53 | output='screen', 54 | parameters=[robot_description] 55 | ) 56 | 57 | gz_spawn_entity = Node( 58 | package='ros_gz_sim', 59 | executable='create', 60 | output='screen', 61 | arguments=['-topic', 'robot_description', '-name', 62 | 'tricyle', '-allow_renaming', 'true'], 63 | ) 64 | 65 | joint_state_broadcaster_spawner = Node( 66 | package='controller_manager', 67 | executable='spawner', 68 | arguments=[ 69 | 'joint_state_broadcaster', 70 | '--param-file', 71 | robot_controllers, 72 | ], 73 | ) 74 | tricycle_controller_spawner = Node( 75 | package='controller_manager', 76 | executable='spawner', 77 | arguments=[ 78 | 'tricycle_controller', 79 | '--param-file', 80 | robot_controllers, 81 | '--controller-ros-args', 82 | '-r /tricycle_controller/cmd_vel:=/cmd_vel' 83 | ], 84 | ) 85 | 86 | # Bridge 87 | bridge = Node( 88 | package='ros_gz_bridge', 89 | executable='parameter_bridge', 90 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 91 | output='screen' 92 | ) 93 | 94 | return LaunchDescription([ 95 | bridge, 96 | # Launch gazebo environment 97 | IncludeLaunchDescription( 98 | PythonLaunchDescriptionSource( 99 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 100 | 'launch', 101 | 'gz_sim.launch.py'])]), 102 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 103 | RegisterEventHandler( 104 | event_handler=OnProcessExit( 105 | target_action=gz_spawn_entity, 106 | on_exit=[joint_state_broadcaster_spawner], 107 | ) 108 | ), 109 | RegisterEventHandler( 110 | event_handler=OnProcessExit( 111 | target_action=joint_state_broadcaster_spawner, 112 | on_exit=[tricycle_controller_spawner], 113 | ) 114 | ), 115 | node_robot_state_publisher, 116 | gz_spawn_entity, 117 | # Launch Arguments 118 | DeclareLaunchArgument( 119 | 'use_sim_time', 120 | default_value=use_sim_time, 121 | description='If true, use simulated clock'), 122 | ]) 123 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/mecanum_drive_example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | # Get URDF via xacro 31 | robot_description_content = Command( 32 | [ 33 | PathJoinSubstitution([FindExecutable(name='xacro')]), 34 | ' ', 35 | PathJoinSubstitution( 36 | [FindPackageShare('gz_ros2_control_demos'), 37 | 'urdf', 'test_mecanum_drive.xacro.urdf'] 38 | ), 39 | ] 40 | ) 41 | robot_description = {'robot_description': robot_description_content} 42 | robot_controllers = PathJoinSubstitution( 43 | [ 44 | FindPackageShare('gz_ros2_control_demos'), 45 | 'config', 46 | 'mecanum_drive_controller.yaml', 47 | ] 48 | ) 49 | 50 | node_robot_state_publisher = Node( 51 | package='robot_state_publisher', 52 | executable='robot_state_publisher', 53 | output='screen', 54 | parameters=[robot_description] 55 | ) 56 | 57 | gz_spawn_entity = Node( 58 | package='ros_gz_sim', 59 | executable='create', 60 | output='screen', 61 | arguments=['-topic', 'robot_description', 62 | '-name', 'mecanum_vehicle', '-allow_renaming', 'true'], 63 | ) 64 | 65 | joint_state_broadcaster_spawner = Node( 66 | package='controller_manager', 67 | executable='spawner', 68 | arguments=['joint_state_broadcaster'], 69 | ) 70 | mecanum_drive_controller_spawner = Node( 71 | package='controller_manager', 72 | executable='spawner', 73 | arguments=[ 74 | 'mecanum_drive_controller', 75 | '--param-file', 76 | robot_controllers, 77 | '--controller-ros-args', 78 | '-r /mecanum_drive_controller/tf_odometry:=/tf', 79 | '--controller-ros-args', 80 | '-r /mecanum_drive_controller/reference:=/cmd_vel', 81 | ], 82 | ) 83 | 84 | # Bridge 85 | bridge = Node( 86 | package='ros_gz_bridge', 87 | executable='parameter_bridge', 88 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 89 | output='screen' 90 | ) 91 | 92 | return LaunchDescription([ 93 | # Launch gazebo environment 94 | IncludeLaunchDescription( 95 | PythonLaunchDescriptionSource( 96 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 97 | 'launch', 98 | 'gz_sim.launch.py'])]), 99 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 100 | RegisterEventHandler( 101 | event_handler=OnProcessExit( 102 | target_action=gz_spawn_entity, 103 | on_exit=[joint_state_broadcaster_spawner], 104 | ) 105 | ), 106 | RegisterEventHandler( 107 | event_handler=OnProcessExit( 108 | target_action=joint_state_broadcaster_spawner, 109 | on_exit=[mecanum_drive_controller_spawner], 110 | ) 111 | ), 112 | bridge, 113 | node_robot_state_publisher, 114 | gz_spawn_entity, 115 | # Launch Arguments 116 | DeclareLaunchArgument( 117 | 'use_sim_time', 118 | default_value=use_sim_time, 119 | description='If true, use simulated clock'), 120 | ]) 121 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | # Get URDF via xacro 31 | robot_description_content = Command( 32 | [ 33 | PathJoinSubstitution([FindExecutable(name='xacro')]), 34 | ' ', 35 | PathJoinSubstitution( 36 | [FindPackageShare('gz_ros2_control_demos'), 37 | 'urdf', 'test_diff_drive.xacro.urdf'] 38 | ), 39 | ' ', 40 | 'namespace:=r1', 41 | ] 42 | ) 43 | robot_description = {'robot_description': robot_description_content} 44 | robot_controllers = PathJoinSubstitution( 45 | [ 46 | FindPackageShare('gz_ros2_control_demos'), 47 | 'config', 48 | 'diff_drive_controller.yaml', 49 | ] 50 | ) 51 | 52 | node_robot_state_publisher = Node( 53 | package='robot_state_publisher', 54 | executable='robot_state_publisher', 55 | namespace='r1', 56 | output='screen', 57 | parameters=[robot_description] 58 | ) 59 | 60 | gz_spawn_entity = Node( 61 | package='ros_gz_sim', 62 | executable='create', 63 | namespace='r1', 64 | output='screen', 65 | arguments=['-topic', 'robot_description', '-name', 66 | 'diff_drive', '-allow_renaming', 'true'], 67 | ) 68 | 69 | joint_state_broadcaster_spawner = Node( 70 | package='controller_manager', 71 | executable='spawner', 72 | arguments=[ 73 | 'joint_state_broadcaster', 74 | '-c', '/r1/controller_manager' 75 | '--controller-ros-args', 76 | '-r /r1/diff_drive_controller/cmd_vel:=/r1/cmd_vel', 77 | ], 78 | ) 79 | diff_drive_base_controller_spawner = Node( 80 | package='controller_manager', 81 | executable='spawner', 82 | arguments=[ 83 | 'diff_drive_base_controller', 84 | '--param-file', 85 | robot_controllers, 86 | '-c', '/r1/controller_manager' 87 | ], 88 | ) 89 | 90 | # Bridge 91 | bridge = Node( 92 | package='ros_gz_bridge', 93 | executable='parameter_bridge', 94 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 95 | output='screen' 96 | ) 97 | 98 | return LaunchDescription([ 99 | # Launch gazebo environment 100 | IncludeLaunchDescription( 101 | PythonLaunchDescriptionSource( 102 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 103 | 'launch', 104 | 'gz_sim.launch.py'])]), 105 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 106 | RegisterEventHandler( 107 | event_handler=OnProcessExit( 108 | target_action=gz_spawn_entity, 109 | on_exit=[joint_state_broadcaster_spawner], 110 | ) 111 | ), 112 | RegisterEventHandler( 113 | event_handler=OnProcessExit( 114 | target_action=joint_state_broadcaster_spawner, 115 | on_exit=[diff_drive_base_controller_spawner], 116 | ) 117 | ), 118 | bridge, 119 | node_robot_state_publisher, 120 | gz_spawn_entity, 121 | # Launch Arguments 122 | DeclareLaunchArgument( 123 | 'use_sim_time', 124 | default_value=use_sim_time, 125 | description='If true, use simulated clock'), 126 | ]) 127 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/cart_example_velocity.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | gz_args = LaunchConfiguration('gz_args', default='') 30 | 31 | # Get URDF via xacro 32 | robot_description_content = Command( 33 | [ 34 | PathJoinSubstitution([FindExecutable(name='xacro')]), 35 | ' ', 36 | PathJoinSubstitution( 37 | [FindPackageShare('gz_ros2_control_demos'), 38 | 'urdf', 'test_cart_velocity.xacro.urdf'] 39 | ), 40 | ] 41 | ) 42 | robot_description = {'robot_description': robot_description_content} 43 | robot_controllers = PathJoinSubstitution( 44 | [ 45 | FindPackageShare('gz_ros2_control_demos'), 46 | 'config', 47 | 'cart_controller_velocity.yaml', 48 | ] 49 | ) 50 | 51 | node_robot_state_publisher = Node( 52 | package='robot_state_publisher', 53 | executable='robot_state_publisher', 54 | output='screen', 55 | parameters=[robot_description] 56 | ) 57 | 58 | gz_spawn_entity = Node( 59 | package='ros_gz_sim', 60 | executable='create', 61 | output='screen', 62 | arguments=['-topic', 'robot_description', 63 | '-name', 'cart', '-allow_renaming', 'true'], 64 | ) 65 | 66 | joint_state_broadcaster_spawner = Node( 67 | package='controller_manager', 68 | executable='spawner', 69 | arguments=['joint_state_broadcaster'], 70 | ) 71 | joint_trajectory_controller_spawner = Node( 72 | package='controller_manager', 73 | executable='spawner', 74 | arguments=[ 75 | 'joint_trajectory_controller', 76 | '--param-file', 77 | robot_controllers, 78 | ], 79 | ) 80 | imu_sensor_broadcaster_spawner = Node( 81 | package='controller_manager', 82 | executable='spawner', 83 | arguments=[ 84 | 'imu_sensor_broadcaster', 85 | '--param-file', 86 | robot_controllers, 87 | ], 88 | ) 89 | 90 | # Bridge 91 | bridge = Node( 92 | package='ros_gz_bridge', 93 | executable='parameter_bridge', 94 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 95 | output='screen' 96 | ) 97 | 98 | return LaunchDescription([ 99 | # Launch gazebo environment 100 | IncludeLaunchDescription( 101 | PythonLaunchDescriptionSource( 102 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 103 | 'launch', 104 | 'gz_sim.launch.py'])]), 105 | launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]), 106 | RegisterEventHandler( 107 | event_handler=OnProcessExit( 108 | target_action=gz_spawn_entity, 109 | on_exit=[joint_state_broadcaster_spawner], 110 | ) 111 | ), 112 | RegisterEventHandler( 113 | event_handler=OnProcessExit( 114 | target_action=joint_state_broadcaster_spawner, 115 | on_exit=[joint_trajectory_controller_spawner, 116 | imu_sensor_broadcaster_spawner], 117 | ) 118 | ), 119 | bridge, 120 | node_robot_state_publisher, 121 | gz_spawn_entity, 122 | # Launch Arguments 123 | DeclareLaunchArgument( 124 | 'use_sim_time', 125 | default_value=use_sim_time, 126 | description='If true, use simulated clock'), 127 | ]) 128 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 146 | 147 | 148 | 149 | 150 | 151 | gz_ros2_control/GazeboSimSystem 152 | 153 | 154 | 155 | -1 156 | 1 157 | 158 | 159 | 160 | 161 | 162 | 163 | -1 164 | 1 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | $(arg namespace) 175 | 176 | $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml 177 | 178 | 179 | 180 | 181 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/diff_drive_example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | def robot_state_publisher(context): 31 | performed_description_format = LaunchConfiguration('description_format').perform(context) 32 | # Get URDF or SDF via xacro 33 | robot_description_content = Command( 34 | [ 35 | PathJoinSubstitution([FindExecutable(name='xacro')]), 36 | ' ', 37 | PathJoinSubstitution([ 38 | FindPackageShare('gz_ros2_control_demos'), 39 | performed_description_format, 40 | f'test_diff_drive.xacro.{performed_description_format}' 41 | ]), 42 | ] 43 | ) 44 | robot_description = {'robot_description': robot_description_content} 45 | node_robot_state_publisher = Node( 46 | package='robot_state_publisher', 47 | executable='robot_state_publisher', 48 | output='screen', 49 | parameters=[robot_description] 50 | ) 51 | return [node_robot_state_publisher] 52 | 53 | robot_controllers = PathJoinSubstitution( 54 | [ 55 | FindPackageShare('gz_ros2_control_demos'), 56 | 'config', 57 | 'diff_drive_controller.yaml', 58 | ] 59 | ) 60 | 61 | gz_spawn_entity = Node( 62 | package='ros_gz_sim', 63 | executable='create', 64 | output='screen', 65 | arguments=['-topic', 'robot_description', '-name', 66 | 'diff_drive', '-allow_renaming', 'true'], 67 | ) 68 | 69 | joint_state_broadcaster_spawner = Node( 70 | package='controller_manager', 71 | executable='spawner', 72 | arguments=['joint_state_broadcaster'], 73 | ) 74 | diff_drive_base_controller_spawner = Node( 75 | package='controller_manager', 76 | executable='spawner', 77 | arguments=[ 78 | 'diff_drive_base_controller', 79 | '--param-file', 80 | robot_controllers, 81 | '--controller-ros-args', 82 | '-r /diff_drive_controller/cmd_vel:=/cmd_vel', 83 | ], 84 | ) 85 | 86 | # Bridge 87 | bridge = Node( 88 | package='ros_gz_bridge', 89 | executable='parameter_bridge', 90 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 91 | output='screen' 92 | ) 93 | 94 | ld = LaunchDescription([ 95 | # Launch gazebo environment 96 | IncludeLaunchDescription( 97 | PythonLaunchDescriptionSource( 98 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 99 | 'launch', 100 | 'gz_sim.launch.py'])]), 101 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 102 | RegisterEventHandler( 103 | event_handler=OnProcessExit( 104 | target_action=gz_spawn_entity, 105 | on_exit=[joint_state_broadcaster_spawner], 106 | ) 107 | ), 108 | RegisterEventHandler( 109 | event_handler=OnProcessExit( 110 | target_action=joint_state_broadcaster_spawner, 111 | on_exit=[diff_drive_base_controller_spawner], 112 | ) 113 | ), 114 | bridge, 115 | gz_spawn_entity, 116 | # Launch Arguments 117 | DeclareLaunchArgument( 118 | 'use_sim_time', 119 | default_value=use_sim_time, 120 | description='If true, use simulated clock'), 121 | DeclareLaunchArgument( 122 | 'description_format', 123 | default_value='urdf', 124 | description='Robot description format to use, urdf or sdf'), 125 | ]) 126 | ld.add_action(OpaqueFunction(function=robot_state_publisher)) 127 | return ld 128 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/ackermann_drive_example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | 30 | def robot_state_publisher(context): 31 | performed_description_format = LaunchConfiguration('description_format').perform(context) 32 | # Get URDF or SDF via xacro 33 | robot_description_content = Command( 34 | [ 35 | PathJoinSubstitution([FindExecutable(name='xacro')]), 36 | ' ', 37 | PathJoinSubstitution([ 38 | FindPackageShare('gz_ros2_control_demos'), 39 | performed_description_format, 40 | f'test_ackermann_drive.xacro.{performed_description_format}' 41 | ]), 42 | ] 43 | ) 44 | robot_description = {'robot_description': robot_description_content} 45 | node_robot_state_publisher = Node( 46 | package='robot_state_publisher', 47 | executable='robot_state_publisher', 48 | output='screen', 49 | parameters=[robot_description] 50 | ) 51 | return [node_robot_state_publisher] 52 | 53 | robot_controllers = PathJoinSubstitution( 54 | [ 55 | FindPackageShare('gz_ros2_control_demos'), 56 | 'config', 57 | 'ackermann_drive_controller.yaml', 58 | ] 59 | ) 60 | 61 | gz_spawn_entity = Node( 62 | package='ros_gz_sim', 63 | executable='create', 64 | output='screen', 65 | arguments=['-topic', 'robot_description', '-name', 66 | 'ackermann', '-allow_renaming', 'true'], 67 | ) 68 | 69 | joint_state_broadcaster_spawner = Node( 70 | package='controller_manager', 71 | executable='spawner', 72 | arguments=['joint_state_broadcaster'], 73 | ) 74 | ackermann_steering_controller_spawner = Node( 75 | package='controller_manager', 76 | executable='spawner', 77 | arguments=['ackermann_steering_controller', 78 | '--param-file', 79 | robot_controllers, 80 | '--controller-ros-args', 81 | '-r /ackermann_steering_controller/tf_odometry:=/tf', 82 | '--controller-ros-args', 83 | '-r /ackermann_steering_controller/reference:=/cmd_vel' 84 | ], 85 | ) 86 | 87 | # Bridge 88 | bridge = Node( 89 | package='ros_gz_bridge', 90 | executable='parameter_bridge', 91 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 92 | output='screen' 93 | ) 94 | 95 | ld = LaunchDescription([ 96 | bridge, 97 | # Launch gazebo environment 98 | IncludeLaunchDescription( 99 | PythonLaunchDescriptionSource( 100 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 101 | 'launch', 102 | 'gz_sim.launch.py'])]), 103 | launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]), 104 | RegisterEventHandler( 105 | event_handler=OnProcessExit( 106 | target_action=gz_spawn_entity, 107 | on_exit=[joint_state_broadcaster_spawner], 108 | ) 109 | ), 110 | RegisterEventHandler( 111 | event_handler=OnProcessExit( 112 | target_action=joint_state_broadcaster_spawner, 113 | on_exit=[ackermann_steering_controller_spawner], 114 | ) 115 | ), 116 | gz_spawn_entity, 117 | # Launch Arguments 118 | DeclareLaunchArgument( 119 | 'use_sim_time', 120 | default_value=use_sim_time, 121 | description='If true, use simulated clock'), 122 | DeclareLaunchArgument( 123 | 'description_format', 124 | default_value='urdf', 125 | description='Robot description format to use, urdf or sdf'), 126 | ]) 127 | ld.add_action(OpaqueFunction(function=robot_state_publisher)) 128 | return ld 129 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | gz_ros2_control/GazeboSimSystem 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | $(find gz_ros2_control_demos)/config/tricycle_drive_controller.yaml 172 | 173 | 174 | 175 | 176 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 17 | from launch.actions import RegisterEventHandler 18 | from launch.event_handlers import OnProcessExit 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 21 | 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | # Launch Arguments 28 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 29 | gz_args = LaunchConfiguration('gz_args', default='') 30 | 31 | # Get URDF via xacro 32 | robot_description_content = Command( 33 | [ 34 | PathJoinSubstitution([FindExecutable(name='xacro')]), 35 | ' ', 36 | PathJoinSubstitution( 37 | [FindPackageShare('gz_ros2_control_demos'), 38 | 'urdf', 'test_cart_ft_sensor.xacro.urdf'] 39 | ), 40 | ] 41 | ) 42 | robot_description = {'robot_description': robot_description_content} 43 | robot_controllers = PathJoinSubstitution( 44 | [ 45 | FindPackageShare('gz_ros2_control_demos'), 46 | 'config', 47 | 'cart_controller_ft_sensor.yaml', 48 | ] 49 | ) 50 | 51 | gazebo_world = PathJoinSubstitution( 52 | [ 53 | FindPackageShare('gz_ros2_control_demos'), 54 | 'worlds', 55 | 'empty_ft_sensor.sdf', 56 | ] 57 | ) 58 | 59 | node_robot_state_publisher = Node( 60 | package='robot_state_publisher', 61 | executable='robot_state_publisher', 62 | output='screen', 63 | parameters=[robot_description] 64 | ) 65 | 66 | gz_spawn_entity = Node( 67 | package='ros_gz_sim', 68 | executable='create', 69 | output='screen', 70 | arguments=['-topic', 'robot_description', 71 | '-name', 'cart', '-allow_renaming', 'true'], 72 | ) 73 | 74 | joint_state_broadcaster_spawner = Node( 75 | package='controller_manager', 76 | executable='spawner', 77 | arguments=['joint_state_broadcaster', 78 | ], 79 | ) 80 | joint_trajectory_controller_spawner = Node( 81 | package='controller_manager', 82 | executable='spawner', 83 | arguments=[ 84 | 'joint_trajectory_controller', 85 | '--param-file', 86 | robot_controllers, 87 | ], 88 | ) 89 | 90 | force_torque_sensor_broadcaster = Node( 91 | package='controller_manager', 92 | executable='spawner', 93 | arguments=[ 94 | 'force_torque_sensor_broadcaster', 95 | '--param-file', 96 | robot_controllers, 97 | ], 98 | ) 99 | 100 | # Bridge 101 | bridge = Node( 102 | package='ros_gz_bridge', 103 | executable='parameter_bridge', 104 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 105 | output='screen' 106 | ) 107 | 108 | return LaunchDescription([ 109 | # Launch gazebo environment 110 | IncludeLaunchDescription( 111 | PythonLaunchDescriptionSource( 112 | [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 113 | 'launch', 114 | 'gz_sim.launch.py'])]), 115 | launch_arguments=[('gz_args', [gz_args, ' -r -v 1 ', gazebo_world])]), 116 | RegisterEventHandler( 117 | event_handler=OnProcessExit( 118 | target_action=gz_spawn_entity, 119 | on_exit=[joint_state_broadcaster_spawner], 120 | ) 121 | ), 122 | RegisterEventHandler( 123 | event_handler=OnProcessExit( 124 | target_action=joint_state_broadcaster_spawner, 125 | on_exit=[joint_trajectory_controller_spawner], 126 | ) 127 | ), 128 | RegisterEventHandler( 129 | event_handler=OnProcessExit( 130 | target_action=joint_trajectory_controller_spawner, 131 | on_exit=[force_torque_sensor_broadcaster], 132 | ) 133 | ), 134 | bridge, 135 | node_robot_state_publisher, 136 | gz_spawn_entity, 137 | # Launch Arguments 138 | DeclareLaunchArgument( 139 | 'use_sim_time', 140 | default_value=use_sim_time, 141 | description='If true, use simulated clock'), 142 | ]) 143 | -------------------------------------------------------------------------------- /gz_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 1 109 | 10.0 110 | true 111 | imu 112 | 113 | 114 | 115 | 116 | 117 | gz_ros2_control/GazeboSimSystem 118 | 119 | 120 | 121 | -15 122 | 15 123 | 124 | 125 | 1.0 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 0 0.8 0 1 148 | 0 0.8 0 1 149 | 0 0.8 0 1 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 0 0 0.8 1 158 | 0 0 0.8 1 159 | 0 0 0.8 1 160 | 161 | 162 | 163 | 164 | 165 | 166 | $(find gz_ros2_control_demos)/config/cart_controller_velocity.yaml 167 | 168 | 171 | 172 | 173 | 174 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # gz_ros2_control 2 | 3 | License | Build Status | Package build 4 | :---------: | :----: | :----------: 5 | [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rdev__gz_ros2_control__ubuntu_noble_amd64)](https://build.ros2.org/job/Rdev__gz_ros2_control__ubuntu_noble_amd64/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary/) 6 | 7 | This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo](http://gazebosim.org/) simulator. 8 | More information about `ros2_control` can be found [here](https://control.ros.org/). 9 | 10 | This package provides a Gazebo-Sim system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. 11 | 12 | ## Compatibility Matrix 13 | 14 | ROS version | Gazebo version | Branch | Binaries hosted at | APT key 15 | -- | -- | -- | -- | -- 16 | Rolling | Jetty | [rolling](https://github.com/ros-controls/gz_ros2_control/tree/rolling) | [packages.ros.org](https://packages.ros.org) | `ros-rolling-gz-ros2-control` 17 | Kilted | Ionic | [kilted](https://github.com/ros-controls/gz_ros2_control/tree/kilted) | [packages.ros.org](https://packages.ros.org) | `ros-kilted-gz-ros2-control` 18 | Jazzy | Harmonic | [jazzy](https://github.com/ros-controls/gz_ros2_control/tree/jazzy) | [packages.ros.org](https://packages.ros.org) | `ros-jazzy-gz-ros2-control` 19 | Humble | Fortress | [humble](https://github.com/ros-controls/gz_ros2_control/tree/humble) | [packages.ros.org](https://packages.ros.org) | `ros-humble-gz-ros2-control` 20 | Humble | Harmonic | [humble](https://github.com/ros-controls/gz_ros2_control/tree/humble) | build from source | - 21 | 22 | ## Documentation 23 | 24 | See the [documentation file](doc/index.rst) or [control.ros.org](https://control.ros.org/rolling/doc/gz_ros2_control/doc/index.html) 25 | 26 | ## Contributing 27 | 28 | As an open-source project, we welcome each contributor, regardless of their background and experience. Pick a [PR](https://github.com/ros-controls/gz_ros2_control/pulls) and review it, or [create your own](https://github.com/ros-controls/gz_ros2_control/contribute)! 29 | If you are new to the project, please read the [contributing guide](https://control.ros.org/rolling/doc/contributing/contributing.html) for more information on how to get started. We are happy to help you with your first contribution. 30 | 31 | ## Build status 32 | 33 | ROS 2 Distro | Branch | Build status | Documentation 34 | :----------: | :----: | :----------: | :-----------: 35 | **Rolling** | [`rolling`](https://github.com/ros-controls/gz_ros2_control/tree/rolling) | [![gazebo_ros2_control CI - Rolling](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-rolling.yaml/badge.svg?branch=rolling)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-rolling.yaml)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__gz_ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__gz_ros2_control__ubuntu_noble_amd64/) [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary/) | [Documentation](https://control.ros.org/rolling/index.html)
[API Reference](https://control.ros.org/rolling/doc/api/index.html) 36 | **Kilted** | [`kilted`](https://github.com/ros-controls/gz_ros2_control/tree/kilted) | [![gazebo_ros2_control CI - Kilted](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-kilted.yaml/badge.svg?branch=rolling)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-kilted.yaml)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__gz_ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__gz_ros2_control__ubuntu_noble_amd64/) [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary/) | [Documentation](https://control.ros.org/kilted/index.html)
[API Reference](https://control.ros.org/kilted/doc/api/index.html) 37 | **Jazzy** | [`jazzy`](https://github.com/ros-controls/gz_ros2_control/tree/jazzy) | [![gazebo_ros2_control CI - Jazzy](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-jazzy.yaml/badge.svg?branch=rolling)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-jazzy.yaml)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__gz_ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__gz_ros2_control__ubuntu_noble_amd64/) [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__gz_ros2_control__ubuntu_noble_amd64__binary/) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) 38 | **Humble** | [`humble`](https://github.com/ros-controls/gz_ros2_control/tree/humble) | [![ign_ros2_control CI - Humble](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-humble.yaml/badge.svg?branch=humble)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci-humble.yaml)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__ign_ros2_control__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__ign_ros2_control__ubuntu_jammy_amd64/) [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__gz_ros2_control__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__gz_ros2_control__ubuntu_jammy_amd64__binary/) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) 39 | 40 | ## Acknowledgements 41 | 42 | The project has received major contributions from companies and institutions [listed on control.ros.org](https://control.ros.org/rolling/doc/acknowledgements/acknowledgements.html) 43 | --------------------------------------------------------------------------------