From 733f3477a68e948811088a8009131860451f8cee Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Thu, 14 Nov 2024 10:20:27 +0100 Subject: [PATCH] Add remove entity node (#629) Signed-off-by: Wiktor Bajor (cherry picked from commit 04446e08e5a1153043e074528a7a3f2ec55449ab) --- ros_gz_sim/CMakeLists.txt | 26 ++++- ros_gz_sim/launch/gz_remove_model.launch.py | 50 +++++++++ ros_gz_sim/src/remove.cpp | 110 ++++++++++++++++++++ ros_gz_sim/test/test_remove.cpp | 56 ++++++++++ ros_gz_sim/test/test_remove_node.launch.py | 49 +++++++++ 5 files changed, 289 insertions(+), 2 deletions(-) create mode 100644 ros_gz_sim/launch/gz_remove_model.launch.py create mode 100644 ros_gz_sim/src/remove.cpp create mode 100644 ros_gz_sim/test/test_remove.cpp create mode 100644 ros_gz_sim/test/test_remove_node.launch.py diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index dc21f8a9..e61622f9 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -48,7 +48,17 @@ target_link_libraries(create gz-msgs::core gz-transport::core ) -ament_target_dependencies(create std_msgs) + +add_executable(remove src/remove.cpp) +ament_target_dependencies(remove + rclcpp + std_msgs +) + +target_link_libraries(remove + gz-msgs::core + gz-transport::core +) add_library(${PROJECT_NAME} SHARED src/Stopwatch.cpp) ament_target_dependencies(${PROJECT_NAME} @@ -104,13 +114,16 @@ install(FILES "launch/ros_gz_sim.launch" "launch/ros_gz_sim.launch.py" "launch/ros_gz_spawn_model.launch.py" + "launch/gz_remove_model.launch.py" DESTINATION share/${PROJECT_NAME}/launch ) install(TARGETS create + remove DESTINATION lib/${PROJECT_NAME} ) + install(TARGETS gzserver DESTINATION lib/${PROJECT_NAME} @@ -151,6 +164,10 @@ if(BUILD_TESTING) test/test_create.cpp ) + ament_add_gtest_executable(test_remove + test/test_remove.cpp + ) + ament_target_dependencies(test_stopwatch rclcpp) target_include_directories(test_stopwatch PUBLIC @@ -165,12 +182,17 @@ if(BUILD_TESTING) gz-transport::core ) + target_link_libraries(test_remove + gz-transport::core + ) + install( - TARGETS test_stopwatch test_create + TARGETS test_stopwatch test_create test_remove DESTINATION lib/${PROJECT_NAME} ) ament_add_gtest_test(test_stopwatch) add_launch_test(test/test_create_node.launch.py TIMEOUT 200) + add_launch_test(test/test_remove_node.launch.py TIMEOUT 200) endif() ament_package() diff --git a/ros_gz_sim/launch/gz_remove_model.launch.py b/ros_gz_sim/launch/gz_remove_model.launch.py new file mode 100644 index 00000000..14dd643a --- /dev/null +++ b/ros_gz_sim/launch/gz_remove_model.launch.py @@ -0,0 +1,50 @@ +# 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 remove 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') + entity_name = LaunchConfiguration('entity_name') + + declare_world_cmd = DeclareLaunchArgument( + 'world', default_value=TextSubstitution(text=''), + description='World name') + declare_entity_name_cmd = DeclareLaunchArgument( + 'entity_name', default_value=TextSubstitution(text=''), + description='SDF filename') + + remove = Node( + package='ros_gz_sim', + executable='remove', + output='screen', + parameters=[{'world': world, 'entity_name': entity_name}], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_cmd) + ld.add_action(declare_entity_name_cmd) + ld.add_action(remove) + + return ld diff --git a/ros_gz_sim/src/remove.cpp b/ros_gz_sim/src/remove.cpp new file mode 100644 index 00000000..43df1758 --- /dev/null +++ b/ros_gz_sim/src/remove.cpp @@ -0,0 +1,110 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include + +#include +#include + + +constexpr unsigned int timeout{5000}; +std::optional retrieve_world_name(const rclcpp::Node::SharedPtr & ros2_node) +{ + gz::transport::Node node; + bool executed{}; + bool result{}; + const std::string service{"/gazebo/worlds"}; + gz::msgs::StringMsg_V worlds_msg; + + // This loop is here to allow the server time to download resources. + while (rclcpp::ok() && !executed) { + RCLCPP_INFO(ros2_node->get_logger(), "Requesting list of world names."); + executed = node.Request(service, timeout, worlds_msg, result); + } + + if (!executed) { + RCLCPP_INFO(ros2_node->get_logger(), "Timed out when getting world names."); + return std::nullopt; + } + + if (!result || worlds_msg.data().empty()) { + RCLCPP_INFO(ros2_node->get_logger(), "Failed to get world names."); + return std::nullopt; + } + + return worlds_msg.data(0); +} + +int remove_entity( + const std::string & entity_name, + const std::string & world_name, + const rclcpp::Node::SharedPtr & ros2_node) +{ + const std::string service{"/world/" + world_name + "/remove"}; + gz::msgs::Entity entity_remove_request; + entity_remove_request.set_name(entity_name); + entity_remove_request.set_type(gz::msgs::Entity_Type_MODEL); + gz::transport::Node node; + gz::msgs::Boolean response; + bool result; + + while(rclcpp::ok() && !node.Request(service, entity_remove_request, timeout, response, result)) { + RCLCPP_WARN( + ros2_node->get_logger(), "Waiting for service [%s] to become available ...", + service.c_str()); + } + if (result && response.data()) { + RCLCPP_INFO(ros2_node->get_logger(), "Entity removal successful."); + return 0; + } else { + RCLCPP_ERROR( + ros2_node->get_logger(), "Entity removal failed.\n %s", + entity_remove_request.DebugString().c_str()); + return 1; + } + RCLCPP_INFO(ros2_node->get_logger(), "Entity removal was interrupted."); + return 1; +} + +int main(int _argc, char ** _argv) +{ + rclcpp::init(_argc, _argv); + auto ros2_node = rclcpp::Node::make_shared("ros_gz_sim_remove_entity"); + ros2_node->declare_parameter("world", ""); + ros2_node->declare_parameter("entity_name", ""); + + std::string world_name = ros2_node->get_parameter("world").as_string(); + if (world_name.empty()) { + world_name = retrieve_world_name(ros2_node).value_or(""); + if (world_name.empty()) { + return -1; + } + } + + const std::string entity_name = + ros2_node->get_parameter("entity_name").as_string(); + if (entity_name.empty()) { + RCLCPP_INFO(ros2_node->get_logger(), + "Entity to remove name is not provided, entity will not be removed."); + return -1; + } + + return remove_entity(entity_name, world_name, ros2_node); +} diff --git a/ros_gz_sim/test/test_remove.cpp b/ros_gz_sim/test/test_remove.cpp new file mode 100644 index 00000000..74a8f2ce --- /dev/null +++ b/ros_gz_sim/test/test_remove.cpp @@ -0,0 +1,56 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include + +#include + +// Simple application that provides a `/remove` service and prints out the +// request's entity name. This works in conjuction with +// test_remove_node.launch.py +int main() +{ + std::mutex m; + std::condition_variable cv; + bool test_complete = false; + + gz::transport::Node node; + auto cb = std::function( + [&]( + const gz::msgs::Entity & _req, + gz::msgs::Boolean & _res) -> bool { + std::cout << _req.name() << std::endl; + _res.set_data(true); + + { + std::lock_guard lk(m); + test_complete = true; + } + cv.notify_one(); + return true; + }); + + node.Advertise("/world/default/remove", cb); + // wait until we receive a message. + std::unique_lock lk(m); + cv.wait(lk, [&] {return test_complete;}); + // Sleep so that the service response can be sent before exiting. + std::this_thread::sleep_for(std::chrono::seconds(1)); +} diff --git a/ros_gz_sim/test/test_remove_node.launch.py b/ros_gz_sim/test/test_remove_node.launch.py new file mode 100644 index 00000000..ebca2429 --- /dev/null +++ b/ros_gz_sim/test/test_remove_node.launch.py @@ -0,0 +1,49 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import unittest + +from launch import LaunchDescription + +from launch_ros.actions import Node + +import launch_testing + + +def generate_test_description(): + entity_name = 'my_robot' + remove = Node(package='ros_gz_sim', + executable='remove', + parameters=[{'world': 'default', 'entity_name': entity_name}], + output='screen') + test_remove = Node(package='ros_gz_sim', executable='test_remove', output='screen') + return LaunchDescription([ + remove, + test_remove, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ]), locals() + + +class WaitForTests(unittest.TestCase): + + def test_termination(self, test_remove, proc_info): + proc_info.assertWaitForShutdown(process=test_remove, timeout=200) + + +@launch_testing.post_shutdown_test() +class RemoveTest(unittest.TestCase): + + def test_output(self, entity_name, test_remove, proc_output): + launch_testing.asserts.assertInStdout(proc_output, entity_name, test_remove)