Bridge as a gz sim system

ros_gz_sim/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(ros_gz_bridge REQUIRED)
 find_package(std_msgs REQUIRED)
 
 # TODO(CH3): Deprecated. Remove on tock.
@@ -20,6 +21,8 @@ if("$ENV{GZ_VERSION}" STREQUAL "" AND NOT "$ENV{IGNITION_VERSION}" STREQUAL "")
   set(ENV{GZ_VERSION} $ENV{IGNITION_VERSION})
 endif()
 
+set(GZ_TARGET_SIM gz-sim)
+
 # Edifice
 if("$ENV{GZ_VERSION}" STREQUAL "edifice")
   find_package(ignition-math6 REQUIRED)
@@ -35,6 +38,7 @@ if("$ENV{GZ_VERSION}" STREQUAL "edifice")
   set(GZ_SIM_VER ${ignition-gazebo5_VERSION_MAJOR})
 
   set(GZ_TARGET_PREFIX ignition)
+  set(GZ_TARGET_SIM ignition-gazebo)
   macro(gz_find_package)
     ign_find_package(${ARGV})
   endmacro()
@@ -88,6 +92,7 @@ else()
   set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR})
 
   set(GZ_TARGET_PREFIX ignition)
+  set(GZ_TARGET_SIM ignition-gazebo)
 
   message(STATUS "Compiling against Gazebo Fortress")
   macro(gz_find_package)
@@ -127,6 +132,22 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION include/${PROJECT_NAME}
 )
 
