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

[<is...@us...>]

Revision: 17852
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17852&view=rev
Author:   isucan
Date:     2009-06-29 02:30:18 +0000 (Mon, 29 Jun 2009)

Log Message:
-----------
small fix for self watch

Modified Paths:
--------------
    pkg/trunk/util/self_watch/self_watch.cpp
    pkg/trunk/world_models/collision_space/src/environmentODE.cpp

Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp	2009-06-29 02:07:12 UTC (rev 17851)
+++ pkg/trunk/util/self_watch/self_watch.cpp	2009-06-29 02:30:18 UTC (rev 17852)
@@ -68,6 +68,8 @@
 	    m_envModels = boost::shared_ptr<planning_environment::CollisionModels>(new planning_environment::CollisionModels("robot_description", m_scaling, m_padding));
 	    m_kmodel = m_envModels->getKinematicModel();
 	    m_collisionSpace = m_envModels->getODECollisionModel();
+	    m_collisionSpace->setSelfCollision(true);
+	    m_collisionSpace->setVerbose(true);
 
 	    // create a state that can be used to monitor the
 	    // changes in the joints of the kinematic model
@@ -129,11 +131,11 @@
 
 	if (contacts.size() > 0)
 	{
-	    ROS_WARN("Collision found in %g seconds", (ros::WallTime::now() - start_time).toSec());
+	    ROS_WARN("Collision found in %g seconds", (ros::WallTime::now() - start_time).toSec());	    
 	    for (unsigned int i = 0 ; i < contacts.size() ; ++i)
 	    {
-		ROS_INFO("Collision between link '%s' and '%s'", contacts[i].link1->name.c_str(), contacts[i].link2 ? contacts[i].link2->name.c_str() : "ENVIRONMENT");
-		ROS_INFO("Contact point (in robot frame): (%g, %g, %g)", contacts[i].pos.x(), contacts[i].pos.y(), contacts[i].pos.z());
+		ROS_INFO("Collision between link '%s' and '%s'", contacts[i].link1 ? contacts[i].link1->name.c_str() : "ENVIRONMENT", contacts[i].link2 ? contacts[i].link2->name.c_str() : "ENVIRONMENT");
+		ROS_INFO("Contact point (in frame %s): (%g, %g, %g)", m_stateMonitor->getFrameId().c_str(), contacts[i].pos.x(), contacts[i].pos.y(), contacts[i].pos.z());
 		ROS_INFO("Contact normal: (%g, %g, %g)", contacts[i].normal.x(), contacts[i].normal.y(), contacts[i].normal.z());
 		ROS_INFO("Contact depth: %g", contacts[i].depth);
 	    }

Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp	2009-06-29 02:07:12 UTC (rev 17851)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp	2009-06-29 02:30:18 UTC (rev 17852)
@@ -332,8 +332,15 @@
 	    EnvironmentModelODE::kGeom* kg1 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o1));
 	    EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
 	    if (kg1 && kg2)
+	    {
 		if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
 		    return;
+		else
+		{
+		    cdata->link1 = kg1->link;
+		    cdata->link2 = kg2->link;
+		}
+	    }
 	}
 	
 	if (cdata->contacts)


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