Feat/add realsense gazebo plugin

rcdt_utilities/CMakeLists.txt

@@ -19,7 +19,7 @@ find_package(tf2_eigen REQUIRED)
 # Install project files
 install(
   DIRECTORY
-  launch rviz
+  launch rviz urdf sdf
   DESTINATION share/${PROJECT_NAME}
 )
 

rcdt_utilities/launch/gazebo_robot.launch.py

@@ -2,36 +2,71 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-from launch.actions import IncludeLaunchDescription
-from launch import LaunchDescription
+from typing import List
+
+from launch import LaunchContext, LaunchDescription
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
 from launch_ros.actions import Node
-from rcdt_utilities.launch_utils import get_file_path
+from rcdt_utilities.launch_utils import LaunchArgument, get_file_path
+
+load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False])
+world_arg = LaunchArgument("world", "")
+use_realsense_arg = LaunchArgument("realsense", False, [True, False])
+
+
+def launch_setup(context: LaunchContext) -> List:
+    load_gazebo_ui = load_gazebo_ui_arg.value(context)
+    world = world_arg.value(context)
+    use_realsense = use_realsense_arg.value(context)
+
+    if world == "":
+        world = get_file_path("rcdt_utilities", ["sdf"], "empty.xml.sdf")
 
-gazebo = IncludeLaunchDescription(
-    get_file_path("ros_gz_sim", ["launch"], "gz_sim.launch.py"),
-    launch_arguments={"gz_args": "empty.sdf -r"}.items(),
-)
+    gz_args = f" -r {world}"
+    if not load_gazebo_ui:
+        gz_args += " -s"
+    gazebo = IncludeLaunchDescription(
+        get_file_path("ros_gz_sim", ["launch"], "gz_sim.launch.py"),
+        launch_arguments={"gz_args": gz_args}.items(),
+    )
+
+    spawn_robot = Node(
+        package="ros_gz_sim",
+        executable="create",
+        arguments=["-topic", "/robot_description"],
+        output="screen",
+    )
 
-spawn_robot = Node(
-    package="ros_gz_sim",
-    executable="create",
-    arguments=["-topic", "/robot_description"],
-    output="screen",
-)
+    bridge_topics = ["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"]
+    if use_realsense:
+        bridge_topics.extend(
+            [
+                "/camera/color/camera_info@sensor_msgs/msg/[email protected]",
+                "/camera/color/image_raw@sensor_msgs/msg/[email protected]",
+                "/camera/depth/camera_info@sensor_msgs/msg/[email protected]",
+                "/camera/depth/image_rect_raw@sensor_msgs/msg/[email protected]",
+            ]
+        )
+
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=bridge_topics,
+    )
 
-sync_clock = Node(
-    package="ros_gz_bridge",
-    executable="parameter_bridge",
-    name="sync_clock",
-    arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
-)
+    return [
+        gazebo,
+        spawn_robot,
+        bridge,
+    ]
 
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
-            gazebo,
-            spawn_robot,
-            sync_clock,
+            load_gazebo_ui_arg.declaration,
+            world_arg.declaration,
+            use_realsense_arg.declaration,
+            OpaqueFunction(function=launch_setup),
         ]
     )