hamedsamie commented 7 years ago

Hi,

I am new to TEB_local_planner. I am using TEB_local_planner for navigating a robot from point A to point B (and I have a global path via global_planner). The problem is that when robot faces the obstacle in front of itself it does not find the "currect" trajectory and it blocks with the warning "trajectory not feasible".

What is strange to is that if I try to block the robot from sides of the robot (left or right) it easily changes its trajectory nd achives to avoid the obstacle.

Here also you can find my TEB local planner parameters. could you please help me to find where is the origin of this issue?

Thanks in advance,

`TebLocalPlannerROS:

odom_topic: /odom/filtered map_frame: map

Trajectory

teb_autosize: True dt_ref: 0.15 dt_hysteresis: 0.1

global_plan_viapoint_sep: 1.0

global_plan_overwrite_orientation: True max_global_plan_lookahead_dist: 5.0 force_reinit_new_goal_dist: 0.5 feasibility_check_no_poses: 5 shrink_horizon_backup: True

Robot

max_vel_x: 1.0 max_vel_x_backwards: 0.2 max_vel_theta: 0.2 acc_lim_x: 0.2 acc_lim_theta: 0.05 min_turning_radius: 0 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"

type: "two_circles"

type: "polygon"
#radius: 0.7 # for type "circular"
#line_start: [-0.3, 0.0] # for type "line"
#line_end: [0.3, 0.0] # for type "line"
#front_offset: 0.425 # for type "two_circles"
#front_radius: 0.5 # for type "two_circles"
#rear_offset: -0.425 # for type "two_circles"
#rear_radius: 0.5 # for type "two_circles"
vertices: [ [0.725, -0.4], [0.725, 0.4], [-0.725, 0.4], [-0.725, -0.4] ] # for type "polygon"
#vertices: [[1.375, -0.4], [1.375, 0.4], [-0.725, 0.4], [-0.725,-0.4]]

GoalTolerance

xy_goal_tolerance: 0.5 yaw_goal_tolerance: 0.15 free_goal_vel: False

Obstacles

min_obstacle_dist: 0.5 #it was 0.9 by Patrick inflation_dist: 0.6 obstacle_association_force_inclusion_factor: 4 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 2.0 obstacle_poses_affected: 30 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" costmap_converter_spin_thread: True costmap_converter_rate: 5

Optimization

no_inner_iterations: 3 no_outer_iterations: 3 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.01 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: 10 weight_kinematics_turning_radius: 0 weight_optimaltime: 400 weight_obstacle: 50 weight_via_point: 100 # New in version 0.4 weight_dynamic_obstacle: 20

Homotopy Class Planner

selection_alternative_time_cost: True enable_homotopy_class_planning: False enable_multithreading: True simple_exploration: False max_number_classes: 2 selection_cost_hysteresis: 1.0 roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False`

hpaugam33 commented 6 years ago

Hi, I'm using Ros Kinetic and this version of TEB. I've exactly the same issue and I think it's due to the footprint type "polygon" because all others types are working just fine. Have you clear this issue now ?

croesmann commented 6 years ago

Hi, how large is your obstacle? Do you also block the current goal of the local planner? In that case, the feasibility check fails I guess. A short video would be very helpful to understand the issue.

sunilsulania commented 6 years ago

I tried waypoint switching without stopping with TEB. I have seen this happening when TEB is given a goal closeby (close enough to have teb local plan directly to goal pose) which is covered with obstacle/obstacle inflation. Failing feasibility check and holding previously held non-zero value in cmd_vel crashes robot into obstacle in my version. After verifying that velocities are not updated to zero when feasibility fails, I updated velocities to zero in move_base whenever this happens to avoid crashing. Just a small workaround but did not get time to really find why this is happening. Hope this helps to find and recreate this issue.