trajectory_planner.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 
39 #include <costmap_2d/footprint.h>
40 #include <string>
41 #include <sstream>
42 #include <math.h>
43 #include <angles/angles.h>
44 
45 
46 
47 #include <boost/algorithm/string.hpp>
48 
49 #include <ros/console.h>
50 
51 //for computing path distance
52 #include <queue>
53 
54 using namespace std;
55 using namespace costmap_2d;
56 
57 namespace base_local_planner{
58 
59  void TrajectoryPlanner::reconfigure(BaseLocalPlannerConfig &cfg)
60  {
61  BaseLocalPlannerConfig config(cfg);
62 
63  boost::mutex::scoped_lock l(configuration_mutex_);
64 
65  acc_lim_x_ = config.acc_lim_x;
66  acc_lim_y_ = config.acc_lim_y;
67  acc_lim_theta_ = config.acc_lim_theta;
68 
69  max_vel_x_ = config.max_vel_x;
70  min_vel_x_ = config.min_vel_x;
71 
72  max_vel_th_ = config.max_vel_theta;
73  min_vel_th_ = config.min_vel_theta;
74  min_in_place_vel_th_ = config.min_in_place_vel_theta;
75 
76  sim_time_ = config.sim_time;
77  sim_granularity_ = config.sim_granularity;
78  angular_sim_granularity_ = config.angular_sim_granularity;
79 
80  pdist_scale_ = config.pdist_scale;
81  gdist_scale_ = config.gdist_scale;
82  occdist_scale_ = config.occdist_scale;
83 
84  if (meter_scoring_) {
85  //if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
86  double resolution = costmap_.getResolution();
87  gdist_scale_ *= resolution;
88  pdist_scale_ *= resolution;
89  occdist_scale_ *= resolution;
90  }
91 
92  oscillation_reset_dist_ = config.oscillation_reset_dist;
93  escape_reset_dist_ = config.escape_reset_dist;
94  escape_reset_theta_ = config.escape_reset_theta;
95 
96  vx_samples_ = config.vx_samples;
97  vtheta_samples_ = config.vtheta_samples;
98 
99  if (vx_samples_ <= 0) {
100  config.vx_samples = 1;
101  vx_samples_ = config.vx_samples;
102  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");
103  }
104  if(vtheta_samples_ <= 0) {
105  config.vtheta_samples = 1;
106  vtheta_samples_ = config.vtheta_samples;
107  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");
108  }
109 
110  heading_lookahead_ = config.heading_lookahead;
111 
112  holonomic_robot_ = config.holonomic_robot;
113 
114  backup_vel_ = config.escape_vel;
115 
116  dwa_ = config.dwa;
117 
118  heading_scoring_ = config.heading_scoring;
119  heading_scoring_timestep_ = config.heading_scoring_timestep;
120 
121  simple_attractor_ = config.simple_attractor;
122 
123  //y-vels
124  string y_string = config.y_vels;
125  vector<string> y_strs;
126  boost::split(y_strs, y_string, boost::is_any_of(", "), boost::token_compress_on);
127 
128  vector<double>y_vels;
129  for(vector<string>::iterator it=y_strs.begin(); it != y_strs.end(); ++it) {
130  istringstream iss(*it);
131  double temp;
132  iss >> temp;
133  y_vels.push_back(temp);
134  //ROS_INFO("Adding y_vel: %e", temp);
135  }
136 
137  y_vels_ = y_vels;
138 
139  }
140 
141  TrajectoryPlanner::TrajectoryPlanner(WorldModel& world_model,
142  const Costmap2D& costmap,
143  std::vector<geometry_msgs::Point> footprint_spec,
144  double acc_lim_x, double acc_lim_y, double acc_lim_theta,
145  double sim_time, double sim_granularity,
146  int vx_samples, int vtheta_samples,
147  double pdist_scale, double gdist_scale, double occdist_scale,
148  double heading_lookahead, double oscillation_reset_dist,
149  double escape_reset_dist, double escape_reset_theta,
150  bool holonomic_robot,
151  double max_vel_x, double min_vel_x,
152  double max_vel_th, double min_vel_th, double min_in_place_vel_th,
153  double backup_vel,
154  bool dwa, bool heading_scoring, double heading_scoring_timestep, bool meter_scoring, bool simple_attractor,
155  vector<double> y_vels, double stop_time_buffer, double sim_period, double angular_sim_granularity)
156  : path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
157  goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
158  costmap_(costmap),
159  world_model_(world_model), footprint_spec_(footprint_spec),
160  sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity),
161  vx_samples_(vx_samples), vtheta_samples_(vtheta_samples),
162  pdist_scale_(pdist_scale), gdist_scale_(gdist_scale), occdist_scale_(occdist_scale),
163  acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta),
164  prev_x_(0), prev_y_(0), escape_x_(0), escape_y_(0), escape_theta_(0), heading_lookahead_(heading_lookahead),
165  oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist),
166  escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot),
167  max_vel_x_(max_vel_x), min_vel_x_(min_vel_x),
168  max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th),
169  backup_vel_(backup_vel),
170  dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep),
171  simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer), sim_period_(sim_period)
172  {
173  //the robot is not stuck to begin with
174  stuck_left = false;
175  stuck_right = false;
176  stuck_left_strafe = false;
177  stuck_right_strafe = false;
178  rotating_left = false;
179  rotating_right = false;
180  strafe_left = false;
181  strafe_right = false;
182 
183  escaping_ = false;
185 
186 
188  }
189 
191 
192  bool TrajectoryPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
193  MapCell cell = path_map_(cx, cy);
194  MapCell goal_cell = goal_map_(cx, cy);
195  if (cell.within_robot) {
196  return false;
197  }
198  occ_cost = costmap_.getCost(cx, cy);
199  if (cell.target_dist == path_map_.obstacleCosts() ||
202  return false;
203  }
204  path_cost = cell.target_dist;
205  goal_cost = goal_cell.target_dist;
206  total_cost = pdist_scale_ * path_cost + gdist_scale_ * goal_cost + occdist_scale_ * occ_cost;
207  return true;
208  }
209 
214  double x, double y, double theta,
215  double vx, double vy, double vtheta,
216  double vx_samp, double vy_samp, double vtheta_samp,
217  double acc_x, double acc_y, double acc_theta,
218  double impossible_cost,
219  Trajectory& traj) {
220 
221  // make sure the configuration doesn't change mid run
222  boost::mutex::scoped_lock l(configuration_mutex_);
223 
224  double x_i = x;
225  double y_i = y;
226  double theta_i = theta;
227 
228  double vx_i, vy_i, vtheta_i;
229 
230  vx_i = vx;
231  vy_i = vy;
232  vtheta_i = vtheta;
233 
234  //compute the magnitude of the velocities
235  double vmag = hypot(vx_samp, vy_samp);
236 
237  //compute the number of steps we must take along this trajectory to be "safe"
238  int num_steps;
239  if(!heading_scoring_) {
240  num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
241  } else {
242  num_steps = int(sim_time_ / sim_granularity_ + 0.5);
243  }
244 
245  //we at least want to take one step... even if we won't move, we want to score our current position
246  if(num_steps == 0) {
247  num_steps = 1;
248  }
249 
250  double dt = sim_time_ / num_steps;
251  double time = 0.0;
252 
253  //create a potential trajectory
254  traj.resetPoints();
255  traj.xv_ = vx_samp;
256  traj.yv_ = vy_samp;
257  traj.thetav_ = vtheta_samp;
258  traj.cost_ = -1.0;
259 
260  //initialize the costs for the trajectory
261  double path_dist = 0.0;
262  double goal_dist = 0.0;
263  double occ_cost = 0.0;
264  double heading_diff = 0.0;
265 
266  for(int i = 0; i < num_steps; ++i){
267  //get map coordinates of a point
268  unsigned int cell_x, cell_y;
269 
270  //we don't want a path that goes off the know map
271  if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
272  traj.cost_ = -1.0;
273  return;
274  }
275 
276  //check the point on the trajectory for legality
277  double footprint_cost = footprintCost(x_i, y_i, theta_i);
278 
279  //if the footprint hits an obstacle this trajectory is invalid
280  if(footprint_cost < 0){
281  traj.cost_ = -1.0;
282  return;
283  //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
284  //it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
285  //come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative,
286  //but safe.
287  /*
288  double max_vel_x, max_vel_y, max_vel_th;
289  //we want to compute the max allowable speeds to be able to stop
290  //to be safe... we'll make sure we can stop some time before we actually hit
291  getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);
292 
293  //check if we can stop in time
294  if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
295  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);
296  //if we can stop... we'll just break out of the loop here.. no point in checking future points
297  break;
298  }
299  else{
300  traj.cost_ = -1.0;
301  return;
302  }
303  */
304  }
305 
306  occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
307 
308  //do we want to follow blindly
309  if (simple_attractor_) {
310  goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
311  (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
312  (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
313  (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
314  } else {
315 
316  bool update_path_and_goal_distances = true;
317 
318  // with heading scoring, we take into account heading diff, and also only score
319  // path and goal distance for one point of the trajectory
320  if (heading_scoring_) {
321  if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {
322  heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
323  } else {
324  update_path_and_goal_distances = false;
325  }
326  }
327 
328  if (update_path_and_goal_distances) {
329  //update path and goal distances
330  path_dist = path_map_(cell_x, cell_y).target_dist;
331  goal_dist = goal_map_(cell_x, cell_y).target_dist;
332 
333  //if a point on this trajectory has no clear path to goal it is invalid
334  if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
335 // ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
336 // goal_dist, path_dist, impossible_cost);
337  traj.cost_ = -2.0;
338  return;
339  }
340  }
341  }
342 
343 
344  //the point is legal... add it to the trajectory
345  traj.addPoint(x_i, y_i, theta_i);
346 
347  //calculate velocities
348  vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
349  vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
350  vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
351 
352  //calculate positions
353  x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
354  y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
355  theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
356 
357  //increment time
358  time += dt;
359  } // end for i < numsteps
360 
361  //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
362  double cost = -1.0;
363  if (!heading_scoring_) {
364  cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
365  } else {
366  cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
367  }
368  traj.cost_ = cost;
369  }
370 
371  double TrajectoryPlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading){
372  unsigned int goal_cell_x, goal_cell_y;
373 
374  // find a clear line of sight from the robot's cell to a farthest point on the path
375  for (int i = global_plan_.size() - 1; i >=0; --i) {
376  if (costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)) {
377  if (lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0) {
378  double gx, gy;
379  costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
380  return fabs(angles::shortest_angular_distance(heading, atan2(gy - y, gx - x)));
381  }
382  }
383  }
384  return DBL_MAX;
385  }
386 
387  //calculate the cost of a ray-traced line
388  double TrajectoryPlanner::lineCost(int x0, int x1,
389  int y0, int y1){
390  //Bresenham Ray-Tracing
391  int deltax = abs(x1 - x0); // The difference between the x's
392  int deltay = abs(y1 - y0); // The difference between the y's
393  int x = x0; // Start x off at the first pixel
394  int y = y0; // Start y off at the first pixel
395 
396  int xinc1, xinc2, yinc1, yinc2;
397  int den, num, numadd, numpixels;
398 
399  double line_cost = 0.0;
400  double point_cost = -1.0;
401 
402  if (x1 >= x0) // The x-values are increasing
403  {
404  xinc1 = 1;
405  xinc2 = 1;
406  }
407  else // The x-values are decreasing
408  {
409  xinc1 = -1;
410  xinc2 = -1;
411  }
412 
413  if (y1 >= y0) // The y-values are increasing
414  {
415  yinc1 = 1;
416  yinc2 = 1;
417  }
418  else // The y-values are decreasing
419  {
420  yinc1 = -1;
421  yinc2 = -1;
422  }
423 
424  if (deltax >= deltay) // There is at least one x-value for every y-value
425  {
426  xinc1 = 0; // Don't change the x when numerator >= denominator
427  yinc2 = 0; // Don't change the y for every iteration
428  den = deltax;
429  num = deltax / 2;
430  numadd = deltay;
431  numpixels = deltax; // There are more x-values than y-values
432  } else { // There is at least one y-value for every x-value
433  xinc2 = 0; // Don't change the x for every iteration
434  yinc1 = 0; // Don't change the y when numerator >= denominator
435  den = deltay;
436  num = deltay / 2;
437  numadd = deltax;
438  numpixels = deltay; // There are more y-values than x-values
439  }
440 
441  for (int curpixel = 0; curpixel <= numpixels; curpixel++) {
442  point_cost = pointCost(x, y); //Score the current point
443 
444  if (point_cost < 0) {
445  return -1;
446  }
447 
448  if (line_cost < point_cost) {
449  line_cost = point_cost;
450  }
451 
452  num += numadd; // Increase the numerator by the top of the fraction
453  if (num >= den) { // Check if numerator >= denominator
454  num -= den; // Calculate the new numerator value
455  x += xinc1; // Change the x as appropriate
456  y += yinc1; // Change the y as appropriate
457  }
458  x += xinc2; // Change the x as appropriate
459  y += yinc2; // Change the y as appropriate
460  }
461 
462  return line_cost;
463  }
464 
465  double TrajectoryPlanner::pointCost(int x, int y){
466  unsigned char cost = costmap_.getCost(x, y);
467  //if the cell is in an obstacle the path is invalid
468  if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){
469  return -1;
470  }
471 
472  return cost;
473  }
474 
475  void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists){
476  global_plan_.resize(new_plan.size());
477  for(unsigned int i = 0; i < new_plan.size(); ++i){
478  global_plan_[i] = new_plan[i];
479  }
480 
481  if( global_plan_.size() > 0 ){
482  geometry_msgs::PoseStamped& final_goal_pose = global_plan_[ global_plan_.size() - 1 ];
483  final_goal_x_ = final_goal_pose.pose.position.x;
484  final_goal_y_ = final_goal_pose.pose.position.y;
486  } else {
488  }
489 
490  if (compute_dists) {
491  //reset the map for new operations
494 
495  //make sure that we update our path based on the global plan and compute costs
498  ROS_DEBUG("Path/Goal distance computed");
499  }
500  }
501 
502  bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
503  double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
504  Trajectory t;
505 
506  double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);
507 
508  //if the trajectory is a legal one... the check passes
509  if(cost >= 0) {
510  return true;
511  }
512  ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);
513 
514  //otherwise the check fails
515  return false;
516  }
517 
518  double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
519  double vtheta, double vx_samp, double vy_samp, double vtheta_samp) {
520  Trajectory t;
521  double impossible_cost = path_map_.obstacleCosts();
522  generateTrajectory(x, y, theta,
523  vx, vy, vtheta,
524  vx_samp, vy_samp, vtheta_samp,
526  impossible_cost, t);
527 
528  // return the cost.
529  return double( t.cost_ );
530  }
531 
532  /*
533  * create the trajectories we wish to score
534  */
535  Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
536  double vx, double vy, double vtheta,
537  double acc_x, double acc_y, double acc_theta) {
538  //compute feasible velocity limits in robot space
539  double max_vel_x = max_vel_x_, max_vel_theta;
540  double min_vel_x, min_vel_theta;
541 
543  double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
544  max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
545  }
546 
547  //should we use the dynamic window approach?
548  if (dwa_) {
549  max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
550  min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);
551 
552  max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
553  min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
554  } else {
555  max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
556  min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);
557 
558  max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
559  min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
560  }
561 
562 
563  //we want to sample the velocity space regularly
564  double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);
565  double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);
566 
567  double vx_samp = min_vel_x;
568  double vtheta_samp = min_vel_theta;
569  double vy_samp = 0.0;
570 
571  //keep track of the best trajectory seen so far
572  Trajectory* best_traj = &traj_one;
573  best_traj->cost_ = -1.0;
574 
575  Trajectory* comp_traj = &traj_two;
576  comp_traj->cost_ = -1.0;
577 
578  Trajectory* swap = NULL;
579 
580  //any cell with a cost greater than the size of the map is impossible
581  double impossible_cost = path_map_.obstacleCosts();
582 
583  //if we're performing an escape we won't allow moving forward
584  if (!escaping_) {
585  //loop through all x velocities
586  for(int i = 0; i < vx_samples_; ++i) {
587  vtheta_samp = 0;
588  //first sample the straight trajectory
589  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
590  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
591 
592  //if the new trajectory is better... let's take it
593  if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
594  swap = best_traj;
595  best_traj = comp_traj;
596  comp_traj = swap;
597  }
598 
599  vtheta_samp = min_vel_theta;
600  //next sample all theta trajectories
601  for(int j = 0; j < vtheta_samples_ - 1; ++j){
602  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
603  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
604 
605  //if the new trajectory is better... let's take it
606  if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
607  swap = best_traj;
608  best_traj = comp_traj;
609  comp_traj = swap;
610  }
611  vtheta_samp += dvtheta;
612  }
613  vx_samp += dvx;
614  }
615 
616  //only explore y velocities with holonomic robots
617  if (holonomic_robot_) {
618  //explore trajectories that move forward but also strafe slightly
619  vx_samp = 0.1;
620  vy_samp = 0.1;
621  vtheta_samp = 0.0;
622  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
623  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
624 
625  //if the new trajectory is better... let's take it
626  if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
627  swap = best_traj;
628  best_traj = comp_traj;
629  comp_traj = swap;
630  }
631 
632  vx_samp = 0.1;
633  vy_samp = -0.1;
634  vtheta_samp = 0.0;
635  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
636  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
637 
638  //if the new trajectory is better... let's take it
639  if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
640  swap = best_traj;
641  best_traj = comp_traj;
642  comp_traj = swap;
643  }
644  }
645  } // end if not escaping
646 
647  //next we want to generate trajectories for rotating in place
648  vtheta_samp = min_vel_theta;
649  vx_samp = 0.0;
650  vy_samp = 0.0;
651 
652  //let's try to rotate toward open space
653  double heading_dist = DBL_MAX;
654 
655  for(int i = 0; i < vtheta_samples_; ++i) {
656  //enforce a minimum rotational velocity because the base can't handle small in-place rotations
657  double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
658  : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
659 
660  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
661  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
662 
663  //if the new trajectory is better... let's take it...
664  //note if we can legally rotate in place we prefer to do that rather than move with y velocity
665  if(comp_traj->cost_ >= 0
666  && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
667  && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
668  double x_r, y_r, th_r;
669  comp_traj->getEndpoint(x_r, y_r, th_r);
670  x_r += heading_lookahead_ * cos(th_r);
671  y_r += heading_lookahead_ * sin(th_r);
672  unsigned int cell_x, cell_y;
673 
674  //make sure that we'll be looking at a legal cell
675  if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
676  double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
677  if (ahead_gdist < heading_dist) {
678  //if we haven't already tried rotating left since we've moved forward
679  if (vtheta_samp < 0 && !stuck_left) {
680  swap = best_traj;
681  best_traj = comp_traj;
682  comp_traj = swap;
683  heading_dist = ahead_gdist;
684  }
685  //if we haven't already tried rotating right since we've moved forward
686  else if(vtheta_samp > 0 && !stuck_right) {
687  swap = best_traj;
688  best_traj = comp_traj;
689  comp_traj = swap;
690  heading_dist = ahead_gdist;
691  }
692  }
693  }
694  }
695 
696  vtheta_samp += dvtheta;
697  }
698 
699  //do we have a legal trajectory
700  if (best_traj->cost_ >= 0) {
701  // avoid oscillations of in place rotation and in place strafing
702  if ( ! (best_traj->xv_ > 0)) {
703  if (best_traj->thetav_ < 0) {
704  if (rotating_right) {
705  stuck_right = true;
706  }
707  rotating_right = true;
708  } else if (best_traj->thetav_ > 0) {
709  if (rotating_left){
710  stuck_left = true;
711  }
712  rotating_left = true;
713  } else if(best_traj->yv_ > 0) {
714  if (strafe_right) {
715  stuck_right_strafe = true;
716  }
717  strafe_right = true;
718  } else if(best_traj->yv_ < 0){
719  if (strafe_left) {
720  stuck_left_strafe = true;
721  }
722  strafe_left = true;
723  }
724 
725  //set the position we must move a certain distance away from
726  prev_x_ = x;
727  prev_y_ = y;
728  }
729 
730  double dist = hypot(x - prev_x_, y - prev_y_);
731  if (dist > oscillation_reset_dist_) {
732  rotating_left = false;
733  rotating_right = false;
734  strafe_left = false;
735  strafe_right = false;
736  stuck_left = false;
737  stuck_right = false;
738  stuck_left_strafe = false;
739  stuck_right_strafe = false;
740  }
741 
742  dist = hypot(x - escape_x_, y - escape_y_);
743  if(dist > escape_reset_dist_ ||
745  escaping_ = false;
746  }
747 
748  return *best_traj;
749  }
750 
751  //only explore y velocities with holonomic robots
752  if (holonomic_robot_) {
753  //if we can't rotate in place or move forward... maybe we can move sideways and rotate
754  vtheta_samp = min_vel_theta;
755  vx_samp = 0.0;
756 
757  //loop through all y velocities
758  for(unsigned int i = 0; i < y_vels_.size(); ++i){
759  vtheta_samp = 0;
760  vy_samp = y_vels_[i];
761  //sample completely horizontal trajectories
762  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
763  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
764 
765  //if the new trajectory is better... let's take it
766  if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
767  double x_r, y_r, th_r;
768  comp_traj->getEndpoint(x_r, y_r, th_r);
769  x_r += heading_lookahead_ * cos(th_r);
770  y_r += heading_lookahead_ * sin(th_r);
771  unsigned int cell_x, cell_y;
772 
773  //make sure that we'll be looking at a legal cell
774  if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
775  double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
776  if (ahead_gdist < heading_dist) {
777  //if we haven't already tried strafing left since we've moved forward
778  if (vy_samp > 0 && !stuck_left_strafe) {
779  swap = best_traj;
780  best_traj = comp_traj;
781  comp_traj = swap;
782  heading_dist = ahead_gdist;
783  }
784  //if we haven't already tried rotating right since we've moved forward
785  else if(vy_samp < 0 && !stuck_right_strafe) {
786  swap = best_traj;
787  best_traj = comp_traj;
788  comp_traj = swap;
789  heading_dist = ahead_gdist;
790  }
791  }
792  }
793  }
794  }
795  }
796 
797  //do we have a legal trajectory
798  if (best_traj->cost_ >= 0) {
799  if (!(best_traj->xv_ > 0)) {
800  if (best_traj->thetav_ < 0) {
801  if (rotating_right){
802  stuck_right = true;
803  }
804  rotating_left = true;
805  } else if(best_traj->thetav_ > 0) {
806  if(rotating_left){
807  stuck_left = true;
808  }
809  rotating_right = true;
810  } else if(best_traj->yv_ > 0) {
811  if(strafe_right){
812  stuck_right_strafe = true;
813  }
814  strafe_left = true;
815  } else if(best_traj->yv_ < 0) {
816  if(strafe_left){
817  stuck_left_strafe = true;
818  }
819  strafe_right = true;
820  }
821 
822  //set the position we must move a certain distance away from
823  prev_x_ = x;
824  prev_y_ = y;
825 
826  }
827 
828  double dist = hypot(x - prev_x_, y - prev_y_);
829  if(dist > oscillation_reset_dist_) {
830  rotating_left = false;
831  rotating_right = false;
832  strafe_left = false;
833  strafe_right = false;
834  stuck_left = false;
835  stuck_right = false;
836  stuck_left_strafe = false;
837  stuck_right_strafe = false;
838  }
839 
840  dist = hypot(x - escape_x_, y - escape_y_);
842  escaping_ = false;
843  }
844 
845  return *best_traj;
846  }
847 
848  //and finally, if we can't do anything else, we want to generate trajectories that move backwards slowly
849  vtheta_samp = 0.0;
850  vx_samp = backup_vel_;
851  vy_samp = 0.0;
852  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
853  acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
854 
855  //if the new trajectory is better... let's take it
856  /*
857  if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
858  swap = best_traj;
859  best_traj = comp_traj;
860  comp_traj = swap;
861  }
862  */
863 
864  //we'll allow moving backwards slowly even when the static map shows it as blocked
865  swap = best_traj;
866  best_traj = comp_traj;
867  comp_traj = swap;
868 
869  double dist = hypot(x - prev_x_, y - prev_y_);
870  if (dist > oscillation_reset_dist_) {
871  rotating_left = false;
872  rotating_right = false;
873  strafe_left = false;
874  strafe_right = false;
875  stuck_left = false;
876  stuck_right = false;
877  stuck_left_strafe = false;
878  stuck_right_strafe = false;
879  }
880 
881  //only enter escape mode when the planner has given a valid goal point
882  if (!escaping_ && best_traj->cost_ > -2.0) {
883  escape_x_ = x;
884  escape_y_ = y;
885  escape_theta_ = theta;
886  escaping_ = true;
887  }
888 
889  dist = hypot(x - escape_x_, y - escape_y_);
890 
891  if (dist > escape_reset_dist_ ||
893  escaping_ = false;
894  }
895 
896 
897  //if the trajectory failed because the footprint hits something, we're still going to back up
898  if(best_traj->cost_ == -1.0)
899  best_traj->cost_ = 1.0;
900 
901  return *best_traj;
902 
903  }
904 
905  //given the current state of the robot, find a good trajectory
907  tf::Stamped<tf::Pose>& drive_velocities){
908 
909  Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
910  Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
911 
912  //reset the map for new operations
915 
916  //temporarily remove obstacles that are within the footprint of the robot
917  std::vector<base_local_planner::Position2DInt> footprint_list =
919  pos,
921  costmap_,
922  true);
923 
924  //mark cells within the initial footprint of the robot
925  for (unsigned int i = 0; i < footprint_list.size(); ++i) {
926  path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
927  }
928 
929  //make sure that we update our path based on the global plan and compute costs
932  ROS_DEBUG("Path/Goal distance computed");
933 
934  //rollout trajectories and find the minimum cost one
935  Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
936  vel[0], vel[1], vel[2],
938  ROS_DEBUG("Trajectories created");
939 
940  /*
941  //If we want to print a ppm file to draw goal dist
942  char buf[4096];
943  sprintf(buf, "base_local_planner.ppm");
944  FILE *fp = fopen(buf, "w");
945  if(fp){
946  fprintf(fp, "P3\n");
947  fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
948  fprintf(fp, "255\n");
949  for(int j = map_.size_y_ - 1; j >= 0; --j){
950  for(unsigned int i = 0; i < map_.size_x_; ++i){
951  int g_dist = 255 - int(map_(i, j).goal_dist);
952  int p_dist = 255 - int(map_(i, j).path_dist);
953  if(g_dist < 0)
954  g_dist = 0;
955  if(p_dist < 0)
956  p_dist = 0;
957  fprintf(fp, "%d 0 %d ", g_dist, 0);
958  }
959  fprintf(fp, "\n");
960  }
961  fclose(fp);
962  }
963  */
964 
965  if(best.cost_ < 0){
966  drive_velocities.setIdentity();
967  }
968  else{
969  tf::Vector3 start(best.xv_, best.yv_, 0);
970  drive_velocities.setOrigin(start);
971  tf::Matrix3x3 matrix;
973  drive_velocities.setBasis(matrix);
974  }
975 
976  return best;
977  }
978 
979  //we need to take the footprint of the robot into account when we calculate cost to obstacles
980  double TrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i){
981  //check if the footprint is legal
983  }
984 
985 
986  void TrajectoryPlanner::getLocalGoal(double& x, double& y){
987  x = path_map_.goal_x_;
988  y = path_map_.goal_y_;
989  }
990 
991 };
992 
993 
int vx_samples_
The number of samples we&#39;ll take in the x dimenstion of the control space.
double angular_sim_granularity_
The distance between angular simulation points.
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
Subclass will implement this method to check a footprint at a given position and orientation for lega...
double computeNewYPosition(double yi, double vx, double vy, double theta, double dt)
Compute y position based on velocity.
double heading_scoring_timestep_
How far to look ahead in time when we score a heading.
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
Compute the components and total cost for a map grid cell.
int vtheta_samples_
The number of samples we&#39;ll take in the theta dimension of the control space.
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
double scoreTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
Generate and score a single trajectory.
void setTargetCells(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cells are considered path based on the global plan.
Definition: map_grid.cpp:171
base_local_planner::FootprintHelper footprint_helper_
double computeNewXPosition(double xi, double vx, double vy, double theta, double dt)
Compute x position based on velocity.
double oscillation_reset_dist_
The distance the robot must travel before it can explore rotational velocities that were unsuccessful...
bool final_goal_position_valid_
True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent...
std::vector< base_local_planner::Position2DInt > getFootprintCells(Eigen::Vector3f pos, std::vector< geometry_msgs::Point > footprint_spec, const costmap_2d::Costmap2D &, bool fill)
Used to get the cells that make up the footprint of the robot.
bool stuck_right
Booleans to keep the robot from oscillating during rotation.
bool within_robot
Mark for cells within the robot footprint.
Definition: map_cell.h:63
const costmap_2d::Costmap2D & costmap_
Provides access to cost map information.
double heading_lookahead_
How far the robot should look ahead of itself when differentiating between different rotational veloc...
static double getYaw(const Quaternion &bt_q)
double goal_y_
The goal distance was last computed from.
Definition: map_grid.h:189
~TrajectoryPlanner()
Destructs a trajectory controller.
bool holonomic_robot_
Is the robot holonomic or not?
double min_in_place_vel_th_
Velocity limits for the controller.
double computeNewThetaPosition(double thetai, double vth, double dt)
Compute orientation based on velocity.
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
double lineCost(int x0, int x1, int y0, int y1)
MapGrid path_map_
The local map grid where we propagate path distance.
double prev_y_
Used to calculate the distance the robot has traveled before reseting oscillation booleans...
bool dwa_
Should we use the dynamic window approach?
bool checkTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
Generate and score a single trajectory.
double sim_time_
The number of seconds each trajectory is "rolled-out".
double cost_
The cost/score of the trajectory.
Definition: trajectory.h:62
double backup_vel_
The velocity to use while backing up.
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
geometry_msgs::TransformStamped t
double sim_period_
The number of seconds to use to compute max/min vels for dwa.
bool simple_attractor_
Enables simple attraction to a goal point.
void setRotation(const Quaternion &q)
#define DBL_MAX
std::vector< double > y_vels_
Y velocities to explore.
double escape_reset_theta_
The distance the robot must travel before it can leave escape mode.
Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta)
Create the trajectories we wish to explore, score them, and return the best option.
void resetPoints()
Clear the trajectory&#39;s points.
Definition: trajectory.cpp:65
unsigned char getCost(unsigned int mx, unsigned int my) const
double escape_theta_
Used to calculate the distance the robot has traveled before reseting escape booleans.
WorldModel & world_model_
The world model that the controller uses for collision detection.
std::vector< geometry_msgs::PoseStamped > global_plan_
The global path for the robot to follow.
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
static Quaternion createQuaternionFromYaw(double yaw)
double footprintCost(double x_i, double y_i, double theta_i)
Checks the legality of the robot footprint at a position and orientation using the world model...
double occdist_scale_
Scaling factors for the controller&#39;s cost function.
MapGrid goal_map_
The local map grid where we propagate goal distance.
bool escaping_
Boolean to keep track of whether we&#39;re in escape mode.
double headingDiff(int cell_x, int cell_y, double x, double y, double heading)
double thetav_
The x, y, and theta velocities of the trajectory.
Definition: trajectory.h:60
TFSIMD_FORCE_INLINE const tfScalar & x() const
double sim_granularity_
The distance between simulation points.
double acc_lim_theta_
The acceleration limits of the robot.
double min(double a, double b)
void addPoint(double x, double y, double th)
Add a point to the end of a trajectory.
Definition: trajectory.cpp:59
Trajectory traj_two
Used for scoring trajectories.
void updatePlan(const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
Update the plan that the controller is following.
void getEndpoint(double &x, double &y, double &th) const
Get the last point of the trajectory.
Definition: trajectory.cpp:71
double target_dist
Distance to planner&#39;s path.
Definition: map_cell.h:59
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:52
double final_goal_y_
The end position of the plan.
void generateTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, double impossible_cost, Trajectory &traj)
Generate and score a single trajectory.
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
bool heading_scoring_
Should we score based on the rollout approach or the heading approach.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
bool stuck_right_strafe
Booleans to keep the robot from oscillating during strafing.
std::vector< geometry_msgs::Point > footprint_spec_
The footprint specification of the robot.
Trajectory findBestPath(tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > &drive_velocities)
Given the current position, orientation, and velocity of the robot, return a trajectory to follow...
Stores path distance and goal distance information used for scoring trajectories. ...
Definition: map_cell.h:44
double computeNewVelocity(double vg, double vi, double a_max, double dt)
Compute velocity based on acceleration.
void getLocalGoal(double &x, double &y)
Accessor for the goal the robot is currently pursuing in world corrdinates.
bool strafe_left
Booleans to keep track of strafe direction for the robot.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void setLocalGoal(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cell is considered the next local goal.
Definition: map_grid.cpp:210
void swap(scoped_ptr< T > &, scoped_ptr< T > &)
double max(double a, double b)
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
bool rotating_right
Booleans to keep track of the direction of rotation for the robot.
def shortest_angular_distance(from_angle, to_angle)
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44
void calculateMinAndMaxDistances(const std::vector< geometry_msgs::Point > &footprint, double &min_dist, double &max_dist)
#define ROS_DEBUG(...)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49