SmacPlanner hybrid can generate path that pass inside lethal area

[FrGrQuim]

Bug report

Required Info:

• Operating System:
  • Linux (custom)
• ROS2 Version:
  • Iron sources
• Version or commit hash:
  • 1.2.9
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

• Get a costmap with obsacles (static or dynamic)
• Place the robot like in the following picture:

Ask to SmacPlanner to compute a path to an accessible goal.

Expected behavior

SmacPlanner fails to compute a path or give a path that don't pass by the lethal inflation.

Actual behavior

Additional information

Planner parameters:

planner_server:
  ros__parameters:
    expected_planner_frequency: 0.1 # 10s
    planner_plugins: ["SmacPlannerHybrid"]
    SmacPlannerHybrid:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      max_iterations: -1
      max_planning_time: 30.0              # max time in s for planner to plan, smooth
      minimum_turning_radius: 1.8         # minimum turning radius in m of path / vehicle
      analytic_expansion_max_length: 2.0
      cost_penalty: 100.0
      use_quadratic_cost_penalty: True
      cache_obstacle_heuristic: True

[padhupradheep]

I think to debug it better, we might also need your costmap params.

Any by the way, are you showing the global costmap in the image above?

[FrGrQuim]

Thanks for your response.
Yes, this is the global costmap. Here are the costmap parameters:

global_costmap:
  global_costmap:
    ros__parameters:
      publish_frequency: 1.0
      update_frequency: 5.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.9 # 0.7 (real radius) + 0.2 (noise margin)
      resolution: 0.1
      track_unknown_space: False
      use_maximum: true

      plugins: ["low_obstacle_layer", "high_obstacle_layer", "obstacle_filter_layer", "fov_filter_layer", "obstacles_avoidance_inflation_layer", "static_layer", "static_inflation_layer"]
      obstacle_filter_layer:
        plugin: "harrowbot_costmap_plugins::StaticObstaclesFilterLayer"
        map_subscribe_transient_local: True
        expansion_distance: 0.3
        static_layer_name: "static_layer"
        application_boundary_expansion_distance: 3.5   # must be equal or bigger than biggest inflation distance
      fov_filter_layer:
        plugin: "harrowbot_costmap_plugins::FovFilterLayer"
        fov_base_frame: 'robot_front_link'
        fov_polygon: [0.15, 0.0, 2.15, 2.7, 4.8, 2.7, 4.8, -2.7, 2.15, -2.7]
        # publish_fov_polygon: True
        # fov_polygon_topic: /fov_polygon
        # convex_fov_polygon: True
        map_topic: "/global_costmap/costmap"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacles_avoidance_inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 1.2
      static_inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.9
      low_obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        footprint_clearing_enabled: true
        min_obstacle_height: 0.08
        max_obstacle_height: 0.30
        observation_sources: pointcloud
        pointcloud:
          data_type: PointCloud2
          topic: /cameras/pointcloud/filtered
          min_obstacle_height: -0.50
          max_obstacle_height: 3.00
          obstacle_min_range: 0.1
          obstacle_max_range: 4.0
          raytrace_min_range: 0.0
          raytrace_max_range: 4.1
          clearing: true
          marking: true
      high_obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        footprint_clearing_enabled: true
        min_obstacle_height: 0.3
        max_obstacle_height: 1.30 # robot height is ~1.20m
        observation_sources: pointcloud
        pointcloud:
          data_type: PointCloud2
          topic: /cameras/pointcloud/filtered
          min_obstacle_height: -0.50
          max_obstacle_height: 3.00
          obstacle_min_range: 0.1
          obstacle_max_range: 8.0
          raytrace_min_range: 0.0
          raytrace_max_range: 8.1
          clearing: true
          marking: true
      always_send_full_costmap: True

Unfortunately, we have two custom layers (obstacle_filter and fov_filter), but the section of the costmap traversed by the path is in our static costmap, which is added by the static_layer.
Feel free to ask me for more information if you need it.

[SteveMacenski]

I'm not quite seeing the issue that you're reporting in these details. It looks like the footprint that you're using is a footprint polygon, but your global costmap parameters show that you're using a radius footprint, so that doesn't quite line up.

If using the polygon footprint, the image you showed seems valid, there's no collision with the environment in that example. There's a nice debugging topic in the Smac Planner now that you can use to visualize the footprints of the output plan overlayed in rviz, which you can turn on with the debug_visualizations parameter https://docs.nav2.org/configuration/packages/smac/configuring-smac-hybrid.html

Note that the blue is not lethal space, its inflated space. If the centerpoint of the robot intersected with that blue space, then it would be in collision. It is perfectly fine for the edges of your robot platform / footprint to enter that area though.

[FrGrQuim]

Hi Steve,

Thanks for your response.

My main question is about understanding why the generated path (yellow line) crosses into the inflated area (no-go zone). As I understand it, the Smac planner should never return a path that goes through cells marked as INSCRIBED_INFLATED_OBSTACLE or LETHAL_OBSTACLE. However, in the image in the post, we see the path passing through the blue area, which is designated as INSCRIBED_INFLATED_OBSTACLE.
Following this, I'm not entirely clear on how the footprint is affecting the path generation. Could you provide more details on this connection?

Thanks in advance!

[SteveMacenski]

I think you mean the green line (?) :-)

the inflated area (no-go zone)

Ah, understood. You've misunderstood the intent of the inflation.

If you had a circular footprint specified using the robot_radius parameter, then the center of the circle, if ever in the inflated (blue) area, would be in collision by way that the inscribed inflated obstacle cost is set as the radius of the robot. That way, we can check a single, center cost point know with certainty of that pose is in collision with an obstacle without having to query all its edge information.

When using a non-circular footprint using the footprint parameter, which appears to be what you're using given the footprint shown although the param file you listed doesn't show that, the footprint points can enter the inflated area, that's valid. Just in the same way a circular footprint could have its edges touch the inscribed inflated cost as long as the center isn't.

For footprint collision checking, the center cost doesn't actually tell us a whole lot about its general cost status -- though that center point should also not be in collision with the inscribed, inflated cost due to ... being still inscribed / inflated. We actually use that feature as a an optimization: we calculate from the robot's footprint the furthest distance from the center of the robot specified in your footprint (rather than the smallest, inscribed) and if the cost is less than that value, then we know no point on the robot is in collision and don't do the full SE2 collision checking of the footprint's edges. If its within that boundary, then we do since its possible some point on it is in collision.

I hope that explains things clearly.

Now, in looking over your image in explaining this, I noticed that your inflation is poorly setup. You should actually be seeing this error warning in your logs about it https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/plugins/inflation_layer.cpp#L177-L185. With this not properly set to be at least the robot's largest distance, then we can't query some non-zero center point cost when in closer proximity to obstacles.

If you log / print https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/smac_planner_hybrid.cpp#L225 you should see that this cost is then zero/-1 which messes up some important optimizations in performance. But either way, wouldn't cause issues in collisions -- which your image doesn't show that the footprint is in collision at all :-)