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

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

Revision: 18307
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18307&view=rev
Author:   isucan
Date:     2009-07-06 04:06:59 +0000 (Mon, 06 Jul 2009)

Log Message:
-----------
more robust transform handling

Modified Paths:
--------------
    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/src/kinematic_model_state_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp

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-06 03:18:49 UTC (rev 18306)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h	2009-07-06 04:06:59 UTC (rev 18307)
@@ -158,6 +158,9 @@
 	ros::Subscriber                  mechanismStateSubscriber_;
 	tf::TransformListener           *tf_;
 
+	/** \brief How long to wait for a TF if it is not yet available, before failing */
+	ros::Duration                    tfWait_;
+
 	planning_models::StateParams    *robotState_;
 	tf::Pose                         pose_;
 	std::string                      robot_frame_;

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-06 03:18:49 UTC (rev 18306)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-07-06 04:06:59 UTC (rev 18307)
@@ -114,6 +114,7 @@
 	
 	double intervalCollisionMap_;
 	double intervalState_;
+
     };
     
 	

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-06 03:18:49 UTC (rev 18306)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp	2009-07-06 04:06:59 UTC (rev 18307)
@@ -44,6 +44,7 @@
     robotState_ = NULL;
     onStateUpdate_ = NULL;
     tf_ = NULL;
