Merge pull request #4 from piyushk/fix_device_registered_cloud

Fix device registered point cloud generation
ros-drivers · Aug 1, 2013 · 4118bca · 4118bca
2 parents b40e5a4 + cf357ef
commit 4118bca
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 19 deletions.
diff --git a/launch/includes/depth_registered.launch b/launch/includes/depth_registered.launch
@@ -13,12 +13,14 @@
   <arg name="rgb"              default="rgb" />
   <arg name="depth"            default="depth" />
   <arg name="depth_registered" default="depth_registered" />
+  <arg name="depth_registration" default="false" />
   <!-- For distinguishing multiple register/XYZRGB nodelets. Default fails if rgb
        or depth contains a namespace. -->
   <arg name="suffix" default="$(arg depth)_$(arg rgb)" />
 
   <!-- Registration nodelet, projecting depth to RGB camera -->
-  <node pkg="nodelet" type="nodelet" name="register_$(arg suffix)"
+  <node unless="$(arg depth_registration)" 
+        pkg="nodelet" type="nodelet" name="register_$(arg suffix)"
         args="load depth_image_proc/register $(arg manager) $(arg bond)"
         respawn="$(arg respawn)">
     <!-- Explicit topic remappings, shouldn't need all of these -->
@@ -28,6 +30,18 @@
     <remap from="depth_registered/image_rect" to="$(arg depth_registered)/image_rect_raw" />
   </node>
 
+  <!-- for device enabled registration, simply rectify the raw image -->
+  <node if="$(arg depth_registration)" 
+        pkg="nodelet" type="nodelet" name="$(arg depth_registered)_rectify_depth"
+        args="load image_proc/rectify $(arg manager) $(arg bond)"
+        respawn="$(arg respawn)">
+    <remap from="image_mono"  to="$(arg depth_registered)/image_raw" />
+    <remap from="image_rect"  to="$(arg depth_registered)/image_rect_raw" />
+    <!-- Use nearest neighbor (0) interpolation so we don't streak across
+         depth boundaries -->
+    <param name="interpolation" value="0" />
+  </node>
+
   <!-- Instead publish registered XYZRGB point cloud -->
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb_$(arg suffix)"
         args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) $(arg bond)"

diff --git a/launch/includes/processing.launch b/launch/includes/processing.launch
@@ -16,6 +16,9 @@
   <arg name="depth_registered_processing" />
   <arg name="disparity_processing" />
   <arg name="disparity_registered_processing" />
+
+  <!-- Is device enabled registration enabled? -->
+  <arg name="depth_registration" default="false" />
 
   <!-- Remapping arguments -->
   <arg name="rgb"              default="rgb" />
@@ -41,31 +44,36 @@
   </include>
 
   <!-- Unregistered depth processing -->
-  <include if="$(arg depth_processing)"
-           file="$(find openni_launch)/launch/includes/depth.launch">
-    <arg name="manager" value="$(arg manager)" />
-    <arg name="respawn" value="$(arg respawn)" />
-    <arg name="depth"   value="$(arg depth)" />
-  </include>
+  <group unless="$(arg depth_registration)">
+    <include if="$(arg depth_processing)"
+      file="$(find openni_launch)/launch/includes/depth.launch">
+      <arg name="manager" value="$(arg manager)" />
+      <arg name="respawn" value="$(arg respawn)" />
+      <arg name="depth"   value="$(arg depth)" />
+    </include>
+  </group>
 
   <!-- Depth-to-RGB registration and processing -->
   <include if="$(arg depth_registered_processing)"
            file="$(find openni_launch)/launch/includes/depth_registered.launch">
-    <arg name="manager"           value="$(arg manager)" />
-    <arg name="rgb"               value="$(arg rgb)" />
-    <arg name="depth"             value="$(arg depth)" />
-    <arg name="depth_registered"  value="$(arg depth_registered)" />
-    <arg name="respawn"           value="$(arg respawn)" />
+    <arg name="manager"            value="$(arg manager)" />
+    <arg name="depth_registration" value="$(arg depth_registration)" />
+    <arg name="rgb"                value="$(arg rgb)" />
+    <arg name="depth"              value="$(arg depth)" />
+    <arg name="depth_registered"   value="$(arg depth_registered)" />
+    <arg name="respawn"            value="$(arg respawn)" />
   </include>
 
   <!-- Unregistered disparity image -->
-  <include if="$(arg disparity_processing)"
-           file="$(find openni_launch)/launch/includes/disparity.launch">
-    <arg name="manager"   value="$(arg manager)" />
-    <arg name="depth"     value="$(arg depth)" />
-    <arg name="projector" value="$(arg projector)" />
-    <arg name="respawn"   value="$(arg respawn)" />
-  </include>
+  <group unless="$(arg depth_registration)">
+    <include if="$(arg disparity_processing)"
+      file="$(find openni_launch)/launch/includes/disparity.launch">
+      <arg name="manager"   value="$(arg manager)" />
+      <arg name="depth"     value="$(arg depth)" />
+      <arg name="projector" value="$(arg projector)" />
+      <arg name="respawn"   value="$(arg respawn)" />
+    </include>
+  </group>
 
   <!-- Registered disparity image -->
   <include if="$(arg disparity_registered_processing)"

diff --git a/launch/openni.launch b/launch/openni.launch
@@ -85,6 +85,7 @@
     <!-- Load standard constellation of processing nodelets -->
     <include file="$(find openni_launch)/launch/includes/processing.launch">
       <arg name="manager"                         value="$(arg manager)" />
+      <arg name="depth_registration"              value="$(arg depth_registration)" />
       <arg name="rgb"                             value="$(arg rgb)" />
       <arg name="ir"                              value="$(arg ir)" />
       <arg name="depth"                           value="$(arg depth)" />