trajectory_planner.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, 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 the Willow Garage 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 
00038 #include <base_local_planner/trajectory_planner.h>
00039 #include <costmap_2d/footprint.h>
00040 #include <string>
00041 #include <sstream>
00042 #include <math.h>
00043 #include <angles/angles.h>
00044 
00045 
00046 
00047 #include <boost/algorithm/string.hpp>
00048 
00049 #include <ros/console.h>
00050 
00051 //for computing path distance
00052 #include <queue>
00053 
00054 using namespace std;
00055 using namespace costmap_2d;
00056 
00057 namespace base_local_planner{
00058 
00059   void TrajectoryPlanner::reconfigure(BaseLocalPlannerConfig &cfg)
00060   {
00061       BaseLocalPlannerConfig config(cfg);
00062 
00063       boost::mutex::scoped_lock l(configuration_mutex_);
00064 
00065       acc_lim_x_ = config.acc_lim_x;
00066       acc_lim_y_ = config.acc_lim_y;
00067       acc_lim_theta_ = config.acc_lim_theta;
00068 
00069       max_vel_x_ = config.max_vel_x;
00070       min_vel_x_ = config.min_vel_x;
00071       
00072       max_vel_th_ = config.max_vel_theta;
00073       min_vel_th_ = config.min_vel_theta;
00074       min_in_place_vel_th_ = config.min_in_place_vel_theta;
00075 
00076       sim_time_ = config.sim_time;
00077       sim_granularity_ = config.sim_granularity;
00078       angular_sim_granularity_ = config.angular_sim_granularity;
00079 
00080       pdist_scale_ = config.pdist_scale;
00081       gdist_scale_ = config.gdist_scale;
00082       occdist_scale_ = config.occdist_scale;
00083 
00084       if (meter_scoring_) {
00085         //if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
00086         double resolution = costmap_.getResolution();
00087         gdist_scale_ *= resolution;
00088         pdist_scale_ *= resolution;
00089         occdist_scale_ *= resolution;
00090       }
00091 
00092       oscillation_reset_dist_ = config.oscillation_reset_dist;
00093       escape_reset_dist_ = config.escape_reset_dist;
00094       escape_reset_theta_ = config.escape_reset_theta;
00095 
00096       vx_samples_ = config.vx_samples;
00097       vtheta_samples_ = config.vtheta_samples;
00098 
00099       if (vx_samples_ <= 0) {
00100           config.vx_samples = 1;
00101           vx_samples_ = config.vx_samples;
00102           ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
00103       }
00104       if(vtheta_samples_ <= 0) {
00105           config.vtheta_samples = 1;
00106           vtheta_samples_ = config.vtheta_samples;
00107           ROS_WARN("You've specified that you don't want any samples in the theta dimension. We'll at least assume that you want to sample one value... so we're going to set vtheta_samples to 1 instead");
00108       }
00109 
00110       heading_lookahead_ = config.heading_lookahead;
00111 
00112       holonomic_robot_ = config.holonomic_robot;
00113       
00114       backup_vel_ = config.escape_vel;
00115 
00116       dwa_ = config.dwa;
00117 
00118       heading_scoring_ = config.heading_scoring;
00119       heading_scoring_timestep_ = config.heading_scoring_timestep;
00120 
00121       simple_attractor_ = config.simple_attractor;
00122 
00123       //y-vels
00124       string y_string = config.y_vels;
00125       vector<string> y_strs;
00126       boost::split(y_strs, y_string, boost::is_any_of(", "), boost::token_compress_on);
00127 
00128       vector<double>y_vels;
00129       for(vector<string>::iterator it=y_strs.begin(); it != y_strs.end(); ++it) {
00130           istringstream iss(*it);
00131           double temp;
00132           iss >> temp;
00133           y_vels.push_back(temp);
00134           //ROS_INFO("Adding y_vel: %e", temp);
00135       }
00136 
00137       y_vels_ = y_vels;
00138       
00139   }
00140 
00141   TrajectoryPlanner::TrajectoryPlanner(WorldModel& world_model,
00142       const Costmap2D& costmap,
00143       std::vector<geometry_msgs::Point> footprint_spec,
00144       double acc_lim_x, double acc_lim_y, double acc_lim_theta,
00145       double sim_time, double sim_granularity,
00146       int vx_samples, int vtheta_samples,
00147       double pdist_scale, double gdist_scale, double occdist_scale,
00148       double heading_lookahead, double oscillation_reset_dist,
00149       double escape_reset_dist, double escape_reset_theta,
00150       bool holonomic_robot,
00151       double max_vel_x, double min_vel_x,
00152       double max_vel_th, double min_vel_th, double min_in_place_vel_th,
00153       double backup_vel,
00154       bool dwa, bool heading_scoring, double heading_scoring_timestep, bool meter_scoring, bool simple_attractor,
00155       vector<double> y_vels, double stop_time_buffer, double sim_period, double angular_sim_granularity)
00156     : path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
00157       goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
00158       costmap_(costmap),
00159     world_model_(world_model), footprint_spec_(footprint_spec),
00160     sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity),
00161     vx_samples_(vx_samples), vtheta_samples_(vtheta_samples),
00162     pdist_scale_(pdist_scale), gdist_scale_(gdist_scale), occdist_scale_(occdist_scale),
00163     acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta),
00164     prev_x_(0), prev_y_(0), escape_x_(0), escape_y_(0), escape_theta_(0), heading_lookahead_(heading_lookahead),
00165     oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist),
00166     escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot),
00167     max_vel_x_(max_vel_x), min_vel_x_(min_vel_x),
00168     max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th),
00169     backup_vel_(backup_vel),
00170     dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep),
00171     simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer), sim_period_(sim_period)
00172   {
00173     //the robot is not stuck to begin with
00174     stuck_left = false;
00175     stuck_right = false;
00176     stuck_left_strafe = false;
00177     stuck_right_strafe = false;
00178     rotating_left = false;
00179     rotating_right = false;
00180     strafe_left = false;
00181     strafe_right = false;
00182 
00183     escaping_ = false;
00184     final_goal_position_valid_ = false;
00185 
00186 
00187     costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);
00188   }
00189 
00190   TrajectoryPlanner::~TrajectoryPlanner(){}
00191 
00192   bool TrajectoryPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
00193     MapCell cell = path_map_(cx, cy);
00194     MapCell goal_cell = goal_map_(cx, cy);
00195     if (cell.within_robot) {
00196         return false;
00197     }
00198     occ_cost = costmap_.getCost(cx, cy);
00199     if (cell.target_dist == path_map_.obstacleCosts() ||
00200         cell.target_dist == path_map_.unreachableCellCosts() ||
00201         occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
00202         return false;
00203     }
00204     path_cost = cell.target_dist;
00205     goal_cost = goal_cell.target_dist;
00206     total_cost = pdist_scale_ * path_cost + gdist_scale_ * goal_cost + occdist_scale_ * occ_cost;
00207     return true;
00208   }
00209 
00213   void TrajectoryPlanner::generateTrajectory(
00214       double x, double y, double theta,
00215       double vx, double vy, double vtheta,
00216       double vx_samp, double vy_samp, double vtheta_samp,
00217       double acc_x, double acc_y, double acc_theta,
00218       double impossible_cost,
00219       Trajectory& traj) {
00220 
00221     // make sure the configuration doesn't change mid run
00222     boost::mutex::scoped_lock l(configuration_mutex_);
00223 
00224     double x_i = x;
00225     double y_i = y;
00226     double theta_i = theta;
00227 
00228     double vx_i, vy_i, vtheta_i;
00229 
00230     vx_i = vx;
00231     vy_i = vy;
00232     vtheta_i = vtheta;
00233 
00234     //compute the magnitude of the velocities
00235     double vmag = hypot(vx_samp, vy_samp);
00236 
00237     //compute the number of steps we must take along this trajectory to be "safe"
00238     int num_steps;
00239     if(!heading_scoring_) {
00240       num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
00241     } else {
00242       num_steps = int(sim_time_ / sim_granularity_ + 0.5);
00243     }
00244 
00245     //we at least want to take one step... even if we won't move, we want to score our current position
00246     if(num_steps == 0) {
00247       num_steps = 1;
00248     }
00249 
00250     double dt = sim_time_ / num_steps;
00251     double time = 0.0;
00252 
00253     //create a potential trajectory
00254     traj.resetPoints();
00255     traj.xv_ = vx_samp;
00256     traj.yv_ = vy_samp;
00257     traj.thetav_ = vtheta_samp;
00258     traj.cost_ = -1.0;
00259 
00260     //initialize the costs for the trajectory
00261     double path_dist = 0.0;
00262     double goal_dist = 0.0;
00263     double occ_cost = 0.0;
00264     double heading_diff = 0.0;
00265 
00266     for(int i = 0; i < num_steps; ++i){
00267       //get map coordinates of a point
00268       unsigned int cell_x, cell_y;
00269 
00270       //we don't want a path that goes off the know map
00271       if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
00272         traj.cost_ = -1.0;
00273         return;
00274       }
00275 
00276       //check the point on the trajectory for legality
00277       double footprint_cost = footprintCost(x_i, y_i, theta_i);
00278 
00279       //if the footprint hits an obstacle this trajectory is invalid
00280       if(footprint_cost < 0){
00281         traj.cost_ = -1.0;
00282         return;
00283         //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
00284         //it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
00285         //come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative,
00286         //but safe.
00287         /*
00288         double max_vel_x, max_vel_y, max_vel_th;
00289         //we want to compute the max allowable speeds to be able to stop
00290         //to be safe... we'll make sure we can stop some time before we actually hit
00291         getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);
00292 
00293         //check if we can stop in time
00294         if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
00295           ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
00296           //if we can stop... we'll just break out of the loop here.. no point in checking future points
00297           break;
00298         }
00299         else{
00300           traj.cost_ = -1.0;
00301           return;
00302         }
00303         */
00304       }
00305 
00306       occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
00307 
00308       //do we want to follow blindly
00309       if (simple_attractor_) {
00310         goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
00311           (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
00312           (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
00313           (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
00314       } else {
00315 
00316         bool update_path_and_goal_distances = true;
00317 
00318         // with heading scoring, we take into account heading diff, and also only score
00319         // path and goal distance for one point of the trajectory
00320         if (heading_scoring_) {
00321           if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {
00322             heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
00323           } else {
00324             update_path_and_goal_distances = false;
00325           }
00326         }
00327 
00328         if (update_path_and_goal_distances) {
00329           //update path and goal distances
00330           path_dist = path_map_(cell_x, cell_y).target_dist;
00331           goal_dist = goal_map_(cell_x, cell_y).target_dist;
00332 
00333           //if a point on this trajectory has no clear path to goal it is invalid
00334           if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
00335 //            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
00336 //                goal_dist, path_dist, impossible_cost);
00337             traj.cost_ = -2.0;
00338             return;
00339           }
00340         }
00341       }
00342 
00343 
00344       //the point is legal... add it to the trajectory
00345       traj.addPoint(x_i, y_i, theta_i);
00346 
00347       //calculate velocities
00348       vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
00349       vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
00350       vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
00351 
00352       //calculate positions
00353       x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
00354       y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
00355       theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
00356 
00357       //increment time
00358       time += dt;
00359     } // end for i < numsteps
00360 
00361     //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
00362     double cost = -1.0;
00363     if (!heading_scoring_) {
00364       cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
00365     } else {
00366       cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
00367     }
00368     traj.cost_ = cost;
00369   }
00370 
00371   double TrajectoryPlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading){
00372     double heading_diff = DBL_MAX;
00373     unsigned int goal_cell_x, goal_cell_y;
00374     const double v2_x = cos(heading);
00375     const double v2_y = sin(heading);
00376 
00377     //find a clear line of sight from the robot's cell to a point on the path
00378     for (int i = global_plan_.size() - 1; i >=0; --i) {
00379       if (costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)) {
00380         if (lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0) {
00381           double gx, gy;
00382           costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
00383           double v1_x = gx - x;
00384           double v1_y = gy - y;
00385 
00386           double perp_dot = v1_x * v2_y - v1_y * v2_x;
00387           double dot = v1_x * v2_x + v1_y * v2_y;
00388 
00389           //get the signed angle
00390           double vector_angle = atan2(perp_dot, dot);
00391 
00392           heading_diff = fabs(vector_angle);
00393           return heading_diff;
00394         }
00395       }
00396     }
00397     return heading_diff;
00398   }
00399 
00400   //calculate the cost of a ray-traced line
00401   double TrajectoryPlanner::lineCost(int x0, int x1,
00402       int y0, int y1){
00403     //Bresenham Ray-Tracing
00404     int deltax = abs(x1 - x0);        // The difference between the x's
00405     int deltay = abs(y1 - y0);        // The difference between the y's
00406     int x = x0;                       // Start x off at the first pixel
00407     int y = y0;                       // Start y off at the first pixel
00408 
00409     int xinc1, xinc2, yinc1, yinc2;
00410     int den, num, numadd, numpixels;
00411 
00412     double line_cost = 0.0;
00413     double point_cost = -1.0;
00414 
00415     if (x1 >= x0)                 // The x-values are increasing
00416     {
00417       xinc1 = 1;
00418       xinc2 = 1;
00419     }
00420     else                          // The x-values are decreasing
00421     {
00422       xinc1 = -1;
00423       xinc2 = -1;
00424     }
00425 
00426     if (y1 >= y0)                 // The y-values are increasing
00427     {
00428       yinc1 = 1;
00429       yinc2 = 1;
00430     }
00431     else                          // The y-values are decreasing
00432     {
00433       yinc1 = -1;
00434       yinc2 = -1;
00435     }
00436 
00437     if (deltax >= deltay)         // There is at least one x-value for every y-value
00438     {
00439       xinc1 = 0;                  // Don't change the x when numerator >= denominator
00440       yinc2 = 0;                  // Don't change the y for every iteration
00441       den = deltax;
00442       num = deltax / 2;
00443       numadd = deltay;
00444       numpixels = deltax;         // There are more x-values than y-values
00445     } else {                      // There is at least one y-value for every x-value
00446       xinc2 = 0;                  // Don't change the x for every iteration
00447       yinc1 = 0;                  // Don't change the y when numerator >= denominator
00448       den = deltay;
00449       num = deltay / 2;
00450       numadd = deltax;
00451       numpixels = deltay;         // There are more y-values than x-values
00452     }
00453 
00454     for (int curpixel = 0; curpixel <= numpixels; curpixel++) {
00455       point_cost = pointCost(x, y); //Score the current point
00456 
00457       if (point_cost < 0) {
00458         return -1;
00459       }
00460 
00461       if (line_cost < point_cost) {
00462         line_cost = point_cost;
00463       }
00464 
00465       num += numadd;              // Increase the numerator by the top of the fraction
00466       if (num >= den) {           // Check if numerator >= denominator
00467         num -= den;               // Calculate the new numerator value
00468         x += xinc1;               // Change the x as appropriate
00469         y += yinc1;               // Change the y as appropriate
00470       }
00471       x += xinc2;                 // Change the x as appropriate
00472       y += yinc2;                 // Change the y as appropriate
00473     }
00474 
00475     return line_cost;
00476   }
00477 
00478   double TrajectoryPlanner::pointCost(int x, int y){
00479     unsigned char cost = costmap_.getCost(x, y);
00480     //if the cell is in an obstacle the path is invalid
00481     if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){
00482       return -1;
00483     }
00484 
00485     return cost;
00486   }
00487 
00488   void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists){
00489     global_plan_.resize(new_plan.size());
00490     for(unsigned int i = 0; i < new_plan.size(); ++i){
00491       global_plan_[i] = new_plan[i];
00492     }
00493 
00494     if( global_plan_.size() > 0 ){
00495       geometry_msgs::PoseStamped& final_goal_pose = global_plan_[ global_plan_.size() - 1 ];
00496       final_goal_x_ = final_goal_pose.pose.position.x;
00497       final_goal_y_ = final_goal_pose.pose.position.y;
00498       final_goal_position_valid_ = true;
00499     } else {
00500       final_goal_position_valid_ = false;
00501     }
00502 
00503     if (compute_dists) {
00504       //reset the map for new operations
00505       path_map_.resetPathDist();
00506       goal_map_.resetPathDist();
00507 
00508       //make sure that we update our path based on the global plan and compute costs
00509       path_map_.setTargetCells(costmap_, global_plan_);
00510       goal_map_.setLocalGoal(costmap_, global_plan_);
00511       ROS_DEBUG("Path/Goal distance computed");
00512     }
00513   }
00514 
00515   bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
00516       double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
00517     Trajectory t;
00518 
00519     double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);
00520 
00521     //if the trajectory is a legal one... the check passes
00522     if(cost >= 0) {
00523       return true;
00524     }
00525     ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);
00526 
00527     //otherwise the check fails
00528     return false;
00529   }
00530 
00531   double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
00532       double vtheta, double vx_samp, double vy_samp, double vtheta_samp) {
00533     Trajectory t;
00534     double impossible_cost = path_map_.obstacleCosts();
00535     generateTrajectory(x, y, theta,
00536                        vx, vy, vtheta,
00537                        vx_samp, vy_samp, vtheta_samp,
00538                        acc_lim_x_, acc_lim_y_, acc_lim_theta_,
00539                        impossible_cost, t);
00540 
00541     // return the cost.
00542     return double( t.cost_ );
00543   }
00544 
00545   /*
00546    * create the trajectories we wish to score
00547    */
00548   Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
00549       double vx, double vy, double vtheta,
00550       double acc_x, double acc_y, double acc_theta) {
00551     //compute feasible velocity limits in robot space
00552     double max_vel_x = max_vel_x_, max_vel_theta;
00553     double min_vel_x, min_vel_theta;
00554 
00555     if( final_goal_position_valid_ ){
00556       double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
00557       max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
00558     }
00559 
00560     //should we use the dynamic window approach?
00561     if (dwa_) {
00562       max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
00563       min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);
00564 
00565       max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
00566       min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
00567     } else {
00568       max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
00569       min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);
00570 
00571       max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
00572       min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
00573     }
00574 
00575 
00576     //we want to sample the velocity space regularly
00577     double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);
00578     double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);
00579 
00580     double vx_samp = min_vel_x;
00581     double vtheta_samp = min_vel_theta;
00582     double vy_samp = 0.0;
00583 
00584     //keep track of the best trajectory seen so far
00585     Trajectory* best_traj = &traj_one;
00586     best_traj->cost_ = -1.0;
00587 
00588     Trajectory* comp_traj = &traj_two;
00589     comp_traj->cost_ = -1.0;
00590 
00591     Trajectory* swap = NULL;
00592 
00593     //any cell with a cost greater than the size of the map is impossible
00594     double impossible_cost = path_map_.obstacleCosts();
00595 
00596     //if we're performing an escape we won't allow moving forward
00597     if (!escaping_) {
00598       //loop through all x velocities
00599       for(int i = 0; i < vx_samples_; ++i) {
00600         vtheta_samp = 0;
00601         //first sample the straight trajectory
00602         generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
00603             acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00604 
00605         //if the new trajectory is better... let's take it
00606         if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
00607           swap = best_traj;
00608           best_traj = comp_traj;
00609           comp_traj = swap;
00610         }
00611 
00612         vtheta_samp = min_vel_theta;
00613         //next sample all theta trajectories
00614         for(int j = 0; j < vtheta_samples_ - 1; ++j){
00615           generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
00616               acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00617 
00618           //if the new trajectory is better... let's take it
00619           if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
00620             swap = best_traj;
00621             best_traj = comp_traj;
00622             comp_traj = swap;
00623           }
00624           vtheta_samp += dvtheta;
00625         }
00626         vx_samp += dvx;
00627       }
00628 
00629       //only explore y velocities with holonomic robots
00630       if (holonomic_robot_) {
00631         //explore trajectories that move forward but also strafe slightly
00632         vx_samp = 0.1;
00633         vy_samp = 0.1;
00634         vtheta_samp = 0.0;
00635         generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
00636             acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00637 
00638         //if the new trajectory is better... let's take it
00639         if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
00640           swap = best_traj;
00641           best_traj = comp_traj;
00642           comp_traj = swap;
00643         }
00644 
00645         vx_samp = 0.1;
00646         vy_samp = -0.1;
00647         vtheta_samp = 0.0;
00648         generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
00649             acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00650 
00651         //if the new trajectory is better... let's take it
00652         if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
00653           swap = best_traj;
00654           best_traj = comp_traj;
00655           comp_traj = swap;
00656         }
00657       }
00658     } // end if not escaping
00659 
00660     //next we want to generate trajectories for rotating in place
00661     vtheta_samp = min_vel_theta;
00662     vx_samp = 0.0;
00663     vy_samp = 0.0;
00664 
00665     //let's try to rotate toward open space
00666     double heading_dist = DBL_MAX;
00667 
00668     for(int i = 0; i < vtheta_samples_; ++i) {
00669       //enforce a minimum rotational velocity because the base can't handle small in-place rotations
00670       double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
00671         : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
00672 
00673       generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
00674           acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00675 
00676       //if the new trajectory is better... let's take it...
00677       //note if we can legally rotate in place we prefer to do that rather than move with y velocity
00678       if(comp_traj->cost_ >= 0
00679           && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
00680           && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
00681         double x_r, y_r, th_r;
00682         comp_traj->getEndpoint(x_r, y_r, th_r);
00683         x_r += heading_lookahead_ * cos(th_r);
00684         y_r += heading_lookahead_ * sin(th_r);
00685         unsigned int cell_x, cell_y;
00686 
00687         //make sure that we'll be looking at a legal cell
00688         if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
00689           double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
00690           if (ahead_gdist < heading_dist) {
00691             //if we haven't already tried rotating left since we've moved forward
00692             if (vtheta_samp < 0 && !stuck_left) {
00693               swap = best_traj;
00694               best_traj = comp_traj;
00695               comp_traj = swap;
00696               heading_dist = ahead_gdist;
00697             }
00698             //if we haven't already tried rotating right since we've moved forward
00699             else if(vtheta_samp > 0 && !stuck_right) {
00700               swap = best_traj;
00701               best_traj = comp_traj;
00702               comp_traj = swap;
00703               heading_dist = ahead_gdist;
00704             }
00705           }
00706         }
00707       }
00708 
00709       vtheta_samp += dvtheta;
00710     }
00711 
00712     //do we have a legal trajectory
00713     if (best_traj->cost_ >= 0) {
00714       // avoid oscillations of in place rotation and in place strafing
00715       if ( ! (best_traj->xv_ > 0)) {
00716         if (best_traj->thetav_ < 0) {
00717           if (rotating_right) {
00718             stuck_right = true;
00719           }
00720           rotating_right = true;
00721         } else if (best_traj->thetav_ > 0) {
00722           if (rotating_left){
00723             stuck_left = true;
00724           }
00725           rotating_left = true;
00726         } else if(best_traj->yv_ > 0) {
00727           if (strafe_right) {
00728             stuck_right_strafe = true;
00729           }
00730           strafe_right = true;
00731         } else if(best_traj->yv_ < 0){
00732           if (strafe_left) {
00733             stuck_left_strafe = true;
00734           }
00735           strafe_left = true;
00736         }
00737 
00738         //set the position we must move a certain distance away from
00739         prev_x_ = x;
00740         prev_y_ = y;
00741       }
00742 
00743       double dist = hypot(x - prev_x_, y - prev_y_);
00744       if (dist > oscillation_reset_dist_) {
00745         rotating_left = false;
00746         rotating_right = false;
00747         strafe_left = false;
00748         strafe_right = false;
00749         stuck_left = false;
00750         stuck_right = false;
00751         stuck_left_strafe = false;
00752         stuck_right_strafe = false;
00753       }
00754 
00755       dist = hypot(x - escape_x_, y - escape_y_);
00756       if(dist > escape_reset_dist_ ||
00757           fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){
00758         escaping_ = false;
00759       }
00760 
00761       return *best_traj;
00762     }
00763 
00764     //only explore y velocities with holonomic robots
00765     if (holonomic_robot_) {
00766       //if we can't rotate in place or move forward... maybe we can move sideways and rotate
00767       vtheta_samp = min_vel_theta;
00768       vx_samp = 0.0;
00769 
00770       //loop through all y velocities
00771       for(unsigned int i = 0; i < y_vels_.size(); ++i){
00772         vtheta_samp = 0;
00773         vy_samp = y_vels_[i];
00774         //sample completely horizontal trajectories
00775         generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
00776             acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00777 
00778         //if the new trajectory is better... let's take it
00779         if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
00780           double x_r, y_r, th_r;
00781           comp_traj->getEndpoint(x_r, y_r, th_r);
00782           x_r += heading_lookahead_ * cos(th_r);
00783           y_r += heading_lookahead_ * sin(th_r);
00784           unsigned int cell_x, cell_y;
00785 
00786           //make sure that we'll be looking at a legal cell
00787           if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
00788             double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
00789             if (ahead_gdist < heading_dist) {
00790               //if we haven't already tried strafing left since we've moved forward
00791               if (vy_samp > 0 && !stuck_left_strafe) {
00792                 swap = best_traj;
00793                 best_traj = comp_traj;
00794                 comp_traj = swap;
00795                 heading_dist = ahead_gdist;
00796               }
00797               //if we haven't already tried rotating right since we've moved forward
00798               else if(vy_samp < 0 && !stuck_right_strafe) {
00799                 swap = best_traj;
00800                 best_traj = comp_traj;
00801                 comp_traj = swap;
00802                 heading_dist = ahead_gdist;
00803               }
00804             }
00805           }
00806         }
00807       }
00808     }
00809 
00810     //do we have a legal trajectory
00811     if (best_traj->cost_ >= 0) {
00812       if (!(best_traj->xv_ > 0)) {
00813         if (best_traj->thetav_ < 0) {
00814           if (rotating_right){
00815             stuck_right = true;
00816           }
00817           rotating_left = true;
00818         } else if(best_traj->thetav_ > 0) {
00819           if(rotating_left){
00820             stuck_left = true;
00821           }
00822           rotating_right = true;
00823         } else if(best_traj->yv_ > 0) {
00824           if(strafe_right){
00825             stuck_right_strafe = true;
00826           }
00827           strafe_left = true;
00828         } else if(best_traj->yv_ < 0) {
00829           if(strafe_left){
00830             stuck_left_strafe = true;
00831           }
00832           strafe_right = true;
00833         }
00834 
00835         //set the position we must move a certain distance away from
00836         prev_x_ = x;
00837         prev_y_ = y;
00838 
00839       }
00840 
00841       double dist = hypot(x - prev_x_, y - prev_y_);
00842       if(dist > oscillation_reset_dist_) {
00843         rotating_left = false;
00844         rotating_right = false;
00845         strafe_left = false;
00846         strafe_right = false;
00847         stuck_left = false;
00848         stuck_right = false;
00849         stuck_left_strafe = false;
00850         stuck_right_strafe = false;
00851       }
00852 
00853       dist = hypot(x - escape_x_, y - escape_y_);
00854       if(dist > escape_reset_dist_ || fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
00855         escaping_ = false;
00856       }
00857 
00858       return *best_traj;
00859     }
00860 
00861     //and finally, if we can't do anything else, we want to generate trajectories that move backwards slowly
00862     vtheta_samp = 0.0;
00863     vx_samp = backup_vel_;
00864     vy_samp = 0.0;
00865     generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
00866         acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
00867 
00868     //if the new trajectory is better... let's take it
00869     /*
00870        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
00871        swap = best_traj;
00872        best_traj = comp_traj;
00873        comp_traj = swap;
00874        }
00875        */
00876 
00877     //we'll allow moving backwards slowly even when the static map shows it as blocked
00878     swap = best_traj;
00879     best_traj = comp_traj;
00880     comp_traj = swap;
00881 
00882     double dist = hypot(x - prev_x_, y - prev_y_);
00883     if (dist > oscillation_reset_dist_) {
00884       rotating_left = false;
00885       rotating_right = false;
00886       strafe_left = false;
00887       strafe_right = false;
00888       stuck_left = false;
00889       stuck_right = false;
00890       stuck_left_strafe = false;
00891       stuck_right_strafe = false;
00892     }
00893 
00894     //only enter escape mode when the planner has given a valid goal point
00895     if (!escaping_ && best_traj->cost_ > -2.0) {
00896       escape_x_ = x;
00897       escape_y_ = y;
00898       escape_theta_ = theta;
00899       escaping_ = true;
00900     }
00901 
00902     dist = hypot(x - escape_x_, y - escape_y_);
00903 
00904     if (dist > escape_reset_dist_ ||
00905         fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
00906       escaping_ = false;
00907     }
00908 
00909 
00910     //if the trajectory failed because the footprint hits something, we're still going to back up
00911     if(best_traj->cost_ == -1.0)
00912       best_traj->cost_ = 1.0;
00913 
00914     return *best_traj;
00915 
00916   }
00917 
00918   //given the current state of the robot, find a good trajectory
00919   Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
00920       tf::Stamped<tf::Pose>& drive_velocities){
00921 
00922     Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
00923     Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
00924 
00925     //reset the map for new operations
00926     path_map_.resetPathDist();
00927     goal_map_.resetPathDist();
00928 
00929     //temporarily remove obstacles that are within the footprint of the robot
00930     std::vector<base_local_planner::Position2DInt> footprint_list =
00931         footprint_helper_.getFootprintCells(
00932             pos,
00933             footprint_spec_,
00934             costmap_,
00935             true);
00936 
00937     //mark cells within the initial footprint of the robot
00938     for (unsigned int i = 0; i < footprint_list.size(); ++i) {
00939       path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
00940     }
00941 
00942     //make sure that we update our path based on the global plan and compute costs
00943     path_map_.setTargetCells(costmap_, global_plan_);
00944     goal_map_.setLocalGoal(costmap_, global_plan_);
00945     ROS_DEBUG("Path/Goal distance computed");
00946 
00947     //rollout trajectories and find the minimum cost one
00948     Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
00949         vel[0], vel[1], vel[2],
00950         acc_lim_x_, acc_lim_y_, acc_lim_theta_);
00951     ROS_DEBUG("Trajectories created");
00952 
00953     /*
00954     //If we want to print a ppm file to draw goal dist
00955     char buf[4096];
00956     sprintf(buf, "base_local_planner.ppm");
00957     FILE *fp = fopen(buf, "w");
00958     if(fp){
00959       fprintf(fp, "P3\n");
00960       fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
00961       fprintf(fp, "255\n");
00962       for(int j = map_.size_y_ - 1; j >= 0; --j){
00963         for(unsigned int i = 0; i < map_.size_x_; ++i){
00964           int g_dist = 255 - int(map_(i, j).goal_dist);
00965           int p_dist = 255 - int(map_(i, j).path_dist);
00966           if(g_dist < 0)
00967             g_dist = 0;
00968           if(p_dist < 0)
00969             p_dist = 0;
00970           fprintf(fp, "%d 0 %d ", g_dist, 0);
00971         }
00972         fprintf(fp, "\n");
00973       }
00974       fclose(fp);
00975     }
00976     */
00977 
00978     if(best.cost_ < 0){
00979       drive_velocities.setIdentity();
00980     }
00981     else{
00982       tf::Vector3 start(best.xv_, best.yv_, 0);
00983       drive_velocities.setOrigin(start);
00984       tf::Matrix3x3 matrix;
00985       matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
00986       drive_velocities.setBasis(matrix);
00987     }
00988 
00989     return best;
00990   }
00991 
00992   //we need to take the footprint of the robot into account when we calculate cost to obstacles
00993   double TrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i){
00994     //check if the footprint is legal
00995     return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_);
00996   }
00997 
00998 
00999   void TrajectoryPlanner::getLocalGoal(double& x, double& y){
01000     x = path_map_.goal_x_;
01001     y = path_map_.goal_y_;
01002   }
01003 
01004 };
01005 
01006 


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