Add speed filter zones to depot and warehouse maps.

[leander-dsouza]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4400
Primary OS tested on	Ubuntu
Robotic platform tested on	Turtlebot4 Gazebo Simulation
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Added namespace support for speed filter with joinWithParentNamespace

• Added a keepout_ and speed_ prefixes to related parameters.

• Added speed zone parameter to the global costmap.

• Added two zones with 30% and 60% max speed limits.

  
  

Description of documentation updates required from your changes

• N/A

Description of how this change was tested

• Tested the speed drop by monitoring the /cmd_vel_nav and /speed_limit topics in the 30% and 60% zones.

  speed_limit_depot.mp4

  As the video shows, the turtlebot starts with 0.5, which drops to 0.3 in the 60% zone, then 0.15 in the 30% max speed limit zone.

  Possible Bug?

  The issue I found was the /speed_limit topic gets triggered when entering a different costmap zone than the robot is currently in. Therefore, if a goal was given within a speed limit zone, it will ignore all the speed limits because the topic is not triggered.

  This can be seen in the above video when the second goal is given. The robot moves with full speed (0.5) in a 60% max speed zone before it enters the 30% max speed zone.

  I have attached another video which may prove to be helpful:

  speed_limit_not_respected.mp4

  Here, the robot moves at full speed despite being in a 30% max limit area. This is due to the /speed_limit topic not being triggered due to no change in the costmap.

---

Future work that may be required in bullet points

• If the intended behaviour is as described above, the documentation could be updated to include this, as it is not intuitive at the current state of the guide.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
...ller/include/nav2_controller/controller_server.hpp	100.00% <ø> (ø)
nav2_controller/src/controller_server.cpp	85.81% <100.00%> (+0.20%)	⬆️
...ostmap_2d/plugins/costmap_filters/speed_filter.cpp	97.59% <100.00%> (+0.02%)	⬆️

... and 3 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

As to the zones:

I'd like the depot to have the 60% on the other side of the shelves as well for consistency - as well as bumping that up to 75%. I'd like to make it a demo, but also not so slow that it drives me nuts 😆 You can just create 1 enveloping polygon that goes over the keepout zoner ather than 4 individual polygons. We'd actually prefer that so that if the keepout is toggled, the robot is still going at the limited speed in the shelving area globally.

Put a 50% zone then encapsilating the keepout zone at the bottom with the pallets - including the open corridor between the 2x on the left side that is still marked as not in a keepout zone.

For the warehouse:

Lower the speed to 80% around the keepout zone in the middle with some margin (like 2m in any direction around it, roughly). Also set a 80% speed limit within a 2m window around each of the i-beam pillars that are in the middle of the map so we slow down near them.

Otherwise, looks good to me, really no major notes beyond the absolute topics!

[leander-dsouza]

As to the zones:

I have updated the maps according to your suggestion :)

[leander-dsouza]

@SteveMacenski I have raised a supposed bug that the speed of the robot does not reduce if the start and goal are in the same speed zone.

Is this the supposed behaviour? (You can check the PR Description for more details) :)

[SteveMacenski]

Depot:

• Please make the 75% zone from the top/bottom of the shelves only (not to the wall)
• Please also include the graph points and areas near the shelves (shouldn't be all the way to the stairwell on the right -- also should be more on the shelf points on the left)
• Increate the 50% zone to include a bit of margin like we did in the warehouse map -- at least cover the lethal space inflating the obstacles

Warehouse:

• Looks good to me - though I think a bit more care on making sure things are aligned/centered with their features would be nice. It looks like that could be improved a bit :-)

[SteveMacenski]

The issue I found was the /speed_limit topic gets triggered when entering a different costmap zone than the robot is currently in. Therefore, if a goal was given within a speed limit zone, it will ignore all the speed limits because the topic is not triggered.

Wouldn't it have already been triggered and reduced the speed from the last request?

