[ROS2] Navigation2 only planning short local plan

edit

Hello,

My navigation2 only plans a really short path, like 0.5 metres ahead. It can navigate to a certain spot but it kind of fails to plan around obstacles, as it only starts to plan around it when its too late. I'm using a modified version of the turtlebot3 parameters. In the picture below you can see the yellow path coming from base_link/laser_frame, thats the path it keeps planning. Also it plans much faster than 1.0Hz (from behavior tree). I tried configuring some parameters but couldn't fix it. Any thoughts?

https://imgur.com/a/WHReYK1

amcl:

  ros__parameters:
    use_sim_time: False
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 720
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 2.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator:
  ros__parameters:
    use_sim_time: False
    bt_xml_filename: "bt_navigator.xml"

dwb_controller:
  ros__parameters:
    use_sim_time: False
    debug_trajectory_details: True
    min_vel_x: 0.0
    min_vel_y: 0.0
    max_vel_x: 0.1
    max_vel_y: 0.0
    max_vel_theta: 3.0
    min_speed_xy: 0.0
    max_speed_xy: 0.1
    min_speed_theta: 0.0
    min_x_velocity_threshold: 0.001
    # Add high threshold velocity for turtlebot 3 issue.
    # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    acc_lim_x: 9999.9
    acc_lim_y: 0.0
    acc_lim_theta: 9999.9
    decel_lim_x: -2.0
    decel_lim_y: 0.0
    decel_lim_theta: -2.0
    vx_samples: 15
    vy_samples: 5
    vtheta_samples: 20
    sim_time: 1.7
    linear_granularity: 0.05
    xy_goal_tolerance: 0.25
    transform_tolerance: 0.2
    critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
    BaseObstacle.scale: 0.02
    PathAlign.scale: 0.0
    GoalAlign.scale: 0.0
    PathDist.scale: 32.0
    GoalDist.scale: 24.0
    RotateToGoal.scale: 32.0

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: False
      global_frame: odom
      plugin_names: ["obstacle_layer", "inflation_layer"]
      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
      rolling_window: true
      publish_frequency: 9.0
      width: 3
      height: 3
      resolution: 0.02
      robot_radius: 0.1
      inflation_layer.cost_scaling_factor: 3.0
      obstacle_layer:
        enabled: True
      always_send_full_costmap: True
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: False
      robot_radius: 0.1
      publish_frequency: 9.0
      obstacle_layer:
        enabled: False
      always_send_full_costmap: True
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: False
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "map.yaml"

lifecycle_manager:
  ros__parameters:
    use_sim_time: False
    autostart: True
    node_names: ['map_server', 'amcl',
                 'world_model', 'dwb_controller',
                 'navfn_planner', 'bt_navigator']

lifecycle_manager_service_client:
  ros__parameters:
    use_sim_time: False

lifecycle_manager_client_service_client:
  ros__parameters:
    use_sim_time: False

navfn_planner:
  ros__parameters:
    use_sim_time: False
    tolerance: 0.2

navfn_planner_GetCostmap_client:
  ros__parameters:
    use_sim_time: False

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

world_model:
  ros__parameters:
    use_sim_time: False