Go to the documentation of this file.00001
00018 #include <ftc_local_planner/transform_global_plan.h>
00019
00020 namespace ftc_local_planner
00021 {
00022 bool getXPose(const tf::TransformListener& tf,
00023 const std::vector<geometry_msgs::PoseStamped>& global_plan,
00024 const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose, int plan_point)
00025 {
00026 if (global_plan.empty())
00027 {
00028 ROS_ERROR("Received plan with zero length");
00029 return false;
00030 }
00031 if(plan_point >= (int)global_plan.size())
00032 {
00033 ROS_ERROR("Goal_functions: Plan_point %d to big. Plan size: %lu",plan_point, global_plan.size());
00034 return false;
00035 }
00036
00037 const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.at(plan_point);
00038 try
00039 {
00040 tf::StampedTransform transform;
00041 tf.waitForTransform(global_frame, ros::Time::now(),
00042 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
00043 plan_goal_pose.header.frame_id, ros::Duration(0.5));
00044 tf.lookupTransform(global_frame, ros::Time(),
00045 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
00046 plan_goal_pose.header.frame_id, transform);
00047
00048 poseStampedMsgToTF(plan_goal_pose, goal_pose);
00049 goal_pose.setData(transform * goal_pose);
00050 goal_pose.stamp_ = transform.stamp_;
00051 goal_pose.frame_id_ = global_frame;
00052
00053 }
00054 catch(tf::LookupException& ex)
00055 {
00056 ROS_ERROR("No Transform available Error: %s\n", ex.what());
00057 return false;
00058 }
00059 catch(tf::ConnectivityException& ex)
00060 {
00061 ROS_ERROR("Connectivity Error: %s\n", ex.what());
00062 return false;
00063 }
00064 catch(tf::ExtrapolationException& ex)
00065 {
00066 ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00067 if (global_plan.size() > 0)
00068 ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
00069
00070 return false;
00071 }
00072 return true;
00073 }
00074 }