From 21d4c2317bbaa1d73260257e27fd61f20eb13ea1 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 21 Feb 2024 15:10:28 -0600 Subject: [PATCH 01/20] Add gzserver with ability to load an SDF file or string Signed-off-by: Addisu Z. Taddese --- ros_gz_sim/CMakeLists.txt | 14 ++++++++ ros_gz_sim/src/gzserver.cpp | 68 +++++++++++++++++++++++++++++++++++++ 2 files changed, 82 insertions(+) create mode 100644 ros_gz_sim/src/gzserver.cpp diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 7f420e09..ec0ed4c8 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -86,6 +86,16 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) +add_executable(gzserver src/gzserver.cpp) +ament_target_dependencies(gzserver + rclcpp + std_msgs +) +target_link_libraries(gzserver + ${GZ_TARGET_PREFIX}-sim${GZ_SIM_VER}::core +) +ament_target_dependencies(gzserver std_msgs) + configure_file( launch/gz_sim.launch.py.in launch/gz_sim.launch.py.configured @@ -105,6 +115,10 @@ install(TARGETS create DESTINATION lib/${PROJECT_NAME} ) +install(TARGETS + gzserver + DESTINATION lib/${PROJECT_NAME} +) install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp new file mode 100644 index 00000000..3cdd43c9 --- /dev/null +++ b/ros_gz_sim/src/gzserver.cpp @@ -0,0 +1,68 @@ +// 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 + +int main(int _argc, char ** _argv) +{ + using namespace gz; + auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); + auto node = rclcpp::Node::make_shared("gzserver"); + auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); + auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); + + common::Console::SetVerbosity(4); + sim::ServerConfig server_config; + + + if (!world_sdf_file.empty()) + { + server_config.SetSdfFile(world_sdf_file); + } + else if (!world_sdf_string.empty()) { + server_config.SetSdfString(world_sdf_string); + } + else { + RCLCPP_ERROR( + node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + return -1; + } + + // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. + + // + // + sdf::Plugin imu_sdf_plugin; + imu_sdf_plugin.SetName("gz::sim::systems::Imu"); + imu_sdf_plugin.SetFilename("gz-sim-imu-system"); + sim::SystemLoader loader; + auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); + std::cout << imu_plugin.value() << std::endl; + + gz::sim::Server server(server_config); + server.RunOnce(); + if (!server.AddSystem(*imu_plugin)) + { + RCLCPP_ERROR( + node->get_logger(), "IMU system not added"); + + } + server.Run(true /*blocking*/, 0, false /*paused*/); +} From 3a5e0998caaabd2d9f81dff3dc24a88080e18dfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 3 Apr 2024 20:56:30 +0200 Subject: [PATCH 02/20] Testing components. 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 | 36 +++- .../launch/gz_sim_composition.launch.py | 61 +++++++ ros_gz_sim/package.xml | 1 + ros_gz_sim/src/gzserver.cpp | 158 +++++++++++++----- 4 files changed, 207 insertions(+), 49 deletions(-) create mode 100644 ros_gz_sim/launch/gz_sim_composition.launch.py diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index ec0ed4c8..f32a4262 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) if(NOT DEFINED ENV{GZ_VERSION}) @@ -86,15 +87,23 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) -add_executable(gzserver src/gzserver.cpp) -ament_target_dependencies(gzserver +add_library(gzserver_component SHARED src/gzserver.cpp) +rclcpp_components_register_nodes(gzserver_component "ros_gz_sim::GzServer") +#add_executable(gzserver src/gzserver.cpp) +ament_target_dependencies(gzserver_component rclcpp + rclcpp_components std_msgs ) -target_link_libraries(gzserver +target_link_libraries(gzserver_component ${GZ_TARGET_PREFIX}-sim${GZ_SIM_VER}::core ) -ament_target_dependencies(gzserver std_msgs) +ament_target_dependencies(gzserver_component std_msgs) +rclcpp_components_register_node( + gzserver_component + PLUGIN "ros_gz_sim::GzServer" + EXECUTABLE gzserver +) configure_file( launch/gz_sim.launch.py.in @@ -111,14 +120,27 @@ install(FILES DESTINATION share/${PROJECT_NAME}/launch ) +install(FILES + "launch/gz_sim_composition.launch.py" + DESTINATION share/${PROJECT_NAME}/launch +) + install(TARGETS create DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS - gzserver - DESTINATION lib/${PROJECT_NAME} + +ament_export_targets(export_gzserver_component) +install(TARGETS gzserver_component + EXPORT export_gzserver_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) +#install(TARGETS +# gzserver +# DESTINATION lib/${PROJECT_NAME} +#) install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/ros_gz_sim/launch/gz_sim_composition.launch.py b/ros_gz_sim/launch/gz_sim_composition.launch.py new file mode 100644 index 00000000..ae822c8d --- /dev/null +++ b/ros_gz_sim/launch/gz_sim_composition.launch.py @@ -0,0 +1,61 @@ + +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch gzsim in a component container.""" + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + world_sdf_file = LaunchConfiguration('world_sdf_file') + world_sdf_string = LaunchConfiguration('world_sdf_string') + + world_sdf_file_arg = DeclareLaunchArgument( + 'world_sdf_file', + default_value='' + ) + + world_sdf_string_arg = DeclareLaunchArgument( + 'world_sdf_string', + default_value='' + ) + + """Generate launch description with multiple components.""" + container = ComposableNodeContainer( + name='my_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='ros_gz_sim', + plugin='ros_gz_sim::GzServer', + name='gzserver', + parameters=[{'world_sdf_file': world_sdf_file}, + {'world_sdf_string': world_sdf_string}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + output='screen', + ) + + return launch.LaunchDescription([container, + world_sdf_file_arg, + world_sdf_string_arg]) diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index a4cad906..2a13ce66 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -17,6 +17,7 @@ ament_index_python libgflags-dev rclcpp + rclcpp_components std_msgs diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 3cdd43c9..17fdb8b7 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -17,52 +17,126 @@ #include #include #include +#include -int main(int _argc, char ** _argv) +namespace ros_gz_sim { - using namespace gz; - auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); - auto node = rclcpp::Node::make_shared("gzserver"); - auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); - auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); + class GzServer : public rclcpp::Node + { + // Class constructor. + public: GzServer(const rclcpp::NodeOptions & options) + : Node("gzserver", options) + { + auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); + auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); - common::Console::SetVerbosity(4); - sim::ServerConfig server_config; + gz::common::Console::SetVerbosity(4); + gz::sim::ServerConfig server_config; + if (!world_sdf_file.empty()) + { + RCLCPP_ERROR(this->get_logger(), "world_sdf_file param used [%s]", world_sdf_file.c_str()); + server_config.SetSdfFile(world_sdf_file); + } + else if (!world_sdf_string.empty()) { + RCLCPP_ERROR(this->get_logger(), "world_sdf_string param used"); + server_config.SetSdfString(world_sdf_string); + } + else { + RCLCPP_ERROR( + this->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + return; + } - if (!world_sdf_file.empty()) - { - server_config.SetSdfFile(world_sdf_file); - } - else if (!world_sdf_string.empty()) { - server_config.SetSdfString(world_sdf_string); - } - else { - RCLCPP_ERROR( - node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); - return -1; - } - - // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. - - // - // - sdf::Plugin imu_sdf_plugin; - imu_sdf_plugin.SetName("gz::sim::systems::Imu"); - imu_sdf_plugin.SetFilename("gz-sim-imu-system"); - sim::SystemLoader loader; - auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); - std::cout << imu_plugin.value() << std::endl; - - gz::sim::Server server(server_config); - server.RunOnce(); - if (!server.AddSystem(*imu_plugin)) - { - RCLCPP_ERROR( - node->get_logger(), "IMU system not added"); + //thread_ = std::thread(std::bind(&GzServer::OnStart, this)); + } + + public: ~GzServer() + { + // Make sure to join the thread on shutdown. + if (thread_.joinable()) { + thread_.join(); + } + } - } - server.Run(true /*blocking*/, 0, false /*paused*/); + public: void OnStart() + { + + + // // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. + + // // + // // + // sdf::Plugin imu_sdf_plugin; + // imu_sdf_plugin.SetName("gz::sim::systems::Imu"); + // imu_sdf_plugin.SetFilename("gz-sim-imu-system"); + // gz::sim::SystemLoader loader; + // auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); + // std::cout << imu_plugin.value() << std::endl; + + // gz::sim::Server server(server_config); + // server.RunOnce(); + // if (!server.AddSystem(*imu_plugin)) + // { + // RCLCPP_ERROR(this->get_logger(), "IMU system not added"); + + // } + // server.Run(true /*blocking*/, 0, false /*paused*/); + } + + private: std::thread thread_; + }; } + +RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer) + +// int main(int _argc, char ** _argv) +// { +// using namespace gz; +// auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); +// auto node = rclcpp::Node::make_shared("gzserver"); +// auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); +// auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); + +// common::Console::SetVerbosity(4); +// sim::ServerConfig server_config; + + +// if (!world_sdf_file.empty()) +// { +// server_config.SetSdfFile(world_sdf_file); +// } +// else if (!world_sdf_string.empty()) { +// server_config.SetSdfString(world_sdf_string); +// } +// else { +// RCLCPP_ERROR( +// node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); +// return -1; +// } + +// // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. + +// // +// // +// sdf::Plugin imu_sdf_plugin; +// imu_sdf_plugin.SetName("gz::sim::systems::Imu"); +// imu_sdf_plugin.SetFilename("gz-sim-imu-system"); +// sim::SystemLoader loader; +// auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); +// std::cout << imu_plugin.value() << std::endl; + +// gz::sim::Server server(server_config); +// server.RunOnce(); +// if (!server.AddSystem(*imu_plugin)) +// { +// RCLCPP_ERROR( +// node->get_logger(), "IMU system not added"); + +// } +// server.Run(true /*blocking*/, 0, false /*paused*/); +// } From 1b3ba4e61e9ae5bbe52d7cf3552d6a0120306dcc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 4 Apr 2024 15:48:22 +0200 Subject: [PATCH 03/20] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- .../launch/gz_sim_composition.launch.py | 20 +----- ros_gz_sim/src/gzserver.cpp | 70 +++++++++---------- 2 files changed, 37 insertions(+), 53 deletions(-) diff --git a/ros_gz_sim/launch/gz_sim_composition.launch.py b/ros_gz_sim/launch/gz_sim_composition.launch.py index ae822c8d..0629bfd1 100644 --- a/ros_gz_sim/launch/gz_sim_composition.launch.py +++ b/ros_gz_sim/launch/gz_sim_composition.launch.py @@ -24,18 +24,7 @@ def generate_launch_description(): - world_sdf_file = LaunchConfiguration('world_sdf_file') - world_sdf_string = LaunchConfiguration('world_sdf_string') - - world_sdf_file_arg = DeclareLaunchArgument( - 'world_sdf_file', - default_value='' - ) - - world_sdf_string_arg = DeclareLaunchArgument( - 'world_sdf_string', - default_value='' - ) + world_sdf_file_param = LaunchConfiguration('world_sdf_file') """Generate launch description with multiple components.""" container = ComposableNodeContainer( @@ -48,14 +37,11 @@ def generate_launch_description(): package='ros_gz_sim', plugin='ros_gz_sim::GzServer', name='gzserver', - parameters=[{'world_sdf_file': world_sdf_file}, - {'world_sdf_string': world_sdf_string}], + parameters=[{'world_sdf_file': world_sdf_file_param}], extra_arguments=[{'use_intra_process_comms': True}], ), ], output='screen', ) - return launch.LaunchDescription([container, - world_sdf_file_arg, - world_sdf_string_arg]) + return launch.LaunchDescription([container,]) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 17fdb8b7..b1806286 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -26,6 +26,19 @@ namespace ros_gz_sim // Class constructor. public: GzServer(const rclcpp::NodeOptions & options) : Node("gzserver", options) + { + thread_ = std::thread(std::bind(&GzServer::OnStart, this)); + } + + public: ~GzServer() + { + // Make sure to join the thread on shutdown. + if (thread_.joinable()) { + thread_.join(); + } + } + + public: void OnStart() { auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); @@ -46,44 +59,29 @@ namespace ros_gz_sim RCLCPP_ERROR( this->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); return; - } - - //thread_ = std::thread(std::bind(&GzServer::OnStart, this)); - } - - public: ~GzServer() - { - // Make sure to join the thread on shutdown. - if (thread_.joinable()) { - thread_.join(); - } - } - - public: void OnStart() - { - - - // // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. - - // // - // // - // sdf::Plugin imu_sdf_plugin; - // imu_sdf_plugin.SetName("gz::sim::systems::Imu"); - // imu_sdf_plugin.SetFilename("gz-sim-imu-system"); - // gz::sim::SystemLoader loader; - // auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); - // std::cout << imu_plugin.value() << std::endl; + } + + // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. + + // + // + sdf::Plugin imu_sdf_plugin; + imu_sdf_plugin.SetName("gz::sim::systems::Imu"); + imu_sdf_plugin.SetFilename("gz-sim-imu-system"); + gz::sim::SystemLoader loader; + auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); + std::cout << imu_plugin.value() << std::endl; - // gz::sim::Server server(server_config); - // server.RunOnce(); - // if (!server.AddSystem(*imu_plugin)) - // { - // RCLCPP_ERROR(this->get_logger(), "IMU system not added"); + gz::sim::Server server(server_config); + server.RunOnce(); + if (!server.AddSystem(*imu_plugin)) + { + RCLCPP_ERROR(this->get_logger(), "IMU system not added"); - // } - // server.Run(true /*blocking*/, 0, false /*paused*/); + } + server.Run(true /*blocking*/, 0, false /*paused*/); } private: std::thread thread_; From 04255e3d60121c70b869ac2e0c4d03f20bbd8855 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 4 Apr 2024 18:57:45 +0200 Subject: [PATCH 04/20] 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/CMakeLists.txt | 2 +- ...osition.launch.py => ros_gz_sim.launch.py} | 29 ++++++++-- ros_gz_sim/src/gzserver.cpp | 56 ++----------------- 3 files changed, 31 insertions(+), 56 deletions(-) rename ros_gz_sim/launch/{gz_sim_composition.launch.py => ros_gz_sim.launch.py} (57%) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index f32a4262..db256cf0 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -121,7 +121,7 @@ install(FILES ) install(FILES - "launch/gz_sim_composition.launch.py" + "launch/ros_gz_sim.launch.py" DESTINATION share/${PROJECT_NAME}/launch ) diff --git a/ros_gz_sim/launch/gz_sim_composition.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py similarity index 57% rename from ros_gz_sim/launch/gz_sim_composition.launch.py rename to ros_gz_sim/launch/ros_gz_sim.launch.py index 0629bfd1..7c339b38 100644 --- a/ros_gz_sim/launch/gz_sim_composition.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -17,18 +17,27 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, TextSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode def generate_launch_description(): + world_sdf_file_arg = DeclareLaunchArgument( + 'world_sdf_file', default_value=TextSubstitution(text='')) + world_sdf_string_arg = DeclareLaunchArgument( + 'world_sdf_string', default_value=TextSubstitution(text='')) + config_file_arg = DeclareLaunchArgument( + 'config_file', default_value=TextSubstitution(text='')) + world_sdf_file_param = LaunchConfiguration('world_sdf_file') + world_sdf_string_param = LaunchConfiguration('world_sdf_string') + bridge_config_file_param = LaunchConfiguration('config_file') """Generate launch description with multiple components.""" container = ComposableNodeContainer( - name='my_container', + name='gz_sim_container', namespace='', package='rclcpp_components', executable='component_container', @@ -37,11 +46,23 @@ def generate_launch_description(): package='ros_gz_sim', plugin='ros_gz_sim::GzServer', name='gzserver', - parameters=[{'world_sdf_file': world_sdf_file_param}], + parameters=[{'world_sdf_file': world_sdf_file_param, + 'world_sdf_string': world_sdf_string_param}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( + package='ros_gz_bridge', + plugin='ros_gz_bridge::RosGzBridge', + name='bridge', + parameters=[{'config_file': bridge_config_file_param}], extra_arguments=[{'use_intra_process_comms': True}], ), ], output='screen', ) - return launch.LaunchDescription([container,]) + return launch.LaunchDescription([ + world_sdf_file_arg, + world_sdf_string_arg, + config_file_arg, + container,]) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index b1806286..c7783753 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include @@ -30,6 +32,7 @@ namespace ros_gz_sim thread_ = std::thread(std::bind(&GzServer::OnStart, this)); } + // Class destructor. public: ~GzServer() { // Make sure to join the thread on shutdown. @@ -38,6 +41,7 @@ namespace ros_gz_sim } } + /// \brief Run the gz sim server. public: void OnStart() { auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); @@ -48,11 +52,9 @@ namespace ros_gz_sim if (!world_sdf_file.empty()) { - RCLCPP_ERROR(this->get_logger(), "world_sdf_file param used [%s]", world_sdf_file.c_str()); server_config.SetSdfFile(world_sdf_file); } else if (!world_sdf_string.empty()) { - RCLCPP_ERROR(this->get_logger(), "world_sdf_string param used"); server_config.SetSdfString(world_sdf_string); } else { @@ -84,57 +86,9 @@ namespace ros_gz_sim server.Run(true /*blocking*/, 0, false /*paused*/); } + /// \brief We don't want to block the ROS thread. private: std::thread thread_; }; } RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer) - -// int main(int _argc, char ** _argv) -// { -// using namespace gz; -// auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); -// auto node = rclcpp::Node::make_shared("gzserver"); -// auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); -// auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); - -// common::Console::SetVerbosity(4); -// sim::ServerConfig server_config; - - -// if (!world_sdf_file.empty()) -// { -// server_config.SetSdfFile(world_sdf_file); -// } -// else if (!world_sdf_string.empty()) { -// server_config.SetSdfString(world_sdf_string); -// } -// else { -// RCLCPP_ERROR( -// node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); -// return -1; -// } - -// // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. - -// // -// // -// sdf::Plugin imu_sdf_plugin; -// imu_sdf_plugin.SetName("gz::sim::systems::Imu"); -// imu_sdf_plugin.SetFilename("gz-sim-imu-system"); -// sim::SystemLoader loader; -// auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); -// std::cout << imu_plugin.value() << std::endl; - -// gz::sim::Server server(server_config); -// server.RunOnce(); -// if (!server.AddSystem(*imu_plugin)) -// { -// RCLCPP_ERROR( -// node->get_logger(), "IMU system not added"); - -// } -// server.Run(true /*blocking*/, 0, false /*paused*/); -// } From d28021a03a8af2fa165c59986bf177edf0e05ccc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 4 Apr 2024 19:58:50 +0200 Subject: [PATCH 05/20] 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/launch/ros_gz_sim.launch.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index 7c339b38..b718b6d2 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -25,11 +25,14 @@ def generate_launch_description(): world_sdf_file_arg = DeclareLaunchArgument( - 'world_sdf_file', default_value=TextSubstitution(text='')) + 'world_sdf_file', default_value=TextSubstitution(text=''), + description='Path to the SDF world file') world_sdf_string_arg = DeclareLaunchArgument( - 'world_sdf_string', default_value=TextSubstitution(text='')) + 'world_sdf_string', default_value=TextSubstitution(text=''), + description='SDF world string') config_file_arg = DeclareLaunchArgument( - 'config_file', default_value=TextSubstitution(text='')) + 'config_file', default_value=TextSubstitution(text=''), + description='Path to the YAML configuration for the bridge') world_sdf_file_param = LaunchConfiguration('world_sdf_file') world_sdf_string_param = LaunchConfiguration('world_sdf_string') From 82633e7c7d3ff661a07cd1d475bccd3fcf1c60ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 4 Apr 2024 20:38:34 +0200 Subject: [PATCH 06/20] Style MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/launch/ros_gz_sim.launch.py | 2 +- ros_gz_sim/src/gzserver.cpp | 114 ++++++++++++------------- 2 files changed, 58 insertions(+), 58 deletions(-) diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index b718b6d2..8e901bb0 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -68,4 +68,4 @@ def generate_launch_description(): world_sdf_file_arg, world_sdf_string_arg, config_file_arg, - container,]) + container, ]) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index c7783753..9138d201 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -23,72 +23,72 @@ namespace ros_gz_sim { - class GzServer : public rclcpp::Node +class GzServer : public rclcpp::Node +{ +public: + // Class constructor. + explicit GzServer(const rclcpp::NodeOptions & options) + : Node("gzserver", options) { - // Class constructor. - public: GzServer(const rclcpp::NodeOptions & options) - : Node("gzserver", options) - { - thread_ = std::thread(std::bind(&GzServer::OnStart, this)); - } + thread_ = std::thread(std::bind(&GzServer::OnStart, this)); + } - // Class destructor. - public: ~GzServer() - { - // Make sure to join the thread on shutdown. - if (thread_.joinable()) { - thread_.join(); - } +public: + // Class destructor. + ~GzServer() + { + // Make sure to join the thread on shutdown. + if (thread_.joinable()) { + thread_.join(); } + } - /// \brief Run the gz sim server. - public: void OnStart() - { - auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); - auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); +public: + /// \brief Run the gz sim server. + void OnStart() + { + auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); + auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); + + gz::common::Console::SetVerbosity(4); + gz::sim::ServerConfig server_config; - gz::common::Console::SetVerbosity(4); - gz::sim::ServerConfig server_config; + if (!world_sdf_file.empty()) { + server_config.SetSdfFile(world_sdf_file); + } else if (!world_sdf_string.empty()) { + server_config.SetSdfString(world_sdf_string); + } else { + RCLCPP_ERROR( + this->get_logger(), + "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + return; + } - if (!world_sdf_file.empty()) - { - server_config.SetSdfFile(world_sdf_file); - } - else if (!world_sdf_string.empty()) { - server_config.SetSdfString(world_sdf_string); - } - else { - RCLCPP_ERROR( - this->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); - return; - } + // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. - // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. + // + // + sdf::Plugin imu_sdf_plugin; + imu_sdf_plugin.SetName("gz::sim::systems::Imu"); + imu_sdf_plugin.SetFilename("gz-sim-imu-system"); + gz::sim::SystemLoader loader; + auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); + std::cout << imu_plugin.value() << std::endl; - // - // - sdf::Plugin imu_sdf_plugin; - imu_sdf_plugin.SetName("gz::sim::systems::Imu"); - imu_sdf_plugin.SetFilename("gz-sim-imu-system"); - gz::sim::SystemLoader loader; - auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); - std::cout << imu_plugin.value() << std::endl; - - gz::sim::Server server(server_config); - server.RunOnce(); - if (!server.AddSystem(*imu_plugin)) - { - RCLCPP_ERROR(this->get_logger(), "IMU system not added"); - - } - server.Run(true /*blocking*/, 0, false /*paused*/); + gz::sim::Server server(server_config); + server.RunOnce(); + if (!server.AddSystem(*imu_plugin)) { + RCLCPP_ERROR(this->get_logger(), "IMU system not added"); } + server.Run(true /*blocking*/, 0, false /*paused*/); + } - /// \brief We don't want to block the ROS thread. - private: std::thread thread_; - }; -} +private: + /// \brief We don't want to block the ROS thread. + std::thread thread_; +}; +} // namespace ros_gz_sim RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer) From 2f19e16856592831c003552c8caec44c75ed9be6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 5 Apr 2024 16:16:43 +0200 Subject: [PATCH 07/20] Remove comments 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 | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index db256cf0..b54269ce 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -89,7 +89,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ add_library(gzserver_component SHARED src/gzserver.cpp) rclcpp_components_register_nodes(gzserver_component "ros_gz_sim::GzServer") -#add_executable(gzserver src/gzserver.cpp) ament_target_dependencies(gzserver_component rclcpp rclcpp_components @@ -137,10 +136,6 @@ install(TARGETS gzserver_component LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -#install(TARGETS -# gzserver -# DESTINATION lib/${PROJECT_NAME} -#) install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} From 1124d281a942a781c49b13a01b6a7543d79be995 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 11 Apr 2024 19:56:49 +0200 Subject: [PATCH 08/20] Launch file for running gz_bridge (#530) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Launch file for the bridge Signed-off-by: Carlos Agüero --- ros_gz_bridge/CMakeLists.txt | 5 + ros_gz_bridge/launch/ros_gz_bridge.launch.py | 109 +++++++++++++++++++ ros_gz_sim/launch/ros_gz_sim.launch.py | 2 +- 3 files changed, 115 insertions(+), 1 deletion(-) create mode 100644 ros_gz_bridge/launch/ros_gz_bridge.launch.py diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 6c2d4b3e..43247aff 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -158,6 +158,11 @@ install( DESTINATION include/${PROJECT_NAME} ) +install( + DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + set(bridge_executables parameter_bridge static_bridge diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch.py b/ros_gz_bridge/launch/ros_gz_bridge.launch.py new file mode 100644 index 00000000..7809299c --- /dev/null +++ b/ros_gz_bridge/launch/ros_gz_bridge.launch.py @@ -0,0 +1,109 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch ros_gz bridge in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + config_file = LaunchConfiguration('config_file') + container_name = LaunchConfiguration('container_name') + namespace = LaunchConfiguration('namespace') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + declare_config_file_cmd = DeclareLaunchArgument( + 'config_file', default_value='', description='YAML config file' + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='ros_gz_bridge', + executable='bridge_node', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[{'config_file': config_file}], + arguments=['--ros-args', '--log-level', log_level], + ), + ], + ) + + load_composable_nodes = ComposableNodeContainer( + condition=IfCondition(use_composition), + name=container_name, + namespace=namespace, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='ros_gz_bridge', + plugin='ros_gz_bridge::RosGzBridge', + name='ros_gz_bridge', + parameters=[{'config_file': config_file}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + output='screen', + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_config_file_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the bridge nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index 8e901bb0..c2e25517 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): """Generate launch description with multiple components.""" container = ComposableNodeContainer( - name='gz_sim_container', + name='ros_gz_container', namespace='', package='rclcpp_components', executable='component_container', From 78127ca861f9e5bbe1ecd69bf14f0baa23cd9c9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 18 Apr 2024 22:03:07 +0200 Subject: [PATCH 09/20] Launch file for running gzserver (#532) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Launch file for the bridge Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 1 + ros_gz_sim/launch/gz_server.launch.py | 91 +++++++++++++++++++++++++++ 2 files changed, 92 insertions(+) create mode 100644 ros_gz_sim/launch/gz_server.launch.py diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index b54269ce..dbc811fe 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -120,6 +120,7 @@ install(FILES ) install(FILES + "launch/gz_server.launch.py" "launch/ros_gz_sim.launch.py" DESTINATION share/${PROJECT_NAME}/launch ) diff --git a/ros_gz_sim/launch/gz_server.launch.py b/ros_gz_sim/launch/gz_server.launch.py new file mode 100644 index 00000000..f13bb471 --- /dev/null +++ b/ros_gz_sim/launch/gz_server.launch.py @@ -0,0 +1,91 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch gzserver in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + world_sdf_file = LaunchConfiguration('world_sdf_file') + world_sdf_string = LaunchConfiguration('world_sdf_string') + container_name = LaunchConfiguration('container_name') + use_composition = LaunchConfiguration('use_composition') + + declare_world_sdf_file_cmd = DeclareLaunchArgument( + 'world_sdf_file', default_value=TextSubstitution(text=''), + description='Path to the SDF world file') + declare_world_sdf_string_cmd = DeclareLaunchArgument( + 'world_sdf_string', default_value=TextSubstitution(text=''), + description='SDF world string') + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='ros_gz_sim', + executable='gzserver', + output='screen', + parameters=[{'world_sdf_file': world_sdf_file, + 'world_sdf_string': world_sdf_string}], + ), + ], + ) + + load_composable_nodes = ComposableNodeContainer( + condition=IfCondition(use_composition), + name=container_name, + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='ros_gz_sim', + plugin='ros_gz_sim::GzServer', + name='gzserver', + parameters=[{'world_sdf_file': world_sdf_file, + 'world_sdf_string': world_sdf_string}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + output='screen', + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_sdf_file_cmd) + ld.add_action(declare_world_sdf_string_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_composition_cmd) + # Add the actions to launch all of the gzserver nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld From c2a26ef61f68adbf3c7e8e32eae989ab464ba714 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 15 May 2024 17:37:15 +0200 Subject: [PATCH 10/20] Launch file for combined gzserver + bridge (#533) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Combined gzserver + bridge launch file. Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 3 +- ros_gz_sim/launch/ros_gz_sim.launch.py | 135 ++++++++++++++++--------- 2 files changed, 89 insertions(+), 49 deletions(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 4f9b0f00..3c76e3a0 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -27,7 +27,6 @@ find_package(gz-msgs REQUIRED) find_package(gz_sim_vendor REQUIRED) find_package(gz-sim REQUIRED) # Needed in launch/gz_sim.launch.py.in -set(GZ_SIM_VER ${gz-sim_VERSION_MAJOR}) gz_find_package(gflags REQUIRED @@ -69,7 +68,7 @@ ament_target_dependencies(gzserver_component std_msgs ) target_link_libraries(gzserver_component - ${GZ_TARGET_PREFIX}-sim${GZ_SIM_VER}::core + gz-sim::core ) ament_target_dependencies(gzserver_component std_msgs) rclcpp_components_register_node( diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index c2e25517..527e295a 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -13,59 +13,100 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Launch gzsim in a component container.""" +"""Launch gzsim + ros_gz_bridge in a component container.""" -import launch -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, TextSubstitution -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - world_sdf_file_arg = DeclareLaunchArgument( + config_file = LaunchConfiguration('config_file') + container_name = LaunchConfiguration('container_name') + namespace = LaunchConfiguration('namespace') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + bridge_log_level = LaunchConfiguration('bridge_log_level') + + world_sdf_file = LaunchConfiguration('world_sdf_file') + world_sdf_string = LaunchConfiguration('world_sdf_string') + + declare_config_file_cmd = DeclareLaunchArgument( + 'config_file', default_value='', description='YAML config file' + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_bridge_log_level_cmd = DeclareLaunchArgument( + 'bridge_log_level', default_value='info', description='Bridge log level' + ) + + declare_world_sdf_file_cmd = DeclareLaunchArgument( 'world_sdf_file', default_value=TextSubstitution(text=''), - description='Path to the SDF world file') - world_sdf_string_arg = DeclareLaunchArgument( + description='Path to the SDF world file' + ) + + declare_world_sdf_string_cmd = DeclareLaunchArgument( 'world_sdf_string', default_value=TextSubstitution(text=''), - description='SDF world string') - config_file_arg = DeclareLaunchArgument( - 'config_file', default_value=TextSubstitution(text=''), - description='Path to the YAML configuration for the bridge') - - world_sdf_file_param = LaunchConfiguration('world_sdf_file') - world_sdf_string_param = LaunchConfiguration('world_sdf_string') - bridge_config_file_param = LaunchConfiguration('config_file') - - """Generate launch description with multiple components.""" - container = ComposableNodeContainer( - name='ros_gz_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='ros_gz_sim', - plugin='ros_gz_sim::GzServer', - name='gzserver', - parameters=[{'world_sdf_file': world_sdf_file_param, - 'world_sdf_string': world_sdf_string_param}], - extra_arguments=[{'use_intra_process_comms': True}], - ), - ComposableNode( - package='ros_gz_bridge', - plugin='ros_gz_bridge::RosGzBridge', - name='bridge', - parameters=[{'config_file': bridge_config_file_param}], - extra_arguments=[{'use_intra_process_comms': True}], - ), - ], - output='screen', + description='SDF world string' ) - return launch.LaunchDescription([ - world_sdf_file_arg, - world_sdf_string_arg, - config_file_arg, - container, ]) + bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('config_file', config_file), + ('container_name', container_name), + ('namespace', namespace), + ('use_composition', use_composition), + ('use_respawn', use_respawn), + ('bridge_log_level', bridge_log_level)]) + + gzserver_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_server.launch.py'])]), + launch_arguments=[('world_sdf_file', world_sdf_file), + ('world_sdf_string', world_sdf_string), + ('use_composition', use_composition), ]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_config_file_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_bridge_log_level_cmd) + ld.add_action(declare_world_sdf_file_cmd) + ld.add_action(declare_world_sdf_string_cmd) + # Add the actions to launch all of the bridge + gzserver nodes + ld.add_action(bridge_description) + ld.add_action(gzserver_description) + + return ld From 6a73faa262eec103b4cd7521b135b6e6f061b784 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 15 May 2024 17:56:42 +0200 Subject: [PATCH 11/20] Launch file for combined spawn_model + bridge (#534) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Combined spawn_model + bridge launch file. Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 2 + ros_gz_sim/launch/gz_spawn_model.launch.py | 94 +++++++++++ ros_gz_sim/launch/ros_gz_sim.launch.py | 1 - .../launch/ros_gz_spawn_model.launch.py | 154 ++++++++++++++++++ 4 files changed, 250 insertions(+), 1 deletion(-) create mode 100644 ros_gz_sim/launch/gz_spawn_model.launch.py create mode 100644 ros_gz_sim/launch/ros_gz_spawn_model.launch.py diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 3c76e3a0..aa0abe7c 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -94,7 +94,9 @@ install(FILES install(FILES "launch/gz_server.launch.py" + "launch/gz_spawn_model.launch.py" "launch/ros_gz_sim.launch.py" + "launch/ros_gz_spawn_model.launch.py" DESTINATION share/${PROJECT_NAME}/launch ) diff --git a/ros_gz_sim/launch/gz_spawn_model.launch.py b/ros_gz_sim/launch/gz_spawn_model.launch.py new file mode 100644 index 00000000..59cb4361 --- /dev/null +++ b/ros_gz_sim/launch/gz_spawn_model.launch.py @@ -0,0 +1,94 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch create to spawn models in gz sim.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + + world = LaunchConfiguration('world') + file = LaunchConfiguration('file') + xml_string = LaunchConfiguration('string') + topic = LaunchConfiguration('topic') + name = LaunchConfiguration('name') + allow_renaming = LaunchConfiguration('allow_renaming') + x = LaunchConfiguration('x', default='0.0') + y = LaunchConfiguration('y', default='0.0') + z = LaunchConfiguration('z', default='0.0') + roll = LaunchConfiguration('R', default='0.0') + pitch = LaunchConfiguration('P', default='0.0') + yaw = LaunchConfiguration('Y', default='0.0') + + declare_world_cmd = DeclareLaunchArgument( + 'world', default_value=TextSubstitution(text=''), + description='World name') + declare_file_cmd = DeclareLaunchArgument( + 'file', default_value=TextSubstitution(text=''), + description='SDF filename') + declare_xml_string_cmd = DeclareLaunchArgument( + 'string', + default_value='', + description='XML string', + ) + declare_topic_cmd = DeclareLaunchArgument( + 'topic', default_value=TextSubstitution(text=''), + description='Get XML from this topic' + ) + declare_name_cmd = DeclareLaunchArgument( + 'name', default_value=TextSubstitution(text=''), + description='Name of the entity' + ) + declare_allow_renaming_cmd = DeclareLaunchArgument( + 'allow_renaming', default_value='False', + description='Whether the entity allows renaming or not' + ) + + load_nodes = Node( + package='ros_gz_sim', + executable='create', + output='screen', + parameters=[{'world': world, + 'file': file, + 'string': xml_string, + 'topic': topic, + 'name': name, + 'allow_renaming': allow_renaming, + 'x': x, + 'y': y, + 'z': z, + 'R': roll, + 'P': pitch, + 'Y': yaw, + }], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_cmd) + ld.add_action(declare_file_cmd) + ld.add_action(declare_xml_string_cmd) + ld.add_action(declare_topic_cmd) + ld.add_action(declare_name_cmd) + ld.add_action(declare_allow_renaming_cmd) + # Add the actions to launch all of the create nodes + ld.add_action(load_nodes) + + return ld diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index 527e295a..d6ba013a 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -1,4 +1,3 @@ - # Copyright 2024 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/ros_gz_sim/launch/ros_gz_spawn_model.launch.py b/ros_gz_sim/launch/ros_gz_spawn_model.launch.py new file mode 100644 index 00000000..e5de33aa --- /dev/null +++ b/ros_gz_sim/launch/ros_gz_spawn_model.launch.py @@ -0,0 +1,154 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch gzsim + ros_gz_bridge in a component container.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + config_file = LaunchConfiguration('config_file') + container_name = LaunchConfiguration('container_name') + namespace = LaunchConfiguration('namespace') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + world = LaunchConfiguration('world') + file = LaunchConfiguration('file') + xml_string = LaunchConfiguration('string') + topic = LaunchConfiguration('topic') + name = LaunchConfiguration('name') + allow_renaming = LaunchConfiguration('allow_renaming') + x = LaunchConfiguration('x', default='0.0') + y = LaunchConfiguration('y', default='0.0') + z = LaunchConfiguration('z', default='0.0') + roll = LaunchConfiguration('R', default='0.0') + pitch = LaunchConfiguration('P', default='0.0') + yaw = LaunchConfiguration('Y', default='0.0') + + declare_config_file_cmd = DeclareLaunchArgument( + 'config_file', default_value='', description='YAML config file' + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition', + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', description='Use composed bringup if True' + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + declare_world_cmd = DeclareLaunchArgument( + 'world', default_value=TextSubstitution(text=''), + description='World name') + + declare_file_cmd = DeclareLaunchArgument( + 'file', default_value=TextSubstitution(text=''), + description='SDF filename') + + declare_xml_string_cmd = DeclareLaunchArgument( + 'string', + default_value='', + description='XML string', + ) + + declare_topic_cmd = DeclareLaunchArgument( + 'topic', default_value=TextSubstitution(text=''), + description='Get XML from this topic' + ) + + declare_name_cmd = DeclareLaunchArgument( + 'name', default_value=TextSubstitution(text=''), + description='Name of the entity' + ) + + declare_allow_renaming_cmd = DeclareLaunchArgument( + 'allow_renaming', default_value='False', + description='Whether the entity allows renaming or not' + ) + + bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('config_file', config_file), + ('container_name', container_name), + ('namespace', namespace), + ('use_composition', use_composition), + ('use_respawn', use_respawn), + ('log_level', log_level), ]) + + spawn_model_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_spawn_model.launch.py'])]), + launch_arguments=[('world', world), + ('file', file), + ('xml_string', xml_string), + ('topic', topic), + ('name', name), + ('allow_renaming', allow_renaming), + ('x', x), + ('y', y), + ('z', z), + ('R', roll), + ('P', pitch), + ('Y', yaw), + ('use_composition', use_composition), ]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_config_file_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_file_cmd) + ld.add_action(declare_xml_string_cmd) + ld.add_action(declare_topic_cmd) + ld.add_action(declare_name_cmd) + ld.add_action(declare_allow_renaming_cmd) + # Add the actions to launch all of the bridge + spawn_model nodes + ld.add_action(bridge_description) + ld.add_action(spawn_model_description) + + return ld From 1ba6ebf9733457f1cc78dab9e8e753c46329bec5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 21 May 2024 17:10:53 +0200 Subject: [PATCH 12/20] Launch gzserver from xml (#548) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Launch file for the bridge Signed-off-by: Carlos Agüero --- launch_gz/launch_gz/__init__.py | 22 ++++ launch_gz/launch_gz/actions/__init__.py | 22 ++++ launch_gz/launch_gz/actions/gzserver.py | 115 ++++++++++++++++++ launch_gz/package.xml | 36 ++++++ launch_gz/resource/launch_gz | 0 launch_gz/setup.py | 49 ++++++++ ros_gz_sim/CMakeLists.txt | 3 +- ros_gz_sim/launch/gzserver.launch | 12 ++ ...gz_server.launch.py => gzserver.launch.py} | 43 +++---- ros_gz_sim/launch/ros_gz_sim.launch.py | 2 +- 10 files changed, 275 insertions(+), 29 deletions(-) create mode 100644 launch_gz/launch_gz/__init__.py create mode 100644 launch_gz/launch_gz/actions/__init__.py create mode 100644 launch_gz/launch_gz/actions/gzserver.py create mode 100644 launch_gz/package.xml create mode 100644 launch_gz/resource/launch_gz create mode 100644 launch_gz/setup.py create mode 100644 ros_gz_sim/launch/gzserver.launch rename ros_gz_sim/launch/{gz_server.launch.py => gzserver.launch.py} (68%) diff --git a/launch_gz/launch_gz/__init__.py b/launch_gz/launch_gz/__init__.py new file mode 100644 index 00000000..42f90cd9 --- /dev/null +++ b/launch_gz/launch_gz/__init__.py @@ -0,0 +1,22 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Main entry point for the `launch_gz` package.""" + +from . import actions + + +__all__ = [ + 'actions', +] \ No newline at end of file diff --git a/launch_gz/launch_gz/actions/__init__.py b/launch_gz/launch_gz/actions/__init__.py new file mode 100644 index 00000000..12994391 --- /dev/null +++ b/launch_gz/launch_gz/actions/__init__.py @@ -0,0 +1,22 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Actions module.""" + +from .gzserver import GzServer + + +__all__ = [ + 'GzServer', +] \ No newline at end of file diff --git a/launch_gz/launch_gz/actions/gzserver.py b/launch_gz/launch_gz/actions/gzserver.py new file mode 100644 index 00000000..29512a45 --- /dev/null +++ b/launch_gz/launch_gz/actions/gzserver.py @@ -0,0 +1,115 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for the GzServer action.""" + +from typing import List +from typing import Optional + +from launch.action import Action +from launch.actions import IncludeLaunchDescription +from launch.frontend import expose_action, Entity, Parser +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.some_substitutions_type import SomeSubstitutionsType +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +@expose_action('gzserver') +class GzServer(Action): + """Action that executes a gzserver ROS [composable] node.""" + + def __init__( + self, + *, + world_sdf_file: SomeSubstitutionsType, + world_sdf_string: SomeSubstitutionsType, + container_name: SomeSubstitutionsType, + use_composition: SomeSubstitutionsType, + **kwargs + ) -> None: + """ + Construct a gzserver action. + + All arguments are forwarded to `ros_gz_sim.launch.gzserver.launch.py`, so see the documentation + of that class for further details. + + :param: world_sdf_file Path to the SDF world file. + :param: world_sdf_string SDF world string. + :param: container_name Name of container that nodes will load in if use composition. + :param: use_composition Use composed bringup if True. + """ + + super().__init__(**kwargs) + self.__world_sdf_file = world_sdf_file + self.__world_sdf_string = world_sdf_string + self.__container_name = container_name + self.__use_composition = use_composition + + @classmethod + def parse(cls, entity: Entity, parser: Parser): + """Parse gzserver.""" + _, kwargs = super().parse(entity, parser) + + world_sdf_file = entity.get_attr( + 'world_sdf_file', data_type=str, + optional=True) + + world_sdf_string = entity.get_attr( + 'world_sdf_string', data_type=str, + optional=True) + + container_name = entity.get_attr( + 'container_name', data_type=str, + optional=True) + + use_composition = entity.get_attr( + 'use_composition', data_type=str, + optional=True) + + if isinstance(world_sdf_file, str): + world_sdf_file = parser.parse_substitution(world_sdf_file) + kwargs['world_sdf_file'] = world_sdf_file + + if isinstance(world_sdf_string, str): + world_sdf_string = parser.parse_substitution(world_sdf_string) + kwargs['world_sdf_string'] = world_sdf_string + + if isinstance(container_name, str): + container_name = parser.parse_substitution(container_name) + kwargs['container_name'] = container_name + + if isinstance(use_composition, str): + use_composition = parser.parse_substitution(use_composition) + kwargs['use_composition'] = use_composition + + return cls, kwargs + + def execute(self, context: LaunchContext) -> Optional[List[Action]]: + """ + Execute the action. + """ + + gzserver_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gzserver.launch.py'])]), + launch_arguments=[('world_sdf_file', self.__world_sdf_file), + ('world_sdf_string', self.__world_sdf_string), + ('container_name', self.__container_name), + ('use_composition', self.__use_composition), + ]) + + return [gzserver_description] diff --git a/launch_gz/package.xml b/launch_gz/package.xml new file mode 100644 index 00000000..da84b0e2 --- /dev/null +++ b/launch_gz/package.xml @@ -0,0 +1,36 @@ + + + + launch_gz + 0.246.0 + gz specific extensions to the launch tool. + + Alejandro Hernandez + Carlos Agüero + + Apache License 2.0 + + Carlos Agüero + + ament_index_python + composition_interfaces + launch + launch_ros + lifecycle_msgs + osrf_pycommon + ros_gz_sim + python3-importlib-metadata + python3-yaml + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/launch_gz/resource/launch_gz b/launch_gz/resource/launch_gz new file mode 100644 index 00000000..e69de29b diff --git a/launch_gz/setup.py b/launch_gz/setup.py new file mode 100644 index 00000000..01352303 --- /dev/null +++ b/launch_gz/setup.py @@ -0,0 +1,49 @@ +from setuptools import find_packages +from setuptools import setup + +package_name = 'launch_gz' + +setup( + name=package_name, + version='0.246.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/' + package_name, ['package.xml']), + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ], + package_data={'launch_gz': ['py.typed']}, + install_requires=[ + 'setuptools', + 'ament_index_python', + 'launch', + 'launch_ros', + 'osrf_pycommon', + 'pyyaml', + ], + zip_safe=True, + author='Carlos Agüero', + author_email='caguero@openrobotics.org', + maintainer='Carlos Agüero, Alejandro Hernandez, Michael Carroll', + maintainer_email='caguero@openrobotics.org, alejandro@openrobotics.org, mjcarroll@intrinsic.ai', + url='https://github.com/gazebosim/ros_gz/launch_gz', + download_url='https://github.com/gazebosim/ros_gz/releases', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='gz specific extensions to `launch`.', + long_description=( + 'This package provides gz specific extensions to the launch package.' + ), + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'launch.frontend.launch_extension': [ + 'launch_gz = launch_gz', + ], + } +) \ No newline at end of file diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index aa0abe7c..06ade215 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -93,7 +93,8 @@ install(FILES ) install(FILES - "launch/gz_server.launch.py" + "launch/gzserver.launch" + "launch/gzserver.launch.py" "launch/gz_spawn_model.launch.py" "launch/ros_gz_sim.launch.py" "launch/ros_gz_spawn_model.launch.py" diff --git a/ros_gz_sim/launch/gzserver.launch b/ros_gz_sim/launch/gzserver.launch new file mode 100644 index 00000000..93d19d22 --- /dev/null +++ b/ros_gz_sim/launch/gzserver.launch @@ -0,0 +1,12 @@ + + + + + + + + diff --git a/ros_gz_sim/launch/gz_server.launch.py b/ros_gz_sim/launch/gzserver.launch.py similarity index 68% rename from ros_gz_sim/launch/gz_server.launch.py rename to ros_gz_sim/launch/gzserver.launch.py index f13bb471..225d6b23 100644 --- a/ros_gz_sim/launch/gz_server.launch.py +++ b/ros_gz_sim/launch/gzserver.launch.py @@ -15,7 +15,7 @@ """Launch gzserver in a component container.""" from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution from launch_ros.actions import ComposableNodeContainer, Node @@ -24,11 +24,6 @@ def generate_launch_description(): - world_sdf_file = LaunchConfiguration('world_sdf_file') - world_sdf_string = LaunchConfiguration('world_sdf_string') - container_name = LaunchConfiguration('container_name') - use_composition = LaunchConfiguration('use_composition') - declare_world_sdf_file_cmd = DeclareLaunchArgument( 'world_sdf_file', default_value=TextSubstitution(text=''), description='Path to the SDF world file') @@ -36,30 +31,24 @@ def generate_launch_description(): 'world_sdf_string', default_value=TextSubstitution(text=''), description='SDF world string') declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', - default_value='ros_gz_container', - description='Name of container that nodes will load in if use composition', - ) + 'container_name', default_value='ros_gz_container', + description='Name of container that nodes will load in if use composition',) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', description='Use composed bringup if True' - ) + 'use_composition', default_value='False', + description='Use composed bringup if True') - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), - actions=[ - Node( - package='ros_gz_sim', - executable='gzserver', - output='screen', - parameters=[{'world_sdf_file': world_sdf_file, - 'world_sdf_string': world_sdf_string}], - ), - ], + load_nodes = Node( + condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('use_composition')])), + package='ros_gz_sim', + executable='gzserver', + output='screen', + parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'), + 'world_sdf_string': LaunchConfiguration('world_sdf_string')}], ) load_composable_nodes = ComposableNodeContainer( - condition=IfCondition(use_composition), - name=container_name, + condition=IfCondition(LaunchConfiguration('use_composition')), + name=LaunchConfiguration('container_name'), namespace='', package='rclcpp_components', executable='component_container', @@ -68,8 +57,8 @@ def generate_launch_description(): package='ros_gz_sim', plugin='ros_gz_sim::GzServer', name='gzserver', - parameters=[{'world_sdf_file': world_sdf_file, - 'world_sdf_string': world_sdf_string}], + parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'), + 'world_sdf_string': LaunchConfiguration('world_sdf_string')}], extra_arguments=[{'use_intra_process_comms': True}], ), ], diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index d6ba013a..dc85994b 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', - 'gz_server.launch.py'])]), + 'gzserver.launch.py'])]), launch_arguments=[('world_sdf_file', world_sdf_file), ('world_sdf_string', world_sdf_string), ('use_composition', use_composition), ]) From 2b079362ba5b988ed2cb6314c5d1d07378b3713c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 22 May 2024 11:28:36 -0500 Subject: [PATCH 13/20] Fix build, remove experimental code Signed-off-by: Addisu Z. Taddese --- ros_gz_sim/CMakeLists.txt | 2 +- ros_gz_sim/src/gzserver.cpp | 21 --------------------- 2 files changed, 1 insertion(+), 22 deletions(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 00d435ed..125b28dd 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -66,7 +66,7 @@ ament_target_dependencies(gzserver std_msgs ) target_link_libraries(gzserver - ${GZ_TARGET_PREFIX}-sim${GZ_SIM_VER}::core + gz-sim::core ) ament_target_dependencies(gzserver std_msgs) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 3cdd43c9..e4cb24c8 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -29,7 +29,6 @@ int main(int _argc, char ** _argv) common::Console::SetVerbosity(4); sim::ServerConfig server_config; - if (!world_sdf_file.empty()) { server_config.SetSdfFile(world_sdf_file); @@ -43,26 +42,6 @@ int main(int _argc, char ** _argv) return -1; } - // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. - - // - // - sdf::Plugin imu_sdf_plugin; - imu_sdf_plugin.SetName("gz::sim::systems::Imu"); - imu_sdf_plugin.SetFilename("gz-sim-imu-system"); - sim::SystemLoader loader; - auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); - std::cout << imu_plugin.value() << std::endl; - gz::sim::Server server(server_config); - server.RunOnce(); - if (!server.AddSystem(*imu_plugin)) - { - RCLCPP_ERROR( - node->get_logger(), "IMU system not added"); - - } server.Run(true /*blocking*/, 0, false /*paused*/); } From 35be061fd921ecccdb0f05f67b864d65a6d60120 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 22 May 2024 12:15:11 -0500 Subject: [PATCH 14/20] Add comment, fix linter Signed-off-by: Addisu Z. Taddese --- ros_gz_sim/src/gzserver.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index e4cb24c8..530f23c7 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -18,25 +18,22 @@ #include #include +// ROS node that executes a gz-sim Server given a world SDF file or string. int main(int _argc, char ** _argv) { - using namespace gz; auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); auto node = rclcpp::Node::make_shared("gzserver"); auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); - common::Console::SetVerbosity(4); - sim::ServerConfig server_config; + gz::common::Console::SetVerbosity(4); + gz::sim::ServerConfig server_config; - if (!world_sdf_file.empty()) - { + if (!world_sdf_file.empty()) { server_config.SetSdfFile(world_sdf_file); - } - else if (!world_sdf_string.empty()) { + } else if (!world_sdf_string.empty()) { server_config.SetSdfString(world_sdf_string); - } - else { + } else { RCLCPP_ERROR( node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); return -1; From 8d01ae858d15212263db0b3bc024a5428c742c1f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 22 May 2024 14:36:17 -0500 Subject: [PATCH 15/20] Fix end of file newline Signed-off-by: Addisu Z. Taddese --- launch_gz/launch_gz/__init__.py | 2 +- launch_gz/launch_gz/actions/__init__.py | 2 +- launch_gz/package.xml | 2 +- launch_gz/setup.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/launch_gz/launch_gz/__init__.py b/launch_gz/launch_gz/__init__.py index 42f90cd9..dbc37965 100644 --- a/launch_gz/launch_gz/__init__.py +++ b/launch_gz/launch_gz/__init__.py @@ -19,4 +19,4 @@ __all__ = [ 'actions', -] \ No newline at end of file +] diff --git a/launch_gz/launch_gz/actions/__init__.py b/launch_gz/launch_gz/actions/__init__.py index 12994391..03fe5afc 100644 --- a/launch_gz/launch_gz/actions/__init__.py +++ b/launch_gz/launch_gz/actions/__init__.py @@ -19,4 +19,4 @@ __all__ = [ 'GzServer', -] \ No newline at end of file +] diff --git a/launch_gz/package.xml b/launch_gz/package.xml index da84b0e2..558f7138 100644 --- a/launch_gz/package.xml +++ b/launch_gz/package.xml @@ -33,4 +33,4 @@ ament_python - \ No newline at end of file + diff --git a/launch_gz/setup.py b/launch_gz/setup.py index 01352303..0f56e2de 100644 --- a/launch_gz/setup.py +++ b/launch_gz/setup.py @@ -46,4 +46,4 @@ 'launch_gz = launch_gz', ], } -) \ No newline at end of file +) From f7eaa49d08ee19d15b5eefb888e46f2ed057b457 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 23 May 2024 19:04:15 +0200 Subject: [PATCH 16/20] Update ros_gz_sim/src/gzserver.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- ros_gz_sim/src/gzserver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 7f5128c7..67e3a07d 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -62,6 +62,7 @@ class GzServer : public rclcpp::Node RCLCPP_ERROR( this->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + rclcpp::shutdown(); return; } From efb5f926c51096148fa9f70b7f9bbe7145b9de9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 23 May 2024 19:04:35 +0200 Subject: [PATCH 17/20] Update ros_gz_sim/src/gzserver.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Addisu Z. Taddese Signed-off-by: Carlos Agüero --- ros_gz_sim/src/gzserver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 67e3a07d..2346390d 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -68,6 +68,7 @@ class GzServer : public rclcpp::Node gz::sim::Server server(server_config); server.Run(true /*blocking*/, 0, false /*paused*/); + rclcpp::shutdown() } private: From 01345e2d95d76998f3ab8fbd341e2164d075a009 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 23 May 2024 21:33:14 +0200 Subject: [PATCH 18/20] Remove launch_gz and use ros_gz_sim instead MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- launch_gz/package.xml | 36 -------------- launch_gz/setup.py | 49 ------------------- ros_gz_sim/CMakeLists.txt | 8 ++- .../{gzserver.launch => gz_server.launch} | 8 +-- ...gzserver.launch.py => gz_server.launch.py} | 6 +-- ros_gz_sim/launch/ros_gz_sim.launch.py | 8 +-- ros_gz_sim/package.xml | 10 ++++ .../resource/ros_gz_sim | 0 .../ros_gz_sim}/__init__.py | 2 +- .../ros_gz_sim}/actions/__init__.py | 0 .../ros_gz_sim}/actions/gzserver.py | 24 ++++----- ros_gz_sim/setup.cfg | 3 ++ 12 files changed, 43 insertions(+), 111 deletions(-) delete mode 100644 launch_gz/package.xml delete mode 100644 launch_gz/setup.py rename ros_gz_sim/launch/{gzserver.launch => gz_server.launch} (72%) rename ros_gz_sim/launch/{gzserver.launch.py => gz_server.launch.py} (95%) rename launch_gz/resource/launch_gz => ros_gz_sim/resource/ros_gz_sim (100%) rename {launch_gz/launch_gz => ros_gz_sim/ros_gz_sim}/__init__.py (92%) rename {launch_gz/launch_gz => ros_gz_sim/ros_gz_sim}/actions/__init__.py (100%) rename {launch_gz/launch_gz => ros_gz_sim/ros_gz_sim}/actions/gzserver.py (85%) create mode 100644 ros_gz_sim/setup.cfg diff --git a/launch_gz/package.xml b/launch_gz/package.xml deleted file mode 100644 index 558f7138..00000000 --- a/launch_gz/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - launch_gz - 0.246.0 - gz specific extensions to the launch tool. - - Alejandro Hernandez - Carlos Agüero - - Apache License 2.0 - - Carlos Agüero - - ament_index_python - composition_interfaces - launch - launch_ros - lifecycle_msgs - osrf_pycommon - ros_gz_sim - python3-importlib-metadata - python3-yaml - rclpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/launch_gz/setup.py b/launch_gz/setup.py deleted file mode 100644 index 0f56e2de..00000000 --- a/launch_gz/setup.py +++ /dev/null @@ -1,49 +0,0 @@ -from setuptools import find_packages -from setuptools import setup - -package_name = 'launch_gz' - -setup( - name=package_name, - version='0.246.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/' + package_name, ['package.xml']), - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ], - package_data={'launch_gz': ['py.typed']}, - install_requires=[ - 'setuptools', - 'ament_index_python', - 'launch', - 'launch_ros', - 'osrf_pycommon', - 'pyyaml', - ], - zip_safe=True, - author='Carlos Agüero', - author_email='caguero@openrobotics.org', - maintainer='Carlos Agüero, Alejandro Hernandez, Michael Carroll', - maintainer_email='caguero@openrobotics.org, alejandro@openrobotics.org, mjcarroll@intrinsic.ai', - url='https://github.com/gazebosim/ros_gz/launch_gz', - download_url='https://github.com/gazebosim/ros_gz/releases', - keywords=['ROS'], - classifiers=[ - 'Intended Audience :: Developers', - 'License :: OSI Approved :: Apache Software License', - 'Programming Language :: Python', - 'Topic :: Software Development', - ], - description='gz specific extensions to `launch`.', - long_description=( - 'This package provides gz specific extensions to the launch package.' - ), - license='Apache License, Version 2.0', - tests_require=['pytest'], - entry_points={ - 'launch.frontend.launch_extension': [ - 'launch_gz = launch_gz', - ], - } -) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 1129ced6..a9b3a1cc 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -27,12 +27,16 @@ find_package(gz-msgs REQUIRED) find_package(gz_sim_vendor REQUIRED) find_package(gz-sim REQUIRED) # Needed in launch/gz_sim.launch.py.in +set(GZ_SIM_VER ${gz-sim_VERSION_MAJOR}) gz_find_package(gflags REQUIRED PKGCONFIG gflags) find_package(std_msgs REQUIRED) +# Install the python module for this package +ament_python_install_package(${PROJECT_NAME}) + add_executable(create src/create.cpp) ament_target_dependencies(create rclcpp @@ -93,8 +97,8 @@ install(FILES ) install(FILES - "launch/gzserver.launch" - "launch/gzserver.launch.py" + "launch/gz_server.launch" + "launch/gz_server.launch.py" "launch/gz_spawn_model.launch.py" "launch/ros_gz_sim.launch.py" "launch/ros_gz_spawn_model.launch.py" diff --git a/ros_gz_sim/launch/gzserver.launch b/ros_gz_sim/launch/gz_server.launch similarity index 72% rename from ros_gz_sim/launch/gzserver.launch rename to ros_gz_sim/launch/gz_server.launch index 93d19d22..9a617649 100644 --- a/ros_gz_sim/launch/gzserver.launch +++ b/ros_gz_sim/launch/gz_server.launch @@ -3,10 +3,10 @@ - - + container_name="$(var container_name)" + use_composition="$(var use_composition)"> + diff --git a/ros_gz_sim/launch/gzserver.launch.py b/ros_gz_sim/launch/gz_server.launch.py similarity index 95% rename from ros_gz_sim/launch/gzserver.launch.py rename to ros_gz_sim/launch/gz_server.launch.py index 225d6b23..c73be0fe 100644 --- a/ros_gz_sim/launch/gzserver.launch.py +++ b/ros_gz_sim/launch/gz_server.launch.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Launch gzserver in a component container.""" +"""Launch gz_server in a component container.""" from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -56,7 +56,7 @@ def generate_launch_description(): ComposableNode( package='ros_gz_sim', plugin='ros_gz_sim::GzServer', - name='gzserver', + name='gz_server', parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'), 'world_sdf_string': LaunchConfiguration('world_sdf_string')}], extra_arguments=[{'use_intra_process_comms': True}], @@ -73,7 +73,7 @@ def generate_launch_description(): ld.add_action(declare_world_sdf_string_cmd) ld.add_action(declare_container_name_cmd) ld.add_action(declare_use_composition_cmd) - # Add the actions to launch all of the gzserver nodes + # Add the actions to launch all of the gz_server nodes ld.add_action(load_nodes) ld.add_action(load_composable_nodes) diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index dc85994b..cad862ea 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -83,11 +83,11 @@ def generate_launch_description(): ('use_respawn', use_respawn), ('bridge_log_level', bridge_log_level)]) - gzserver_description = IncludeLaunchDescription( + gz_server_description = IncludeLaunchDescription( PythonLaunchDescriptionSource( [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', - 'gzserver.launch.py'])]), + 'gz_server.launch.py'])]), launch_arguments=[('world_sdf_file', world_sdf_file), ('world_sdf_string', world_sdf_string), ('use_composition', use_composition), ]) @@ -104,8 +104,8 @@ def generate_launch_description(): ld.add_action(declare_bridge_log_level_cmd) ld.add_action(declare_world_sdf_file_cmd) ld.add_action(declare_world_sdf_string_cmd) - # Add the actions to launch all of the bridge + gzserver nodes + # Add the actions to launch all of the bridge + gz_server nodes ld.add_action(bridge_description) - ld.add_action(gzserver_description) + ld.add_action(gz_server_description) return ld diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index e0d4d95d..c53110a7 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -10,12 +10,22 @@ Apache 2.0 Alejandro Hernandez + Addisu Taddese + Carlos Agüero ament_cmake + ament_cmake_python pkg-config ament_index_python + composition_interfaces + launch + launch_ros libgflags-dev + lifecycle_msgs + osrf_pycommon + python3-importlib-metadata + python3-yaml rclcpp rclcpp_components std_msgs diff --git a/launch_gz/resource/launch_gz b/ros_gz_sim/resource/ros_gz_sim similarity index 100% rename from launch_gz/resource/launch_gz rename to ros_gz_sim/resource/ros_gz_sim diff --git a/launch_gz/launch_gz/__init__.py b/ros_gz_sim/ros_gz_sim/__init__.py similarity index 92% rename from launch_gz/launch_gz/__init__.py rename to ros_gz_sim/ros_gz_sim/__init__.py index dbc37965..17b9696c 100644 --- a/launch_gz/launch_gz/__init__.py +++ b/ros_gz_sim/ros_gz_sim/__init__.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Main entry point for the `launch_gz` package.""" +"""Main entry point for the `ros_gz_sim` package.""" from . import actions diff --git a/launch_gz/launch_gz/actions/__init__.py b/ros_gz_sim/ros_gz_sim/actions/__init__.py similarity index 100% rename from launch_gz/launch_gz/actions/__init__.py rename to ros_gz_sim/ros_gz_sim/actions/__init__.py diff --git a/launch_gz/launch_gz/actions/gzserver.py b/ros_gz_sim/ros_gz_sim/actions/gzserver.py similarity index 85% rename from launch_gz/launch_gz/actions/gzserver.py rename to ros_gz_sim/ros_gz_sim/actions/gzserver.py index 29512a45..7c722fe4 100644 --- a/launch_gz/launch_gz/actions/gzserver.py +++ b/ros_gz_sim/ros_gz_sim/actions/gzserver.py @@ -26,23 +26,23 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -@expose_action('gzserver') +@expose_action('gz_server') class GzServer(Action): - """Action that executes a gzserver ROS [composable] node.""" + """Action that executes a gz_server ROS [composable] node.""" def __init__( self, *, - world_sdf_file: SomeSubstitutionsType, - world_sdf_string: SomeSubstitutionsType, - container_name: SomeSubstitutionsType, - use_composition: SomeSubstitutionsType, + world_sdf_file: Optional[SomeSubstitutionsType], + world_sdf_string: Optional[SomeSubstitutionsType], + container_name: Optional[SomeSubstitutionsType], + use_composition: Optional[SomeSubstitutionsType], **kwargs ) -> None: """ - Construct a gzserver action. + Construct a gz_server action. - All arguments are forwarded to `ros_gz_sim.launch.gzserver.launch.py`, so see the documentation + All arguments are forwarded to `ros_gz_sim.launch.gz_server.launch.py`, so see the documentation of that class for further details. :param: world_sdf_file Path to the SDF world file. @@ -59,7 +59,7 @@ def __init__( @classmethod def parse(cls, entity: Entity, parser: Parser): - """Parse gzserver.""" + """Parse gz_server.""" _, kwargs = super().parse(entity, parser) world_sdf_file = entity.get_attr( @@ -101,15 +101,15 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]: Execute the action. """ - gzserver_description = IncludeLaunchDescription( + gz_server_description = IncludeLaunchDescription( PythonLaunchDescriptionSource( [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', - 'gzserver.launch.py'])]), + 'gz_server.launch.py'])]), launch_arguments=[('world_sdf_file', self.__world_sdf_file), ('world_sdf_string', self.__world_sdf_string), ('container_name', self.__container_name), ('use_composition', self.__use_composition), ]) - return [gzserver_description] + return [gz_server_description] diff --git a/ros_gz_sim/setup.cfg b/ros_gz_sim/setup.cfg new file mode 100644 index 00000000..ebdb35c8 --- /dev/null +++ b/ros_gz_sim/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +launch.frontend.launch_extension = + ros_gz_sim = ros_gz_sim \ No newline at end of file From 305097b3fe1889f7b2ccfe431d471df5971aa2bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 23 May 2024 21:38:24 +0200 Subject: [PATCH 19/20] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/package.xml | 5 ----- ros_gz_sim/src/gzserver.cpp | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index c53110a7..246a2fff 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -18,14 +18,9 @@ pkg-config ament_index_python - composition_interfaces launch launch_ros libgflags-dev - lifecycle_msgs - osrf_pycommon - python3-importlib-metadata - python3-yaml rclcpp rclcpp_components std_msgs diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 2346390d..aa4361f5 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -68,7 +68,7 @@ class GzServer : public rclcpp::Node gz::sim::Server server(server_config); server.Run(true /*blocking*/, 0, false /*paused*/); - rclcpp::shutdown() + rclcpp::shutdown(); } private: From 2416aafce0bd0b4e3ff2f1ca3ecfdb3e4d2813cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 23 May 2024 22:21:11 +0200 Subject: [PATCH 20/20] Mostly styule. 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_sim/actions/gzserver.py | 168 +++++++++++----------- ros_gz_sim/setup.cfg | 2 +- 2 files changed, 83 insertions(+), 87 deletions(-) diff --git a/ros_gz_sim/ros_gz_sim/actions/gzserver.py b/ros_gz_sim/ros_gz_sim/actions/gzserver.py index 7c722fe4..14df9aab 100644 --- a/ros_gz_sim/ros_gz_sim/actions/gzserver.py +++ b/ros_gz_sim/ros_gz_sim/actions/gzserver.py @@ -19,97 +19,93 @@ from launch.action import Action from launch.actions import IncludeLaunchDescription -from launch.frontend import expose_action, Entity, Parser +from launch.frontend import Entity, expose_action, Parser from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare + @expose_action('gz_server') class GzServer(Action): - """Action that executes a gz_server ROS [composable] node.""" - - def __init__( - self, - *, - world_sdf_file: Optional[SomeSubstitutionsType], - world_sdf_string: Optional[SomeSubstitutionsType], - container_name: Optional[SomeSubstitutionsType], - use_composition: Optional[SomeSubstitutionsType], - **kwargs - ) -> None: - """ - Construct a gz_server action. - - All arguments are forwarded to `ros_gz_sim.launch.gz_server.launch.py`, so see the documentation - of that class for further details. - - :param: world_sdf_file Path to the SDF world file. - :param: world_sdf_string SDF world string. - :param: container_name Name of container that nodes will load in if use composition. - :param: use_composition Use composed bringup if True. - """ - - super().__init__(**kwargs) - self.__world_sdf_file = world_sdf_file - self.__world_sdf_string = world_sdf_string - self.__container_name = container_name - self.__use_composition = use_composition - - @classmethod - def parse(cls, entity: Entity, parser: Parser): - """Parse gz_server.""" - _, kwargs = super().parse(entity, parser) - - world_sdf_file = entity.get_attr( - 'world_sdf_file', data_type=str, - optional=True) - - world_sdf_string = entity.get_attr( - 'world_sdf_string', data_type=str, - optional=True) - - container_name = entity.get_attr( - 'container_name', data_type=str, - optional=True) - - use_composition = entity.get_attr( - 'use_composition', data_type=str, - optional=True) - - if isinstance(world_sdf_file, str): - world_sdf_file = parser.parse_substitution(world_sdf_file) - kwargs['world_sdf_file'] = world_sdf_file - - if isinstance(world_sdf_string, str): - world_sdf_string = parser.parse_substitution(world_sdf_string) - kwargs['world_sdf_string'] = world_sdf_string - - if isinstance(container_name, str): - container_name = parser.parse_substitution(container_name) - kwargs['container_name'] = container_name - - if isinstance(use_composition, str): - use_composition = parser.parse_substitution(use_composition) - kwargs['use_composition'] = use_composition - - return cls, kwargs - - def execute(self, context: LaunchContext) -> Optional[List[Action]]: - """ - Execute the action. - """ - - gz_server_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), - 'launch', - 'gz_server.launch.py'])]), - launch_arguments=[('world_sdf_file', self.__world_sdf_file), - ('world_sdf_string', self.__world_sdf_string), - ('container_name', self.__container_name), - ('use_composition', self.__use_composition), - ]) - - return [gz_server_description] + """Action that executes a gz_server ROS [composable] node.""" + + def __init__( + self, + *, + world_sdf_file: Optional[SomeSubstitutionsType] = None, + world_sdf_string: Optional[SomeSubstitutionsType] = None, + container_name: Optional[SomeSubstitutionsType] = None, + use_composition: Optional[SomeSubstitutionsType] = None, + **kwargs + ) -> None: + """ + Construct a gz_server action. + + All arguments are forwarded to `ros_gz_sim.launch.gz_server.launch.py`, + so see the documentation of that class for further details. + + :param: world_sdf_file Path to the SDF world file. + :param: world_sdf_string SDF world string. + :param: container_name Name of container that nodes will load in if use composition. + :param: use_composition Use composed bringup if True. + """ + super().__init__(**kwargs) + self.__world_sdf_file = world_sdf_file + self.__world_sdf_string = world_sdf_string + self.__container_name = container_name + self.__use_composition = use_composition + + @classmethod + def parse(cls, entity: Entity, parser: Parser): + """Parse gz_server.""" + _, kwargs = super().parse(entity, parser) + + world_sdf_file = entity.get_attr( + 'world_sdf_file', data_type=str, + optional=True) + + world_sdf_string = entity.get_attr( + 'world_sdf_string', data_type=str, + optional=True) + + container_name = entity.get_attr( + 'container_name', data_type=str, + optional=True) + + use_composition = entity.get_attr( + 'use_composition', data_type=str, + optional=True) + + if isinstance(world_sdf_file, str): + world_sdf_file = parser.parse_substitution(world_sdf_file) + kwargs['world_sdf_file'] = world_sdf_file + + if isinstance(world_sdf_string, str): + world_sdf_string = parser.parse_substitution(world_sdf_string) + kwargs['world_sdf_string'] = world_sdf_string + + if isinstance(container_name, str): + container_name = parser.parse_substitution(container_name) + kwargs['container_name'] = container_name + + if isinstance(use_composition, str): + use_composition = parser.parse_substitution(use_composition) + kwargs['use_composition'] = use_composition + + return cls, kwargs + + def execute(self, context: LaunchContext) -> Optional[List[Action]]: + """Execute the action.""" + gz_server_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_server.launch.py'])]), + launch_arguments=[('world_sdf_file', self.__world_sdf_file), + ('world_sdf_string', self.__world_sdf_string), + ('container_name', self.__container_name), + ('use_composition', self.__use_composition), ]) + + return [gz_server_description] diff --git a/ros_gz_sim/setup.cfg b/ros_gz_sim/setup.cfg index ebdb35c8..c044b41d 100644 --- a/ros_gz_sim/setup.cfg +++ b/ros_gz_sim/setup.cfg @@ -1,3 +1,3 @@ [options.entry_points] launch.frontend.launch_extension = - ros_gz_sim = ros_gz_sim \ No newline at end of file + ros_gz_sim = ros_gz_sim