[Personalrobots-commit] SF.net SVN: personalrobots:[17846] pkg/trunk/motion_planning/ planning_environment

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

Revision: 17846
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17846&view=rev
Author:   isucan
Date:     2009-06-29 01:08:31 +0000 (Mon, 29 Jun 2009)

Log Message:
-----------
fix locking issues

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
    pkg/trunk/motion_planning/planning_environment/mainpage.dox
    pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp

Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt	2009-06-29 01:07:52 UTC (rev 17845)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt	2009-06-29 01:08:31 UTC (rev 17846)
@@ -12,7 +12,8 @@
 					 src/kinematic_state_constraint_evaluator.cpp
 					 src/planning_monitor.cpp)
 rospack_add_openmp_flags(planning_environment)
-					 
+rospack_link_boost(planning_environment thread)
+ 
 # Tests
 
 # Create a model of the PR2

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h	2009-06-29 01:07:52 UTC (rev 17845)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h	2009-06-29 01:08:31 UTC (rev 17846)
@@ -44,6 +44,8 @@
 #include <robot_msgs/CollisionMap.h>
 #include <robot_msgs/AttachedObject.h>
 
+#include <boost/thread/mutex.hpp>
+
 namespace planning_environment
 {
 
@@ -148,6 +150,8 @@
 	
 	CollisionModels                                               *cm_;
 	collision_space::EnvironmentModel                             *collisionSpace_;
+	double                                                         boxScale_;
+	boost::mutex                                                   mapUpdateLock_;
 	
 	bool                                                           haveMap_;
 	ros::Time                                                      lastMapUpdate_;	

Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-06-29 01:07:52 UTC (rev 17845)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-06-29 01:08:31 UTC (rev 17846)
@@ -30,7 +30,7 @@
 
 The monitor classes are:
 - \b KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'mechanism_state' topic.
-- \b CollisionSpaceMonitor : same as \b KinematicModelStateMonitor except it also monitors the state of the collision environment. It uses the 'collision_map' topic to receive new full maps and the 'collision_map_update' to receive updates. Attaching objects to robot links is possible using the 'attach_object' topic.
+- \b CollisionSpaceMonitor : same as \b KinematicModelStateMonitor except it also monitors the state of the collision environment. It uses the 'collision_map' topic to receive new full maps and the 'collision_map_update' to receive updates. Attaching objects to robot links is possible using the 'attach_object' topic. The '~box_scale' parameter is used as the constant that gets multiplied to a box's maximum extent to obtain the radius of the sphere used in collision checking.
 - \b PlanningMonitor : same as \b CollisionSpaceMonitor except it also offers the ability to evaluate kinematic constraints and check paths and states for validity.
 
       <!-- Provide links to specific auto-generated API

Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-29 01:07:52 UTC (rev 17845)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-29 01:08:31 UTC (rev 17846)
@@ -42,9 +42,9 @@
 namespace planning_environment
 {
     
-    static inline double radiusOfBox(const robot_msgs::Point32 &point)
+    static inline double maxCoord(const robot_msgs::Point32 &point)
     {
-	return std::max(std::max(point.x, point.y), point.z) * 1.73;
+	return std::max(std::max(point.x, point.y), point.z);
     }
 }
 
@@ -62,19 +62,22 @@
     if (!tf_)
 	tf_ = new tf::TransformListener();
     
+    // the factor by which a boxes maximum extent is multiplied to get the radius of the sphere to be placed in the collision space
+    nh_.param<double>("~box_scale", boxScale_, 2.0);
+    
     collisionSpace_ = cm_->getODECollisionModel().get();
     
     collisionMapNotifier_ = new tf::MessageNotifier<robot_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
-    ROS_DEBUG("Listening to collision_map");
+    ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapNotifier_->getTargetFramesString().c_str());
 
     collisionMapUpdateNotifier_ = new tf::MessageNotifier<robot_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1), "collision_map_update", getFrameId(), 1);
-    ROS_DEBUG("Listening to collision_map_update");
+    ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
 
     if (cm_->loadedModels())
     {
 	attachedBodyNotifier_ = new tf::MessageNotifier<robot_msgs::AttachedObject>(*tf_, boost::bind(&CollisionSpaceMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
 	attachedBodyNotifier_->setTargetFrame(cm_->getCollisionCheckLinks());
-	ROS_DEBUG("Listening to attach_object");
+	ROS_DEBUG("Listening to attach_object using message notifier with target frame %s", attachedBodyNotifier_->getTargetFramesString().c_str());
     }
 }
 
@@ -115,6 +118,8 @@
 
 void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const robot_msgs::CollisionMapConstPtr &collisionMap, bool clear)
 {
+    boost::mutex::scoped_lock lock(mapUpdateLock_);
+    
     int n = collisionMap->get_boxes_size();
     
     ROS_DEBUG("Received %d points (collision map)", n);
@@ -124,7 +129,7 @@
 
     // we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
     bool transform = !frame_id_.empty() && collisionMap->header.frame_id != frame_id_;
-    
+
     ros::WallTime startTime = ros::WallTime::now();
     double *data = n > 0 ? new double[4 * n] : NULL;	
 
@@ -158,7 +163,7 @@
 	    data[i4 + 1] = pso.point.y;
 	    data[i4 + 2] = pso.point.z;
 	    
-	    data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
+	    data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * boxScale_;
 	}
 	
 	if (err)
@@ -166,6 +171,7 @@
     }
     else
     {
+
 #pragma omp parallel for
 	for (int i = 0 ; i < n ; ++i)
 	{
@@ -174,15 +180,17 @@
 	    data[i4 + 1] = collisionMap->boxes[i].center.y;
 	    data[i4 + 2] = collisionMap->boxes[i].center.z;
 	    
-	    data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
+	    data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * boxScale_;
 	}
     }
     
     collisionSpace_->lock();
+
     if (clear)
 	collisionSpace_->clearObstacles();
     if (n > 0)
         collisionSpace_->addPointCloud(n, data);
+
     collisionSpace_->unlock();
     
     if (data)


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