I’m now getting to some detailed integration testing and the node achievement logic is absolutely beautiful. I’m testing in some odd situations and it triggers exactly where someone would say along the actual trajectory traveled where it should using the dot product of the edge vector bisector with the distance vector.
B.E.A.U.T.I.F.U.L.
There’s still much integration testing to do of all the features and adding in the appropriate BT nodes to integrate into a navigation application, but I hope to be ready for beta testers in a couple of weeks’ time.
Anyone interested in giving it a whirl?
Edit: Figure showing is better than telling, see this YouTube video where I showcase one of my current tests
You can see in it that the server calculates the route and passes it to the navigation stack to process it. As the robot passes nodes, it triggers a feedback message to the test client indicating changes in state of tracking from one Edge to the next with its associated Node IDs. Not shown is the backend Route Operations which can be triggered on entering / exiting a particular edge or on achieving a particular node (like changing speed or opening a door).
This showcases the MVP level capabilities, which I’m still testing all the different edge situations to make sure things are rock solid at an application level - though given the density of unit testing I’ve already written, I’m pretty confident that it works except for little hiccups. But this is one big and important step, this is the first time all of the software written has been executed in concert and I’m really happy that things seem to be working as I would expect!
This demo will be made available in the PR to come as a nav2_simple_commander route server demo.