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..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
@@ -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_{false};
};
} // 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..d83cc7a9 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
@@ -174,30 +175,48 @@ std::vector readFromYamlFile(const std::string & filename)
{
std::vector ret;
std::ifstream in(filename);
+ resource_retriever::Retriever retriever;
+ resource_retriever::MemoryResource res;
auto logger = rclcpp::get_logger("readFromYamlFile");
- if (!in.is_open()) {
+
+ 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 = retriever.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..2f712e87 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"
@@ -43,7 +43,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..9d261be5 100644
--- a/ros_gz_bridge/test/bridge_config.cpp
+++ b/ros_gz_bridge/test/bridge_config.cpp
@@ -71,14 +71,14 @@ class BridgeConfig : public ::testing::Test
TEST_F(BridgeConfig, Minimum)
{
auto results = ros_gz_bridge::readFromYamlFile("test/config/minimum.yaml");
- EXPECT_EQ(4u, results.size());
+ ASSERT_EQ(4u, results.size());
{
auto config = results[0];
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 +88,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 +98,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 +108,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 +117,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 +126,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 +138,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 +196,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 +207,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 +220,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 +233,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 +247,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 +268,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 +278,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/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/src/ros_gz_bridge_system.cpp b/ros_gz_sim/src/ros_gz_bridge_system.cpp
new file mode 100644
index 00000000..8d79c822
--- /dev/null
+++ b/ros_gz_sim/src/ros_gz_bridge_system.cpp
@@ -0,0 +1,99 @@
+// 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")) {
+ gzerr << "No found. Plugin disabled." << std::endl;
+ return;
+ }
+
+ std::string filename = _sdf->Get("config_file");
+
+ // Create the bridge passing the parameters as rclcpp::NodeOptions().
+ this->dataPtr->bridge = std::make_shared(
+ rclcpp::NodeOptions().append_parameter_override("config_file", filename));
+
+ // 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/src/ros_gz_bridge_system.hpp b/ros_gz_sim/src/ros_gz_bridge_system.hpp
new file mode 100644
index 00000000..4b7e0cf3
--- /dev/null
+++ b/ros_gz_sim/src/ros_gz_bridge_system.hpp
@@ -0,0 +1,60 @@
+// 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_BRIDGE_SYSTEM_HPP_
+#define ROS_GZ_BRIDGE_SYSTEM_HPP_
+
+#include
+#include
+#include
+#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_BRIDGE_SYSTEM_HPP_
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
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..7aa37830
--- /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_bridge/test/config/full.yaml
+
+
+
+