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 #include <base_local_planner/goal_functions.h>
00038
00039 namespace base_local_planner {
00040
00041 double getGoalPositionDistance(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y) {
00042 return hypot(goal_x - global_pose.getOrigin().x(), goal_y - global_pose.getOrigin().y());
00043 }
00044
00045 double getGoalOrientationAngleDifference(const tf::Stamped<tf::Pose>& global_pose, double goal_th) {
00046 double yaw = tf::getYaw(global_pose.getRotation());
00047 return angles::shortest_angular_distance(yaw, goal_th);
00048 }
00049
00050 void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub) {
00051
00052 if(path.empty())
00053 return;
00054
00055
00056 nav_msgs::Path gui_path;
00057 gui_path.poses.resize(path.size());
00058 gui_path.header.frame_id = path[0].header.frame_id;
00059 gui_path.header.stamp = path[0].header.stamp;
00060
00061
00062 for(unsigned int i=0; i < path.size(); i++){
00063 gui_path.poses[i] = path[i];
00064 }
00065
00066 pub.publish(gui_path);
00067 }
00068
00069 void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
00070 ROS_ASSERT(global_plan.size() >= plan.size());
00071 std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
00072 std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
00073 while(it != plan.end()){
00074 const geometry_msgs::PoseStamped& w = *it;
00075
00076 double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
00077 double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
00078 double distance_sq = x_diff * x_diff + y_diff * y_diff;
00079 if(distance_sq < 1){
00080 ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
00081 break;
00082 }
00083 it = plan.erase(it);
00084 global_it = global_plan.erase(global_it);
00085 }
00086 }
00087
00088 bool transformGlobalPlan(
00089 const tf::TransformListener& tf,
00090 const std::vector<geometry_msgs::PoseStamped>& global_plan,
00091 const tf::Stamped<tf::Pose>& global_pose,
00092 const costmap_2d::Costmap2D& costmap,
00093 const std::string& global_frame,
00094 std::vector<geometry_msgs::PoseStamped>& transformed_plan){
00095 const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
00096
00097 transformed_plan.clear();
00098
00099 try {
00100 if (!global_plan.size() > 0) {
00101 ROS_ERROR("Received plan with zero length");
00102 return false;
00103 }
00104
00105
00106 tf::StampedTransform plan_to_global_transform;
00107 tf.waitForTransform(global_frame, ros::Time::now(),
00108 plan_pose.header.frame_id, plan_pose.header.stamp,
00109 plan_pose.header.frame_id, ros::Duration(0.5));
00110 tf.lookupTransform(global_frame, ros::Time(),
00111 plan_pose.header.frame_id, plan_pose.header.stamp,
00112 plan_pose.header.frame_id, plan_to_global_transform);
00113
00114
00115 tf::Stamped<tf::Pose> robot_pose;
00116 tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
00117
00118
00119 double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
00120 costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
00121
00122 unsigned int i = 0;
00123 double sq_dist_threshold = dist_threshold * dist_threshold;
00124 double sq_dist = 0;
00125
00126
00127 while(i < (unsigned int)global_plan.size()) {
00128 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
00129 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
00130 sq_dist = x_diff * x_diff + y_diff * y_diff;
00131 if (sq_dist <= sq_dist_threshold) {
00132 break;
00133 }
00134 ++i;
00135 }
00136
00137 tf::Stamped<tf::Pose> tf_pose;
00138 geometry_msgs::PoseStamped newer_pose;
00139
00140
00141 while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
00142 const geometry_msgs::PoseStamped& pose = global_plan[i];
00143 poseStampedMsgToTF(pose, tf_pose);
00144 tf_pose.setData(plan_to_global_transform * tf_pose);
00145 tf_pose.stamp_ = plan_to_global_transform.stamp_;
00146 tf_pose.frame_id_ = global_frame;
00147 poseStampedTFToMsg(tf_pose, newer_pose);
00148
00149 transformed_plan.push_back(newer_pose);
00150
00151 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
00152 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
00153 sq_dist = x_diff * x_diff + y_diff * y_diff;
00154
00155 ++i;
00156 }
00157 }
00158 catch(tf::LookupException& ex) {
00159 ROS_ERROR("No Transform available Error: %s\n", ex.what());
00160 return false;
00161 }
00162 catch(tf::ConnectivityException& ex) {
00163 ROS_ERROR("Connectivity Error: %s\n", ex.what());
00164 return false;
00165 }
00166 catch(tf::ExtrapolationException& ex) {
00167 ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00168 if (global_plan.size() > 0)
00169 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());
00170
00171 return false;
00172 }
00173
00174 return true;
00175 }
00176
00177 bool getGoalPose(const tf::TransformListener& tf,
00178 const std::vector<geometry_msgs::PoseStamped>& global_plan,
00179 const std::string& global_frame, tf::Stamped<tf::Pose> &goal_pose) {
00180 const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
00181 try{
00182 if (!global_plan.size() > 0)
00183 {
00184 ROS_ERROR("Recieved plan with zero length");
00185 return false;
00186 }
00187
00188 tf::StampedTransform transform;
00189 tf.waitForTransform(global_frame, ros::Time::now(),
00190 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
00191 plan_goal_pose.header.frame_id, ros::Duration(0.5));
00192 tf.lookupTransform(global_frame, ros::Time(),
00193 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
00194 plan_goal_pose.header.frame_id, transform);
00195
00196 poseStampedMsgToTF(plan_goal_pose, goal_pose);
00197 goal_pose.setData(transform * goal_pose);
00198 goal_pose.stamp_ = transform.stamp_;
00199 goal_pose.frame_id_ = global_frame;
00200
00201 }
00202 catch(tf::LookupException& ex) {
00203 ROS_ERROR("No Transform available Error: %s\n", ex.what());
00204 return false;
00205 }
00206 catch(tf::ConnectivityException& ex) {
00207 ROS_ERROR("Connectivity Error: %s\n", ex.what());
00208 return false;
00209 }
00210 catch(tf::ExtrapolationException& ex) {
00211 ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00212 if (global_plan.size() > 0)
00213 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());
00214
00215 return false;
00216 }
00217 return true;
00218 }
00219
00220 bool isGoalReached(const tf::TransformListener& tf,
00221 const std::vector<geometry_msgs::PoseStamped>& global_plan,
00222 const costmap_2d::Costmap2D& costmap,
00223 const std::string& global_frame,
00224 tf::Stamped<tf::Pose>& global_pose,
00225 const nav_msgs::Odometry& base_odom,
00226 double rot_stopped_vel, double trans_stopped_vel,
00227 double xy_goal_tolerance, double yaw_goal_tolerance){
00228
00229
00230 tf::Stamped<tf::Pose> goal_pose;
00231 getGoalPose(tf, global_plan, global_frame, goal_pose);
00232
00233 double goal_x = goal_pose.getOrigin().getX();
00234 double goal_y = goal_pose.getOrigin().getY();
00235 double goal_th = tf::getYaw(goal_pose.getRotation());
00236
00237
00238 if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
00239
00240 if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
00241
00242 if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
00243 return true;
00244 }
00245 }
00246
00247 return false;
00248 }
00249
00250 bool stopped(const nav_msgs::Odometry& base_odom,
00251 const double& rot_stopped_velocity, const double& trans_stopped_velocity){
00252 return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
00253 && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
00254 && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
00255 }
00256 };