Merge pull request #94 from clearpathrobotics/lcamero/oakd_pointcloud

OAKD Pointcloud

clearpath_generator_robot/clearpath_generator_robot/param/sensors.py

@@ -92,6 +92,11 @@ def __init__(
 
         self.param_file.update(self.sensor.get_ros_parameters())
 
+        if 'luxonis_oakd' in self.param_file.parameters:
+            luxonis_params = self.param_file.parameters['luxonis_oakd']
+            self.param_file.parameters[self.sensor.name] = luxonis_params
+            self.param_file.parameters.pop('luxonis_oakd')
+
     def generate_config(self):
         sensor_writer = ParamWriter(self.param_file)
         sensor_writer.write_file()

clearpath_sensors/config/luxonis_oakd.yaml

@@ -1,25 +1,22 @@
-oakd:
+luxonis_oakd:
   ros__parameters:
     camera:
       i_enable_imu: false
       i_enable_ir: false
       i_floodlight_brightness: 0
       i_laser_dot_brightness: 100
       i_nn_type: none
-      i_pipeline_type: RGB
+      i_pipeline_type: RGBD
       i_usb_speed: SUPER_PLUS
       i_usb_port_id: ''
     rgb:
       i_board_socket_id: 0
       i_fps: 30.0
       i_height: 720
-      i_interleaved: false
+      i_width: 1280
       i_max_q_size: 10
-      i_preview_size: 250
-      i_enable_preview: true
-      i_low_bandwidth: true
-      i_keep_preview_aspect_ratio: true
-      i_publish_topic: false
-      i_resolution: '1080P'
+    stereo:
+      i_fps: 30.0
+      i_height: 720
       i_width: 1280
     use_sim_time: false

clearpath_sensors/launch/luxonis_oakd.launch.py

@@ -25,34 +25,26 @@
 # 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.
+import os
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 from launch_ros.substitutions import FindPackageShare
 
 
-def generate_launch_description():
+def launch_setup(context):
     parameters = LaunchConfiguration('parameters')
     namespace = LaunchConfiguration('namespace')
 
-    arg_parameters = DeclareLaunchArgument(
-        'parameters',
-        default_value=PathJoinSubstitution([
-          FindPackageShare('clearpath_sensors'),
-          'config',
-          'intel_realsense.yaml'
-        ]))
-
-    arg_namespace = DeclareLaunchArgument(
-        'namespace',
-        default_value='')
+    name = os.path.basename(namespace.perform(context))
 
     depthai_oakd_node = ComposableNode(
         package='depthai_ros_driver',
-        name='oakd',
+        name=name,
         namespace=namespace,
         plugin='depthai_ros_driver::Camera',
         parameters=[parameters],
@@ -81,19 +73,48 @@ def generate_launch_description():
         extra_arguments=[{'use_intra_process_comms': True}],
     )
 
+    depthai_pcl_node = ComposableNode(
+        package='depth_image_proc',
+        plugin='depth_image_proc::PointCloudXyzNode',
+        name='point_cloud_xyz_node',
+        namespace=namespace,
+        remappings=[
+            ('image_rect', 'stereo/image'),
+            ('camera_info', 'stereo/camera_info'),
+        ],
+    )
+
     image_processing_container = ComposableNodeContainer(
         name='image_processing_container',
         namespace=namespace,
         package='rclcpp_components',
         executable='component_container',
         composable_node_descriptions=[
-          depthai_oakd_node
+          depthai_oakd_node,
+          depthai_pcl_node,
         ],
         output='screen'
     )
 
+    return [image_processing_container]
+
+
+def generate_launch_description():
+    # Launch configurations
+    arg_parameters = DeclareLaunchArgument(
+        'parameters',
+        default_value=PathJoinSubstitution([
+          FindPackageShare('clearpath_sensors'),
+          'config',
+          'intel_realsense.yaml'
+        ]))
+
+    arg_namespace = DeclareLaunchArgument(
+        'namespace',
+        default_value='')
+
     ld = LaunchDescription()
     ld.add_action(arg_parameters)
     ld.add_action(arg_namespace)
-    ld.add_action(image_processing_container)
+    ld.add_action(OpaqueFunction(function=launch_setup))
     return ld