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


bipedRobinBase_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko
autogenerated on Wed Oct 9 2013 10:07:14