initial commit

.github/workflows/format.yaml

@@ -0,0 +1,17 @@
+name: Format
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+      - main
+
+jobs:
+  pre-commit:
+    name: pre-commit
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v3
+      - uses: actions/setup-python@v3
+      - uses: pre-commit/action@v3.0.0

.pre-commit-config.yaml

@@ -0,0 +1,38 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: fix-byte-order-marker
+  - repo: https://github.com/psf/black
+    rev: 24.10.0
+    hooks:
+      - id: black
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.3.0
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: CHANGELOG.rst

CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.5)
+project(realsense2_gz_description)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY urdf launch rviz world
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

LICENSE.txt

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2024, Marques Rasmussen.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,68 @@
+# Realsense Gazebo description
+
+[![Format Status](https://github.com/MarqRazz/realsense2_gz_description/actions/workflows/format.yaml/badge.svg)](https://github.com/MarqRazz/realsense2_gz_description/actions/workflows/format.yaml)
+
+Description: This ROS 2 package is designed to be used in unison with [realsense2_description](https://github.com/IntelRealSense/realsense-ros/tree/ros2-master/realsense2_description) and allows for easy definition of Realsense cameras that can be simulated in Gazebo Fortress and newer. It may support other versions of Ignition Gazebo but this has not been tested.
+
+## Running Example Launch
+
+This package includes a launch file to start Gazebo, bridge the data to ROS 2 and display the simulated camera data in Rviz.
+
+Once you have built this package and sourced your workspace you can run
+```bash
+ros2 launch realsense2_gz_description example_realsense_gazebo.launch.py
+```
+
+> Note: you can specify `headless:=false` and it will also open the Gazebo GUI.
+
+Which should start a simulated camera in Gazebo with a few objects in front of it. In the Rviz window that launches you can see the RGB images streaming along with the point cloud in the main view.
+
+<img src="doc/realsense_gazebo.png"  width="50%" >
+
+
+# Example Usage in URDF
+
+In your robots urdf.xacro include the desired Realsense model along with its gazebo description.
+```xml
+<xacro:include filename="$(find realsense2_description)/urdf/_d415.urdf.xacro" />
+<xacro:include filename="$(find realsense2_gz_description)/urdf/_d415.gazebo.xacro" />
+
+```
+Then call the xacros and specify the same `name` and other optional arguments.
+```xml
+<!-- URDF xacro-->
+<xacro:sensor_d415 parent="world" name="$(arg camera_name)" ...>
+  <origin xyz="0 0 0" rpy="0 0 0"/>
+</xacro:sensor_d415>
+<!-- Gazebo xacro-->
+<xacro:gazebo_d415 name="$(arg camera_name)" .../>
+```
+
+> Note: Gazebo plugins can only be included once so the xacros in this repo assume that a parent will include/run the required Sensors plugin when starting simulation.
+This plugin can be started from your URDF or world.sdf file.
+```xml
+<gazebo>
+  <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+    <render_engine>ogre2</render_engine>
+  </plugin>
+</gazebo>
+```  
+
+you can also refer to the the [example.urdf.xacro](./urdf/example_d415_gazebo.urdf.xacro) included.
+
+## Contributing
+
+pre-commit is a tool to automatically run formatting checks on each commit, which saves you from manually running them.
+This repo requires formatting to pass before changes will be accepted.
+
+Install pre-commit like this:
+
+```
+pip3 install pre-commit
+```
+
+Run this in the top directory of the repo to set up the git hooks:
+
+```
+pre-commit install
+```

launch/example_realsense_gazebo.launch.py

@@ -0,0 +1,170 @@
+# Author: Marq Rasmussen
+
+from launch import LaunchDescription, LaunchContext
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+
+
+def launch_gz(context: LaunchContext):
+    gz_world_file = (
+        get_package_share_directory("realsense2_gz_description") + "/world/example.sdf"
+    )
+    # -r is to run the simulation on start
+    # -v is the verbose level
+    #  0: No output, 1: Error, 2: Error and warning, 3: Error, warning, and info, 4: Error, warning, info, and debug.
+    sim_options = "-r -v 3"
+    if LaunchConfiguration("headless").perform(context) == "true":
+        sim_options += " -s"  # -s is to only run the server (headless mode).
+    gz_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"]
+        ),
+        launch_arguments=[("gz_args", [f"{sim_options} {gz_world_file}"])],
+    )
+    return [gz_launch_description]
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument("rviz", default_value="true", description="Launch RViz?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "headless", default_value="true", description="Launch Gazebo headless?"
+        )
+    )
+
+    # Initialize Arguments
+    launch_rviz = LaunchConfiguration("rviz")
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("realsense2_gz_description"), "rviz", "example.rviz"]
+    )
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("realsense2_gz_description"),
+                    "urdf",
+                    "example_d415_gazebo.urdf.xacro",
+                ]
+            ),
+            " ",
+            "camera_name:=gz_camera",
+            " ",
+        ]
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[
+            {
+                "use_sim_time": True,
+                "robot_description": robot_description_content,
+            }
+        ],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        parameters=[{"use_sim_time": True}],
+        arguments=["-d", rviz_config_file],
+        condition=IfCondition(launch_rviz),
+    )
+
+    ignition_spawn_entity = Node(
+        package="ros_gz_sim",
+        executable="create",
+        output="screen",
+        arguments=[
+            "-string",
+            robot_description_content,
+            "-name",
+            "realsense_camera",
+            "-allow_renaming",
+            "true",
+            "-x",
+            "0.0",
+            "-y",
+            "0.0",
+            "-z",
+            "0.0",
+            "-R",
+            "0.0",
+            "-P",
+            "0.0",
+            "-Y",
+            "0.0",
+        ],
+    )
+
+    # Bridge the camera data to ROS and match the default topics that the real camera would publish
+    # Note the gz_topic_name comes from _realsense_model.gazebo.xacro defaults which defaults to `camera` here.
+    gazebo_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[{"use_sim_time": True}],
+        arguments=[
+            "/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image",
+            "/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image",
+            "/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked",
+            "/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo",
+            "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
+        ],
+        remappings=[
+            (
+                "/camera/image",
+                "/camera/camera/color/image_raw",
+            ),
+            (
+                "/camera/depth_image",
+                "/camera/camera/depth_registered/image_rect",
+            ),
+            (
+                "/camera/points",
+                "/camera/camera/depth/color/points",
+            ),
+            (
+                "/camera/camera_info",
+                "/camera/camera/color/camera_info",
+            ),
+            (
+                "/camera/camera_info",
+                "/camera/camera/depth_registered/camera_info",
+            ),
+        ],
+        output="screen",
+    )
+
+    nodes_to_start = [
+        robot_state_publisher_node,
+        rviz_node,
+        OpaqueFunction(function=launch_gz),
+        ignition_spawn_entity,
+        gazebo_bridge,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes_to_start)

package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>realsense2_gz_description</name>
+  <version>0.0.1</version>
+  <description>URDF xacro(s) that enable simulating Realsense Cameras in Gazebo</description>
+  <maintainer email="marq.razz@gmail.com">Marq Rasmussen</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>robot_state_publisher</depend>
+  <depend>realsense2_description</depend>
+  <depend>ros_gz_bridge</depend>
+  <depend>ros_gz_sim</depend>
+  <depend>rviz2</depend>
+  <depend>sensor_msgs</depend>
+  <depend>xacro</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>