But that's a pretty good point - can you check in the speed filter? Maybe we should make sure we set this on the initial path request if we're in a zone (And make sure to properly reset at the end of a navigation request so we don't have dirty state between requests)

[leander-dsouza]

Depot:

Warehouse:

These are the results of the suggestions:

Depot

I have covered the graph points and have enclosed the inflation of the obstacles.

Warehouse

I have ensured the same size and translation of the speed zone across each column.

[leander-dsouza]

The issue I found was the /speed_limit topic gets triggered when entering a different costmap zone than the robot is currently in. Therefore, if a goal was given within a speed limit zone, it will ignore all the speed limits because the topic is not triggered.

Wouldn't it have already been triggered and reduced the speed from the last request?

But that's a pretty good point - can you check in the speed filter? Maybe we should make sure we set this on the initial path request if we're in a zone (And make sure to properly reset at the end of a navigation request so we don't have dirty state between requests)

This is because the speed filter only publishes limit values if the speed limit of the current zone varies from the previously stored zone. This saves computation, but will not downscale the speed if the start point is in a filter zone.
For reference, this is the conditional expression.

I have removed this check locally, and everything works as expected. I have not included this in the PR as I am not sure whether this is by design. In addition, several unittests actively test for this comparison.

[SteveMacenski]

On the zones, only 1 more thing (sorry sorry sorry): the warehouse map's big square in the center isn't aligned with the keepout zone. Is there a reason for this or should the left border be reduced by ~1m?

This saves computation, but will not downscale the speed if the start point is in a filter zone.

But if speed_limit_ == speed_limit_prev_ wouldnt that mean the current speed limit is already sent? Why isn't it being used? I think that's the crux of the issue. Between navigation requests, is it being reset by the controller but not by the costmap filter state?

[leander-dsouza]

On the zones, only 1 more thing (sorry sorry sorry): the warehouse map's big square in the center isn't aligned with the keepout zone. Is there a reason for this or should the left border be reduced by ~1m?

No worries, Steve :) I am happy to receive active feedback. I have reduced the speed zone at the central part of the warehouse map.

[leander-dsouza]

But if speed_limit_ == speed_limit_prev_ wouldnt that mean the current speed limit is already sent? Why isn't it being used? I think that's the crux of the issue. Between navigation requests, is it being reset by the controller but not by the costmap filter state?

This is because the controller_server node resets the controllers after the goal is reached or exited.
To be specific, the onGoalExit() function resets the controllers.

In order to combat this, I have saved the last speed limit and applied it to computeControl() function, as this is called whenever a new goal is in sequence.

[SteveMacenski]

Perhaps one more thing that might be good to totally button this up in a follow up PR. You introduced the idea of being able to easily launch the depot or the warehouse - but the graph is only good for the depot.

    declare_graph_file_cmd = DeclareLaunchArgument(
        'graph',
        default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'),
    )

It might be nice to have this updated to have the same naming schema for the maps / zone files you setup & a graph for that warehouse world. A tutorial for how to do is here: https://docs.nav2.org/tutorials/docs/route_server_tools.html

[leander-dsouza]

Perhaps one more thing that might be good to totally button this up in a follow up PR. You introduced the idea of being able to easily launch the depot or the warehouse - but the graph is only good for the depot.

Yes, sure, Steve.
I will add a route graph for the warehouse map in an upcoming PR.

[SteveMacenski]

I'm not sure I think this is best, the robot can be teleoped around which might not use the controller server to change this value.

I think the better solution is to publish the speed limit the first time its called for a task to update it immediately on execution. This state from the last speed limit could be very stale potentially & if we use the current pose / state instead, then we know we have current information.

For example, when we clean up a task in onGoalExit, we can reset the last speed limit in the costmap filter so that when we compute a new one on the first task, it gets immediately sent out since the old one is reset as non-existent.

[leander-dsouza]

I'm not sure how to go about this, Steve.
I do not know how to determine which costmap (global/local) would have the speed filter, in order to get a reset function for the last speed limit.
I think I might need some help on this, as I do not know how to technically get the speed filter plugin from the costmap.

[SteveMacenski]

Just spitballing:

When in the controller server we do reset of the controller / goal checker / progress checker, we could have a similar for the costmap layers to do any state cleanup at the end of a task. The implementation for all layers but the speed layer (maybe others could benefit from this?) is just empty but the speed layer we can override to reset the speed limit state. We shouldn't call it 'reset' since that's used to describe total resets already.

Another cleaner idea is to look at the controllers and see which and why the speed limit is being reset and consider not resetting that part of the state. I'm looking at the RPP controller and I don't see any reason that it should modify the last speed limit. So:

• Which planners are resetting that speed limit state?
• Is there a way to maintain it for those cases?
• Is this the best direction to go?

[leander-dsouza]

Approach - 1: Selectively reset speed filter

Yes, resetting specific layers/plugins of the costmap makes sense.
However, since the speed filter is predominantly used by the global costmap, we need another instance of it running, as by default, the local costmap is the main instance in controller_server.

Approach - 2: Set the controllers to the last known speed limit upon exit

• The following planners use setSpeedlimit():

  • rotation_shim_controller
  • regulated_pure_pursuit_controller
  • mppi_controller
  • dwb_plugins/kinematics_parameters
  • graceful_controller

  Looking at these examples, I do not think we need to reset the speed limits of the controllers.

• Inside the onGoalExit() function, we can simply set the speed of the controllers to that last known state after reset.

[SteveMacenski]

Ah good point on Approach 1.

For Approach 2: but which are doing what you mention in resetting its speed limit state when we finish the task? You mentioned that the speed limit wasn't being respected bc the speed limit layer had it still set but the controller reverted back to the original limits after reset() is called between tasks. Please find which controller(s) do that and we can see how we can fix that. For instance, I think if you test RPP, that shouldn't be resetting from my example, does it?

[leander-dsouza]

• All controllers would reset to the max speed limit once the goal is reached/reset. This is because we loop through all the controllers and reset them once we call onGoalExit().

• Since the speed limit is set only in the callback, whenever a new goal is given, it defaults to max speed. This is because the speed filter node only publishes when there is a change. Hence, all controllers would have no memory after the goal is reached/reset.

[SteveMacenski]

Look at the controllers' implementations of reset. I checked a couple and there was no adjustment of speed limit. I could only see that MPPI did from my quick scan and pretty sure RPP doesn't.

whenever a new goal is given, it defaults to max speed.

Where does that happen? If we don't publish a new speed limit bc its the same in the layer, then this isn't triggered and the state of the controllers should prevail. If the reset() doesn't change the limits, then it should persist.

[leander-dsouza]

My bad, you are correct, Steve.

Could you please point out how the MPPI resets its speed upon reset? I was not able to understand its behavior upon resetting its optimizer.

In addition, the nav2_rotation_shim_controller sets the angular velocity to maximum on reset.

I assume the next steps are to remove the resetting of the speed for different controllers.
I probably think that this could be moved to a different PR then.

[SteveMacenski]

The line here

In addition, the nav2_rotation_shim_controller sets the angular velocity to maximum on reset.

That should have nothing to do with speed limits

[leander-dsouza]

I have tested the following planners in simulation:

• nav2_graceful_controller::GracefulController
• dwb_core::DWBLocalPlanner
• nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController

All of them work if they start in a speed limit zone.
I have reverted the most recent commit of storing the last speed limit.

If I remove the resetting of the constraints in the MPPI controller, then everything works as expected.
Should I open a separate PR for this change?

[SteveMacenski]

@leander-dsouza yes please! I don't think we should always not reset, but I think when the Controller::Reset is called that's the right move to exclude it. When reset is called from an internal parameter change, I think we should reset those limits. So what we can do is have the reset take a bool for whether to reset dynamic speed limits and set it true for Controller::Reset(Optimizer::Reset) but false elsewhere