Skip to content

Commit

Permalink
Depth-only point cloud launch file for Non-RGB PrimeSense device. (#132)
Browse files Browse the repository at this point in the history
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 <[email protected]>
  • Loading branch information
TiNredmc authored Jul 27, 2023
1 parent c990db1 commit e07df4e
Showing 1 changed file with 75 additions and 0 deletions.
75 changes: 75 additions & 0 deletions openni2_camera/launch/camera_with_cloud_norgb.launch.py
Original file line number Diff line number Diff line change
@@ -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])

0 comments on commit e07df4e

Please sign in to comment.