Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <eband_local_planner/conversions_and_types.h>
00040
00041
00042 namespace eband_local_planner{
00043
00044
00045 void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D)
00046 {
00047
00048 tf::Pose pose_tf;
00049
00050
00051 tf::poseMsgToTF(pose, pose_tf);
00052
00053
00054 double useless_pitch, useless_roll, yaw;
00055 pose_tf.getBasis().getEulerYPR(yaw, useless_pitch, useless_roll);
00056
00057
00058 yaw = angles::normalize_angle(yaw);
00059
00060
00061 pose2D.x = pose.position.x;
00062 pose2D.y = pose.position.y;
00063 pose2D.theta = yaw;
00064
00065 return;
00066 }
00067
00068
00069 void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D)
00070 {
00071
00072 tf::Quaternion frame_quat;
00073
00074
00075 frame_quat = tf::createQuaternionFromYaw(pose2D.theta);
00076
00077
00078 pose.position.x = pose2D.x;
00079 pose.position.y = pose2D.y;
00080 pose.position.z = 0.0;
00081
00082
00083 pose.orientation.x = frame_quat.x();
00084 pose.orientation.y = frame_quat.y();
00085 pose.orientation.z = frame_quat.z();
00086 pose.orientation.w = frame_quat.w();
00087
00088 return;
00089 }
00090
00091
00092 bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
00093 const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
00094 std::vector<geometry_msgs::PoseStamped>& transformed_plan, std::vector<int>& start_end_counts)
00095 {
00096 const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
00097
00098
00099 transformed_plan.clear();
00100
00101 try
00102 {
00103 if (!global_plan.size() > 0)
00104 {
00105 ROS_ERROR("Recieved plan with zero length");
00106 return false;
00107 }
00108
00109 tf::StampedTransform transform;
00110 tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,
00111 plan_pose.header.frame_id, transform);
00112
00113
00114 tf::Stamped<tf::Pose> robot_pose;
00115 robot_pose.setIdentity();
00116 robot_pose.frame_id_ = costmap.getBaseFrameID();
00117 robot_pose.stamp_ = ros::Time();
00118 tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);
00119
00120
00121 double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
00122
00123 unsigned int i = 0;
00124 double sq_dist_threshold = dist_threshold * dist_threshold;
00125 double sq_dist = DBL_MAX;
00126
00127
00128
00129 std::vector<int> start_end_count;
00130 start_end_count.assign(2, 0);
00131
00132
00133
00134 ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) );
00135 i = (unsigned int) global_plan.size() - (unsigned int) start_end_counts.at(0);
00136
00137
00138
00139 while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold)
00140 {
00141 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
00142 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
00143 sq_dist = x_diff * x_diff + y_diff * y_diff;
00144
00145
00146
00147 if( sq_dist > sq_dist_threshold )
00148 ++i;
00149 else
00150
00151 start_end_count.at(0) = (int) (((unsigned int) global_plan.size()) - i);
00152
00153 }
00154
00155
00156 tf::Stamped<tf::Pose> tf_pose;
00157 geometry_msgs::PoseStamped newer_pose;
00158
00159
00160 while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold)
00161 {
00162 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
00163 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
00164 sq_dist = x_diff * x_diff + y_diff * y_diff;
00165
00166 const geometry_msgs::PoseStamped& pose = global_plan[i];
00167 poseStampedMsgToTF(pose, tf_pose);
00168 tf_pose.setData(transform * tf_pose);
00169 tf_pose.stamp_ = transform.stamp_;
00170 tf_pose.frame_id_ = global_frame;
00171 poseStampedTFToMsg(tf_pose, newer_pose);
00172
00173 transformed_plan.push_back(newer_pose);
00174
00175
00176
00177 start_end_count.at(1) = int (((unsigned int) global_plan.size()) - i);
00178
00179
00180 ++i;
00181 }
00182
00183
00184
00185 start_end_counts = start_end_count;
00186
00187 }
00188 catch(tf::LookupException& ex)
00189 {
00190 ROS_ERROR("No Transform available Error: %s\n", ex.what());
00191 return false;
00192 }
00193 catch(tf::ConnectivityException& ex)
00194 {
00195 ROS_ERROR("Connectivity Error: %s\n", ex.what());
00196 return false;
00197 }
00198 catch(tf::ExtrapolationException& ex)
00199 {
00200 ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00201 if (global_plan.size() > 0)
00202 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());
00203
00204 return false;
00205 }
00206
00207 return true;
00208 }
00209
00210
00211 }