search

ros-navigation / navigation2

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

Global Planner can't generate path for the goal given in unknown space with SLAM #3992

Closed Rak-r closed 11 months ago

Rak-r commented 11 months ago

Question

Required Info:

Expected behavior

When setting the goal to an undetected space, Smac A* Hybrid planner should generate the path

Actual behavior

Global planner Failed to generate path

Additional information

I am trying to setup NAV2 for physical robot with online mapping and localization. But first testing in simulation. After launching the slam toolbox online async with mapping mode and NAV2 everything starts good. I set the goal to some arbitrary point and planner fails to generate the path and after that behaviour server starts with backup. Once backup done some part of map is updated and then if I set the goal inside the white area (global costmap) it starts to generate global plan.

I tried changing the costmap params but no progress. What I want to know is that if I give goal point, isn't the global planner should be able to generate the path with having info from global costmap.? What's causing this issue to happen. It might be something trivial I ham having but looked for configuration guides and other NAV2 default repos. Still getting the same query.

To update the map with slight travel, I tweaked minimum_travel_distance and minimum_travel_heading params in slam_toolbox. It works but still I have to give goal to white map area.

I visualised that global and local costmaps are generated. I am using simulated lidar sensor and also having depth cam setup. I tried to changing the sensor ranges as well. The other issue comes up in terminal log in both sensor modalities is:

[planner_server]: GridBased: failed to create plan, invalid use: Starting point in lethal space! Cannot create feasible plan. planner_server-2] [WARN] [1700588297.287430276] [nav2_costmap_2d]: Robot is out of bounds of the costmap! [planner_server-2] [WARN] [1700588297.287468315] [global_costmap.global_costmap]: Sensor origin at (-0.34, 0.00) is out of map bounds (-0.34, 0.00) to (214748367.63, 214748367.97). The costmap cannot raytrace for it.

First video shows the failed path generation

https://github.com/ros-planning/navigation2/assets/85680564/2631564b-0d10-4233-b3c2-4261090b75d9

Second video for small goal in the space and starts to map

https://github.com/ros-planning/navigation2/assets/85680564/c6ad2d03-c47a-4c24-b9bd-cbdbc25f3a6e

NAV2 params for Gloabl and local costmaps and planner server

` 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: False rolling_window: true width: 5 height: 5 resolution: 0.05 #0.25 footprint: "[[-0.6, 0.285475], [-0.6, -0.285475], [0.6, -0.285475], [0.6, 0.285475]]"

robot_radius: 0.5

  plugins: ["obstacle_layer", "inflation_layer"]
  inflation_layer:
    plugin: "nav2_costmap_2d::InflationLayer"
    cost_scaling_factor: 1.0
    inflation_radius: 0.3 #0.55
  obstacle_layer:
    plugin: "nav2_costmap_2d::ObstacleLayer"
    enabled: True
    observation_sources: scan
    scan:
      topic: /scan
      max_obstacle_height: 3.0
      clearing: True
      marking: True
      data_type: "LaserScan"
      raytrace_max_range: 10.0
      raytrace_min_range: 0.3 #1.0
      obstacle_max_range: 10.0 #1.0
      obstacle_min_range: 1.0
    plugin: "nav2_costmap_2d::StaticLayer"
    map_subscribe_transient_local: True
  always_send_full_costmap: True

local_costmap_client: ros__parameters: use_sim_time: False local_costmap_rclcpp_node: ros__parameters: use_sim_time: False '

global costmap configurations

` global_costmap: global_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: map robot_base_frame: base_link use_sim_time: false footprint: "[[-0.6, 0.285475], [-0.6, -0.285475], [0.6, -0.285475], [0.6, 0.285475]]"

robot_radius: 0.5

  resolution: 0.05 #0.25 #0.05
  track_unknown_space: true
  unknown_cost_value: 120
  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: 3.0
      clearing: True
      marking: True
      data_type: "LaserScan"
      raytrace_max_range: 10.0
      raytrace_min_range: 0.3 #1.0
      obstacle_max_range: 10.0 #1.0
      obstacle_min_range: 1.0
  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: 0.3 #0.55
  always_send_full_costmap: True

global_costmap_client: ros__parameters: use_sim_time: False global_costmap_rclcpp_node: ros__parameters: use_sim_time: False '

` planner_server: ros__parameters: planner_plugins: ["GridBased"] use_sim_time: False

GridBased:
  plugin: "nav2_smac_planner/SmacPlannerHybrid"
  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)
  tolerance: 1.0 # 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
  allow_unknown: true
  cost_scaling_factor: 0.9
  neutral_cost: 50                 # allow traveling in unknown space
  max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
  max_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
  max_planning_time: 10.0              # max time in s for planner to plan, smooth
  motion_model_for_search: "REEDS_SHEPP" #"DUBIN"    # Hybrid-A* DUBIN, REEDS_SHEPP
  angle_quantization_bins: 72         # Number of angle bins for search
  analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
  analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
  minimum_turning_radius: 0.8 #0.5       # minimum turning radius in m of path / vehicle
  reverse_penalty: 1.4                # Penalty to apply if motion is reversing, must be => 1
  change_penalty: 0.3 # 0.1                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
  non_straight_penalty: 1.2           # Penalty to apply if motion is non-straight, must be => 1
  cost_penalty: 2.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
  retrospective_penalty: 0.015
  lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
  cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
  smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path
  viz_expansions: True                   

  smoother:
    max_iterations: 1000
    w_smooth: 0.3
    w_data: 0.2
    tolerance: 1.0e-10
    do_refinement: true
    refinement_num: 2

planner_server_rclcpp_node: ros__parameters: use_sim_time: False `

Implementation considerations

I have asked something similar in robotics stack exchange : https://robotics.stackexchange.com/questions/105576/nav2-and-slam-toolbox-without-prior-map?noredirect=1#comment42765_105576

It would great, if someone could guide what I am lacking and suggest some insights.

SteveMacenski commented 11 months ago

First video

It should be pretty clear why that failed; you're sending it a command clearly off of the map. That's not unknown space, that's space that's not even mapped to be even unknown, its out of bounds.

Second video

Your resolution of sensor is too low it seems. We do raycasting to clear out cells, so if your resolution is too low, there aren't enough "free" measurements in a given probablistic cell to mark it as free yet. When you move the robot, it triggers another update and thus more "free" measurements from a call come along to expand the space. The cells directly around the robot are marked the first time because the low-resolution is enough to hit the free threshold within that overlapping space only.

This is not a Nav2 issue but a SLAM issue with your sensor, use a higher resolution sensor to have more dense beams to be able to clear out more cells as unoccupied in a given measurement snapshot and/or use a depth camera SLAM instead of a lidar SLAM that models your sensor's limitations more accurately. SLAM Toolbox is built for professional grade lidars, typically ~0.5 degree resolution or higher