+# ROS Gz bridge
+add_library(ROSGzPlugin SHARED src/ros_gz.cpp)
+target_link_libraries(ROSGzPlugin PUBLIC
+  ${GZ_TARGET_SIM}${GZ_SIM_VER}::core
+  rclcpp::rclcpp
+  ros_gz_bridge::ros_gz_bridge
+)
+target_include_directories(ROSGzPlugin PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/ros_gz_sim>"
+  "$<INSTALL_INTERFACE:include/ros_gz_sim>"
+)
+install(
+  TARGETS ROSGzPlugin
+  DESTINATION lib
+)
+
 configure_file(
   launch/gz_sim.launch.py.in
   launch/gz_sim.launch.py.configured

ros_gz_sim/include/ros_gz_sim/ros_gz.hpp

@@ -0,0 +1,50 @@
+// Copyright 2024 Open Source Robotics Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS_GZ_SIM__ROS_GZ_HPP_
+#define ROS_GZ_SIM__ROS_GZ_HPP_
+
+#include <memory>
+#include <gz/sim/System.hh>
+#include <sdf/sdf.hh>
+
+namespace ros_gz_sim
+{
+// Private class.
+class ROSGzPluginPrivate;
+
+/// \brief ToDo
+class ROSGzPlugin
+  : public gz::sim::System,
+  public gz::sim::ISystemConfigure
+{
+public:
+  // \brief Constructor.
+  ROSGzPlugin();
+
+  /// \brief Destructor.
+  ~ROSGzPlugin() override;
+
+  // Documentation inherited.
+  void Configure(
+    const gz::sim::Entity & _entity,
+    const std::shared_ptr<const sdf::Element> & _sdf,
+    gz::sim::EntityComponentManager & _ecm,
+    gz::sim::EventManager & _eventMgr) override;
+
+  /// \brief Private data pointer.
+  std::unique_ptr<ROSGzPluginPrivate> dataPtr;
+};
+}  // namespace ros_gz_sim
+#endif  // ROS_GZ_SIM__ROS_GZ_HPP_

ros_gz_sim/package.xml

@@ -16,6 +16,7 @@
   <depend>ament_index_python</depend>
   <depend>libgflags-dev</depend>
   <depend>rclcpp</depend>
+  <depend>ros_gz_bridge</depend>
   <depend>std_msgs</depend>
 
   <!-- Harmonic -->

ros_gz_sim/src/ros_gz.cpp

@@ -0,0 +1,119 @@
+// 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 <iostream>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <gz/common/Util.hh>
+#include <gz/plugin/Register.hh>
+#include <rclcpp/rclcpp.hpp>
+#include <ros_gz_bridge/ros_gz_bridge.hpp>
+#include <ros_gz_bridge/bridge_config.hpp>
+
+#include "ros_gz.hpp"
+
+namespace ros_gz_sim
+{
+/// \brief Private ROSGzPlugin data class.
+class ROSGzPluginPrivate
+{
+public:
+  /// \brief The ROS 2 <--> Gz bridge.
+  std::shared_ptr<ros_gz_bridge::RosGzBridge> bridge;
+
+  /// \brief The ROS 2 executor.
+  std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> exec;
+
+  /// \brief A thread to call spin and not block the Gazebo thread.
+  std::thread thread;
+};
+
+//////////////////////////////////////////////////
+ROSGzPlugin::ROSGzPlugin()
+: System(), dataPtr(new ROSGzPluginPrivate())
+{
+}
+
+//////////////////////////////////////////////////
+ROSGzPlugin::~ROSGzPlugin()
+{
+  if (this->dataPtr->exec) {
+    this->dataPtr->exec->cancel();
+    this->dataPtr->thread.join();
+  }
+}
+
+//////////////////////////////////////////////////
+void ROSGzPlugin::Configure(
+  const gz::sim::Entity & /*_entity*/,
+  const std::shared_ptr<const sdf::Element> & _sdf,
+  gz::sim::EntityComponentManager & /*_ecm*/,
+  gz::sim::EventManager & /*_eventMgr*/)
+{
+  // Ensure that ROS is setup.
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  if (!_sdf->HasElement("config_file")) {
+    std::cerr << "No <config_file> found. Plugin disabled." << std::endl;
+    return;
+  }
+
+  // Sanity check: Make sure that the config file exists and it's a file.
+  std::string filename = _sdf->Get<std::string>("config_file");
+  std::string path = gz::common::findFile(filename);
+  if (!gz::common::isFile(path)) {
+    std::cerr << "Unable to open YAML file [" << filename
+              << "], check your GAZEBO_RESOURCE_PATH settings." << std::endl;
+    return;
+  }
+
+  // Create the bridge passing the parameters as rclcpp::NodeOptions().
+  this->dataPtr->bridge = std::make_shared<ros_gz_bridge::RosGzBridge>(
+    rclcpp::NodeOptions().append_parameter_override("config_file", path));
+
+  // Create the executor.
+  this->dataPtr->exec =
+    std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  this->dataPtr->exec->add_node(this->dataPtr->bridge);
+
+  // Spin in a separate thread to not block Gazebo.
+  this->dataPtr->thread = std::thread([this]() {this->dataPtr->exec->spin();});
+}
+}  // namespace ros_gz_sim
+
+#if (IGNITION_GAZEBO_MAJOR_VERSION == 6)
+IGNITION_ADD_PLUGIN(
+  ros_gz_sim::ROSGzPlugin,
+  gz::sim::System,
+  ros_gz_sim::ROSGzPlugin::ISystemConfigure)
+#else
+GZ_ADD_PLUGIN(
+  ros_gz_sim::ROSGzPlugin,
+  gz::sim::System,
+  ros_gz_sim::ROSGzPlugin::ISystemConfigure)
+#endif
+
+#if (IGNITION_GAZEBO_MAJOR_VERSION == 6)
+IGNITION_ADD_PLUGIN_ALIAS(
+  ros_gz_sim::ROSGzPlugin,
+  "ros_gz_sim::ROSGzPlugin")
+#else
+GZ_ADD_PLUGIN_ALIAS(
+  ros_gz_sim::ROSGzPlugin,
+  "ros_gz_sim::ROSGzPlugin")
+#endif

ros_gz_sim_demos/CMakeLists.txt

@@ -33,6 +33,12 @@ install(
   DESTINATION share/${PROJECT_NAME}/worlds
 )
 
+install(
+  DIRECTORY
+    config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
 
 ament_package()

ros_gz_sim_demos/config/full.yaml

@@ -0,0 +1,18 @@
+# Full set of configurations
+- ros_topic_name: "ros_chatter"
+  gz_topic_name: "gz_chatter"
+  ros_type_name: "std_msgs/msg/String"
+  gz_type_name: "ignition.msgs.StringMsg"
+  subscriber_queue: 5
+  publisher_queue: 6
+  lazy: true
+  direction: ROS_TO_GZ
+
+- ros_topic_name: "ros_chatter"
+  gz_topic_name: "gz_chatter"
+  ros_type_name: "std_msgs/msg/String"
+  gz_type_name: "ignition.msgs.StringMsg"
+  subscriber_queue: 10
+  publisher_queue: 20
+  lazy: false
+  direction: GZ_TO_ROS

ros_gz_sim_demos/launch/ros_gz.launch.py

@@ -0,0 +1,42 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+
+
+def generate_launch_description():
+
+    pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'),
+        ),
+        launch_arguments={'gz_args': PathJoinSubstitution([
+            pkg_ros_gz_sim_demos,
+            'worlds',
+            'ros_gz.sdf'
+        ])}.items(),
+    )
+
+    return LaunchDescription([
+        gazebo,
+    ])

ros_gz_sim_demos/worlds/ros_gz.sdf

@@ -0,0 +1,77 @@
+<?xml version="1.0" ?>
+<!--
+  ROS 2 <-> Gazebo Sim plugin demo.
+
+-->
+<sdf version="1.8">
+  <world name="demo">
+
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-contact-system"
+      name="gz::sim::systems::Contact">
+    </plugin>
+
+    <light name="sun" type="directional">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <plugin name="ros_gz_sim::ROSGzPlugin"
+            filename="libROSGzPlugin.so">
+      <config_file>ros_gz_sim_demos/config/full.yaml</config_file>
+    </plugin>
+
+  </world>
+</sdf>