Add a service for enabling/disabling the collision monitor

[armgits]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5463
Primary OS tested on	Ubuntu
Robotic platform tested on	Turtlebot 3 Loopback Simulation
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Added a service to globally enable/disable collision monitor
• Added a new Toggle interface in nav2_msgs that is similar to SetBool from std_srvs

Description of documentation updates required from your changes

Mentioning this interface in the Collision Monitor Node documentation if needed.

Description of how this change was tested

• This change was tested in TB3 loopback simulation with the service requests sent from CLI. Upon toggling off the collision monitor, robot doesn't slowdown/stop when the polygons overlap with the collision points on the map.

---

Future work that may be required in bullet points

• Implementing behavior tree node by @DavidG-Develop
• Any code quality improvements since this is my first time!

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
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

Also check out the failing linting and DCO CI jobs and resolve their issues please!

How did you test these changes?

[DavidG-Develop]

Although this solution seems like it should also work by disabling and enabling all polygons using this service I feel like a simpler solution could be a better one... Currently you must set a parameter for each polygon that is set up and if we have many polygons that could unnecessarily increase time needed for the service call execution... Also now you set the enabled param for all polygons to true or false but it could be very possible that some of those polygons should not be enabled as they were also disabled before the service call to toggle off.

I would propose the following:

• Keep the service server set up as is
• Change the service topic name to use "~/" notation for namespace resolution
• In the service callback just set the enabled_ to true (+ log that the collision monitor is now on/of or has been enabled/disabled)
• in the cmdVelInCallbackStamped do something simmilar to:

void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::SharedPtr msg)
{
  // If message contains NaN or Inf, ignore
  if (!nav2_util::validateTwist(msg->twist)) {
    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
    return;
  }
  if (enabled_){
    process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}, msg->header);
  } else {
    // Republish the message
    Action robot_action{DO_NOTHING, {msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}, ""};
    publishVelocity(robot_action, msg->header);
  }
}

This way the node acts simply as a relay for cmd vel taking it from input and publishing it on output witouth doing any additional computation and not generating the polygons.

This change requires to be tested which I could not do it on my machine. Please provide any feedback from testing.

If your PC is not powerfull enough to run the sim you can firstly try running gazebo headless to reduce the load, secondly you can try the Loopback simulator which can be used so simple testing.

[armgits]

I'm working on the changes/fixes suggested above by Steve, but your solution looks pretty good too. I'm not sure if the process() method could be entirely skipped.

[SteveMacenski]

Something nice that running the full function does is still exercise this condition to check of sensor data is not updating correctly. Even if we disable the collision element of things, stopping due to hardware issues seems still useful. But, that could be abstracted into a lambda or function and used both here and in the else condition.

@armgits can you make this change if you agree that there's nothing else in the process we should ensure we do?

unnecessarily increase time needed for the service call execution

I do also generally agree with that. Instead, we could just update the enabled for each polygon with a new API instead of using dynamic parameter callbacks. But moot if we change the enabled globally

This change requires to be tested which I could not do it on my machine. Please provide any feedback from testing.

Why are you unable to test on your machine? If you have the compute resources to compile Nav2, you probably have all you need to simulate it too

[SteveMacenski]

@armgits any update?

[armgits]

I'm still working and here are some updates so far:

@armgits can you make this change if you agree that there's nothing else in the process we should ensure we do?

I made this change locally but some unit tests are failing after extracting the check condition into a function which I'm looking into. I noticed that the process() function returns if the process_active_ flag isn't set. I think this applies even in disabled state right (to check sensor data and publish velocity)?

I was also thinking if we could have separate process() methods for enabled and disabled states like processEnabledState() and processDisabledState() to keep things clean in the subscription callback.

Why are you unable to test on your machine? If you have the compute resources to compile Nav2, you probably have all you need to simulate it too

