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

[<rdi...@us...>]

Revision: 9184
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9184&view=rev
Author:   rdiankov
Date:     2009-01-11 01:50:44 +0000 (Sun, 11 Jan 2009)

Log Message:
-----------
fixed several bugs with the robot links pruning laser_can util, should be working perfect now

Modified Paths:
--------------
    pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
    pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
    pkg/trunk/util/laser_scan/mainpage.dox
    pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp

Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml	2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml	2009-01-11 01:50:44 UTC (rev 9184)
@@ -8,5 +8,5 @@
 
   <node pkg="phase_space" type="phase_space_node"/>
   <!-- launch octave scripts to start the openrave client -->
-  <node pkg="ormanipulation" type="runperception.m" output="screen"/>
+<!--   <node pkg="ormanipulation" type="runperception.m" output="screen"/> -->
 </launch>

Modified: pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml	2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml	2009-01-11 01:50:44 UTC (rev 9184)
@@ -2,7 +2,7 @@
 <launch>
   <machine name="localhost-openrave" address="localhost"/>
   <node machine="localhost-openrave" pkg="openraveros" type="openraveros" output="screen">
-    <env name="OPENRAVE_DATA" value="$(optenv OPENRAVE_DATA):$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
-    <env name="OPENRAVE_PLUGINS" value="$(optenv OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
+    <env name="OPENRAVE_DATA" value="$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
+    <env name="OPENRAVE_PLUGINS" value="$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
   </node>
 </launch>

Modified: pkg/trunk/util/laser_scan/mainpage.dox
===================================================================
--- pkg/trunk/util/laser_scan/mainpage.dox	2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/util/laser_scan/mainpage.dox	2009-01-11 01:50:44 UTC (rev 9184)
@@ -21,9 +21,10 @@
 \subsection mean Mean Filter
 \todo Document
 
-\subsection self_collision Self Collision Filter
-\todo document
+\subsection robotlinks_filter Robot Links Pruning Filter
 
+RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull that is dynamically generated based on the mesh of the robot.
+
 \subsection shadows Scan Shadows Filter
 \todo document
 

Modified: pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp	2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp	2009-01-11 01:50:44 UTC (rev 9184)
@@ -22,19 +22,11 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
+/// \author Rosen Diankov (rdi...@cs...)
 
-/**
-@mainpage
-
-@htmlinclude manifest.html
-
-\author Rosen Diankov (rdi...@cs...)
-
-@b RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull.
-
- **/
 #include <vector>
 #include <sstream>
+#include <cstdio>
 
 #include <ros/node.h>
 #include <std_msgs/PointCloud.h>
@@ -121,13 +113,13 @@
         else
             ROS_ERROR("failed to init robot %s", robotname.c_str());
 
-        s_pmasternode->subscribe("tilt_laser_cloud_filtered", _pointcloudin,  &RobotLinksFilter::PointCloudCallback, this, 10);
+        s_pmasternode->subscribe("tilt_laser_cloud_filtered", _pointcloudin,  &RobotLinksFilter::PointCloudCallback, this, 2);
         s_pmasternode->advertise<std_msgs::PointCloud> ("robotlinks_cloud_filtered", 10);
     }
     virtual ~RobotLinksFilter()
     {
         if( s_pmasternode != NULL ) {
-            s_pmasternode->unsubscribe("tilt_scan");
+            s_pmasternode->unsubscribe("tilt_laser_cloud_filtered");
             s_pmasternode->unadvertise("robotlinks_cloud_filtered");
         }
     }
@@ -218,7 +210,7 @@
         _pointcloudout.set_chan_size(_pointcloudin.chan.size());
         for(int ichan = 0; ichan < (int)_pointcloudin.chan.size(); ++ichan) {
             _pointcloudout.chan[ichan].name = _pointcloudin.chan[ichan].name;
-            _pointcloudout.chan[ichan].set_vals_size(_pointcloudin.chan[ichan].vals.size());
+            _pointcloudout.chan[ichan].set_vals_size(totalpoints);
         }
 
         for(int oldindex = 0, newindex = 0; oldindex < (int)_vlaserpoints.size(); ++oldindex) {
@@ -231,7 +223,7 @@
             ++newindex;
         }
 
-        ROS_INFO("published %d points, processing time=%fs\n", totalpoints, (ros::Time::now()-stampprocess).toSec());
+        ROS_INFO("published %d points, processing time=%fs", totalpoints, (ros::Time::now()-stampprocess).toSec());
         s_pmasternode->publish("robotlinks_cloud_filtered",_pointcloudout);
     }
 
