Skip to content

Instantly share code, notes, and snippets.

@Maik93
Last active November 29, 2018 15:43
Show Gist options
  • Save Maik93/44782fc6ea7d0e9721e40f8e38b5ae0e to your computer and use it in GitHub Desktop.
Save Maik93/44782fc6ea7d0e9721e40f8e38b5ae0e to your computer and use it in GitHub Desktop.
ROS launcher for Intel Relalsense R200 that uses depth_image_proc for the RGB point cloud composition.
<!-- Sample launch file for using RealSense R200 camera with default configurations -->
<launch>
<arg name="camera" default="camera" />
<arg name="camera_type" default="R200" /> <!-- Type of camera -->
<arg name="serial_no" default="" />
<arg name="usb_port_id" default="" /> <!-- USB "Bus#-Port#" -->
<arg name="manager" default="nodelet_manager" />
<!-- These 'arg' tags are just place-holders for passing values from test files.
The recommended way is to pass the values directly into the 'param' tags. -->
<arg name="mode" default="manual" />
<arg name="color_fps" default="30" />
<param name="$(arg camera)/driver/mode" type="str" value="$(arg mode)" />
<param name="$(arg camera)/driver/color_fps" type="int" value="$(arg color_fps)" />
<!-- Refer to the Wiki http://wiki.ros.org/realsense_camera for list of supported parameters -->
<arg name="rgb_camera_info" value="color/camera_info"/>
<arg name="rgb_img_rect" value="color/image_raw"/> <!--Raw color image-->
<arg name="depth_camera_info" value="depth/camera_info"/>
<arg name="depth_imgraw" value="depth/image_raw"/> <!--Raw depth image-->
<arg name="depth_imgrect" value="depth/image_rect"/> <!--Rectified depth image-->
<arg name="depth_reg_camera_info" value="depth_registered/camera_info"/>
<arg name="depth_reg_imgrect" value="depth_registered/image_rect"/> <!--Transported to rgb_camera frame-->
<arg name="out_cloud" value="depth_registered/points"/>
<group ns="$(arg camera)">
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
<include file="$(find realsense_camera)/launch/includes/nodelet.launch.xml">
<arg name="manager" value="$(arg manager)" />
<arg name="camera" value="$(arg camera)" />
<arg name="camera_type" value="$(arg camera_type)" />
<arg name="serial_no" value="$(arg serial_no)" />
<arg name="usb_port_id" value="$(arg usb_port_id)" />
</include>
<!-- Convert depth from mm (in uint16) to meters -->
<node pkg="nodelet" type="nodelet" name="convert_metric" args="load depth_image_proc/convert_metric $(arg manager)">
<remap from="image_raw" to="$(arg depth_imgraw)"/>
<remap from="image" to="$(arg depth_imgrect)"/>
</node>
<!-- Transport depth image in rgb_camera frame -->
<node pkg="nodelet" type="nodelet" name="register" args="load depth_image_proc/register $(arg manager)">
<remap from="rgb/camera_info" to="$(arg rgb_camera_info)"/>
<remap from="depth/camera_info" to="$(arg depth_camera_info)"/>
<remap from="depth/image_rect" to="$(arg depth_imgrect)"/>
<remap from="depth_registered/camera_info" to="$(arg depth_reg_camera_info)"/>
<remap from="depth_registered/image_rect" to="$(arg depth_reg_imgrect)"/>
</node>
<!-- Construct point cloud of the rgb and depth topics -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) --no-bond">
<remap from="rgb/camera_info" to="$(arg rgb_camera_info)" />
<remap from="rgb/image_rect_color" to="$(arg rgb_img_rect)"/>
<remap from="depth_registered/image_rect" to="$(arg depth_reg_imgrect)"/>
<remap from="depth_registered/points" to="$(arg out_cloud)"/>
</node>
</group>
</launch>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment