$search
00001 #include <safe_teleop_base/safe_trajectory_planner.h> 00002 00003 using namespace std; 00004 using namespace costmap_2d; 00005 using namespace base_local_planner; 00006 00007 namespace safe_teleop { 00008 SafeTrajectoryPlanner::SafeTrajectoryPlanner(WorldModel& world_model, 00009 const Costmap2D& costmap, 00010 std::vector<geometry_msgs::Point> footprint_spec, 00011 double inscribed_radius, double circumscribed_radius, 00012 double acc_lim_x, double acc_lim_y, double acc_lim_theta, 00013 double sim_time, double sim_granularity, 00014 int vx_samples, int vy_samples, int vtheta_samples, 00015 double userdist_scale, double occdist_scale, 00016 double max_vel_x, double min_vel_x, 00017 double max_vel_y, double min_vel_y, 00018 double max_vel_th, double min_vel_th, 00019 bool holonomic_robot, bool dwa) 00020 : map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()), costmap_(costmap), 00021 world_model_(world_model), footprint_spec_(footprint_spec), 00022 inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), 00023 sim_time_(sim_time), sim_granularity_(sim_granularity), 00024 vx_samples_(vx_samples), vy_samples_(vy_samples), vtheta_samples_(vtheta_samples), 00025 userdist_scale_(userdist_scale), occdist_scale_(occdist_scale), 00026 acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta), 00027 max_vel_x_(max_vel_x), min_vel_x_(min_vel_x), 00028 max_vel_y_(max_vel_y), min_vel_y_(min_vel_y), 00029 max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), 00030 holonomic_robot_(holonomic_robot), dwa_(dwa) 00031 { 00032 } 00033 00034 SafeTrajectoryPlanner::~SafeTrajectoryPlanner(){} 00035 00036 //create and score a trajectory given the current pose of the robot and selected velocities 00037 void SafeTrajectoryPlanner::generateTrajectory( 00038 double x, double y, double theta, 00039 double vx, double vy, double vtheta, 00040 double vx_samp, double vy_samp, double vtheta_samp, 00041 double acc_x, double acc_y, double acc_theta, 00042 double impossible_cost, 00043 double dx, double dy, double dtheta, 00044 Trajectory& traj){ 00045 double x_i = x; 00046 double y_i = y; 00047 double theta_i = theta; 00048 00049 double vx_i, vy_i, vtheta_i; 00050 00051 vx_i = vx; 00052 vy_i = vy; 00053 vtheta_i = vtheta; 00054 00055 //compute the magnitude of the velocities 00056 // double vmag = sqrt(vx_samp * vx_samp + vy_samp * vy_samp); 00057 00058 //compute the number of steps we must take along this trajectory to be "safe" 00059 int num_steps = int(sim_time_ / sim_granularity_ + 0.5); 00060 00061 double dt = sim_time_ / num_steps; 00062 double time = 0.0; 00063 00064 //create a potential trajectory 00065 traj.resetPoints(); 00066 traj.xv_ = vx_samp; 00067 traj.yv_ = vy_samp; 00068 traj.thetav_ = vtheta_samp; 00069 traj.cost_ = -1.0; 00070 00071 if(num_steps == 0) 00072 return; 00073 00074 //initialize the costs for the trajectory 00075 double user_dist = 0.0; 00076 double occ_cost = 0.0; 00077 00078 for(int i = 0; i < num_steps; ++i){ 00079 //get map coordinates of a point 00080 unsigned int cell_x, cell_y; 00081 00082 //we don't want a path that goes off the know map 00083 if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){ 00084 traj.cost_ = -1.0; 00085 return; 00086 } 00087 00088 //check the point on the trajectory for legality 00089 double footprint_cost = footprintCost(x_i, y_i, theta_i); 00090 00091 //if the footprint hits an obstacle this trajectory is invalid 00092 if(footprint_cost < 0){ 00093 traj.cost_ = -1.0; 00094 return; 00095 } 00096 00097 occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y))); 00098 00099 time += dt; 00100 00101 //the point is legal... add it to the trajectory 00102 traj.addPoint(x_i, y_i, theta_i); 00103 00104 //calculate velocities 00105 vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt); 00106 vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt); 00107 vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt); 00108 00109 //calculate positions 00110 x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt); 00111 y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt); 00112 theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt); 00113 00114 } 00115 00116 user_dist = sqrt((vx_samp - dx) * (vx_samp - dx) + 00117 (vy_samp - dy) * (vy_samp - dy) + 00118 (vtheta_samp - dtheta) * (vtheta_samp - dtheta)); 00119 00120 traj.cost_ = user_dist * userdist_scale_ + occdist_scale_ * occ_cost; 00121 } 00122 00123 //calculate the cost of a ray-traced line 00124 double SafeTrajectoryPlanner::lineCost(int x0, int x1, 00125 int y0, int y1){ 00126 //Bresenham Ray-Tracing 00127 int deltax = abs(x1 - x0); // The difference between the x's 00128 int deltay = abs(y1 - y0); // The difference between the y's 00129 int x = x0; // Start x off at the first pixel 00130 int y = y0; // Start y off at the first pixel 00131 00132 int xinc1, xinc2, yinc1, yinc2; 00133 int den, num, numadd, numpixels; 00134 00135 double line_cost = 0.0; 00136 double point_cost = -1.0; 00137 00138 if (x1 >= x0) // The x-values are increasing 00139 { 00140 xinc1 = 1; 00141 xinc2 = 1; 00142 } 00143 else // The x-values are decreasing 00144 { 00145 xinc1 = -1; 00146 xinc2 = -1; 00147 } 00148 00149 if (y1 >= y0) // The y-values are increasing 00150 { 00151 yinc1 = 1; 00152 yinc2 = 1; 00153 } 00154 else // The y-values are decreasing 00155 { 00156 yinc1 = -1; 00157 yinc2 = -1; 00158 } 00159 00160 if (deltax >= deltay) // There is at least one x-value for every y-value 00161 { 00162 xinc1 = 0; // Don't change the x when numerator >= denominator 00163 yinc2 = 0; // Don't change the y for every iteration 00164 den = deltax; 00165 num = deltax / 2; 00166 numadd = deltay; 00167 numpixels = deltax; // There are more x-values than y-values 00168 } 00169 else // There is at least one y-value for every x-value 00170 { 00171 xinc2 = 0; // Don't change the x for every iteration 00172 yinc1 = 0; // Don't change the y when numerator >= denominator 00173 den = deltay; 00174 num = deltay / 2; 00175 numadd = deltax; 00176 numpixels = deltay; // There are more y-values than x-values 00177 } 00178 00179 for (int curpixel = 0; curpixel <= numpixels; curpixel++) 00180 { 00181 point_cost = pointCost(x, y); //Score the current point 00182 00183 if(point_cost < 0) 00184 return -1; 00185 00186 if(line_cost < point_cost) 00187 line_cost = point_cost; 00188 00189 num += numadd; // Increase the numerator by the top of the fraction 00190 if (num >= den) // Check if numerator >= denominator 00191 { 00192 num -= den; // Calculate the new numerator value 00193 x += xinc1; // Change the x as appropriate 00194 y += yinc1; // Change the y as appropriate 00195 } 00196 x += xinc2; // Change the x as appropriate 00197 y += yinc2; // Change the y as appropriate 00198 } 00199 00200 return line_cost; 00201 } 00202 00203 double SafeTrajectoryPlanner::pointCost(int x, int y){ 00204 unsigned char cost = costmap_.getCost(x, y); 00205 //if the cell is in an obstacle the path is invalid 00206 if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){ 00207 return -1; 00208 } 00209 00210 return cost; 00211 } 00212 00213 //create the trajectories we wish to score 00214 Trajectory SafeTrajectoryPlanner::createTrajectories(double x, double y, double theta, 00215 double vx, double vy, double vtheta, 00216 double acc_x, double acc_y, double acc_theta, 00217 double dx, double dy, double dtheta){ 00218 //compute feasible velocity limits in robot space 00219 double max_vel_x, max_vel_y, max_vel_theta; 00220 double min_vel_x, min_vel_y, min_vel_theta; 00221 00222 //should we use the dynamic window approach? 00223 if(dwa_){ 00224 max_vel_x = min(max_vel_x_, vx + acc_x * .1); 00225 min_vel_x = max(min_vel_x_, vx - acc_x * .1); 00226 00227 max_vel_y = min(max_vel_y_, vy + acc_y * .1); 00228 min_vel_y = max(min_vel_y_, vy - acc_y * .1); 00229 00230 max_vel_theta = min(max_vel_th_, vtheta + acc_theta * .1); 00231 min_vel_theta = max(min_vel_th_, vtheta - acc_theta * .1); 00232 } else { 00233 max_vel_x = min(max_vel_x_, vx + acc_x * sim_time_); 00234 min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_); 00235 00236 max_vel_y = min(max_vel_y_, vy + acc_y * sim_time_); 00237 min_vel_y = max(min_vel_y_, vy - acc_y * sim_time_); 00238 00239 max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_); 00240 min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_); 00241 } 00242 00243 //keep track of the best trajectory seen so far 00244 Trajectory* best_traj = &traj_one; 00245 best_traj->cost_ = -1.0; 00246 00247 Trajectory* comp_traj = &traj_two; 00248 comp_traj->cost_ = -1.0; 00249 00250 Trajectory* swap = NULL; 00251 00252 //any cell with a cost greater than the size of the map is impossible 00253 double impossible_cost = map_.map_.size(); 00254 00255 // first sample the user suggested trajectory 00256 generateTrajectory(x, y, theta, vx, vy, vtheta, dx, dy, dtheta, acc_x, acc_y, acc_theta, impossible_cost, dx, dy, dtheta, *comp_traj); 00257 00258 //if the new trajectory is better... let's take it 00259 if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ 00260 swap = best_traj; 00261 best_traj = comp_traj; 00262 comp_traj = swap; 00263 } 00264 00265 //we want to sample the velocity space regularly 00266 double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1); 00267 double dvy = (max_vel_y - min_vel_y) / (vy_samples_ - 1); 00268 double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1); 00269 00270 double vx_samp = 0.0; 00271 double vy_samp = 0.0; 00272 double vtheta_samp = 0.0; 00273 00274 if (holonomic_robot_) { 00275 //loop through all x velocities 00276 vx_samp = min_vel_x; 00277 for(int i = 0; i < vx_samples_; ++i){ 00278 vy_samp = 0; 00279 vtheta_samp = 0; 00280 //first sample the straight trajectory 00281 generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, dx, dy, dtheta, *comp_traj); 00282 00283 //if the new trajectory is better... let's take it 00284 if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ 00285 swap = best_traj; 00286 best_traj = comp_traj; 00287 comp_traj = swap; 00288 } 00289 00290 vy_samp = min_vel_y; 00291 for (int k = 0; k < vy_samples_; k++) { 00292 vtheta_samp = 0; 00293 //first sample the trajectory with no rotation 00294 generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, dx, dy, dtheta, *comp_traj); 00295 00296 //if the new trajectory is better... let's take it 00297 if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ 00298 swap = best_traj; 00299 best_traj = comp_traj; 00300 comp_traj = swap; 00301 } 00302 00303 00304 vtheta_samp = min_vel_theta; 00305 //next sample all theta trajectories 00306 for(int j = 0; j < vtheta_samples_ - 1; ++j){ 00307 generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, dx, dy, dtheta, *comp_traj); 00308 00309 //if the new trajectory is better... let's take it 00310 if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ 00311 swap = best_traj; 00312 best_traj = comp_traj; 00313 comp_traj = swap; 00314 } 00315 vtheta_samp += dvtheta; 00316 } 00317 vy_samp += dvy; 00318 } 00319 vx_samp += dvx; 00320 } 00321 } else { 00322 vy_samp = 0.0; 00323 //loop through all x velocities 00324 vx_samp = min_vel_x; 00325 for(int i = 0; i < vx_samples_; ++i){ 00326 vtheta_samp = 0; 00327 //first sample the straight trajectory 00328 generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, dx, dy, dtheta, *comp_traj); 00329 00330 //if the new trajectory is better... let's take it 00331 if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ 00332 swap = best_traj; 00333 best_traj = comp_traj; 00334 comp_traj = swap; 00335 } 00336 00337 vtheta_samp = min_vel_theta; 00338 //next sample all theta trajectories 00339 for(int j = 0; j < vtheta_samples_ - 1; ++j){ 00340 generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, dx, dy, dtheta, *comp_traj); 00341 00342 //if the new trajectory is better... let's take it 00343 if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ 00344 swap = best_traj; 00345 best_traj = comp_traj; 00346 comp_traj = swap; 00347 } 00348 vtheta_samp += dvtheta; 00349 00350 } 00351 vx_samp += dvx; 00352 } 00353 } 00354 00355 00356 00357 return *best_traj; 00358 } 00359 00360 //given the current state of the robot, find a good trajectory 00361 Trajectory SafeTrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel, 00362 tf::Stamped<tf::Pose> user_vel, tf::Stamped<tf::Pose>& drive_velocities){ 00363 00364 double yaw = tf::getYaw(global_pose.getRotation()); 00365 double vel_yaw = tf::getYaw(global_vel.getRotation()); 00366 double dyaw = tf::getYaw(user_vel.getRotation()); 00367 00368 double x = global_pose.getOrigin().getX(); 00369 double y = global_pose.getOrigin().getY(); 00370 double theta = yaw; 00371 00372 double vx = global_vel.getOrigin().getX(); 00373 double vy = global_vel.getOrigin().getY(); 00374 double vtheta = vel_yaw; 00375 00376 double dx = user_vel.getOrigin().getX(); 00377 double dy = user_vel.getOrigin().getY(); 00378 double dtheta = dyaw; 00379 00380 //reset the map for new operations 00381 map_.resetPathDist(); 00382 00383 //temporarily remove obstacles that are within the footprint of the robot 00384 vector<base_local_planner::Position2DInt> footprint_list = getFootprintCells(x, y, theta, true); 00385 00386 //mark cells within the initial footprint of the robot 00387 for(unsigned int i = 0; i < footprint_list.size(); ++i){ 00388 map_(footprint_list[i].x, footprint_list[i].y).within_robot = true; 00389 } 00390 00391 //rollout trajectories and find the minimum cost one 00392 Trajectory best = createTrajectories(x, y, theta, vx, vy, vtheta, acc_lim_x_, acc_lim_y_, acc_lim_theta_, dx, dy, dtheta); 00393 ROS_DEBUG("Trajectories created"); 00394 00395 if(best.cost_ < 0) { 00396 drive_velocities.setIdentity(); 00397 ROS_INFO("No safe trajectory found"); 00398 } 00399 else { 00400 btVector3 start(best.xv_, best.yv_, 0); 00401 drive_velocities.setOrigin(start); 00402 btMatrix3x3 matrix; 00403 matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_)); 00404 drive_velocities.setBasis(matrix); 00405 } 00406 00407 return best; 00408 } 00409 00410 Trajectory SafeTrajectoryPlanner::findPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel, 00411 tf::Stamped<tf::Pose> user_vel){ 00412 00413 double yaw = tf::getYaw(global_pose.getRotation()); 00414 double vel_yaw = tf::getYaw(global_vel.getRotation()); 00415 double dyaw = tf::getYaw(user_vel.getRotation()); 00416 00417 double x = global_pose.getOrigin().getX(); 00418 double y = global_pose.getOrigin().getY(); 00419 double theta = yaw; 00420 00421 double vx = global_vel.getOrigin().getX(); 00422 double vy = global_vel.getOrigin().getY(); 00423 double vtheta = vel_yaw; 00424 00425 double dx = user_vel.getOrigin().getX(); 00426 double dy = user_vel.getOrigin().getY(); 00427 double dtheta = dyaw; 00428 00429 //reset the map for new operations 00430 map_.resetPathDist(); 00431 00432 //temporarily remove obstacles that are within the footprint of the robot 00433 vector<base_local_planner::Position2DInt> footprint_list = getFootprintCells(x, y, theta, true); 00434 00435 //mark cells within the initial footprint of the robot 00436 for(unsigned int i = 0; i < footprint_list.size(); ++i){ 00437 map_(footprint_list[i].x, footprint_list[i].y).within_robot = true; 00438 } 00439 00440 Trajectory traj; 00441 double impossible_cost = map_.map_.size(); 00442 generateTrajectory(x, y, theta, vx, vy, vtheta, dx, dy, dtheta, 00443 acc_lim_x_, acc_lim_y_, acc_lim_theta_, impossible_cost, dx, dy, dtheta, traj); 00444 00445 return traj; 00446 } 00447 00448 //we need to take the footprint of the robot into account when we calculate cost to obstacles 00449 double SafeTrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i){ 00450 //build the oriented footprint 00451 double cos_th = cos(theta_i); 00452 double sin_th = sin(theta_i); 00453 vector<geometry_msgs::Point> oriented_footprint; 00454 for(unsigned int i = 0; i < footprint_spec_.size(); ++i){ 00455 geometry_msgs::Point new_pt; 00456 new_pt.x = x_i + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th); 00457 new_pt.y = y_i + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th); 00458 oriented_footprint.push_back(new_pt); 00459 } 00460 00461 geometry_msgs::Point robot_position; 00462 robot_position.x = x_i; 00463 robot_position.y = y_i; 00464 00465 //check if the footprint is legal 00466 double footprint_cost = world_model_.footprintCost(robot_position, oriented_footprint, inscribed_radius_, circumscribed_radius_); 00467 00468 return footprint_cost; 00469 } 00470 00471 void SafeTrajectoryPlanner::getLineCells(int x0, int x1, int y0, int y1, vector<base_local_planner::Position2DInt>& pts){ 00472 //Bresenham Ray-Tracing 00473 int deltax = abs(x1 - x0); // The difference between the x's 00474 int deltay = abs(y1 - y0); // The difference between the y's 00475 int x = x0; // Start x off at the first pixel 00476 int y = y0; // Start y off at the first pixel 00477 00478 int xinc1, xinc2, yinc1, yinc2; 00479 int den, num, numadd, numpixels; 00480 00481 base_local_planner::Position2DInt pt; 00482 00483 if (x1 >= x0) // The x-values are increasing 00484 { 00485 xinc1 = 1; 00486 xinc2 = 1; 00487 } 00488 else // The x-values are decreasing 00489 { 00490 xinc1 = -1; 00491 xinc2 = -1; 00492 } 00493 00494 if (y1 >= y0) // The y-values are increasing 00495 { 00496 yinc1 = 1; 00497 yinc2 = 1; 00498 } 00499 else // The y-values are decreasing 00500 { 00501 yinc1 = -1; 00502 yinc2 = -1; 00503 } 00504 00505 if (deltax >= deltay) // There is at least one x-value for every y-value 00506 { 00507 xinc1 = 0; // Don't change the x when numerator >= denominator 00508 yinc2 = 0; // Don't change the y for every iteration 00509 den = deltax; 00510 num = deltax / 2; 00511 numadd = deltay; 00512 numpixels = deltax; // There are more x-values than y-values 00513 } 00514 else // There is at least one y-value for every x-value 00515 { 00516 xinc2 = 0; // Don't change the x for every iteration 00517 yinc1 = 0; // Don't change the y when numerator >= denominator 00518 den = deltay; 00519 num = deltay / 2; 00520 numadd = deltax; 00521 numpixels = deltay; // There are more y-values than x-values 00522 } 00523 00524 for (int curpixel = 0; curpixel <= numpixels; curpixel++) 00525 { 00526 pt.x = x; //Draw the current pixel 00527 pt.y = y; 00528 pts.push_back(pt); 00529 00530 num += numadd; // Increase the numerator by the top of the fraction 00531 if (num >= den) // Check if numerator >= denominator 00532 { 00533 num -= den; // Calculate the new numerator value 00534 x += xinc1; // Change the x as appropriate 00535 y += yinc1; // Change the y as appropriate 00536 } 00537 x += xinc2; // Change the x as appropriate 00538 y += yinc2; // Change the y as appropriate 00539 } 00540 } 00541 00542 //get the cellsof a footprint at a given position 00543 vector<base_local_planner::Position2DInt> SafeTrajectoryPlanner::getFootprintCells(double x_i, double y_i, double theta_i, bool fill){ 00544 vector<base_local_planner::Position2DInt> footprint_cells; 00545 00546 //if we have no footprint... do nothing 00547 if(footprint_spec_.size() <= 1){ 00548 unsigned int mx, my; 00549 if(costmap_.worldToMap(x_i, y_i, mx, my)){ 00550 Position2DInt center; 00551 center.x = mx; 00552 center.y = my; 00553 footprint_cells.push_back(center); 00554 } 00555 return footprint_cells; 00556 } 00557 00558 //pre-compute cos and sin values 00559 double cos_th = cos(theta_i); 00560 double sin_th = sin(theta_i); 00561 double new_x, new_y; 00562 unsigned int x0, y0, x1, y1; 00563 unsigned int last_index = footprint_spec_.size() - 1; 00564 00565 for(unsigned int i = 0; i < last_index; ++i){ 00566 //find the cell coordinates of the first segment point 00567 new_x = x_i + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th); 00568 new_y = y_i + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th); 00569 if(!costmap_.worldToMap(new_x, new_y, x0, y0)) 00570 return footprint_cells; 00571 00572 //find the cell coordinates of the second segment point 00573 new_x = x_i + (footprint_spec_[i + 1].x * cos_th - footprint_spec_[i + 1].y * sin_th); 00574 new_y = y_i + (footprint_spec_[i + 1].x * sin_th + footprint_spec_[i + 1].y * cos_th); 00575 if(!costmap_.worldToMap(new_x, new_y, x1, y1)) 00576 return footprint_cells; 00577 00578 getLineCells(x0, x1, y0, y1, footprint_cells); 00579 } 00580 00581 //we need to close the loop, so we also have to raytrace from the last pt to first pt 00582 new_x = x_i + (footprint_spec_[last_index].x * cos_th - footprint_spec_[last_index].y * sin_th); 00583 new_y = y_i + (footprint_spec_[last_index].x * sin_th + footprint_spec_[last_index].y * cos_th); 00584 if(!costmap_.worldToMap(new_x, new_y, x0, y0)) 00585 return footprint_cells; 00586 00587 new_x = x_i + (footprint_spec_[0].x * cos_th - footprint_spec_[0].y * sin_th); 00588 new_y = y_i + (footprint_spec_[0].x * sin_th + footprint_spec_[0].y * cos_th); 00589 if(!costmap_.worldToMap(new_x, new_y, x1, y1)) 00590 return footprint_cells; 00591 00592 getLineCells(x0, x1, y0, y1, footprint_cells); 00593 00594 if(fill) 00595 getFillCells(footprint_cells); 00596 00597 return footprint_cells; 00598 } 00599 00600 void SafeTrajectoryPlanner::getFillCells(vector<base_local_planner::Position2DInt>& footprint){ 00601 //quick bubble sort to sort pts by x 00602 base_local_planner::Position2DInt swap, pt; 00603 unsigned int i = 0; 00604 while(i < footprint.size() - 1){ 00605 if(footprint[i].x > footprint[i + 1].x){ 00606 swap = footprint[i]; 00607 footprint[i] = footprint[i + 1]; 00608 footprint[i + 1] = swap; 00609 if(i > 0) 00610 --i; 00611 } 00612 else 00613 ++i; 00614 } 00615 00616 i = 0; 00617 base_local_planner::Position2DInt min_pt; 00618 base_local_planner::Position2DInt max_pt; 00619 unsigned int min_x = footprint[0].x; 00620 unsigned int max_x = footprint[footprint.size() -1].x; 00621 //walk through each column and mark cells inside the footprint 00622 for(unsigned int x = min_x; x <= max_x; ++x){ 00623 if(i >= footprint.size() - 1) 00624 break; 00625 00626 if(footprint[i].y < footprint[i + 1].y){ 00627 min_pt = footprint[i]; 00628 max_pt = footprint[i + 1]; 00629 } 00630 else{ 00631 min_pt = footprint[i + 1]; 00632 max_pt = footprint[i]; 00633 } 00634 00635 i += 2; 00636 while(i < footprint.size() && footprint[i].x == x){ 00637 if(footprint[i].y < min_pt.y) 00638 min_pt = footprint[i]; 00639 else if(footprint[i].y > max_pt.y) 00640 max_pt = footprint[i]; 00641 ++i; 00642 } 00643 00644 //loop though cells in the column 00645 for(unsigned int y = min_pt.y; y < max_pt.y; ++y){ 00646 pt.x = x; 00647 pt.y = y; 00648 footprint.push_back(pt); 00649 } 00650 } 00651 } 00652 00653 }; 00654 00655