@@ -248,13 +240,19 @@
         FOREACH(itp, pointcloudin.pts)
             vlaserpoints[index++] = LASERPOINT(Vector(itp->x,itp->y,itp->z,1),0);
 
-        FOREACH(ithull, _vLinkHulls) {
-            _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampstart, bttransform);
-            ithull->tstart = GetTransform(bttransform);
-            _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampend, bttransform);
-            ithull->tend = GetTransform(bttransform);
+        try {
+            FOREACH(ithull, _vLinkHulls) {
+                _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampstart, bttransform);
+                ithull->tstart = GetTransform(bttransform);
+                _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampend, bttransform);
+                ithull->tend = GetTransform(bttransform);
+            }
         }
-        
+        catch(tf::TransformException& ex) {
+            ROS_WARN("failed to get tf frame");
+            return;
+        }
+
         // points are independent from each and loop can be parallelized
         #pragma omp parallel for schedule(dynamic,128)
         for(int i = 0; i < (int)vlaserpoints.size(); ++i) {
@@ -266,7 +264,8 @@
 
                 bool bInside = true;
                 FOREACH(itplane, ithull->vconvexhull) {
-                    if( dot4(*itplane,tinv * laserpoint.pt) > 0 ) {
+                    Vector v = tinv * laserpoint.pt; v.w = 1;
+                    if( dot4(*itplane,v) > 0 ) {
                         bInside = false;
                         break;
                     }
@@ -285,17 +284,28 @@
     void PruneWithSimpleTiming(const std_msgs::PointCloud& pointcloudin, vector<LASERPOINT>& vlaserpoints)
     {
         tf::Stamped<btTransform> bttransform;
+        vlaserpoints.resize(0);
+        
+        // compute new hulls
+        try {
+            FOREACH(ithull, _vLinkHulls) {
+                _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, pointcloudin.header.stamp, bttransform);
+                ithull->tstart = GetTransform(bttransform);
+            }
+        }
+        catch(tf::TransformException& ex) {
+            ROS_WARN("failed to get tf frame");
+            return;
+        }
+        
         vlaserpoints.resize(pointcloudin.pts.size());
 
         int index = 0;
         FOREACH(itp, pointcloudin.pts)
             vlaserpoints[index++] = LASERPOINT(Vector(itp->x,itp->y,itp->z,1),0);
 
-        // compute new hulls
         FOREACH(ithull, _vLinkHulls) {
-            _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, pointcloudin.header.stamp, bttransform);
-            TransformMatrix tinvstart = GetTransform(bttransform).inverse();
-
+            TransformMatrix tinvstart = ithull->tstart.inverse();
             ithull->vnewconvexhull.resize(ithull->vconvexhull.size());
             vector<Vector>::iterator itnewplane = ithull->vnewconvexhull.begin();
             FOREACH(itplane, ithull->vconvexhull) {
@@ -314,7 +324,7 @@
             FOREACH(ithull, _vLinkHulls) {
 
                 bool bInside = true;
-                FOREACH(itplane, ithull->vconvexhull) {
+                FOREACH(itplane, ithull->vnewconvexhull) {
                     if( dot4(*itplane,laserpoint.pt) > 0 ) {
                         bInside = false;
                         break;
@@ -347,8 +357,8 @@
         bool bSuccess = false;
         boolT ismalloc = 0;           // True if qhull should free points in qh_freeqhull() or reallocation  
         char flags[]= "qhull Tv"; // option flags for qhull, see qh_opt.htm 
-        FILE *outfile = NULL;    // output from qh_produce_output(), use NULL to skip qh_produce_output()  
-        FILE *errfile = stderr;    // error messages from qhull code  
+        FILE *outfile = NULL;    // stdout, output from qh_produce_output(), use NULL to skip qh_produce_output()  
+        FILE *errfile = tmpfile();    // stderr, error messages from qhull code  
         
         int exitcode= qh_new_qhull (dim, qpoints.size()/3, &qpoints[0], ismalloc, flags, outfile, errfile);
         if (!exitcode) { // no error
@@ -421,7 +431,7 @@
     }
 
     ros::init(argc,argv);
-    s_pmasternode.reset(new ros::node("roobtlinks_filter"));
+    s_pmasternode.reset(new ros::node("robobtlinks_filter"));
 
     if( !s_pmasternode->checkMaster() )
         return -1;


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