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

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

Revision: 19147
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19147&view=rev
Author:   isucan
Date:     2009-07-17 22:29:38 +0000 (Fri, 17 Jul 2009)

Log Message:
-----------
callback for finding collisions

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
    pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-07-17 22:29:08 UTC (rev 19146)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-07-17 22:29:38 UTC (rev 19147)
@@ -56,11 +56,13 @@
 	
 	PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf, frame_id)
 	{
+	    onCollisionContact_ = NULL;	    
 	    loadParams();
 	}
 	
 	PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf)
 	{
+	    onCollisionContact_ = NULL;	    
 	    loadParams();
 	}
 	
@@ -101,6 +103,12 @@
 	/** \brief Transform the kinematic joint to the frame requested */
 	bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
 	
+	/** \brief Set a callback to be called when a collision is found */
+	void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback) 
+	{
+	    onCollisionContact_ = callback;
+	}
+	
     protected:
 
 	/** \brief Load ROS parameters */
@@ -111,6 +119,9 @@
 	
 	/** \brief Check the path assuming it is in the frame of the model */
 	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path, bool verbose) const;
+
+	/** \brief User callback when a collision is found */
+	boost::function<void(collision_space::EnvironmentModel::Contact&)> onCollisionContact_;
 	
 	motion_planning_msgs::KinematicConstraints kcPath_;
 	motion_planning_msgs::KinematicConstraints kcGoal_;

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-07-17 22:29:08 UTC (rev 19146)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-07-17 22:29:38 UTC (rev 19147)
@@ -278,11 +278,16 @@
     getEnvironmentModel()->updateRobotModel();
 
     // check for collision
-    bool valid = !getEnvironmentModel()->isCollision();
+    std::vector<collision_space::EnvironmentModel::Contact> contacts;
+    bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, 1);
     
     getKinematicModel()->unlock();
     getEnvironmentModel()->unlock();
     
+    if (onCollisionContact_)
+	for (unsigned int i = 0 ; i < contacts.size() ; ++i)
+	    onCollisionContact_(contacts[i]);
+    
     return valid;
 }
 
@@ -297,7 +302,8 @@
     getEnvironmentModel()->updateRobotModel();
 
     // check for collision
-    bool valid = !getEnvironmentModel()->isCollision();
+    std::vector<collision_space::EnvironmentModel::Contact> contacts;
+    bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, 1);
     
     if (valid)
     {	    
@@ -309,7 +315,11 @@
     
     getKinematicModel()->unlock();
     getEnvironmentModel()->unlock();
-    
+
+    if (onCollisionContact_)
+	for (unsigned int i = 0 ; i < contacts.size() ; ++i)
+	    onCollisionContact_(contacts[i]);
+
     return valid;
 }
 
@@ -324,7 +334,8 @@
     getEnvironmentModel()->updateRobotModel();
     
     // check for collision
-    bool valid = !getEnvironmentModel()->isCollision();
+    std::vector<collision_space::EnvironmentModel::Contact> contacts;
+    bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, 1);
     
     if (valid)
     {	    
@@ -339,6 +350,10 @@
     getKinematicModel()->unlock();
     getEnvironmentModel()->unlock();
     
+    if (onCollisionContact_)
+	for (unsigned int i = 0 ; i < contacts.size() ; ++i)
+	    onCollisionContact_(contacts[i]);
+    
     return valid;    
 }
 
@@ -425,8 +440,13 @@
 	getEnvironmentModel()->updateRobotModel();
 	
 	// check for collision
-	valid = !getEnvironmentModel()->isCollision();
-
+	std::vector<collision_space::EnvironmentModel::Contact> contacts;
+	valid = !getEnvironmentModel()->getCollisionContacts(contacts, 1);
+	
+	if (onCollisionContact_)
+	    for (unsigned int i = 0 ; i < contacts.size() ; ++i)
+		onCollisionContact_(contacts[i]);
+	
 	if (verbose && !valid)
 	    ROS_INFO("isPathValid: State %d is in collision", i);
 


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