├── .github ├── CODEOWNERS └── workflows │ ├── build-and-test.sh │ ├── ros2-ci.yml │ └── triage.yml ├── .gitignore ├── .pre-commit-config.yaml ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── README_RENAME.md ├── RELEASING.md ├── img └── video_img.png ├── ros_gz ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md └── package.xml ├── ros_gz_bridge ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── bin │ ├── ros_gz_bridge_generate_factories │ ├── ros_gz_bridge_generate_tests │ └── ros_gz_bridge_markdown_table ├── images │ └── bridge_image_exchange.png ├── include │ └── ros_gz_bridge │ │ ├── bridge_config.hpp │ │ ├── convert.hpp │ │ ├── convert │ │ ├── actuator_msgs.hpp │ │ ├── builtin_interfaces.hpp │ │ ├── geometry_msgs.hpp │ │ ├── gps_msgs.hpp │ │ ├── nav_msgs.hpp │ │ ├── rcl_interfaces.hpp │ │ ├── ros_gz_interfaces.hpp │ │ ├── rosgraph_msgs.hpp │ │ ├── sensor_msgs.hpp │ │ ├── std_msgs.hpp │ │ ├── tf2_msgs.hpp │ │ ├── trajectory_msgs.hpp │ │ └── vision_msgs.hpp │ │ ├── convert_decl.hpp │ │ └── ros_gz_bridge.hpp ├── launch │ ├── clock_bridge.launch │ ├── ros_gz_bridge.launch │ └── ros_gz_bridge.launch.py ├── package.xml ├── resource │ ├── get_factory.cpp.em │ ├── get_mappings.cpp.em │ ├── pkg_factories.cpp.em │ ├── pkg_factories.hpp.em │ └── ros_gz_bridge ├── ros_gz_bridge │ ├── __init__.py │ ├── actions │ │ ├── __init__.py │ │ └── ros_gz_bridge.py │ └── mappings.py ├── setup.cfg ├── src │ ├── bridge_config.cpp │ ├── bridge_handle.cpp │ ├── bridge_handle.hpp │ ├── bridge_handle_gz_to_ros.cpp │ ├── bridge_handle_gz_to_ros.hpp │ ├── bridge_handle_ros_to_gz.cpp │ ├── bridge_handle_ros_to_gz.hpp │ ├── bridge_types.cpp │ ├── convert │ │ ├── actuator_msgs.cpp │ │ ├── builtin_interfaces.cpp │ │ ├── geometry_msgs.cpp │ │ ├── gps_msgs.cpp │ │ ├── nav_msgs.cpp │ │ ├── rcl_interfaces.cpp │ │ ├── rcl_interfaces_TEST.cpp │ │ ├── ros_gz_interfaces.cpp │ │ ├── ros_gz_interfaces_TEST.cpp │ │ ├── rosgraph_msgs.cpp │ │ ├── sensor_msgs.cpp │ │ ├── std_msgs.cpp │ │ ├── tf2_msgs.cpp │ │ ├── trajectory_msgs.cpp │ │ ├── utils.cpp │ │ ├── utils.hpp │ │ └── vision_msgs.cpp │ ├── factory.hpp │ ├── factory_interface.cpp │ ├── factory_interface.hpp │ ├── get_factory.hpp │ ├── get_mappings.hpp │ ├── parameter_bridge.cpp │ ├── ros_gz_bridge.cpp │ ├── service_factories │ │ ├── ros_gz_interfaces.cpp │ │ └── ros_gz_interfaces.hpp │ ├── service_factory.hpp │ ├── service_factory_interface.cpp │ ├── service_factory_interface.hpp │ └── static_bridge.cpp └── test │ ├── bridge_config.cpp │ ├── config │ ├── empty.yaml │ ├── full.yaml │ ├── invalid.yaml │ └── minimum.yaml │ ├── launch │ ├── test_gz_server.launch.py │ ├── test_gz_subscriber.launch.py │ ├── test_launch_action.launch │ ├── test_launch_action.launch.py │ └── test_ros_subscriber.launch.py │ ├── resource │ ├── gz_publisher.cpp.em │ ├── gz_subscriber.cpp.em │ ├── ros_pkg_subscriber.cpp.em │ └── ros_publisher.cpp.em │ ├── serverclient │ ├── gz_server.cpp │ └── ros_client.cpp │ ├── subscribers │ ├── launch_action_subscriber.cpp │ ├── ros_subscriber.cpp │ ├── ros_subscriber.hpp │ └── ros_subscriber │ │ └── ros_gz_interfaces_subscriber.cpp │ └── utils │ ├── gz_test_msg.cpp │ ├── gz_test_msg.hpp │ ├── ros_test_msg.cpp │ ├── ros_test_msg.hpp │ └── test_utils.hpp ├── ros_gz_image ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── package.xml ├── src │ └── image_bridge.cpp └── test │ ├── AMENT_IGNORE │ ├── launch │ └── test_ros_subscriber.launch │ ├── publishers │ └── gz_publisher.cpp │ ├── ros_subscriber.test │ ├── subscribers │ └── ros_subscriber.cpp │ └── test_utils.h ├── ros_gz_interfaces ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── msg │ ├── Altimeter.msg │ ├── Contact.msg │ ├── Contacts.msg │ ├── Dataframe.msg │ ├── Entity.msg │ ├── EntityFactory.msg │ ├── EntityWrench.msg │ ├── Float32Array.msg │ ├── GuiCamera.msg │ ├── JointWrench.msg │ ├── Light.msg │ ├── LogicalCameraImage.msg │ ├── LogicalCameraImageModel.msg │ ├── MaterialColor.msg │ ├── ParamVec.msg │ ├── SensorNoise.msg │ ├── StringVec.msg │ ├── TrackVisual.msg │ ├── VideoRecord.msg │ ├── WorldControl.msg │ └── WorldReset.msg ├── package.xml └── srv │ ├── ControlWorld.srv │ ├── DeleteEntity.srv │ ├── SetEntityPose.srv │ └── SpawnEntity.srv ├── ros_gz_point_cloud ├── CHANGELOG.rst ├── CMakeLists.txt ├── COLCON_IGNORE ├── examples │ ├── depth_camera.sdf │ ├── gpu_lidar.sdf │ └── rgbd_camera.sdf ├── package.xml └── src │ ├── point_cloud.cc │ └── point_cloud.hh ├── ros_gz_sim ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── ros_gz_sim │ │ ├── Stopwatch.hpp │ │ ├── delete_entity.hpp │ │ ├── gzserver.hpp │ │ ├── set_entity_pose.hpp │ │ └── spawn_entity.hpp ├── launch │ ├── gz_remove_model.launch.py │ ├── gz_server.launch │ ├── gz_server.launch.py │ ├── gz_sim.launch.py.in │ ├── gz_spawn_model.launch │ ├── gz_spawn_model.launch.py │ ├── ros_gz_sim.launch │ ├── ros_gz_sim.launch.py │ ├── ros_gz_spawn_model.launch │ └── ros_gz_spawn_model.launch.py ├── package.xml ├── resource │ └── ros_gz_sim ├── ros_gz_sim │ ├── __init__.py │ └── actions │ │ ├── __init__.py │ │ ├── gz_spawn_model.py │ │ └── gzserver.py ├── setup.cfg ├── src │ ├── Stopwatch.cpp │ ├── create.cpp │ ├── delete_entity.cpp │ ├── gzserver.cpp │ ├── remove.cpp │ ├── set_entity_pose.cpp │ └── spawn_entity.cpp └── test │ ├── test_create.cpp │ ├── test_create_node.launch.py │ ├── test_delete_entity.cpp │ ├── test_remove.cpp │ ├── test_remove_node.launch.py │ ├── test_set_entity_pose.cpp │ ├── test_spawn_entity.cpp │ └── test_stopwatch.cpp ├── ros_gz_sim_demos ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ ├── air_pressure.yaml │ ├── battery.yaml │ ├── camera.yaml │ ├── diff_drive.yaml │ ├── gpu_lidar.yaml │ ├── imu.yaml │ ├── magnetometer.yaml │ ├── navsat.yaml │ ├── navsat_gpsfix.yaml │ ├── rgbd_camera_bridge.yaml │ ├── tf_bridge.yaml │ └── triggered_camera.yaml ├── hooks │ └── ros_gz_sim_demos.dsv.in ├── images │ ├── air_pressure_demo.png │ ├── battery_demo.png │ ├── camera_demo.png │ ├── depth_camera_demo.png │ ├── diff_drive_demo.png │ ├── gpu_lidar_demo.png │ ├── imu_demo.png │ ├── joint_states.png │ ├── magnetometer_demo.png │ ├── navsat_demo.png │ ├── rgbd_camera_demo.png │ ├── robot_state_publisher_demo.png │ ├── shapes_demo.png │ └── tf_bridge.gif ├── launch │ ├── air_pressure.launch.xml │ ├── battery.launch.py │ ├── camera.launch.xml │ ├── depth_camera.launch.py │ ├── diff_drive.launch.xml │ ├── gpu_lidar.launch.py │ ├── gpu_lidar_bridge.launch.xml │ ├── image_bridge.launch.py │ ├── imu.launch.xml │ ├── joint_states.launch.py │ ├── magnetometer.launch.xml │ ├── navsat.launch.xml │ ├── navsat_gpsfix.launch.xml │ ├── rgbd_camera.launch.py │ ├── rgbd_camera_bridge.launch.xml │ ├── robot_description_publisher.launch.py │ ├── sdf_parser.launch.py │ ├── tf_bridge.launch.xml │ └── triggered_camera.launch.xml ├── models │ ├── cardboard_box │ │ ├── materials │ │ │ └── textures │ │ │ │ └── cardboard_box.png │ │ ├── meshes │ │ │ └── cardboard_box.dae │ │ ├── model.config │ │ ├── model.sdf │ │ └── thumbnails │ │ │ ├── 1.png │ │ │ ├── 2.png │ │ │ ├── 3.png │ │ │ ├── 4.png │ │ │ └── 5.png │ ├── double_pendulum_model.sdf │ ├── rrbot.xacro │ └── vehicle │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── resources │ ├── delete_entity.gif │ ├── set_entity.gif │ └── spawn.gif ├── rviz │ ├── camera.rviz │ ├── depth_camera.rviz │ ├── diff_drive.rviz │ ├── gpu_lidar.rviz │ ├── gpu_lidar_bridge.rviz │ ├── imu.rviz │ ├── joint_states.rviz │ ├── rgbd_camera.rviz │ ├── rgbd_camera_bridge.rviz │ ├── robot_description_publisher.rviz │ ├── tf_bridge.rviz │ └── vehicle.rviz └── worlds │ ├── default.sdf │ └── vehicle.sdf ├── test_ros_gz_bridge ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ └── test_ros_gz_bridge.cpp └── tools ├── .codespell_ignore_words └── pyproject.toml /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | # More info: 2 | # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners 3 | 4 | * @adityapande-1995 5 | * @ahcorde 6 | -------------------------------------------------------------------------------- /.github/workflows/build-and-test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -ev 3 | 4 | # Configuration. 5 | export COLCON_WS=~/ws 6 | export COLCON_WS_SRC=${COLCON_WS}/src 7 | export DEBIAN_FRONTEND=noninteractive 8 | export ROS_PYTHON_VERSION=3 9 | 10 | apt update -qq 11 | apt install -qq -y lsb-release wget curl build-essential 12 | 13 | echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list 14 | wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - 15 | 16 | # Dependencies. 17 | echo "deb http://packages.ros.org/ros2-testing/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-testing.list 18 | curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - 19 | apt-get update -qq 20 | apt-get install -y python3-colcon-common-extensions \ 21 | python3-rosdep \ 22 | libcli11-dev 23 | 24 | rosdep init 25 | rosdep update 26 | rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO $ROSDEP_ARGS 27 | 28 | # Build. 29 | source /opt/ros/$ROS_DISTRO/setup.bash 30 | mkdir -p $COLCON_WS_SRC 31 | cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC 32 | cd $COLCON_WS 33 | colcon build --event-handlers console_direct+ 34 | 35 | # Tests. 36 | colcon test --event-handlers console_direct+ 37 | colcon test-result 38 | -------------------------------------------------------------------------------- /.github/workflows/ros2-ci.yml: -------------------------------------------------------------------------------- 1 | name: ROS2 CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | pre-commit: 7 | name: Pre-commit 8 | runs-on: ubuntu-latest 9 | steps: 10 | - name: Checkout 11 | uses: actions/checkout@v4 12 | 13 | - name: Set up Python 14 | uses: actions/setup-python@v4 15 | 16 | - name: Run pre-commit 17 | uses: pre-commit/action@v3.0.1 18 | 19 | ros_gz_ci: 20 | name: ros_gz CI 21 | runs-on: ubuntu-latest 22 | strategy: 23 | fail-fast: false 24 | matrix: 25 | include: 26 | - docker-image: "ubuntu:24.04" 27 | gz-version: "harmonic" 28 | ros-distro: "rolling" 29 | container: 30 | image: ${{ matrix.docker-image }} 31 | steps: 32 | - name: Checkout 33 | uses: actions/checkout@v4 34 | - name: Build and Test 35 | run: .github/workflows/build-and-test.sh 36 | env: 37 | DOCKER_IMAGE: ${{ matrix.docker-image }} 38 | GZ_VERSION: ${{ matrix.gz-version }} 39 | ROS_DISTRO: ${{ matrix.ros-distro }} 40 | -------------------------------------------------------------------------------- /.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: actions/add-to-project@v0.5.0 14 | with: 15 | project-url: https://github.com/orgs/gazebosim/projects/7 16 | github-token: ${{ secrets.TRIAGE_TOKEN }} 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | **/__pycache__ 2 | .vscode 3 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v5.0.0 4 | hooks: 5 | - id: check-added-large-files 6 | args: ['--maxkb=500'] 7 | - id: check-merge-conflict 8 | - id: check-xml 9 | - id: check-yaml 10 | - id: end-of-file-fixer 11 | - id: mixed-line-ending 12 | - id: trailing-whitespace 13 | 14 | - repo: https://github.com/codespell-project/codespell 15 | rev: v2.4.1 16 | hooks: 17 | - id: codespell 18 | additional_dependencies: 19 | - tomli 20 | args: 21 | [--toml=./tools/pyproject.toml] 22 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | -------------------------------------------------------------------------------- /README_RENAME.md: -------------------------------------------------------------------------------- 1 | # Description 2 | 3 | These are shim packages for `ros_gz` that redirect executable calls to the related `ros_gz` executable. 4 | These require `ros_gz` to be installed though, but are set up to depend on `ros_gz`. 5 | 6 | This allows users to do either of these and get equivalent behavior: 7 | 8 | ```bash 9 | ros2 run ros_gz parameter_bridge [...] 10 | ros2 run ros_gz parameter_bridge [...] # Will emit deprecation warning 11 | ``` 12 | 13 | Additionally, installed files like launch files, message interfaces etc. are **duplicated** versions of the ones in `ros_gz` (but renamed as appropriate), and point to `ros_gz` dependencies as well (e.g. launch files pointing to `ros_gz` nodes.) 14 | -------------------------------------------------------------------------------- /img/video_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/img/video_img.png -------------------------------------------------------------------------------- /ros_gz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ros_gz) 3 | find_package(ament_cmake REQUIRED) 4 | 5 | if(BUILD_TESTING) 6 | find_package(ament_lint_auto REQUIRED) 7 | ament_lint_auto_find_test_dependencies() 8 | endif() 9 | 10 | ament_package() 11 | -------------------------------------------------------------------------------- /ros_gz/README.md: -------------------------------------------------------------------------------- 1 | # ROS-Gazebo packages 2 | 3 | `ros_gz` is a metapackage that installs all packages integrating 4 | [ROS](http://www.ros.org/) and [Gazebo](https://gazebosim.org): 5 | -------------------------------------------------------------------------------- /ros_gz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | ros_gz 7 | 3.0.1 8 | Meta-package containing interfaces for using ROS 2 with Gazebo simulation. 9 | Aditya Pande 10 | Alejandro Hernandez 11 | Apache 2.0 12 | 13 | Louise Poubel 14 | 15 | ament_cmake 16 | 17 | ros_gz_bridge 18 | ros_gz_sim 19 | ros_gz_sim_demos 20 | ros_gz_image 21 | 22 | 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /ros_gz_bridge/bin/ros_gz_bridge_generate_factories: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2022 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 | 17 | import argparse 18 | import os 19 | import sys 20 | 21 | # add ros_gz_bridge to the Python path 22 | ros_gz_bridge_root = os.path.join(os.path.dirname(__file__), '..') 23 | sys.path.insert(0, os.path.abspath(ros_gz_bridge_root)) 24 | 25 | from ros_gz_bridge import generate_cpp 26 | 27 | 28 | def main(argv=sys.argv[1:]): 29 | parser = argparse.ArgumentParser( 30 | description='Generate C++ template specializations.', 31 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 32 | parser.add_argument( 33 | '--output-path', 34 | required=True, 35 | help='The basepath of the generated C++ files') 36 | parser.add_argument( 37 | '--template-dir', 38 | required=True, 39 | help='The location of the template files') 40 | parser.add_argument( 41 | '--gz-msgs-ver', 42 | required=False, 43 | default="0.0.0", 44 | help='Gazebo messages version') 45 | 46 | args = parser.parse_args(argv) 47 | msgs_ver = tuple(map(int, args.gz_msgs_ver.split('.'))) 48 | 49 | try: 50 | return generate_cpp( 51 | args.output_path, 52 | args.template_dir, 53 | msgs_ver, 54 | ) 55 | except RuntimeError as e: 56 | print(str(e), file=sys.stderr) 57 | return 1 58 | 59 | 60 | if __name__ == '__main__': 61 | sys.exit(main()) 62 | -------------------------------------------------------------------------------- /ros_gz_bridge/bin/ros_gz_bridge_generate_tests: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2022 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 | 17 | import argparse 18 | import os 19 | import sys 20 | 21 | # add ros_gz_bridge to the Python path 22 | ros_gz_bridge_root = os.path.join(os.path.dirname(__file__), '..') 23 | sys.path.insert(0, os.path.abspath(ros_gz_bridge_root)) 24 | 25 | from ros_gz_bridge import generate_test_cpp 26 | 27 | 28 | def main(argv=sys.argv[1:]): 29 | parser = argparse.ArgumentParser( 30 | description='Generate C++ template specializations.', 31 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 32 | parser.add_argument( 33 | '--output-path', 34 | required=True, 35 | help='The basepath of the generated C++ files') 36 | parser.add_argument( 37 | '--template-dir', 38 | required=True, 39 | help='The location of the template files') 40 | parser.add_argument( 41 | '--gz-msgs-ver', 42 | required=False, 43 | default="0.0.0", 44 | help='Gazebo messages version') 45 | 46 | args = parser.parse_args(argv) 47 | msgs_ver = tuple(map(int, args.gz_msgs_ver.split('.'))) 48 | 49 | try: 50 | return generate_test_cpp( 51 | args.output_path, 52 | args.template_dir, 53 | msgs_ver 54 | ) 55 | except RuntimeError as e: 56 | print(str(e), file=sys.stderr) 57 | return 1 58 | 59 | 60 | if __name__ == '__main__': 61 | sys.exit(main()) 62 | -------------------------------------------------------------------------------- /ros_gz_bridge/bin/ros_gz_bridge_markdown_table: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2022 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 argparse 17 | import os 18 | import sys 19 | 20 | # add ros_gz_bridge to the Python path 21 | ros_gz_bridge_root = os.path.join(os.path.dirname(__file__), '..') 22 | sys.path.insert(0, os.path.abspath(ros_gz_bridge_root)) 23 | 24 | from ros_gz_bridge import mappings 25 | 26 | def main(argv=sys.argv[1:]): 27 | parser = argparse.ArgumentParser( 28 | description='Generate Markdown table of supported messages.', 29 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 30 | parser.add_argument( 31 | '--gz-msgs-ver', 32 | required=False, 33 | default="0.0.0", 34 | help='Gazebo messages version') 35 | 36 | args = parser.parse_args(argv) 37 | msgs_ver = tuple(map(int, args.gz_msgs_ver.split('.'))) 38 | 39 | rows = [] 40 | rows.append(f'| {"ROS type":35}| {"Gazebo Transport Type":35}|') 41 | rows.append(f'|{"-":-<36}|:{"-":-<34}:|') 42 | 43 | for mapping in mappings(msgs_ver): 44 | rows.append('| {:35}| {:35}|'.format( 45 | mapping.ros2_package_name + '/msg/' + mapping.ros2_message_name, 46 | mapping.gz_string())) 47 | print('\n'.join(rows)) 48 | 49 | 50 | if __name__ == '__main__': 51 | sys.exit(main()) 52 | -------------------------------------------------------------------------------- /ros_gz_bridge/images/bridge_image_exchange.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_bridge/images/bridge_image_exchange.png -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 ROS_GZ_BRIDGE__CONVERT_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #endif // ROS_GZ_BRIDGE__CONVERT_HPP_ 30 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/actuator_msgs.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Rudis Laboratories LLC 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 ROS_GZ_BRIDGE__CONVERT__ACTUATOR_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__ACTUATOR_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | 21 | // ROS 2 messages 22 | #include 23 | 24 | #include 25 | 26 | namespace ros_gz_bridge 27 | { 28 | // actuator_msgs 29 | template<> 30 | void 31 | convert_ros_to_gz( 32 | const actuator_msgs::msg::Actuators & ros_msg, 33 | gz::msgs::Actuators & gz_msg); 34 | 35 | template<> 36 | void 37 | convert_gz_to_ros( 38 | const gz::msgs::Actuators & gz_msg, 39 | actuator_msgs::msg::Actuators & ros_msg); 40 | 41 | } // namespace ros_gz_bridge 42 | 43 | #endif // ROS_GZ_BRIDGE__CONVERT__ACTUATOR_MSGS_HPP_ 44 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/builtin_interfaces.hpp: -------------------------------------------------------------------------------- 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 | #ifndef ROS_GZ_BRIDGE__CONVERT__BUILTIN_INTERFACES_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__BUILTIN_INTERFACES_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include "ros_gz_bridge/convert_decl.hpp" 23 | 24 | namespace ros_gz_bridge 25 | { 26 | 27 | template<> 28 | void 29 | convert_ros_to_gz( 30 | const builtin_interfaces::msg::Time & ros_msg, 31 | gz::msgs::Time & gz_msg); 32 | 33 | template<> 34 | void 35 | convert_gz_to_ros( 36 | const gz::msgs::Time & gz_msg, 37 | builtin_interfaces::msg::Time & ros_msg); 38 | 39 | } // namespace ros_gz_bridge 40 | 41 | #endif // ROS_GZ_BRIDGE__CONVERT__BUILTIN_INTERFACES_HPP_ 42 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp: -------------------------------------------------------------------------------- 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 | #ifndef ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | 21 | // ROS 2 messages 22 | #include 23 | 24 | #include 25 | 26 | namespace ros_gz_bridge 27 | { 28 | template<> 29 | void 30 | convert_ros_to_gz( 31 | const gps_msgs::msg::GPSFix & ros_msg, 32 | gz::msgs::NavSat & gz_msg); 33 | 34 | template<> 35 | void 36 | convert_gz_to_ros( 37 | const gz::msgs::NavSat & gz_msg, 38 | gps_msgs::msg::GPSFix & ros_msg); 39 | } // namespace ros_gz_bridge 40 | 41 | #endif // ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_ 42 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/nav_msgs.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 ROS_GZ_BRIDGE__CONVERT__NAV_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__NAV_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | #include 21 | 22 | // ROS 2 messages 23 | #include 24 | 25 | #include 26 | 27 | namespace ros_gz_bridge 28 | { 29 | // nav_msgs 30 | template<> 31 | void 32 | convert_ros_to_gz( 33 | const nav_msgs::msg::Odometry & ros_msg, 34 | gz::msgs::Odometry & gz_msg); 35 | 36 | template<> 37 | void 38 | convert_gz_to_ros( 39 | const gz::msgs::Odometry & gz_msg, 40 | nav_msgs::msg::Odometry & ros_msg); 41 | 42 | template<> 43 | void 44 | convert_ros_to_gz( 45 | const nav_msgs::msg::Odometry & ros_msg, 46 | gz::msgs::OdometryWithCovariance & gz_msg); 47 | 48 | template<> 49 | void 50 | convert_gz_to_ros( 51 | const gz::msgs::OdometryWithCovariance & gz_msg, 52 | nav_msgs::msg::Odometry & ros_msg); 53 | 54 | } // namespace ros_gz_bridge 55 | 56 | #endif // ROS_GZ_BRIDGE__CONVERT__NAV_MSGS_HPP_ 57 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/rcl_interfaces.hpp: -------------------------------------------------------------------------------- 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 | #ifndef ROS_GZ_BRIDGE__CONVERT__RCL_INTERFACES_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__RCL_INTERFACES_HPP_ 17 | 18 | // GZ messages 19 | #include 20 | 21 | // ROS 2 messages 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace ros_gz_bridge 29 | { 30 | 31 | template<> 32 | void 33 | convert_ros_to_gz( 34 | const rcl_interfaces::msg::ParameterValue & ros_msg, 35 | gz::msgs::Any & ign_msg); 36 | 37 | template<> 38 | void 39 | convert_gz_to_ros( 40 | const gz::msgs::Any & ign_msg, 41 | rcl_interfaces::msg::ParameterValue & ros_msg); 42 | 43 | } // namespace ros_gz_bridge 44 | #endif // ROS_GZ_BRIDGE__CONVERT__RCL_INTERFACES_HPP_ 45 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/rosgraph_msgs.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 ROS_GZ_BRIDGE__CONVERT__ROSGRAPH_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__ROSGRAPH_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | 21 | // ROS 2 messages 22 | #include 23 | 24 | #include 25 | 26 | namespace ros_gz_bridge 27 | { 28 | 29 | template<> 30 | void 31 | convert_gz_to_ros( 32 | const gz::msgs::Clock & gz_msg, 33 | rosgraph_msgs::msg::Clock & ros_msg); 34 | 35 | template<> 36 | void 37 | convert_ros_to_gz( 38 | const rosgraph_msgs::msg::Clock & ros_msg, 39 | gz::msgs::Clock & gz_msg); 40 | 41 | } // namespace ros_gz_bridge 42 | 43 | #endif // ROS_GZ_BRIDGE__CONVERT__ROSGRAPH_MSGS_HPP_ 44 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/tf2_msgs.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 ROS_GZ_BRIDGE__CONVERT__TF2_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__TF2_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | 21 | // ROS 2 messages 22 | #include 23 | 24 | #include 25 | 26 | namespace ros_gz_bridge 27 | { 28 | 29 | template<> 30 | void 31 | convert_ros_to_gz( 32 | const tf2_msgs::msg::TFMessage & ros_msg, 33 | gz::msgs::Pose_V & gz_msg); 34 | 35 | template<> 36 | void 37 | convert_gz_to_ros( 38 | const gz::msgs::Pose_V & gz_msg, 39 | tf2_msgs::msg::TFMessage & ros_msg); 40 | 41 | } // namespace ros_gz_bridge 42 | 43 | #endif // ROS_GZ_BRIDGE__CONVERT__TF2_MSGS_HPP_ 44 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/trajectory_msgs.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 ROS_GZ_BRIDGE__CONVERT__TRAJECTORY_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__TRAJECTORY_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | 21 | // ROS 2 messages 22 | #include 23 | 24 | #include 25 | 26 | namespace ros_gz_bridge 27 | { 28 | 29 | template<> 30 | void 31 | convert_ros_to_gz( 32 | const trajectory_msgs::msg::JointTrajectoryPoint & ros_msg, 33 | gz::msgs::JointTrajectoryPoint & gz_msg); 34 | 35 | template<> 36 | void 37 | convert_gz_to_ros( 38 | const gz::msgs::JointTrajectoryPoint & gz_msg, 39 | trajectory_msgs::msg::JointTrajectoryPoint & ros_msg); 40 | 41 | template<> 42 | void 43 | convert_ros_to_gz( 44 | const trajectory_msgs::msg::JointTrajectory & ros_msg, 45 | gz::msgs::JointTrajectory & gz_msg); 46 | 47 | template<> 48 | void 49 | convert_gz_to_ros( 50 | const gz::msgs::JointTrajectory & gz_msg, 51 | trajectory_msgs::msg::JointTrajectory & ros_msg); 52 | 53 | 54 | } // namespace ros_gz_bridge 55 | 56 | #endif // ROS_GZ_BRIDGE__CONVERT__TRAJECTORY_MSGS_HPP_ 57 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 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 ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_ 17 | 18 | // Gazebo Msgs 19 | #include 20 | #include 21 | 22 | // ROS 2 messages 23 | #include "vision_msgs/msg/detection2_d_array.hpp" 24 | #include "vision_msgs/msg/detection3_d_array.hpp" 25 | #include 26 | 27 | namespace ros_gz_bridge 28 | { 29 | template<> 30 | void 31 | convert_ros_to_gz( 32 | const vision_msgs::msg::Detection2D & ros_msg, 33 | gz::msgs::AnnotatedAxisAligned2DBox & gz_msg); 34 | 35 | template<> 36 | void 37 | convert_gz_to_ros( 38 | const gz::msgs::AnnotatedAxisAligned2DBox & gz_msg, 39 | vision_msgs::msg::Detection2D & ros_msg); 40 | 41 | template<> 42 | void 43 | convert_ros_to_gz( 44 | const vision_msgs::msg::Detection2DArray & ros_msg, 45 | gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg); 46 | 47 | template<> 48 | void 49 | convert_gz_to_ros( 50 | const gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg, 51 | vision_msgs::msg::Detection2DArray & ros_msg); 52 | 53 | template<> 54 | void 55 | convert_ros_to_gz( 56 | const vision_msgs::msg::Detection3D & ros_msg, 57 | gz::msgs::AnnotatedOriented3DBox & gz_msg); 58 | 59 | template<> 60 | void 61 | convert_gz_to_ros( 62 | const gz::msgs::AnnotatedOriented3DBox & gz_msg, 63 | vision_msgs::msg::Detection3D & ros_msg); 64 | 65 | template<> 66 | void 67 | convert_ros_to_gz( 68 | const vision_msgs::msg::Detection3DArray & ros_msg, 69 | gz::msgs::AnnotatedOriented3DBox_V & gz_msg); 70 | 71 | template<> 72 | void 73 | convert_gz_to_ros( 74 | const gz::msgs::AnnotatedOriented3DBox_V & gz_msg, 75 | vision_msgs::msg::Detection3DArray & ros_msg); 76 | } // namespace ros_gz_bridge 77 | 78 | #endif // ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_ 79 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/convert_decl.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 ROS_GZ_BRIDGE__CONVERT_DECL_HPP_ 16 | #define ROS_GZ_BRIDGE__CONVERT_DECL_HPP_ 17 | 18 | namespace ros_gz_bridge 19 | { 20 | 21 | template 22 | void 23 | convert_ros_to_gz( 24 | const ROS_T & ros_msg, 25 | GZ_T & gz_msg); 26 | 27 | template 28 | void 29 | convert_gz_to_ros( 30 | const GZ_T & gz_msg, 31 | ROS_T & ros_msg); 32 | 33 | } // namespace ros_gz_bridge 34 | 35 | #endif // ROS_GZ_BRIDGE__CONVERT_DECL_HPP_ 36 | -------------------------------------------------------------------------------- /ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp: -------------------------------------------------------------------------------- 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 | #ifndef ROS_GZ_BRIDGE__ROS_GZ_BRIDGE_HPP_ 16 | #define ROS_GZ_BRIDGE__ROS_GZ_BRIDGE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include "ros_gz_bridge/bridge_config.hpp" 26 | 27 | namespace ros_gz_bridge 28 | { 29 | /// Forward declarations 30 | class BridgeHandle; 31 | 32 | /// \brief Component container for the ROS-GZ Bridge 33 | class RosGzBridge : public rclcpp::Node 34 | { 35 | public: 36 | /// \brief Constructor 37 | /// \param[in] options options control creation of the ROS 2 node 38 | explicit RosGzBridge(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 39 | 40 | /// \brief Add a new ROS-GZ bridge to the node 41 | /// \param[in] config Parameters to control creation of a new bridge 42 | void add_bridge(const BridgeConfig & config); 43 | 44 | /// \brief Create a new ROS-GZ bridge for a service 45 | /// \param[in] ros_type_name Name of the ROS service (eg ros_bz_interfaces/srv/ControlWorld) 46 | /// \param[in] gz_req_type_name Gazebo service request type 47 | /// \param[in] gz_req_type_name Gazebo service response type 48 | /// \param[in] service_name Address of the service to be bridged 49 | void add_service_bridge( 50 | const std::string & ros_type_name, 51 | const std::string & gz_req_type_name, 52 | const std::string & gz_rep_type_name, 53 | const std::string & service_name); 54 | 55 | protected: 56 | /// \brief Periodic callback to check connectivity and liveliness 57 | void spin(); 58 | 59 | protected: 60 | /// \brief Pointer to Gazebo node used to create publishers/subscribers 61 | std::shared_ptr gz_node_; 62 | 63 | /// \brief List of bridge handles 64 | std::vector> handles_; 65 | 66 | /// \brief List of bridged ROS services 67 | std::vector services_; 68 | 69 | /// \brief Timer to control periodic callback 70 | rclcpp::TimerBase::SharedPtr heartbeat_timer_; 71 | }; 72 | } // namespace ros_gz_bridge 73 | 74 | #endif // ROS_GZ_BRIDGE__ROS_GZ_BRIDGE_HPP_ 75 | -------------------------------------------------------------------------------- /ros_gz_bridge/launch/clock_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 35 | 36 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /ros_gz_bridge/launch/ros_gz_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /ros_gz_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros_gz_bridge 5 | 3.0.1 6 | Bridge communication between ROS and Gazebo Transport 7 | Aditya Pande 8 | Alejandro Hernandez 9 | 10 | Apache 2.0 11 | 12 | Shivesh Khaitan 13 | Louise Poubel 14 | Carlos Agüero 15 | 16 | ament_cmake 17 | ament_cmake_python 18 | pkg-config 19 | rosidl_pycommon 20 | 21 | actuator_msgs 22 | geometry_msgs 23 | gps_msgs 24 | launch 25 | launch_ros 26 | nav_msgs 27 | rclcpp 28 | rclcpp_components 29 | ros_gz_interfaces 30 | rosgraph_msgs 31 | sensor_msgs 32 | std_msgs 33 | tf2_msgs 34 | trajectory_msgs 35 | yaml_cpp_vendor 36 | vision_msgs 37 | 38 | gz_msgs_vendor 39 | gz_transport_vendor 40 | 41 | ament_cmake_gtest 42 | ament_lint_auto 43 | ament_lint_common 44 | launch_testing_ament_cmake 45 | launch_ros 46 | launch_testing 47 | 48 | 49 | ament_cmake 50 | 51 | 52 | -------------------------------------------------------------------------------- /ros_gz_bridge/resource/get_factory.cpp.em: -------------------------------------------------------------------------------- 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 | // generated from ros_gz_bridge/resource/get_factory.cpp.em 16 | 17 | #include 18 | #include 19 | 20 | #include "get_factory.hpp" 21 | 22 | @[for ros2_package_name in sorted(ros2_package_names)]@ 23 | #include "factories/@(ros2_package_name).hpp" 24 | @[end for]@ 25 | 26 | #include "service_factories/ros_gz_interfaces.hpp" 27 | 28 | namespace ros_gz_bridge 29 | { 30 | 31 | std::shared_ptr 32 | get_factory_impl( 33 | const std::string & ros_type_name, 34 | const std::string & gz_type_name) 35 | { 36 | std::shared_ptr impl = nullptr; 37 | @[for ros2_package_name in sorted(ros2_package_names)]@ 38 | impl = get_factory__@(ros2_package_name)(ros_type_name, gz_type_name); 39 | if (impl) {return impl;} 40 | @[end for] 41 | return nullptr; 42 | } 43 | 44 | std::shared_ptr 45 | get_factory( 46 | const std::string & ros_type_name, 47 | const std::string & gz_type_name) 48 | { 49 | std::shared_ptr factory; 50 | factory = get_factory_impl(ros_type_name, gz_type_name); 51 | if (factory) { 52 | return factory; 53 | } 54 | 55 | throw std::runtime_error("No template specialization for the pair"); 56 | } 57 | 58 | std::shared_ptr 59 | get_service_factory( 60 | const std::string & ros_type_name, 61 | const std::string & gz_req_type_name, 62 | const std::string & gz_rep_type_name) 63 | { 64 | std::shared_ptr impl; 65 | 66 | impl = get_service_factory__ros_gz_interfaces( 67 | ros_type_name, gz_req_type_name, gz_rep_type_name); 68 | if (impl) {return impl;} 69 | 70 | std::ostringstream oss{"No template specialization for the specified service type {"}; 71 | oss << ros_type_name << "}, gz request type {" << gz_req_type_name 72 | << "}, gz request type {" << gz_req_type_name << "}, gz reply type name {" 73 | << gz_rep_type_name << "}"; 74 | throw std::runtime_error(oss.str()); 75 | } 76 | 77 | } // namespace ros_gz_bridge 78 | -------------------------------------------------------------------------------- /ros_gz_bridge/resource/get_mappings.cpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros_gz_bridge/resource/get_mappings.cpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Methods for determining mappings between 6 | @# Gazebo and ROS interfaces 7 | @# 8 | @# EmPy template for generating get_mappings.cpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - mappings (list of ros_gz_bridge.Mapping) 15 | @# Mapping between messages as well as their fields 16 | @############################################### 17 | @ 18 | #include 19 | #include 20 | 21 | #include "get_mappings.hpp" 22 | 23 | namespace ros_gz_bridge 24 | { 25 | 26 | bool 27 | get_gz_to_ros_mapping(const std::string & gz_type_name, std::string & ros_type_name) 28 | { 29 | @[if not mappings]@ 30 | (void)gz_type_name; 31 | (void)ros_type_name; 32 | @[end if]@ 33 | 34 | @[for m in mappings]@ 35 | if (gz_type_name == "@(m.gz_string())" || gz_type_name == "@(m.ign_string())") 36 | { 37 | ros_type_name = "@(m.ros2_string())"; 38 | return true; 39 | } 40 | @[end for]@ 41 | 42 | return false; 43 | } 44 | 45 | bool 46 | get_ros_to_gz_mapping(const std::string & ros_type_name, std::string & gz_type_name) 47 | { 48 | @[if not mappings]@ 49 | (void)gz_type_name; 50 | (void)ros_type_name; 51 | @[end if]@ 52 | 53 | @[for m in mappings]@ 54 | if (ros_type_name == "@(m.ros2_string())") 55 | { 56 | gz_type_name = "@(m.gz_string())"; 57 | return true; 58 | } 59 | @[end for]@ 60 | 61 | return false; 62 | } 63 | 64 | std::multimap 65 | get_all_message_mappings_ros_to_gz() 66 | { 67 | static std::multimap mappings = { 68 | @[for m in mappings]@ 69 | { 70 | "@(m.ros2_string())", // ROS 2 71 | "@(m.gz_string())", // Gazebo 72 | }, 73 | { 74 | "@(m.ros2_string())", // ROS 2 75 | "@(m.ign_string())", // Gazebo 76 | }, 77 | @[end for]@ 78 | }; 79 | return mappings; 80 | } 81 | 82 | } // namespace ros_gz_bridge 83 | -------------------------------------------------------------------------------- /ros_gz_bridge/resource/pkg_factories.cpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros_gz_bridge/resource/pkg_factories.cpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Factory template specializations based on 6 | @# message types of a single ROS 2 package 7 | @# 8 | @# EmPy template for generating factories/.cpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - ros2_package_name (str) 15 | @# The ROS 2 package name of this file 16 | @############################################### 17 | @ 18 | 19 | #include "factories/@(ros2_package_name).hpp" 20 | 21 | #include 22 | #include 23 | 24 | #include "factory.hpp" 25 | #include "ros_gz_bridge/convert/@(ros2_package_name).hpp" 26 | 27 | namespace ros_gz_bridge 28 | { 29 | 30 | std::shared_ptr 31 | get_factory__@(ros2_package_name)( 32 | const std::string & ros_type_name, 33 | const std::string & gz_type_name) 34 | { 35 | @[for m in mappings]@ 36 | if ((ros_type_name == "@(m.ros2_string())" || ros_type_name.empty()) && 37 | (gz_type_name == "@(m.gz_string())" || gz_type_name == "@(m.ign_string())")) 38 | { 39 | return std::make_shared< 40 | Factory< 41 | @(m.ros2_type()), 42 | @(m.gz_type()) 43 | > 44 | >("@(m.ros2_string())", "@(m.gz_string())"); 45 | } 46 | @[end for]@ 47 | return nullptr; 48 | } 49 | 50 | @[for m in mappings]@ 51 | template<> 52 | void 53 | Factory< 54 | @(m.ros2_type()), 55 | @(m.gz_type()) 56 | >::convert_ros_to_gz( 57 | const @(m.ros2_type()) & ros_msg, 58 | @(m.gz_type()) & gz_msg) 59 | { 60 | ros_gz_bridge::convert_ros_to_gz(ros_msg, gz_msg); 61 | } 62 | 63 | template<> 64 | void 65 | Factory< 66 | @(m.ros2_type()), 67 | @(m.gz_type()) 68 | >::convert_gz_to_ros( 69 | const @(m.gz_type()) & gz_msg, 70 | @(m.ros2_type()) & ros_msg) 71 | { 72 | ros_gz_bridge::convert_gz_to_ros(gz_msg, ros_msg); 73 | } 74 | @[end for]@ 75 | 76 | } // namespace ros_gz_bridge 77 | -------------------------------------------------------------------------------- /ros_gz_bridge/resource/pkg_factories.hpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros_gz_bridge/resource/pkg_factories.hpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Factory template specializations based on 6 | @# message types of a single ROS 2 package 7 | @# 8 | @# EmPy template for generating factories/.hpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - ros2_package_name (str) 15 | @# The ROS 2 package name of this file 16 | @############################################### 17 | @ 18 | 19 | #ifndef FACTORIES_@(ros2_package_name.upper()) 20 | #define FACTORIES_@(ros2_package_name.upper()) 21 | 22 | #include 23 | #include 24 | 25 | #include "factory_interface.hpp" 26 | 27 | namespace ros_gz_bridge 28 | { 29 | 30 | std::shared_ptr 31 | get_factory__@(ros2_package_name)( 32 | const std::string & ros_type_name, 33 | const std::string & gz_type_name); 34 | 35 | } // namespace ros_gz_bridge 36 | 37 | #endif // FACTORIES_@(ros2_package_name.upper()) 38 | -------------------------------------------------------------------------------- /ros_gz_bridge/resource/ros_gz_bridge: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_bridge/resource/ros_gz_bridge -------------------------------------------------------------------------------- /ros_gz_bridge/ros_gz_bridge/actions/__init__.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 | """Actions module.""" 16 | 17 | from .ros_gz_bridge import RosGzBridge 18 | 19 | 20 | __all__ = [ 21 | 'RosGzBridge', 22 | ] 23 | -------------------------------------------------------------------------------- /ros_gz_bridge/setup.cfg: -------------------------------------------------------------------------------- 1 | [options.entry_points] 2 | launch.frontend.launch_extension = 3 | ros_gz_bridge = ros_gz_bridge 4 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/bridge_handle.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 "bridge_handle.hpp" 16 | 17 | #include 18 | #include 19 | 20 | #include "get_factory.hpp" 21 | 22 | namespace ros_gz_bridge 23 | { 24 | BridgeHandle::BridgeHandle( 25 | rclcpp::Node::SharedPtr ros_node, 26 | std::shared_ptr gz_node, 27 | const BridgeConfig & config) 28 | : ros_node_(ros_node), 29 | gz_node_(gz_node), 30 | config_(config), 31 | factory_(get_factory(config.ros_type_name, config.gz_type_name)) 32 | { 33 | ros_node_->get_parameter("override_timestamps_with_wall_time", 34 | override_timestamps_with_wall_time_); 35 | } 36 | 37 | BridgeHandle::~BridgeHandle() = default; 38 | 39 | bool BridgeHandle::IsLazy() const 40 | { 41 | return config_.is_lazy; 42 | } 43 | 44 | void BridgeHandle::Start() 45 | { 46 | if (!this->HasPublisher()) { 47 | this->StartPublisher(); 48 | } 49 | 50 | if (!this->IsLazy() && !this->HasSubscriber()) { 51 | this->StartSubscriber(); 52 | } 53 | } 54 | 55 | void BridgeHandle::Spin() 56 | { 57 | if (!this->IsLazy()) { 58 | return; 59 | } 60 | 61 | if (this->HasSubscriber() && this->NumSubscriptions() == 0) { 62 | RCLCPP_DEBUG( 63 | this->ros_node_->get_logger(), 64 | "Bridge [%s] - No subscriptions found, stopping bridge", 65 | config_.ros_topic_name.c_str()); 66 | this->StopSubscriber(); 67 | } else if (!this->HasSubscriber() && this->NumSubscriptions() > 0) { 68 | RCLCPP_DEBUG( 69 | this->ros_node_->get_logger(), 70 | "Bridge [%s] - Subscriptions found, starting bridge", 71 | config_.ros_topic_name.c_str()); 72 | this->StartSubscriber(); 73 | } 74 | } 75 | } // namespace ros_gz_bridge 76 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/bridge_handle_gz_to_ros.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 "bridge_handle_gz_to_ros.hpp" 18 | 19 | namespace ros_gz_bridge 20 | { 21 | 22 | BridgeHandleGzToRos::~BridgeHandleGzToRos() = default; 23 | 24 | size_t BridgeHandleGzToRos::NumSubscriptions() const 25 | { 26 | // Return number of ROS subscriptions 27 | size_t valid_subscriptions = 0; 28 | 29 | if (this->ros_publisher_ != nullptr) { 30 | // Use info_by_topic rather than get_subscription_count 31 | // to filter out potential bidirectional bridge 32 | auto topic_info = this->ros_node_->get_subscriptions_info_by_topic( 33 | this->config_.ros_topic_name); 34 | 35 | for (auto & topic : topic_info) { 36 | if (topic.node_name() == this->ros_node_->get_name()) { 37 | continue; 38 | } 39 | valid_subscriptions++; 40 | } 41 | } 42 | 43 | return valid_subscriptions; 44 | } 45 | 46 | bool BridgeHandleGzToRos::HasPublisher() const 47 | { 48 | return this->ros_publisher_ != nullptr; 49 | } 50 | 51 | void BridgeHandleGzToRos::StartPublisher() 52 | { 53 | // Start ROS publisher 54 | this->ros_publisher_ = this->factory_->create_ros_publisher( 55 | this->ros_node_, 56 | this->config_.ros_topic_name, 57 | this->config_.publisher_queue_size); 58 | } 59 | 60 | bool BridgeHandleGzToRos::HasSubscriber() const 61 | { 62 | // Return Gazebo subscriber status 63 | return this->gz_subscriber_ != nullptr; 64 | } 65 | 66 | void BridgeHandleGzToRos::StartSubscriber() 67 | { 68 | // Start Gazebo subscriber 69 | this->factory_->create_gz_subscriber( 70 | this->gz_node_, 71 | this->config_.gz_topic_name, 72 | this->config_.subscriber_queue_size, 73 | this->ros_publisher_, 74 | override_timestamps_with_wall_time_); 75 | 76 | this->gz_subscriber_ = this->gz_node_; 77 | } 78 | 79 | void BridgeHandleGzToRos::StopSubscriber() 80 | { 81 | // Stop Gazebo subscriber 82 | if (!this->gz_subscriber_) { 83 | return; 84 | } 85 | 86 | this->gz_subscriber_->Unsubscribe(this->config_.gz_topic_name); 87 | this->gz_subscriber_.reset(); 88 | } 89 | 90 | } // namespace ros_gz_bridge 91 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/bridge_handle_gz_to_ros.hpp: -------------------------------------------------------------------------------- 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 | #ifndef BRIDGE_HANDLE_GZ_TO_ROS_HPP_ 16 | #define BRIDGE_HANDLE_GZ_TO_ROS_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include "bridge_handle.hpp" 24 | 25 | namespace ros_gz_bridge 26 | { 27 | 28 | /// \brief Create a Gazebo to ROS Bridge 29 | /// 30 | /// Bridge is from Gazebo Subscription (input) to ROS Publisher (output) 31 | class BridgeHandleGzToRos : public BridgeHandle 32 | { 33 | public: 34 | /// \brief Constructor 35 | using BridgeHandle::BridgeHandle; 36 | 37 | /// \brief Destructor 38 | ~BridgeHandleGzToRos() override; 39 | 40 | protected: 41 | /// \brief Documentation inherited 42 | size_t NumSubscriptions() const override; 43 | 44 | /// \brief Documentation inherited 45 | bool HasPublisher() const override; 46 | 47 | /// \brief Documentation inherited 48 | void StartPublisher() override; 49 | 50 | /// \brief Documentation inherited 51 | bool HasSubscriber() const override; 52 | 53 | /// \brief Documentation inherited 54 | void StartSubscriber() override; 55 | 56 | /// \brief Documentation inherited 57 | void StopSubscriber() override; 58 | 59 | protected: 60 | /// \brief Gazebo subscriber, populated when subscriber active 61 | std::shared_ptr gz_subscriber_ = {nullptr}; 62 | 63 | /// \brief ROS publisher, populated when publisher active 64 | rclcpp::PublisherBase::SharedPtr ros_publisher_ = {nullptr}; 65 | }; 66 | 67 | } // namespace ros_gz_bridge 68 | 69 | #endif // BRIDGE_HANDLE_GZ_TO_ROS_HPP_ 70 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/bridge_handle_ros_to_gz.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 "bridge_handle_ros_to_gz.hpp" 16 | 17 | #include 18 | 19 | namespace ros_gz_bridge 20 | { 21 | 22 | BridgeHandleRosToGz::~BridgeHandleRosToGz() = default; 23 | 24 | size_t BridgeHandleRosToGz::NumSubscriptions() const 25 | { 26 | // Return number of gazebo subscriptions 27 | // Gazebo publishers can only detect presence of connections, and not 28 | // get the actual count. 29 | // Gazebo transport also cannot differentiate connections locally and 30 | // remotely, so a bidirectional bridge will always subscribe. 31 | // \TODO(mjcarroll) Add transport APIs for correctly querying number 32 | // of remote subscriptions 33 | if (this->gz_publisher_.Valid() && this->gz_publisher_.HasConnections()) { 34 | return 1; 35 | } 36 | return 0; 37 | } 38 | 39 | bool BridgeHandleRosToGz::HasPublisher() const 40 | { 41 | // Return Gazebo publisher status 42 | return this->gz_publisher_.Valid(); 43 | } 44 | 45 | void BridgeHandleRosToGz::StartPublisher() 46 | { 47 | // Start Gazebo publisher 48 | this->gz_publisher_ = this->factory_->create_gz_publisher( 49 | this->gz_node_, 50 | this->config_.gz_topic_name, 51 | this->config_.publisher_queue_size); 52 | } 53 | 54 | bool BridgeHandleRosToGz::HasSubscriber() const 55 | { 56 | // Return ROS subscriber status 57 | return this->ros_subscriber_ != nullptr; 58 | } 59 | 60 | void BridgeHandleRosToGz::StartSubscriber() 61 | { 62 | // Start ROS subscriber 63 | this->ros_subscriber_ = this->factory_->create_ros_subscriber( 64 | this->ros_node_, 65 | this->config_.ros_topic_name, 66 | this->config_.subscriber_queue_size, 67 | this->gz_publisher_); 68 | } 69 | 70 | void BridgeHandleRosToGz::StopSubscriber() 71 | { 72 | // Stop ROS subscriber 73 | this->ros_subscriber_.reset(); 74 | } 75 | 76 | } // namespace ros_gz_bridge 77 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/bridge_handle_ros_to_gz.hpp: -------------------------------------------------------------------------------- 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 | #ifndef BRIDGE_HANDLE_ROS_TO_GZ_HPP_ 16 | #define BRIDGE_HANDLE_ROS_TO_GZ_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "bridge_handle.hpp" 22 | 23 | namespace ros_gz_bridge 24 | { 25 | 26 | /// \brief Create a bridge ROS to GZ Bridge 27 | /// 28 | /// Bridge is from ROS Subscription (input) to GZ Publisher (output) 29 | class BridgeHandleRosToGz : public BridgeHandle 30 | { 31 | public: 32 | /// \brief Constructor 33 | using BridgeHandle::BridgeHandle; 34 | 35 | /// \brief Destructor 36 | ~BridgeHandleRosToGz() override; 37 | 38 | protected: 39 | /// \brief Documentation inherited 40 | size_t NumSubscriptions() const override; 41 | 42 | /// \brief Documentation inherited 43 | bool HasPublisher() const override; 44 | 45 | /// \brief Documentation inherited 46 | void StartPublisher() override; 47 | 48 | /// \brief Documentation inherited 49 | bool HasSubscriber() const override; 50 | 51 | /// \brief Documentation inherited 52 | void StartSubscriber() override; 53 | 54 | /// \brief Documentation inherited 55 | void StopSubscriber() override; 56 | 57 | protected: 58 | /// \brief ROS subscriber, populated when subscription active 59 | rclcpp::SubscriptionBase::SharedPtr ros_subscriber_ = {nullptr}; 60 | 61 | /// \brief Gazebo publisher, populated when publisher active 62 | gz::transport::Node::Publisher gz_publisher_; 63 | }; 64 | 65 | } // namespace ros_gz_bridge 66 | 67 | #endif // BRIDGE_HANDLE_ROS_TO_GZ_HPP_ 68 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/actuator_msgs.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Rudis Laboratories LLC 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 "convert/utils.hpp" 16 | #include "ros_gz_bridge/convert/actuator_msgs.hpp" 17 | 18 | namespace ros_gz_bridge 19 | { 20 | template<> 21 | void 22 | convert_ros_to_gz( 23 | const actuator_msgs::msg::Actuators & ros_msg, 24 | gz::msgs::Actuators & gz_msg) 25 | { 26 | convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); 27 | 28 | for (auto i = 0u; i < ros_msg.position.size(); ++i) { 29 | gz_msg.add_position(ros_msg.position[i]); 30 | } 31 | 32 | for (auto i = 0u; i < ros_msg.velocity.size(); ++i) { 33 | gz_msg.add_velocity(ros_msg.velocity[i]); 34 | } 35 | for (auto i = 0u; i < ros_msg.normalized.size(); ++i) { 36 | gz_msg.add_normalized(ros_msg.normalized[i]); 37 | } 38 | } 39 | 40 | template<> 41 | void 42 | convert_gz_to_ros( 43 | const gz::msgs::Actuators & gz_msg, 44 | actuator_msgs::msg::Actuators & ros_msg) 45 | { 46 | convert_gz_to_ros(gz_msg.header(), ros_msg.header); 47 | 48 | for (auto i = 0; i < gz_msg.position_size(); ++i) { 49 | ros_msg.position.push_back(gz_msg.position(i)); 50 | } 51 | 52 | for (auto i = 0; i < gz_msg.velocity_size(); ++i) { 53 | ros_msg.velocity.push_back(gz_msg.velocity(i)); 54 | } 55 | 56 | for (auto i = 0; i < gz_msg.normalized_size(); ++i) { 57 | ros_msg.normalized.push_back(gz_msg.normalized(i)); 58 | } 59 | } 60 | 61 | } // namespace ros_gz_bridge 62 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/builtin_interfaces.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 "ros_gz_bridge/convert/builtin_interfaces.hpp" 16 | 17 | namespace ros_gz_bridge 18 | { 19 | template<> 20 | void 21 | convert_ros_to_gz( 22 | const builtin_interfaces::msg::Time & ros_msg, 23 | gz::msgs::Time & gz_msg) 24 | { 25 | gz_msg.set_sec(ros_msg.sec); 26 | gz_msg.set_nsec(ros_msg.nanosec); 27 | } 28 | 29 | template<> 30 | void 31 | convert_gz_to_ros( 32 | const gz::msgs::Time & gz_msg, 33 | builtin_interfaces::msg::Time & ros_msg) 34 | { 35 | ros_msg.sec = gz_msg.sec(); 36 | ros_msg.nanosec = gz_msg.nsec(); 37 | } 38 | } // namespace ros_gz_bridge 39 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/gps_msgs.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 "convert/utils.hpp" 18 | #include "ros_gz_bridge/convert/gps_msgs.hpp" 19 | 20 | namespace ros_gz_bridge 21 | { 22 | 23 | template<> 24 | void 25 | convert_ros_to_gz( 26 | const gps_msgs::msg::GPSFix & ros_msg, 27 | gz::msgs::NavSat & gz_msg) 28 | { 29 | convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); 30 | gz_msg.set_latitude_deg(ros_msg.latitude); 31 | gz_msg.set_longitude_deg(ros_msg.longitude); 32 | gz_msg.set_altitude(ros_msg.altitude); 33 | gz_msg.set_frame_id(ros_msg.header.frame_id); 34 | 35 | gz_msg.set_velocity_east(cos(ros_msg.track) * ros_msg.speed); 36 | gz_msg.set_velocity_north(sin(ros_msg.track) * ros_msg.speed); 37 | gz_msg.set_velocity_up(ros_msg.climb); 38 | } 39 | 40 | template<> 41 | void 42 | convert_gz_to_ros( 43 | const gz::msgs::NavSat & gz_msg, 44 | gps_msgs::msg::GPSFix & ros_msg) 45 | { 46 | convert_gz_to_ros(gz_msg.header(), ros_msg.header); 47 | ros_msg.header.frame_id = frame_id_gz_to_ros(gz_msg.frame_id()); 48 | ros_msg.latitude = gz_msg.latitude_deg(); 49 | ros_msg.longitude = gz_msg.longitude_deg(); 50 | ros_msg.altitude = gz_msg.altitude(); 51 | 52 | ros_msg.speed = sqrt( 53 | gz_msg.velocity_east() * gz_msg.velocity_east() + 54 | gz_msg.velocity_north() * gz_msg.velocity_north()); 55 | ros_msg.track = atan2(gz_msg.velocity_north(), gz_msg.velocity_east()); 56 | ros_msg.climb = gz_msg.velocity_up(); 57 | 58 | // position_covariance is not supported in gz::msgs::NavSat. 59 | ros_msg.position_covariance_type = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN; 60 | ros_msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; 61 | } 62 | 63 | } // namespace ros_gz_bridge 64 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/rosgraph_msgs.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 "convert/utils.hpp" 18 | #include "ros_gz_bridge/convert/rosgraph_msgs.hpp" 19 | 20 | namespace ros_gz_bridge 21 | { 22 | 23 | template<> 24 | void 25 | convert_gz_to_ros( 26 | const gz::msgs::Clock & gz_msg, 27 | rosgraph_msgs::msg::Clock & ros_msg) 28 | { 29 | ros_msg.clock = rclcpp::Time(gz_msg.sim().sec(), gz_msg.sim().nsec()); 30 | } 31 | 32 | template<> 33 | void 34 | convert_ros_to_gz( 35 | const rosgraph_msgs::msg::Clock & ros_msg, 36 | gz::msgs::Clock & gz_msg) 37 | { 38 | gz_msg.mutable_sim()->set_sec(ros_msg.clock.sec); 39 | gz_msg.mutable_sim()->set_nsec(ros_msg.clock.nanosec); 40 | } 41 | 42 | } // namespace ros_gz_bridge 43 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/tf2_msgs.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 "convert/utils.hpp" 16 | #include "ros_gz_bridge/convert/tf2_msgs.hpp" 17 | 18 | namespace ros_gz_bridge 19 | { 20 | 21 | template<> 22 | void 23 | convert_ros_to_gz( 24 | const tf2_msgs::msg::TFMessage & ros_msg, 25 | gz::msgs::Pose_V & gz_msg) 26 | { 27 | gz_msg.clear_pose(); 28 | for (auto const & t : ros_msg.transforms) { 29 | auto p = gz_msg.add_pose(); 30 | convert_ros_to_gz(t, *p); 31 | } 32 | 33 | if (!ros_msg.transforms.empty()) { 34 | convert_ros_to_gz( 35 | ros_msg.transforms[0].header, 36 | (*gz_msg.mutable_header())); 37 | } 38 | } 39 | 40 | template<> 41 | void 42 | convert_gz_to_ros( 43 | const gz::msgs::Pose_V & gz_msg, 44 | tf2_msgs::msg::TFMessage & ros_msg) 45 | { 46 | ros_msg.transforms.clear(); 47 | for (auto const & p : gz_msg.pose()) { 48 | geometry_msgs::msg::TransformStamped tf; 49 | convert_gz_to_ros(p, tf); 50 | ros_msg.transforms.push_back(tf); 51 | } 52 | } 53 | 54 | } // namespace ros_gz_bridge 55 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/utils.cpp: -------------------------------------------------------------------------------- 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 | #include "convert/utils.hpp" 16 | 17 | #include 18 | 19 | namespace ros_gz_bridge 20 | { 21 | 22 | // This can be used to replace `::` with `/` to make frame_id compatible with TF 23 | std::string replace_delimiter( 24 | const std::string & input, 25 | const std::string & old_delim, 26 | const std::string new_delim) 27 | { 28 | std::string output; 29 | output.reserve(input.size()); 30 | 31 | std::size_t last_pos = 0; 32 | 33 | while (last_pos < input.size()) { 34 | std::size_t pos = input.find(old_delim, last_pos); 35 | output += input.substr(last_pos, pos - last_pos); 36 | if (pos != std::string::npos) { 37 | output += new_delim; 38 | pos += old_delim.size(); 39 | } 40 | last_pos = pos; 41 | } 42 | 43 | return output; 44 | } 45 | 46 | 47 | std::string frame_id_gz_to_ros(const std::string & frame_id) 48 | { 49 | return replace_delimiter(frame_id, "::", "/"); 50 | } 51 | 52 | } // namespace ros_gz_bridge 53 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/convert/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 CONVERT__UTILS_HPP_ 16 | #define CONVERT__UTILS_HPP_ 17 | 18 | #include 19 | 20 | namespace ros_gz_bridge 21 | { 22 | 23 | // This can be used to replace `::` with `/` to make frame_id compatible with TF 24 | std::string replace_delimiter( 25 | const std::string & input, 26 | const std::string & old_delim, 27 | const std::string new_delim); 28 | 29 | std::string frame_id_gz_to_ros(const std::string & frame_id); 30 | 31 | } // namespace ros_gz_bridge 32 | 33 | #endif // CONVERT__UTILS_HPP_ 34 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/factory_interface.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 "factory_interface.hpp" 16 | 17 | namespace ros_gz_bridge 18 | { 19 | 20 | FactoryInterface::~FactoryInterface() 21 | { 22 | } 23 | 24 | } // namespace ros_gz_bridge 25 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/factory_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 FACTORY_INTERFACE_HPP_ 16 | #define FACTORY_INTERFACE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | // include Gazebo Transport 22 | #include 23 | 24 | // include ROS 2 25 | #include 26 | 27 | namespace ros_gz_bridge 28 | { 29 | 30 | class FactoryInterface 31 | { 32 | public: 33 | virtual ~FactoryInterface() = 0; 34 | 35 | virtual 36 | rclcpp::PublisherBase::SharedPtr 37 | create_ros_publisher( 38 | rclcpp::Node::SharedPtr ros_node, 39 | const std::string & topic_name, 40 | size_t queue_size) = 0; 41 | 42 | virtual 43 | gz::transport::Node::Publisher 44 | create_gz_publisher( 45 | std::shared_ptr gz_node, 46 | const std::string & topic_name, 47 | size_t queue_size) = 0; 48 | 49 | virtual 50 | rclcpp::SubscriptionBase::SharedPtr 51 | create_ros_subscriber( 52 | rclcpp::Node::SharedPtr ros_node, 53 | const std::string & topic_name, 54 | size_t queue_size, 55 | gz::transport::Node::Publisher & gz_pub) = 0; 56 | 57 | virtual 58 | void 59 | create_gz_subscriber( 60 | std::shared_ptr node, 61 | const std::string & topic_name, 62 | size_t queue_size, 63 | rclcpp::PublisherBase::SharedPtr ros_pub, 64 | bool override_timestamps_with_wall_time) = 0; 65 | }; 66 | 67 | } // namespace ros_gz_bridge 68 | 69 | #endif // FACTORY_INTERFACE_HPP_ 70 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/get_factory.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 GET_FACTORY_HPP_ 16 | #define GET_FACTORY_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "factory_interface.hpp" 22 | #include "service_factory_interface.hpp" 23 | 24 | namespace ros_gz_bridge 25 | { 26 | 27 | std::shared_ptr 28 | get_factory( 29 | const std::string & ros_type_name, 30 | const std::string & gz_type_name); 31 | 32 | std::shared_ptr 33 | get_service_factory( 34 | const std::string & ros_type_name, 35 | const std::string & gz_req_type_name, 36 | const std::string & gz_rep_type_name); 37 | 38 | } // namespace ros_gz_bridge 39 | 40 | #endif // GET_FACTORY_HPP_ 41 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/get_mappings.hpp: -------------------------------------------------------------------------------- 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 | #ifndef GET_MAPPINGS_HPP_ 16 | #define GET_MAPPINGS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace ros_gz_bridge 22 | { 23 | 24 | bool 25 | get_gz_to_ros_mapping(const std::string & gz_type_name, std::string & ros_type_name); 26 | 27 | bool 28 | get_ros_to_gz_mapping(const std::string & ros_type_name, std::string & gz_type_name); 29 | 30 | std::multimap 31 | get_all_message_mappings_ros_to_gz(); 32 | 33 | } // namespace ros_gz_bridge 34 | 35 | #endif // GET_MAPPINGS_HPP_ 36 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/service_factories/ros_gz_interfaces.hpp: -------------------------------------------------------------------------------- 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 | #ifndef SERVICE_FACTORIES__ROS_GZ_INTERFACES_HPP_ 16 | #define SERVICE_FACTORIES__ROS_GZ_INTERFACES_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "service_factory_interface.hpp" 22 | 23 | namespace ros_gz_bridge 24 | { 25 | 26 | std::shared_ptr 27 | get_service_factory__ros_gz_interfaces( 28 | const std::string & ros_type_name, 29 | const std::string & gz_req_type_name, 30 | const std::string & gz_rep_type_name); 31 | 32 | } // namespace ros_gz_bridge 33 | 34 | #endif // SERVICE_FACTORIES__ROS_GZ_INTERFACES_HPP_ 35 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/service_factory_interface.cpp: -------------------------------------------------------------------------------- 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 | #include "service_factory_interface.hpp" 16 | 17 | namespace ros_gz_bridge 18 | { 19 | 20 | ServiceFactoryInterface::~ServiceFactoryInterface() 21 | { 22 | } 23 | 24 | } // namespace ros_gz_bridge 25 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/service_factory_interface.hpp: -------------------------------------------------------------------------------- 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 | #ifndef SERVICE_FACTORY_INTERFACE_HPP_ 16 | #define SERVICE_FACTORY_INTERFACE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace ros_gz_bridge 27 | { 28 | 29 | class ServiceFactoryInterface 30 | { 31 | public: 32 | virtual ~ServiceFactoryInterface() = 0; 33 | 34 | virtual 35 | rclcpp::ServiceBase::SharedPtr 36 | create_ros_service( 37 | rclcpp::Node::SharedPtr ros_node, 38 | std::shared_ptr gz_node, 39 | const std::string & service_name) = 0; 40 | }; 41 | 42 | } // namespace ros_gz_bridge 43 | 44 | #endif // SERVICE_FACTORY_INTERFACE_HPP_ 45 | -------------------------------------------------------------------------------- /ros_gz_bridge/src/static_bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 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 | #include 17 | 18 | #include 19 | #include 20 | 21 | using RosGzBridge = ros_gz_bridge::RosGzBridge; 22 | 23 | ////////////////////////////////////////////////// 24 | int main(int argc, char * argv[]) 25 | { 26 | // ROS node 27 | rclcpp::init(argc, argv); 28 | auto bridge_node = std::make_shared(rclcpp::NodeOptions()); 29 | 30 | // Set lazy subscriber on a global basis 31 | bool lazy_subscription = false; 32 | bridge_node->declare_parameter("lazy", false); 33 | bridge_node->get_parameter("lazy", lazy_subscription); 34 | 35 | // bridge one example topic 36 | ros_gz_bridge::BridgeConfig config; 37 | config.ros_topic_name = "chatter"; 38 | config.gz_topic_name = "chatter"; 39 | config.ros_type_name = "std_msgs/msg/String"; 40 | config.gz_type_name = "gz.msgs.StringMsg"; 41 | config.is_lazy = lazy_subscription; 42 | 43 | bridge_node->add_bridge(config); 44 | 45 | rclcpp::spin(bridge_node); 46 | 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/config/empty.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_bridge/test/config/empty.yaml -------------------------------------------------------------------------------- /ros_gz_bridge/test/config/full.yaml: -------------------------------------------------------------------------------- 1 | # Full set of configurations 2 | - ros_topic_name: "ros_chatter" 3 | gz_topic_name: "gz_chatter" 4 | ros_type_name: "std_msgs/msg/String" 5 | gz_type_name: "ignition.msgs.StringMsg" 6 | subscriber_queue: 5 7 | publisher_queue: 6 8 | lazy: true 9 | direction: ROS_TO_GZ 10 | 11 | - ros_topic_name: "ros_chatter" 12 | gz_topic_name: "gz_chatter" 13 | ros_type_name: "std_msgs/msg/String" 14 | gz_type_name: "ignition.msgs.StringMsg" 15 | subscriber_queue: 10 16 | publisher_queue: 20 17 | lazy: false 18 | direction: GZ_TO_ROS 19 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/config/invalid.yaml: -------------------------------------------------------------------------------- 1 | ros_topic_name: "ros_chatter" 2 | gz_topic_name: "gz_chatter" 3 | ros_type_name: "std_msgs/msg/String" 4 | gz_type_name: "ignition.msgs.StringMsg" 5 | subscriber_queue: 5 6 | publisher_queue: 6 7 | lazy: true 8 | direction: ROS_TO_GZ 9 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/config/minimum.yaml: -------------------------------------------------------------------------------- 1 | # Set just topic name, applies to both 2 | - topic_name: "chatter" 3 | ros_type_name: "std_msgs/msg/String" 4 | gz_type_name: "ignition.msgs.StringMsg" 5 | 6 | # Set just ROS topic name, applies to both 7 | - ros_topic_name: "chatter_ros" 8 | ros_type_name: "std_msgs/msg/String" 9 | gz_type_name: "ignition.msgs.StringMsg" 10 | 11 | # Set just GZ topic name, applies to both 12 | - gz_topic_name: "chatter_gz" 13 | ros_type_name: "std_msgs/msg/String" 14 | gz_type_name: "ignition.msgs.StringMsg" 15 | 16 | # Set each topic name explicitly 17 | - ros_topic_name: "chatter_both_ros" 18 | gz_topic_name: "chatter_both_gz" 19 | ros_type_name: "std_msgs/msg/String" 20 | gz_type_name: "ignition.msgs.StringMsg" 21 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/launch/test_gz_server.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 | import unittest 16 | 17 | from launch import LaunchDescription 18 | 19 | from launch_ros.actions import Node 20 | 21 | import launch_testing 22 | 23 | 24 | def generate_test_description(): 25 | 26 | server = Node( 27 | package='ros_gz_bridge', 28 | executable='test_gz_server', 29 | output='screen' 30 | ) 31 | process_under_test = Node( 32 | package='ros_gz_bridge', 33 | executable='test_ros_client', 34 | output='screen' 35 | ) 36 | 37 | # Bridge 38 | bridge = Node( 39 | package='ros_gz_bridge', 40 | executable='parameter_bridge', 41 | arguments=[ 42 | '/gz_ros/test/serviceclient/world_control@ros_gz_interfaces/srv/ControlWorld', 43 | ], 44 | output='screen' 45 | ) 46 | return LaunchDescription([ 47 | bridge, 48 | server, 49 | process_under_test, 50 | launch_testing.util.KeepAliveProc(), 51 | launch_testing.actions.ReadyToTest(), 52 | ]), locals() 53 | 54 | 55 | class ROSSubscriberTest(unittest.TestCase): 56 | 57 | def test_termination(self, process_under_test, proc_info): 58 | proc_info.assertWaitForShutdown(process=process_under_test, timeout=200) 59 | 60 | 61 | @launch_testing.post_shutdown_test() 62 | class ROSSubscriberTestAfterShutdown(unittest.TestCase): 63 | 64 | def test_exit_code(self, process_under_test, proc_info): 65 | launch_testing.asserts.assertExitCodes( 66 | proc_info, 67 | [launch_testing.asserts.EXIT_OK], 68 | process_under_test 69 | ) 70 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/launch/test_launch_action.launch: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 23 | 26 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/resource/gz_publisher.cpp.em: -------------------------------------------------------------------------------- 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 | // This file is generated from test/resource/gz_publisher.cpp.em 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "utils/test_utils.hpp" 28 | #include "utils/gz_test_msg.hpp" 29 | 30 | /// \brief Flag used to break the publisher loop and terminate the program. 31 | static std::atomic g_terminatePub(false); 32 | 33 | ////////////////////////////////////////////////// 34 | /// \brief Function callback executed when a SIGINT or SIGTERM signals are 35 | /// captured. This is used to break the infinite loop that publishes messages 36 | /// and exit the program smoothly. 37 | /// \param[in] _signal signal number (interrupt or terminate) 38 | void signal_handler(int _signal) 39 | { 40 | if (_signal == SIGINT || _signal == SIGTERM) { 41 | g_terminatePub = true; 42 | } 43 | } 44 | 45 | ////////////////////////////////////////////////// 46 | int main(int /*argc*/, char **/*argv*/) 47 | { 48 | // Install a signal handler for SIGINT and SIGTERM. 49 | std::signal(SIGINT, signal_handler); 50 | std::signal(SIGTERM, signal_handler); 51 | 52 | // Create a transport node and advertise a topic. 53 | gz::transport::Node node; 54 | 55 | @[for m in mappings]@ 56 | // @(m.gz_string()). 57 | auto @(m.unique())_pub = 58 | node.Advertise<@(m.gz_type())>("@(m.unique())"); 59 | @(m.gz_type()) @(m.unique())_msg; 60 | ros_gz_bridge::testing::createTestMsg(@(m.unique())_msg); 61 | 62 | @[end for]@ 63 | 64 | // Publish messages at 100 Hz. 65 | while (!g_terminatePub) { 66 | @[for m in mappings]@ 67 | @(m.unique())_pub.Publish(@(m.unique())_msg); 68 | @[end for]@ 69 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 70 | } 71 | 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/resource/gz_subscriber.cpp.em: -------------------------------------------------------------------------------- 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 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "utils/test_utils.hpp" 24 | #include "utils/gz_test_msg.hpp" 25 | 26 | ////////////////////////////////////////////////// 27 | /// \brief A class for testing Gazebo Transport topic subscription. 28 | template 29 | class MyTestClass 30 | { 31 | /// \brief Class constructor. 32 | /// \param[in] _topic Topic to subscribe. 33 | public: explicit MyTestClass(const std::string & _topic) 34 | { 35 | EXPECT_TRUE(this->node.Subscribe(_topic, &MyTestClass::Cb, this)); 36 | } 37 | 38 | /// \brief Member function called each time a topic update is received. 39 | /// \param[in] _msg Gazebo message to be validated 40 | public: void Cb(const GZ_T & _msg) 41 | { 42 | ros_gz_bridge::testing::compareTestMsg(std::make_shared(_msg)); 43 | this->callbackExecuted = true; 44 | } 45 | 46 | /// \brief Member variables that flag when the actions are executed. 47 | public: bool callbackExecuted = false; 48 | 49 | /// \brief Transport node; 50 | private: gz::transport::Node node; 51 | }; 52 | 53 | @[for m in mappings]@ 54 | ///////////////////////////////////////////////// 55 | TEST(GzSubscriberTest, @(m.unique())) 56 | { 57 | MyTestClass<@(m.gz_type())> client("@(m.unique())"); 58 | 59 | using namespace std::chrono_literals; 60 | ros_gz_bridge::testing::waitUntilBoolVar( 61 | client.callbackExecuted, 10ms, 200); 62 | 63 | EXPECT_TRUE(client.callbackExecuted); 64 | } 65 | 66 | @[end for]@ 67 | ///////////////////////////////////////////////// 68 | int main(int argc, char ** argv) 69 | { 70 | ::testing::InitGoogleTest(&argc, argv); 71 | return RUN_ALL_TESTS(); 72 | } 73 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/resource/ros_pkg_subscriber.cpp.em: -------------------------------------------------------------------------------- 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 | #include 17 | 18 | #include 19 | 20 | #include "ros_subscriber.hpp" 21 | 22 | using ros_subscriber::MyTestClass; 23 | 24 | @[for m in mappings]@ 25 | ///////////////////////////////////////////////// 26 | TEST(ROSSubscriberTest, @(m.unique())) 27 | { 28 | MyTestClass<@(m.ros2_type())> client("@(m.unique())"); 29 | 30 | using namespace std::chrono_literals; 31 | ros_gz_bridge::testing::waitUntilBoolVarAndSpin( 32 | ros_subscriber::TestNode(), client.callbackExecuted, 10ms, 200); 33 | 34 | EXPECT_TRUE(client.callbackExecuted); 35 | } 36 | 37 | @[end for]@ 38 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/resource/ros_publisher.cpp.em: -------------------------------------------------------------------------------- 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 | #include "utils/test_utils.hpp" 17 | #include "utils/ros_test_msg.hpp" 18 | 19 | ////////////////////////////////////////////////// 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = rclcpp::Node::make_shared("ros_string_publisher"); 24 | rclcpp::Rate loop_rate(100); 25 | 26 | @[for m in mappings]@ 27 | // @(m.ros2_string()). 28 | auto @(m.unique())_pub = 29 | node->create_publisher<@(m.ros2_type())>("@(m.unique())", 1); 30 | @(m.ros2_type()) @(m.unique())_msg; 31 | ros_gz_bridge::testing::createTestMsg(@(m.unique())_msg); 32 | 33 | @[end for]@ 34 | 35 | while (rclcpp::ok()) { 36 | // Publish all messages. 37 | @[for m in mappings]@ 38 | @(m.unique())_pub->publish(@(m.unique())_msg); 39 | @[end for]@ 40 | 41 | rclcpp::spin_some(node); 42 | loop_rate.sleep(); 43 | } 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/serverclient/gz_server.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 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include "utils/test_utils.hpp" 27 | #include "utils/gz_test_msg.hpp" 28 | 29 | /// \brief Flag used to break the waiting loop and terminate the program. 30 | static std::atomic_flag g_terminateSrv(false); 31 | 32 | ////////////////////////////////////////////////// 33 | /// \brief Function callback executed when a SIGINT or SIGTERM signals are 34 | /// captured. This is used to break the infinite loop that handles requests 35 | /// and exit the program smoothly. 36 | void signal_handler(int _signal) 37 | { 38 | if (_signal == SIGINT || _signal == SIGTERM) { 39 | g_terminateSrv.clear(); 40 | } 41 | } 42 | 43 | ////////////////////////////////////////////////// 44 | bool control_world( 45 | const gz::msgs::WorldControl &, 46 | gz::msgs::Boolean & _res) 47 | { 48 | _res.set_data(true); 49 | return true; 50 | } 51 | 52 | ////////////////////////////////////////////////// 53 | int main(int /*argc*/, char **/*argv*/) 54 | { 55 | g_terminateSrv.test_and_set(); 56 | // Install a signal handler for SIGINT and SIGTERM. 57 | std::signal(SIGINT, signal_handler); 58 | std::signal(SIGTERM, signal_handler); 59 | 60 | // Create a transport node and advertise a topic. 61 | gz::transport::Node node; 62 | 63 | // gz::msgs::WorldControl. 64 | node.Advertise( 65 | "/gz_ros/test/serviceclient/world_control", 66 | &control_world); 67 | 68 | // Requests are handled in another thread. 69 | // Wait until a signal is sent. 70 | while (g_terminateSrv.test_and_set()) { 71 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 72 | } 73 | 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/serverclient/ros_client.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 | // Unless required by applicable law or agreed to in writing, software 9 | // distributed under the License is distributed on an "AS IS" BASIS, 10 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | // See the License for the specific language governing permissions and 12 | // limitations under the License. 13 | 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | #include "ros_gz_interfaces/srv/control_world.hpp" 23 | 24 | using namespace std::chrono_literals; 25 | 26 | ///////////////////////////////////////////////// 27 | TEST(ROSClientTest, WorldControl) 28 | { 29 | rclcpp::init(0, NULL); 30 | auto node = std::make_shared("test_ros_client_to_gz_service"); 31 | auto client = node->create_client( 32 | "/gz_ros/test/serviceclient/world_control"); 33 | std::this_thread::sleep_for(1s); 34 | ASSERT_TRUE(client->wait_for_service(5s)); 35 | auto msg = std::make_shared(); 36 | auto future = client->async_send_request(msg); 37 | rclcpp::executors::SingleThreadedExecutor ex; 38 | ex.add_node(node); 39 | ex.spin_until_future_complete(future); 40 | auto res = future.get(); 41 | ASSERT_TRUE(res->success); 42 | } 43 | 44 | ///////////////////////////////////////////////// 45 | int main(int argc, char ** argv) 46 | { 47 | ::testing::InitGoogleTest(&argc, argv); 48 | return RUN_ALL_TESTS(); 49 | } 50 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/subscribers/ros_subscriber.cpp: -------------------------------------------------------------------------------- 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 | #include 17 | 18 | #include 19 | 20 | #include "ros_subscriber.hpp" 21 | 22 | static std::shared_ptr kTestNode; 23 | 24 | ///////////////////////////////////////////////// 25 | rclcpp::Node * ros_subscriber::TestNode() 26 | { 27 | if (kTestNode == nullptr) { 28 | kTestNode = rclcpp::Node::make_shared("ros_subscriber"); 29 | } 30 | return kTestNode.get(); 31 | } 32 | 33 | ///////////////////////////////////////////////// 34 | int main(int argc, char ** argv) 35 | { 36 | ::testing::InitGoogleTest(&argc, argv); 37 | rclcpp::init(argc, argv); 38 | auto ret = RUN_ALL_TESTS(); 39 | kTestNode.reset(); 40 | return ret; 41 | } 42 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/subscribers/ros_subscriber.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 SUBSCRIBERS__ROS_SUBSCRIBER_HPP_ 16 | #define SUBSCRIBERS__ROS_SUBSCRIBER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include "utils/test_utils.hpp" 23 | #include "utils/ros_test_msg.hpp" 24 | 25 | namespace ros_subscriber 26 | { 27 | ///////////////////////////////////////////////// 28 | /// \brief Retrieve a common node used for testing 29 | rclcpp::Node * TestNode(); 30 | 31 | ///////////////////////////////////////////////// 32 | /// \brief A class for testing ROS topic subscription. 33 | template 34 | class MyTestClass 35 | { 36 | public: 37 | /// \brief Class constructor. 38 | explicit MyTestClass(const std::string & _topic) 39 | { 40 | using std::placeholders::_1; 41 | this->sub = TestNode()->create_subscription( 42 | _topic, 1, 43 | std::bind(&MyTestClass::Cb, this, _1)); 44 | } 45 | 46 | /// \brief Member function called each time a topic update is received. 47 | 48 | public: 49 | void Cb(const ROS_T & _msg) 50 | { 51 | ros_gz_bridge::testing::compareTestMsg(std::make_shared(_msg)); 52 | this->callbackExecuted = true; 53 | } 54 | 55 | /// \brief Member variables that flag when the actions are executed. 56 | 57 | public: 58 | bool callbackExecuted = false; 59 | 60 | /// \brief ROS subscriber; 61 | 62 | private: 63 | typename rclcpp::Subscription::SharedPtr sub; 64 | }; 65 | 66 | 67 | } // namespace ros_subscriber 68 | 69 | #endif // SUBSCRIBERS__ROS_SUBSCRIBER_HPP_ 70 | -------------------------------------------------------------------------------- /ros_gz_bridge/test/utils/test_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 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 UTILS__TEST_UTILS_HPP_ 16 | #define UTILS__TEST_UTILS_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace ros_gz_bridge 25 | { 26 | namespace testing 27 | { 28 | 29 | /// \brief Wait until a boolean variable is set to true for a given number 30 | /// of times. 31 | /// \param[in out] _boolVar The bool variable. 32 | /// \param[in] _sleepEach Time duration to wait between each retry. 33 | /// \param[in] _retries The number of retries. 34 | /// 35 | /// E.g.: 36 | /// using namespace std::chrono_literals; 37 | /// waitUntilBoolVar(myVar, 1ms, 10); 38 | template 39 | void waitUntilBoolVar( 40 | bool & _boolVar, 41 | const std::chrono::duration & _sleepEach, 42 | const int _retries) 43 | { 44 | int i = 0; 45 | while (!_boolVar && i < _retries) { 46 | ++i; 47 | std::this_thread::sleep_for(_sleepEach); 48 | } 49 | } 50 | 51 | /// \brief Wait until a boolean variable is set to true for a given number 52 | /// of times. This function calls ros::spinOnce each iteration. 53 | /// \param[in out] _boolVar The bool variable. 54 | /// \param[in] _sleepEach Time duration to wait between each retry. 55 | /// \param[in] _retries The number of retries. 56 | /// 57 | /// E.g.: 58 | /// using namespace std::chrono_literals; 59 | /// waitUntilBoolVar(myVar, 1ms, 10); 60 | template 61 | void waitUntilBoolVarAndSpin( 62 | rclcpp::Node * node, 63 | bool & _boolVar, 64 | const std::chrono::duration & _sleepEach, 65 | const int _retries) 66 | { 67 | int i = 0; 68 | while (!_boolVar && i < _retries && rclcpp::ok()) { 69 | ++i; 70 | std::this_thread::sleep_for(_sleepEach); 71 | rclcpp::spin_some(node->get_node_base_interface()); 72 | } 73 | } 74 | 75 | } // namespace testing 76 | } // namespace ros_gz_bridge 77 | #endif // UTILS__TEST_UTILS_HPP_ 78 | -------------------------------------------------------------------------------- /ros_gz_image/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ros_gz_image) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(image_transport REQUIRED) 15 | find_package(ros_gz_bridge REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | 19 | find_package(gz_transport_vendor REQUIRED) 20 | find_package(gz-transport REQUIRED) 21 | 22 | find_package(gz_msgs_vendor REQUIRED) 23 | find_package(gz-msgs REQUIRED) 24 | 25 | include_directories(include) 26 | 27 | set(executable 28 | image_bridge 29 | ) 30 | 31 | add_executable(${executable} 32 | src/image_bridge.cpp 33 | ) 34 | 35 | target_link_libraries(${executable} 36 | ${std_msgs_TARGETS} 37 | image_transport::image_transport 38 | ros_gz_bridge::ros_gz_bridge 39 | rclcpp::rclcpp 40 | gz-msgs::core 41 | gz-transport::core 42 | ) 43 | 44 | install(TARGETS ${executable} 45 | DESTINATION lib/${PROJECT_NAME} 46 | ) 47 | 48 | if(BUILD_TESTING) 49 | find_package(ament_lint_auto REQUIRED) 50 | ament_lint_auto_find_test_dependencies() 51 | 52 | # find_package(rostest REQUIRED) 53 | # 54 | # set(test_publishers 55 | # gz_publisher 56 | # ) 57 | # 58 | # set(test_subscribers 59 | # ros_subscriber 60 | # ) 61 | # 62 | # foreach(test_publisher ${test_publishers}) 63 | # add_executable(${test_publisher}_image 64 | # test/publishers/${test_publisher}.cpp 65 | # ) 66 | # target_link_libraries(${test_publisher}_image 67 | # ${catkin_LIBRARIES} 68 | # $gz-msgs$::core 69 | # $gz-transport::core 70 | # gtest 71 | # gtest_main 72 | # ) 73 | # endforeach(test_publisher) 74 | # 75 | # foreach(test_subscriber ${test_subscribers}) 76 | # add_rostest_gtest(test_${test_subscriber}_image 77 | # test/${test_subscriber}.test 78 | # test/subscribers/${test_subscriber}.cpp) 79 | # target_link_libraries(test_${test_subscriber}_image 80 | # ${catkin_LIBRARIES} 81 | # $gz-msgs::core 82 | # $gz-transport::core 83 | # ) 84 | # endforeach(test_subscriber) 85 | endif() 86 | 87 | ament_package() 88 | -------------------------------------------------------------------------------- /ros_gz_image/README.md: -------------------------------------------------------------------------------- 1 | # Image utilities for using ROS and Gazebo Transport 2 | 3 | This package provides a unidirectional bridge for images from Gazebo to ROS. 4 | The bridge subscribes to Gazebo image messages (`gz::msgs::Image`) 5 | and republishes them to ROS using [image_transport](http://wiki.ros.org/image_transport). 6 | 7 | For compressed images, install 8 | [compressed_image_transport](http://wiki.ros.org/compressed_image_transport) 9 | and the bridge will publish `/compressed` images. The same goes for other 10 | `image_transport` plugins. 11 | 12 | To run the bridge from the command line: 13 | 14 | ```shell 15 | ros2 run ros_gz_image image_bridge /topic1 /topic2 16 | ``` 17 | 18 | You can also modify the [Quality of Service (QoS) policy](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-policies) used to publish images using an additional `qos` ROS parameter. For example: 19 | 20 | ```shell 21 | ros2 run ros_gz_image image_bridge /topic1 /topic2 --ros-args qos:=sensor_data 22 | ``` 23 | -------------------------------------------------------------------------------- /ros_gz_image/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_gz_image 3 | 3.0.1 4 | Image utilities for Gazebo simulation with ROS. 5 | Apache 2.0 6 | Aditya Pande 7 | Alejandro Hernandez 8 | 9 | Louise Poubel 10 | 11 | ament_cmake 12 | pkg-config 13 | 14 | image_transport 15 | ros_gz_bridge 16 | rclcpp 17 | sensor_msgs 18 | 19 | gz_msgs_vendor 20 | gz_transport_vendor 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /ros_gz_image/test/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_image/test/AMENT_IGNORE -------------------------------------------------------------------------------- /ros_gz_image/test/launch/test_ros_subscriber.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ros_gz_image/test/publishers/gz_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 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 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "../test_utils.h" 23 | 24 | /// \brief Flag used to break the publisher loop and terminate the program. 25 | static std::atomic g_terminatePub(false); 26 | 27 | ////////////////////////////////////////////////// 28 | /// \brief Function callback executed when a SIGINT or SIGTERM signals are 29 | /// captured. This is used to break the infinite loop that publishes messages 30 | /// and exit the program smoothly. 31 | void signal_handler(int _signal) 32 | { 33 | if (_signal == SIGINT || _signal == SIGTERM) 34 | g_terminatePub = true; 35 | } 36 | 37 | ////////////////////////////////////////////////// 38 | int main(int /*argc*/, char **/*argv*/) 39 | { 40 | // Install a signal handler for SIGINT and SIGTERM. 41 | std::signal(SIGINT, signal_handler); 42 | std::signal(SIGTERM, signal_handler); 43 | 44 | // Create a transport node and advertise a topic. 45 | gz::transport::Node node; 46 | 47 | // gz::msgs::Image. 48 | auto image_pub = node.Advertise("image"); 49 | gz::msgs::Image image_msg; 50 | ros_gz_image::testing::createTestMsg(image_msg); 51 | 52 | // Publish messages at 1Hz. 53 | while (!g_terminatePub) 54 | { 55 | image_pub.Publish(image_msg); 56 | 57 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 58 | } 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /ros_gz_image/test/ros_subscriber.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ros_gz_image/test/subscribers/ros_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019 Open Source Robotics Foundation 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 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "../test_utils.h" 23 | 24 | ////////////////////////////////////////////////// 25 | /// \brief A class for testing ROS topic subscription. 26 | template 27 | class MyTestClass 28 | { 29 | /// \brief Class constructor. 30 | public: MyTestClass(const std::string &_topic) 31 | { 32 | this->sub = this->n.subscribe(_topic, 1000, &MyTestClass::Cb, this); 33 | } 34 | 35 | /// \brief Member function called each time a topic update is received. 36 | public: void Cb(const ROS_T& _msg) 37 | { 38 | ros_gz_image::testing::compareTestMsg(_msg); 39 | this->callbackExecuted = true; 40 | }; 41 | 42 | /// \brief Member variables that flag when the actions are executed. 43 | public: bool callbackExecuted = false; 44 | 45 | /// \brief ROS node handle; 46 | private: ros::NodeHandle n; 47 | 48 | /// \brief ROS subscriber; 49 | private: ros::Subscriber sub; 50 | }; 51 | 52 | ///////////////////////////////////////////////// 53 | TEST(ROSSubscriberTest, Image) 54 | { 55 | MyTestClass client("image"); 56 | 57 | using namespace std::chrono_literals; 58 | ros_gz_image::testing::waitUntilBoolVarAndSpin( 59 | client.callbackExecuted, 10ms, 200); 60 | 61 | EXPECT_TRUE(client.callbackExecuted); 62 | } 63 | 64 | ///////////////////////////////////////////////// 65 | int main(int argc, char **argv) 66 | { 67 | ::testing::InitGoogleTest(&argc, argv); 68 | ros::init(argc, argv, "ros_image_subscriber"); 69 | 70 | return RUN_ALL_TESTS(); 71 | } 72 | -------------------------------------------------------------------------------- /ros_gz_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ros_gz_interfaces) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(builtin_interfaces REQUIRED) 14 | find_package(rcl_interfaces REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(rosidl_default_generators REQUIRED) 18 | 19 | set(msg_files 20 | "msg/Altimeter.msg" 21 | "msg/Contact.msg" 22 | "msg/Contacts.msg" 23 | "msg/Dataframe.msg" 24 | "msg/Entity.msg" 25 | "msg/EntityFactory.msg" 26 | "msg/EntityWrench.msg" 27 | "msg/Float32Array.msg" 28 | "msg/GuiCamera.msg" 29 | "msg/JointWrench.msg" 30 | "msg/Light.msg" 31 | "msg/LogicalCameraImage.msg" 32 | "msg/LogicalCameraImageModel.msg" 33 | "msg/MaterialColor.msg" 34 | "msg/ParamVec.msg" 35 | "msg/SensorNoise.msg" 36 | "msg/StringVec.msg" 37 | "msg/TrackVisual.msg" 38 | "msg/VideoRecord.msg" 39 | "msg/WorldControl.msg" 40 | "msg/WorldReset.msg" 41 | ) 42 | 43 | set(srv_files 44 | "srv/ControlWorld.srv" 45 | "srv/DeleteEntity.srv" 46 | "srv/SetEntityPose.srv" 47 | "srv/SpawnEntity.srv" 48 | ) 49 | 50 | rosidl_generate_interfaces(${PROJECT_NAME} 51 | ${msg_files} 52 | ${srv_files} 53 | DEPENDENCIES builtin_interfaces std_msgs geometry_msgs rcl_interfaces 54 | ADD_LINTER_TESTS 55 | ) 56 | 57 | ament_export_dependencies(rosidl_default_runtime) 58 | ament_package() 59 | -------------------------------------------------------------------------------- /ros_gz_interfaces/README.md: -------------------------------------------------------------------------------- 1 | # Message and service data structures for interacting with Gazebo from ROS2 2 | 3 | This package currently contains some Gazebo-specific ROS message and service data structures (.msg and .srv) 4 | 5 | ## Messages (.msg) 6 | 7 | * [Contact](msg/Contact.msg): related to [ignition::msgs::Contact](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/contact.proto). Contact info between collisions in Gazebo Sim. 8 | * [Contacts](msg/Contacts.msg): related to [ignition::msgs::Contacts](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/contacts.proto). A list of contacts. 9 | * [Entity](msg/Entity.msg): related to [ignition::msgs::Entity](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/entity.proto). Entity of Gazebo Sim. 10 | * [EntityFactory](msg/EntityFactory.msg): related to [ignition::msgs::EntityFactory](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/entity_factory.proto). Message to create a new entity. 11 | * [EntityWrench](msg/EntityWrench.msg): related to [ignition::msgs::EntityWrench](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/gz/msgs/entity_wrench.proto). Wrench to be applied to a specified Entity of Gazebo Sim. 12 | * [Light](msg/Light.msg): related to [ignition::msgs::Light](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/light.proto). Light info in Gazebo Sim. 13 | * [WorldControl](msg/WorldControl.msg): related to [ignition::msgs::WorldControl](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/world_control.proto). Message to control world of Gazebo Sim. 14 | * [WorldReset](msg/WorldReset.msg): related to [ignition::msgs::WorldReset](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/world_reset.proto). Reset time and model of simulation. 15 | 16 | ## Services (.srv) 17 | 18 | * [ControlWorld](srv/ControlWorld.srv): Control world of Gazebo Sim,for example,pasue,pasue with multiple steps,resume,etc. 19 | * [DeleteEntity](srv/DeleteEntity.srv): Delete Entity in Gazebo Sim 20 | * [SetEntityPose](srv/SetEntityPose.srv): Set pose of Entity in Gazebo Sim 21 | * [SpawnEntity](srv/SpawnEntity.srv): Spawn a Entity in Gazebo Sim 22 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Altimeter.msg: -------------------------------------------------------------------------------- 1 | # A message for Altimeter readings. 2 | 3 | # Optional header data. 4 | std_msgs/Header header 5 | 6 | # Vertical position data, in meters. 7 | float64 vertical_position 8 | 9 | # Vertical velocity data, in meters/second. 10 | float64 vertical_velocity 11 | 12 | # Vertical reference. 13 | float64 vertical_reference 14 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Contact.msg: -------------------------------------------------------------------------------- 1 | ros_gz_interfaces/Entity collision1 # Contact collision1 2 | ros_gz_interfaces/Entity collision2 # Contact collision2 3 | geometry_msgs/Vector3[] positions # List of contact position 4 | geometry_msgs/Vector3[] normals # List of contact normals 5 | float64[] depths # List of penetration depths 6 | ros_gz_interfaces/JointWrench[] wrenches # List of joint wrenches including forces/torques 7 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Contacts.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # Time stamp 2 | ros_gz_interfaces/Contact[] contacts # List of contacts 3 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Dataframe.msg: -------------------------------------------------------------------------------- 1 | # Message containing some payload as well as sender and destination 2 | # Intended for use with the comms system in Gazebo 3 | std_msgs/Header header # Time stamp 4 | string src_address # Address of the sender 5 | string dst_address # Address of the destination 6 | uint8[] data # Payload 7 | float64 rssi # Received Signal Strength Indicator 8 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Entity.msg: -------------------------------------------------------------------------------- 1 | # Entity type: constant definition 2 | uint8 NONE = 0 3 | uint8 LIGHT = 1 4 | uint8 MODEL = 2 5 | uint8 LINK = 3 6 | uint8 VISUAL = 4 7 | uint8 COLLISION = 5 8 | uint8 SENSOR = 6 9 | uint8 JOINT = 7 10 | 11 | uint64 id # Entity unique identifier across all types. Defaults to 0 12 | string name # Entity name, which is not guaranteed to be unique. 13 | uint8 type # Entity type. 14 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/EntityFactory.msg: -------------------------------------------------------------------------------- 1 | string name # New name for the entity, overrides the name on the SDF 2 | bool allow_renaming false # Whether the server is allowed to rename the entity in case of 3 | # overlap with existing entities. 4 | 5 | # Only one method is supported at a time (sdf,sdf_filename,clone_name) 6 | string sdf # SDF description in string format 7 | string sdf_filename # Full path to SDF file. 8 | string clone_name # Name of entity to clone 9 | 10 | geometry_msgs/Pose pose # Pose where the entity will be spawned in the world. 11 | string relative_to "world" # Pose is defined relative to the frame of this entity. 12 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/EntityWrench.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # Time stamp 2 | ros_gz_interfaces/Entity entity # Entity 3 | geometry_msgs/Wrench wrench # Wrench to be applied to entity 4 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Float32Array.msg: -------------------------------------------------------------------------------- 1 | float32[] data 2 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/GuiCamera.msg: -------------------------------------------------------------------------------- 1 | # Message for a GUI Camera. 2 | 3 | # Optional header data. 4 | std_msgs/Header header 5 | 6 | string name 7 | string view_controller 8 | geometry_msgs/Pose pose 9 | TrackVisual track 10 | 11 | # Type of projection: "perspective" or "orthographic". 12 | string projection_type 13 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/JointWrench.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # Time stamp 2 | std_msgs/String body_1_name # Body 1 name string 3 | std_msgs/UInt32 body_1_id # Body 1 id 4 | std_msgs/String body_2_name # Body 2 name string 5 | std_msgs/UInt32 body_2_id # Body 2 id 6 | 7 | geometry_msgs/Wrench body_1_wrench # Body 1 wrench 8 | geometry_msgs/Wrench body_2_wrench # Body 2 wrench 9 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/Light.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # Optional header data 2 | 3 | string name # Light name 4 | 5 | # Light type: constant definition 6 | uint8 POINT = 0 7 | uint8 SPOT = 1 8 | uint8 DIRECTIONAL = 2 9 | 10 | uint8 type # Light type (from constant definitions) 11 | 12 | geometry_msgs/Pose pose # Light pose 13 | std_msgs/ColorRGBA diffuse # Light diffuse emission 14 | std_msgs/ColorRGBA specular # Light specular emission 15 | float32 attenuation_constant # Constant variable in attenuation formula 16 | float32 attenuation_linear # Linear variable in attenuation formula 17 | float32 attenuation_quadratic # Quadratic variable in attenuation formula 18 | geometry_msgs/Vector3 direction # Light direction 19 | float32 range # Light range 20 | bool cast_shadows # Enable/disable shadow casting 21 | float32 spot_inner_angle # Spotlight inner cone angle 22 | float32 spot_outer_angle # Spotlight outer cone angle 23 | float32 spot_falloff # Falloff between inner and outer cone 24 | 25 | uint32 id # Unique id of the light 26 | 27 | uint32 parent_id # Unique id of the light's parent 28 | 29 | float32 intensity # Light intensity 30 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/LogicalCameraImage.msg: -------------------------------------------------------------------------------- 1 | # Optional header data. 2 | std_msgs/Header header 3 | 4 | # Logical camera pose 5 | geometry_msgs/Pose pose 6 | 7 | # Detected models 8 | LogicalCameraImageModel[] model 9 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/LogicalCameraImageModel.msg: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Pose pose 3 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/MaterialColor.msg: -------------------------------------------------------------------------------- 1 | # Entities that match to apply material color: constant definition 2 | uint8 FIRST = 0 3 | uint8 ALL = 1 4 | 5 | std_msgs/Header header # Optional header data 6 | ros_gz_interfaces/Entity entity # Entity to change material color 7 | std_msgs/ColorRGBA ambient # Ambient color 8 | std_msgs/ColorRGBA diffuse # Diffuse color 9 | std_msgs/ColorRGBA specular # Specular color 10 | std_msgs/ColorRGBA emissive # Emissive color 11 | float64 shininess # Specular exponent 12 | uint8 entity_match # Entities that match to apply material color 13 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/ParamVec.msg: -------------------------------------------------------------------------------- 1 | # A message for a vector of parameters data. 2 | 3 | # Optional header data. 4 | std_msgs/Header header 5 | 6 | # A set of name, value pairs 7 | rcl_interfaces/Parameter[] params 8 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/SensorNoise.msg: -------------------------------------------------------------------------------- 1 | # A message for specifying sensor noise. 2 | 3 | # Noise type 4 | uint8 NONE = 0 5 | uint8 GAUSSIAN = 2 6 | uint8 GAUSSIAN_QUANTIZED = 3 7 | 8 | # Optional header data. 9 | std_msgs/Header header 10 | 11 | # The type of noise 12 | uint8 type 13 | 14 | # Noise mean 15 | # Used by GAUSSIAN and GAUSSIAN_QUANTIZED 16 | float64 mean 17 | 18 | # Noise standard deviation 19 | # Used by GAUSSIAN and GAUSSIAN_QUANTIZED 20 | float64 stddev 21 | 22 | # Noise mean bias 23 | # Used by GAUSSIAN and GAUSSIAN_QUANTIZED 24 | float64 bias_mean 25 | 26 | # Noise standard deviation bias 27 | float64 bias_stddev 28 | 29 | # Noise precision 30 | # Used by GAUSSIAN_QUANTIZED 31 | float64 precision 32 | 33 | # For type "gaussian*", the standard deviation of the noise used to 34 | # drive a process to model slow variations in a sensor bias. 35 | float64 dynamic_bias_stddev 36 | 37 | # For type "gaussian*", the correlation time in seconds of the 38 | # noise used to drive a process to model slow variations in a sensor bias. 39 | # A typical value, when used, would be on the order of 40 | # 3600 seconds (1 hour). 41 | float64 dynamic_bias_correlation_time 42 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/StringVec.msg: -------------------------------------------------------------------------------- 1 | # A message for a vector of string data. 2 | 3 | # Optional header data. 4 | std_msgs/Header header 5 | 6 | # The vector of strings. 7 | string[] data 8 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/TrackVisual.msg: -------------------------------------------------------------------------------- 1 | # Message for a tracking a rendering::Visual with a rendering::Camera. 2 | 3 | # Optional header data. 4 | std_msgs/Header header 5 | 6 | # Name of the visual to track. 7 | string name 8 | 9 | # Id of the visual to track. 10 | uint32 id 11 | 12 | # True to have the tracking camera inherit the orientation of 13 | # the tracked visual. 14 | bool inherit_orientation 15 | 16 | # Minimum follow distance. 17 | float64 min_dist 18 | 19 | # Maximum follow distance. 20 | float64 max_dist 21 | 22 | # If set to true, the position of the camera is fixed. 23 | bool is_static 24 | 25 | # If set to true, the position of the camera is relative to the. 26 | # model reference frame. 27 | bool use_model_frame 28 | 29 | # Position of the camera. 30 | geometry_msgs/Vector3 xyz 31 | 32 | # If set to true, the camera inherits the yaw rotation of the model. 33 | bool inherit_yaw 34 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/VideoRecord.msg: -------------------------------------------------------------------------------- 1 | # A message that allows for control of video recording functions. 2 | 3 | # Optional header data. 4 | std_msgs/Header header 5 | 6 | # True to start video recording. 7 | bool start 8 | 9 | # True to stop video recording. 10 | bool stop 11 | 12 | # Video encoding format, e.g. "mp4", "ogv". 13 | string format 14 | 15 | # filename of the recorded video. 16 | string save_filename 17 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/WorldControl.msg: -------------------------------------------------------------------------------- 1 | bool pause # Paused state. 2 | bool step # 3 | uint32 multi_step 0 # Paused after stepping multi_step. 4 | ros_gz_interfaces/WorldReset reset # 5 | uint32 seed # 6 | builtin_interfaces/Time run_to_sim_time # A simulation time in the future to run to and 7 | # then pause. 8 | -------------------------------------------------------------------------------- /ros_gz_interfaces/msg/WorldReset.msg: -------------------------------------------------------------------------------- 1 | bool all false # Reset time and model 2 | bool time_only false # Reset time only 3 | bool model_only false # Reset model only 4 | -------------------------------------------------------------------------------- /ros_gz_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_gz_interfaces 3 | 3.0.1 4 | Message and service data structures for interacting with Gazebo from ROS2. 5 | Apache 2.0 6 | Louise Poubel 7 | Zhenpeng Ge 8 | Aditya Pande 9 | Alejandro Hernandez 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | builtin_interfaces 14 | std_msgs 15 | geometry_msgs 16 | rcl_interfaces 17 | 18 | builtin_interfaces 19 | std_msgs 20 | geometry_msgs 21 | rosidl_default_runtime 22 | rcl_interfaces 23 | 24 | ament_lint_common 25 | rosidl_interface_packages 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /ros_gz_interfaces/srv/ControlWorld.srv: -------------------------------------------------------------------------------- 1 | ros_gz_interfaces/WorldControl world_control # Message to Control world in Gazebo Sim 2 | --- 3 | bool success # Return true if control is successful. 4 | -------------------------------------------------------------------------------- /ros_gz_interfaces/srv/DeleteEntity.srv: -------------------------------------------------------------------------------- 1 | ros_gz_interfaces/Entity entity # Gazebo Sim entity to be deleted. 2 | --- 3 | bool success # Return true if deletion is successful. 4 | -------------------------------------------------------------------------------- /ros_gz_interfaces/srv/SetEntityPose.srv: -------------------------------------------------------------------------------- 1 | ros_gz_interfaces/Entity entity # Gazebo Sim entity. 2 | geometry_msgs/Pose pose # Pose of entity. 3 | --- 4 | bool success # Return true if set successfully. 5 | -------------------------------------------------------------------------------- /ros_gz_interfaces/srv/SpawnEntity.srv: -------------------------------------------------------------------------------- 1 | ros_gz_interfaces/EntityFactory entity_factory # Message to create a new entity 2 | --- 3 | bool success # Return true if spawned successfully. 4 | -------------------------------------------------------------------------------- /ros_gz_point_cloud/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros1_ign_point_cloud 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.0 (2019-08-15) 6 | ------------------ 7 | 8 | 0.6.3 (2019-08-04) 9 | ------------------ 10 | 11 | 0.6.2 (2019-08-04) 12 | ------------------ 13 | 14 | 0.6.1 (2019-08-04) 15 | ------------------ 16 | 17 | 0.6.0 (2019-08-02) 18 | ------------------ 19 | 20 | * 0.5.0 21 | * Merge pull request `#28 `_ from osrf/pointcloudpacked 22 | Bridge point cloud packed 23 | * Contributors: Nate Koenig 24 | 25 | * Merge pull request `#28 `_ from osrf/pointcloudpacked 26 | Bridge point cloud packed 27 | * Contributors: Nate Koenig 28 | 29 | 0.4.0 (2019-07-16) 30 | ------------------ 31 | 32 | 0.3.1 (2019-07-01) 33 | ------------------ 34 | 35 | 0.3.0 (2019-06-28) 36 | ------------------ 37 | * 0.2.0 38 | * Merge pull request `#21 `_ from osrf/lidar 39 | Point clouds from lidars 40 | * fix RGBD's color 41 | * final tweaks 42 | * PC2 for gpu_lidar, 1 vertical sample 43 | * Start of lidar PC 44 | * Demos package (`#19 `_) 45 | * Start of demos package: camera 46 | * IMU 47 | * depth camera 48 | * magnetometer 49 | * lidar, base launch 50 | * READMEs, RGBD camera 51 | * screenshots 52 | * missing IMU 53 | * set plugin path env 54 | * It's best to always set it 55 | * Point clouds for RGBD cameras (`#17 `_) 56 | * Beginning of point cloud package 57 | * Populating image data, but result is not correct. Must find out where's the source of the problem. 58 | * RGB -> BGR: why? 59 | * Cleanup code and example 60 | * pointcloud -> point_cloud 61 | * add keys - how was this working before? 62 | * install wget 63 | * well, we need ign-gz2 :sweat_smile: 64 | * README update 65 | * PR feedback 66 | * .travis/build: rosdep skip ignition keys (`#18 `_) 67 | * .travis/build: rosdep skip ignition keys 68 | * Update build 69 | * Contributors: Nate Koenig, chapulina 70 | 71 | 0.2.2 (2019-05-20) 72 | ------------------ 73 | 74 | 0.2.1 (2019-05-11) 75 | ------------------ 76 | 77 | 0.2.0 (2019-05-09) 78 | ------------------ 79 | 80 | 0.1.0 (2019-03-20) 81 | ------------------ 82 | -------------------------------------------------------------------------------- /ros_gz_point_cloud/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ros_gz_point_cloud) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | sensor_msgs) 16 | 17 | find_package(ignition-gazebo2 2.1 QUIET REQUIRED) 18 | set(GZ_SIM_VER ${ignition-gazebo2_VERSION_MAJOR}) 19 | 20 | find_package(ignition-rendering2 QUIET REQUIRED) 21 | set(GZ_RENDERING_VER ${ignition-rendering2_VERSION_MAJOR}) 22 | 23 | find_package(ignition-sensors2 QUIET REQUIRED) 24 | set(GZ_SENSORS_VER ${ignition-sensors2_VERSION_MAJOR}) 25 | 26 | catkin_package() 27 | 28 | include_directories( 29 | include 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | set(plugin_name RosGzPointCloud) 34 | add_library(${plugin_name} SHARED 35 | src/point_cloud.cc 36 | ) 37 | target_link_libraries(${plugin_name} 38 | ignition-gazebo${GZ_SIM_VER}::core 39 | ignition-rendering${GZ_RENDERING_VER}::core 40 | ignition-sensors${GZ_SENSORS_VER}::core 41 | ${catkin_LIBRARIES} 42 | ) 43 | install(TARGETS ${plugin_name} 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 47 | ) 48 | install( 49 | DIRECTORY 50 | examples/ 51 | DESTINATION 52 | ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples 53 | ) 54 | 55 | # TODO(CH3): Install symlink to deprecated library name. Remove on tock. 56 | string(REPLACE "RosGz" "RosIgn" plugin_name_ign ${plugin_name}) 57 | if(WIN32) 58 | # symlinks on Windows require admin privileges, fallback to copy 59 | ADD_CUSTOM_COMMAND(TARGET ${plugin_name} POST_BUILD 60 | COMMAND "${CMAKE_COMMAND}" -E copy 61 | "$" 62 | "$/${plugin_name_ign}") 63 | else() 64 | file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/lib") 65 | EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${plugin_name} ${plugin_name_ign} WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/lib") 66 | INSTALL(FILES ${PROJECT_BINARY_DIR}/lib/${plugin_name_ign} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 67 | endif() 68 | -------------------------------------------------------------------------------- /ros_gz_point_cloud/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_point_cloud/COLCON_IGNORE -------------------------------------------------------------------------------- /ros_gz_point_cloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_gz_point_cloud 3 | 0.7.0 4 | Point cloud utilities for Gazebo simulation with ROS. 5 | Apache 2.0 6 | Aditya Pande 7 | Alejandro Hernandez 8 | 9 | Louise Poubel 10 | 11 | catkin 12 | 13 | ignition-gazebo2 14 | ignition-rendering2 15 | ignition-sensors2 16 | roscpp 17 | sensor_msgs 18 | 19 | message_runtime 20 | 21 | -------------------------------------------------------------------------------- /ros_gz_point_cloud/src/point_cloud.hh: -------------------------------------------------------------------------------- 1 | // Copyright 2019 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 ROS_GZ_POINTCLOUD__POINTCLOUD_HPP_ 16 | #define ROS_GZ_POINTCLOUD__POINTCLOUD_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace ros_gz_point_cloud 22 | { 23 | // Forward declarations. 24 | class PointCloudPrivate; 25 | 26 | /// \brief System which publishes ROS PointCloud2 messages for RGBD or GPU lidar sensors. 27 | /// 28 | /// This plugin should be attached to an RGBD or GPU lidar sensor (i.e. ) 29 | /// 30 | /// Important: load `gz::sim::systems::Sensors` as well, which will create the sensor. 31 | /// 32 | /// SDF parameters: 33 | /// * ``: Namespace for ROS node, defaults to sensor scoped name 34 | /// * ``: ROS topic to publish to, defaults to "points" 35 | /// * ``: TF frame name to populate message header, defaults to sensor scoped name 36 | /// * ``: Render engine name, defaults to 'ogre2' 37 | /// * ``: Scene name, defaults to 'scene' 38 | class PointCloud: 39 | public gz::sim::System, 40 | public gz::sim::ISystemConfigure, 41 | public gz::sim::ISystemPostUpdate 42 | { 43 | /// \brief Constructor 44 | 45 | public: 46 | PointCloud(); 47 | 48 | /// \brief Destructor 49 | 50 | public: 51 | ~PointCloud() override = default; 52 | 53 | // Documentation inherited 54 | 55 | public: 56 | void Configure( 57 | const gz::sim::Entity & _entity, 58 | const std::shared_ptr < const sdf::Element > & _sdf, 59 | gz::sim::EntityComponentManager & _ecm, 60 | gz::sim::EventManager & _eventMgr) override; 61 | 62 | // Documentation inherited 63 | 64 | public: 65 | void PostUpdate( 66 | const gz::sim::UpdateInfo & _info, 67 | const gz::sim::EntityComponentManager & _ecm) override; 68 | 69 | /// \brief Private data pointer. 70 | 71 | private: 72 | std::unique_ptr < PointCloudPrivate > dataPtr; 73 | }; 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /ros_gz_sim/include/ros_gz_sim/delete_entity.hpp: -------------------------------------------------------------------------------- 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 | /// \file delete_entity.hpp 16 | /// \brief Defines utilities and a ROS 2 node for deleting entities from a Gazebo simulation. 17 | 18 | #ifndef ROS_GZ_SIM__DELETE_ENTITY_HPP_ 19 | #define ROS_GZ_SIM__DELETE_ENTITY_HPP_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | /// \brief A ROS 2 node for deleting entities from a Gazebo simulation. 29 | class EntityDeleter : public rclcpp::Node { 30 | public: 31 | /// \brief Default constructor. Initializes the node and delete entity client. 32 | EntityDeleter(); 33 | 34 | /// \brief Constructor that accepts an external delete service client. 35 | /// \param[in] client Shared pointer to the delete entity service client. 36 | explicit EntityDeleter( 37 | rclcpp::Client::SharedPtr client); 38 | 39 | /// \brief Deletes an entity from the simulation. 40 | /// \param[in] entity_name Name of the entity. 41 | /// \param[in] entity_id ID of the entity. 42 | /// \param[in] entity_type Type of the entity. 43 | /// \return True if the entity was deleted successfully, false otherwise. 44 | bool delete_entity( 45 | const std::string & entity_name, int entity_id, 46 | int entity_type); 47 | 48 | protected: 49 | /// \brief Client used to call the delete entity service. 50 | rclcpp::Client::SharedPtr client_; 51 | }; 52 | 53 | #endif // ROS_GZ_SIM__DELETE_ENTITY_HPP_ 54 | -------------------------------------------------------------------------------- /ros_gz_sim/include/ros_gz_sim/gzserver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Open Source Robotics Foundation 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 ROS_GZ_SIM__GZSERVER_HPP_ 16 | #define ROS_GZ_SIM__GZSERVER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | // ROS node that executes a gz-sim Server given a world SDF file or string. 22 | namespace ros_gz_sim 23 | { 24 | class GzServer : public rclcpp::Node 25 | { 26 | public: 27 | // Class constructor. 28 | explicit GzServer(const rclcpp::NodeOptions & options); 29 | 30 | public: 31 | // Class destructor. 32 | ~GzServer(); 33 | 34 | public: 35 | /// \brief Run the gz sim server. 36 | void OnStart(); 37 | 38 | private: 39 | /// \internal 40 | /// \brief Private data pointer. 41 | GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) 42 | }; 43 | } // namespace ros_gz_sim 44 | 45 | #endif // ROS_GZ_SIM__GZSERVER_HPP_ 46 | -------------------------------------------------------------------------------- /ros_gz_sim/launch/gz_remove_model.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 | """Launch remove models in gz sim.""" 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import DeclareLaunchArgument 19 | from launch.substitutions import LaunchConfiguration, TextSubstitution 20 | from launch_ros.actions import Node 21 | 22 | 23 | def generate_launch_description(): 24 | 25 | world = LaunchConfiguration('world') 26 | entity_name = LaunchConfiguration('entity_name') 27 | 28 | declare_world_cmd = DeclareLaunchArgument( 29 | 'world', default_value=TextSubstitution(text=''), 30 | description='World name') 31 | declare_entity_name_cmd = DeclareLaunchArgument( 32 | 'entity_name', default_value=TextSubstitution(text=''), 33 | description='SDF filename') 34 | 35 | remove = Node( 36 | package='ros_gz_sim', 37 | executable='remove', 38 | output='screen', 39 | parameters=[{'world': world, 'entity_name': entity_name}], 40 | ) 41 | 42 | # Create the launch description and populate 43 | ld = LaunchDescription() 44 | 45 | # Declare the launch options 46 | ld.add_action(declare_world_cmd) 47 | ld.add_action(declare_entity_name_cmd) 48 | ld.add_action(remove) 49 | 50 | return ld 51 | -------------------------------------------------------------------------------- /ros_gz_sim/launch/gz_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ros_gz_sim/launch/gz_server.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 | """Launch gz_server in a component container.""" 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import DeclareLaunchArgument 19 | from launch.substitutions import LaunchConfiguration, TextSubstitution 20 | from ros_gz_sim.actions import GzServer 21 | 22 | 23 | def generate_launch_description(): 24 | 25 | declare_world_sdf_file_cmd = DeclareLaunchArgument( 26 | 'world_sdf_file', default_value=TextSubstitution(text=''), 27 | description='Path to the SDF world file') 28 | declare_world_sdf_string_cmd = DeclareLaunchArgument( 29 | 'world_sdf_string', default_value=TextSubstitution(text=''), 30 | description='SDF world string') 31 | declare_container_name_cmd = DeclareLaunchArgument( 32 | 'container_name', default_value='ros_gz_container', 33 | description='Name of container that nodes will load in if use composition',) 34 | declare_create_own_container_cmd = DeclareLaunchArgument( 35 | 'create_own_container', default_value='False', 36 | description='Whether we should start our own ROS container when using composition.',) 37 | declare_use_composition_cmd = DeclareLaunchArgument( 38 | 'use_composition', default_value='False', 39 | description='Use composed bringup if True') 40 | 41 | gz_server_action = GzServer( 42 | world_sdf_file=LaunchConfiguration('world_sdf_file'), 43 | world_sdf_string=LaunchConfiguration('world_sdf_string'), 44 | container_name=LaunchConfiguration('container_name'), 45 | create_own_container=LaunchConfiguration('create_own_container'), 46 | use_composition=LaunchConfiguration('use_composition'), 47 | ) 48 | 49 | # Create the launch description and populate 50 | ld = LaunchDescription() 51 | 52 | # Declare the launch options 53 | ld.add_action(declare_world_sdf_file_cmd) 54 | ld.add_action(declare_world_sdf_string_cmd) 55 | ld.add_action(declare_container_name_cmd) 56 | ld.add_action(declare_create_own_container_cmd) 57 | ld.add_action(declare_use_composition_cmd) 58 | # Add the gz_server action 59 | ld.add_action(gz_server_action) 60 | 61 | return ld 62 | -------------------------------------------------------------------------------- /ros_gz_sim/launch/gz_spawn_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /ros_gz_sim/launch/ros_gz_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 19 | 20 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /ros_gz_sim/launch/ros_gz_spawn_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 34 | 35 | 36 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /ros_gz_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | ros_gz_sim 6 | 3.0.1 7 | Tools for using Gazebo Sim simulation with ROS. 8 | 9 | 10 | Aditya Pande 11 | Alejandro Hernandez 12 | 13 | Apache 2.0 14 | 15 | 16 | Addisu Taddese 17 | Carlos Agüero 18 | Alejandro Hernandez 19 | 20 | 21 | ament_cmake 22 | ament_cmake_python 23 | pkg-config 24 | 25 | 26 | ament_index_python 27 | builtin_interfaces 28 | cli11 29 | geometry_msgs 30 | gz_math_vendor 31 | gz_msgs_vendor 32 | gz_sim_vendor 33 | gz_transport_vendor 34 | launch 35 | launch_ros 36 | libgflags-dev 37 | rclcpp 38 | rclcpp_components 39 | rcpputils 40 | ros_gz_interfaces 41 | std_msgs 42 | tf2 43 | tf2_ros 44 | 45 | 46 | ament_lint_auto 47 | ament_lint_common 48 | launch_ros 49 | launch_testing 50 | launch_testing_ament_cmake 51 | 52 | 53 | ament_cmake 54 | 55 | 56 | -------------------------------------------------------------------------------- /ros_gz_sim/resource/ros_gz_sim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim/resource/ros_gz_sim -------------------------------------------------------------------------------- /ros_gz_sim/ros_gz_sim/__init__.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 | """Main entry point for the `ros_gz_sim` package.""" 16 | 17 | from . import actions 18 | 19 | 20 | __all__ = [ 21 | 'actions', 22 | ] 23 | -------------------------------------------------------------------------------- /ros_gz_sim/ros_gz_sim/actions/__init__.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 | """Actions module.""" 16 | 17 | from .gz_spawn_model import GzSpawnModel 18 | from .gzserver import GzServer 19 | 20 | 21 | __all__ = [ 22 | 'GzSpawnModel', 23 | 'GzServer', 24 | ] 25 | -------------------------------------------------------------------------------- /ros_gz_sim/setup.cfg: -------------------------------------------------------------------------------- 1 | [options.entry_points] 2 | launch.frontend.launch_extension = 3 | ros_gz_sim = ros_gz_sim 4 | -------------------------------------------------------------------------------- /ros_gz_sim/src/gzserver.cpp: -------------------------------------------------------------------------------- 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 | #include "ros_gz_sim/gzserver.hpp" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | namespace ros_gz_sim 28 | { 29 | 30 | class GzServer::Implementation 31 | { 32 | /// \brief We don't want to block the ROS thread. 33 | 34 | public: 35 | std::thread thread; 36 | }; 37 | 38 | GzServer::GzServer(const rclcpp::NodeOptions & options) 39 | : Node("gzserver", options), dataPtr(gz::utils::MakeUniqueImpl()) 40 | { 41 | this->dataPtr->thread = std::thread(std::bind(&GzServer::OnStart, this)); 42 | } 43 | 44 | GzServer::~GzServer() 45 | { 46 | // Make sure to join the thread on shutdown. 47 | if (this->dataPtr->thread.joinable()) { 48 | this->dataPtr->thread.join(); 49 | } 50 | } 51 | 52 | void GzServer::OnStart() 53 | { 54 | auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); 55 | auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); 56 | 57 | gz::common::Console::SetVerbosity(4); 58 | gz::sim::ServerConfig server_config; 59 | 60 | if (!world_sdf_file.empty()) { 61 | server_config.SetSdfFile(world_sdf_file); 62 | } else if (!world_sdf_string.empty()) { 63 | server_config.SetSdfString(world_sdf_string); 64 | } else { 65 | RCLCPP_ERROR( 66 | this->get_logger(), 67 | "Must specify either 'world_sdf_file' or 'world_sdf_string'"); 68 | rclcpp::shutdown(); 69 | return; 70 | } 71 | 72 | gz::sim::Server server(server_config); 73 | server.Run(true /*blocking*/, 0, false /*paused*/); 74 | rclcpp::shutdown(); 75 | } 76 | 77 | } // namespace ros_gz_sim 78 | 79 | RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer) 80 | -------------------------------------------------------------------------------- /ros_gz_sim/test/test_create.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 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 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // Simple application that provides a `/create` service and prints out the 26 | // sdf_filename of the request. This works in conjection with 27 | // test_create_node.launch.py 28 | int main() 29 | { 30 | std::mutex m; 31 | std::condition_variable cv; 32 | bool test_complete = false; 33 | 34 | gz::transport::Node node; 35 | auto cb = std::function( 36 | [&]( 37 | const gz::msgs::EntityFactory & _req, 38 | gz::msgs::Boolean & _res) -> bool { 39 | std::cout << _req.sdf_filename() << std::endl; 40 | _res.set_data(true); 41 | 42 | { 43 | std::lock_guard lk(m); 44 | test_complete = true; 45 | } 46 | cv.notify_one(); 47 | return true; 48 | }); 49 | 50 | node.Advertise("/world/default/create", cb); 51 | // wait until we receive a message. 52 | std::unique_lock lk(m); 53 | cv.wait(lk, [&] {return test_complete;}); 54 | // Sleep so that the service response can be sent before exiting. 55 | std::this_thread::sleep_for(std::chrono::seconds(1)); 56 | } 57 | -------------------------------------------------------------------------------- /ros_gz_sim/test/test_create_node.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 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 | import unittest 16 | 17 | from launch import LaunchDescription 18 | 19 | from launch_ros.actions import Node 20 | 21 | import launch_testing 22 | 23 | 24 | def generate_test_description(): 25 | expected_file_name = 'nonexistent/long/file_name' 26 | create = Node(package='ros_gz_sim', executable='create', 27 | parameters=[{'world': 'default', 'file': expected_file_name}], output='screen') 28 | test_create = Node(package='ros_gz_sim', executable='test_create', output='screen') 29 | return LaunchDescription([ 30 | create, 31 | test_create, 32 | launch_testing.util.KeepAliveProc(), 33 | launch_testing.actions.ReadyToTest(), 34 | ]), locals() 35 | 36 | 37 | class WaitForTests(unittest.TestCase): 38 | 39 | def test_termination(self, test_create, proc_info): 40 | proc_info.assertWaitForShutdown(process=test_create, timeout=200) 41 | 42 | 43 | @launch_testing.post_shutdown_test() 44 | class CreateTest(unittest.TestCase): 45 | 46 | def test_output(self, expected_file_name, test_create, proc_output): 47 | launch_testing.asserts.assertInStdout(proc_output, expected_file_name, test_create) 48 | -------------------------------------------------------------------------------- /ros_gz_sim/test/test_remove.cpp: -------------------------------------------------------------------------------- 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 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | // Simple application that provides a `/remove` service and prints out the 26 | // request's entity name. This works in conjunction with 27 | // test_remove_node.launch.py 28 | int main() 29 | { 30 | std::mutex m; 31 | std::condition_variable cv; 32 | bool test_complete = false; 33 | 34 | gz::transport::Node node; 35 | auto cb = std::function( 36 | [&]( 37 | const gz::msgs::Entity & _req, 38 | gz::msgs::Boolean & _res) -> bool { 39 | std::cout << _req.name() << std::endl; 40 | _res.set_data(true); 41 | 42 | { 43 | std::lock_guard lk(m); 44 | test_complete = true; 45 | } 46 | cv.notify_one(); 47 | return true; 48 | }); 49 | 50 | node.Advertise("/world/default/remove", cb); 51 | // wait until we receive a message. 52 | std::unique_lock lk(m); 53 | cv.wait(lk, [&] {return test_complete;}); 54 | // Sleep so that the service response can be sent before exiting. 55 | std::this_thread::sleep_for(std::chrono::seconds(1)); 56 | } 57 | -------------------------------------------------------------------------------- /ros_gz_sim/test/test_remove_node.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 | import unittest 16 | 17 | from launch import LaunchDescription 18 | 19 | from launch_ros.actions import Node 20 | 21 | import launch_testing 22 | 23 | 24 | def generate_test_description(): 25 | entity_name = 'my_robot' 26 | remove = Node(package='ros_gz_sim', 27 | executable='remove', 28 | parameters=[{'world': 'default', 'entity_name': entity_name}], 29 | output='screen') 30 | test_remove = Node(package='ros_gz_sim', executable='test_remove', output='screen') 31 | return LaunchDescription([ 32 | remove, 33 | test_remove, 34 | launch_testing.util.KeepAliveProc(), 35 | launch_testing.actions.ReadyToTest(), 36 | ]), locals() 37 | 38 | 39 | class WaitForTests(unittest.TestCase): 40 | 41 | def test_termination(self, test_remove, proc_info): 42 | proc_info.assertWaitForShutdown(process=test_remove, timeout=200) 43 | 44 | 45 | @launch_testing.post_shutdown_test() 46 | class RemoveTest(unittest.TestCase): 47 | 48 | def test_output(self, entity_name, test_remove, proc_output): 49 | launch_testing.asserts.assertInStdout(proc_output, entity_name, test_remove) 50 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ros_gz_sim_demos) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | if(BUILD_TESTING) 8 | find_package(ament_lint_auto REQUIRED) 9 | ament_lint_auto_find_test_dependencies() 10 | endif() 11 | 12 | install( 13 | DIRECTORY 14 | config/ 15 | DESTINATION share/${PROJECT_NAME}/config 16 | ) 17 | 18 | install( 19 | DIRECTORY 20 | launch/ 21 | DESTINATION share/${PROJECT_NAME}/launch 22 | ) 23 | 24 | install( 25 | DIRECTORY 26 | rviz/ 27 | DESTINATION share/${PROJECT_NAME}/rviz 28 | ) 29 | 30 | install( 31 | DIRECTORY 32 | models/ 33 | DESTINATION share/${PROJECT_NAME}/models 34 | ) 35 | 36 | install( 37 | DIRECTORY 38 | worlds/ 39 | DESTINATION share/${PROJECT_NAME}/worlds 40 | ) 41 | 42 | ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") 43 | 44 | ament_package() 45 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/air_pressure.yaml: -------------------------------------------------------------------------------- 1 | # Air pressure bridge configuration. 2 | - topic_name: "air_pressure" 3 | ros_type_name: "sensor_msgs/msg/FluidPressure" 4 | gz_type_name: "gz.msgs.FluidPressure" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/battery.yaml: -------------------------------------------------------------------------------- 1 | # Battery configuration. 2 | - topic_name: "/model/vehicle_blue/cmd_vel" 3 | ros_type_name: "geometry_msgs/msg/Twist" 4 | gz_type_name: "gz.msgs.Twist" 5 | lazy: true 6 | direction: ROS_TO_GZ 7 | 8 | - topic_name: "/model/vehicle_blue/battery/linear_battery/state" 9 | ros_type_name: "sensor_msgs/msg/BatteryState" 10 | gz_type_name: "gz.msgs.BatteryState" 11 | lazy: true 12 | direction: GZ_TO_ROS 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/camera.yaml: -------------------------------------------------------------------------------- 1 | # Camera bridge configuration. 2 | - topic_name: "/camera" 3 | ros_type_name: "sensor_msgs/msg/Image" 4 | gz_type_name: "gz.msgs.Image" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | 8 | - topic_name: "/camera_info" 9 | ros_type_name: "sensor_msgs/msg/CameraInfo" 10 | gz_type_name: "gz.msgs.CameraInfo" 11 | lazy: true 12 | direction: GZ_TO_ROS 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/diff_drive.yaml: -------------------------------------------------------------------------------- 1 | # Diff drive configuration. 2 | - topic_name: "/model/vehicle_blue/cmd_vel" 3 | ros_type_name: "geometry_msgs/msg/Twist" 4 | gz_type_name: "gz.msgs.Twist" 5 | lazy: true 6 | direction: ROS_TO_GZ 7 | 8 | - topic_name: "/model/vehicle_blue/odometry" 9 | ros_type_name: "nav_msgs/msg/Odometry" 10 | gz_type_name: "gz.msgs.Odometry" 11 | lazy: true 12 | direction: GZ_TO_ROS 13 | 14 | - topic_name: "/model/vehicle_green/cmd_vel" 15 | ros_type_name: "geometry_msgs/msg/Twist" 16 | gz_type_name: "gz.msgs.Twist" 17 | lazy: true 18 | direction: ROS_TO_GZ 19 | 20 | - topic_name: "/model/vehicle_green/odometry" 21 | ros_type_name: "nav_msgs/msg/Odometry" 22 | gz_type_name: "gz.msgs.Odometry" 23 | lazy: true 24 | direction: GZ_TO_ROS 25 | 26 | - ros_topic_name: "/tf" 27 | gz_topic_name: "/model/vehicle_green/tf" 28 | ros_type_name: "tf2_msgs/msg/TFMessage" 29 | gz_type_name: "gz.msgs.Pose_V" 30 | direction: GZ_TO_ROS 31 | 32 | - ros_topic_name: "/tf" 33 | gz_topic_name: "/model/vehicle_blue/tf" 34 | ros_type_name: "tf2_msgs/msg/TFMessage" 35 | gz_type_name: "gz.msgs.Pose_V" 36 | direction: GZ_TO_ROS 37 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/gpu_lidar.yaml: -------------------------------------------------------------------------------- 1 | # GPU lidar configuration. 2 | - topic_name: "/lidar" 3 | ros_type_name: "sensor_msgs/msg/LaserScan" 4 | gz_type_name: "gz.msgs.LaserScan" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | 8 | - topic_name: "/lidar/points" 9 | ros_type_name: "sensor_msgs/msg/PointCloud2" 10 | gz_type_name: "gz.msgs.PointCloudPacked" 11 | lazy: true 12 | direction: GZ_TO_ROS 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/imu.yaml: -------------------------------------------------------------------------------- 1 | # IMU configuration. 2 | - topic_name: "/imu" 3 | ros_type_name: "sensor_msgs/msg/Imu" 4 | gz_type_name: "gz.msgs.IMU" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/magnetometer.yaml: -------------------------------------------------------------------------------- 1 | # Magnetometer configuration. 2 | - topic_name: "/magnetometer" 3 | ros_type_name: "sensor_msgs/msg/MagneticField" 4 | gz_type_name: "gz.msgs.Magnetometer" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/navsat.yaml: -------------------------------------------------------------------------------- 1 | # Navsat configuration. 2 | - topic_name: "/navsat" 3 | ros_type_name: "sensor_msgs/msg/NavSatFix" 4 | gz_type_name: "gz.msgs.NavSat" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/navsat_gpsfix.yaml: -------------------------------------------------------------------------------- 1 | # Navsat_gpsfix configuration. 2 | - topic_name: "/navsat" 3 | ros_type_name: "gps_msgs/msg/GPSFix" 4 | gz_type_name: "gz.msgs.NavSat" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/rgbd_camera_bridge.yaml: -------------------------------------------------------------------------------- 1 | # rgbd_camera_bridge configuration. 2 | - ros_topic_name: "/camera/image" 3 | gz_topic_name: "/camera" 4 | ros_type_name: "sensor_msgs/msg/Image" 5 | gz_type_name: "gz.msgs.Image" 6 | lazy: true 7 | direction: GZ_TO_ROS 8 | 9 | - ros_topic_name: "/camera/camera_info" 10 | gz_topic_name: "/camera_info" 11 | ros_type_name: "sensor_msgs/msg/CameraInfo" 12 | gz_type_name: "gz.msgs.CameraInfo" 13 | lazy: true 14 | direction: GZ_TO_ROS 15 | 16 | - topic_name: "/rgbd_camera/image" 17 | ros_type_name: "sensor_msgs/msg/Image" 18 | gz_type_name: "gz.msgs.Image" 19 | lazy: true 20 | direction: GZ_TO_ROS 21 | 22 | - topic_name: "/rgbd_camera/camera_info" 23 | ros_type_name: "sensor_msgs/msg/CameraInfo" 24 | gz_type_name: "gz.msgs.CameraInfo" 25 | lazy: true 26 | direction: GZ_TO_ROS 27 | 28 | - topic_name: "/rgbd_camera/depth_image" 29 | ros_type_name: "sensor_msgs/msg/Image" 30 | gz_type_name: "gz.msgs.Image" 31 | lazy: true 32 | direction: GZ_TO_ROS 33 | 34 | - topic_name: "/rgbd_camera/points" 35 | ros_type_name: "sensor_msgs/msg/PointCloud2" 36 | gz_type_name: "gz.msgs.PointCloudPacked" 37 | lazy: true 38 | direction: GZ_TO_ROS 39 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/tf_bridge.yaml: -------------------------------------------------------------------------------- 1 | # tf_bridge configuration. 2 | - ros_topic_name: "/joint_states" 3 | gz_topic_name: "/world/default/model/double_pendulum_with_base0/joint_state" 4 | ros_type_name: "sensor_msgs/msg/JointState" 5 | gz_type_name: "gz.msgs.Model" 6 | lazy: true 7 | direction: GZ_TO_ROS 8 | 9 | - ros_topic_name: "/tf" 10 | gz_topic_name: "/model/double_pendulum_with_base0/pose" 11 | ros_type_name: "tf2_msgs/msg/TFMessage" 12 | gz_type_name: "gz.msgs.Pose_V" 13 | lazy: true 14 | direction: GZ_TO_ROS 15 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/config/triggered_camera.yaml: -------------------------------------------------------------------------------- 1 | # triggered_cammera configuration. 2 | - topic_name: "/camera" 3 | ros_type_name: "sensor_msgs/msg/Image" 4 | gz_type_name: "gz.msgs.Image" 5 | lazy: true 6 | direction: GZ_TO_ROS 7 | 8 | - topic_name: "/camera_info" 9 | ros_type_name: "sensor_msgs/msg/CameraInfo" 10 | gz_type_name: "gz.msgs.CameraInfo" 11 | lazy: true 12 | direction: GZ_TO_ROS 13 | 14 | - topic_name: "/camera/trigger" 15 | ros_type_name: "std_msgs/msg/Bool" 16 | gz_type_name: "gz.msgs.Boolean" 17 | lazy: true 18 | direction: ROS_TO_GZ 19 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in: -------------------------------------------------------------------------------- 1 | prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share 2 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/air_pressure_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/air_pressure_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/battery_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/battery_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/camera_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/camera_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/depth_camera_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/depth_camera_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/diff_drive_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/diff_drive_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/gpu_lidar_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/gpu_lidar_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/imu_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/imu_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/joint_states.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/joint_states.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/magnetometer_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/magnetometer_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/navsat_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/navsat_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/rgbd_camera_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/rgbd_camera_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/robot_state_publisher_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/robot_state_publisher_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/shapes_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/shapes_demo.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/images/tf_bridge.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/images/tf_bridge.gif -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/air_pressure.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/battery.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | from launch import LaunchDescription 19 | from launch.actions import DeclareLaunchArgument 20 | from launch.actions import IncludeLaunchDescription 21 | from launch.conditions import IfCondition 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import Node 25 | from ros_gz_bridge.actions import RosGzBridge 26 | 27 | 28 | def generate_launch_description(): 29 | 30 | pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 31 | pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') 32 | 33 | # RQt 34 | rqt = Node( 35 | package='rqt_plot', 36 | executable='rqt_plot', 37 | # FIXME: Why isn't the topic being populated on the UI? RQt issue? 38 | arguments=['--force-discover', 39 | '/model/vehicle_blue/battery/linear_battery/state/percentage'], 40 | condition=IfCondition(LaunchConfiguration('rqt')) 41 | ) 42 | 43 | # Gazebo 44 | gz_sim = IncludeLaunchDescription( 45 | PythonLaunchDescriptionSource( 46 | os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), 47 | launch_arguments={ 48 | 'gz_args': '-r -z 1000000 linear_battery_demo.sdf' 49 | }.items(), 50 | ) 51 | 52 | # Bridge 53 | ros_gz_bridge = RosGzBridge( 54 | bridge_name='ros_gz_bridge', 55 | config_file=os.path.join(pkg_ros_gz_sim_demos, 'config', 'battery.yaml'), 56 | ) 57 | 58 | return LaunchDescription([ 59 | gz_sim, 60 | DeclareLaunchArgument('rqt', default_value='true', 61 | description='Open RQt.'), 62 | ros_gz_bridge, 63 | rqt 64 | ]) 65 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/camera.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/depth_camera.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch.actions import IncludeLaunchDescription 22 | from launch.conditions import IfCondition 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch.substitutions import LaunchConfiguration 25 | 26 | from launch_ros.actions import Node 27 | 28 | 29 | def generate_launch_description(): 30 | 31 | pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 32 | 33 | gz_sim = IncludeLaunchDescription( 34 | PythonLaunchDescriptionSource( 35 | os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py') 36 | ), 37 | # launch_arguments={ 38 | # 'gz_args': '-r depth_camera.sdf' 39 | # }.items(), 40 | ) 41 | 42 | # RQt 43 | rqt = Node( 44 | package='rqt_image_view', 45 | executable='rqt_image_view', 46 | arguments=['/camera'], 47 | condition=IfCondition(LaunchConfiguration('rqt')) 48 | ) 49 | 50 | # RViz 51 | rviz = Node( 52 | package='rviz2', 53 | executable='rviz2', 54 | # FIXME: Generate new RViz config once this demo is usable again 55 | # arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'depth_camera.rviz')], 56 | condition=IfCondition(LaunchConfiguration('rviz')) 57 | ) 58 | 59 | # Bridge 60 | bridge = Node( 61 | package='ros_gz_bridge', 62 | executable='parameter_bridge', 63 | arguments=['/depth_camera@sensor_msgs/msg/Image@gz.msgs.Image'], 64 | output='screen' 65 | ) 66 | 67 | # FIXME: need a SDF file (depth_camera.sdf) inside ros_gz_point_cloud 68 | return LaunchDescription([ 69 | gz_sim, 70 | DeclareLaunchArgument('rviz', default_value='true', 71 | description='Open RViz.'), 72 | DeclareLaunchArgument('rqt', default_value='true', 73 | description='Open RQt.'), 74 | bridge, 75 | rviz, 76 | rqt 77 | ]) 78 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/diff_drive.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/gpu_lidar.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch.actions import IncludeLaunchDescription 22 | from launch.conditions import IfCondition 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch.substitutions import LaunchConfiguration 25 | 26 | from launch_ros.actions import Node 27 | 28 | 29 | def generate_launch_description(): 30 | 31 | pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 32 | 33 | gz_sim = IncludeLaunchDescription( 34 | PythonLaunchDescriptionSource( 35 | os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), 36 | # launch_arguments={ 37 | # 'gz_args': '-r gpu_lidar.sdf' 38 | # }.items(), 39 | ) 40 | 41 | # RViz 42 | rviz = Node( 43 | package='rviz2', 44 | executable='rviz2', 45 | # FIXME: Generate new RViz config once this demo is usable again 46 | # arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'gpu_lidar.rviz')], 47 | condition=IfCondition(LaunchConfiguration('rviz')) 48 | ) 49 | 50 | # Bridge 51 | bridge = Node( 52 | package='ros_gz_bridge', 53 | executable='parameter_bridge', 54 | arguments=['/lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan/'], 55 | output='screen' 56 | ) 57 | 58 | # FIXME: need a SDF file (gpu_lidar.sdf) inside ros_gz_point_cloud/ 59 | return LaunchDescription([ 60 | gz_sim, 61 | DeclareLaunchArgument('rviz', default_value='true', 62 | description='Open RViz.'), 63 | bridge, 64 | rviz 65 | ]) 66 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/gpu_lidar_bridge.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/image_bridge.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch.actions import IncludeLaunchDescription 22 | from launch.conditions import IfCondition 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch.substitutions import LaunchConfiguration 25 | 26 | from launch_ros.actions import Node 27 | 28 | 29 | def generate_launch_description(): 30 | 31 | pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 32 | 33 | gz_sim = IncludeLaunchDescription( 34 | PythonLaunchDescriptionSource( 35 | os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), 36 | launch_arguments={ 37 | 'gz_args': '-r sensors_demo.sdf' 38 | }.items(), 39 | ) 40 | 41 | # RQt 42 | rqt = Node( 43 | package='rqt_image_view', 44 | executable='rqt_image_view', 45 | arguments=[LaunchConfiguration('image_topic')], 46 | condition=IfCondition(LaunchConfiguration('rqt')) 47 | ) 48 | 49 | # Bridge 50 | bridge = Node( 51 | package='ros_gz_image', 52 | executable='image_bridge', 53 | arguments=['camera', 'depth_camera', 'rgbd_camera/image', 'rgbd_camera/depth_image'], 54 | output='screen' 55 | ) 56 | 57 | return LaunchDescription([ 58 | gz_sim, 59 | DeclareLaunchArgument('rqt', default_value='true', 60 | description='Open RQt.'), 61 | DeclareLaunchArgument('image_topic', default_value='/camera', 62 | description='Topic to start viewing in RQt.'), 63 | bridge, 64 | rqt 65 | ]) 66 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/imu.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/magnetometer.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/navsat.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/navsat_gpsfix.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/rgbd_camera.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch.actions import IncludeLaunchDescription 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | 24 | from launch_ros.actions import Node 25 | 26 | 27 | def generate_launch_description(): 28 | 29 | pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 30 | 31 | gz_sim = IncludeLaunchDescription( 32 | PythonLaunchDescriptionSource( 33 | os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), 34 | # launch_arguments={ 35 | # 'gz_args': '-r rgbd_camera.sdf' 36 | # }.items(), 37 | ) 38 | 39 | # FIXME: need rviz configuration migration 40 | # rviz = Node( 41 | # package='rviz2', 42 | # executable='rviz2', 43 | # arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'rgbd_camera.rviz')], 44 | # condition=IfCondition(LaunchConfiguration('rviz')) 45 | # ) 46 | 47 | # Bridge 48 | bridge = Node( 49 | package='ros_gz_bridge', 50 | executable='parameter_bridge', 51 | arguments=['/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image', 52 | '/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image'], 53 | output='screen' 54 | ) 55 | 56 | # FIXME: need a SDF file (depth_camera.sdf) inside ros_gz_point_cloud/ 57 | return LaunchDescription([ 58 | gz_sim, 59 | DeclareLaunchArgument('rviz', default_value='true', 60 | description='Open RViz.'), 61 | bridge, 62 | # rviz, 63 | ]) 64 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/rgbd_camera_bridge.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/tf_bridge.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/launch/triggered_camera.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/materials/textures/cardboard_box.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/models/cardboard_box/materials/textures/cardboard_box.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cardboard box 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A closed cardboard box. 19 | 20 | 21 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.15 0 0 0 5 | 6 | 7 | 2 8 | 9 | 0.041666667 10 | 0 11 | 0 12 | 0.056666667 13 | 0 14 | 0.068333333 15 | 16 | 17 | 18 | 19 | 20 | 0.5 0.4 0.3 21 | 22 | 23 | 24 | 25 | 26 | 1.0 27 | 1.0 28 | 29 | 30 | 31 | 32 | 10000000.0 33 | 1.0 34 | 0.001 35 | 0.1 36 | 37 | 38 | 39 | 40 | 41 | 0 0 -0.15 0 0 0 42 | 43 | 44 | meshes/cardboard_box.dae 45 | 1.25931896 1.007455168 0.755591376 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/thumbnails/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/models/cardboard_box/thumbnails/1.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/thumbnails/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/models/cardboard_box/thumbnails/2.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/thumbnails/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/models/cardboard_box/thumbnails/3.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/thumbnails/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/models/cardboard_box/thumbnails/4.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/cardboard_box/thumbnails/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/models/cardboard_box/thumbnails/5.png -------------------------------------------------------------------------------- /ros_gz_sim_demos/models/vehicle/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | vehicle 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Dharini Dutia 9 | dharini@openrobotics.org 10 | 11 | 12 | 13 | Parser plugin `sdformat_urdf` compatible differential drive vehicle model. Used to read 14 | models described in SDFormat and outputs URDF C++ DOM structures. 15 | 16 | 17 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_gz_sim_demos 3 | 3.0.1 4 | Demos using Gazebo Sim simulation with ROS. 5 | Apache 2.0 6 | Aditya Pande 7 | Alejandro Hernandez 8 | 9 | Louise Poubel 10 | 11 | ament_cmake 12 | 13 | gz_sim_vendor 14 | 15 | image_transport_plugins 16 | robot_state_publisher 17 | ros_gz_bridge 18 | ros_gz_sim 19 | ros_gz_image 20 | 21 | 22 | rqt_image_view 23 | rqt_plot 24 | rqt_topic 25 | rviz_imu_plugin 26 | rviz2 27 | sdformat_urdf 28 | tf2_ros 29 | xacro 30 | 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | -------------------------------------------------------------------------------- /ros_gz_sim_demos/resources/delete_entity.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/resources/delete_entity.gif -------------------------------------------------------------------------------- /ros_gz_sim_demos/resources/set_entity.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/resources/set_entity.gif -------------------------------------------------------------------------------- /ros_gz_sim_demos/resources/spawn.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gazebosim/ros_gz/6ab206d991072014230c1355e7566fe4899f80d0/ros_gz_sim_demos/resources/spawn.gif -------------------------------------------------------------------------------- /test_ros_gz_bridge/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package test_ros_gz_bridge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.1 (2025-05-26) 6 | ------------------ 7 | 8 | 3.0.0 (2025-05-06) 9 | ------------------ 10 | 11 | 2.1.6 (2025-03-21) 12 | ------------------ 13 | 14 | 2.1.5 (2025-02-24) 15 | ------------------ 16 | 17 | 2.1.4 (2025-02-12) 18 | ------------------ 19 | 20 | 2.1.3 (2025-01-14) 21 | ------------------ 22 | 23 | 2.1.2 (2024-10-31) 24 | ------------------ 25 | 26 | 2.1.1 (2024-10-14) 27 | ------------------ 28 | 29 | 2.1.0 (2024-09-12) 30 | ------------------ 31 | 32 | 2.0.1 (2024-08-29) 33 | ------------------ 34 | 35 | 2.0.0 (2024-07-22) 36 | ------------------ 37 | 38 | 1.0.1 (2024-07-03) 39 | ------------------ 40 | * Prepare for 1.0.0 Release (`#495 `_) 41 | * 0.244.14 42 | * Changelog 43 | * Correctly export ros_gz_bridge for downstream targets (`#503 `_) (`#506 `_) 44 | * Correctly export ros_gz_bridge for downstream targets (`#503 `_) 45 | * Contributors: Addisu Z. Taddese, Alejandro Hernández Cordero, Michael Carroll 46 | 47 | 1.0.0 (2024-04-24) 48 | ------------------ 49 | * Correctly export ros_gz_bridge for downstream targets (`#503 `_) 50 | * Contributors: Michael Carroll 51 | -------------------------------------------------------------------------------- /test_ros_gz_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(test_ros_gz_bridge) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(ros_gz_bridge REQUIRED) 15 | 16 | 17 | if(BUILD_TESTING) 18 | find_package(ament_lint_auto REQUIRED) 19 | ament_lint_auto_find_test_dependencies() 20 | 21 | find_package(ament_cmake_gtest REQUIRED) 22 | find_package(launch_testing_ament_cmake REQUIRED) 23 | ament_find_gtest() 24 | 25 | ament_add_gtest(test_ros_gz_bridge src/test_ros_gz_bridge.cpp) 26 | target_link_libraries(test_ros_gz_bridge ros_gz_bridge::ros_gz_bridge) 27 | endif() 28 | 29 | ament_package() 30 | -------------------------------------------------------------------------------- /test_ros_gz_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | test_ros_gz_bridge 5 | 3.0.1 6 | Bridge communication between ROS and Gazebo Transport 7 | Aditya Pande 8 | Alejandro Hernandez 9 | 10 | Apache 2.0 11 | 12 | Michael Carroll 13 | 14 | ament_cmake 15 | 16 | ros_gz_bridge 17 | 18 | ament_cmake_gtest 19 | ament_lint_auto 20 | ament_lint_common 21 | launch_testing_ament_cmake 22 | launch_ros 23 | launch_testing 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /test_ros_gz_bridge/src/test_ros_gz_bridge.cpp: -------------------------------------------------------------------------------- 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 | #include 16 | 17 | #include 18 | 19 | class test_ros_gz_bridge : public ::testing::Test 20 | { 21 | public: 22 | static void SetUpTestCase() 23 | { 24 | rclcpp::init(0, nullptr); 25 | } 26 | 27 | static void TearDownTestCase() 28 | { 29 | rclcpp::shutdown(); 30 | } 31 | }; 32 | 33 | TEST_F(test_ros_gz_bridge, SpawnNode) 34 | { 35 | ros_gz_bridge::RosGzBridge node; 36 | } 37 | -------------------------------------------------------------------------------- /tools/.codespell_ignore_words: -------------------------------------------------------------------------------- 1 | Lew 2 | EmPy 3 | -------------------------------------------------------------------------------- /tools/pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.codespell] 2 | ignore-words = "tools/.codespell_ignore_words" 3 | --------------------------------------------------------------------------------