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

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

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

Log Message:
-----------
fixed openrave bug in displaying CollisionMap message

Modified Paths:
--------------
    pkg/trunk/3rdparty/openrave/Makefile
    pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
    pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
    pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp

Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile	2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/3rdparty/openrave/Makefile	2009-01-11 12:40:07 UTC (rev 9206)
@@ -2,7 +2,7 @@
 
 SVN_DIR = openrave_svn
 # Should really specify a revision
-SVN_REVISION = -r 603
+SVN_REVISION = -r 604
 SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
 include $(shell rospack find mk)/svn_checkout.mk
 

Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h	2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h	2009-01-11 12:40:07 UTC (rev 9206)
@@ -54,6 +54,7 @@
     void newdatacb()
     {
         // create the new kinbody
+        GetEnv()->LockPhysics(true);
         KinBody* pbody = GetEnv()->CreateKinBody();
 
         _vaabbs.resize(_topicmsg.boxes.size());
@@ -61,6 +62,7 @@
         FOREACH(itmsgab, _topicmsg.boxes) {
             itab->pos = Vector(itmsgab->center.x, itmsgab->center.y, itmsgab->center.z);
             itab->extents = Vector(itmsgab->extents.x, itmsgab->extents.y, itmsgab->extents.z);
+            //RAVELOG_VERBOSEA("pos%d: %f %f %f\n", (int)(itmsgab-_topicmsg.boxes.begin()), itab->pos.x, itab->pos.y, itab->pos.z);
             ++itab;
         }
 
@@ -84,22 +86,27 @@
 
         {
             boost::mutex::scoped_lock lock(_mutex);
-            GetEnv()->LockPhysics(true);
 
             // remove all unlocked bodies
             TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
             while(itbody != _mapbodies.end()) {
                 if( !itbody->second->IsLocked() ) {
+                    BODY* b = itbody->second.get();
+                    KinBody::Link* plink = itbody->second->GetOffsetLink();
+                    assert( plink != NULL );
+                    GetEnv()->RemoveKinBody(plink->GetParent());
                     _mapbodies.erase(itbody++);
                 }
                 else
                     ++itbody;
             }
-
-            GetEnv()->LockPhysics(false);
         }
         
-        BODY* b = AddKinBody(pbody, NULL);
+        GetEnv()->LockPhysics(false);
+
+        MocapData* pdata = new MocapData();
+        pdata->strOffsetLink = pbody->GetLinks().front()->GetName();
+        BODY* b = AddKinBody(pbody, pdata);
         if( b == NULL ) {
             RAVELOG_ERRORA("removing/destroying kinbody\n");
             GetEnv()->RemoveKinBody(pbody, true);

Modified: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h	2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h	2009-01-11 12:40:07 UTC (rev 9206)
@@ -397,7 +397,7 @@
                     KinBody::Link* plink = itbody->second->GetOffsetLink();
                     if( plink != NULL ) {                        
                         RAVELOG_DEBUGA("object %S expired %fs\n", plink->GetParent()->GetName(), (float)(curtime-itbody->second->lastupdated).toSec());
-                        GetEnv()->RemoveKinBody(plink->GetParent());
+                        GetEnv()->RemoveKinBody(plink->GetParent(), true);
                     }
 
                     GetEnv()->LockPhysics(false);

Modified: pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp	2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp	2009-01-11 12:40:07 UTC (rev 9206)
@@ -223,7 +223,7 @@
             ++newindex;
         }
 
-        ROS_INFO("published %d points, processing time=%fs", totalpoints, (ros::Time::now()-stampprocess).toSec());
+        ROS_DEBUG("robotlinks_filter_node published %d points, processing time=%fs", totalpoints, (ros::Time::now()-stampprocess).toSec());
         s_pmasternode->publish("robotlinks_cloud_filtered",_pointcloudout);
     }
 


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