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

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

Revision: 17816
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17816&view=rev
Author:   isucan
Date:     2009-06-27 17:54:19 +0000 (Sat, 27 Jun 2009)

Log Message:
-----------
support for map updates + a bit more documentation

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_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/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h	2009-06-27 17:51:48 UTC (rev 17815)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h	2009-06-27 17:54:19 UTC (rev 17816)
@@ -78,6 +78,8 @@
 	{
 	    if (collisionMapNotifier_)
 		delete collisionMapNotifier_;
+	    if (collisionMapUpdateNotifier_)
+		delete collisionMapUpdateNotifier_;
 	    if (attachedBodyNotifier_)
 		delete attachedBodyNotifier_;
 	}
@@ -139,7 +141,9 @@
     protected:
 	
 	void setupCSM(void);
+	void updateCollisionSpace(const robot_msgs::CollisionMapConstPtr &collisionMap, bool clear);
 	void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
+	void collisionMapUpdateCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
 	void attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject);
 	
 	CollisionModels                                               *cm_;
@@ -148,6 +152,7 @@
 	bool                                                           haveMap_;
 	ros::Time                                                      lastMapUpdate_;	
 	tf::MessageNotifier<robot_msgs::CollisionMap>                 *collisionMapNotifier_;
+	tf::MessageNotifier<robot_msgs::CollisionMap>                 *collisionMapUpdateNotifier_;
 	tf::MessageNotifier<robot_msgs::AttachedObject>               *attachedBodyNotifier_;
 	
 	boost::function<void(const robot_msgs::CollisionMapConstPtr)>  onBeforeMapUpdate_;

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-06-27 17:51:48 UTC (rev 17815)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-06-27 17:54:19 UTC (rev 17816)
@@ -68,39 +68,39 @@
 	{
 	}
 
-	/** Return true if recent enough data is available so that planning is considered safe */
+	/** \brief Return true if recent enough data is available so that planning is considered safe */
 	bool isEnvironmentSafe(void) const;
 	
-	/** Check if the full state of the robot is valid */
+	/** \brief Check if the full state of the robot is valid */
 	bool isStateValidOnPath(const planning_models::StateParams *state) const;
 
-	/** Check if the full state of the robot is valid */
+	/** \brief Check if the full state of the robot is valid */
 	bool isStateValidAtGoal(const planning_models::StateParams *state) const;
 	
-	/** Check if the path is valid */
+	/** \brief Check if the path is valid */
 	bool isPathValid(const motion_planning_msgs::KinematicPath &path) const;
 	
-	/** Set the kinematic constraints the monitor should use when checking a path */
+	/** \brief Set the kinematic constraints the monitor should use when checking a path */
 	void setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc);
 
-	/** Set the kinematic constraints the monitor should use when checking a path's last state (the goal) */
+	/** \brief Set the kinematic constraints the monitor should use when checking a path's last state (the goal) */
 	void setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc);
 	
-	/** Transform the frames in which constraints are specified to the one requested */
+	/** \brief Transform the frames in which constraints are specified to the one requested */
 	bool transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
 	
-	/** Transform the kinematic path to the frame requested */
+	/** \brief Transform the kinematic path to the frame requested */
 	bool transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const;
 
-	/** Transform the kinematic joint to the frame requested */
+	/** \brief Transform the kinematic joint to the frame requested */
 	bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
 	
     protected:
 	
-	/** Transform the joint parameters (if needed) to a target frame */
+	/** \brief Transform the joint parameters (if needed) to a target frame */
 	bool transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target) const;
 	
-	/** Check the path assuming it is in the frame of the model */
+	/** \brief Check the path assuming it is in the frame of the model */
 	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path) const;
 	
 	motion_planning_msgs::KinematicConstraints kcPath_;

Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-06-27 17:51:48 UTC (rev 17815)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-06-27 17:54:19 UTC (rev 17816)
@@ -18,20 +18,33 @@
 
 \section codeapi Code API
 
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
+The intended use for this package is to instantiante one of the two
+model classes and potentially one of the monitor classes.
 
+The model classes are:
+- \b RobotModels : allows the instantiation of various robot models (for example, a kinematic one) based on data from the parameter server. The URDF robot description and the .yaml files describing collision and planning information are assumed to be loaded.
+
+- \b CollisionModels : allows the instantiation of various robot models (for example, a kinematic one) and various collision spaces, based on data from the parameter server. This class inherits from \b RobotModels. The URDF robot description and the .yaml files describing collision and planning information are assumed to be loaded.
+
+<hr>
+
+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 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
+documentation within your package that is of particular interest to a
+reader. Doxygen will document pretty much every part of your code, so
+do your best here to point the reader to the actual API.
+
 If your codebase is fairly large or has different sets of APIs, you
 should use the doxygen 'group' tag to keep these APIs together. For
 example, the roscpp documentation has 'libros' group.
 -->
 
-\section rosapi ROS API
+<!-- \section rosapi ROS API
 
-<!--
 Names are very important in ROS because they can be remapped on the
 command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
 APPEAR IN THE CODE. You should list names of every topic, service and

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-27 17:51:48 UTC (rev 17815)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-27 17:54:19 UTC (rev 17816)
@@ -67,6 +67,9 @@
     collisionMapNotifier_ = new tf::MessageNotifier<robot_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
     ROS_DEBUG("Listening to collision_map");
 
+    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");
+
     if (cm_->loadedModels())
     {
 	attachedBodyNotifier_ = new tf::MessageNotifier<robot_msgs::AttachedObject>(*tf_, boost::bind(&CollisionSpaceMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
@@ -99,8 +102,18 @@
 	ROS_INFO("Map received!");
 }
 
+void planning_environment::CollisionSpaceMonitor::collisionMapUpdateCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
+{
+    updateCollisionSpace(collisionMap, false);
+}
+
 void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
 {
+    updateCollisionSpace(collisionMap, true);
+}
+
+void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const robot_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+{
     int n = collisionMap->get_boxes_size();
     ROS_DEBUG("Received %d points (collision map)", n);
     
@@ -164,7 +177,8 @@
     }
     
     collisionSpace_->lock();
-    collisionSpace_->clearObstacles();
+    if (clear)
+	collisionSpace_->clearObstacles();
     collisionSpace_->addPointCloud(n, data);
     collisionSpace_->unlock();
     


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