Adding files.

Signed-off-by: Carlos Agüero <[email protected]>

ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp

@@ -0,0 +1,57 @@
+// 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_BRIDGE_SYSTEM_HPP_
+#define ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_
+
+#include <memory>
+#include <gz/sim/System.hh>
+#include <sdf/sdf.hh>
+
+namespace ros_gz_sim
+{
+// Private class.
+class ROSGzBridgeSystemPrivate;
+
+/// Important: This system, although functional, it's still under development.
+/// The API or its parameters can change in the near future.
+/// \brief A ROS 2 <--> gz bridge system.
+///
+/// This system can be configured with the following SDF parameters:
+/// * Optional parameters:
+///    * <config_file>: Path to a YAML file containing the list of topics to be
+///                     bridged.
+class ROSGzBridgeSystem
+  : public gz::sim::System,
+  public gz::sim::ISystemConfigure
+{
+public:
+  // \brief Constructor.
+  ROSGzBridgeSystem();
+
+  /// \brief Destructor.
+  ~ROSGzBridgeSystem() 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<ROSGzBridgeSystemPrivate> dataPtr;
+};
+}  // namespace ros_gz_sim
+#endif  // ROS_GZ_SIM__ROS_GZ_BRIDGE_SYSTEM_HPP_

ros_gz_sim/src/ros_gz_bridge_system.cpp

@@ -0,0 +1,106 @@
+// 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/bridge_config.hpp>
+#include <ros_gz_bridge/ros_gz_bridge.hpp>
+
+#include "ros_gz_bridge_system.hpp"
+
+namespace ros_gz_sim
+{
+/// \brief Private ROSGzBridgeSystem data class.
+class ROSGzBridgeSystemPrivate
+{
+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;
+};
+
+//////////////////////////////////////////////////
+ROSGzBridgeSystem::ROSGzBridgeSystem()
+: System(), dataPtr(new ROSGzBridgeSystemPrivate())
+{
+}
+
+//////////////////////////////////////////////////
+ROSGzBridgeSystem::~ROSGzBridgeSystem()
+{
+  if (this->dataPtr->exec) {
+    this->dataPtr->exec->cancel();
+    this->dataPtr->thread.join();
+  }
+}
+
+//////////////////////////////////////////////////
+void ROSGzBridgeSystem::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 GZ_SIM_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
+
+GZ_ADD_PLUGIN(
+  ros_gz_sim::ROSGzBridgeSystem,
+  gz::sim::System,
+  ros_gz_sim::ROSGzBridgeSystem::ISystemConfigure)
+
+GZ_ADD_PLUGIN_ALIAS(
+  ros_gz_sim::ROSGzBridgeSystem,
+  "ros_gz_sim::ROSGzBridge")

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: "gz.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: "gz.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="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-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::ROSGzBridge"
+            filename="ros-gz-bridge-system">
+      <config_file>package://ros_gz_sim_demos/config/full.yaml</config_file>
+    </plugin>
+
+  </world>
+</sdf>