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 + + + +