From e07df4ea928515a5bb16a3bad169e093a5bf54f0 Mon Sep 17 00:00:00 2001
From: TinLethax <thipok17@gmail.com>
Date: Thu, 27 Jul 2023 23:34:25 +0000
Subject: [PATCH] Depth-only point cloud launch file for Non-RGB PrimeSense
 device. (#132)

Some Depth camera based on PS1080 SoC such as Asus Xtion and (some)
Xtion Pro don't come with RGB camera.

The original 'camera_with_cloud' launch file requires the missing RGB
color stream, which Xtion and (some) Xtion Pro don't provide. This make
point cloud topic publish no message at all.

Solving by changing `depth_image_proc::PointCloudXyzrgbNode` to
`depth_image_proc::PointCloudXyzNode` and remove all RGB related topic
remap in launch file. Resulted in the new launch file named
'camera_with_cloud_norgb'. Modified from 'camera_with_cloud' launch
file.

I tested with these following device and environment

- Asus Xtion ([Possibly pre-production or custom made
unit](https://www.aliexpress.com/item/1005004788630548.html?))
- ROS2 Humble
- Ubuntu 22.04 on WSL2
- USBIP

Signed-off-by: Tin LetHax <thipok17@gmail.com>
---
 .../launch/camera_with_cloud_norgb.launch.py  | 75 +++++++++++++++++++
 1 file changed, 75 insertions(+)
 create mode 100644 openni2_camera/launch/camera_with_cloud_norgb.launch.py

diff --git a/openni2_camera/launch/camera_with_cloud_norgb.launch.py b/openni2_camera/launch/camera_with_cloud_norgb.launch.py
new file mode 100644
index 0000000..d06bae6
--- /dev/null
+++ b/openni2_camera/launch/camera_with_cloud_norgb.launch.py
@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2020-2023, Michael Ferguson
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# 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 OWNER 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.
+
+import launch
+import launch_ros.actions
+import launch_ros.descriptions
+
+
+def generate_launch_description():
+
+    namespace = '/camera'
+
+    container = launch_ros.actions.ComposableNodeContainer(
+            name='container',
+            namespace=namespace,
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                # Driver
+                launch_ros.descriptions.ComposableNode(
+                    package='openni2_camera',
+                    plugin='openni2_wrapper::OpenNI2Driver',
+                    name='driver',
+                    namespace=namespace,
+                    parameters=[{'depth_registration': False},
+                                {'use_device_time': True}],
+                    remappings=[('depth/image', 'depth_registered/image_raw')],
+                ),
+                # Create XYZ point cloud
+                launch_ros.descriptions.ComposableNode(
+                    package='depth_image_proc',
+                    plugin='depth_image_proc::PointCloudXyzNode',
+                    name='points_xyz',
+                    namespace=namespace,
+                    parameters=[{'queue_size': 10}],
+                    remappings=[('image_rect', 'depth/image_raw'),
+                                ('camera_info', 'depth/camera_info'),
+                                ('points', 'depth/points')],
+                ),
+            ],
+            output='screen',
+    )
+
+    return launch.LaunchDescription([container])