How to configure Smac Planner or costmap for right-side navigation behavior?

[brayanpa]

Tuning / Configuration Goal

I would like the robot to prefer navigating on the right side of the path (as a person would), instead of always going through the center. The goal is to make the navigation behavior more socially acceptable and less obstructive, especially in shared environments like hallways.

What I've Tried

So far, I’ve been handling this using preferred lane costmaps, where I artificially modify the costmap to favor the right side. This works, but it is very tedious because I have to create a separate costmap for every possible path and load it depending on the path.

I also experimented with modifying the planner source code directly. Specifically, I changed the getTraversalCost() function in the Smac planner to reduce the cost of right-side movements using a condition like if (getMotionPrimitiveIndex() == 2). While this does push the planner to favor right turns, it also introduces unstable behaviors and causes zigzag motion in many scenarios.

I am now looking for a cleaner or more robust way to achieve right-side preference. My question is: Is there a recommended way to configure the planner or costmap parameters to prefer right-side navigation?

[SteveMacenski]

There are some examples of social costmap layers like https://github.com/robotics-upo/nav2_social_costmap_plugin that you might want to look into.

You could make a costmap layer with asymmetric inflation costs based on the robot's orientation (probably in the local costmap / controller frame to make this computationally efficient; but maybe could get away with it on the global costmap).

The annotation bit is also a good direction to go, though I think you can do it more automatically by making a simple program to shift all the occupied cells right/left by a certain distance and all the new cells get marked with a high but non-lethal cost. Then, just use the keepout costmap filter with that mask.

Another option that I've done with annotations in the past when I wanted the robot to not just be on the right but not just be in the center all the time is to annotate a block of cost in the middle of aisles so that the planner prefers to stick to the left / right and when it needs to move between them it does so with purpose.

Finally, you could use something like the Nav2 Route Server that is newly available in Nav2 which plans and routes on graphs that can have lanes / aisles / directions enabled explicitly :-)

I'm sure there are more options if I keep thinking, but these are the easy low-hanging fruit I can immediately come up with. Some of these would be layers / tools I'd be happy to host in the Nav2 stack itself if you were interested in contributing them since this seems like a general-purpose non-application-specific need.