Todo List

- Member teb_local_planner::PoseSE2::operator= (const PoseSE2 &rhs)
- exception safe version of the assignment operator
- Class teb_local_planner::TebLocalPlannerROS
- Escape behavior, more efficient obstacle handling
- Member teb_local_planner::TebLocalPlannerROS::updateObstacleContainerWithCostmap ()
Include temporal coherence among obstacle msgs (id vector)

Include properties for dynamic obstacles (e.g. using constant velocity model)

- Class teb_local_planner::TebOptimalPlanner
: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions.

: We introduced the non-fast mode with the support of dynamic obstacles (which leads to better results in terms of x-y-t homotopy planning). However, we have not tested this mode intensively yet, so we keep the legacy fast mode as default until we finish our tests.

- Member teb_local_planner::TebOptimalPlanner::AddEdgesDynamicObstacles (double weight_multiplier=1.0)
- Should we also add neighbors to decrease jiggling/oscillations
- Member teb_local_planner::TebOptimalPlanner::computeCurrentCost (double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
Remove the scaling term for the alternative time cost.

Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself?

- Member teb_local_planner::TebOptimalPlanner::getFullTrajectory (std::vector< TrajectoryPointMsg > &trajectory) const
- The acceleration profile is not added at the moment.
- Member teb_local_planner::TebVisualization::publishObstacles (const ObstContainer &obstacles) const
- Move filling of the marker message to polygon class in order to avoid checking types.
- Class teb_local_planner::TimedElasticBand
- Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class.
- Member teb_local_planner::TimedElasticBand::findClosestTrajectoryPose (const Eigen::Ref< const Eigen::Vector2d > &ref_point, double *distance=NULL, int begin_idx=0) const
implement a more efficient version that first performs a coarse search.

implement a fast approximation that assumes only one local minima w.r.t to the distance: Allows simple comparisons starting from the middle of the trajectory.

- Member teb_local_planner::TimedElasticBand::findClosestTrajectoryPose (const Eigen::Ref< const Eigen::Vector2d > &ref_line_start, const Eigen::Ref< const Eigen::Vector2d > &ref_line_end, double *distance=NULL) const
implement a more efficient version that first performs a coarse search.

implement a fast approximation that assumes only one local minima w.r.t to the distance: Allows simple comparisons starting from the middle of the trajectory.

- Member teb_local_planner::TimedElasticBand::findClosestTrajectoryPose (const Point2dContainer &vertices, double *distance=NULL) const
implement a more efficient version that first performs a coarse search.

implement a fast approximation that assumes only one local minima w.r.t to the distance: Allows simple comparisons starting from the middle of the trajectory.

- Member teb_local_planner::TimedElasticBand::updateAndPruneTEB (boost::optional< const PoseSE2 & > new_start, boost::optional< const PoseSE2 & > new_goal, int min_samples=3)
Smooth the trajectory here and test the performance improvement of the optimization.

Implement a updateAndPruneTEB based on a new reference path / pose sequence.