Add prevent_look_ahead_poses_near_goal to fix oscillation

cfg/TebLocalPlannerReconfigure.cfg

@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 
+import math
 from dynamic_reconfigure.parameter_generator_catkin import *
 #from local_planner_limits import add_generic_localplanner_params
 
@@ -30,6 +31,14 @@ grp_trajectory.add("dt_hysteresis", double_t, 0,
 	"Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
 	0.1, 0.002,  0.5)
 
+grp_trajectory.add("min_samples", int_t, 0,
+	"Minimum number of samples (should be always greater than 2)",
+	3, 3, 10000)
+
+grp_trajectory.add("max_samples", int_t, 0,
+	"Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore.",
+	500, 3, 10000)
+
 grp_trajectory.add("global_plan_overwrite_orientation",   bool_t,   0,
 	"Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
 	True)
@@ -41,7 +50,11 @@ grp_trajectory.add("allow_init_with_backwards_motion",   bool_t,   0,
 grp_trajectory.add("max_global_plan_lookahead_dist",   double_t,   0,
   "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
   3.0, 0, 50.0) 
-
+
+grp_trajectory.add("global_plan_prune_distance",   double_t,   0,
+  "Distance between robot and via_points of global plan which is used for pruning",
+  1.0, 0.01, 50.0)
+
 grp_trajectory.add("force_reinit_new_goal_dist",   double_t,   0,
 	"Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
 	1.0, 0.0, 10.0)	
@@ -58,6 +71,18 @@ grp_trajectory.add("publish_feedback",   bool_t,   0,
   "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
   False)    
 
+grp_trajectory.add("min_resolution_collision_check_angular",   double_t,   0,
+  "Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad]",
+  math.pi, 0.0, math.pi)
+
+grp_trajectory.add("control_look_ahead_poses",   int_t,   0,
+  "Index of the pose used to extract the velocity command",
+  1, 1, 20)
+
+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

@@ -85,6 +85,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
@@ -248,7 +249,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

@@ -1095,7 +1095,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)
     dt += teb_.TimeDiff(counter);

src/teb_config.cpp

@@ -67,6 +67,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);
@@ -174,15 +175,21 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   trajectory.teb_autosize = cfg.teb_autosize;
   trajectory.dt_ref = cfg.dt_ref;
   trajectory.dt_hysteresis = cfg.dt_hysteresis;
+  trajectory.min_samples = cfg.min_samples;
+  trajectory.max_samples = cfg.max_samples;
   trajectory.global_plan_overwrite_orientation = cfg.global_plan_overwrite_orientation;
   trajectory.allow_init_with_backwards_motion = cfg.allow_init_with_backwards_motion;
   trajectory.global_plan_viapoint_sep = cfg.global_plan_viapoint_sep;
   trajectory.via_points_ordered = cfg.via_points_ordered;
   trajectory.max_global_plan_lookahead_dist = cfg.max_global_plan_lookahead_dist;
+  trajectory.global_plan_prune_distance = cfg.global_plan_prune_distance;
   trajectory.exact_arc_length = cfg.exact_arc_length;
   trajectory.force_reinit_new_goal_dist = cfg.force_reinit_new_goal_dist;
   trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses;
   trajectory.publish_feedback = cfg.publish_feedback;
+  trajectory.min_resolution_collision_check_angular = cfg.min_resolution_collision_check_angular;
+  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;