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

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

Revision: 2155
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2155&view=rev
Author:   isucan
Date:     2008-07-26 00:05:50 +0000 (Sat, 26 Jul 2008)

Log Message:
-----------
fixed clear function in collision space

Modified Paths:
--------------
    pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
    pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp

Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-07-26 00:04:01 UTC (rev 2154)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-07-26 00:05:50 UTC (rev 2155)
@@ -119,7 +119,7 @@
 	m_collisionSpace->unlock();
 	
 	// temp obstacle
-	double sphere[3] = {0.8,0.2,0.4};    
+	double sphere[6] = {0.8,0.2,0.4};    
 	m_collisionSpace->addPointCloud(1, sphere, 0.15);
 	
 #ifdef DISPLAY_ODE_SPACES
@@ -162,25 +162,42 @@
     
     void pointCloudCallback(void)
     {
-	unsigned int n = m_cloud.get_pts_size();
+	const int frac = 50;
+	unsigned int n = m_cloud.get_pts_size()/frac;
 	printf("received %u points\n", n);
 
+	
+#ifdef DISPLAY_ODE_SPACES
+	spaces.clear();	
+#endif
+
+
 	ros::Time startTime = ros::Time::now();
 	double *data = new double[3 * n];	
 	for (unsigned int i = 0 ; i < n ; ++i)
 	{
 	    unsigned int i3 = i * 3;	    
-	    data[i3    ] = m_cloud.pts[i].x;
-	    data[i3 + 1] = m_cloud.pts[i].y;
-	    data[i3 + 2] = m_cloud.pts[i].z;
+	    data[i3    ] = m_cloud.pts[i * frac].x;
+	    data[i3 + 1] = m_cloud.pts[i * frac].y;
+	    data[i3 + 2] = m_cloud.pts[i * frac].z;
 	}
 	
 	m_collisionSpace->lock();
 	m_collisionSpace->clearObstacles();
-	m_collisionSpace->addPointCloud(n, data, 0.01);
+	m_collisionSpace->addPointCloud(n, data, 0.03);
 	m_collisionSpace->unlock();
 	
 	delete[] data;
+
+#ifdef DISPLAY_ODE_SPACES
+	collision_space::EnvironmentModelODE* okm = dynamic_cast<collision_space::EnvironmentModelODE*>(m_collisionSpace);
+	if (okm)
+	{
+	    spaces.addSpace(okm->getODESpace(), 1.0f, 0.0f, 0.0f);
+	    for (unsigned int i = 0 ; i < okm->getModelCount() ; ++i)
+		spaces.addSpace(okm->getModelODESpace(i), 0.1f, 0.5f, (float)(i + 1)/(float)okm->getModelCount());
+	}
+#endif
 	
 	double tupd = (ros::Time::now() - startTime).to_double();	
 	printf("Updated world model in %f seconds\n", tupd);

Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp	2008-07-26 00:04:01 UTC (rev 2154)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp	2008-07-26 00:05:50 UTC (rev 2155)
@@ -226,7 +226,8 @@
 void collision_space::EnvironmentModelODE::clearObstacles(void)
 {
     m_collide2.clear();
-    freeMemory();
+    if (m_space)
+	dSpaceDestroy(m_space);
     m_space = dHashSpaceCreate(0);
     m_collide2.setup();
 }


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