Added pointcloud support to OakD

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,49 @@ def generate_launch_description():
  extra_arguments=[{'use_intra_process_comms': True}],
  )
 
+ depthai_pcl_node = ComposableNode(
+ package='depth_image_proc',
+ plugin='depth_image_proc::PointCloudXyzrgbNode',
+ name='point_cloud_xyzrgb_node',
+ namespace=namespace,
+ remappings=[
+ ('depth_registered/image_rect', 'stereo/image'),
+ ('rgb/image_rect_color', 'color/image'),
+ ('rgb/camera_info', 'color/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