Teb local planner diverged and jerking when resetting trajectory

asked 2022-11-22 05:39:42 -0600

Hello.

I'm building non-holonomic diff robot with TEB local planner.

The problem occurs when local planning trajectory is disturbed by some obstacles.

Divergence detection message occurs and warning message keep saying "Resetting planner"

I acknowlede of this sequence, however as the planner reset, robot slightly moves forward and bump to the obstacle...

I think cmd_vel is jerking and transmitting for a short time when planner is resetting as the local planner doesn't care of the costmap..

My project goal is for robot to make it keep wait until the obstacle moves away when there is no other feasible trajectory to the goal point( using move_base_flex to keep tyring forever to finish the goal ).

I want teb local planner keep resetting unless the trajectory is feasible, especially not transmitting any cmd_vel when trajectory diverged detected or not feasible(not jerking -> this is the big issue now).

What is the reason of this problem and how can I solve this problem.

Thank you sincerley

p.s. Below is my param setting.

TebLocalPlannerROS:

odom_topic: odometry/filtered map_frame: map # map

# Trajectory

teb_autosize: True dt_ref: 0.35 dt_hysteresis: 0.06 global_plan_overwrite_orientation: true # defualt : true max_global_plan_lookahead_dist: 2.0 #default 2.0 feasibility_check_no_poses: 1 global_plan_viapoint_sep: 0.4 via_points_orderd: true publish_feedback: False allow_init_with_backwards_motion: False

# Robot

max_vel_x: 0.7 # default 0.25 max_vel_x_backwards: 0.2 max_vel_theta: 0.5 # default 0.45 acc_lim_x: 0.06 # default 0.5 acc_lim_theta: 0.275 # default : 0.4 min_turning_radius: 0 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "polygon" #"line" vertices: [[-0.34, -0.24], [-0.34, 0.24], [0.14, 0.24], [0.14, -0.24]] #radius: 0.2 # for type "circular" #line_start: [-0.2, 0.0] # for type "line" #line_end: [0.2, 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"

# GoalTolerance

xy_goal_tolerance: 0.65 yaw_goal_tolerance: 0.65 trans_stopped_vel: 0.2 theta_stopped_vel: 0.2 free_goal_vel: False complete_global_plan: False # what is this for??
# Obstacles

min_obstacle_dist: 0.1 # This value must also include our robot radius, since footprint_model is set to "point" or "line". inflation_dist: 1.0 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 #default : 1.0 obstacle_poses_affected: 100 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5 # default : 5

#Reduce_velocity_near_obstacles: obstacle_proximity_ratio_max_vel: 0.85 obstacle_proximity_lower_bound: 0.45 obstacle_proximity_upper_bound: 0.2 # does it mean

# Optimization

no_inner_iterations: 5 no_outer_iterations: 4 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.05 weight_max_vel_x: 2 weight_max_vel_theta: 2 weight_acc_lim_x: 100 weight_acc_lim_theta: 2 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 950 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 10 weight_via_point: 10

weight_dynamic_obstacle: 10 # not in use yet

alternative_time_cost: False # not in use yet

# Homotopy Class Planner

enable_homotopy_class_planning: False # I turned it of because of #[ WARN] [1654158502.701863979]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1477 seconds enable_multithreading ... (more)

edit retag flag offensive close merge delete