[Personalrobots-commit] SF.net SVN: personalrobots:[6967] pkg/trunk

[<vij...@us...>]

Revision: 6967
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6967&view=rev
Author:   vijaypradeep
Date:     2008-11-19 03:23:28 +0000 (Wed, 19 Nov 2008)

Log Message:
-----------
Added config params to point_cloud assembler. Added launch script to start the assembler

Modified Paths:
--------------
    pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
    pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp

Added Paths:
-----------
    pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch

Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp	2008-11-19 03:22:35 UTC (rev 6966)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp	2008-11-19 03:23:28 UTC (rev 6967)
@@ -95,7 +95,8 @@
 
   LaserScan scan_;
 
-  unsigned int max_scans_;
+  unsigned int max_scans_ ;
+  bool ignore_laser_skew_ ;
 
   deque<LaserScan> scan_hist_ ;            //!< Stores history of scans. We want them in time-ordered, which is (in most situations) the same as time-of-receipt-ordered
   unsigned int total_pts_ ;                //!< Stores the total number of range points in the entire stored history of scans. Useful for estimating points/scan
@@ -109,6 +110,7 @@
     subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
 
     param("point_cloud_assembler/max_scans", max_scans_, (unsigned int) 400) ;
+    param("~ignore_laser_skew", ignore_laser_skew_, true) ;
 
     total_pts_ = 0 ;                                                                   // We're always going to start with no points in our history
   }
@@ -165,8 +167,16 @@
 
       try
       {
-        tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
-        
+        if (ignore_laser_skew_)  // Do it the fast (approximate) way
+	{
+	  projector_.projectLaser(cur_scan, projector_cloud) ;
+          tf_.transformPointCloud(req.target_frame_id, projector_cloud, target_frame_cloud) ;
+	}
+	else                     // Do it the slower (more accurate) way
+	{
+	  tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
+        }
+
         for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++)               // Populate full_cloud from the cloud
         {
           resp.cloud.pts[cloud_count].x        = target_frame_cloud.pts[j].x ;

Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp	2008-11-19 03:22:35 UTC (rev 6966)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp	2008-11-19 03:23:28 UTC (rev 6967)
@@ -60,6 +60,7 @@
     
   bool first_time_ ;
   
+  std::string fixed_frame_ ;
   
   PointCloudSnapshotter() : ros::node("point_cloud_snapshotter")
   {
@@ -67,6 +68,10 @@
     
     advertise<PointCloud> ("full_cloud", 1) ;
     subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
+
+    std::string default_fixed_frame = "torso" ;
+
+    param("~fixed_frame", fixed_frame_, std::string("torso")) ;
     
     first_time_ = true ;
   }
@@ -90,7 +95,7 @@
       
       req.begin = prev_signal_.header.stamp ;
       req.end   = cur_signal_.header.stamp ;
-      req.target_frame_id = "torso" ;
+      req.target_frame_id = fixed_frame_ ;
       
       printf("PointCloudSnapshotter::Making Service Call...\n") ;
       ros::service::call("build_cloud", req, resp) ;

Added: pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch	                        (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch	2008-11-19 03:23:28 UTC (rev 6967)
@@ -0,0 +1,12 @@
+<launch>
+  <node pkg="point_cloud_assembler" type="point_cloud_assembler" output="screen"  name="assembler">
+    <remap from="scan" to="tilt_scan"/>
+    <param name="ignore_laser_skew" type="bool" value="true" />
+  </node>
+
+  <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+    <remap from="laser_scanner_signal" to="laser_controller/laser_scanner_signal"/>
+    <param name="fixed_frame" type="string" value="torso" />
+  </node>
+
+</launch>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.