From 2b9cccfda88c452f58d9572e998b01d7fb0b4890 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 3 Jan 2024 02:22:58 +0800 Subject: [PATCH 01/17] Update README to include harmonic (#473) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update README to include harmonic Signed-off-by: Luca Della Vedova * Remove humble-harmonic combination Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova Signed-off-by: Carlos Agüero --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 35f1cfa0..aeb6f6fa 100644 --- a/README.md +++ b/README.md @@ -10,9 +10,10 @@ Humble | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | 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 -Rolling | Edifice | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source +Iron | Harmonic | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | only from source 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 For information on ROS(1) and Gazebo compatibility, refer to the [noetic branch README](https://github.com/gazebosim/ros_gz/tree/noetic) From 15019e84c1b018c0841d6806196ba2c45c7e89fb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 3 Jan 2024 04:00:31 -0600 Subject: [PATCH 02/17] Add tip about non-parallel builds to README (#477) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index aeb6f6fa..6ae04f5e 100644 --- a/README.md +++ b/README.md @@ -125,6 +125,9 @@ The following steps are for Linux and OSX. cd ~/ws colcon build ``` + > [!TIP] + > The `ros_gz` library makes heavy use of templates which causes compilers to consume a lot of memory. If your build fails with `c++: fatal error: Killed signal terminated program cc1plus` + > try building with `colcon build --parallel-workers=1 --executor sequential`. ## ROSCon 2022 From 8d4b48b460aeea34aabd9818f1e32d93e9013716 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 3 Jan 2024 12:23:32 -0600 Subject: [PATCH 03/17] Additional tip for limiting number of cpus (#480) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 6ae04f5e..3e0b5137 100644 --- a/README.md +++ b/README.md @@ -127,7 +127,8 @@ The following steps are for Linux and OSX. ``` > [!TIP] > The `ros_gz` library makes heavy use of templates which causes compilers to consume a lot of memory. If your build fails with `c++: fatal error: Killed signal terminated program cc1plus` - > try building with `colcon build --parallel-workers=1 --executor sequential`. + > try building with `colcon build --parallel-workers=1 --executor sequential`. You might also have to set `export MAKEFLAGS="-j 1"` before running `colcon build` to limit + > the number of processors used to build a single package. ## ROSCon 2022 From 178f5bbd4c111503c46c1a0eb575b5b3c2737116 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 30 Jan 2024 14:31:11 -0600 Subject: [PATCH 04/17] Undeprecate use of commandline flags (#491) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- ros_gz_sim/src/create.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ros_gz_sim/src/create.cpp b/ros_gz_sim/src/create.cpp index 70e837af..db2b7452 100644 --- a/ros_gz_sim/src/create.cpp +++ b/ros_gz_sim/src/create.cpp @@ -214,10 +214,8 @@ int main(int _argc, char ** _argv) ros2_node->get_logger(), "Must specify either -file, -param, -string or -topic"); return -1; } - RCLCPP_WARN( - ros2_node->get_logger(), - "Usage of Commandline flags for spawning entities is deprecated. Please use ROS 2 parameters." - ); + // TODO(azeey) Deprecate use of command line flags in ROS 2 K-turtle in + // favor of ROS 2 parameters. } else { RCLCPP_ERROR( ros2_node->get_logger(), "Must specify either file, string or topic as ROS 2 parameters"); From e23573861863e3badc3585870570743ae9aef3c0 Mon Sep 17 00:00:00 2001 From: Katherine Scott Date: Wed, 21 Feb 2024 13:13:24 -0800 Subject: [PATCH 05/17] Update README.md to clarify what will be supported in Jazzy (#499) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Also Added a note pointing to REP-2000 for additional details. --------- Signed-off-by: Katherine Scott Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 3e0b5137..6affa43d 100644 --- a/README.md +++ b/README.md @@ -11,10 +11,14 @@ Humble | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | on 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 +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 +* 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) From 4a078e04f43822342854c40a0a7ef7aad50f6f5e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 22 Feb 2024 13:21:24 -0600 Subject: [PATCH 06/17] Add a virtual destructor to suppress compiler warning (#502) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michael Carroll Signed-off-by: Carlos Agüero --- ros_gz_bridge/CMakeLists.txt | 1 + .../src/service_factory_interface.cpp | 24 +++++++++++++++++++ .../src/service_factory_interface.hpp | 2 ++ 3 files changed, 27 insertions(+) create mode 100644 ros_gz_bridge/src/service_factory_interface.cpp diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index ba45273c..829301ab 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -110,6 +110,7 @@ add_library(${bridge_lib} src/bridge_handle_ros_to_gz.cpp src/bridge_handle_gz_to_ros.cpp src/factory_interface.cpp + src/service_factory_interface.cpp src/service_factories/ros_gz_interfaces.cpp src/ros_gz_bridge.cpp ${convert_files} diff --git a/ros_gz_bridge/src/service_factory_interface.cpp b/ros_gz_bridge/src/service_factory_interface.cpp new file mode 100644 index 00000000..4789e066 --- /dev/null +++ b/ros_gz_bridge/src/service_factory_interface.cpp @@ -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. + +#include "service_factory_interface.hpp" + +namespace ros_gz_bridge +{ + +ServiceFactoryInterface::~ServiceFactoryInterface() +{ +} + +} // namespace ros_gz_bridge diff --git a/ros_gz_bridge/src/service_factory_interface.hpp b/ros_gz_bridge/src/service_factory_interface.hpp index 9d4f7334..766ce515 100644 --- a/ros_gz_bridge/src/service_factory_interface.hpp +++ b/ros_gz_bridge/src/service_factory_interface.hpp @@ -29,6 +29,8 @@ namespace ros_gz_bridge class ServiceFactoryInterface { public: + virtual ~ServiceFactoryInterface() = 0; + virtual rclcpp::ServiceBase::SharedPtr create_ros_service( From 69b00efaa14e291ed95612d949c8af26da285d0e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 22 Feb 2024 13:43:40 -0600 Subject: [PATCH 07/17] Correctly export ros_gz_bridge for downstream targets (#503) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michael Carroll Signed-off-by: Carlos Agüero --- ros_gz_bridge/CMakeLists.txt | 37 ++++++++++++------- test_ros_gz_bridge/CMakeLists.txt | 29 +++++++++++++++ test_ros_gz_bridge/package.xml | 28 ++++++++++++++ test_ros_gz_bridge/src/test_ros_gz_bridge.cpp | 36 ++++++++++++++++++ 4 files changed, 117 insertions(+), 13 deletions(-) create mode 100644 test_ros_gz_bridge/CMakeLists.txt create mode 100644 test_ros_gz_bridge/package.xml create mode 100644 test_ros_gz_bridge/src/test_ros_gz_bridge.cpp diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 829301ab..6c2d4b3e 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -100,7 +100,7 @@ add_custom_command( COMMENT "Generating factories for interface types") set(bridge_lib - ros_gz_bridge_lib + ros_gz_bridge ) add_library(${bridge_lib} @@ -130,8 +130,12 @@ ament_target_dependencies(${bridge_lib} ) target_include_directories(${bridge_lib} - PUBLIC include - PRIVATE src ${generated_path} + PUBLIC + "$" + "$" + PRIVATE + "$" + "$" ) target_link_libraries(${bridge_lib} @@ -144,19 +148,16 @@ rclcpp_components_register_node( PLUGIN ros_gz_bridge::RosGzBridge EXECUTABLE bridge_node) -install(TARGETS ${bridge_lib} +install(TARGETS ${bridge_lib} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) -ament_export_include_directories(include) -ament_export_libraries(${bridge_lib}) - set(bridge_executables parameter_bridge static_bridge @@ -175,7 +176,7 @@ foreach(bridge ${bridge_executables}) ${BRIDGE_MESSAGE_TYPES} ) install(TARGETS ${bridge} - DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) endforeach() @@ -330,9 +331,19 @@ if(BUILD_TESTING) endif() -ament_export_dependencies( - rclcpp - ${BRIDGE_MESSAGE_TYPES} -) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${bridge_lib}) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +# specific order: dependents before dependencies +ament_export_dependencies(rclcpp) +ament_export_dependencies(rclcpp_components) +ament_export_dependencies(${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}) +ament_export_dependencies(${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}) +ament_export_dependencies(yaml_cpp_vendor) +ament_export_dependencies(${BRIDGE_MESSAGE_TYPES}) ament_package() diff --git a/test_ros_gz_bridge/CMakeLists.txt b/test_ros_gz_bridge/CMakeLists.txt new file mode 100644 index 00000000..9a3bb8a8 --- /dev/null +++ b/test_ros_gz_bridge/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) + +project(test_ros_gz_bridge) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(ros_gz_bridge REQUIRED) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + ament_find_gtest() + + ament_add_gtest(test_ros_gz_bridge src/test_ros_gz_bridge.cpp) + target_link_libraries(test_ros_gz_bridge ros_gz_bridge::ros_gz_bridge) +endif() + +ament_package() diff --git a/test_ros_gz_bridge/package.xml b/test_ros_gz_bridge/package.xml new file mode 100644 index 00000000..124b8044 --- /dev/null +++ b/test_ros_gz_bridge/package.xml @@ -0,0 +1,28 @@ + + + + test_ros_gz_bridge + 0.246.0 + Bridge communication between ROS and Gazebo Transport + Aditya Pande + Alejandro Hernandez + + Apache 2.0 + + Michael Carroll + + ament_cmake + + ros_gz_bridge + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch_testing_ament_cmake + launch_ros + launch_testing + + + ament_cmake + + diff --git a/test_ros_gz_bridge/src/test_ros_gz_bridge.cpp b/test_ros_gz_bridge/src/test_ros_gz_bridge.cpp new file mode 100644 index 00000000..a32ca8e5 --- /dev/null +++ b/test_ros_gz_bridge/src/test_ros_gz_bridge.cpp @@ -0,0 +1,36 @@ +// 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 + +class test_ros_gz_bridge : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(test_ros_gz_bridge, SpawnNode) +{ + ros_gz_bridge::RosGzBridge node; +} From 023edca13ce0c5c5fcd27fe32f6324054582b966 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 15 Mar 2024 15:13:26 +0100 Subject: [PATCH 08/17] Feedback updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 17 +++++++++++++++++ ros_gz_sim/package.xml | 1 + ros_gz_sim_demos/CMakeLists.txt | 6 ++++++ ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in | 2 +- 4 files changed, 25 insertions(+), 1 deletion(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 7f420e09..eb79bf94 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(ros_gz_bridge REQUIRED) find_package(std_msgs REQUIRED) if(NOT DEFINED ENV{GZ_VERSION}) @@ -86,6 +87,22 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) +# ROS Gz bridge +add_library(ros-gz-bridge-system SHARED src/ros_gz_bridge_system.cpp) +target_link_libraries(ros-gz-bridge-system PUBLIC + gz-sim${GZ_SIM_VER}::core + rclcpp::rclcpp + ros_gz_bridge::ros_gz_bridge +) +target_include_directories(ros-gz-bridge-system PUBLIC + "$" + "$" +) +install( + TARGETS ros-gz-bridge-system + DESTINATION lib +) + configure_file( launch/gz_sim.launch.py.in launch/gz_sim.launch.py.configured diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index a4cad906..bbad141a 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -17,6 +17,7 @@ ament_index_python libgflags-dev rclcpp + ros_gz_bridge std_msgs diff --git a/ros_gz_sim_demos/CMakeLists.txt b/ros_gz_sim_demos/CMakeLists.txt index 8e0f580a..d0626157 100644 --- a/ros_gz_sim_demos/CMakeLists.txt +++ b/ros_gz_sim_demos/CMakeLists.txt @@ -33,6 +33,12 @@ install( DESTINATION share/${PROJECT_NAME}/worlds ) +install( + DIRECTORY + config/ + DESTINATION share/${PROJECT_NAME}/config +) + ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") ament_package() diff --git a/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in b/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in index a0c6d4fc..68a3bc14 100644 --- a/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in +++ b/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in @@ -1 +1 @@ -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share From 8f9e452b01420beb31a7d33b55fc4a349f46158c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 15 Mar 2024 15:44:48 +0100 Subject: [PATCH 09/17] Adding files. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- .../ros_gz_sim/ros_gz_bridge_system.hpp | 57 ++++++++++ ros_gz_sim/src/ros_gz_bridge_system.cpp | 106 ++++++++++++++++++ ros_gz_sim_demos/config/full.yaml | 18 +++ ros_gz_sim_demos/launch/ros_gz.launch.py | 42 +++++++ ros_gz_sim_demos/worlds/ros_gz.sdf | 77 +++++++++++++ 5 files changed, 300 insertions(+) create mode 100644 ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp create mode 100644 ros_gz_sim/src/ros_gz_bridge_system.cpp create mode 100644 ros_gz_sim_demos/config/full.yaml create mode 100644 ros_gz_sim_demos/launch/ros_gz.launch.py create mode 100644 ros_gz_sim_demos/worlds/ros_gz.sdf diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp new file mode 100644 index 00000000..dc1e58de --- /dev/null +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Open Source Robotics Foundation +// +// 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. + +#ifndef ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ +#define ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ + +#include +#include +#include + +namespace ros_gz_sim +{ +// Private class. +class ROSGzBridgeSystemPrivate; + +/// Important: This system, although functional, it's still under development. +/// The API or its parameters can change in the near future. +/// \brief A ROS 2 <--> gz bridge system. +/// +/// This system can be configured with the following SDF parameters: +/// * Optional parameters: +/// * : Path to a YAML file containing the list of topics to be +/// bridged. +class ROSGzBridgeSystem + : public gz::sim::System, + public gz::sim::ISystemConfigure +{ +public: + // \brief Constructor. + ROSGzBridgeSystem(); + + /// \brief Destructor. + ~ROSGzBridgeSystem() override; + + // Documentation inherited. + void Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; + + /// \brief Private data pointer. + std::unique_ptr dataPtr; +}; +} // namespace ros_gz_sim +#endif // ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ diff --git a/ros_gz_sim/src/ros_gz_bridge_system.cpp b/ros_gz_sim/src/ros_gz_bridge_system.cpp new file mode 100644 index 00000000..04a53344 --- /dev/null +++ b/ros_gz_sim/src/ros_gz_bridge_system.cpp @@ -0,0 +1,106 @@ +// 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 +#include + +#include "ros_gz_bridge_system.hpp" + +namespace ros_gz_sim +{ +/// \brief Private ROSGzBridgeSystem data class. +class ROSGzBridgeSystemPrivate +{ +public: + /// \brief The ROS 2 <--> Gz bridge. + std::shared_ptr bridge; + + /// \brief The ROS 2 executor. + std::shared_ptr exec; + + /// \brief A thread to call spin and not block the Gazebo thread. + std::thread thread; +}; + +////////////////////////////////////////////////// +ROSGzBridgeSystem::ROSGzBridgeSystem() +: System(), dataPtr(new ROSGzBridgeSystemPrivate()) +{ +} + +////////////////////////////////////////////////// +ROSGzBridgeSystem::~ROSGzBridgeSystem() +{ + if (this->dataPtr->exec) { + this->dataPtr->exec->cancel(); + this->dataPtr->thread.join(); + } +} + +////////////////////////////////////////////////// +void ROSGzBridgeSystem::Configure( + const gz::sim::Entity & /*_entity*/, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & /*_ecm*/, + gz::sim::EventManager & /*_eventMgr*/) +{ + // Ensure that ROS is setup. + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + if (!_sdf->HasElement("config_file")) { + std::cerr << "No found. Plugin disabled." << std::endl; + return; + } + + // Sanity check: Make sure that the config file exists and it's a file. + std::string filename = _sdf->Get("config_file"); + std::string path = gz::common::findFile(filename); + if (!gz::common::isFile(path)) { + std::cerr << "Unable to open YAML file [" << filename + << "], check your GZ_SIM_RESOURCE_PATH settings." << std::endl; + return; + } + + // Create the bridge passing the parameters as rclcpp::NodeOptions(). + this->dataPtr->bridge = std::make_shared( + rclcpp::NodeOptions().append_parameter_override("config_file", path)); + + // Create the executor. + this->dataPtr->exec = + std::make_shared(); + this->dataPtr->exec->add_node(this->dataPtr->bridge); + + // Spin in a separate thread to not block Gazebo. + this->dataPtr->thread = std::thread([this]() {this->dataPtr->exec->spin();}); +} +} // namespace ros_gz_sim + +GZ_ADD_PLUGIN( + ros_gz_sim::ROSGzBridgeSystem, + gz::sim::System, + ros_gz_sim::ROSGzBridgeSystem::ISystemConfigure) + +GZ_ADD_PLUGIN_ALIAS( + ros_gz_sim::ROSGzBridgeSystem, + "ros_gz_sim::ROSGzBridge") diff --git a/ros_gz_sim_demos/config/full.yaml b/ros_gz_sim_demos/config/full.yaml new file mode 100644 index 00000000..5d473824 --- /dev/null +++ b/ros_gz_sim_demos/config/full.yaml @@ -0,0 +1,18 @@ +# Full set of configurations +- ros_topic_name: "ros_chatter" + gz_topic_name: "gz_chatter" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "gz.msgs.StringMsg" + subscriber_queue: 5 + publisher_queue: 6 + lazy: true + direction: ROS_TO_GZ + +- ros_topic_name: "ros_chatter" + gz_topic_name: "gz_chatter" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "gz.msgs.StringMsg" + subscriber_queue: 10 + publisher_queue: 20 + lazy: false + direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/ros_gz.launch.py b/ros_gz_sim_demos/launch/ros_gz.launch.py new file mode 100644 index 00000000..c1c00938 --- /dev/null +++ b/ros_gz_sim_demos/launch/ros_gz.launch.py @@ -0,0 +1,42 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + + pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), + ), + launch_arguments={'gz_args': PathJoinSubstitution([ + pkg_ros_gz_sim_demos, + 'worlds', + 'ros_gz.sdf' + ])}.items(), + ) + + return LaunchDescription([ + gazebo, + ]) diff --git a/ros_gz_sim_demos/worlds/ros_gz.sdf b/ros_gz_sim_demos/worlds/ros_gz.sdf new file mode 100644 index 00000000..d8f772fe --- /dev/null +++ b/ros_gz_sim_demos/worlds/ros_gz.sdf @@ -0,0 +1,77 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + package://ros_gz_sim_demos/config/full.yaml + + + + From c2eb0729230be9705133952a9ade48527833288d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 20 Mar 2024 15:19:48 +0100 Subject: [PATCH 10/17] Includes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp index dc1e58de..b9e1fb73 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp @@ -16,6 +16,9 @@ #define ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ #include +#include +#include +#include #include #include From 78503427132107d91b397b17a2dc2dfd7bd86cd0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 22 Mar 2024 09:44:39 +0100 Subject: [PATCH 11/17] Use resource_retriever in the bridge (#515) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero Signed-off-by: Carlos Agüero Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Carlos Agüero --- ros_gz_bridge/CMakeLists.txt | 8 ++++ .../include/ros_gz_bridge/ros_gz_bridge.hpp | 3 ++ ros_gz_bridge/package.xml | 1 + ros_gz_bridge/src/bridge_config.cpp | 22 +++++------ ros_gz_bridge/src/ros_gz_bridge.cpp | 10 +++-- ros_gz_bridge/test/bridge_config.cpp | 37 +++++++++++-------- ros_gz_bridge/test/config/full.yaml | 4 +- ros_gz_bridge/test/config/invalid.yaml | 2 +- ros_gz_bridge/test/config/minimum.yaml | 8 ++-- ros_gz_sim/src/ros_gz_bridge_system.cpp | 9 +---- ros_gz_sim_demos/CMakeLists.txt | 6 --- ros_gz_sim_demos/config/full.yaml | 18 --------- ros_gz_sim_demos/worlds/ros_gz.sdf | 2 +- 13 files changed, 59 insertions(+), 71 deletions(-) delete mode 100644 ros_gz_sim_demos/config/full.yaml diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 6c2d4b3e..97e98a78 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(resource_retriever REQUIRED) find_package(yaml_cpp_vendor REQUIRED) if(NOT DEFINED ENV{GZ_VERSION}) @@ -120,6 +121,7 @@ add_library(${bridge_lib} target_link_libraries(${bridge_lib} ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core + resource_retriever::resource_retriever ) ament_target_dependencies(${bridge_lib} @@ -309,6 +311,11 @@ if(BUILD_TESTING) DESTINATION lib/${PROJECT_NAME} ) + install(DIRECTORY + test/config + DESTINATION share/${PROJECT_NAME}/test/ + ) + add_launch_test(test/launch/test_ros_subscriber.launch.py TIMEOUT 200 ARGS "gz_msgs_ver:=${GZ_MSGS_VERSION_FULL}" @@ -341,6 +348,7 @@ ament_export_targets(export_${PROJECT_NAME}) # specific order: dependents before dependencies ament_export_dependencies(rclcpp) ament_export_dependencies(rclcpp_components) +ament_export_dependencies(resource_retriever) ament_export_dependencies(${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}) ament_export_dependencies(${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}) ament_export_dependencies(yaml_cpp_vendor) diff --git a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp index 735cdecc..1e159507 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp @@ -68,6 +68,9 @@ class RosGzBridge : public rclcpp::Node /// \brief Timer to control periodic callback rclcpp::TimerBase::SharedPtr heartbeat_timer_; + + /// \brief Whether the config_file parameter was parsed or not. + bool config_file_parsed_; }; } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index cf626a38..fb32ea75 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -22,6 +22,7 @@ nav_msgs rclcpp rclcpp_components + resource_retriever ros_gz_interfaces rosgraph_msgs sensor_msgs diff --git a/ros_gz_bridge/src/bridge_config.cpp b/ros_gz_bridge/src/bridge_config.cpp index d15e0dd8..ac1fbb46 100644 --- a/ros_gz_bridge/src/bridge_config.cpp +++ b/ros_gz_bridge/src/bridge_config.cpp @@ -20,6 +20,7 @@ #include #include +#include #include namespace ros_gz_bridge @@ -173,31 +174,30 @@ std::vector readFromYaml(std::istream & in) std::vector readFromYamlFile(const std::string & filename) { std::vector ret; - std::ifstream in(filename); + resource_retriever::Retriever r; + resource_retriever::MemoryResource res; auto logger = rclcpp::get_logger("readFromYamlFile"); - if (!in.is_open()) { + + try { + res = r.get(filename); + } catch (const resource_retriever::Exception & exc) { RCLCPP_ERROR( logger, "Could not parse config: failed to open file [%s]", filename.c_str()); return ret; } - // Compute file size to warn on empty configuration - const auto fbegin = in.tellg(); - in.seekg(0, std::ios::end); - const auto fend = in.tellg(); - const auto fsize = fend - fbegin; - - if (fsize == 0) { + // Sanity check: Avoid empty configuration. + if (res.size == 0) { RCLCPP_ERROR( logger, "Could not parse config: file empty [%s]", filename.c_str()); return ret; } - in.seekg(0, std::ios::beg); - return readFromYaml(in); + std::string str(reinterpret_cast(res.data.get()), res.size); + return readFromYamlString(str); } std::vector readFromYamlString(const std::string & data) diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp index f542ee90..3b15bcca 100644 --- a/ros_gz_bridge/src/ros_gz_bridge.cpp +++ b/ros_gz_bridge/src/ros_gz_bridge.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include +#include + #include "bridge_handle_ros_to_gz.hpp" #include "bridge_handle_gz_to_ros.hpp" @@ -24,7 +24,8 @@ namespace ros_gz_bridge { RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options) -: rclcpp::Node("ros_gz_bridge", options) +: rclcpp::Node("ros_gz_bridge", options), + config_file_parsed_(false) { gz_node_ = std::make_shared(); @@ -43,7 +44,8 @@ void RosGzBridge::spin() if (handles_.empty()) { std::string config_file; this->get_parameter("config_file", config_file); - if (!config_file.empty()) { + if (!config_file.empty() && !config_file_parsed_) { + config_file_parsed_ = true; auto entries = readFromYamlFile(config_file); for (const auto & entry : entries) { this->add_bridge(entry); diff --git a/ros_gz_bridge/test/bridge_config.cpp b/ros_gz_bridge/test/bridge_config.cpp index 07987865..ac192abf 100644 --- a/ros_gz_bridge/test/bridge_config.cpp +++ b/ros_gz_bridge/test/bridge_config.cpp @@ -70,7 +70,8 @@ class BridgeConfig : public ::testing::Test TEST_F(BridgeConfig, Minimum) { - auto results = ros_gz_bridge::readFromYamlFile("test/config/minimum.yaml"); + auto results = ros_gz_bridge::readFromYamlFile( + "package://ros_gz_bridge/test/config/minimum.yaml"); EXPECT_EQ(4u, results.size()); { @@ -78,7 +79,7 @@ TEST_F(BridgeConfig, Minimum) EXPECT_EQ("chatter", config.ros_topic_name); EXPECT_EQ("chatter", config.gz_topic_name); EXPECT_EQ("std_msgs/msg/String", config.ros_type_name); - EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name); + EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name); EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy); @@ -88,7 +89,7 @@ TEST_F(BridgeConfig, Minimum) EXPECT_EQ("chatter_ros", config.ros_topic_name); EXPECT_EQ("chatter_ros", config.gz_topic_name); EXPECT_EQ("std_msgs/msg/String", config.ros_type_name); - EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name); + EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name); EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy); @@ -98,7 +99,7 @@ TEST_F(BridgeConfig, Minimum) EXPECT_EQ("chatter_gz", config.ros_topic_name); EXPECT_EQ("chatter_gz", config.gz_topic_name); EXPECT_EQ("std_msgs/msg/String", config.ros_type_name); - EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name); + EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name); EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy); @@ -108,7 +109,7 @@ TEST_F(BridgeConfig, Minimum) EXPECT_EQ("chatter_both_ros", config.ros_topic_name); EXPECT_EQ("chatter_both_gz", config.gz_topic_name); EXPECT_EQ("std_msgs/msg/String", config.ros_type_name); - EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name); + EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name); EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size); EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy); @@ -117,7 +118,8 @@ TEST_F(BridgeConfig, Minimum) TEST_F(BridgeConfig, FullGz) { - auto results = ros_gz_bridge::readFromYamlFile("test/config/full.yaml"); + auto results = ros_gz_bridge::readFromYamlFile( + "package://ros_gz_bridge/test/config/full.yaml"); EXPECT_EQ(2u, results.size()); { @@ -125,7 +127,7 @@ TEST_F(BridgeConfig, FullGz) EXPECT_EQ("ros_chatter", config.ros_topic_name); EXPECT_EQ("gz_chatter", config.gz_topic_name); EXPECT_EQ("std_msgs/msg/String", config.ros_type_name); - EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name); + EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name); EXPECT_EQ(6u, config.publisher_queue_size); EXPECT_EQ(5u, config.subscriber_queue_size); EXPECT_EQ(true, config.is_lazy); @@ -137,7 +139,7 @@ TEST_F(BridgeConfig, FullGz) EXPECT_EQ("ros_chatter", config.ros_topic_name); EXPECT_EQ("gz_chatter", config.gz_topic_name); EXPECT_EQ("std_msgs/msg/String", config.ros_type_name); - EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name); + EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name); EXPECT_EQ(20u, config.publisher_queue_size); EXPECT_EQ(10u, config.subscriber_queue_size); EXPECT_EQ(false, config.is_lazy); @@ -195,7 +197,7 @@ TEST_F(BridgeConfig, ParseDirection) R"( - topic_name: foo ros_type_name: std_msgs/msg/String - gz_type_name: ignition.msgs.StringMsg)"; + gz_type_name: gz.msgs.StringMsg)"; auto results = ros_gz_bridge::readFromYamlString(yaml); EXPECT_EQ(ros_gz_bridge::BridgeDirection::BIDIRECTIONAL, results[0].direction); @@ -206,7 +208,7 @@ TEST_F(BridgeConfig, ParseDirection) R"( - topic_name: foo ros_type_name: std_msgs/msg/String - gz_type_name: ignition.msgs.StringMsg + gz_type_name: gz.msgs.StringMsg direction: BIDIRECTIONAL )"; @@ -219,7 +221,7 @@ TEST_F(BridgeConfig, ParseDirection) R"( - topic_name: foo ros_type_name: std_msgs/msg/String - gz_type_name: ignition.msgs.StringMsg + gz_type_name: gz.msgs.StringMsg direction: ROS_TO_GZ )"; @@ -232,7 +234,7 @@ TEST_F(BridgeConfig, ParseDirection) R"( - topic_name: foo ros_type_name: std_msgs/msg/String - gz_type_name: ignition.msgs.StringMsg + gz_type_name: gz.msgs.StringMsg direction: GZ_TO_ROS )"; @@ -246,7 +248,7 @@ TEST_F(BridgeConfig, ParseDirection) R"( - topic_name: foo ros_type_name: std_msgs/msg/String - gz_type_name: ignition.msgs.StringMsg + gz_type_name: gz.msgs.StringMsg direction: foobar )"; @@ -267,7 +269,8 @@ TEST_F(BridgeConfig, InvalidFileDoesntExist) TEST_F(BridgeConfig, InvalidTopLevel) { - auto results = ros_gz_bridge::readFromYamlFile("test/config/invalid.yaml"); + auto results = ros_gz_bridge::readFromYamlFile( + "package://ros_gz_bridge/test/config/invalid.yaml"); EXPECT_EQ(0u, results.size()); EXPECT_EQ( "Could not parse config: top level must be a YAML sequence", @@ -276,9 +279,11 @@ TEST_F(BridgeConfig, InvalidTopLevel) TEST_F(BridgeConfig, EmptyYAML) { - auto results = ros_gz_bridge::readFromYamlFile("test/config/empty.yaml"); + auto results = ros_gz_bridge::readFromYamlFile( + "package://ros_gz_bridge/test/config/empty.yaml"); EXPECT_EQ(0u, results.size()); EXPECT_EQ( - "Could not parse config: file empty [test/config/empty.yaml]", + "Could not parse config: file empty [" + "package://ros_gz_bridge/test/config/empty.yaml]", g_last_log_event.message); } diff --git a/ros_gz_bridge/test/config/full.yaml b/ros_gz_bridge/test/config/full.yaml index ac838101..5d473824 100644 --- a/ros_gz_bridge/test/config/full.yaml +++ b/ros_gz_bridge/test/config/full.yaml @@ -2,7 +2,7 @@ - ros_topic_name: "ros_chatter" gz_topic_name: "gz_chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" subscriber_queue: 5 publisher_queue: 6 lazy: true @@ -11,7 +11,7 @@ - ros_topic_name: "ros_chatter" gz_topic_name: "gz_chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" subscriber_queue: 10 publisher_queue: 20 lazy: false diff --git a/ros_gz_bridge/test/config/invalid.yaml b/ros_gz_bridge/test/config/invalid.yaml index c14740ad..d1e4cbeb 100644 --- a/ros_gz_bridge/test/config/invalid.yaml +++ b/ros_gz_bridge/test/config/invalid.yaml @@ -1,7 +1,7 @@ ros_topic_name: "ros_chatter" gz_topic_name: "gz_chatter" ros_type_name: "std_msgs/msg/String" -gz_type_name: "ignition.msgs.StringMsg" +gz_type_name: "gz.msgs.StringMsg" subscriber_queue: 5 publisher_queue: 6 lazy: true diff --git a/ros_gz_bridge/test/config/minimum.yaml b/ros_gz_bridge/test/config/minimum.yaml index e0a6b1ca..0d5ef30e 100644 --- a/ros_gz_bridge/test/config/minimum.yaml +++ b/ros_gz_bridge/test/config/minimum.yaml @@ -1,20 +1,20 @@ # Set just topic name, applies to both - topic_name: "chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Set just ROS topic name, applies to both - ros_topic_name: "chatter_ros" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Set just GZ topic name, applies to both - gz_topic_name: "chatter_gz" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Set each topic name explicitly - ros_topic_name: "chatter_both_ros" gz_topic_name: "chatter_both_gz" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" diff --git a/ros_gz_sim/src/ros_gz_bridge_system.cpp b/ros_gz_sim/src/ros_gz_bridge_system.cpp index 04a53344..d01880a1 100644 --- a/ros_gz_sim/src/ros_gz_bridge_system.cpp +++ b/ros_gz_sim/src/ros_gz_bridge_system.cpp @@ -73,18 +73,11 @@ void ROSGzBridgeSystem::Configure( return; } - // Sanity check: Make sure that the config file exists and it's a file. std::string filename = _sdf->Get("config_file"); - std::string path = gz::common::findFile(filename); - if (!gz::common::isFile(path)) { - std::cerr << "Unable to open YAML file [" << filename - << "], check your GZ_SIM_RESOURCE_PATH settings." << std::endl; - return; - } // Create the bridge passing the parameters as rclcpp::NodeOptions(). this->dataPtr->bridge = std::make_shared( - rclcpp::NodeOptions().append_parameter_override("config_file", path)); + rclcpp::NodeOptions().append_parameter_override("config_file", filename)); // Create the executor. this->dataPtr->exec = diff --git a/ros_gz_sim_demos/CMakeLists.txt b/ros_gz_sim_demos/CMakeLists.txt index d0626157..8e0f580a 100644 --- a/ros_gz_sim_demos/CMakeLists.txt +++ b/ros_gz_sim_demos/CMakeLists.txt @@ -33,12 +33,6 @@ install( DESTINATION share/${PROJECT_NAME}/worlds ) -install( - DIRECTORY - config/ - DESTINATION share/${PROJECT_NAME}/config -) - ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") ament_package() diff --git a/ros_gz_sim_demos/config/full.yaml b/ros_gz_sim_demos/config/full.yaml deleted file mode 100644 index 5d473824..00000000 --- a/ros_gz_sim_demos/config/full.yaml +++ /dev/null @@ -1,18 +0,0 @@ -# Full set of configurations -- ros_topic_name: "ros_chatter" - gz_topic_name: "gz_chatter" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" - subscriber_queue: 5 - publisher_queue: 6 - lazy: true - direction: ROS_TO_GZ - -- ros_topic_name: "ros_chatter" - gz_topic_name: "gz_chatter" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" - subscriber_queue: 10 - publisher_queue: 20 - lazy: false - direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/worlds/ros_gz.sdf b/ros_gz_sim_demos/worlds/ros_gz.sdf index d8f772fe..7aa37830 100644 --- a/ros_gz_sim_demos/worlds/ros_gz.sdf +++ b/ros_gz_sim_demos/worlds/ros_gz.sdf @@ -70,7 +70,7 @@ - package://ros_gz_sim_demos/config/full.yaml + package://ros_gz_bridge/test/config/full.yaml From 53ef851b5bd5686acec103f841ec3c80fb23bda0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 22 Mar 2024 18:15:45 +0100 Subject: [PATCH 12/17] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz_bridge_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_gz_sim/src/ros_gz_bridge_system.cpp b/ros_gz_sim/src/ros_gz_bridge_system.cpp index d01880a1..8d79c822 100644 --- a/ros_gz_sim/src/ros_gz_bridge_system.cpp +++ b/ros_gz_sim/src/ros_gz_bridge_system.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include +#include #include #include #include @@ -69,7 +69,7 @@ void ROSGzBridgeSystem::Configure( } if (!_sdf->HasElement("config_file")) { - std::cerr << "No found. Plugin disabled." << std::endl; + gzerr << "No found. Plugin disabled." << std::endl; return; } From 139f655ff263ad0ad2d22301af0b6b12b05c86bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 25 Mar 2024 20:55:40 +0100 Subject: [PATCH 13/17] ign to gz (#519) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Signed-off-by: Carlos Agüero --- README.md | 2 +- README_RENAME.md | 2 +- ros_gz_bridge/README.md | 22 ++++++++++---------- ros_gz_image/README.md | 2 +- ros_gz_point_cloud/examples/depth_camera.sdf | 2 +- ros_gz_point_cloud/examples/gpu_lidar.sdf | 2 +- ros_gz_point_cloud/examples/rgbd_camera.sdf | 2 +- ros_gz_sim/README.md | 2 +- 8 files changed, 18 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 6affa43d..8346b53b 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ Rolling | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | 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 -* ROS 2 Jazzy Jalisco is slated for release on May 23rd, 2024. [Full ROS 2 release information is available in REP-2000.] +* 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) diff --git a/README_RENAME.md b/README_RENAME.md index 17007f4e..92ba754f 100644 --- a/README_RENAME.md +++ b/README_RENAME.md @@ -7,7 +7,7 @@ This allows users to do either of these and get equivalent behavior: ```bash ros2 run ros_gz parameter_bridge [...] -ros2 run ros_ign parameter_bridge [...] # Will emit deprecation warning +ros2 run ros_gz parameter_bridge [...] # Will emit deprecation warning ``` Additionally, installed files like launch files, message interfaces etc. are **duplicated** versions of the ones in `ros_gz` (but renamed as appropriate), and point to `ros_gz` dependencies as well (e.g. launch files pointing to `ros_gz` nodes.) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 7a911460..dee6bdd8 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -110,7 +110,7 @@ Now we start the Gazebo Transport talker. ``` # Shell C: -ign topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"' +gz topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"' ``` ## Example 1b: ROS 2 talker and Gazebo Transport listener @@ -127,7 +127,7 @@ Now we start the Gazebo Transport listener. ``` # Shell B: -ign topic -e -t /chatter +gz topic -e -t /chatter ``` Now we start the ROS talker. @@ -148,14 +148,14 @@ First we start Gazebo Sim (don't forget to hit play, or Gazebo Sim won't generat ``` # Shell A: -ign gazebo sensors_demo.sdf +gz sim sensors_demo.sdf ``` Let's see the topic where camera images are published. ``` # Shell B: -ign topic -l | grep image +gz topic -l | grep image /rgbd_camera/depth_image /rgbd_camera/image ``` @@ -206,15 +206,15 @@ On terminal B, we start a ROS 2 listener: And terminal C, publish an Gazebo message: -`ign topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"'` +`gz topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"'` At this point, you should see the ROS 2 listener echoing the message. Now let's try the other way around, ROS 2 -> Gazebo. -On terminal D, start an Igntion listener: +On terminal D, start an Gazebo listener: -`ign topic -e -t /chatter` +`gz topic -e -t /chatter` And on terminal E, publish a ROS 2 message: @@ -232,7 +232,7 @@ On terminal A, start the service bridge: On terminal B, start Gazebo, it will be paused by default: -`ign gazebo shapes.sdf` +`gz sim shapes.sdf` On terminal C, make a ROS request to unpause simulation: @@ -262,19 +262,19 @@ bridge may be specified: gz_type_name: "gz.msgs.StringMsg" # Set just GZ topic name, applies to both -- gz_topic_name: "chatter_ign" +- gz_topic_name: "chatter_gz" ros_type_name: "std_msgs/msg/String" gz_type_name: "gz.msgs.StringMsg" # Set each topic name explicitly - ros_topic_name: "chatter_both_ros" - gz_topic_name: "chatter_both_ign" + gz_topic_name: "chatter_both_gz" ros_type_name: "std_msgs/msg/String" gz_type_name: "gz.msgs.StringMsg" # Full set of configurations - ros_topic_name: "ros_chatter" - gz_topic_name: "ign_chatter" + gz_topic_name: "gz_chatter" ros_type_name: "std_msgs/msg/String" gz_type_name: "gz.msgs.StringMsg" subscriber_queue: 5 # Default 10 diff --git a/ros_gz_image/README.md b/ros_gz_image/README.md index 99db4da1..a6c9c5e5 100644 --- a/ros_gz_image/README.md +++ b/ros_gz_image/README.md @@ -1,7 +1,7 @@ # Image utilities for using ROS and Gazebo Transport This package provides a unidirectional bridge for images from Gazebo to ROS. -The bridge subscribes to Gazebo image messages (`ignition::msgs::Image`) +The bridge subscribes to Gazebo image messages (`gz::msgs::Image`) and republishes them to ROS using [image_transport](http://wiki.ros.org/image_transport). For compressed images, install diff --git a/ros_gz_point_cloud/examples/depth_camera.sdf b/ros_gz_point_cloud/examples/depth_camera.sdf index 574d0ce4..85beba43 100644 --- a/ros_gz_point_cloud/examples/depth_camera.sdf +++ b/ros_gz_point_cloud/examples/depth_camera.sdf @@ -15,7 +15,7 @@ 3. Load the example world, unpaused: - ign gazebo -r examples/depth_camera.sdf + gz sim -r examples/depth_camera.sdf 4. Launch RViz to visualize the point cloud: diff --git a/ros_gz_point_cloud/examples/gpu_lidar.sdf b/ros_gz_point_cloud/examples/gpu_lidar.sdf index 7bd3a735..3feed57a 100644 --- a/ros_gz_point_cloud/examples/gpu_lidar.sdf +++ b/ros_gz_point_cloud/examples/gpu_lidar.sdf @@ -15,7 +15,7 @@ 3. Load this world, unpaused: - ign gazebo -r examples/gpu_lidar.sdf + gz sim -r examples/gpu_lidar.sdf 4. Launch RViz to visualize the point cloud: diff --git a/ros_gz_point_cloud/examples/rgbd_camera.sdf b/ros_gz_point_cloud/examples/rgbd_camera.sdf index 64a77b6a..235cb225 100644 --- a/ros_gz_point_cloud/examples/rgbd_camera.sdf +++ b/ros_gz_point_cloud/examples/rgbd_camera.sdf @@ -16,7 +16,7 @@ 3. Load the example world, unpaused: - ign gazebo -r examples/rgbd_camera.sdf + gz sim -r examples/rgbd_camera.sdf 4. Launch RViz to visualize the point cloud: diff --git a/ros_gz_sim/README.md b/ros_gz_sim/README.md index c4561a5d..227d53a4 100644 --- a/ros_gz_sim/README.md +++ b/ros_gz_sim/README.md @@ -29,7 +29,7 @@ ros2 launch ros_gz_sim gz_sim.launch.py then spawn a model: ``` -ros2 run ros_gz_sim create -world default -file 'https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Gazebo' +ros2 run ros_gz_sim create -world default -file 'https://fuel.gazebosim.org/1.0/openrobotics/models/Gazebo' ``` See more options with: From 70d50df3b856ca742aa2d1ed362121b986671ee6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 27 Mar 2024 09:50:41 +0100 Subject: [PATCH 14/17] Remove iron CI from rolling (#518) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Signed-off-by: Carlos Agüero --- .github/workflows/ros2-ci.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ros2-ci.yml b/.github/workflows/ros2-ci.yml index aa092b73..e06727b6 100644 --- a/.github/workflows/ros2-ci.yml +++ b/.github/workflows/ros2-ci.yml @@ -10,13 +10,10 @@ jobs: fail-fast: false matrix: include: - - docker-image: "ubuntu:22.04" - gz-version: "garden" - ros-distro: "iron" - - docker-image: "ubuntu:22.04" + - docker-image: "ubuntu:24.04" gz-version: "garden" ros-distro: "rolling" - - docker-image: "ubuntu:22.04" + - docker-image: "ubuntu:24.04" gz-version: "harmonic" ros-distro: "rolling" container: From 022d4c69df4f580015a5a6eb35e80e6509ec4f4b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 29 Mar 2024 12:37:58 -0500 Subject: [PATCH 15/17] Keep rolling on Jammy and apply a temporary fix for rosdep (#522) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Pins rosdep to a known good version based on https://wiki.ros.org/SnapshotRepository --------- Signed-off-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- .github/workflows/ros2-ci.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ros2-ci.yml b/.github/workflows/ros2-ci.yml index e06727b6..1732131e 100644 --- a/.github/workflows/ros2-ci.yml +++ b/.github/workflows/ros2-ci.yml @@ -10,25 +10,26 @@ jobs: fail-fast: false matrix: include: - - docker-image: "ubuntu:24.04" + - docker-image: "ubuntu:22.04" gz-version: "garden" ros-distro: "rolling" - - docker-image: "ubuntu:24.04" + - docker-image: "ubuntu:22.04" gz-version: "harmonic" ros-distro: "rolling" container: image: ${{ matrix.docker-image }} steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Build and Test run: .github/workflows/build-and-test.sh env: DOCKER_IMAGE: ${{ matrix.docker-image }} GZ_VERSION: ${{ matrix.gz-version }} ROS_DISTRO: ${{ matrix.ros-distro }} + ROSDISTRO_INDEX_URL: https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml # Temporarily pin rosdep to fix rolling on Jammy - name: Build sdformat_urdf from source - uses: actions/checkout@v2 + uses: actions/checkout@v4 if: ${{ matrix.gz-version }} == "garden" with: repository: ros/sdformat_urdf From 13f40087d99b92c5bc4c8b157a951aad1efc0d9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 29 Mar 2024 22:17:00 +0100 Subject: [PATCH 16/17] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- .../include/ros_gz_bridge/ros_gz_bridge.hpp | 2 +- ros_gz_bridge/src/bridge_config.cpp | 23 +++++++++++++++++-- ros_gz_bridge/src/ros_gz_bridge.cpp | 3 +-- ros_gz_bridge/test/bridge_config.cpp | 5 ++-- .../ros_gz_bridge_system.hpp | 0 5 files changed, 25 insertions(+), 8 deletions(-) rename ros_gz_sim/{include/ros_gz_sim => src}/ros_gz_bridge_system.hpp (100%) diff --git a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp index 1e159507..8e93a271 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp @@ -70,7 +70,7 @@ class RosGzBridge : public rclcpp::Node rclcpp::TimerBase::SharedPtr heartbeat_timer_; /// \brief Whether the config_file parameter was parsed or not. - bool config_file_parsed_; + bool config_file_parsed_{false}; }; } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/src/bridge_config.cpp b/ros_gz_bridge/src/bridge_config.cpp index ac1fbb46..d83cc7a9 100644 --- a/ros_gz_bridge/src/bridge_config.cpp +++ b/ros_gz_bridge/src/bridge_config.cpp @@ -174,13 +174,32 @@ std::vector readFromYaml(std::istream & in) std::vector readFromYamlFile(const std::string & filename) { std::vector ret; - resource_retriever::Retriever r; + std::ifstream in(filename); + resource_retriever::Retriever retriever; resource_retriever::MemoryResource res; auto logger = rclcpp::get_logger("readFromYamlFile"); + if (in.is_open()) { + // Compute file size to warn on empty configuration + const auto fbegin = in.tellg(); + in.seekg(0, std::ios::end); + const auto fend = in.tellg(); + const auto fsize = fend - fbegin; + + if (fsize == 0) { + RCLCPP_ERROR( + logger, + "Could not parse config: file empty [%s]", filename.c_str()); + return ret; + } + + in.seekg(0, std::ios::beg); + return readFromYaml(in); + } + try { - res = r.get(filename); + res = retriever.get(filename); } catch (const resource_retriever::Exception & exc) { RCLCPP_ERROR( logger, diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp index 3b15bcca..2f712e87 100644 --- a/ros_gz_bridge/src/ros_gz_bridge.cpp +++ b/ros_gz_bridge/src/ros_gz_bridge.cpp @@ -24,8 +24,7 @@ namespace ros_gz_bridge { RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options) -: rclcpp::Node("ros_gz_bridge", options), - config_file_parsed_(false) +: rclcpp::Node("ros_gz_bridge", options) { gz_node_ = std::make_shared(); diff --git a/ros_gz_bridge/test/bridge_config.cpp b/ros_gz_bridge/test/bridge_config.cpp index ac192abf..9d261be5 100644 --- a/ros_gz_bridge/test/bridge_config.cpp +++ b/ros_gz_bridge/test/bridge_config.cpp @@ -70,9 +70,8 @@ class BridgeConfig : public ::testing::Test TEST_F(BridgeConfig, Minimum) { - auto results = ros_gz_bridge::readFromYamlFile( - "package://ros_gz_bridge/test/config/minimum.yaml"); - EXPECT_EQ(4u, results.size()); + auto results = ros_gz_bridge::readFromYamlFile("test/config/minimum.yaml"); + ASSERT_EQ(4u, results.size()); { auto config = results[0]; diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp b/ros_gz_sim/src/ros_gz_bridge_system.hpp similarity index 100% rename from ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp rename to ros_gz_sim/src/ros_gz_bridge_system.hpp From 7d9b91d1c4a7167f2b42361bf3fd9b6a019614ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 29 Mar 2024 22:35:03 +0100 Subject: [PATCH 17/17] ifdef MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz_bridge_system.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros_gz_sim/src/ros_gz_bridge_system.hpp b/ros_gz_sim/src/ros_gz_bridge_system.hpp index b9e1fb73..4b7e0cf3 100644 --- a/ros_gz_sim/src/ros_gz_bridge_system.hpp +++ b/ros_gz_sim/src/ros_gz_bridge_system.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ -#define ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ +#ifndef ROS_GZ_BRIDGE_SYSTEM_HPP_ +#define ROS_GZ_BRIDGE_SYSTEM_HPP_ #include #include @@ -57,4 +57,4 @@ class ROSGzBridgeSystem std::unique_ptr dataPtr; }; } // namespace ros_gz_sim -#endif // ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_ +#endif // ROS_GZ_BRIDGE_SYSTEM_HPP_