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

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

Revision: 18537
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18537&view=rev
Author:   isucan
Date:     2009-07-09 02:06:18 +0000 (Thu, 09 Jul 2009)

Log Message:
-----------
updated method for maintaining robot pose

Modified Paths:
--------------
    pkg/trunk/highlevel/move_arm/src/move_arm.cpp
    pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
    pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
    pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_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
    pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp

Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -462,7 +462,7 @@
 	for (unsigned int i = 0 ; i < joints.size() ; ++i)
 	{
 	    start_state[i].header.frame_id = planningMonitor_->getFrameId();
-	    start_state[i].header.stamp = planningMonitor_->lastStateUpdate();
+	    start_state[i].header.stamp = planningMonitor_->lastMechanismStateUpdate();
 	    start_state[i].joint_name = joints[i]->name;
 	    st.copyParamsJoint(start_state[i].value, joints[i]->name);
 	}
@@ -497,7 +497,7 @@
 			motion_planning_msgs::JointConstraint jc;
 			jc.joint_name = arm_joint_names_[i];
 			jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
-			jc.header.stamp = planningMonitor_->lastStateUpdate();
+			jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
 			unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
 			for (unsigned int j = 0 ; j < u ; ++j)
 			{

Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -429,7 +429,7 @@
 	return;
     
     visualization_msgs::Marker mk;        
-    mk.header.stamp = psetup->model->planningMonitor->lastStateUpdate();
+    mk.header.stamp = psetup->model->planningMonitor->lastMechanismStateUpdate();
     mk.header.frame_id = psetup->model->planningMonitor->getFrameId();
     mk.ns = nh_.getName();
     mk.id = 1;    

Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -56,7 +56,7 @@
 	    planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, frame);
 	}
 	else
-	    planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
+	    planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, "torso_lift_link");
 	
 	planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &OMPLPlanning::planToGoal, this);
     }

Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -93,7 +93,7 @@
 	for (unsigned int i = 0 ; i < req.goal_constraints.joint_constraint.size(); ++i)
 	{
 	    req.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
-	    req.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
+	    req.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
 	    req.goal_constraints.joint_constraint[i].joint_name = names[i];
 	    req.goal_constraints.joint_constraint[i].value.resize(1);
 	    req.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
@@ -146,7 +146,7 @@
 	
 	req.goal_constraints.joint_constraint.resize(1);
 	req.goal_constraints.joint_constraint[0].header.stamp = ros::Time::now();
-	req.goal_constraints.joint_constraint[0].header.frame_id = "base_link";
+	req.goal_constraints.joint_constraint[0].header.frame_id = "/base_link";
 	req.goal_constraints.joint_constraint[0].joint_name = "base_joint";
 	req.goal_constraints.joint_constraint[0].value.resize(3);
 	req.goal_constraints.joint_constraint[0].toleranceAbove.resize(3);
@@ -166,7 +166,7 @@
 
 
 	// allow 1 second computation time
-	req.allowed_time = 20.0;
+	req.allowed_time = 0.5;
 	
 	// define the service messages
 	motion_planning_srvs::MotionPlan::Response res;

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h	2009-07-09 02:06:18 UTC (rev 18537)
@@ -115,24 +115,35 @@
 	    return robot_frame_;
 	}
 
-	/** \brief Return true if a full mechanism state has been received */
+	/** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
 	bool haveState(void) const
 	{
 	    return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
 	}
-
+	
 	/** \brief Return the time of the last state update */
-	const ros::Time& lastStateUpdate(void) const
+	const ros::Time& lastMechanismStateUpdate(void) const
 	{
-	    return lastStateUpdate_;
+	    return lastMechanismStateUpdate_;
 	}
 
-	/** \brief Wait until a full mechanism state is received */
+	/** \brief Return the time of the last pose update */
+	const ros::Time& lastPoseUpdate(void) const
+	{
+	    return lastPoseUpdate_;
+	}
+	
+	/** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
 	void waitForState(void) const;
 
-	/** \brief Return true if a full mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
-	bool isStateUpdated(double sec) const;
+	/** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
+	bool isMechanismStateUpdated(double sec) const;
 
+	/** \brief Return true if a pose has been received in the last
+	    sec seconds. If sec < 10us, this function always returns
+	    true. */
+	bool isPoseUpdated(double sec) const;
+	
 	/** \brief Return true if the pose is included in the state */
 	bool isPoseIncluded(void) const
 	{
@@ -170,7 +181,8 @@
 
 	bool                             havePose_;
 	bool                             haveMechanismState_;
-	ros::Time                        lastStateUpdate_;
+	ros::Time                        lastMechanismStateUpdate_;
+	ros::Time                        lastPoseUpdate_;
     };
 
 }

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-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-07-09 02:06:18 UTC (rev 18537)
@@ -114,7 +114,8 @@
 	
 	double intervalCollisionMap_;
 	double intervalState_;