This has been slowing me down. Gazebo isn’t running properly on either of my laptops (even in headless mode), so I’ve been testing with loopback simulation instead. Based on my testing with the old logic after sorting that out last week, I can confirm the service interface responded correctly to CLI requests, but haven't seen any polygons published or changes in robot's behavior with enabled/disabled states. (I'm testing with this tutorial Using Collision Monitor)

[SteveMacenski]

I noticed that the process() function returns if the process_active_ flag isn't set.

That flags is for the lifecycle state transition, that it can process data because the node is in the Activate state to process data, versus in the process of being deactivated or configuring or so on.

I think right here, here, and here you could just add a check for enabled_. If not enabled, it continue or not publish the polygon or not log changes. But, it should still then do the checks on stopping due to sensor stoppages which I think is still crucial.

---

What's up with gazebo? You can also try the loopback simulator instead, we have launch files using those in the nav2_bringup package.

[armgits]

I have completed adding the new changes, tested them successfully in loopback simulation, passes all unit tests and is linted. While I ensured to sign off new commits, I want to sign off the old commits too and push the changes.

For now, I just tried rebasing my local branch from the commit just before my first one last week, leaving everything else as it is upon which I made new commits. Based on this:

Is this okay?

[SteveMacenski]

Sure! You could always just commit here and then pull in / rebase on main afterwards. We squash merge all PRs anyway so not having the cleanest git commit history is not a big deal (to me at least; we have so much more to focus on 😆 ) since it'll all get squashed into a single commit onto main. Just look out for DCO and if it doesn't pass, look at the job and just do what the instructions there tell you to do.

[SteveMacenski]

@armgits something's gone wrong here, you have included all the recent commits from the Nav2 branch. That is not correct and the diff shows them in this PR versus only your changes.

[armgits]

I tried to sign off only my commits and leave the following commits untouched first but the DCO didn't pass that way, so I had to sign off all the following commits too.

I synced my branch with the main branch that brought in all the new commits, was that the mistake?

[armgits]

I can roll back my branch to just before I made my first commit and reapply just my commits from there. That will not include other commits and diff would show only my changes.

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
...2_collision_monitor/src/collision_monitor_node.cpp	97.18% <100.00%> (+0.08%)	⬆️

... 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]

Yeah I'd grab your changes and many cherry pick them onto a clean branch or otherwise roll back to a state without all these excess commits and try again. You don't need to rebase beyond what's needed for DCO which shouldn't be pulling in changes

[SteveMacenski]

You missed one for notifyActionState

[mergify]

@armgits, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[armgits]

After checking CircleCI console, the build stage keeps timing out after an hour. In the previous tests where this was successful, only the collision monitor package was selected to build but now a lot more, if not all the packages are built again which is taking a lot of time.

[SteveMacenski]

Got it - I just retriggered and with the cache that should build.

Let me know when you want me to to review again!

[armgits]

Please review it today and let me know! The only thing I'd note is the waitToggle function in test body... It's duplicate code from another wait function and was wondering if that duplication could be reduced.

[SteveMacenski]

Did you test this after this change to make sure you see it functionally working?

LGTM!

Would you be interested in contributing some Behavior Tree Nodes in in order to use this feature from within behavior trees? You'd get your name here: https://docs.nav2.org/plugins/index.html#behavior-tree-nodes 😉 EnableCollisionMonitor and DisableCollisionMonitor to call the service.

Please add to the Kilted migration guide this new feature for the collision monitor so the community knows as they upgrade!

[armgits]

@SteveMacenski yes, I did test the change and can confirm that it functionally works. I've updated the testing part of it in the PR description. And I am very much interested in further working on this feature! Can I also pick other issues while this one's ongoing?

[DavidG-Develop]

@SteveMacenski @armgits I have the BT plugin already prepped and ready to ship, I am just waiting for this here to get all the green lights and to be merged so I can than open a PR with th BT plugins (as you recommended in the issue to keep the seperation for individual reviews). If that is no longer the case and want to do it in a single PR we can also do that.

You also mentioned

EnableCollisionMonitor and DisableCollisionMonitor to call the service.

