Added param for verbosity on the image proc (warping) and added the respective arguments to the ccny_openni_launchers

Author: Carlos Jaramillo

.gitignore

@@ -33,3 +33,4 @@ lib
 msg_gen
 srv_gen
 src/*/cfg
+warp.yml

ccny_openni_launch/launch/include/device.launch

@@ -35,3 +35,4 @@
   </node>
 
 </launch>
+

ccny_openni_launch/launch/include/play.launch

@@ -2,8 +2,10 @@
 <launch>
 
   <arg name="bag_name" />
+  <arg name="bag_rate"  default="1.0" />
+  <arg name="bag_start" default="0.0" />
 
   <node pkg="rosbag" type="play" name="play" output="screen"
-    args="$(arg bag_name) --clock --start=0.0 --queue=1000 --rate=1.0"/>
+    args="$(arg bag_name) --clock --start=$(arg bag_start) --queue=1000 --rate=$(arg bag_rate)"/>
 
-</launch>
+</launch>

ccny_openni_launch/launch/include/proc.launch

@@ -3,7 +3,10 @@
 
   <arg name="manager_name" />
   <arg name="calib_path"   /> 
-  <arg name="publish_cloud" default="false"/>
+  <arg name="publish_cloud" />
+  <arg name="scale" />
+  <arg name="unwarp" />
+  <arg name="verbose" /> 
 
   <!-- Debayered images -->
   <node pkg="nodelet" type="nodelet" name="debayer"
@@ -14,13 +17,14 @@
 
   <node pkg="nodelet" type="nodelet" name="rgbd_image_proc"
         args="load ccny_rgbd/RGBDImageProcNodelet $(arg manager_name)"
-        output="screen">
+        output="log">
 
     <!-- Resample by a factor of 2-->
-    <param name="scale" value="1.0"/>
-    <param name="unwarp" value="true"/>
+    <param name="scale" value="$(arg scale)"/>
+    <param name="unwarp" value="$(arg unwarp)"/>
     <param name="publish_cloud" value="$(arg publish_cloud)"/>
     <param name="calib_path" value="$(arg calib_path)"/>
+    <param name="verbose" value="$(arg verbose)"/>
 
   </node> 
 

ccny_openni_launch/launch/openni.launch

@@ -2,9 +2,12 @@
 <launch>
 
   <!-- Parameters -->
-  <arg name="manager_name" value="rgbd_manager"/>
+  <arg name="manager_name" default="rgbd_manager"/>
   <arg name="calib_path" default="$(find ccny_rgbd)/data/calibration_openni_default"/> 
-  <arg name="publish_cloud" default="false"/>
+  <arg name="unwarp" default="false"/> 
+  <arg name="scale" default="1.0"/> 
+  <arg name="publish_cloud" default="true"/>
+  <arg name="verbose" default="false"/>  # to display rgbd_image_proc messages
   <arg name="image_mode" default="2" />
   <arg name="depth_mode" default="2"/>
   # modes: 
@@ -28,7 +31,10 @@
   <include file="$(find ccny_openni_launch)/launch/include/proc.launch">       
      <arg name="manager_name"  value="$(arg manager_name)" />
      <arg name="calib_path"    value="$(arg calib_path)" />
-     <arg name="publish_cloud" value="$(arg publish_cloud)" />
+     <arg name="unwarp"        value="$(arg unwarp)" />
+     <arg name="scale"        value="$(arg scale)" />
+     <arg name="publish_cloud" value="$(arg publish_cloud)" />  
+     <arg name="verbose"       value="$(arg verbose)"/>  
   </include>
 
-</launch>
+</launch>

ccny_openni_launch/launch/openni_play.launch

@@ -5,9 +5,15 @@
 
   <!-- Parameters -->
   <arg name="bag_name" />
-  <arg name="manager_name" value="rgbd_manager"/>
-  <arg name="calib_path" default="$(find ccny_rgbd)/data/calibration_asus_xtion_pro"/> 
+  <arg name="bag_rate"  default="1.0" />
+  <arg name="bag_start" default="0.0" />
+  
+  <arg name="manager_name" default="rgbd_manager"/>
+  <arg name="calib_path" default="$(find ccny_rgbd)/data/calibration_openni_default"/> 
+  <arg name="unwarp" default="false"/> 
+  <arg name="scale" default="1.0"/>
   <arg name="publish_cloud" default="false"/>
+  <arg name="verbose" default="false"/>  # for rgbd_image_proc messages
 
   <!-- Nodelet manager -->
   <node pkg="nodelet" type="nodelet" name="$(arg manager_name)" args="manager"
@@ -16,13 +22,18 @@
   <!-- RGBD playback -->
   <include file="$(find ccny_openni_launch)/launch/include/play.launch">       
      <arg name="bag_name" value="$(arg bag_name)" />
+     <arg name="bag_rate" value="$(arg bag_rate)" />
+     <arg name="bag_start" value="$(arg bag_start)" />
   </include>
 
   <!-- RGBD processing -->
   <include file="$(find ccny_openni_launch)/launch/include/proc.launch">       
      <arg name="manager_name"  value="$(arg manager_name)" />
      <arg name="calib_path"    value="$(arg calib_path)" />
+     <arg name="unwarp"        value="$(arg unwarp)" />
+     <arg name="scale"        value="$(arg scale)" />
      <arg name="publish_cloud" value="$(arg publish_cloud)" />
+     <arg name="verbose"       value="$(arg verbose)"/>  
   </include>
 
 </launch>

ccny_openni_launch/launch/openni_record.launch

@@ -3,8 +3,8 @@
 
   <!-- Parameters -->
   <arg name="bag_name" />
-  <arg name="manager_name" value="rgbd_manager"/>
-  <arg name="calib_path" default="$(find ccny_rgbd)/data/calibration_asus_xtion_pro"/> 
+  <arg name="manager_name" default="rgbd_manager"/>
+  <arg name="calib_path" default="$(find ccny_rgbd)/data/calibration_openni_default"/> 
 
   <arg name="image_mode" default="2" />
   <arg name="depth_mode" default="2"/>
@@ -30,4 +30,4 @@
      <arg name="bag_name" value="$(arg bag_name)" />
   </include>
 
-</launch>
+</launch>

ccny_rgbd/include/ccny_rgbd/apps/rgbd_image_proc.h

@@ -112,8 +112,9 @@ class RGBDImageProc
     int queue_size_;          ///< ROS subscriber (and publisher) queue size parameter
 
     std::string calib_path_;  ///< Path to folder where calibration files are stored
-    bool unwarp_;             ///< Whetehr to perform depth unwarping based on polynomial model
-    bool publish_cloud_;      ///< Whetehr to calculate and publish the dense PointCloud
+    bool verbose_;             ///< Whether to print the rectification and unwarping messages
+    bool unwarp_;             ///< Whether to perform depth unwarping based on polynomial model
+    bool publish_cloud_;      ///< Whether to calculate and publish the dense PointCloud
 
     /** @brief Downasampling scale (0, 1]. For example, 
      * 2.0 will result in an output image half the size of the input

ccny_rgbd/src/apps/rgbd_image_proc.cpp

@@ -41,6 +41,8 @@ RGBDImageProc::RGBDImageProc(
     scale_ = 1.0;
   if (!nh_private_.getParam("unwarp", unwarp_))
     unwarp_ = true;
+  if (!nh_private_.getParam("verbose", verbose_))
+    verbose_ = false;
   if (!nh_private_.getParam("publish_cloud", publish_cloud_))
     publish_cloud_ = true;
   if (!nh_private_.getParam("calib_path", calib_path_))
@@ -318,11 +320,12 @@ void RGBDImageProc::RGBDCallback(
   // **** print diagnostics
 
   double dur_total = dur_rectify + dur_reproject + dur_unwarp + dur_cloud + dur_allocate;
-  
-  ROS_INFO("Rect %.1f Reproj %.1f Unwarp %.1f Cloud %.1f Alloc %.1f Total %.1f ms", 
-    dur_rectify, dur_reproject,  dur_unwarp, dur_cloud, dur_allocate,
-    dur_total);
-
+  if(verbose_)
+  {
+    ROS_INFO("Rect %.1f Reproj %.1f Unwarp %.1f Cloud %.1f Alloc %.1f Total %.1f ms",
+             dur_rectify, dur_reproject,  dur_unwarp, dur_cloud, dur_allocate,
+             dur_total);
+  }
   // **** publish
   rgb_publisher_.publish(rgb_out_msg);
   depth_publisher_.publish(depth_out_msg);