Robot stops very often, even on straight paths with no obstacles around.

[subodh-malgonde]

I have a car-like robot. It stops very frequently even when there are no obstacles around and the path is straight. See the below video to visualize this stuttering behaviour.

Click on the image to play the video. Redirects to YouTube

I have set the move_base controller_frequency to 10 Hz. I have also set enable_homotopy_class_planning to False, so that it consumes less processing power. These are my params:

TebLocalPlannerROS:

 odom_topic: /vesc/odom
 map_topic: /map

 ########## Trajectory ##########
  
 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 4
    
 ########## Robot ##########
         
 max_vel_x: 1.5
 max_vel_x_backwards: 0.7
 max_vel_y: 0.0
 max_vel_theta: 2.5 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
 acc_lim_x: 6.0
 acc_lim_theta: 4.0

 # ********************** Carlike robot parameters ********************
 min_turning_radius: 0.85        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
 wheelbase: 0.34                 # Wheelbase of our robot
 cmd_angle_instead_rotvel: False # stage simulator takes the angle instead of the rotvel as input (twist message)
 # ********************************************************************

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "polygon"
   vertices: [ [-0.28,-0.15], [0.28,-0.15], [0.28,0.15], [-0.28, 0.15] ]

 ########## GoalTolerance ##########
    
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1

 # Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed
 free_goal_vel: False
    
 ########## Obstacles ##########
    
 min_obstacle_dist: 0.15 # This value must also include our robot's expansion, since footprint_model is set to "line".
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0

 # Buffer zone around obstacles with non-zero penalty costs (should be larger than
 # min_obstacle_dist in order to take effect). Default is 0.6.
 inflation_dist: 0.10
 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: 10.0
 weight_max_vel_theta: 10.0
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 10
 weight_kinematics_turning_radius: 50
 weight_optimaltime: 1
 weight_obstacle: 50

 ########## Homotopy Class Planner ##########

 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 2
 selection_cost_hysteresis: 1.0
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

[adamkrawczyk]

Hi, I have the same problem, teb starts to operate at 0.2Hz instead of ~20 and robot stops while driving. Have you found any solution?

[cuongng313]

Hi, have you solved the problem ? I tried to increase the penalty_epsilion so that it is equal or little bit bigger than the acc_lim_x and it worked. In my case, penalty_epsilon: 0.3 and max_lim_x: 0.3. But I received the warning so I think it is not the optimal solution.

[adamkrawczyk]

I found that this might be connected to computation power. Thank you for reply I will try this

[adamkrawczyk]

I've checked with more cpu-s and the same problem. Setting penalty_epsilon also didn't solve the issue

[cuongng313]

Have you tried to reduce the acceleration of x-axis to the value being smaller than penalty_epsilon ? I set all the accelerations so that they are little bit smaller than the penalty value and it worked.

[adamkrawczyk]

It also didn't work. Actually my case might be a little different, a robot has software stop when no cmd_vel for longer than 1s, when the robot is driving then the planner frequency drops and perform one loop for 2-8s which looks like it's stopping. In simulation I've discovered that it actually not stopping.