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 :-)

[SteveMacenski]

@FrGrQuim any feedback on that explaination and if this is still a concern?

[SteveMacenski]

@FrGrQuim final check in here!

[FrGrQuim]

Hi @SteveMacenski ,

Sorry for the lack of response. We've been struggling to find a way to reproduce this issue, and some more critical problems on our side have taken priority. However, we recently encountered the issue again. I will try to provide you with a rosbag today that contains as much relevant information as possible to help you understand the bug.

Regarding your response, the footprint you see in the screenshot is the one from the collision monitor, but for the planner and costmap, we use the robot radius. So unless the two are linked, I’m pretty sure the path planner is using the circular footprint.

The issue we’re facing at the application level is that we have an IsPathValid node, which triggers path recomputation when the path is not valid. The problem is that the generated path (green line, sorry for the confusion) touches the INSCRIBED_INFLATED_OBSTACLE, causing the system to continuously loop between the generated path and IsPathValid returning FAILURE (path is not valid).

[FrGrQuim]

Here is a rosbag where you can see the issue :

not_valid_path_generated.zip
On the following image you can see the path crossing the INSCRIBED_INFLATED_OBSTACLE area.

Due to this, the isPathValid condition is never met, and the newly generated path crosses it again, leading to the same behavior repeating.

[SteveMacenski]

I’m pretty sure the path planner is using the circular footprint.

That would probably be your issue then. Is it in collision with the footprint the Smac Planner knows about (i.e. global costmap)? If not, then its working as expected. If you are in confined spaces and running into issues, then I think you need to use the actual footprint, not an approximate (or use a conservative radius).

Iron sources

Also, Iron's been EOL for some time. Can you reproduce this on another distribution? It may be that we discovered and resolved this problem some time ago, but since Iron isn't an active distribution, fixes have halted on it.

[FrGrQuim]

Just to clarify, when I mention that the planner uses the circular radius, I am referring to the robot_radius.

If you are in confined spaces and running into issues, then I think you need to use the actual footprint, not an approximate (or use a conservative radius)

There are no physical collisions, don't worry. The issue is purely virtual. It arises because the SmacPlanner generates a path that is considered invalid by the IsPathValid service, which is part of the planner_server. Since both IsPathValid and the SmacPlanner are managed by the same planner_server (along with the global costmap), I assumed they share the same footprint/robot_radius and then the same rules for detecting virtual collisions.

Given this assumption, I don't understand how it is possible for a freshly generated path to be considered invalid by the planner_server that has generated it.

Regarding the distribution we use, we plan to upgrade to Jazzy, but there are higher-priority tasks that need to be completed first.

[SteveMacenski]

I assumed they share the same footprint/robot_radius and then the same rules for detecting virtual collisions.

See https://github.com/ros-navigation/navigation2/tree/main/nav2_smac_planner#potential-fields. I think you do not have a good inflation setup and you would see warnings / errors in your terminal if you had a non-EOL distribution. The inflation radius must be large enough for run-time optimizations w.r.t. cost checks against the center point of the robot. The inflation radius must be strictly greater than or equal to the robot's largest cross sectional radius (ie. so if a 10x10 cm box, it would need to be sqrt(10*10 + 10*10)/2). We need some non-zero cost to exist at the center cost of the vehicle to check if we wish to do full SE2 collision checking or not, depending if its even possible that we're in collision given that cost and our known decay function. From now looking back at your images, its clear to me that you essentially have no inflation radius setup, its perhaps only 1-2 cells wide

https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/collision_checker.cpp#L57-L65

Its likely in Iron that not only is this error not present, but the underlying checks to make sure we have a validly configured costmap inflation radius aren't performed, so its not doing or checking the right thing. Increase your inflation radius and I think you should be good to go.

P.S. Issues like this are the risk one takes by using a non-supported ROS 2 distribution - I would highly encourage upgrading. Iron stopped recieving updates in Nav2 nearly 6 months ago

[SteveMacenski]

@FrGrQuim any feedback?