TEB Local Planner jerky in negative x direction

edit

When I issue the robot command goal to go forward in the map in real life, the robot is able to do so smoothly and quickly. There is a bit of a "heading hunting" issue but we can tune for that. However, when we request the robot to travel in the negative x-direction, it rotates to face the goal, but then the motion is extremely jerky and unsure. Anyone came across such an issue before?

This is my TEB_params

TebLocalPlannerROS:

 odom_topic: /caato_0/odom
 map_frame: caato_0/odom

 # Trajectory
 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: False
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5
 publish_feedback: True

 # Robot
 max_vel_x: 0.4
 max_vel_x_backwards: 0.4
 max_vel_theta: 0.3
 acc_lim_x: 0.25
 acc_lim_theta: 0.5
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "polygon"
   # line_start: [-0.3, 0.0] # for type "line"
   # line_end: [0.3, 0.0] # for type "line"
   # front_offset: 0.2 # for type "two_circles"
   # front_radius: 0.2 # for type "two_circles"
   # rear_offset: 0.2 # for type "two_circles"
   # rear_radius: 0.2 # for type "two_circles"
   # vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
   vertices: [ [0.3, 0.28], [0.3, -0.28], [-0.5, -0.28], [-0.5, 0.28] ]

 # GoalTolerance
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
 complete_global_plan: True

 # Obstacles
 min_obstacle_dist: 0.25
 inflation_dist: 0.1
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 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: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 selection_alternative_time_cost: False # not in use yet

 # Homotopy Class Planner
 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 2
 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

# Recovery
 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10