point-cloud from camera/camera_info/depth topics

asked 2023-03-12 12:09:50 -0500

bhomaidan gravatar image

updated 2023-03-12 12:29:56 -0500

I do like to use the image_proc to rectify a color image from a topic before using rectified color image to detect edges and then I want to use the (edges + the rectified depth image) to create a point-cloud of edges using depth_image_proc/point_cloud_xyzrgb.

The topics that I have are:

  • /camera/color/camera_info
  • /camera/color/image_raw
  • /camera/depth/camera_info
  • /camera/depth/image_rect_raw

In addition to a topic for the edges image /camera/edges

My Launch File looks like this:

    <launch>
      <!--
      To distinguish between the cases where the rgb image is
      1280x1024 versus 640x480.  This affects the pipeline.
      -->
      <arg name="high_res_rgb" default="true"/>

      <arg name="cloud_input_ns" value="/camera/edges"/>

      <!-- Edge Detection Selected Lang -->
      <arg name="py_nodes" value="true"/>
      <arg name="in_topic_name" value="/camera/color/color_downsampled" if="$(arg high_res_rgb)"/>
      <arg name="in_topic_name" value="/camera/color/image_rect_color" unless="$(arg high_res_rgb)"/>
      <arg name="out_topic_name" value="/camera/edges"/>

      <!-- Nodelet manager for this pipeline -->
      <node pkg="nodelet" type="nodelet" args="manager"
            name="record_player_manager" output="screen"/>

      <!-- Debayer and undistort the rgb image
          TODO: this should be split into nodelets -->
      <node 
        pkg="image_proc" 
        type="image_proc" 
        name="ip_node1" 
        ns="/camera/color"
      />

      <!-- Edge Detection -->
      <include file="$(find edge_detection)/launch/edge_detection.launch">
        <arg name="py_nodes" value="$(arg py_nodes)"/>
        <arg name="in_topic_name" value="$(arg in_topic_name)"/>
        <arg name="out_topic_name" value="$(arg out_topic_name)"/>
      </include>

      <!-- The depth image is already rectified and registered
          to the camera optical frame, but stored in mm; convert it
          to meters -->
      <node pkg="nodelet" type="nodelet" name="metric_rect" 
            args="load depth_image_proc/convert_metric record_player_manager --no-bond">
        <remap from="image_raw" to="/camera/depth/image_rect_raw"/>
        <!-- <remap from="image" to="camera/depth/image"/> -->
      </node>

      <!-- Downsample and crop rgb image before converting to cloud, if it's high res -->
      <node pkg="nodelet" type="nodelet" name="downsample_rgb"
            args="load image_proc/crop_decimate record_player_manager --no-bond"
            if="$(arg high_res_rgb)">
        <param name="decimation_x" value="2"/>
        <param name="decimation_y" value="2"/>
        <param name="width" value="1280"/>
        <param name="height" value="960"/>
        <remap from="camera" to="camera/rgb"/>
        <remap from="camera_out" to="$(arg cloud_input_ns)"/>
        <remap from="camera/color/image_raw" to="camera/color/image_rect_color"/>
        <remap from="$(arg cloud_input_ns)/image_raw" to="$(arg cloud_input_ns)/image_rect_color"/>
      </node>

      <!-- Convert it into a point cloud -->
      <node pkg="nodelet" type="nodelet" name="cloudify"
            args="load depth_image_proc/point_cloud_xyzrgb record_player_manager --no-bond">
        <remap from="/camera/depth/image_rect_raw" to="/camera/depth/image"/>
        <remap from="/depth_registered/points" to="camera/depth_registered/points"/>
        <remap from="/camera/edges" to="$(arg cloud_input_ns)/image_rect_color"/>
        <remap from="/camera/color/camera_info" to="$(arg cloud_input_ns)/camera_info"/>
      </node>
    </launch>

But I don't see any point cloud! can you please tell me how can I modify my launch file to show a point cloud of my edges? thanks in advance.

edit retag flag offensive close merge delete