+    tfWait_ = ros::Duration(0.1);
     havePose_ = haveMechanismState_ = false;
     if (rm_->loadedModels())
     {
@@ -123,43 +124,57 @@
 	if (tf_->canTransform(frame_id_, robot_frame_, mechanismState->header.stamp))
 	{
 	    tf::Stamped<tf::Pose> transf;
-	    tf_->lookupTransform(frame_id_, robot_frame_, mechanismState->header.stamp, transf);
-	    pose_ = transf;
-
-	    if (!planarJoint_.empty())
+	    bool ok = true;
+	    try
 	    {
-		double planar_joint[3];
-		planar_joint[0] = pose_.getOrigin().x();
-		planar_joint[1] = pose_.getOrigin().y();
-
-		double yaw, pitch, roll;
-		pose_.getBasis().getEulerZYX(yaw, pitch, roll);
-		planar_joint[2] = yaw;
-
-		bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
-		change = change || this_changed;
+		tf_->lookupTransform(frame_id_, robot_frame_, mechanismState->header.stamp, transf);
 	    }
-
-	    if (!floatingJoint_.empty())
+	    catch(...)
 	    {
-		double floating_joint[7];
-		floating_joint[0] = pose_.getOrigin().x();
-		floating_joint[1] = pose_.getOrigin().y();
-		floating_joint[2] = pose_.getOrigin().z();
-		btQuaternion q = pose_.getRotation();
-		floating_joint[3] = q.x();
-		floating_joint[4] = q.y();
-		floating_joint[5] = q.z();
-		floating_joint[6] = q.w();
-
-		bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
-		change = change || this_changed;
+		ok = false;
 	    }
-
-	    havePose_ = true;
+	    
+	    if (ok)
+	    {		
+		pose_ = transf;
+		
+		if (!planarJoint_.empty())
+		{
+		    double planar_joint[3];
+		    planar_joint[0] = pose_.getOrigin().x();
+		    planar_joint[1] = pose_.getOrigin().y();
+		    
+		    double yaw, pitch, roll;
+		    pose_.getBasis().getEulerZYX(yaw, pitch, roll);
+		    planar_joint[2] = yaw;
+		    
+		    bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
+		    change = change || this_changed;
+		}
+		
+		if (!floatingJoint_.empty())
+		{
+		    double floating_joint[7];
+		    floating_joint[0] = pose_.getOrigin().x();
+		    floating_joint[1] = pose_.getOrigin().y();
+		    floating_joint[2] = pose_.getOrigin().z();
+		    btQuaternion q = pose_.getRotation();
+		    floating_joint[3] = q.x();
+		    floating_joint[4] = q.y();
+		    floating_joint[5] = q.z();
+		    floating_joint[6] = q.w();
+		    
+		    bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
+		    change = change || this_changed;
+		}
+		
+		havePose_ = true;
+	    }
+	    else
+		ROS_ERROR("Transforming from link '%s' to link '%s' failed", robot_frame_.c_str(), frame_id_.c_str());
 	}
 	else
-	    ROS_WARN("Unable fo find transform from link '%s' to link '%s'", robot_frame_.c_str(), frame_id_.c_str());
+	    ROS_WARN("Unable to find transform from link '%s' to link '%s'", robot_frame_.c_str(), frame_id_.c_str());
     }
 
     if (change && onStateUpdate_ != NULL)

Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-07-06 03:18:49 UTC (rev 18306)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-07-06 04:06:59 UTC (rev 18307)
@@ -84,7 +84,25 @@
 {
     bool res = true;
     for (unsigned int i = 0; i < kc.pose_constraint.size() ; ++i)
-	tf_->transformPose(target, kc.pose_constraint[i].pose, kc.pose_constraint[i].pose);
+    {
+	bool ok = false;
+	if (tf_->canTransform(target, kc.pose_constraint[i].pose.header.frame_id, kc.pose_constraint[i].pose.header.stamp, tfWait_))
+	{
+	    try
+	    {
+		tf_->transformPose(target, kc.pose_constraint[i].pose, kc.pose_constraint[i].pose);
+		ok = true;
+	    }
+	    catch(...)
+	    {
+	    }
+	}
+	if (!ok)
+	{
+	    ROS_ERROR("Unable to transform pose constraint on link '%s' to frame '%s'", kc.pose_constraint[i].link_name.c_str(), target.c_str());
+	    res = false;
+	}
+    }
     
     // if there are any floating or planar joints, transform them
     if (getKinematicModel()->getModelInfo().planarJoints.size() > 0 || getKinematicModel()->getModelInfo().floatingJoints.size() > 0)
@@ -175,7 +193,23 @@
 	pose.getBasis().setEulerYPR(params[index + 2], 0.0, 0.0);
 	pose.stamp_ = header.stamp;
 	pose.frame_id_ = header.frame_id;
-	tf_->transformPose(target, pose, pose);
+	bool ok = false;
+	if (tf_->canTransform(target, pose.frame_id_, pose.stamp_, tfWait_))
+	{
+	    try
+	    {
+		tf_->transformPose(target, pose, pose);
+		ok = true;
+	    }
+	    catch(...)
+	    {
+	    }
+	}
+	if (!ok)
+	{
+	    ROS_ERROR("Unable to transform planar joint '%s' to frame '%s'", name.c_str(), target.c_str());
+	    return false;
+	}
 	params[index + 0] = pose.getOrigin().getX();
 	params[index + 1] = pose.getOrigin().getY();
 	btScalar dummy;
@@ -192,7 +226,23 @@
 	pose.setRotation(q);
 	pose.stamp_ = header.stamp;
 	pose.frame_id_ = header.frame_id;
-	tf_->transformPose(target, pose, pose);
+	bool ok = false;
+	if (tf_->canTransform(target, pose.frame_id_, pose.stamp_, tfWait_))
+	{
+	    try
+	    {
+		tf_->transformPose(target, pose, pose);
+		ok = true;
+	    }
+	    catch(...)
+	    {
+	    }
+	}
+	if (!ok)
+	{
+	    ROS_ERROR("Unable to transform floating joint '%s' to frame '%s'", name.c_str(), target.c_str());
+	    return false;
+	}
 	params[index + 0] = pose.getOrigin().getX();
 	params[index + 1] = pose.getOrigin().getY();
 	params[index + 2] = pose.getOrigin().getZ();
@@ -265,7 +315,6 @@
     return valid;    
 }
 
-
 bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path, bool verbose) const
 {
     if (path.header.frame_id != getFrameId())


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