but I would rather stick with ToggleCollisionMonitor for bt plugin and a input port enable to keep up the consistency and reduce doubled code.

Do we also want to add a enabled: true parameter that can than be read at startup in CollisionMonitor::getParameters and be fed to the enabled_ variable which we can than use as an initial state. It might often be usefull, like in case the robot is powered off when docked and after bringup we need to move away from a dock, to have the initial state to be disabled (we save one service call 😃)

[SteveMacenski]

And I am very much interested in further working on this feature! Can I also pick other issues while this one's ongoing?

Sure! What other areas are you interested in working on? I can come up with a few ideas. One that comes to mind is understanding and testing #5033 with its respective PR and/or implementing #4794 if you want something collision monitor related!

I have the BT plugin already prepped and ready to ship,

Great! I think you can open that as a draft now as long as you link it back here so we know to test it once this is merged (unless you've already tested from a fork of this branch?)

Do we also want to add a enabled: true parameter that can than be read at startup in CollisionMonitor::getParameters and be fed to the enabled_ variable which we can than use as an initial state.

Sure! Add that in your PR :-)

[DavidG-Develop]

I opened a PR with the bt stuff, but I feel that keeping the param tied to this PR might be better. I would recommend that @armgits also includes the parameter change into this PR as that way we have 2 encased PRs that don't change the same files and should just be able to be merged one after another easier.

@armgits are you okay with you adding the param? It has to be added into the param file in the bringup package (name wise is up to you if you want to stick with just enabled or start_enabled or smthn similar) and read in the CollisionMonitor::getParameters to be used as initial value for your enabled_ variable.

Great! I think you can open that as a draft now as long as you link it back here so we know to test it once this is merged (unless you've already tested from a fork of this branch?)

I did not test it directly on the fork of this branch, but I already did have a rudimentary implementation of this solution (as we discussed in the issue before) so I did have where to test it at. Also, as it is just a service called any service server that takes than the agreed upon nav2_msgs::srv::Toggle should be enough to test it in my opinion. (i did also write a test for it)

[SteveMacenski]

I feel that keeping the param tied to this PR might be better.

I would like to minimize the additional changes requested to @armgits so we can get this merged in ASAP which would unblock the BT PR and another enabled PR as a matter of efficiency. I think its OK for now to merge this without it since it doesn't create a bug, just brings up a future enhancement that we have in mind.

@armgits please make that one open change and we can merge. If you are open to contributing the enabled parameter PR as well, you can do that immediately after that change and I can merge after I merge this one!

[mergify]

This pull request is in conflict. Could you fix it @armgits?

[armgits]

I think someone has already done that bit and now when I pushed my changes there's a merge conflict.

Update: I just realized that change was already done on the main branch, I'll rewrite mine to exactly match the main branch and see if the merge conflict could be resolved.

[DavidG-Develop]

I would like to minimize the additional changes requested to @armgits so we can get this merged in ASAP which would unblock the BT PR and another enabled PR as a matter of efficiency. I think its OK for now to merge this without it since it doesn't create a bug, just brings up a future enhancement that we have in mind.

Understood 👍

@armgits you can than decide if you want to do a pr for the enabled param than or if I should.

[armgits]

I will do the PR for enabled parameter once this gets merged.

[SteveMacenski]

@armgits can you deal with the merge conflict?

[armgits]

@SteveMacenski I tried, but there's still a small portion of code that's in conflict... Could it be manually resolved by accepting the incoming changes to the main branch?

[SteveMacenski]

No - you need to hit the "Resolve Conflicts" button and resolve it manually before I can merge

[armgits]

My bad! Just learnt that I could edit that and remove the markers, it should be good now.

[SteveMacenski]

Great! Merged!

The BT Nodes and Enabled Parameter are up next - thanks everyone!

[keshavchintamani]

@armgits @DavidG-Develop thanks for all the effort to get this PR through! Im keen to test it in one of our use cases which is going live soon.
@SteveMacenski any estimates on when this PR will be back ported into humble?