Add remove entity node (#629) (#647)

Signed-off-by: Wiktor Bajor <[email protected]>
(cherry picked from commit 04446e0)

Co-authored-by: Wiktor Bajor <[email protected]>

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()

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

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 <gz/msgs/boolean.pb.h>
+#include <gz/msgs/entity.pb.h>
+#include <gz/msgs/stringmsg_v.pb.h>
+
+#include <string>
+#include <optional>
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+#include <gz/transport/Node.hh>
+
+
+constexpr unsigned int timeout{5000};
+std::optional<std::string> 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);
+}

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 <gz/msgs/boolean.pb.h>
+#include <gz/msgs/entity.pb.h>
+
+#include <chrono>
+#include <condition_variable>
+#include <functional>
+#include <mutex>
+
+#include <gz/transport/Node.hh>
+
+// 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<std::mutex> 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<std::mutex> 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));
+}

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)