From 790050df8dcdfff83dcf811b9eef4f655ef67ffa Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Tue, 25 Jun 2024 09:33:15 +0200
Subject: [PATCH] Add a ROS node that runs Gazebo (#500) (#567)

* Add gzserver with ability to load an SDF file or string

---------

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
(cherry picked from commit 92a2891f4adf35e4a4119aca2447dee93e22a06a)

Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 ros_gz_sim/CMakeLists.txt   | 14 ++++++++++++
 ros_gz_sim/src/gzserver.cpp | 44 +++++++++++++++++++++++++++++++++++++
 2 files changed, 58 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 1cf5b4cc..125b28dd 100644
--- a/ros_gz_sim/CMakeLists.txt
+++ b/ros_gz_sim/CMakeLists.txt
@@ -60,6 +60,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-sim::core
+)
+ament_target_dependencies(gzserver std_msgs)
+
 configure_file(
   launch/gz_sim.launch.py.in
   launch/gz_sim.launch.py.configured
@@ -79,6 +89,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..530f23c7
--- /dev/null
+++ b/ros_gz_sim/src/gzserver.cpp
@@ -0,0 +1,44 @@
+// 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/common/Console.hh>
+#include <gz/sim/Server.hh>
+#include <gz/sim/SystemLoader.hh>
+#include <gz/sim/ServerConfig.hh>
+#include <rclcpp/rclcpp.hpp>
+
+// ROS node that executes a gz-sim Server given a world SDF file or string.
+int main(int _argc, char ** _argv)
+{
+  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", "");
+
+  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(
+      node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'");
+    return -1;
+  }
+
+  gz::sim::Server server(server_config);
+  server.Run(true /*blocking*/, 0, false /*paused*/);
+}