Navigation planning too near to obstacles

edit

I'm using the following configuration in my navigation stack, but as you can see in the image, the planner picks paths too close to the obstacles, so it could get stuck.

I'm using NavfnROS as global planner and DWAPlanner as local planner. I used this guide to configure my navigation, specially cost_scaling_factor (low theoretically means further from obstacles) and inflation_radious(higher means further from obstacles too). I've picked

inflation radius = 0.45, cost scaling factor = 3.0

, but changing cost_scaling doesn't help much. Higher inflation_radius improves everything, but can't go through narrow corridors.

I'll put here my configuration, but keep in mind I've used DWA as localplanner in my launch (even though TrajectoryPlannerROS is configured, it shouln't use it, right?)

Costmap_common.yaml

footprint: [[-0.26, -0.26], [-0.26, 0.26], [0.26, 0.26], [0.26, -0.26]] #Se puede aumentar para seguridad
footprint_padding: 0.01

#robot_radius: 0.26

robot_base_frame: base_footprint

update_frequency: 4.0
publish_frequency: 3.0
transform_tolerance: 10.0

resolution: 0.05

obstacle_range: 2.5
raytrace_range: 3.0

#layer definitions
static:
map_topic: /map
subscribe_to_updates: true

obstacles_laser:
    observation_sources: laser
    laser: {data_type: LaserScan, clearing: true, marking: true, topic: scan, inf_is_valid: true}

inflation:
    cost_scaling_factor: 3.0 #It was 10.0 before
    inflation_radius: 0.45 #Before 0.35

Costmap_global_static.yaml

global_frame: map
rolling_window: false
track_unknown_space: true

plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

Costmap_local

global_frame: odom
rolling_window: true

plugins:
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}
  - {name: inflation,                 type: "costmap_2d::InflationLayer"}

Planner.yaml

controller_frequency: 10.0
recovery_behaviour_enabled: true

NavfnROS:
  allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.
  default_tolerance: 0.1 # A tolerance on the goal point for the planner.

TrajectoryPlannerROS:
  # Robot Configuration Parameters
  acc_lim_x: 0.8
  acc_lim_theta: 1.67

  max_vel_x: 0.3
  min_vel_x: 0.1

  max_vel_theta: 1.0
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.2#0.4

  holonomic_robot: false
  escape_vel: -0.1

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.2
  xy_goal_tolerance: 0.3
  latch_xy_goal_tolerance: false

  # Forward Simulation Parameters
  sim_time: 2.0
  sim_granularity: 0.02
  angular_sim_granularity: 0.02
  vx_samples: 10 #Antes estaba a 6, asi puede necesitar mas procesamiento pero ira mejor
  vtheta_samples: 20
  controller_frequency: 10.0

  # Trajectory scoring parameters
  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
  pdist_scale: 0.6  #     The weighting for how much the controller should stay close to the path it was given . default 0.6
  gdist_scale: 0.8 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8

  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories
  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false
  heading_scoring_timestep: 0.8   #How far to look ahead in time ...