TebLocal path is very different from global path in a tight situation

[skstudying]

When I use teb to control robot, it occurs that the local path (red) is very different from global path (green). This only happends when the robot is close to obstacle or I hope it go throuth a narrow road such as fellow pic.

The attach video is also telling the same problem.

https://user-images.githubusercontent.com/62501789/198000018-77f8971b-4815-4e88-9737-04caeb3dce0e.mp4

Here is my params: Costmap_common_params: obstacle_range: 3.0 raytrace_range: 3.0

footprint: [[0.005, 0.005], [0.005, -0.005], [-0.005, -0.005], [-0.005, 0.005]]

footprint: [[0.32, 0.22], [0.32, -0.22], [-0.33, -0.22], [-0.33, -0.17], [0.11, -0.17], [0.11, 0.17], [-0.33, 0.17], [-0.33, 0.22]] footprint_inflation: 0 inflation_radius: 0 max_obstacle_height: 5.0 min_obstacle_height: 0.0 observation_sources: scan point_cloud_sensor scan: {sensor_frame: laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true} point_cloud_sensor: {sensor_frame: camera_link, data_type: PointCloud2, topic: /camera/depth/points, marking: true, clearing: true} static_layer: map_topic: /map_nav

Teb_local_planner_params: TebLocalPlannerROS:

odom_topic: odom

Trajectory

teb_autosize: True dt_ref: 0.3 dt_hysteresis: 0.1 max_samples: 500 global_plan_overwrite_orientation: False allow_init_with_backwards_motion: True #backwards when goal behind max_global_plan_lookahead_dist: 2.0 global_plan_viapoint_sep: -1 global_plan_prune_distance: 1 exact_arc_length: False feasibility_check_no_poses: 0 publish_feedback: False

Robot

max_vel_x: 0.3 max_vel_x_backwards: 0.15 max_vel_y: 0.0 max_vel_theta: 0.3 acc_lim_x: 2.5 acc_lim_theta: 3.2 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

footprint_model: type: "polygon" vertices: [ [0.32, 0.23], [0.32, -0.23], [-0.33, -0.23], [-0.33, -0.17], [0.11, -0.17], [0.11, 0.17], [-0.33, 0.17], [-0.33, 0.23] ]

GoalTolerance

xy_goal_tolerance: 0.02 yaw_goal_tolerance: 0.03 free_goal_vel: False complete_global_plan: True

Obstacles

legacy_obstacle_association: False

min_obstacle_dist: 0.01 # This value must also include our robot radius, since footprint_model is set to "point". inflation_dist: 0.01 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 0.05 obstacle_poses_affected: 15

dynamic_obstacle_inflation_dist: 0.3 include_dynamic_obstacles: False

costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5

Optimization

no_inner_iterations: 3 no_outer_iterations: 2 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.05 obstacle_cost_exponent: 4 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 # must be > 0 weight_shortest_path: 0 weight_obstacle: 100 weight_inflation: 0.2 weight_dynamic_obstacle: 10 weight_dynamic_obstacle_inflation: 0.2 weight_viapoint: 1 weight_adapt_factor: 2

Homotopy Class Planner

enable_homotopy_class_planning: False enable_multithreading: True max_number_classes: 2 selection_cost_hysteresis: 1.0 selection_prefer_initial_plan: 0.9 selection_obst_cost_scale: 100.0 selection_alternative_time_cost: False

roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 roadmap_graph_area_length_scale: 1.0 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_heading_threshold: 0.45 switching_blocking_period: 0.0 viapoints_all_candidates: True delete_detours_backwards: True max_ratio_detours_duration_best_duration: 3.0 visualize_hc_graph: False visualize_with_time_as_z_axis_scale: 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