Ask Your Question

Oscillation using TEB local planner with car-like setup

asked 2019-08-16 04:42:13 -0600

mysqo gravatar image

I've come across other topics mentioning similar problems, oscillation when using the TEB local planner for a car-like robot. But nothing has helped me so far, this topic yet remains unanswered, which I think experiences the same problem:

Our robot uses a GPS, IMU and encoders for the wheels. All is fused together with robot_navigation package.

When I simulating the same robot in Gazebo everything works fine, and I do NOT see this oscillation when navigating. However, when I use the real robot, and give it a goal, it immediately turns left, and then after a while turns right, making up this snake like path.

Oscillation when navigating

I've tried multiple configurations in teb_local_planner_params. This is the current config:

    base_local_planner: teb_local_planner/TebLocalPlannerROS


#### Miscellaneous Parameters ####
  odom_topic: "odometry/imu_filtered"
  map_frame:  "/map" 

#### Robot Configuration Parameters ####
  acc_lim_x:           0.15
  acc_lim_theta:       0.3
  max_vel_x:           0.15
  max_vel_x_backwards: 0.15 
  max_vel_theta: 0.2 

# ********************** Carlike robot parameters ********************
  min_turning_radius: 1       # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
  wheelbase: 0.4525                  # Wheelbase of our robot
  cmd_angle_instead_rotvel: true 
# ********************************************************************

  max_vel_y: 0.0
  acc_lim_y: 0.0 

  footprint_model/type:         "point"     

  #### Goal Tolerance Parameters ####
  #   2.5  degrees: 0.0436332313
  #   5.0  degrees: 0.0872664626
  #   10.0 degrees: 0.174532925
  xy_goal_tolerance:  0.2
  yaw_goal_tolerance: 0.2 
  free_goal_vel:      false 

  #### Trajectory Configuration Parameters ####
  dt_ref:                            0.3 
  dt_hysteresis:                     0.1
  min_samples:                       3  
  global_plan_overwrite_orientation: true
  global_plan_viapoint_sep:         -0.1
  max_global_plan_lookahead_dist:    10
  force_reinit_new_goal_dist:        1.0   
  feasibility_check_no_poses:        4 
  publish_feedback:                  false
  shrink_horizon_backup:             true
  allow_init_with_backwards_motion:  false
  exact_arc_length:                  false
  shrink_horizon_min_duration:       10.0
  teb_autosize:                      true

  #### Obstacle Parameters ####
  min_obstacle_dist:                   0.5
  include_costmap_obstacles:           false
  costmap_obstacles_behind_robot_dist: 0.1
  obstacle_poses_affected:             1
  inflation_dist:                      0.0 

  include_dynamic_obstacles:                   false
  legacy_obstacle_association:                 false
  obstacle_association_cutoff_factor:          5.0 

#  The following parameters are relevant only if costmap_converter plugins are desired (see tutorial):
#  "costmap_converter::CostmapToPolygonsDBSMCCH"
#  "costmap_converter::CostmapToLinesDBSRANSAC"
#  "costmap_converter::CostmapToLinesDBSMCCH"
#  "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  costmap_converter_plugin:      "" 
  costmap_converter_spin_thread: true 
  costmap_converter_rate:        5.0  

  #### Optimization Parameters ####
  optimization_activate: true
  optimization_verbose: false
  no_inner_iterations:              5  
  no_outer_iterations:              4  
  penalty_epsilon:                  0.1 
  weight_max_vel_x:                 2.0 
  weight_max_vel_theta:             1.0 
  weight_acc_lim_x:                 1.0 
  weight_acc_lim_theta:             1.0 
  weight_kinematics_nh:             1000.0 
  weight_kinematics_forward_drive:  1000.0
  weight_kinematics_turning_radius: 10.0 
  weight_optimaltime:               100.0
  weight_obstacle:                  1.0
  weight_viapoint:                  1.0 
  weight_inflation:                 0.1 
  weight_adapt_factor:              2.0 

  #### Parallel Planning in distinctive Topologies ####
  enable_homotopy_class_planning:  true
  enable_multithreading:           true 
  max_number_classes:              4    
  selection_cost_hysteresis:       1.0 
  selection_obst_cost_scale:       100.0 
  selection_viapoint_cost_scale:   1.0  
  selection_alternative_time_cost: false 
  roadmap_graph_no_samples:        15
  roadmap_graph_area_width:        6.0 
  h_signature_prescaler:           1.0 
  h_signature_threshold:           0.1 
  obstacle_heading_threshold:      1.0
  visualize_hc_graph:              false 
  viapoints_all_candidates:        false
  switching_blocking_period:       0.0

This is the global costmap params:

  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 1.0
  static_map: true

  static_map: true
  rolling_window: true
  width: 25.0
  height: 25.0
  resolution: 0.010
  transform_tolerance: 10.0
  cost_scaling_factor: 0.0
  unknown_cost_value: 253
  inflation_radius: 0.0

And this is the local_costmap_params:

  global_frame: /world
  robot_base_frame: /base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: true

  static_map: false
  rolling_window: true
  width: 5.0
  height: 5.0
  resolution: 0.01
  transform_tolerance: 10.0
  cost_scaling_factor: 0.0
  unknown_cost_value: 253
edit retag flag offensive close merge delete


I am facing the same problem while using the TEB, after weeks of confusion my team find that it might be happening because of less computational power of my laptop CPU. When we trying to implement the same on i7 desktop CPU its map updating smoothly but because of some issues with networking we still can't able to run it completely on i7 but we are little bit sure its computation which update with delay which left the robot with some constant velocity for some time. Hence the robot start oscillating right and left.

bharatdixit16701 gravatar image bharatdixit16701  ( 2019-11-07 00:58:25 -0600 )edit

Yes, we faced that issue as well, when you're running the planner it gives you warnings indicating the loop time is compromised. But the main problem was actually that the GPS location on the robot and in the .xacro model didn't match. On our real robot we put in on the back of the robot, but in the simulation it was located in the center. So what we did was change the GPS and placed it in the center of the robot :) Then it begun to work! @bharatdixit16701

mysqo gravatar image mysqo  ( 2019-11-13 04:20:15 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2019-11-13 04:20:45 -0600

mysqo gravatar image

The main problem was actually that the GPS location on the robot and in the .xacro model didn't match. On our real robot we put in on the back of the robot, but in the simulation, it was located in the center. So what we did was change the GPS and placed it in the center of the robot :) Then it begun to work!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2019-08-16 04:25:52 -0600

Seen: 813 times

Last updated: Nov 13 '19