Stephen_Z's profile - activity

I have written a full-coverage global path planner plug-in, which returns a "raster" pattern shown below. To follow this path, I am using teb_local_planner's (TEB) path following ability. When i first applied TEB, the local planner would find a local minimum at the first cut-back (turning section in the back and forth pattern) it came to and would simply stay there and oscillate. To solve this, i reduced the prune path parameter (which i found in a header file) from 1m to 0.1m. This solved the issue very well. Also, i had to be sure that TEB would never request a new global plan so i edited out all re-planning requests.

My current issue is that sometimes segments of the global plan requires the robot to go down a dead end path segment, turn, and return exactly how it came. In this scenario TEB always finds a local minimum at this path end and gets stuck. I have to bump the robot forward to get out of this situation.

Does anyone know how i can resolve this? Perhaps removing way-points within the local region (not sure how/where to do this)? or some new weight parameter? Thank you in advance.

My system is ROS Kinetic and Ubuntu 16.04.

Edit: here is my teb_local_planner configuration.

TebLocalPlannerROS:
 #################################
 # Trajectory
 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: False # I supply the desired orientation 
 global_plan_viapoint_sep: 0.01 #  there are 20 between each green point shown in rviz
 allow_init_with_backwards_motion: False
 max_global_plan_lookahead_dist: 0.7 # If too high then it cuts corners even with  via_point weight high??
 force_reinit_new_goal_dist: 0.6 # not sure about this one but lower seemed better...?
 feasibility_check_no_poses: 0 # never replan no matter what. even if completely stuck (TEB replans when desperate even with planner_frequency=0 for some reason)
 via_points_ordered: True  # path following. false increases oscillation around the path
 #################################
 # Robot     # Note: the real robot never recovers from a oscillation. However, the simulation often does.
 max_vel_x: 0.5 # these are simulation values. for the real robot they are about 3m/s 
 max_vel_x_backwards: 0.5
 max_vel_y: 0.5
 max_vel_theta: 5 
 acc_lim_x: 10.0
 acc_lim_y: 10
 acc_lim_theta: 9 
 min_turning_radius: 0.0
 #################################
 # Note: I added these parameters to help on the real robot (not needed in simulation)
 min_vel_x: 0.0 
 min_vel_y: 0.0
 min_vel_theta: 0
 min_vel_x_backwards: 0.0
 footprint_model:
   type: "point"
#################################
 # GoalTolerance
 xy_goal_tolerance: 0.1
 yaw_goal_tolerance: 0.1
 free_goal_vel: True
#################################   
 # Obstacles
 min_obstacle_dist: 0.5 # This value must also include our robot radius, since footprint_model is set to "point".
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 0.5
 obstacle_poses_affected: 30
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 #################################
 # Optimization
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 480 # these are high to encourage the real robot not to go painfully slow
 weight_max_vel_y: 480
 weight_max_vel_theta: 520
 weight_acc_lim_x: 250
 weight_acc_lim_y: 250
 weight_acc_lim_theta: 280
 weight_kinematics_nh: 5400 # My robot is holonomic but i want to prefer staying tangent to the path
 weight_kinematics_forward_drive: 550 # I definitely prefer going forward
 weight_optimaltime: 500 # I dont particularly care how long the path takes within reason but higher ...