search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.45k stars 1.24k forks source link

Local costmap doesn't detect obstacles untill the robot is too close to the obstacle #4299

Closed HazemAshraf11 closed 3 months ago

HazemAshraf11 commented 4 months ago

Required Info:

Steps to reproduce issue

The navigation works with the static map but when I introduce a new obstacle, the LaserScan sees it but the local costmap doesn't react until the robot is too close and it hits the obstacle. the width = 15 and height = 15 so the obstacle should be visible in the local costmap. Here are the parameters I am using. I used an obstacle layer instead of a voxel layer because for some reason I get an error saying (Sensor origin out of map bounds. The costmap can't raytrace for it.) and the local planner doesn't work. The only solution I found was to replace the voxel layer with an obstacle layer. Capture1 Capture2

Params File

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    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: 60
    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: 1.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
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 5.0
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 3.0
      yaw_goal_tolerance: 1.0
      stateful: True
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 1.5
      max_linear_accel: 10.0
      max_linear_decel: 10.0
      lookahead_dist: 3.0
      min_lookahead_dist: 0.3
      max_lookahead_dist: 3.0
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 0.1
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.05
      use_approach_linear_velocity_scaling: true
      max_allowed_time_to_collision: 1.0
      use_regulated_linear_velocity_scaling: true
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 0.9
      regulated_linear_scaling_min_speed: 0.25
      use_rotate_to_heading: false
      rotate_to_heading_min_angle: 0.1
      max_angular_accel: 0.5
      goal_dist_tol: 0.25
      cost_scaling_dist: 0.3
      cost_scaling_gain: 1.0
      inflation_cost_scaling_factor: 3.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 15
      height: 15
      resolution: 0.05
      footprint: "[ [2.5, 0.9], [2.5, -0.9], [-1.8, -0.9], [-1.8, 0.9] ]"
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 3.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 5.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [2.5, 0.9], [2.5, -0.9], [-1.8, -0.9], [-1.8, 0.9] ]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 5.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 3.0
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "robot_map2.yaml"

map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: False

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "smac_planner/SmacPlanner"
      tolerance: 0.5                    # tolerance for planning if unable to reach exact pose, in meters, for 2D node
      downsample_costmap: false         # whether or not to downsample the map
      downsampling_factor: 1            # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      allow_unknown: false              # allow traveling in unknown space
      max_iterations: -1                # maximum total iterations to search for before failing
      max_on_approach_iterations: 1000  # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
      max_planning_time_ms: 2000.0      # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      smooth_path: false                # Whether to smooth searched path
      motion_model_for_search: "DUBIN"  # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp
      angle_quantization_bins: 72       # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search)
      minimum_turning_radius: 5.0       # For SE2 node & smoother: minimum turning radius in m of path / vehicle
      reverse_penalty: 2.1              # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.20              # For SE2 node: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.05        # For SE2 node: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 20.0                # For SE2 node: penalty to apply to higher cost zones

      smoother:
        smoother:
          w_curve: 30.0                 # weight to minimize curvature of path
          w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
          w_smooth: 30000.0             # weight to maximize smoothness of path
          w_cost: 0.025                 # weight to steer robot away from collision and cost
          cost_scaling_factor: 1.0     # this should match the inflation layer's parameter

        # I do not recommend users mess with this unless they're doing production tuning
        optimizer:
          max_time: 0.10                # maximum compute time for smoother
          max_iterations: 500           # max iterations of smoother
          debug_optimizer: false        # print debug info
          gradient_tol: 1.0e-10
          fn_tol: 1.0e-20
          param_tol: 1.0e-15
          advanced:
            min_line_search_step_size: 1.0e-20
            max_num_line_search_step_size_iterations: 50
            line_search_sufficient_function_decrease: 1.0e-20
            max_num_line_search_direction_restarts: 10
            max_line_search_step_expansion: 50

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["back_up", "wait"]
    back_up:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.1
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

Expected behavior

The obstacles within the 15*15 m^2 square around the robot should be visible in the local costmap

Actual behavior

They are not visible untill the robot is too close. If they appear and the robot leaves, they become visible in the costmap within that square until the robot get far away.

tonynajjar commented 4 months ago

You said that the problem was when you use the voxel layer right? I only see the config with the obstacle layer. Can you add the problematic config?

Additionally, you're using Foxy? Foxy is end of life and not supported anymore. Can you try to reproduce it on humble or more recent?

HazemAshraf11 commented 4 months ago

The local costmap doesn't even work when I use a voxel layer, and it gives me this warning "Sensor origin out of map bounds. The costmap can't raytrace for it." When I use an obstacle layer, the obstacles don't appear in the local costmap until it's too late as you can see in the two pictures.

I am going to use a jetson nano later and it doesn't support Ubuntu 22.04 so I can't use humble.

HazemAshraf11 commented 4 months ago

I am pretty sure the problem is related to the ( obstacle layer.data source.obstacle_max_range ) parameter. It's default value is 2.5 and indeed the obstacles don't appear in the local costmap if they are more than 2.5 m far from the sensor. After they appear, they stay in this map untill the robot moves and they become out of the 15*15 m^2 square. But when I change this parameter, it doesn't do any thing. it's like it only works with the defined default value.

HazemAshraf11 commented 4 months ago

I found the problem. It's "obstacle_range" not "obstacle_max_range". In the documentation, it's "obstacle_max_range" but maybe that's for humble

padhupradheep commented 4 months ago

Yepp, you are right @HazemAshraf11

You can see the PR here: https://github.com/open-navigation/navigation2/pull/2126/files

It can be also found in foxy to galactic migration guide: https://navigation.ros.org/migration/Foxy.html

can we close the issue now @HazemAshraf11 ?