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

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

Revision: 20312
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20312&view=rev
Author:   isucan
Date:     2009-07-31 21:02:32 +0000 (Fri, 31 Jul 2009)

Log Message:
-----------
no longer using deprecated canTransform

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

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h	2009-07-31 20:52:46 UTC (rev 20311)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h	2009-07-31 21:02:32 UTC (rev 20312)
@@ -203,9 +203,6 @@
 	tf::MessageNotifier<mapping_msgs::AttachedObject>
 	                                *attachedBodyNotifier_;
 
-	/** \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/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp	2009-07-31 20:52:46 UTC (rev 20311)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp	2009-07-31 21:02:32 UTC (rev 20312)
@@ -48,7 +48,6 @@
     onStateUpdate_ = NULL;
     onAfterAttachBody_ = NULL;
     attachedBodyNotifier_ = NULL;
-    tfWait_ = ros::Duration(0.1);
     havePose_ = haveMechanismState_ = false;
 
     if (rm_->loadedModels())

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-07-31 20:52:46 UTC (rev 20311)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-07-31 21:02:32 UTC (rev 20312)
@@ -94,17 +94,23 @@
     for (unsigned int i = 0; i < kc.pose_constraint.size() ; ++i)
     {
 	bool ok = false;
-	if (tf_->canTransform(target, kc.pose_constraint[i].pose.header.frame_id, kc.pose_constraint[i].pose.header.stamp, tfWait_))
+	
+	unsigned int steps = 0;
+	while (!tf_->canTransform(target, kc.pose_constraint[i].pose.header.frame_id, kc.pose_constraint[i].pose.header.stamp) && steps < 10)
 	{
-	    try
-	    {
-		tf_->transformPose(target, kc.pose_constraint[i].pose, kc.pose_constraint[i].pose);
-		ok = true;
-	    }
-	    catch(...)
-	    {
-	    }
+	    ros::Duration(0.01).sleep();
+	    steps++;
 	}
+	
+	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());
@@ -202,17 +208,23 @@
 	pose.stamp_ = header.stamp;
 	pose.frame_id_ = header.frame_id;
 	bool ok = false;
-	if (tf_->canTransform(target, pose.frame_id_, pose.stamp_, tfWait_))
+
+	unsigned int steps = 0;
+	while (!tf_->canTransform(target, pose.frame_id_, pose.stamp_) && steps < 10)
 	{
-	    try
-	    {
-		tf_->transformPose(target, pose, pose);
-		ok = true;
-	    }
-	    catch(...)
-	    {
-	    }
+	    ros::Duration(0.01).sleep();
+	    steps++;
 	}
+	
+	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());
@@ -235,22 +247,29 @@
 	pose.stamp_ = header.stamp;
 	pose.frame_id_ = header.frame_id;
 	bool ok = false;
-	if (tf_->canTransform(target, pose.frame_id_, pose.stamp_, tfWait_))
+	
+	unsigned int steps = 0;
+	while (!tf_->canTransform(target, pose.frame_id_, pose.stamp_) && steps < 10)
 	{
-	    try
-	    {
-		tf_->transformPose(target, pose, pose);
-		ok = true;
-	    }
-	    catch(...)
-	    {
-	    }
+	    ros::Duration(0.01).sleep();
+	    steps++;
 	}
+	
+	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();


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