TEB local planner stuck while optimizing path

[r91andersson]

We're observing strange local path planning while sending goals that are more than 3m away. We've increased the local costmap but no change. When goals are further than 3m away, the orientation of the goal isn't taken into account while the local planner computes the trajectory. This can be seen in the images below. I've sent 3 different goals in different orientations to show that the planner doesn't take care of the orientation. Moreover, the local planner plans a very strange trajectory. Even though the goal is almost in front of the robot. The only time the local planner works if we sending goals that are no more than 3m away. We're not using any obstacles avoidance.

This problem occurs both when running in simulation and with real robot.

@croesmann, do you have any idea?

This is our local costmap config:

local_costmap:
  global_frame: /world
  robot_base_frame: /base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: true
  rolling_window: true
  width: 10.0
  height: 10.0
  resolution: 0.01
  transform_tolerance: 10
  cost_scaling_factor: 0.0
  unknown_cost_value: 253

This is the global costmap config:

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 1.0
  static_map: true
  rolling_window: false
  width: 20.0
  height: 20.0
  resolution: 0.010
  transform_tolerance: 10
  cost_scaling_factor: 0.0
  unknown_cost_value: 253
  inflation_radius: 0.0
  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

costmap_common_params config

footprint: [[0.625, 0.775], [0.625, -0.775], [-0.625, -0.775], [-0.625, 0.775]]

publish_frequency: 1.0

obstacle_layer:
  obstacle_range: 5.0
  raytrace_range: 8.5

move_base_teb config

# Planner selection
#base_global_planner: "navfn/NavfnROS"
base_global_planner: "global_planner/GlobalPlanner"

base_local_planner: "teb_local_planner/TebLocalPlannerROS"

GlobalPlanner:
  allow_unknown:           true 
  default_tolerance:       0.0   
  visualize_potential:     falsePointCloud2.
  use_dijkstra:            true  
  use_quadratic:           true  
  use_grid_path:           false 
  old_navfn_behavior:      false
  lethal_cost:             254  
  neutral_cost:            0    
  cost_factor:             0.    
  publish_potential:       true  
  orientation_mode:        0    
  orientation_window_size: 1

Teb Config:

base_local_planner: teb_local_planner/TebLocalPlannerROS

TebLocalPlannerROS:

 odom_topic: "odometry/imu_filtered"
 map_frame:  "/map"
 # Trajectory

 teb_autosize: True
 dt_ref: 0.4
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: False
 allow_init_with_backwards_motion: False
 #max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 2

 # Robot

 max_vel_x: 0.2 # 0.45
 max_vel_x_backwards: 0.15
 max_vel_y:  0.0
 max_vel_theta: 0.2 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
 acc_lim_x:  0.25
 acc_lim_theta:  0.15

 # ********************** Carlike robot parameters ********************
 min_turning_radius: 2       # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
 wheelbase: 0.625                  # Wheelbase of our robot
 cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
 # ********************************************************************

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "point"
 control_look_ahed_poses: 3
 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.5
 free_goal_vel: False

 # Obstacles
 min_obstacle_dist: 0.0 # This value must also include our robot's expansion, since footprint_model is set to "line".
 include_costmap_obstacles: False
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 30
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 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: 1000
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet

 # Homotopy Class Planner
 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False
 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

[teymurov27]

I've looked your parameters and noticed that your local_costmap global_frame is /world. I want to ask why you gave /world for global_frame. Generally it should be /odom or /map. Please try with /odom and tell me if something changes.

[lucafei]

hi, @r91andersson, did you solve this issue?