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

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

Revision: 17231
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17231&view=rev
Author:   isucan
Date:     2009-06-17 18:30:56 +0000 (Wed, 17 Jun 2009)

Log Message:
-----------
check path safety

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

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-17 18:30:25 UTC (rev 17230)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-06-17 18:30:56 UTC (rev 17231)
@@ -56,6 +56,8 @@
 	
 	PlanningMonitor(CollisionModels *cm, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), frame_id)
 	{
+	    nh_.param("~refresh_interval_collision_map", intervalCollisionMap_, 3.0);
+	    nh_.param("~refresh_interval_kinematic_state", intervalState_, 0.5);
 	}
 	
 	PlanningMonitor(CollisionModels *cm) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm))
@@ -66,6 +68,9 @@
 	{
 	}
 
+	/** 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 */
 	bool isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const;
 
@@ -100,6 +105,9 @@
 	
 	motion_planning_msgs::KinematicConstraints kcPath_;
 	motion_planning_msgs::KinematicConstraints kcGoal_;
+	
+	double intervalCollisionMap_;
+	double intervalState_;
     };
     
 	

Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-06-17 18:30:25 UTC (rev 17230)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-06-17 18:30:56 UTC (rev 17231)
@@ -37,6 +37,22 @@
 #include "planning_environment/planning_monitor.h"
 #include "planning_environment/kinematic_state_constraint_evaluator.h"
 
+bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
+{
+    if (isMapUpdated(intervalCollisionMap_))
+    {
+	ROS_WARN("Planning is not safe: map is not up to date");
+	return false;
+    }
+    
+    if (isStateUpdated(intervalState_))
+    {
+	ROS_WARN("Planning is not safe: robot state is not up to date");
+	return false;
+    }
+    return true;
+}
+
 void planning_environment::PlanningMonitor::setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc)
 {
     kcPath_ = kc;


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