1
$\begingroup$

Problem Description:

I am working on an autonomous navigation project for an agricultural truck that needs to move smoothly and predictably on dynamically defined virtual lanes. The navigation is guided by GPS waypoints that can vary according to the field in which the truck operates. My goal is to ensure that the planner computes paths only within these virtual lanes, avoiding areas outside of them.

Currently, I am using the SMAC planner from the Navigation2 Stack and have explored several approaches, but still face challenges:

Approaches Used: Generating Virtual Lanes:

I use a script (visualize_lanes_from_gps_and_wps.py) to generate virtual lanes (center, left, and right) with visualization markers based on GPS waypoints.

Virtual_Highway

I also created a script (lane_costmap_generator.py) to generate costmaps over these lanes, assigning different cost values to each one to try and guide the planner to prefer the desired lane.

Costmap lanes

1 - Problems Encountered:

  • The planner does not restrict itself to the generated virtual lanes. As shown in the attached images and videos, the planner path (red line) "escapes" from the virtual lanes and expands outside of them, using the standard (global/local) costmaps from the Navigation2 Stack.

violating lanes boundary

  • I tried using virtual "walls" generated by point clouds (PCD2). Still, since there are no real physical obstacles, the Nav2 layers (such as voxel and obstacle) do not generate real occupancy "felt" by the planner, causing it to ignore these "walls."

Ignore Walls PCD2 wall

2-Attempted Solutions:

  • Binary Masks Filters: I applied cost filters using binary masks, adjusting values to make certain lanes more permissive (value 0) or more restrictive (value 100). However, the planner does not prioritize one lane over another and continues expanding outside the virtual lanes regardless of the configured cost values.

What I Need to Know: What is the best way to ensure that the SMAC planner is restricted only to the virtual lanes?

  • How can I configure the Navigation2 Stack to ensure that the planner expands primarily within the costmaps generated for the virtual lanes and avoids or minimizes expansion into the local and global costmaps?

  • If using keepout zones is appropriate, is it possible to configure them dynamically to consider everything outside the virtual lanes as a keepout zone? If so, how could this be implemented?** Or just manually as tutorial image below:

enter image description here

Secondary Question:

Additionally, I am using a script (follow_dynamic_gps_wps_lanes.py) that dynamically changes the GPS waypoints to another lane when an obstacle is detected in the current lane. The problem is that the planner does not correctly recalculate the path for these new waypoints. It tries to follow all the waypoints shifted to another lane, including those already behind the truck, causing it to turn back instead of following only the points ahead.

What I Have Tried:

I have tried using methods such as preempt, cancelNav, clearCostmap, but the planner still tries to recalculate the route from the beginning instead of continuing only with the waypoints ahead of the truck.

displace to other lane

Question:

How can I adjust the path calculation to consider only the shifted side-lanes waypoints ahead of the truck, avoiding unwanted returns?

A video describing the planner "violating" virtual lanes boundaries and above issues can be checked in this link Shifting to side lane and boundaries restriction fails

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

I believe what you are looking for is Nav2 Plugin/Costmap Filters/Keepout Filter

which does pretty much what you expect. You can define "No-Go-Zones" with this additional Plugin you can simply activate within standard Nav2 parameters yaml.

With it, you can define zones as if there was a static obstacle at this position, and the planner will act accordingly. But SLAM/AMCL will not try to find those obstacles to match lidar-points.

Define your virtual lanes with thin obstacles left&right and you should be golden.

But Keepout-zones are supposed to be static (e.g. avoid stairs). If you want a dynamic solution, use a custom cost map layer tutorial

Improve this answer
$\endgroup$
2
  • 1
    $\begingroup$ Hi thanks for the attention. I have already tried to customize the keepout filter source code, to receive a lethal custom costmap that I have done (costmap with lethal cells around-outside the lane), but did not work. As you said for static PGM map mask it works. I think you are correct in trying to create a custom layer (derived from voxel layer or obstacle layer, i.e) to receive this custom keepout_zone costmap I've created (areas outside the virtual lanes). That's the idea right? Have you already created a custom layer in ROS2? Does it work and how easy is doing this? Thanks $\endgroup$
    – Marcus Vinicius
    Commented Sep 4 at 9:44
  • $\begingroup$ Some students I supervised have created custom layers in the context of their master thesis. Took them a couple weeks with a couple months of ROS2 experience. Nothing I would dare to publish though, as it is very unstable. But there exist tutorials - one of which I have linked as "tutorial" in my answer. $\endgroup$
    – Scoeerg
    Commented Sep 4 at 11:33

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.