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