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

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

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

Log Message:
-----------
more debug messages + bugfix in move_arm

Modified Paths:
--------------
    pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
    pkg/trunk/highlevel/move_arm/src/move_arm.cpp
    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/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp	2009-06-27 22:30:32 UTC (rev 17828)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp	2009-06-27 23:06:04 UTC (rev 17829)
@@ -303,7 +303,7 @@
 	    else
 	    {
 		std::cout << "Moving to " << config << "..." << std::endl;
-		if (move_arm.execute(goals[config], feedback, ros::Duration(10.0)) != robot_actions::SUCCESS)
+		if (move_arm.execute(goals[config], feedback, ros::Duration(allowed_time)) != robot_actions::SUCCESS)
 		    std::cerr << "Failed achieving goal" << std::endl;
 		else
 		    std::cout << "Success!" << std::endl;

Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp	2009-06-27 22:30:32 UTC (rev 17828)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp	2009-06-27 23:06:04 UTC (rev 17829)
@@ -183,6 +183,7 @@
 	int                trajectoryId = -1;
 	bool               approx       = false;
 	ros::Duration      eps(0.01);
+	ros::Duration      epsLong(0.1);
 	while (true)
 	{
 	    // if we have to stop, do so
@@ -210,7 +211,10 @@
 		if (clientPlan.call(req, res))
 		{
 		    if (res.path.states.empty())
-			ROS_WARN("Unable to plan path to desired goal");
+		    {
+		        ROS_WARN("Unable to plan path to desired goal");
+			epsLong.sleep();
+		    }
 		    else
 		    {
 			if (res.unsafe)
@@ -250,12 +254,15 @@
 	    if (isPreemptRequested() || !node_handle_.ok())
 		result = robot_actions::PREEMPTED;
 
+	    // if preeemt was requested while we are planning, terminate
+	    if (result != robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
+	        break;
 
 	    // stop the robot if we need to
 	    if (feedback == pr2_robot_actions::MoveArmState::MOVING)
 	    {
 		bool safe = planningMonitor_->isEnvironmentSafe();
-		bool valid = planningMonitor_->isPathValid(currentPath_);
+		bool valid = planningMonitor_->isPathValid(currentPath_, true);
 		if (result == robot_actions::PREEMPTED || !safe || !valid)
 		{
 		    if (result == robot_actions::PREEMPTED)

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 22:30:32 UTC (rev 17828)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-06-27 23:06:04 UTC (rev 17829)
@@ -78,7 +78,7 @@
 	bool isStateValidAtGoal(const planning_models::StateParams *state) const;
 	
 	/** \brief Check if the path is valid */
-	bool isPathValid(const motion_planning_msgs::KinematicPath &path) const;
+	bool isPathValid(const motion_planning_msgs::KinematicPath &path, bool verbose) const;
 	
 	/** \brief Set the kinematic constraints the monitor should use when checking a path */
 	void setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc);
@@ -104,7 +104,7 @@
 	bool transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target) const;
 	
 	/** \brief Check the path assuming it is in the frame of the model */
-	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path) const;
+	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path, bool verbose) const;
 	
 	motion_planning_msgs::KinematicConstraints kcPath_;
 	motion_planning_msgs::KinematicConstraints kcGoal_;

Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-06-27 22:30:32 UTC (rev 17828)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-06-27 23:06:04 UTC (rev 17829)
@@ -258,21 +258,21 @@
 }
 
 
-bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path) const
+bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path, bool verbose) const
 {
     if (path.header.frame_id != getFrameId())
     {
 	motion_planning_msgs::KinematicPath pathT = path;
 	if (transformPathToFrame(pathT, getFrameId()))
-	    return isPathValidAux(pathT);
+	  return isPathValidAux(pathT, verbose);
 	else
 	    return false;
     }
     else
-	return isPathValidAux(path);
+      return isPathValidAux(path, verbose);
 }
 
-bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path) const
+bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path, bool verbose) const
 {    
     boost::scoped_ptr<planning_models::StateParams> sp(getKinematicModel()->newStateParams());
     
@@ -339,10 +339,17 @@
 	
 	// check for collision
 	valid = !getEnvironmentModel()->isCollision();
-    
+
+	if (verbose && !valid)
+	    ROS_INFO("isPathValid: State %d is in collision", i);
+
 	// check for validity
 	if (valid)
+	{
 	    valid = ks.decide(sp->getParams());
+	    if (verbose && !valid)
+	        ROS_INFO("isPathValid: State %d does not satisfy constraints", i);
+	}
     }
 
     // if we got to the last state, we also check the goal constraints
@@ -351,6 +358,8 @@
 	ks.add(getKinematicModel(), kcGoal_.joint_constraint);
 	ks.add(getKinematicModel(), kcGoal_.pose_constraint);
 	valid = ks.decide(sp->getParams());
+	if (verbose && !valid)
+	    ROS_INFO("isPathValid: Goal state does not satisfy constraints");
     }
     
     getKinematicModel()->unlock();


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