rst-tu-dortmund/teb_local_planner

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

skstudying opened this issue · 0 comments

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.
11

The attach video is also telling the same problem.

output.compress-video-online.com.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