Bridge as a gz sim system

ros_gz_bridge/CMakeLists.txt

@@ -148,7 +148,9 @@ ament_target_dependencies(${bridge_lib}
 )
 
 target_include_directories(${bridge_lib}
-  PUBLIC include
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/>"
   PRIVATE src ${generated_path}
 )
 
@@ -162,13 +164,13 @@ rclcpp_components_register_node(
   PLUGIN ros_gz_bridge::RosGzBridge
   EXECUTABLE bridge_node)
 
-install(TARGETS ${bridge_lib}
+install(TARGETS ${bridge_lib} EXPORT export_${PROJECT_NAME}
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
         RUNTIME DESTINATION bin)
 
 install(
-  DIRECTORY include/
+  DIRECTORY include/${PROJECT_NAME}
   DESTINATION include
 )
 
@@ -348,6 +350,8 @@ if(BUILD_TESTING)
 
 endif()
 
+ament_export_targets(export_${PROJECT_NAME})
+
 ament_export_dependencies(
   rclcpp
   ${BRIDGE_MESSAGE_TYPES}

ros_gz_image/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(ros_gz_bridge REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
 # TODO(CH3): Deprecated. Remove on tock.

ros_gz_sim/CMakeLists.txt

@@ -12,6 +12,8 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(ros_gz_bridge REQUIRED)
 find_package(std_msgs REQUIRED)
 
 # TODO(CH3): Deprecated. Remove on tock.
@@ -20,6 +22,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 +39,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 +93,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 +133,21 @@ 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_lib
+)
+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,116 @@
+// 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 <filesystem>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <thread>
+
+#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()
+{
+  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::filesystem::path configFile = _sdf->Get<std::string>("config_file");
+  if (!std::filesystem::is_regular_file(configFile)) {
+    std::cerr << "[" << configFile << "] is not a regular file. Plugin disabled"
+              << 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", configFile));
+
+  // 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/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,
+    ])