00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <teb_local_planner/timed_elastic_band.h>
00040
00041
00042 namespace teb_local_planner
00043 {
00044
00045
00046 TimedElasticBand::TimedElasticBand()
00047 {
00048 }
00049
00050 TimedElasticBand::~TimedElasticBand()
00051 {
00052 ROS_DEBUG("Destructor Timed_Elastic_Band...");
00053 clearTimedElasticBand();
00054 }
00055
00056
00057 void TimedElasticBand::addPose(const PoseSE2& pose, bool fixed)
00058 {
00059 VertexPose* pose_vertex = new VertexPose(pose, fixed);
00060 pose_vec_.push_back( pose_vertex );
00061 return;
00062 }
00063
00064 void TimedElasticBand::addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed)
00065 {
00066 VertexPose* pose_vertex = new VertexPose(position, theta, fixed);
00067 pose_vec_.push_back( pose_vertex );
00068 return;
00069 }
00070
00071 void TimedElasticBand::addPose(double x, double y, double theta, bool fixed)
00072 {
00073 VertexPose* pose_vertex = new VertexPose(x, y, theta, fixed);
00074 pose_vec_.push_back( pose_vertex );
00075 return;
00076 }
00077
00078 void TimedElasticBand::addTimeDiff(double dt, bool fixed)
00079 {
00080 VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt, fixed);
00081 timediff_vec_.push_back( timediff_vertex );
00082 return;
00083 }
00084
00085
00086 void TimedElasticBand::addPoseAndTimeDiff(double x, double y, double angle, double dt)
00087 {
00088 if (sizePoses() != sizeTimeDiffs())
00089 {
00090 addPose(x,y,angle,false);
00091 addTimeDiff(dt,false);
00092 }
00093 else
00094 ROS_ERROR("Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
00095 return;
00096 }
00097
00098
00099
00100 void TimedElasticBand::addPoseAndTimeDiff(const PoseSE2& pose, double dt)
00101 {
00102 if (sizePoses() != sizeTimeDiffs())
00103 {
00104 addPose(pose,false);
00105 addTimeDiff(dt,false);
00106 } else
00107 ROS_ERROR("Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
00108 return;
00109 }
00110
00111 void TimedElasticBand::addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt)
00112 {
00113 if (sizePoses() != sizeTimeDiffs())
00114 {
00115 addPose(position, theta,false);
00116 addTimeDiff(dt,false);
00117 } else
00118 ROS_ERROR("Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
00119 return;
00120 }
00121
00122
00123 void TimedElasticBand::deletePose(unsigned int index)
00124 {
00125 ROS_ASSERT(index<pose_vec_.size());
00126 delete pose_vec_.at(index);
00127 pose_vec_.erase(pose_vec_.begin()+index);
00128 }
00129
00130 void TimedElasticBand::deletePoses(unsigned int index, unsigned int number)
00131 {
00132 ROS_ASSERT(index+number<=pose_vec_.size());
00133 for (unsigned int i = index; i<index+number; ++i)
00134 delete pose_vec_.at(i);
00135 pose_vec_.erase(pose_vec_.begin()+index, pose_vec_.begin()+index+number);
00136 }
00137
00138 void TimedElasticBand::deleteTimeDiff(unsigned int index)
00139 {
00140 ROS_ASSERT(index<timediff_vec_.size());
00141 delete timediff_vec_.at(index);
00142 timediff_vec_.erase(timediff_vec_.begin()+index);
00143 }
00144
00145 void TimedElasticBand::deleteTimeDiffs(unsigned int index, unsigned int number)
00146 {
00147 ROS_ASSERT(index+number<=timediff_vec_.size());
00148 for (unsigned int i = index; i<index+number; ++i)
00149 delete timediff_vec_.at(i);
00150 timediff_vec_.erase(timediff_vec_.begin()+index, timediff_vec_.begin()+index+number);
00151 }
00152
00153 inline void TimedElasticBand::insertPose(unsigned int index, const PoseSE2& pose)
00154 {
00155 VertexPose* pose_vertex = new VertexPose(pose);
00156 pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
00157 }
00158
00159 inline void TimedElasticBand::insertPose(unsigned int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
00160 {
00161 VertexPose* pose_vertex = new VertexPose(position, theta);
00162 pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
00163 }
00164
00165 inline void TimedElasticBand::insertPose(unsigned int index, double x, double y, double theta)
00166 {
00167 VertexPose* pose_vertex = new VertexPose(x, y, theta);
00168 pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
00169 }
00170
00171 inline void TimedElasticBand::insertTimeDiff(unsigned int index, double dt)
00172 {
00173 VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt);
00174 timediff_vec_.insert(timediff_vec_.begin()+index, timediff_vertex);
00175 }
00176
00177
00178 void TimedElasticBand::clearTimedElasticBand()
00179 {
00180 for (PoseSequence::iterator pose_it = pose_vec_.begin(); pose_it != pose_vec_.end(); ++pose_it)
00181 delete *pose_it;
00182 pose_vec_.clear();
00183
00184 for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
00185 delete *dt_it;
00186 timediff_vec_.clear();
00187 }
00188
00189
00190 void TimedElasticBand::setPoseVertexFixed(unsigned int index, bool status)
00191 {
00192 ROS_ASSERT(index<sizePoses());
00193 pose_vec_.at(index)->setFixed(status);
00194 }
00195
00196 void TimedElasticBand::setTimeDiffVertexFixed(unsigned int index, bool status)
00197 {
00198 ROS_ASSERT(index<sizeTimeDiffs());
00199 timediff_vec_.at(index)->setFixed(status);
00200 }
00201
00202
00203 void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_samples)
00204 {
00206 for(unsigned int i=0; i < sizeTimeDiffs(); ++i)
00207 {
00208 if(TimeDiff(i) > dt_ref + dt_hysteresis)
00209 {
00210
00211
00212 double newtime = 0.5*TimeDiff(i);
00213
00214 TimeDiff(i) = newtime;
00215 insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
00216 insertTimeDiff(i+1,newtime);
00217
00218 ++i;
00219 }
00220 else if(TimeDiff(i) < dt_ref - dt_hysteresis && (int)sizeTimeDiffs()>min_samples)
00221 {
00222
00223
00224 if(i < (sizeTimeDiffs()-1))
00225 {
00226 TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
00227 deleteTimeDiff(i);
00228 deletePose(i+1);
00229 }
00230 }
00231 }
00232 }
00233
00234
00235 double TimedElasticBand::getSumOfAllTimeDiffs() const
00236 {
00237 double time = 0;
00238
00239 for(TimeDiffSequence::const_iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
00240 {
00241 time += (*dt_it)->dt();
00242 }
00243 return time;
00244 }
00245
00246 double TimedElasticBand::getAccumulatedDistance() const
00247 {
00248 double dist = 0;
00249
00250 for(std::size_t i=1; i<sizePoses(); ++i)
00251 {
00252 dist += (Pose(i).position() - Pose(i-1).position()).norm();
00253 }
00254 return dist;
00255 }
00256
00257 bool TimedElasticBand::initTEBtoGoal(const PoseSE2& start, const PoseSE2& goal, double diststep, double timestep, int min_samples)
00258 {
00259 if (!isInit())
00260 {
00261 addPose(start);
00262 setPoseVertexFixed(0,true);
00263
00264 if (diststep!=0)
00265 {
00266 Eigen::Vector2d point_to_goal = goal.position()-start.position();
00267 double dir_to_goal = std::atan2(point_to_goal[1],point_to_goal[0]);
00268 double dx = diststep*std::cos(dir_to_goal);
00269 double dy = diststep*std::sin(dir_to_goal);
00270
00271 double dist_to_goal = point_to_goal.norm();
00272 double no_steps_d = dist_to_goal/std::abs(diststep);
00273 unsigned int no_steps = (unsigned int) std::floor(no_steps_d);
00274
00275 for (unsigned int i=1; i<=no_steps; i++)
00276 {
00277 if (i==no_steps && no_steps_d==(float) no_steps)
00278 break;
00279 addPoseAndTimeDiff(start.x()+i*dx,start.y()+i*dy,dir_to_goal,timestep);
00280 }
00281
00282 }
00283
00284
00285 if ( (int)sizePoses() < min_samples-1 )
00286 {
00287 ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
00288 while ((int)sizePoses() < min_samples-1)
00289 {
00290
00291 addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep );
00292 }
00293 }
00294
00295
00296 addPoseAndTimeDiff(goal,timestep);
00297 setPoseVertexFixed(sizePoses()-1,true);
00298 }
00299 else
00300 {
00301 ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
00302 ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
00303 return false;
00304 }
00305 return true;
00306 }
00307
00308
00309 bool TimedElasticBand::initTEBtoGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double dt, bool estimate_orient, int min_samples)
00310 {
00311
00312 if (!isInit())
00313 {
00314 addPose(plan.front().pose.position.x ,plan.front().pose.position.y, tf::getYaw(plan.front().pose.orientation));
00315 setPoseVertexFixed(0,true);
00316
00317 for (unsigned int i=1; i<plan.size()-1; ++i)
00318 {
00319 double yaw;
00320 if (estimate_orient)
00321 {
00322
00323 double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
00324 double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
00325 yaw = std::atan2(dy,dx);
00326 }
00327 else
00328 {
00329 yaw = tf::getYaw(plan[i].pose.orientation);
00330 }
00331 addPoseAndTimeDiff(plan[i].pose.position.x, plan[i].pose.position.y, yaw, dt);
00332 }
00333
00334 PoseSE2 goal(plan.back().pose);
00335
00336
00337 if ( (int)sizePoses() < min_samples-1 )
00338 {
00339 ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
00340 while ((int)sizePoses() < min_samples-1)
00341 {
00342
00343 addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), dt );
00344 }
00345 }
00346
00347
00348 addPoseAndTimeDiff(goal, dt);
00349 setPoseVertexFixed(sizePoses()-1,true);
00350 }
00351 else
00352 {
00353 ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
00354 ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
00355 return false;
00356 }
00357
00358 return true;
00359 }
00360
00361
00362 int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance, int begin_idx) const
00363 {
00364 std::vector<double> dist_vec;
00365 dist_vec.reserve(sizePoses());
00366
00367 int n = sizePoses();
00368
00369
00370 for (int i = begin_idx; i < n; i++)
00371 {
00372 Eigen::Vector2d diff = ref_point - Pose(i).position();
00373 dist_vec.push_back(diff.norm());
00374 }
00375
00376 if (dist_vec.empty())
00377 return -1;
00378
00379
00380 int index_min = 0;
00381
00382 double last_value = dist_vec.at(0);
00383 for (int i=1; i < (int)dist_vec.size(); i++)
00384 {
00385 if (dist_vec.at(i) < last_value)
00386 {
00387 last_value = dist_vec.at(i);
00388 index_min = i;
00389 }
00390 }
00391 if (distance)
00392 *distance = last_value;
00393 return begin_idx+index_min;
00394 }
00395
00396
00397 int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance) const
00398 {
00399 std::vector<double> dist_vec;
00400 dist_vec.reserve(sizePoses());
00401
00402 int n = sizePoses();
00403
00404
00405 for (int i = 0; i < n; i++)
00406 {
00407 Eigen::Vector2d point = Pose(i).position();
00408 double diff = distance_point_to_segment_2d(point, ref_line_start, ref_line_end);
00409 dist_vec.push_back(diff);
00410 }
00411
00412 if (dist_vec.empty())
00413 return -1;
00414
00415
00416 int index_min = 0;
00417
00418 double last_value = dist_vec.at(0);
00419 for (int i=1; i < (int)dist_vec.size(); i++)
00420 {
00421 if (dist_vec.at(i) < last_value)
00422 {
00423 last_value = dist_vec.at(i);
00424 index_min = i;
00425 }
00426 }
00427 if (distance)
00428 *distance = last_value;
00429 return index_min;
00430 }
00431
00432 int TimedElasticBand::findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance) const
00433 {
00434 if (vertices.empty())
00435 return 0;
00436 else if (vertices.size() == 1)
00437 return findClosestTrajectoryPose(vertices.front());
00438 else if (vertices.size() == 2)
00439 return findClosestTrajectoryPose(vertices.front(), vertices.back());
00440
00441 std::vector<double> dist_vec;
00442 dist_vec.reserve(sizePoses());
00443
00444 int n = sizePoses();
00445
00446
00447 for (int i = 0; i < n; i++)
00448 {
00449 Eigen::Vector2d point = Pose(i).position();
00450 double diff = HUGE_VAL;
00451 for (int j = 0; j < (int) vertices.size()-1; ++j)
00452 {
00453 diff = std::min(diff, distance_point_to_segment_2d(point, vertices[j], vertices[j+1]));
00454 }
00455 diff = std::min(diff, distance_point_to_segment_2d(point, vertices.back(), vertices.front()));
00456 dist_vec.push_back(diff);
00457 }
00458
00459 if (dist_vec.empty())
00460 return -1;
00461
00462
00463 int index_min = 0;
00464
00465 double last_value = dist_vec.at(0);
00466 for (int i=1; i < (int)dist_vec.size(); i++)
00467 {
00468 if (dist_vec.at(i) < last_value)
00469 {
00470 last_value = dist_vec.at(i);
00471 index_min = i;
00472 }
00473 }
00474 if (distance)
00475 *distance = last_value;
00476 return index_min;
00477 }
00478
00479
00480 int TimedElasticBand::findClosestTrajectoryPose(const Obstacle& obstacle, double* distance) const
00481 {
00482 const PointObstacle* pobst = dynamic_cast<const PointObstacle*>(&obstacle);
00483 if (pobst)
00484 return findClosestTrajectoryPose(pobst->position(), distance);
00485
00486 const LineObstacle* lobst = dynamic_cast<const LineObstacle*>(&obstacle);
00487 if (lobst)
00488 return findClosestTrajectoryPose(lobst->start(), lobst->end(), distance);
00489
00490 const PolygonObstacle* polyobst = dynamic_cast<const PolygonObstacle*>(&obstacle);
00491 if (polyobst)
00492 return findClosestTrajectoryPose(polyobst->vertices(), distance);
00493
00494 return findClosestTrajectoryPose(obstacle.getCentroid(), distance);
00495 }
00496
00497
00498
00499
00500 bool TimedElasticBand::detectDetoursBackwards(double threshold) const
00501 {
00502 if (sizePoses()<2) return false;
00503
00504 Eigen::Vector2d d_start_goal = BackPose().position() - Pose(0).position();
00505 d_start_goal.normalize();
00506
00508 for(unsigned int i=0; i < sizePoses(); ++i)
00509 {
00510 Eigen::Vector2d orient_vector(cos( Pose(i).theta() ), sin( Pose(i).theta() ) );
00511 if (orient_vector.dot(d_start_goal) < threshold)
00512 {
00513 ROS_DEBUG("detectDetoursBackwards() - mark TEB for deletion: start-orientation vs startgoal-vec");
00514 return true;
00515 }
00516 }
00517
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532 return false;
00533 }
00534
00535
00536
00537
00538 void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples)
00539 {
00540
00541
00542
00543 if (new_start && sizePoses()>0)
00544 {
00545
00546
00547 double dist_cache = (new_start->position()- Pose(0).position()).norm();
00548 double dist;
00549 int lookahead = std::min<int>( int(sizePoses())-min_samples, 10);
00550
00551 int nearest_idx = 0;
00552 for (int i = 1; i<=lookahead; ++i)
00553 {
00554 dist = (new_start->position()- Pose(i).position()).norm();
00555 if (dist<dist_cache)
00556 {
00557 dist_cache = dist;
00558 nearest_idx = i;
00559 }
00560 else break;
00561 }
00562
00563
00564 if (nearest_idx>0)
00565 {
00566
00567
00568 deletePoses(1, nearest_idx);
00569 deleteTimeDiffs(1, nearest_idx);
00570 }
00571
00572
00573 Pose(0) = *new_start;
00574 }
00575
00576 if (new_goal && sizePoses()>0)
00577 {
00578 BackPose() = *new_goal;
00579 }
00580 };
00581
00582
00583
00584
00585
00586 }