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

[<ge...@us...>]

Revision: 2622
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2622&view=rev
Author:   gerkey
Date:     2008-08-06 02:36:30 +0000 (Wed, 06 Aug 2008)

Log Message:
-----------
Added optional range_cutoff argument to projectLaser, and added usage of it
to wavefront_player.

Modified Paths:
--------------
    pkg/trunk/nav/wavefront_player/wavefront_player.cc
    pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
    pkg/trunk/util/laser_scan_utils/src/laser_scan.cc

Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc	2008-08-06 02:14:47 UTC (rev 2621)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc	2008-08-06 02:36:30 UTC (rev 2622)
@@ -280,9 +280,9 @@
         dist_penalty(2.0),   // overridden by param retrieval below!
         plan_halfwidth(5.0),
         dist_eps(1.0),       // overridden by param retrieval below!
-        ang_eps(DTOR(10.0)),
+        ang_eps(DTOR(4.0)),
         cycletime(0.1),
-        laser_maxrange(4.0),
+        laser_maxrange(10.0),
         laser_buffer_time(3.0),
         lookahead_maxdist(2.0),
         lookahead_distweight(5.0),
@@ -433,7 +433,7 @@
 
     // Assemble a point cloud, in the laser's frame
     std_msgs::PointCloudFloat32 local_cloud;
-    projector_.projectLaser(*it, local_cloud);
+    projector_.projectLaser(*it, local_cloud, laser_maxrange);
     
     // Convert to a point cloud in the map frame
     std_msgs::PointCloudFloat32 global_cloud;

Modified: pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
===================================================================
--- pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h	2008-08-06 02:14:47 UTC (rev 2621)
+++ pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h	2008-08-06 02:36:30 UTC (rev 2622)
@@ -62,7 +62,7 @@
       /** \brief Project Laser Scan
        * This will project a laser scan from a linear array into a 3D point cloud
        */
-      void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out);
+      void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out, double range_cutoff=-1.0);
       
       /** \brief Return the unit vectors for this configuration
        * Return the unit vectors for this configuration.

Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc	2008-08-06 02:14:47 UTC (rev 2621)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc	2008-08-06 02:36:30 UTC (rev 2622)
@@ -33,7 +33,7 @@
 namespace laser_scan{
 
   
-  void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out)
+  void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out, double range_cutoff)
   {
     NEWMAT::Matrix ranges(2, scan_in.ranges_size);
     double * matPointer = ranges.Store();
@@ -64,8 +64,9 @@
     unsigned int count = 0;
     for (unsigned int index = 0; index< scan_in.ranges_size; index++)
       {
-        if (matPointer[index] <= scan_in.range_max 
-            && matPointer[index] >= scan_in.range_min) //only valid
+        if ((matPointer[index] < scan_in.range_max) &&
+            ((range_cutoff < 0.0) || (matPointer[index] < range_cutoff)) &&
+            (matPointer[index] > scan_in.range_min)) //only valid
           {
             cloud_out.pts[count].x = outputMat[index];
             cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];


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