goal_functions.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
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     //given an empty path we won't do anything
00052     if(path.empty())
00053       return;
00054 
00055     //create a path message
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     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
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       // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
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     transformed_plan.clear();
00096 
00097     if (global_plan.empty()) {
00098       ROS_ERROR("Received plan with zero length");
00099       return false;
00100     }
00101 
00102     const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
00103     try {
00104       // get plan_to_global_transform from plan frame to global_frame
00105       tf::StampedTransform plan_to_global_transform;
00106       tf.waitForTransform(global_frame, ros::Time::now(),
00107                           plan_pose.header.frame_id, plan_pose.header.stamp,
00108                           plan_pose.header.frame_id, ros::Duration(0.5));
00109       tf.lookupTransform(global_frame, ros::Time(),
00110                          plan_pose.header.frame_id, plan_pose.header.stamp, 
00111                          plan_pose.header.frame_id, plan_to_global_transform);
00112 
00113       //let's get the pose of the robot in the frame of the plan
00114       tf::Stamped<tf::Pose> robot_pose;
00115       tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
00116 
00117       //we'll discard points on the plan that are outside the local costmap
00118       double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
00119                                        costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
00120 
00121       unsigned int i = 0;
00122       double sq_dist_threshold = dist_threshold * dist_threshold;
00123       double sq_dist = 0;
00124 
00125       //we need to loop to a point on the plan that is within a certain distance of the robot
00126       while(i < (unsigned int)global_plan.size()) {
00127         double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
00128         double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
00129         sq_dist = x_diff * x_diff + y_diff * y_diff;
00130         if (sq_dist <= sq_dist_threshold) {
00131           break;
00132         }
00133         ++i;
00134       }
00135 
00136       tf::Stamped<tf::Pose> tf_pose;
00137       geometry_msgs::PoseStamped newer_pose;
00138 
00139       //now we'll transform until points are outside of our distance threshold
00140       while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
00141         const geometry_msgs::PoseStamped& pose = global_plan[i];
00142         poseStampedMsgToTF(pose, tf_pose);
00143         tf_pose.setData(plan_to_global_transform * tf_pose);
00144         tf_pose.stamp_ = plan_to_global_transform.stamp_;
00145         tf_pose.frame_id_ = global_frame;
00146         poseStampedTFToMsg(tf_pose, newer_pose);
00147 
00148         transformed_plan.push_back(newer_pose);
00149 
00150         double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
00151         double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
00152         sq_dist = x_diff * x_diff + y_diff * y_diff;
00153 
00154         ++i;
00155       }
00156     }
00157     catch(tf::LookupException& ex) {
00158       ROS_ERROR("No Transform available Error: %s\n", ex.what());
00159       return false;
00160     }
00161     catch(tf::ConnectivityException& ex) {
00162       ROS_ERROR("Connectivity Error: %s\n", ex.what());
00163       return false;
00164     }
00165     catch(tf::ExtrapolationException& ex) {
00166       ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00167       if (!global_plan.empty())
00168         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());
00169 
00170       return false;
00171     }
00172 
00173     return true;
00174   }
00175 
00176   bool getGoalPose(const tf::TransformListener& tf,
00177       const std::vector<geometry_msgs::PoseStamped>& global_plan,
00178       const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose) {
00179     if (global_plan.empty())
00180     {
00181       ROS_ERROR("Received plan with zero length");
00182       return false;
00183     }
00184 
00185     const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
00186     try{
00187       tf::StampedTransform transform;
00188       tf.waitForTransform(global_frame, ros::Time::now(),
00189                           plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
00190                           plan_goal_pose.header.frame_id, ros::Duration(0.5));
00191       tf.lookupTransform(global_frame, ros::Time(),
00192                          plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
00193                          plan_goal_pose.header.frame_id, transform);
00194 
00195       poseStampedMsgToTF(plan_goal_pose, goal_pose);
00196       goal_pose.setData(transform * goal_pose);
00197       goal_pose.stamp_ = transform.stamp_;
00198       goal_pose.frame_id_ = global_frame;
00199 
00200     }
00201     catch(tf::LookupException& ex) {
00202       ROS_ERROR("No Transform available Error: %s\n", ex.what());
00203       return false;
00204     }
00205     catch(tf::ConnectivityException& ex) {
00206       ROS_ERROR("Connectivity Error: %s\n", ex.what());
00207       return false;
00208     }
00209     catch(tf::ExtrapolationException& ex) {
00210       ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00211       if (global_plan.size() > 0)
00212         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());
00213 
00214       return false;
00215     }
00216     return true;
00217   }
00218 
00219   bool isGoalReached(const tf::TransformListener& tf,
00220       const std::vector<geometry_msgs::PoseStamped>& global_plan,
00221       const costmap_2d::Costmap2D& costmap __attribute__((unused)),
00222       const std::string& global_frame,
00223       tf::Stamped<tf::Pose>& global_pose,
00224       const nav_msgs::Odometry& base_odom,
00225       double rot_stopped_vel, double trans_stopped_vel,
00226       double xy_goal_tolerance, double yaw_goal_tolerance){
00227 
00228     //we assume the global goal is the last point in the global plan
00229     tf::Stamped<tf::Pose> goal_pose;
00230     getGoalPose(tf, global_plan, global_frame, goal_pose);
00231 
00232     double goal_x = goal_pose.getOrigin().getX();
00233     double goal_y = goal_pose.getOrigin().getY();
00234     double goal_th = tf::getYaw(goal_pose.getRotation());
00235 
00236     //check to see if we've reached the goal position
00237     if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
00238       //check to see if the goal orientation has been reached
00239       if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
00240         //make sure that we're actually stopped before returning success
00241         if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
00242           return true;
00243       }
00244     }
00245 
00246     return false;
00247   }
00248 
00249   bool stopped(const nav_msgs::Odometry& base_odom, 
00250       const double& rot_stopped_velocity, const double& trans_stopped_velocity){
00251     return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity 
00252       && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
00253       && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
00254   }
00255 };


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:30