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     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       // get plan_to_global_transform from plan frame to global_frame
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       //let's get the pose of the robot in the frame of the plan
00115       tf::Stamped<tf::Pose> robot_pose;
00116       tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
00117 
00118       //we'll discard points on the plan that are outside the local costmap
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       //we need to loop to a point on the plan that is within a certain distance of the robot
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       //now we'll transform until points are outside of our distance threshold
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         //we assume the global goal is the last point in the global plan
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     //check to see if we've reached the goal position
00238     if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
00239       //check to see if the goal orientation has been reached
00240       if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
00241         //make sure that we're actually stopped before returning success
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 };


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08