transform_global_plan.cpp
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 }


asr_ftc_local_planner
Author(s): Marek Felix
autogenerated on Wed Jun 19 2019 19:38:18