Merge pull request #6 from osrf/feature/composition-launch

add launch composition example for depthai_ros_driver

README.md

@@ -71,6 +71,7 @@ Sample models are available [here](https://github.com/luxonis/depthai/tree/main/
     ros2 component load /ComponentManager depthai_ros_driver rr::DepthAIBaseRos2
     ```
 3. Or, ros2 launch: `ros2 launch depthai_ros_driver depthai_node.launch.xml`
+4. Or, ros2 launch with composition: `ros2 launch depthai_ros_driver depthai_composition.py`
 
 # Trouble shooting
 If you see an error similar to this:

depthai_ros_driver/launch/depthai_composition.launch.xml

@@ -0,0 +1,93 @@
+<?xml version="1.0"?>
+<!-- ROS2 launch for depthai_ros_driver with composition -->
+<!-- Note: This is only available in ROS2 Rolling, H turtle. https://github.com/ros2/launch_ros/pull/235.
+     Disclaimer: We only provide this launch file as an reference for future use. A full test on this is required -->
+
+<launch>
+  <arg name="ns" default="depthai"/> <!-- This is not in used -->
+  <arg name="node_name" default="depthai_node"/>
+  <arg name="queue_size" default="1"/>
+  <arg name="camera_name" default="bw1097"/>
+
+  <!-- Note: These are not in used by the node in the original code, TODO: Clarify -->
+  <arg name="camera_param_uri" default="package://depthai_ros_driver/params/camera/"
+    description = "Location of parameters is {camera_param_uri}/{camera_name}/{stream}.yaml"/>
+  <arg name="cmd_file" default="$(find-pkg-share depthai_ros_driver)/resources/depthai.cmd"/>
+  <!-- end -->
+
+  <arg name="calibration_file" default="$(find-pkg-share depthai_ros_driver)/resources/calibration/$(var camera_name)/depthai.calib"/>
+  <arg name="blob_file" default="$(find-pkg-share depthai_ros_driver)/resources/mobilenet_ssd.blob"/>
+  <arg name="blob_file_config" default="$(find-pkg-share depthai_ros_driver)/resources/mobilenet-ssd.json"/>
+
+  <arg name="force_usb2" default="false"/>
+  <arg name="sync_video_meta" default="false"/>
+  <arg name="compute_bbox_depth" default="false"/>
+  <arg name="full_fov_nn" default="true"/>
+
+  <arg name="rgb_height" default="1080"/>
+  <arg name="rgb_fps" default="30"/>
+  <arg name="depth_height" default="720"/>
+  <arg name="depth_fps" default="30"/>
+  <arg name="shaves" default="14"/>
+  <arg name="cmx_slices" default="14"/>
+  <arg name="nn_engines" default="2"/>
+
+  <arg name="use_gdb" default="false"/>
+
+  <arg name="launch_prefix" default="/usr/bin/gdb -/- args" if="$(var use_gdb)"/>
+  <arg name="launch_prefix" default="" unless="$(var use_gdb)"/>
+
+  <!--
+  Possible streams:
+  * 'left' - left mono camera preview
+  * 'right' - right mono camera preview
+  * 'previewout' - preview of stream sent to the NN
+  * 'metaout' - CNN output tensors
+  * 'depth' - the raw depth map, disparity converted to real life distance
+  * 'disparity' - disparity map, the diaparity between left and right cameras, in pixels
+  * 'disparity_color' - disparity map colorized
+  * 'meta_d2h' - device metadata stream
+  * 'video' - H.264/H.265 encoded color camera frames
+  * 'jpegout' - JPEG encoded color camera frames
+  * 'object_tracker' - Object tracker results
+  -->
+
+  <arg name="stream_list" default="[left, right, metaout, previewout, disparity, disparity_color, depth, video]"/>
+
+  <group>
+    <node_container pkg="rclcpp_components" exec="component_container" name="container">
+      <composable_node pkg="depthai_ros_driver" plugin="rr::DepthAIBaseRos2" name="$(var node_name)">
+        <param name="camera_name" value="$(var camera_name)"/>
+
+        <param name="cmd_file" value="$(var cmd_file)"/>
+        <param name="calibration_file" value="$(var calibration_file)"/>
+        <param name="blob_file" value="$(var blob_file)"/>
+        <param name="blob_file_config" value="$(var blob_file_config)"/>
+        <param name="stream_list" value="$(var stream_list)"/>
+
+        <param name="sync_video_meta" value="$(var sync_video_meta)"/>
+        <param name="compute_bbox_depth" value="$(var compute_bbox_depth)"/>
+        <param name="full_fov_nn" value="$(var full_fov_nn)"/>
+        <param name="force_usb2" value="$(var force_usb2)"/>
+
+        <param name="depth_fps" value="$(var depth_fps)"/>
+        <param name="depth_height" value="$(var depth_height)"/>
+        <param name="rgb_fps" value="$(var rgb_fps)"/>
+        <param name="rgb_height" value="$(var rgb_height)"/>
+
+        <param name="shaves" value="$(var shaves)"/>
+        <param name="cmx_slices" value="$(var cmx_slices)"/>
+        <param name="nn_engines" value="$(var nn_engines)"/>
+
+        <param name="queue_size" value="$(var queue_size)"/>
+      </composable_node>
+    </node_container>     
+  </group>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
+    <param name="tf_prefix" value=""/>
+    <param name="robot_description"
+      value="$(command 'xacro 
+        $(find-pkg-share depthai_ros_driver)/urdf/$(var camera_name).urdf.xacro')"/>
+  </node>
+</launch>

depthai_ros_driver/launch/depthai_composition.py

@@ -0,0 +1,116 @@
+# Copyright 2021 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.
+
+"""Launch the depthai driver in composition mode"""
+
+from ament_index_python.packages import get_package_share_directory
+
+import os
+import launch
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.actions import Node
+from launch.substitutions import Command
+
+################################################################################
+# Parameters
+camera_name = "bw1097"
+driver_pkg_dir = get_package_share_directory('depthai_ros_driver')
+
+queue_size = 1
+calibration_file = os.path.join(
+  driver_pkg_dir, f'resources/calibration/{camera_name}/depthai.calib')
+blob_file = os.path.join(driver_pkg_dir, 'resources/mobilenet_ssd.blob')
+blob_file_config = os.path.join(driver_pkg_dir, 'resources/mobilenet-ssd.json')
+force_usb2 = False
+sync_video_meta = False
+compute_bbox_depth = False
+full_fov_nn = True
+rgb_height = 1080
+rgb_fps = 30
+depth_height = 720
+depth_fps = 30
+shaves = 14
+cmx_slices = 14
+nn_engines = 2
+use_gdb = False
+stream_list = [
+  'left', 'right', 'metaout', 'previewout', 'disparity',
+  'disparity_color', 'depth', 'video']
+
+xacro_path = os.path.join(driver_pkg_dir, f'urdf/{camera_name}.urdf.xacro')
+
+# Possible streams:
+# *'left' - left mono camera preview
+# *'right' - right mono camera preview
+# *'previewout' - preview of stream sent to the NN
+# *'metaout' - CNN output tensors
+# *'depth' - the raw depth map, disparity converted to real life distance
+# *'disparity' - disparity map, the disparity between left and right cams, in pixels
+# *'disparity_color' - disparity map colorized
+# *'meta_d2h' - device metadata stream
+# *'video' - H.264/H.265 encoded color camera frames
+# *'jpegout' - JPEG encoded color camera frames
+# *'object_tracker' - Object tracker results
+
+################################################################################
+
+def generate_launch_description():
+    """Generate launch description with multiple components."""
+
+    container = ComposableNodeContainer(
+            name='composition_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='depthai_ros_driver',
+                    plugin='rr::DepthAIBaseRos2',
+                    name='depthai_node',
+                    parameters=[{
+                        "camera_name": camera_name,
+                        "calibration_file": calibration_file,
+                        "blob_file": blob_file,
+                        "blob_file_config": blob_file_config,
+                        "stream_list": stream_list,
+                        "sync_video_meta": sync_video_meta,
+                        "compute_bbox_depth": compute_bbox_depth,
+                        "full_fov_nn": full_fov_nn,
+                        "force_usb2": force_usb2,
+                        "depth_fps": depth_fps,
+                        "depth_height": depth_height,
+                        "rgb_fps": rgb_fps,
+                        "rgb_height": rgb_height,
+                        "shaves": shaves,
+                        "cmx_slices": cmx_slices,
+                        "nn_engines": nn_engines,
+                        "queue_size": queue_size,
+                    }]
+                )
+            ],
+            output='screen',
+    )
+
+    robot_state_pub = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            "tf_prefix": "",
+            'robot_description': Command([f'xacro {xacro_path}'])
+        }]
+    )
+
+    return launch.LaunchDescription([container, robot_state_pub])