Oscillation using TEB local planner with car-like setup
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/3070...
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.
I've tried multiple configurations in teb_local_planner_params. This is the current config:
base_local_planner: teb_local_planner/TebLocalPlannerROS
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_costmap:
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:
local_costmap:
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
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.
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