Rosanswers logo

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: https://answers.ros.org/question/307053/the-robot-does-not-follow-path-proper-if-goal-in-map-frame/

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

Originally posted by mysqo on ROS Answers with karma: 13 on 2019-08-16

Post score: 1

Original comments

Comment by bharatdixit16701 on 2019-11-07:
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.

Comment by mysqo on 2019-11-13:
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


1 Answer 1


Rosanswers logo

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!

Originally posted by mysqo with karma: 13 on 2019-11-13

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.