-
+	double intervalPose_;
+	
     };
     
 	

Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-07-09 02:06:18 UTC (rev 18537)
@@ -191,6 +191,8 @@
 
  - @b "~refresh_interval_kinematic_state"/double : if more than this interval passes, the maintained kinematic state is considered invalid
 
+ - @b "~refresh_interval_pose"/double : if more than this interval passes, the maintained pose is considered invalid
+
  - @b "~bounding_planes"/string : a sequence of plane equations specified as "a1 b1 c1 d1 a2 b2 c2 d2 ..." where each plane is defined by the equation ax+by+cz+d=0
 
  - @b "~box_scale"/double : boxes from the collision map are approximated with spheres using the extents of the boxes. The maximum extent of the box is multiplied by the constant specified by this parameter to obtain the radius of the sphere approximating the box

Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -105,6 +105,9 @@
 
 bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
 {
+    if (!haveMap_)
+	return false;
+    
     // less than 10us is considered 0 
     if (sec > 1e-5 && lastMapUpdate_ < ros::Time::now() - ros::Duration(sec))
 	return false;

Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -114,7 +114,7 @@
 
     first_time = false;
 
-    lastStateUpdate_ = mechanismState->header.stamp;
+    lastMechanismStateUpdate_ = mechanismState->header.stamp;
     if (!haveMechanismState_)
 	haveMechanismState_ = robotState_->seenAll();
 
@@ -169,6 +169,7 @@
 		    change = change || this_changed;
 		}
 		
+		lastPoseUpdate_ = tm;
 		havePose_ = true;
 	    }
 	    else
@@ -188,24 +189,39 @@
     while (nh_.ok() && !haveState())
     {
 	if (s == 0)
-	    ROS_INFO("Waiting for mechanism state ...");
+	    ROS_INFO("Waiting for robot state ...");
 	s = (s + 1) % 40;
 	ros::spinOnce();
 	ros::Duration().fromSec(0.05).sleep();
     }
     if (haveState())
-	ROS_INFO("Mechanism state received!");
+	ROS_INFO("Robot state received!");
 }
 
-bool planning_environment::KinematicModelStateMonitor::isStateUpdated(double sec) const
-{
+bool planning_environment::KinematicModelStateMonitor::isMechanismStateUpdated(double sec) const
+{  
+    if (!haveMechanismState_)
+	return false;
+    
     // less than 10us is considered 0 
-    if (sec > 1e-5 && lastStateUpdate_ < ros::Time::now() - ros::Duration(sec))
+    if (sec > 1e-5 && lastMechanismStateUpdate_ < ros::Time::now() - ros::Duration(sec))
 	return false;
     else
 	return true;
 }
 
+bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
+{  
+    if (!havePose_)
+	return false;
+    
+    // less than 10us is considered 0 
+    if (sec > 1e-5 && lastPoseUpdate_ < ros::Time::now() - ros::Duration(sec))
+	return false;
+    else
+	return true;
+}
+
 void planning_environment::KinematicModelStateMonitor::printRobotState(void) const
 {
     std::stringstream ss;

Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-07-09 02:06:18 UTC (rev 18537)
@@ -42,6 +42,7 @@
 {
     nh_.param<double>("~refresh_interval_collision_map", intervalCollisionMap_, 0.0);
     nh_.param<double>("~refresh_interval_kinematic_state", intervalState_, 0.0);
+    nh_.param<double>("~refresh_interval_pose", intervalPose_, 0.0);
 }
 
 bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
@@ -52,11 +53,18 @@
         return false;
     }
   
-    if (!isStateUpdated(intervalState_))
+    if (!isMechanismStateUpdated(intervalState_))
     {
         ROS_WARN("Planning is not safe: robot state not updated in the last %f seconds", intervalState_);
         return false;
-    }
+    }  
+
+    if (includePose_)
+	if (!isPoseUpdated(intervalPose_))
+	{
+	    ROS_WARN("Planning is not safe: robot pose not updated in the last %f seconds", intervalPose_);
+	    return false;
+	}
     return true;
 }
 


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