Add prevent_look_ahead_poses_near_goal to fix oscillation

Never look ahead to the last few poses of the plan in order to prevent
oscillating forward and backward over the goal when the
xy_goal_tolerance is small. Specified by new parameter
prevent_look_ahead_poses_near_goal whose default is zero.

cfg/TebLocalPlannerReconfigure.cfg

@@ -66,6 +66,10 @@ grp_trajectory.add("control_look_ahead_poses", int_t, 0,
   "Index of the pose used to extract the velocity command",
   1, 1, 100)     
 
+grp_trajectory.add("prevent_look_ahead_poses_near_goal",   int_t,   0,
+  "Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small",
+  0, 0, 20)
+
 grp_trajectory.add("visualize_with_time_as_z_axis_scale",    double_t,   0,
   "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
   0, 0, 1)

include/teb_local_planner/teb_config.h

@@ -86,6 +86,7 @@ class TebConfig
     bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
     double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad]
     int control_look_ahead_poses; //! Index of the pose used to extract the velocity command
+    int prevent_look_ahead_poses_near_goal; //! Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small
   } trajectory; //!< Trajectory related parameters
 
   //! Robot related parameters
@@ -262,7 +263,8 @@ class TebConfig
     trajectory.publish_feedback = false;
     trajectory.min_resolution_collision_check_angular = M_PI;
     trajectory.control_look_ahead_poses = 1;
-
+    trajectory.prevent_look_ahead_poses_near_goal = 0;
+
     // Robot
 
     robot.max_vel_x = 0.4;

src/optimal_planner.cpp

@@ -1148,7 +1148,7 @@ bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega
     omega = 0;
     return false;
   }
-  look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
+  look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1 - cfg_->trajectory.prevent_look_ahead_poses_near_goal));
   double dt = 0.0;
   for(int counter = 0; counter < look_ahead_poses; ++counter)
   {

src/teb_config.cpp

@@ -68,6 +68,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
   nh.param("publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback);
   nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular);
   nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses);
+  nh.param("prevent_look_ahead_poses_near_goal", trajectory.prevent_look_ahead_poses_near_goal, trajectory.prevent_look_ahead_poses_near_goal);
 
   // Robot
   nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
@@ -198,6 +199,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses;
   trajectory.publish_feedback = cfg.publish_feedback;
   trajectory.control_look_ahead_poses = cfg.control_look_ahead_poses;
+  trajectory.prevent_look_ahead_poses_near_goal = cfg.prevent_look_ahead_poses_near_goal;
 
   // Robot     
   robot.max_vel_x = cfg.max_vel_x;