From dd51540c4d6d5924ae56d6c040e1d09e73984308 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 3 Jul 2024 17:03:07 +0200 Subject: [PATCH] Imported upstream version '1.0.1' of 'upstream' --- README.md | 14 +- ros_gz/CHANGELOG.rst | 24 ++ ros_gz/package.xml | 2 +- ros_gz_bridge/CHANGELOG.rst | 130 +++++++++++ ros_gz_bridge/CMakeLists.txt | 8 + ros_gz_bridge/README.md | 1 + .../convert/ros_gz_interfaces.hpp | 28 +++ ros_gz_bridge/launch/ros_gz_bridge.launch | 18 ++ ros_gz_bridge/launch/ros_gz_bridge.launch.py | 114 ++++++++++ ros_gz_bridge/package.xml | 6 +- ros_gz_bridge/resource/ros_gz_bridge | 0 ros_gz_bridge/ros_gz_bridge/__init__.py | 12 +- .../ros_gz_bridge/actions/__init__.py | 22 ++ .../ros_gz_bridge/actions/ros_gz_bridge.py | 147 +++++++++++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 2 + ros_gz_bridge/setup.cfg | 3 + ros_gz_bridge/src/convert/gps_msgs.cpp | 2 +- .../src/convert/ros_gz_interfaces.cpp | 80 +++++++ ros_gz_bridge/src/convert/sensor_msgs.cpp | 12 +- ros_gz_bridge/src/factory.hpp | 16 +- ros_gz_bridge/test/utils/gz_test_msg.cpp | 57 ++++- ros_gz_bridge/test/utils/gz_test_msg.hpp | 18 ++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 45 ++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 18 ++ ros_gz_image/CHANGELOG.rst | 46 ++++ ros_gz_image/package.xml | 2 +- ros_gz_image/test/publishers/gz_publisher.cpp | 2 +- ros_gz_image/test/test_utils.h | 18 +- ros_gz_interfaces/CHANGELOG.rst | 47 ++++ ros_gz_interfaces/CMakeLists.txt | 2 + ros_gz_interfaces/README.md | 1 + ros_gz_interfaces/msg/EntityWrench.msg | 3 + ros_gz_interfaces/msg/MaterialColor.msg | 12 + ros_gz_interfaces/package.xml | 2 +- ros_gz_point_cloud/src/point_cloud.cc | 94 ++++---- ros_gz_point_cloud/src/point_cloud.hh | 18 +- ros_gz_sim/CHANGELOG.rst | 84 +++++++ ros_gz_sim/CMakeLists.txt | 44 ++++ ros_gz_sim/README.md | 74 +++++++ ros_gz_sim/launch/gz_server.launch | 12 + ros_gz_sim/launch/gz_server.launch.py | 80 +++++++ ros_gz_sim/launch/gz_sim.launch.py.in | 78 ++++++- ros_gz_sim/launch/gz_spawn_model.launch | 28 +++ ros_gz_sim/launch/gz_spawn_model.launch.py | 94 ++++++++ ros_gz_sim/launch/ros_gz_sim.launch | 26 +++ ros_gz_sim/launch/ros_gz_sim.launch.py | 111 ++++++++++ .../launch/ros_gz_spawn_model.launch.py | 154 +++++++++++++ ros_gz_sim/package.xml | 8 +- ros_gz_sim/resource/ros_gz_sim | 0 ros_gz_sim/ros_gz_sim/__init__.py | 22 ++ ros_gz_sim/ros_gz_sim/actions/__init__.py | 24 ++ .../ros_gz_sim/actions/gz_spawn_model.py | 207 ++++++++++++++++++ ros_gz_sim/ros_gz_sim/actions/gzserver.py | 111 ++++++++++ ros_gz_sim/setup.cfg | 3 + ros_gz_sim/src/gzserver.cpp | 80 +++++++ ros_gz_sim_demos/CHANGELOG.rst | 63 ++++++ ros_gz_sim_demos/package.xml | 2 +- test_ros_gz_bridge/CHANGELOG.rst | 9 + test_ros_gz_bridge/package.xml | 2 +- 59 files changed, 2237 insertions(+), 105 deletions(-) create mode 100644 ros_gz_bridge/launch/ros_gz_bridge.launch create mode 100644 ros_gz_bridge/launch/ros_gz_bridge.launch.py create mode 100644 ros_gz_bridge/resource/ros_gz_bridge create mode 100644 ros_gz_bridge/ros_gz_bridge/actions/__init__.py create mode 100644 ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py create mode 100644 ros_gz_bridge/setup.cfg create mode 100644 ros_gz_interfaces/msg/EntityWrench.msg create mode 100644 ros_gz_interfaces/msg/MaterialColor.msg create mode 100644 ros_gz_sim/launch/gz_server.launch create mode 100644 ros_gz_sim/launch/gz_server.launch.py create mode 100644 ros_gz_sim/launch/gz_spawn_model.launch create mode 100644 ros_gz_sim/launch/gz_spawn_model.launch.py create mode 100644 ros_gz_sim/launch/ros_gz_sim.launch create mode 100644 ros_gz_sim/launch/ros_gz_sim.launch.py create mode 100644 ros_gz_sim/launch/ros_gz_spawn_model.launch.py create mode 100644 ros_gz_sim/resource/ros_gz_sim create mode 100644 ros_gz_sim/ros_gz_sim/__init__.py create mode 100644 ros_gz_sim/ros_gz_sim/actions/__init__.py create mode 100644 ros_gz_sim/ros_gz_sim/actions/gz_spawn_model.py create mode 100644 ros_gz_sim/ros_gz_sim/actions/gzserver.py create mode 100644 ros_gz_sim/setup.cfg create mode 100644 ros_gz_sim/src/gzserver.cpp diff --git a/README.md b/README.md index 8346b53bb..1445dacfb 100644 --- a/README.md +++ b/README.md @@ -7,17 +7,19 @@ Foxy | Edifice | [foxy](https://github.com/gazebosim/ros_gz/tree/foxy) | only fr Galactic | Edifice | [galactic](https://github.com/gazebosim/ros_gz/tree/galactic) | https://packages.ros.org Galactic | Fortress | [galactic](https://github.com/gazebosim/ros_gz/tree/galactic) | only from source Humble | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | https://packages.ros.org -Humble | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | only from source -Iron | Fortress | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | https://packages.ros.org -Iron | Garden | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | only from source -Iron | Harmonic | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | only from source -Jazzy* | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | only from source +Humble | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | [gazebo packages](https://gazebosim.org/docs/latest/ros_installation#gazebo-garden-with-ros-2-humble-iron-or-rolling-use-with-caution-)[^1] +Humble | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | [gazebo packages](https://gazebosim.org/docs/harmonic/ros_installation#-gazebo-harmonic-with-ros-2-humble-iron-or-rolling-use-with-caution-)[^1] +Iron | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | https://packages.ros.org +Iron | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | only from source +Iron | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | only from source Jazzy* | Garden | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source Jazzy* | Harmonic | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | https://packages.ros.org Rolling | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | https://packages.ros.org Rolling | Garden | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source Rolling | Harmonic | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source +[^1]: Binaries for these pairings are provided from a the packages.osrfoundation.org repository. Refer to https://gazebosim.org/docs/latest/ros_installation for installation instructions. + * ROS 2 Jazzy Jalisco is slated for release on May 23rd, 2024. [Full ROS 2 release information is available in REP-2000.] For information on ROS(1) and Gazebo compatibility, refer to the [noetic branch README](https://github.com/gazebosim/ros_gz/tree/noetic) @@ -91,7 +93,7 @@ Install either [Edifice, Fortress, or Garden](https://gazebosim.org/docs). Set the `GZ_VERSION` environment variable to the Gazebo version you'd like to compile against. For example: - export GZ_VERSION=edifice + export GZ_VERSION=edifice # IMPORTANT: Replace with correct version > You only need to set this variable when compiling, not when running. diff --git a/ros_gz/CHANGELOG.rst b/ros_gz/CHANGELOG.rst index cb466ade8..5fec4881a 100644 --- a/ros_gz/CHANGELOG.rst +++ b/ros_gz/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package ros_gz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Prepare for 1.0.0 Release (`#495 `_) +* 0.244.14 +* Changelog +* 0.244.13 +* Changelog +* 0.244.12 +* Changelog +* 0.246.0 +* Update changelogs +* Port: humble to ros2 (`#386 `_) +* Merge branch 'humble' into mjcarroll/humble_to_ros2 +* Update maintainers (`#376 `_) +* Humble ➡️ ROS2 (`#323 `_) + Humble ➡️ ROS2 +* Merge branch 'humble' into ports/humble_to_ros2 +* 0.245.0 +* Changelog +* humble to ros2 (`#311 `_) + Co-authored-by: Michael Carroll +* Merge remote-tracking branch 'origin/humble' into ahcorde/humble_to_ros2 +* Contributors: Addisu Z. Taddese, Aditya Pande, Alejandro Hernández Cordero, Jose Luis Rivero, Michael Carroll, ahcorde + 1.0.0 (2024-04-24) ------------------ diff --git a/ros_gz/package.xml b/ros_gz/package.xml index 370af439d..d55e4d163 100644 --- a/ros_gz/package.xml +++ b/ros_gz/package.xml @@ -4,7 +4,7 @@ ros_gz - 1.0.0 + 1.0.1 Meta-package containing interfaces for using ROS 2 with Gazebo simulation. Aditya Pande Alejandro Hernandez diff --git a/ros_gz_bridge/CHANGELOG.rst b/ros_gz_bridge/CHANGELOG.rst index 78eaa2194..851a202cb 100644 --- a/ros_gz_bridge/CHANGELOG.rst +++ b/ros_gz_bridge/CHANGELOG.rst @@ -2,6 +2,136 @@ Changelog for package ros_gz_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Add support for gz.msgs.EntityWrench (base branch: ros2) (`#573 `_) +* Merge pull request `#571 `_ from azeey/jazzy_to_ros2 + Merge jazzy ➡️ ros2 +* Merge branch 'ros2' into jazzy_to_ros2 +* Use memcpy instead of std::copy when bridging images (`#565 `_) + While testing ros <-> gz communication using the bridge I noticed that the bridge was talking quite a bit of time copying images from Gazebo to ROS. I found that the std::copy operation that we're doing is substantially slower than the memcpy alternative. I think that in principle this shouldn't happen but the numbers are quite clear. Perhaps std::copy is doing something that doesn't use cache effectively + --------- + Co-authored-by: Jose Luis Rivero +* Merge jazzy into ros2 +* Merge pull request `#569 `_ from azeey/iron_to_jazzy + Merge iron ➡️ jazzy +* Merge iron into jazzy +* Add option to change material color from ROS. (`#521 `_) + Forward port of `#486 `_. + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. + (cherry picked from commit 78dc4823121f085594e6028a93f1e571eb04f58b) +* Merge pull request `#564 `_ from azeey/humble_to_iron + Humble ➡️ Iron +* Merge humble -> iron +* Use `ignoreLocalMessages` in the bridge (`#559 `_) + * Ignore local messages +* Update launch files with name parameter (`#556 `_) + * Name is required. +* Ensure the same container is used for the bridge and gz_server (`#553 `_) + This also adds a required `name` parameter for the bridge so that + multiple different bridges can be created without name collision +* Launch ros_gz_bridge from xml (`#550 `_) + * Add gzserver with ability to load an SDF file or string +* Launch gzserver and the bridge as composable nodes (`#528 `_) + * Add gzserver with ability to load an SDF file or string +* Add option to change material color from ROS. (`#521 `_) + Forward port of `#486 `_. + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. +* populate imu covariances when converting (`#375 `_) (`#540 `_) + Co-authored-by: El Jawad Alaa +* Prepare for 1.0.0 Release (`#495 `_) +* Use gz_vendor packages (`#531 `_) +* [backport Humble] Create bridge for GPSFix msg (`#316 `_) (`#538 `_) + Co-authored-by: Rousseau Vincent +* [backport Iron] Create bridge for GPSFix msg (`#316 `_) (`#537 `_) + Co-authored-by: Rousseau Vincent +* 0.244.14 +* Changelog +* Added conversion for Detection3D and Detection3DArray (`#523 `_) (`#526 `_) + Co-authored-by: wittenator <9154515+wittenator@users.noreply.github.com> +* Added conversion for Detection3D and Detection3DArray (`#523 `_) (`#525 `_) + Co-authored-by: wittenator <9154515+wittenator@users.noreply.github.com> +* [Backport rolling] Add ROS namespaces to GZ topics (`#517 `_) + Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> +* ign to gz (`#519 `_) +* Add ROS namespaces to GZ topics (`#512 `_) + Co-authored-by: Alejandro Hernández Cordero +* Correctly export ros_gz_bridge for downstream targets (`#503 `_) (`#506 `_) +* Add a virtual destructor to suppress compiler warning (`#502 `_) (`#505 `_) + Co-authored-by: Michael Carroll +* Correctly export ros_gz_bridge for downstream targets (`#503 `_) +* Add a virtual destructor to suppress compiler warning (`#502 `_) +* Add option to change material color from ROS. (`#486 `_) + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. + --------- + Co-authored-by: Alejandro Hernández Cordero + Co-authored-by: Addisu Z. Taddese + Co-authored-by: Addisu Z. Taddese +* 0.244.13 +* Changelog +* backport pr 374 (`#489 `_) +* populate imu covariances when converting (`#488 `_) +* 0.244.12 +* Changelog +* Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (`#468 `_) (`#470 `_) +* Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (`#468 `_) +* Added messages for 2D Bounding Boxes to ros_gz_bridge (`#458 `_) (`#466 `_) + Co-authored-by: Alejandro Hernandez Cordero +* populate imu covariances when converting (`#375 `_) +* 0.246.0 +* Update changelogs +* Add harmonic CI (`#447 `_) + * Add harmonic CI + * Include garden options + * Add harmonic stanza + * Additional message headers + --------- +* SensorNoise msg bridging (`#417 `_) +* Added Altimeter msg bridging (`#413 `_) +* Update README.md (`#411 `_) + The ROS type for gz.msgs.NavSat messages should be **sensor_msgs/msg/NavSatFix** instead of **sensor_msgs/msg/NavSatFixed** +* Add missing rosidl_cmake dep to ros_gz_bridge (`#391 `_) + Co-authored-by: Chris Lalancette +* allow converting from/to TwistWithCovarianceStamped (`#374 `_) + * allow converting from/to TwistWithCovarianceStamped + -------- + Co-authored-by: Alejandro Hernández Cordero +* Added doc (`#393 `_) +* Port: humble to ros2 (`#386 `_) +* Merge branch 'humble' into mjcarroll/humble_to_ros2 +* allow converting from/to PoseWithCovarianceStamped (`#381 `_) + * allow converting from/to PoseWithCovarianceStamped +* Add actuator_msgs to bridge. (`#378 `_) +* Update maintainers (`#376 `_) +* Fix warning message (`#371 `_) +* Improve error messages around config loading (`#356 `_) +* Bringing the Joy to gazebo. (`#350 `_) + Enable using the gazebo bridge with Joy. +* Fix double wait in ros_gz_bridge (`#347 `_) +* Create bridge for GPSFix msg (`#316 `_) +* Humble ➡️ ROS2 (`#323 `_) + Humble ➡️ ROS2 +* Merge branch 'humble' into ports/humble_to_ros2 +* 0.245.0 +* Changelog +* humble to ros2 (`#311 `_) + Co-authored-by: Michael Carroll +* Remove Humble+ deprecations (`#312 `_) + * Remove Humble+ deprecations +* Merge remote-tracking branch 'origin/humble' into ahcorde/humble_to_ros2 +* Remove all ignition references on ROS 2 branch (`#302 `_) + * Remove all shims + * Update CMakeLists and package.xml for garden + * Complete garden gz renaming + * Drop fortress CI +* Contributors: Addisu Z. Taddese, Aditya Pande, Alejandro Hernández Cordero, Arjun K Haridas, Benjamin Perseghetti, Carlos Agüero, El Jawad Alaa, Jose Luis Rivero, Krzysztof Wojciechowski, Michael Carroll, Rousseau Vincent, Victor T. Noppeney, Yadu, ahcorde, wittenator, ymd-stella + 1.0.0 (2024-04-24) ------------------ * Use gz_vendor packages (`#531 `_) diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 125acb5ef..44911577c 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -22,6 +22,9 @@ find_package(gz-transport REQUIRED) find_package(gz_msgs_vendor REQUIRED) find_package(gz-msgs REQUIRED) +# Install the python module for this package +ament_python_install_package(${PROJECT_NAME}) + set(GZ_MSGS_VERSION_FULL ${gz-msgs_VERSION}) set(BRIDGE_MESSAGE_TYPES @@ -133,6 +136,11 @@ install( DESTINATION include/${PROJECT_NAME} ) +install( + DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + set(bridge_executables parameter_bridge static_bridge diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 98792d92c..572892347 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -34,6 +34,7 @@ The following message types can be bridged for topics: | ros_gz_interfaces/msg/Contacts | gz.msgs.Contacts | | ros_gz_interfaces/msg/Dataframe | gz.msgs.Dataframe | | ros_gz_interfaces/msg/Entity | gz.msgs.Entity | +| ros_gz_interfaces/msg/EntityWrench | gz.msgs.EntityWrench | | ros_gz_interfaces/msg/Float32Array | gz.msgs.Float_V | | ros_gz_interfaces/msg/GuiCamera | gz.msgs.GUICamera | | ros_gz_interfaces/msg/JointWrench | gz.msgs.JointWrench | diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp index 426894a05..b49f02d7f 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp @@ -18,6 +18,7 @@ // Gazebo Msgs #include #include +#include #include #include #include @@ -25,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -36,6 +38,7 @@ // ROS 2 messages #include #include +#include #include #include #include @@ -43,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -93,6 +97,18 @@ convert_gz_to_ros( const gz::msgs::Entity & gz_msg, ros_gz_interfaces::msg::Entity & ros_msg); +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::EntityWrench & ros_msg, + gz::msgs::EntityWrench & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::EntityWrench & gz_msg, + ros_gz_interfaces::msg::EntityWrench & ros_msg); + template<> void convert_ros_to_gz( @@ -153,6 +169,18 @@ convert_gz_to_ros( const gz::msgs::Light & gz_msg, ros_gz_interfaces::msg::Light & ros_msg); +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg); + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch b/ros_gz_bridge/launch/ros_gz_bridge.launch new file mode 100644 index 000000000..929c72fe5 --- /dev/null +++ b/ros_gz_bridge/launch/ros_gz_bridge.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch.py b/ros_gz_bridge/launch/ros_gz_bridge.launch.py new file mode 100644 index 000000000..5ad46b9df --- /dev/null +++ b/ros_gz_bridge/launch/ros_gz_bridge.launch.py @@ -0,0 +1,114 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch ros_gz bridge in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + name = LaunchConfiguration('name') + config_file = LaunchConfiguration('config_file') + container_name = LaunchConfiguration('container_name') + namespace = LaunchConfiguration('namespace') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + declare_name_cmd = DeclareLaunchArgument( + 'name', description='Name of ros_gz_bridge node' + ) + + declare_config_file_cmd = DeclareLaunchArgument( + 'config_file', default_value='', description='YAML config file' + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='ros_gz_bridge', + executable='bridge_node', + name=name, + namespace=namespace, + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[{'config_file': config_file}], + arguments=['--ros-args', '--log-level', log_level], + ), + ], + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='ros_gz_bridge', + plugin='ros_gz_bridge::RosGzBridge', + name=name, + namespace=namespace, + parameters=[{'config_file': config_file}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_name_cmd) + ld.add_action(declare_config_file_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the bridge nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index fe4024489..bd06f26ba 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ ros_gz_bridge - 1.0.0 + 1.0.1 Bridge communication between ROS and Gazebo Transport Aditya Pande Alejandro Hernandez @@ -11,14 +11,18 @@ Shivesh Khaitan Louise Poubel + Carlos Agüero ament_cmake + ament_cmake_python pkg-config rosidl_pycommon actuator_msgs geometry_msgs gps_msgs + launch + launch_ros nav_msgs rclcpp rclcpp_components diff --git a/ros_gz_bridge/resource/ros_gz_bridge b/ros_gz_bridge/resource/ros_gz_bridge new file mode 100644 index 000000000..e69de29bb diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py index d1cf42897..f6ea05eed 100644 --- a/ros_gz_bridge/ros_gz_bridge/__init__.py +++ b/ros_gz_bridge/ros_gz_bridge/__init__.py @@ -20,6 +20,12 @@ from rosidl_pycommon import expand_template +from . import actions + +__all__ = [ + 'actions', +] + @dataclass class MessageMapping: @@ -41,15 +47,15 @@ def ign_string(self): return f'ignition.msgs.{self.gz_message_name}' def ign_type(self): - # Return GZ type of a message (eg ignition::msgs::Bool) - return f'ignition::msgs::{self.gz_message_name}' + # Return GZ type of a message (eg gz::msgs::Bool) + return f'gz::msgs::{self.gz_message_name}' def gz_string(self): # Return GZ string version of a message (eg ignition.msgs.Bool) return f'gz.msgs.{self.gz_message_name}' def gz_type(self): - # Return GZ type of a message (eg ignition::msgs::Bool) + # Return GZ type of a message (eg gz::msgs::Bool) return f'gz::msgs::{self.gz_message_name}' def unique(self): diff --git a/ros_gz_bridge/ros_gz_bridge/actions/__init__.py b/ros_gz_bridge/ros_gz_bridge/actions/__init__.py new file mode 100644 index 000000000..0a2c26657 --- /dev/null +++ b/ros_gz_bridge/ros_gz_bridge/actions/__init__.py @@ -0,0 +1,22 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Actions module.""" + +from .ros_gz_bridge import RosGzBridge + + +__all__ = [ + 'RosGzBridge', +] diff --git a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py new file mode 100644 index 000000000..9704f44e8 --- /dev/null +++ b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py @@ -0,0 +1,147 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for the ros_gz bridge action.""" + +from typing import List +from typing import Optional + +from launch.action import Action +from launch.actions import IncludeLaunchDescription +from launch.frontend import Entity, expose_action, Parser +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.some_substitutions_type import SomeSubstitutionsType +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +@expose_action('ros_gz_bridge') +class RosGzBridge(Action): + """Action that executes a ros_gz bridge ROS [composable] node.""" + + def __init__( + self, + *, + name: SomeSubstitutionsType, + config_file: Optional[SomeSubstitutionsType] = None, + container_name: Optional[SomeSubstitutionsType] = None, + namespace: Optional[SomeSubstitutionsType] = None, + use_composition: Optional[SomeSubstitutionsType] = None, + use_respawn: Optional[SomeSubstitutionsType] = None, + log_level: Optional[SomeSubstitutionsType] = None, + **kwargs + ) -> None: + """ + Construct a ros_gz bridge action. + + All arguments are forwarded to `ros_gz_bridge.launch.ros_gz_bridge.launch.py`, + so see the documentation of that class for further details. + + :param: name Name of ros_gz_bridge node + :param: config_file YAML config file. + :param: container_name Name of container that nodes will load in if use composition. + :param: namespace Top-level namespace. + :param: use_composition Use composed bringup if True. + :param: use_respawn Whether to respawn if a node crashes (when composition is disabled). + :param: log_level Log level. + """ + super().__init__(**kwargs) + self.__name = name + self.__config_file = config_file + self.__container_name = container_name + self.__namespace = namespace + self.__use_composition = use_composition + self.__use_respawn = use_respawn + self.__log_level = log_level + + @classmethod + def parse(cls, entity: Entity, parser: Parser): + """Parse ros_gz_bridge.""" + _, kwargs = super().parse(entity, parser) + + name = entity.get_attr( + 'name', data_type=str, + optional=False) + + config_file = entity.get_attr( + 'config_file', data_type=str, + optional=False) + + container_name = entity.get_attr( + 'container_name', data_type=str, + optional=True) + + namespace = entity.get_attr( + 'namespace', data_type=str, + optional=True) + + use_composition = entity.get_attr( + 'use_composition', data_type=str, + optional=True) + + use_respawn = entity.get_attr( + 'use_respawn', data_type=str, + optional=True) + + log_level = entity.get_attr( + 'log_level', data_type=str, + optional=True) + + if isinstance(name, str): + name = parser.parse_substitution(name) + kwargs['name'] = name + + if isinstance(config_file, str): + config_file = parser.parse_substitution(config_file) + kwargs['config_file'] = config_file + + if isinstance(container_name, str): + container_name = parser.parse_substitution(container_name) + kwargs['container_name'] = container_name + + if isinstance(namespace, str): + namespace = parser.parse_substitution(namespace) + kwargs['namespace'] = namespace + + if isinstance(use_composition, str): + use_composition = parser.parse_substitution(use_composition) + kwargs['use_composition'] = use_composition + + if isinstance(use_respawn, str): + use_respawn = parser.parse_substitution(use_respawn) + kwargs['use_respawn'] = use_respawn + + if isinstance(log_level, str): + log_level = parser.parse_substitution(log_level) + kwargs['log_level'] = log_level + + return cls, kwargs + + def execute(self, context: LaunchContext) -> Optional[List[Action]]: + """Execute the action.""" + ros_gz_bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('name', self.__name), + ('config_file', self.__config_file), + ('container_name', self.__container_name), + ('namespace', self.__namespace), + ('use_composition', self.__use_composition), + ('use_respawn', self.__use_respawn), + ('log_level', self.__log_level), ]) + + return [ros_gz_bridge_description] diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 808ee5c6c..203e33d1a 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -64,10 +64,12 @@ Mapping('Contacts', 'Contacts'), Mapping('Dataframe', 'Dataframe'), Mapping('Entity', 'Entity'), + Mapping('EntityWrench', 'EntityWrench'), Mapping('Float32Array', 'Float_V'), Mapping('GuiCamera', 'GUICamera'), Mapping('JointWrench', 'JointWrench'), Mapping('Light', 'Light'), + Mapping('MaterialColor', 'MaterialColor'), Mapping('ParamVec', 'Param'), Mapping('ParamVec', 'Param_V'), Mapping('SensorNoise', 'SensorNoise'), diff --git a/ros_gz_bridge/setup.cfg b/ros_gz_bridge/setup.cfg new file mode 100644 index 000000000..1f7078137 --- /dev/null +++ b/ros_gz_bridge/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +launch.frontend.launch_extension = + ros_gz_bridge = ros_gz_bridge diff --git a/ros_gz_bridge/src/convert/gps_msgs.cpp b/ros_gz_bridge/src/convert/gps_msgs.cpp index de8062394..cf2774d66 100644 --- a/ros_gz_bridge/src/convert/gps_msgs.cpp +++ b/ros_gz_bridge/src/convert/gps_msgs.cpp @@ -55,7 +55,7 @@ convert_gz_to_ros( ros_msg.track = atan2(gz_msg.velocity_north(), gz_msg.velocity_east()); ros_msg.climb = gz_msg.velocity_up(); - // position_covariance is not supported in Ignition::Msgs::NavSat. + // position_covariance is not supported in gz::msgs::NavSat. ros_msg.position_covariance_type = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN; ros_msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; } diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 8da4225c1..eb923b344 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -139,6 +139,28 @@ convert_gz_to_ros( } } +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::EntityWrench & ros_msg, + gz::msgs::EntityWrench & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + convert_ros_to_gz(ros_msg.entity, (*gz_msg.mutable_entity())); + convert_ros_to_gz(ros_msg.wrench, (*gz_msg.mutable_wrench())); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::EntityWrench & gz_msg, + ros_gz_interfaces::msg::EntityWrench & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + convert_gz_to_ros(gz_msg.entity(), ros_msg.entity); + convert_gz_to_ros(gz_msg.wrench(), ros_msg.wrench); +} + template<> void convert_ros_to_gz( @@ -378,6 +400,64 @@ convert_gz_to_ros( ros_msg.intensity = gz_msg.intensity(); } +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg) +{ + using EntityMatch = gz::msgs::MaterialColor::EntityMatch; + + switch (ros_msg.entity_match) { + case ros_gz_interfaces::msg::MaterialColor::FIRST: + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_FIRST); + break; + case ros_gz_interfaces::msg::MaterialColor::ALL: + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_ALL); + break; + default: + std::cerr << "Unsupported entity match type [" + << ros_msg.entity_match << "]\n"; + } + + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + convert_ros_to_gz(ros_msg.entity, *gz_msg.mutable_entity()); + convert_ros_to_gz(ros_msg.ambient, *gz_msg.mutable_ambient()); + convert_ros_to_gz(ros_msg.diffuse, *gz_msg.mutable_diffuse()); + convert_ros_to_gz(ros_msg.specular, *gz_msg.mutable_specular()); + convert_ros_to_gz(ros_msg.emissive, *gz_msg.mutable_emissive()); + + gz_msg.set_shininess(ros_msg.shininess); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg) +{ + using EntityMatch = gz::msgs::MaterialColor::EntityMatch; + if (gz_msg.entity_match() == EntityMatch::MaterialColor_EntityMatch_FIRST) { + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; +/* *INDENT-OFF* */ + } else if (gz_msg.entity_match() == + EntityMatch::MaterialColor_EntityMatch_ALL) { +/* *INDENT-ON* */ + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; + } else { + std::cerr << "Unsupported EntityMatch [" << + gz_msg.entity_match() << "]" << std::endl; + } + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + convert_gz_to_ros(gz_msg.entity(), ros_msg.entity); + convert_gz_to_ros(gz_msg.ambient(), ros_msg.ambient); + convert_gz_to_ros(gz_msg.diffuse(), ros_msg.diffuse); + convert_gz_to_ros(gz_msg.specular(), ros_msg.specular); + convert_gz_to_ros(gz_msg.emissive(), ros_msg.emissive); + + ros_msg.shininess = gz_msg.shininess(); +} + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/src/convert/sensor_msgs.cpp b/ros_gz_bridge/src/convert/sensor_msgs.cpp index 887f16d0f..f537f9087 100644 --- a/ros_gz_bridge/src/convert/sensor_msgs.cpp +++ b/ros_gz_bridge/src/convert/sensor_msgs.cpp @@ -166,13 +166,11 @@ convert_gz_to_ros( ros_msg.is_bigendian = false; ros_msg.step = ros_msg.width * num_channels * octets_per_channel; - - auto count = ros_msg.step * ros_msg.height; ros_msg.data.resize(ros_msg.step * ros_msg.height); - std::copy( - gz_msg.data().begin(), - gz_msg.data().begin() + count, - ros_msg.data.begin()); + + // Prefer memcpy over std::copy for performance reasons, + // see https://github.com/gazebosim/ros_gz/pull/565 + memcpy(ros_msg.data.data(), gz_msg.data().c_str(), gz_msg.data().size()); } template<> @@ -520,7 +518,7 @@ convert_gz_to_ros( ros_msg.longitude = gz_msg.longitude_deg(); ros_msg.altitude = gz_msg.altitude(); - // position_covariance is not supported in Ignition::Msgs::NavSat. + // position_covariance is not supported in gz::msgs::NavSat. ros_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; ros_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; } diff --git a/ros_gz_bridge/src/factory.hpp b/ros_gz_bridge/src/factory.hpp index ec00af7b6..1ee9ded7d 100644 --- a/ros_gz_bridge/src/factory.hpp +++ b/ros_gz_bridge/src/factory.hpp @@ -20,6 +20,7 @@ #include #include +#include // include ROS 2 #include @@ -105,17 +106,16 @@ class Factory : public FactoryInterface size_t /*queue_size*/, rclcpp::PublisherBase::SharedPtr ros_pub) { - std::function subCb = - [this, ros_pub](const GZ_T & _msg, const gz::transport::MessageInfo & _info) + std::function subCb = + [this, ros_pub](const GZ_T & _msg) { - // Ignore messages that are published from this bridge. - if (!_info.IntraProcess()) { - this->gz_callback(_msg, ros_pub); - } + this->gz_callback(_msg, ros_pub); }; - node->Subscribe(topic_name, subCb); + // Ignore messages that are published from this bridge. + gz::transport::SubscribeOptions opts; + opts.SetIgnoreLocalMessages(true); + node->Subscribe(topic_name, subCb, opts); } protected: diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index f1a0b0ff7..b13a06b1c 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -288,7 +288,9 @@ void compareTestMsg(const std::shared_ptr & _msg) gz::msgs::SensorNoise expected_msg; createTestMsg(expected_msg); - EXPECT_EQ(expected_msg.type(), gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED); + EXPECT_EQ( + expected_msg.type(), + gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED); EXPECT_EQ(expected_msg.mean(), _msg->mean()); EXPECT_EQ(expected_msg.stddev(), _msg->stddev()); EXPECT_EQ(expected_msg.bias_mean(), _msg->bias_mean()); @@ -524,6 +526,30 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_EQ(expected_msg.type(), _msg->type()); } +void createTestMsg(gz::msgs::EntityWrench & _msg) +{ + gz::msgs::Header header_msg; + gz::msgs::Entity entity_msg; + gz::msgs::Wrench wrench_msg; + + createTestMsg(header_msg); + createTestMsg(entity_msg); + createTestMsg(wrench_msg); + + _msg.mutable_header()->CopyFrom(header_msg); + _msg.mutable_entity()->CopyFrom(entity_msg); + _msg.mutable_wrench()->CopyFrom(wrench_msg); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gz::msgs::EntityWrench expected_msg; + createTestMsg(expected_msg); + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->entity())); + compareTestMsg(std::make_shared(_msg->wrench())); +} + void createTestMsg(gz::msgs::Contact & _msg) { gz::msgs::Entity collision1; @@ -1344,6 +1370,35 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity()); } +void createTestMsg(gz::msgs::MaterialColor & _msg) +{ + createTestMsg(*_msg.mutable_header()); + createTestMsg(*_msg.mutable_entity()); + createTestMsg(*_msg.mutable_ambient()); + createTestMsg(*_msg.mutable_diffuse()); + createTestMsg(*_msg.mutable_specular()); + createTestMsg(*_msg.mutable_emissive()); + + _msg.set_shininess(1.0); + _msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gz::msgs::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->entity())); + compareTestMsg(std::make_shared(_msg->ambient())); + compareTestMsg(std::make_shared(_msg->diffuse())); + compareTestMsg(std::make_shared(_msg->specular())); + compareTestMsg(std::make_shared(_msg->emissive())); + + EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); + EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match()); +} + void createTestMsg(gz::msgs::GUICamera & _msg) { gz::msgs::Header header_msg; diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 3fd7d6235..cff1a0af8 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -291,6 +293,14 @@ void createTestMsg(gz::msgs::Entity & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gz::msgs::EntityWrench & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(gz::msgs::Contact & _msg); @@ -451,6 +461,14 @@ void createTestMsg(gz::msgs::Light & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gz::msgs::MaterialColor & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(gz::msgs::GUICamera & _msg); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 90b90bddf..42f9b90b7 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -622,6 +622,34 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity); } +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.entity); + createTestMsg(_msg.ambient); + createTestMsg(_msg.diffuse); + createTestMsg(_msg.specular); + createTestMsg(_msg.emissive); + _msg.shininess = 1.0; + _msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + compareTestMsg(std::make_shared(_msg->entity)); + compareTestMsg(std::make_shared(_msg->ambient)); + compareTestMsg(std::make_shared(_msg->diffuse)); + compareTestMsg(std::make_shared(_msg->specular)); + compareTestMsg(std::make_shared(_msg->emissive)); + + EXPECT_EQ(expected_msg.shininess, _msg->shininess); + EXPECT_EQ(expected_msg.entity_match, _msg->entity_match); +} + void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) { createTestMsg(_msg.header); @@ -839,6 +867,23 @@ void compareTestMsg(const std::shared_ptr & _msg EXPECT_EQ(expected_msg.type, _msg->type); } +void createTestMsg(ros_gz_interfaces::msg::EntityWrench & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.entity); + createTestMsg(_msg.wrench); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::EntityWrench expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header)); + compareTestMsg(std::make_shared(_msg->entity)); + compareTestMsg(std::make_shared(_msg->wrench)); +} + void createTestMsg(ros_gz_interfaces::msg::Contact & _msg) { createTestMsg(_msg.collision1); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index eb42d2fb8..682113055 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -408,6 +410,14 @@ void createTestMsg(ros_gz_interfaces::msg::Light & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::Entity & _msg); @@ -416,6 +426,14 @@ void createTestMsg(ros_gz_interfaces::msg::Entity & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ros_gz_interfaces::msg::EntityWrench & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::Contact & _msg); diff --git a/ros_gz_image/CHANGELOG.rst b/ros_gz_image/CHANGELOG.rst index 8ed7016e8..10997b1df 100644 --- a/ros_gz_image/CHANGELOG.rst +++ b/ros_gz_image/CHANGELOG.rst @@ -2,6 +2,52 @@ Changelog for package ros1_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Merge pull request `#571 `_ from azeey/jazzy_to_ros2 + Merge jazzy ➡️ ros2 +* Merge jazzy into ros2 +* Merge pull request `#569 `_ from azeey/iron_to_jazzy + Merge iron ➡️ jazzy +* Merge iron into jazzy +* Prepare for 1.0.0 Release (`#495 `_) +* Use gz_vendor packages (`#531 `_) +* 0.244.14 +* Changelog +* ign to gz (`#519 `_) +* 0.244.13 +* Changelog +* 0.244.12 +* Changelog +* 0.246.0 +* Update changelogs +* Add harmonic CI (`#447 `_) + * Add harmonic CI + * Include garden options + * Add harmonic stanza + * Additional message headers + --------- +* Port: humble to ros2 (`#386 `_) +* Merge branch 'humble' into mjcarroll/humble_to_ros2 +* Update maintainers (`#376 `_) +* Fix linter error by reordering headers (`#373 `_) +* Add QoS profile parameter to image bridge (`#335 `_) +* Fix double wait in ros_gz_bridge (`#347 `_) +* Humble ➡️ ROS2 (`#323 `_) + Humble ➡️ ROS2 +* Merge branch 'humble' into ports/humble_to_ros2 +* 0.245.0 +* Changelog +* humble to ros2 (`#311 `_) + Co-authored-by: Michael Carroll +* Merge remote-tracking branch 'origin/humble' into ahcorde/humble_to_ros2 +* Remove all ignition references on ROS 2 branch (`#302 `_) + * Remove all shims + * Update CMakeLists and package.xml for garden + * Complete garden gz renaming + * Drop fortress CI +* Contributors: Addisu Z. Taddese, Aditya Pande, Alejandro Hernández Cordero, Jose Luis Rivero, Michael Carroll, Sebastian Castro, ahcorde, ymd-stella + 1.0.0 (2024-04-24) ------------------ * Use gz_vendor packages (`#531 `_) diff --git a/ros_gz_image/package.xml b/ros_gz_image/package.xml index b6ca00481..1ea41b206 100644 --- a/ros_gz_image/package.xml +++ b/ros_gz_image/package.xml @@ -1,6 +1,6 @@ ros_gz_image - 1.0.0 + 1.0.1 Image utilities for Gazebo simulation with ROS. Apache 2.0 Aditya Pande diff --git a/ros_gz_image/test/publishers/gz_publisher.cpp b/ros_gz_image/test/publishers/gz_publisher.cpp index 24c745b8b..6bf9aa7f2 100644 --- a/ros_gz_image/test/publishers/gz_publisher.cpp +++ b/ros_gz_image/test/publishers/gz_publisher.cpp @@ -45,7 +45,7 @@ int main(int /*argc*/, char **/*argv*/) gz::transport::Node node; // gz::msgs::Image. - auto image_pub = node.Advertise("image"); + auto image_pub = node.Advertise("image"); gz::msgs::Image image_msg; ros_gz_image::testing::createTestMsg(image_msg); diff --git a/ros_gz_image/test/test_utils.h b/ros_gz_image/test/test_utils.h index 02996f3e9..f6509eec1 100644 --- a/ros_gz_image/test/test_utils.h +++ b/ros_gz_image/test/test_utils.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include namespace ros_gz_image { @@ -142,7 +142,7 @@ namespace testing /// \brief Create a message used for testing. /// \param[out] _msg The message populated. - void createTestMsg(ignition::msgs::Header &_msg) + void createTestMsg(gz::msgs::Header &_msg) { auto seq_entry = _msg.add_data(); seq_entry->set_key("seq"); @@ -156,9 +156,9 @@ namespace testing /// \brief Compare a message with the populated for testing. /// \param[in] _msg The message to compare. - void compareTestMsg(const ignition::msgs::Header &_msg) + void compareTestMsg(const gz::msgs::Header &_msg) { - ignition::msgs::Header expected_msg; + gz::msgs::Header expected_msg; createTestMsg(expected_msg); EXPECT_EQ(expected_msg.stamp().sec(), _msg.stamp().sec()); @@ -183,24 +183,24 @@ namespace testing /// \brief Create a message used for testing. /// \param[out] _msg The message populated. - void createTestMsg(ignition::msgs::Image &_msg) + void createTestMsg(gz::msgs::Image &_msg) { - ignition::msgs::Header header_msg; + gz::msgs::Header header_msg; createTestMsg(header_msg); _msg.mutable_header()->CopyFrom(header_msg); _msg.set_width(320); _msg.set_height(240); - _msg.set_pixel_format_type(ignition::msgs::PixelFormatType::RGB_INT8); + _msg.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8); _msg.set_step(_msg.width() * 3); _msg.set_data(std::string(_msg.height() * _msg.step(), '1')); } /// \brief Compare a message with the populated for testing. /// \param[in] _msg The message to compare. - void compareTestMsg(const ignition::msgs::Image &_msg) + void compareTestMsg(const gz::msgs::Image &_msg) { - ignition::msgs::Image expected_msg; + gz::msgs::Image expected_msg; createTestMsg(expected_msg); compareTestMsg(_msg.header()); diff --git a/ros_gz_interfaces/CHANGELOG.rst b/ros_gz_interfaces/CHANGELOG.rst index d91d74698..3d35db530 100644 --- a/ros_gz_interfaces/CHANGELOG.rst +++ b/ros_gz_interfaces/CHANGELOG.rst @@ -2,6 +2,53 @@ Changelog for package ros_gz_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Add support for gz.msgs.EntityWrench (base branch: ros2) (`#573 `_) +* Add option to change material color from ROS. (`#521 `_) + Forward port of `#486 `_. + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. + (cherry picked from commit 78dc4823121f085594e6028a93f1e571eb04f58b) +* Add option to change material color from ROS. (`#521 `_) + Forward port of `#486 `_. + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. +* Prepare for 1.0.0 Release (`#495 `_) +* 0.244.14 +* Changelog +* Add option to change material color from ROS. (`#486 `_) + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. + --------- + Co-authored-by: Alejandro Hernández Cordero + Co-authored-by: Addisu Z. Taddese + Co-authored-by: Addisu Z. Taddese +* 0.244.13 +* Changelog +* 0.244.12 +* Changelog +* 0.246.0 +* Update changelogs +* SensorNoise msg bridging (`#417 `_) +* Added Altimeter msg bridging (`#413 `_) +* Port: humble to ros2 (`#386 `_) +* Merge branch 'humble' into mjcarroll/humble_to_ros2 +* Update maintainers (`#376 `_) +* Humble ➡️ ROS2 (`#323 `_) + Humble ➡️ ROS2 +* Merge branch 'humble' into ports/humble_to_ros2 +* Export rcl_interfaces exec dependency (`#317 `_) +* 0.245.0 +* Changelog +* humble to ros2 (`#311 `_) + Co-authored-by: Michael Carroll +* Merge remote-tracking branch 'origin/humble' into ahcorde/humble_to_ros2 +* Contributors: Addisu Z. Taddese, Aditya Pande, Alejandro Hernández Cordero, Benjamin Perseghetti, Jose Luis Rivero, Michael Carroll, Victor T. Noppeney, ahcorde + 1.0.0 (2024-04-24) ------------------ diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt index 49c49126f..9f3d7a1fb 100644 --- a/ros_gz_interfaces/CMakeLists.txt +++ b/ros_gz_interfaces/CMakeLists.txt @@ -23,10 +23,12 @@ set(msg_files "msg/Dataframe.msg" "msg/Entity.msg" "msg/EntityFactory.msg" + "msg/EntityWrench.msg" "msg/Float32Array.msg" "msg/GuiCamera.msg" "msg/JointWrench.msg" "msg/Light.msg" + "msg/MaterialColor.msg" "msg/ParamVec.msg" "msg/SensorNoise.msg" "msg/StringVec.msg" diff --git a/ros_gz_interfaces/README.md b/ros_gz_interfaces/README.md index 88ed27d76..610cb7e34 100644 --- a/ros_gz_interfaces/README.md +++ b/ros_gz_interfaces/README.md @@ -8,6 +8,7 @@ This package currently contains some Gazebo-specific ROS message and service dat * [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. * [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. * [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. +* [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. * [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. * [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. * [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. diff --git a/ros_gz_interfaces/msg/EntityWrench.msg b/ros_gz_interfaces/msg/EntityWrench.msg new file mode 100644 index 000000000..c1a039875 --- /dev/null +++ b/ros_gz_interfaces/msg/EntityWrench.msg @@ -0,0 +1,3 @@ +std_msgs/Header header # Time stamp +ros_gz_interfaces/Entity entity # Entity +geometry_msgs/Wrench wrench # Wrench to be applied to entity \ No newline at end of file diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg new file mode 100644 index 000000000..cda67d27a --- /dev/null +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -0,0 +1,12 @@ +# Entities that match to apply material color: constant definition +uint8 FIRST = 0 +uint8 ALL = 1 + +std_msgs/Header header # Optional header data +ros_gz_interfaces/Entity entity # Entity to change material color +std_msgs/ColorRGBA ambient # Ambient color +std_msgs/ColorRGBA diffuse # Diffuse color +std_msgs/ColorRGBA specular # Specular color +std_msgs/ColorRGBA emissive # Emissive color +float64 shininess # Specular exponent +uint8 entity_match # Entities that match to apply material color diff --git a/ros_gz_interfaces/package.xml b/ros_gz_interfaces/package.xml index f236eaa40..33a6db4d9 100644 --- a/ros_gz_interfaces/package.xml +++ b/ros_gz_interfaces/package.xml @@ -1,6 +1,6 @@ ros_gz_interfaces - 1.0.0 + 1.0.1 Message and service data structures for interacting with Gazebo from ROS2. Apache 2.0 Louise Poubel diff --git a/ros_gz_point_cloud/src/point_cloud.cc b/ros_gz_point_cloud/src/point_cloud.cc index 66a0e5974..7b0820aa5 100644 --- a/ros_gz_point_cloud/src/point_cloud.cc +++ b/ros_gz_point_cloud/src/point_cloud.cc @@ -13,19 +13,19 @@ // limitations under the License. #include "point_cloud.hh" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -35,7 +35,7 @@ IGNITION_ADD_PLUGIN( ros_gz_point_cloud::PointCloud, - ignition::gazebo::System, + gz::sim::System, ros_gz_point_cloud::PointCloud::ISystemConfigure, ros_gz_point_cloud::PointCloud::ISystemPostUpdate) @@ -76,49 +76,49 @@ class ros_gz_point_cloud::PointCloudPrivate /// \param[in] _ecm Immutable reference to ECM. public: - void LoadDepthCamera(const ignition::gazebo::EntityComponentManager & _ecm); + void LoadDepthCamera(const gz::sim::EntityComponentManager & _ecm); /// \brief Get RGB camera from rendering. /// \param[in] _ecm Immutable reference to ECM. public: - void LoadRgbCamera(const ignition::gazebo::EntityComponentManager & _ecm); + void LoadRgbCamera(const gz::sim::EntityComponentManager & _ecm); /// \brief Get GPU rays from rendering. /// \param[in] _ecm Immutable reference to ECM. public: - void LoadGpuRays(const ignition::gazebo::EntityComponentManager & _ecm); + void LoadGpuRays(const gz::sim::EntityComponentManager & _ecm); /// \brief Rendering scene which manages the cameras. public: - ignition::rendering::ScenePtr scene_; + gz::rendering::ScenePtr scene_; /// \brief Entity ID for sensor within Gazebo. public: - ignition::gazebo::Entity entity_; + gz::sim::Entity entity_; /// \brief Rendering depth camera public: - std::shared_ptr < ignition::rendering::DepthCamera > depth_camera_; + std::shared_ptr < gz::rendering::DepthCamera > depth_camera_; /// \brief Rendering RGB camera public: - std::shared_ptr < ignition::rendering::Camera > rgb_camera_; + std::shared_ptr < gz::rendering::Camera > rgb_camera_; /// \brief Rendering GPU lidar public: - std::shared_ptr < ignition::rendering::GpuRays > gpu_rays_; + std::shared_ptr < gz::rendering::GpuRays > gpu_rays_; /// \brief Keep latest image from RGB camera. public: - ignition::rendering::Image rgb_image_; + gz::rendering::Image rgb_image_; /// \brief Message populated with latest image from RGB camera. @@ -128,12 +128,12 @@ class ros_gz_point_cloud::PointCloudPrivate /// \brief Connection to depth frame event. public: - ignition::common::ConnectionPtr depth_connection_; + gz::common::ConnectionPtr depth_connection_; /// \brief Connection to GPU rays frame event. public: - ignition::common::ConnectionPtr gpu_rays_connection_; + gz::common::ConnectionPtr gpu_rays_connection_; /// \brief Node to publish ROS messages. @@ -179,18 +179,18 @@ PointCloud::PointCloud() ////////////////////////////////////////////////// void PointCloud::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr < const sdf::Element > & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { this->dataPtr->entity_ = _entity; - if (_ecm.Component < ignition::gazebo::components::RgbdCamera > (_entity) != nullptr) { + if (_ecm.Component < gz::sim::components::RgbdCamera > (_entity) != nullptr) { this->dataPtr->type_ = SensorType::RGBD_CAMERA; - } else if (_ecm.Component < ignition::gazebo::components::DepthCamera > (_entity) != nullptr) { + } else if (_ecm.Component < gz::sim::components::DepthCamera > (_entity) != nullptr) { this->dataPtr->type_ = SensorType::DEPTH_CAMERA; - } else if (_ecm.Component < ignition::gazebo::components::GpuLidar > (_entity) != nullptr) { + } else if (_ecm.Component < gz::sim::components::GpuLidar > (_entity) != nullptr) { this->dataPtr->type_ = SensorType::GPU_LIDAR; } else { ROS_ERROR_NAMED( @@ -208,7 +208,7 @@ void PointCloud::Configure( } // Sensor scoped name - auto scoped_name = ignition::gazebo::scopedName(this->dataPtr->entity_, _ecm, "/", false); + auto scoped_name = gz::sim::scopedName(this->dataPtr->entity_, _ecm, "/", false); // ROS node auto ns = _sdf->Get < std::string > ("namespace", scoped_name).first; @@ -229,14 +229,14 @@ void PointCloud::Configure( ////////////////////////////////////////////////// void PointCloud::PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) { this->dataPtr->current_time_ = _info.simTime; // Find engine / scene if (!this->dataPtr->scene_) { - auto engine = ignition::rendering::engine(this->dataPtr->engine_name_); + auto engine = gz::rendering::engine(this->dataPtr->engine_name_); if (!engine) { return; } @@ -268,11 +268,11 @@ void PointCloud::PostUpdate( ////////////////////////////////////////////////// void PointCloudPrivate::LoadDepthCamera( - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::EntityComponentManager & _ecm) { // Sensor name scoped from the model auto sensor_name = - ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + gz::sim::scopedName(this->entity_, _ecm, "::", false); sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor @@ -285,7 +285,7 @@ void PointCloudPrivate::LoadDepthCamera( } this->depth_camera_ = - std::dynamic_pointer_cast < ignition::rendering::DepthCamera > (sensor); + std::dynamic_pointer_cast < gz::rendering::DepthCamera > (sensor); if (!this->depth_camera_) { ROS_ERROR_NAMED( "ros_gz_point_cloud", @@ -302,11 +302,11 @@ void PointCloudPrivate::LoadDepthCamera( ////////////////////////////////////////////////// void PointCloudPrivate::LoadRgbCamera( - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::EntityComponentManager & _ecm) { // Sensor name scoped from the model auto sensor_name = - ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + gz::sim::scopedName(this->entity_, _ecm, "::", false); sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor @@ -315,7 +315,7 @@ void PointCloudPrivate::LoadRgbCamera( return; } - this->rgb_camera_ = std::dynamic_pointer_cast < ignition::rendering::Camera > (sensor); + this->rgb_camera_ = std::dynamic_pointer_cast < gz::rendering::Camera > (sensor); if (!this->rgb_camera_) { ROS_ERROR_NAMED( "ros_gz_point_cloud", @@ -328,11 +328,11 @@ void PointCloudPrivate::LoadRgbCamera( ////////////////////////////////////////////////// void PointCloudPrivate::LoadGpuRays( - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::EntityComponentManager & _ecm) { // Sensor name scoped from the model auto sensor_name = - ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + gz::sim::scopedName(this->entity_, _ecm, "::", false); sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor @@ -342,7 +342,7 @@ void PointCloudPrivate::LoadGpuRays( } this->gpu_rays_ = - std::dynamic_pointer_cast < ignition::rendering::GpuRays > (sensor); + std::dynamic_pointer_cast < gz::rendering::GpuRays > (sensor); if (!this->gpu_rays_) { ROS_ERROR_NAMED( "ros_gz_point_cloud", @@ -395,7 +395,7 @@ void PointCloudPrivate::OnNewDepthFrame( // Fill message // Logic borrowed from // https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_depth_camera.cpp - auto sec_nsec = ignition::math::durationToSecNsec(this->current_time_); + auto sec_nsec = gz::math::durationToSecNsec(this->current_time_); sensor_msgs::PointCloud2 msg; msg.header.frame_id = this->frame_id_; @@ -486,11 +486,11 @@ void PointCloudPrivate::OnNewDepthFrame( // Clamp according to REP 117 if (depth > this->depth_camera_->FarClipPlane()) { - *iter_z = ignition::math::INF_D; + *iter_z = gz::math::INF_D; msg.is_dense = false; } if (depth < this->depth_camera_->NearClipPlane()) { - *iter_z = -ignition::math::INF_D; + *iter_z = -gz::math::INF_D; msg.is_dense = false; } } else if (nullptr != this->gpu_rays_) { diff --git a/ros_gz_point_cloud/src/point_cloud.hh b/ros_gz_point_cloud/src/point_cloud.hh index 437efac9c..d8a46d463 100644 --- a/ros_gz_point_cloud/src/point_cloud.hh +++ b/ros_gz_point_cloud/src/point_cloud.hh @@ -16,7 +16,7 @@ #define ROS_GZ_POINTCLOUD__POINTCLOUD_HPP_ #include -#include +#include namespace ros_gz_point_cloud { @@ -36,9 +36,9 @@ namespace ros_gz_point_cloud /// * ``: Render engine name, defaults to 'ogre2' /// * ``: Scene name, defaults to 'scene' class PointCloud: - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPostUpdate + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { /// \brief Constructor @@ -54,17 +54,17 @@ public: public: void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr < const sdf::Element > & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited public: void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; /// \brief Private data pointer. diff --git a/ros_gz_sim/CHANGELOG.rst b/ros_gz_sim/CHANGELOG.rst index acbb95df2..7aef01615 100644 --- a/ros_gz_sim/CHANGELOG.rst +++ b/ros_gz_sim/CHANGELOG.rst @@ -2,6 +2,90 @@ Changelog for package ros_gz_sim ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Merge pull request `#571 `_ from azeey/jazzy_to_ros2 + Merge jazzy ➡️ ros2 +* Merge jazzy into ros2 +* Merge pull request `#569 `_ from azeey/iron_to_jazzy + Merge iron ➡️ jazzy +* Merge remote-tracking branch 'origin/jazzy' into iron_to_jazzy +* Add a ROS node that runs Gazebo (`#500 `_) (`#567 `_) + * Add gzserver with ability to load an SDF file or string + --------- + (cherry picked from commit 92a2891f4adf35e4a4119aca2447dee93e22a06a) + Co-authored-by: Addisu Z. Taddese +* Merge iron into jazzy +* Merge pull request `#564 `_ from azeey/humble_to_iron + Humble ➡️ Iron +* Merge humble -> iron +* Update launch files with name parameter (`#556 `_) + * Name is required. +* Launch gz_spawn_model from xml (`#551 `_) + Spawn models from XML. + Co-authored-by: Addisu Z. Taddese +* Launch ros_gz_bridge from xml (`#550 `_) + * Add gzserver with ability to load an SDF file or string +* Launch gzserver and the bridge as composable nodes (`#528 `_) + * Add gzserver with ability to load an SDF file or string +* Add a ROS node that runs Gazebo (`#500 `_) + * Add gzserver with ability to load an SDF file or string + --------- +* Prepare for 1.0.0 Release (`#495 `_) +* Use gz_vendor packages (`#531 `_) +* 0.244.14 +* Changelog +* ign to gz (`#519 `_) +* Support `` in `package.xml` exports (`#492 `_) + This copies the implementation from `gazebo_ros_paths.py` to provide a + way for packages to set resource paths from `package.xml`. + ``` + e.g. + + + + ``` + The value of `gazebo_model_path` and `gazebo_media_path` is appended to `GZ_SIM_RESOURCE_PATH` + The value of `plugin_path` appended to `GZ_SIM_SYSTEM_PLUGIN_PATH` + --------- +* Undeprecate use of commandline flags (`#491 `_) +* 0.244.13 +* Changelog +* Remove deprecations using ros_gz_sim_create (`#476 `_) +* Added support for using ROS 2 parameters to spawn entities in Gazebo using ros_gz_sim::create (`#475 `_) +* Fix bug in `create` where command line arguments were truncated (`#472 `_) +* 0.244.12 +* Changelog +* Filter ROS arguments before gflags parsing (`#453 `_) +* 0.246.0 +* Update changelogs +* Add harmonic CI (`#447 `_) + * Add harmonic CI + * Include garden options + * Add harmonic stanza + * Additional message headers + --------- +* Replace deprecated ign_find_package with gz_find_package (`#432 `_) + Co-authored-by: jmackay2 +* Port: humble to ros2 (`#386 `_) +* Merge branch 'humble' into mjcarroll/humble_to_ros2 +* Update maintainers (`#376 `_) +* set on_exit_shutdown argument for gz-sim ExecuteProcess (`#355 `_) +* Humble ➡️ ROS2 (`#323 `_) + Humble ➡️ ROS2 +* Merge branch 'humble' into ports/humble_to_ros2 +* 0.245.0 +* Changelog +* humble to ros2 (`#311 `_) + Co-authored-by: Michael Carroll +* Merge remote-tracking branch 'origin/humble' into ahcorde/humble_to_ros2 +* Remove all ignition references on ROS 2 branch (`#302 `_) + * Remove all shims + * Update CMakeLists and package.xml for garden + * Complete garden gz renaming + * Drop fortress CI +* Contributors: Addisu Z. Taddese, Aditya Pande, Alejandro Hernández Cordero, Ayush Singh, Carlos Agüero, Jose Luis Rivero, Michael Carroll, ahcorde, andermi, jmackay2, mergify[bot] + 1.0.0 (2024-04-24) ------------------ * Use gz_vendor packages (`#531 `_) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 1cf5b4cce..dc21f8a9b 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(gz_math_vendor REQUIRED) @@ -33,6 +34,9 @@ gz_find_package(gflags PKGCONFIG gflags) find_package(std_msgs REQUIRED) +# Install the python module for this package +ament_python_install_package(${PROJECT_NAME}) + add_executable(create src/create.cpp) ament_target_dependencies(create rclcpp @@ -60,6 +64,23 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) +add_library(gzserver_component SHARED src/gzserver.cpp) +rclcpp_components_register_nodes(gzserver_component "ros_gz_sim::GzServer") +ament_target_dependencies(gzserver_component + rclcpp + rclcpp_components + std_msgs +) +target_link_libraries(gzserver_component + gz-sim::core +) +ament_target_dependencies(gzserver_component std_msgs) +rclcpp_components_register_node( + gzserver_component + PLUGIN "ros_gz_sim::GzServer" + EXECUTABLE gzserver +) + configure_file( launch/gz_sim.launch.py.in launch/gz_sim.launch.py.configured @@ -75,10 +96,33 @@ install(FILES DESTINATION share/${PROJECT_NAME}/launch ) +install(FILES + "launch/gz_server.launch" + "launch/gz_server.launch.py" + "launch/gz_spawn_model.launch" + "launch/gz_spawn_model.launch.py" + "launch/ros_gz_sim.launch" + "launch/ros_gz_sim.launch.py" + "launch/ros_gz_spawn_model.launch.py" + DESTINATION share/${PROJECT_NAME}/launch +) + install(TARGETS create DESTINATION lib/${PROJECT_NAME} ) +install(TARGETS + gzserver + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_targets(export_gzserver_component) +install(TARGETS gzserver_component + EXPORT export_gzserver_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/ros_gz_sim/README.md b/ros_gz_sim/README.md index 227d53a4b..cf4e8ca97 100644 --- a/ros_gz_sim/README.md +++ b/ros_gz_sim/README.md @@ -37,3 +37,77 @@ See more options with: ``` ros2 run ros_gz_sim create --helpshort ``` + +### Using `` to export model paths in `package.xml` + +The `` tag inside the `` tag of a `package.xml` file can be +used to add paths to `GZ_SIM_RESOURCE_PATH` and `GZ_SIM_SYSTEM_PLUGIN_PATH`, +which are environment variables used to configure Gazebo search paths for +resources (e.g. SDFormat files, meshes, etc) and plugins respectively. + +The values in the attributes `gazebo_model_path` and `gazebo_media_path` are +appended to `GZ_SIM_RESOURCE_PATH`. The value of `plugin_path` is appended to +`GZ_SIM_SYSTEM_PLUGIN_PATH`. See the +[Finding resources](https://gazebosim.org/api/sim/8/resources.html) tutorial to +learn more about these environment variables. + +The keyword `${prefix}` can be used when setting these values and it will be +expanded to the package's share path (i.e., the value of +`ros2 pkg prefix --share `) + +```xml + + + + + + +``` + +Thus the required directory needs to be installed from `CMakeLists.txt` + +```cmake +install(DIRECTORY models + DESTINATION share/${PROJECT_NAME}) +``` + +In order to reference the models in a ROS package unambiguously, it is +recommended to set the value of `gazebo_model_path` to be the parent +of the `prefix`. + +```xml + + + + +``` + +Consider an example where we have a ROS package called `my_awesome_pkg` +and it contains an SDFormat model cool `cool_robot`: + +```bash +my_awesome_pkg +├── models +│   └── cool_robot +│   ├── model.config +│   └── model.sdf +└── package.xml +``` + +With `gazebo_model_path="${prefix}/../` set up, we can +reference the `cool_robot` model in a world file using the package name +in the `uri`: + +```xml + + + + package://my_awesome_pkg/models/cool_robot + + + +``` + +However, if we set `gazebo_model_path=${prefix}/models`, we would +need to reference `cool_robot` as `package://cool_robot`, which +might have a name conflict with other models in the system. diff --git a/ros_gz_sim/launch/gz_server.launch b/ros_gz_sim/launch/gz_server.launch new file mode 100644 index 000000000..9a617649f --- /dev/null +++ b/ros_gz_sim/launch/gz_server.launch @@ -0,0 +1,12 @@ + + + + + + + + diff --git a/ros_gz_sim/launch/gz_server.launch.py b/ros_gz_sim/launch/gz_server.launch.py new file mode 100644 index 000000000..c73be0fea --- /dev/null +++ b/ros_gz_sim/launch/gz_server.launch.py @@ -0,0 +1,80 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch gz_server in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + declare_world_sdf_file_cmd = DeclareLaunchArgument( + 'world_sdf_file', default_value=TextSubstitution(text=''), + description='Path to the SDF world file') + declare_world_sdf_string_cmd = DeclareLaunchArgument( + 'world_sdf_string', default_value=TextSubstitution(text=''), + description='SDF world string') + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition',) + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + load_nodes = Node( + condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('use_composition')])), + package='ros_gz_sim', + executable='gzserver', + output='screen', + parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'), + 'world_sdf_string': LaunchConfiguration('world_sdf_string')}], + ) + + load_composable_nodes = ComposableNodeContainer( + condition=IfCondition(LaunchConfiguration('use_composition')), + name=LaunchConfiguration('container_name'), + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='ros_gz_sim', + plugin='ros_gz_sim::GzServer', + name='gz_server', + parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'), + 'world_sdf_string': LaunchConfiguration('world_sdf_string')}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + output='screen', + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_sdf_file_cmd) + ld.add_action(declare_world_sdf_string_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_composition_cmd) + # Add the actions to launch all of the gz_server nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/ros_gz_sim/launch/gz_sim.launch.py.in b/ros_gz_sim/launch/gz_sim.launch.py.in index 7bed5243e..d41bede62 100644 --- a/ros_gz_sim/launch/gz_sim.launch.py.in +++ b/ros_gz_sim/launch/gz_sim.launch.py.in @@ -14,20 +14,86 @@ """Launch Gazebo Sim with command line arguments.""" +import os from os import environ +from ament_index_python.packages import get_package_share_directory +from catkin_pkg.package import InvalidPackage, PACKAGE_MANIFEST_FILENAME, parse_package +from ros2pkg.api import get_package_names from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.actions import ExecuteProcess, Shutdown from launch.substitutions import LaunchConfiguration +# Copied from https://github.com/ros-simulation/gazebo_ros_pkgs/blob/79fd94c6da76781a91499bc0f54b70560b90a9d2/gazebo_ros/scripts/gazebo_ros_paths.py +""" +Search for model, plugin and media paths exported by packages. + +e.g. + + + +${prefix} is replaced by package's share directory in install. + +Thus the required directory needs to be installed from CMakeLists.txt +e.g. install(DIRECTORY models + DESTINATION share/${PROJECT_NAME}) +""" + +class GazeboRosPaths: + + @staticmethod + def get_paths(): + gazebo_model_path = [] + gazebo_plugin_path = [] + gazebo_media_path = [] + + for package_name in get_package_names(): + package_share_path = get_package_share_directory(package_name) + package_file_path = os.path.join(package_share_path, PACKAGE_MANIFEST_FILENAME) + if os.path.isfile(package_file_path): + try: + package = parse_package(package_file_path) + except InvalidPackage: + continue + for export in package.exports: + if export.tagname == 'gazebo_ros': + if 'gazebo_model_path' in export.attributes: + xml_path = export.attributes['gazebo_model_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_model_path.append(xml_path) + if 'plugin_path' in export.attributes: + xml_path = export.attributes['plugin_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_plugin_path.append(xml_path) + if 'gazebo_media_path' in export.attributes: + xml_path = export.attributes['gazebo_media_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_media_path.append(xml_path) + + gazebo_model_path = os.pathsep.join(gazebo_model_path + gazebo_media_path) + gazebo_plugin_path = os.pathsep.join(gazebo_plugin_path) + + return gazebo_model_path, gazebo_plugin_path + def launch_gz(context, *args, **kwargs): - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': - ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')]), - 'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': # TODO(CH3): To support pre-garden. Deprecated. - ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')])} + model_paths, plugin_paths = GazeboRosPaths.get_paths() + + env = { + "GZ_SIM_SYSTEM_PLUGIN_PATH": os.pathsep.join( + [ + environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", default=""), + environ.get("LD_LIBRARY_PATH", default=""), + plugin_paths, + ] + ), + "GZ_SIM_RESOURCE_PATH": os.pathsep.join( + [ + environ.get("GZ_SIM_RESOURCE_PATH", default=""), + model_paths, + ] + ), + } gz_args = LaunchConfiguration('gz_args').perform(context) gz_version = LaunchConfiguration('gz_version').perform(context) diff --git a/ros_gz_sim/launch/gz_spawn_model.launch b/ros_gz_sim/launch/gz_spawn_model.launch new file mode 100644 index 000000000..3f2890484 --- /dev/null +++ b/ros_gz_sim/launch/gz_spawn_model.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + diff --git a/ros_gz_sim/launch/gz_spawn_model.launch.py b/ros_gz_sim/launch/gz_spawn_model.launch.py new file mode 100644 index 000000000..59cb43617 --- /dev/null +++ b/ros_gz_sim/launch/gz_spawn_model.launch.py @@ -0,0 +1,94 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch create to spawn models in gz sim.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + + world = LaunchConfiguration('world') + file = LaunchConfiguration('file') + xml_string = LaunchConfiguration('string') + topic = LaunchConfiguration('topic') + name = LaunchConfiguration('name') + allow_renaming = LaunchConfiguration('allow_renaming') + x = LaunchConfiguration('x', default='0.0') + y = LaunchConfiguration('y', default='0.0') + z = LaunchConfiguration('z', default='0.0') + roll = LaunchConfiguration('R', default='0.0') + pitch = LaunchConfiguration('P', default='0.0') + yaw = LaunchConfiguration('Y', default='0.0') + + declare_world_cmd = DeclareLaunchArgument( + 'world', default_value=TextSubstitution(text=''), + description='World name') + declare_file_cmd = DeclareLaunchArgument( + 'file', default_value=TextSubstitution(text=''), + description='SDF filename') + declare_xml_string_cmd = DeclareLaunchArgument( + 'string', + default_value='', + description='XML string', + ) + declare_topic_cmd = DeclareLaunchArgument( + 'topic', default_value=TextSubstitution(text=''), + description='Get XML from this topic' + ) + declare_name_cmd = DeclareLaunchArgument( + 'name', default_value=TextSubstitution(text=''), + description='Name of the entity' + ) + declare_allow_renaming_cmd = DeclareLaunchArgument( + 'allow_renaming', default_value='False', + description='Whether the entity allows renaming or not' + ) + + load_nodes = Node( + package='ros_gz_sim', + executable='create', + output='screen', + parameters=[{'world': world, + 'file': file, + 'string': xml_string, + 'topic': topic, + 'name': name, + 'allow_renaming': allow_renaming, + 'x': x, + 'y': y, + 'z': z, + 'R': roll, + 'P': pitch, + 'Y': yaw, + }], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_cmd) + ld.add_action(declare_file_cmd) + ld.add_action(declare_xml_string_cmd) + ld.add_action(declare_topic_cmd) + ld.add_action(declare_name_cmd) + ld.add_action(declare_allow_renaming_cmd) + # Add the actions to launch all of the create nodes + ld.add_action(load_nodes) + + return ld diff --git a/ros_gz_sim/launch/ros_gz_sim.launch b/ros_gz_sim/launch/ros_gz_sim.launch new file mode 100644 index 000000000..89b048b24 --- /dev/null +++ b/ros_gz_sim/launch/ros_gz_sim.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py new file mode 100644 index 000000000..cad862ea7 --- /dev/null +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -0,0 +1,111 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch gzsim + ros_gz_bridge in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + config_file = LaunchConfiguration('config_file') + container_name = LaunchConfiguration('container_name') + namespace = LaunchConfiguration('namespace') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + bridge_log_level = LaunchConfiguration('bridge_log_level') + + world_sdf_file = LaunchConfiguration('world_sdf_file') + world_sdf_string = LaunchConfiguration('world_sdf_string') + + declare_config_file_cmd = DeclareLaunchArgument( + 'config_file', default_value='', description='YAML config file' + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_bridge_log_level_cmd = DeclareLaunchArgument( + 'bridge_log_level', default_value='info', description='Bridge log level' + ) + + declare_world_sdf_file_cmd = DeclareLaunchArgument( + 'world_sdf_file', default_value=TextSubstitution(text=''), + description='Path to the SDF world file' + ) + + declare_world_sdf_string_cmd = DeclareLaunchArgument( + 'world_sdf_string', default_value=TextSubstitution(text=''), + description='SDF world string' + ) + + bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('config_file', config_file), + ('container_name', container_name), + ('namespace', namespace), + ('use_composition', use_composition), + ('use_respawn', use_respawn), + ('bridge_log_level', bridge_log_level)]) + + gz_server_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_server.launch.py'])]), + launch_arguments=[('world_sdf_file', world_sdf_file), + ('world_sdf_string', world_sdf_string), + ('use_composition', use_composition), ]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_config_file_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_bridge_log_level_cmd) + ld.add_action(declare_world_sdf_file_cmd) + ld.add_action(declare_world_sdf_string_cmd) + # Add the actions to launch all of the bridge + gz_server nodes + ld.add_action(bridge_description) + ld.add_action(gz_server_description) + + return ld diff --git a/ros_gz_sim/launch/ros_gz_spawn_model.launch.py b/ros_gz_sim/launch/ros_gz_spawn_model.launch.py new file mode 100644 index 000000000..e5de33aa4 --- /dev/null +++ b/ros_gz_sim/launch/ros_gz_spawn_model.launch.py @@ -0,0 +1,154 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch gzsim + ros_gz_bridge in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + config_file = LaunchConfiguration('config_file') + container_name = LaunchConfiguration('container_name') + namespace = LaunchConfiguration('namespace') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + world = LaunchConfiguration('world') + file = LaunchConfiguration('file') + xml_string = LaunchConfiguration('string') + topic = LaunchConfiguration('topic') + name = LaunchConfiguration('name') + allow_renaming = LaunchConfiguration('allow_renaming') + x = LaunchConfiguration('x', default='0.0') + y = LaunchConfiguration('y', default='0.0') + z = LaunchConfiguration('z', default='0.0') + roll = LaunchConfiguration('R', default='0.0') + pitch = LaunchConfiguration('P', default='0.0') + yaw = LaunchConfiguration('Y', default='0.0') + + declare_config_file_cmd = DeclareLaunchArgument( + 'config_file', default_value='', description='YAML config file' + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + declare_world_cmd = DeclareLaunchArgument( + 'world', default_value=TextSubstitution(text=''), + description='World name') + + declare_file_cmd = DeclareLaunchArgument( + 'file', default_value=TextSubstitution(text=''), + description='SDF filename') + + declare_xml_string_cmd = DeclareLaunchArgument( + 'string', + default_value='', + description='XML string', + ) + + declare_topic_cmd = DeclareLaunchArgument( + 'topic', default_value=TextSubstitution(text=''), + description='Get XML from this topic' + ) + + declare_name_cmd = DeclareLaunchArgument( + 'name', default_value=TextSubstitution(text=''), + description='Name of the entity' + ) + + declare_allow_renaming_cmd = DeclareLaunchArgument( + 'allow_renaming', default_value='False', + description='Whether the entity allows renaming or not' + ) + + bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('config_file', config_file), + ('container_name', container_name), + ('namespace', namespace), + ('use_composition', use_composition), + ('use_respawn', use_respawn), + ('log_level', log_level), ]) + + spawn_model_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_spawn_model.launch.py'])]), + launch_arguments=[('world', world), + ('file', file), + ('xml_string', xml_string), + ('topic', topic), + ('name', name), + ('allow_renaming', allow_renaming), + ('x', x), + ('y', y), + ('z', z), + ('R', roll), + ('P', pitch), + ('Y', yaw), + ('use_composition', use_composition), ]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_config_file_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_file_cmd) + ld.add_action(declare_xml_string_cmd) + ld.add_action(declare_topic_cmd) + ld.add_action(declare_name_cmd) + ld.add_action(declare_allow_renaming_cmd) + # Add the actions to launch all of the bridge + spawn_model nodes + ld.add_action(bridge_description) + ld.add_action(spawn_model_description) + + return ld diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 1047196df..7d8a0bf48 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -2,7 +2,7 @@ ros_gz_sim - 1.0.0 + 1.0.1 Tools for using Gazebo Sim simulation with ROS. Alejandro Hernandez Aditya Pande @@ -10,13 +10,19 @@ Apache 2.0 Alejandro Hernandez + Addisu Taddese + Carlos Agüero ament_cmake + ament_cmake_python pkg-config ament_index_python + launch + launch_ros libgflags-dev rclcpp + rclcpp_components std_msgs gz_math_vendor diff --git a/ros_gz_sim/resource/ros_gz_sim b/ros_gz_sim/resource/ros_gz_sim new file mode 100644 index 000000000..e69de29bb diff --git a/ros_gz_sim/ros_gz_sim/__init__.py b/ros_gz_sim/ros_gz_sim/__init__.py new file mode 100644 index 000000000..17b9696c3 --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/__init__.py @@ -0,0 +1,22 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Main entry point for the `ros_gz_sim` package.""" + +from . import actions + + +__all__ = [ + 'actions', +] diff --git a/ros_gz_sim/ros_gz_sim/actions/__init__.py b/ros_gz_sim/ros_gz_sim/actions/__init__.py new file mode 100644 index 000000000..bea18edd1 --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/actions/__init__.py @@ -0,0 +1,24 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Actions module.""" + +from .gz_spawn_model import GzSpawnModel +from .gzserver import GzServer + + +__all__ = [ + 'GzSpawnModel', + 'GzServer', +] diff --git a/ros_gz_sim/ros_gz_sim/actions/gz_spawn_model.py b/ros_gz_sim/ros_gz_sim/actions/gz_spawn_model.py new file mode 100644 index 000000000..7626eb019 --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/actions/gz_spawn_model.py @@ -0,0 +1,207 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for the gz_spawn_model action.""" + +from typing import List +from typing import Optional + +from launch.action import Action +from launch.actions import IncludeLaunchDescription +from launch.frontend import Entity, expose_action, Parser +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.some_substitutions_type import SomeSubstitutionsType +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +@expose_action('gz_spawn_model') +class GzSpawnModel(Action): + """Action that executes a gz_spawn_model ROS node.""" + + def __init__( + self, + *, + world: Optional[SomeSubstitutionsType] = None, + file: Optional[SomeSubstitutionsType] = None, + xml_string: Optional[SomeSubstitutionsType] = None, + topic: Optional[SomeSubstitutionsType] = None, + name: Optional[SomeSubstitutionsType] = None, + allow_renaming: Optional[SomeSubstitutionsType] = None, + x: Optional[SomeSubstitutionsType] = None, + y: Optional[SomeSubstitutionsType] = None, + z: Optional[SomeSubstitutionsType] = None, + roll: Optional[SomeSubstitutionsType] = None, + pitch: Optional[SomeSubstitutionsType] = None, + yaw: Optional[SomeSubstitutionsType] = None, + **kwargs + ) -> None: + """ + Construct a gz_spawn_model action. + + All arguments are forwarded to `ros_gz_sim.launch.gz_spawn_model.launch.py`, + so see the documentation of that class for further details. + + :param: world World name. + :param: file SDF filename. + :param: xml_string XML string. + :param: topic Get XML from this topic. + :param: name Name of the entity. + :param: allow_renaming Whether the entity allows renaming or not. + :param: x X coordinate. + :param: y Y coordinate. + :param: z Z coordinate. + :param: roll Roll orientation. + :param: pitch Pitch orientation. + :param: yaw Yaw orientation. + """ + super().__init__(**kwargs) + self.__world = world + self.__file = file + self.__xml_string = xml_string + self.__topic = topic + self.__name = name + self.__allow_renaming = allow_renaming + self.__x = x + self.__y = y + self.__z = z + self.__roll = roll + self.__pitch = pitch + self.__yaw = yaw + + @classmethod + def parse(cls, entity: Entity, parser: Parser): + """Parse gz_spawn_model.""" + _, kwargs = super().parse(entity, parser) + + world = entity.get_attr( + 'world', data_type=str, + optional=True) + + file = entity.get_attr( + 'file', data_type=str, + optional=True) + + xml_string = entity.get_attr( + 'xml_string', data_type=str, + optional=True) + + topic = entity.get_attr( + 'topic', data_type=str, + optional=True) + + name = entity.get_attr( + 'name', data_type=str, + optional=True) + + allow_renaming = entity.get_attr( + 'allow_renaming', data_type=str, + optional=True) + + x = entity.get_attr( + 'x', data_type=str, + optional=True) + + y = entity.get_attr( + 'y', data_type=str, + optional=True) + + z = entity.get_attr( + 'z', data_type=str, + optional=True) + + roll = entity.get_attr( + 'roll', data_type=str, + optional=True) + + pitch = entity.get_attr( + 'pitch', data_type=str, + optional=True) + + yaw = entity.get_attr( + 'yaw', data_type=str, + optional=True) + + if isinstance(world, str): + world = parser.parse_substitution(world) + kwargs['world'] = world + + if isinstance(file, str): + file = parser.parse_substitution(file) + kwargs['file'] = file + + if isinstance(xml_string, str): + xml_string = parser.parse_substitution(xml_string) + kwargs['xml_string'] = xml_string + + if isinstance(topic, str): + topic = parser.parse_substitution(topic) + kwargs['topic'] = topic + + if isinstance(name, str): + name = parser.parse_substitution(name) + kwargs['name'] = name + + if isinstance(allow_renaming, str): + allow_renaming = parser.parse_substitution(allow_renaming) + kwargs['allow_renaming'] = allow_renaming + + if isinstance(x, str): + x = parser.parse_substitution(x) + kwargs['x'] = x + + if isinstance(y, str): + y = parser.parse_substitution(y) + kwargs['y'] = y + + if isinstance(z, str): + z = parser.parse_substitution(z) + kwargs['z'] = z + + if isinstance(roll, str): + roll = parser.parse_substitution(roll) + kwargs['roll'] = roll + + if isinstance(pitch, str): + pitch = parser.parse_substitution(pitch) + kwargs['pitch'] = pitch + + if isinstance(yaw, str): + yaw = parser.parse_substitution(yaw) + kwargs['yaw'] = yaw + + return cls, kwargs + + def execute(self, context: LaunchContext) -> Optional[List[Action]]: + """Execute the action.""" + gz_spawn_model_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_spawn_model.launch.py'])]), + launch_arguments=[('world', self.__world), + ('file', self.__file), + ('xml_string', self.__xml_string), + ('topic', self.__topic), + ('name', self.__name), + ('allow_renaming', self.__allow_renaming), + ('x', self.__x), + ('y', self.__y), + ('z', self.__z), + ('roll', self.__roll), + ('pitch', self.__pitch), + ('yaw', self.__yaw), ]) + + return [gz_spawn_model_description] diff --git a/ros_gz_sim/ros_gz_sim/actions/gzserver.py b/ros_gz_sim/ros_gz_sim/actions/gzserver.py new file mode 100644 index 000000000..14df9aaba --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/actions/gzserver.py @@ -0,0 +1,111 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for the GzServer action.""" + +from typing import List +from typing import Optional + +from launch.action import Action +from launch.actions import IncludeLaunchDescription +from launch.frontend import Entity, expose_action, Parser +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.some_substitutions_type import SomeSubstitutionsType +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +@expose_action('gz_server') +class GzServer(Action): + """Action that executes a gz_server ROS [composable] node.""" + + def __init__( + self, + *, + world_sdf_file: Optional[SomeSubstitutionsType] = None, + world_sdf_string: Optional[SomeSubstitutionsType] = None, + container_name: Optional[SomeSubstitutionsType] = None, + use_composition: Optional[SomeSubstitutionsType] = None, + **kwargs + ) -> None: + """ + Construct a gz_server action. + + All arguments are forwarded to `ros_gz_sim.launch.gz_server.launch.py`, + so see the documentation of that class for further details. + + :param: world_sdf_file Path to the SDF world file. + :param: world_sdf_string SDF world string. + :param: container_name Name of container that nodes will load in if use composition. + :param: use_composition Use composed bringup if True. + """ + super().__init__(**kwargs) + self.__world_sdf_file = world_sdf_file + self.__world_sdf_string = world_sdf_string + self.__container_name = container_name + self.__use_composition = use_composition + + @classmethod + def parse(cls, entity: Entity, parser: Parser): + """Parse gz_server.""" + _, kwargs = super().parse(entity, parser) + + world_sdf_file = entity.get_attr( + 'world_sdf_file', data_type=str, + optional=True) + + world_sdf_string = entity.get_attr( + 'world_sdf_string', data_type=str, + optional=True) + + container_name = entity.get_attr( + 'container_name', data_type=str, + optional=True) + + use_composition = entity.get_attr( + 'use_composition', data_type=str, + optional=True) + + if isinstance(world_sdf_file, str): + world_sdf_file = parser.parse_substitution(world_sdf_file) + kwargs['world_sdf_file'] = world_sdf_file + + if isinstance(world_sdf_string, str): + world_sdf_string = parser.parse_substitution(world_sdf_string) + kwargs['world_sdf_string'] = world_sdf_string + + if isinstance(container_name, str): + container_name = parser.parse_substitution(container_name) + kwargs['container_name'] = container_name + + if isinstance(use_composition, str): + use_composition = parser.parse_substitution(use_composition) + kwargs['use_composition'] = use_composition + + return cls, kwargs + + def execute(self, context: LaunchContext) -> Optional[List[Action]]: + """Execute the action.""" + gz_server_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_server.launch.py'])]), + launch_arguments=[('world_sdf_file', self.__world_sdf_file), + ('world_sdf_string', self.__world_sdf_string), + ('container_name', self.__container_name), + ('use_composition', self.__use_composition), ]) + + return [gz_server_description] diff --git a/ros_gz_sim/setup.cfg b/ros_gz_sim/setup.cfg new file mode 100644 index 000000000..c044b41d2 --- /dev/null +++ b/ros_gz_sim/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +launch.frontend.launch_extension = + ros_gz_sim = ros_gz_sim diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp new file mode 100644 index 000000000..aa4361f51 --- /dev/null +++ b/ros_gz_sim/src/gzserver.cpp @@ -0,0 +1,80 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS node that executes a gz-sim Server given a world SDF file or string. +namespace ros_gz_sim +{ +class GzServer : public rclcpp::Node +{ +public: + // Class constructor. + explicit GzServer(const rclcpp::NodeOptions & options) + : Node("gzserver", options) + { + thread_ = std::thread(std::bind(&GzServer::OnStart, this)); + } + +public: + // Class destructor. + ~GzServer() + { + // Make sure to join the thread on shutdown. + if (thread_.joinable()) { + thread_.join(); + } + } + +public: + /// \brief Run the gz sim server. + void OnStart() + { + auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); + auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); + + gz::common::Console::SetVerbosity(4); + gz::sim::ServerConfig server_config; + + if (!world_sdf_file.empty()) { + server_config.SetSdfFile(world_sdf_file); + } else if (!world_sdf_string.empty()) { + server_config.SetSdfString(world_sdf_string); + } else { + RCLCPP_ERROR( + this->get_logger(), + "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + rclcpp::shutdown(); + return; + } + + gz::sim::Server server(server_config); + server.Run(true /*blocking*/, 0, false /*paused*/); + rclcpp::shutdown(); + } + +private: + /// \brief We don't want to block the ROS thread. + std::thread thread_; +}; +} // namespace ros_gz_sim + +RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer) diff --git a/ros_gz_sim_demos/CHANGELOG.rst b/ros_gz_sim_demos/CHANGELOG.rst index 42752ea11..6a5f8f7a0 100644 --- a/ros_gz_sim_demos/CHANGELOG.rst +++ b/ros_gz_sim_demos/CHANGELOG.rst @@ -2,6 +2,69 @@ Changelog for package ros1_gz_sim_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Prepare for 1.0.0 Release (`#495 `_) +* Use gz_vendor packages (`#531 `_) +* [backport Humble] Create bridge for GPSFix msg (`#316 `_) (`#538 `_) + Co-authored-by: Rousseau Vincent +* [backport Iron] Create bridge for GPSFix msg (`#316 `_) (`#537 `_) + Co-authored-by: Rousseau Vincent +* 0.244.14 +* Changelog +* 0.244.13 +* Changelog +* Remove deprecations using ros_gz_sim_create (`#476 `_) +* 0.244.12 +* Changelog +* 0.246.0 +* Update changelogs +* Added more topic to the bridge (`#422 `_) +* Fix incorrect subscription on demo (`#405 `_) (`#408 `_) + This PR fixes an incorrect subscription on one of the demos. Running + ``` + ros2 launch ros_gz_sim_demos gpu_lidar_bridge.launch.py + ``` + causes rviz2 to crash and exit with the error: + ``` + rviz2-3] + [rviz2-3] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() + [rviz2-3] This error state is being overwritten: + [rviz2-3] + [rviz2-3] 'create_subscription() called for existing topic name rt/lidar with incompatible type sensor_msgs::msg::dds\_::PointCloud2\_, at ./src/subscription.cpp:146, at ./src/rcl/subscription.c:108' + [rviz2-3] + [rviz2-3] with this new error message: + [rviz2-3] + [rviz2-3] 'invalid allocator, at ./src/rcl/subscription.c:218' + [rviz2-3] + [rviz2-3] rcutils_reset_error() should be called after error handling to avoid this. + ``` + This is due to an incorrect subscription on the part of the demo. This + PR fixes it by getting a subscription to the right topic for the + pointcloud display. (`lidar/points` instead of `lidar`). Was tested on + garden + humble. + Co-authored-by: Arjo Chakravarty +* Port: humble to ros2 (`#386 `_) +* Merge branch 'humble' into mjcarroll/humble_to_ros2 +* Update maintainers (`#376 `_) +* Rename 'ign gazebo' to 'gz sim' (`#343 `_) +* Create bridge for GPSFix msg (`#316 `_) +* Humble ➡️ ROS2 (`#323 `_) + Humble ➡️ ROS2 +* Merge branch 'humble' into ports/humble_to_ros2 +* Fixed ros_gz_sim_demos launch files (`#319 `_) +* 0.245.0 +* Changelog +* humble to ros2 (`#311 `_) + Co-authored-by: Michael Carroll +* Merge remote-tracking branch 'origin/humble' into ahcorde/humble_to_ros2 +* Remove all ignition references on ROS 2 branch (`#302 `_) + * Remove all shims + * Update CMakeLists and package.xml for garden + * Complete garden gz renaming + * Drop fortress CI +* Contributors: Addisu Z. Taddese, Aditya Pande, Alejandro Hernández Cordero, Clyde McQueen, Jose Luis Rivero, Michael Carroll, Rousseau Vincent, ahcorde + 1.0.0 (2024-04-24) ------------------ * Use gz_vendor packages (`#531 `_) diff --git a/ros_gz_sim_demos/package.xml b/ros_gz_sim_demos/package.xml index cb65108c2..a169b8e71 100644 --- a/ros_gz_sim_demos/package.xml +++ b/ros_gz_sim_demos/package.xml @@ -1,6 +1,6 @@ ros_gz_sim_demos - 1.0.0 + 1.0.1 Demos using Gazebo Sim simulation with ROS. Apache 2.0 Aditya Pande diff --git a/test_ros_gz_bridge/CHANGELOG.rst b/test_ros_gz_bridge/CHANGELOG.rst index f4cad53ca..72a2d2143 100644 --- a/test_ros_gz_bridge/CHANGELOG.rst +++ b/test_ros_gz_bridge/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package test_ros_gz_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2024-07-03) +------------------ +* Prepare for 1.0.0 Release (`#495 `_) +* 0.244.14 +* Changelog +* Correctly export ros_gz_bridge for downstream targets (`#503 `_) (`#506 `_) +* Correctly export ros_gz_bridge for downstream targets (`#503 `_) +* Contributors: Addisu Z. Taddese, Alejandro Hernández Cordero, Michael Carroll + 1.0.0 (2024-04-24) ------------------ * Correctly export ros_gz_bridge for downstream targets (`#503 `_) diff --git a/test_ros_gz_bridge/package.xml b/test_ros_gz_bridge/package.xml index 659c820b9..bbe05aebe 100644 --- a/test_ros_gz_bridge/package.xml +++ b/test_ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ test_ros_gz_bridge - 1.0.0 + 1.0.1 Bridge communication between ROS and Gazebo Transport Aditya Pande Alejandro Hernandez