Docking controller + go-to-dock BT node

[SteveMacenski]

Add a controller suited specifically for docking with some open standards on docks. Then add in a go-to-dock Bt node to go to a staging point and then enable the docking controller to dock the robot. This will allow autonomous docking with the is-charge-low BT node for a robot to go back home when it needs to.

The docking controller should work on a configuration of the dock shape, using april tags, or some other form of standardizable relative pose estimation for the docking controller to target an approach at a particular vector and end position.

https://datascience.stackexchange.com/questions/6838/when-to-use-random-forest-over-svm-and-vice-versa

Also offer a Bt with this built in (if charge < X, go to docking subtree. Once charge > Y, reenter navigation subtree)

[tal-grossman]

I'v been working on such a controller and BT node my self for our robot, and I would like to have it out-of-the-box in the nav2 stack.

I can also guess this issue and adding BTActionServer PR are some how related to this issue and will provide an easy implementation method.

I still have a design conflict and i was wondering if should it be addressed in the PR. the use-case is transition from one application server to another. for example:

go-to-dock to navigateToPose Or navigateToPose to FollowDynamicObject

should one server stop and perform some kind of procedure and only then start the second server (FSM like behavior)? I know that we kinda get that behavior when alternating between action servers during the behavior tree runtime.

Another way to resolve this case is to have some-kind of application server manager which will abort one server and trigger the other one. In my opinion this way is messier and less deterministic.

anyway I think this use-case it worth considering @SteveMacenski .

[SteveMacenski]

Those 2 issues are not related at all. If you can create a controller plugin for docking, then in the BT you must only use the FollowPath BT node with the controller_id set to the docking controller plugin name in the controller server. There shouldn't be any additional need for those capabilities except when making autonomy logic through the BT (which we aim to support, but is not required to work in our framework for that; NavigateToPose BT node exists so any BT library can use it).

Nav2 can have different Bts run for different tasks. If you send a navigate to pose action to BT navigator, it can include an optional BT to be able to use any arbitrary BT for your task. But the going to dock could be trivially baked into the main BT by just checking if battery is low condition node (provided) or requested, then execute a sub-tree that gets a robot to a staging pose and switch to the docking specific controller. So a subtree might look like a control node to check if any condition under it is true, if so, execute a specific BT node. So if docking is requested (so BT node gets a ROS2 service / action / topic / param to dock) or the battery is low (using battery info messages), then execute a subtree that has the robot go back to a staging pose which can be trivially parameterized and then run the docking controller until the charge status is charging.

So in summary, the main issue here is just having the controller docking plugin. Once we have that, it would take about 2-3 days of engineering time to integrate that into a new BT to take advantage of it and dock when battery is low or someone requests it automatically. We can even have that as I describe above as 100% scalable to any application so that could be enabled by complete default for all users.

[SteveMacenski]

I think this is not what we should do, but its a half-baked idea I want documented:

We could have the charging controller / process be an action itself. Combine the process of driving to the dock, docking, and processing battery / dock messages on charge state until full, and then return when fully charged or charged above % asked in the action interface.

I'd argue that the on-dock-charging should be an action for sure, but combining it with the docking controller?

[jwallace42]

I like this half baked idea it would definitely be useful for a lot of applications. However, there definitely are instances were we should not charge at the doc. I.e some mechanical changes/fixes or maintenance?

[SteveMacenski]

Sure, I think that would be something to note. I think the stop until charged enough -> go out would be done in the BT (looking at it again now) so that could always be customized

[ajtudela]

Hi, as I mentioned to @SteveMacenski in this https://github.com/ros-planning/navigation2/pull/3553, I in the process of migrating to ROS 2 our docking system. I'ld like to explain what we have implemented in ROS 1, my thoughts how I wish to implemented in ROS 2, ask for improvements and if it's fit in the navigation core.

Here is our docking architecture:

I'll try to be brief. In ROS 1, the whole docking system is implemented under an action server with a FSM built into it. That server can be called using three "commands": docking, undocking and record the station.

• Docking: Checks if there is a near free station (we have three stations in our lab), go to the base point of that station, search the station and approach to the station. Our robots uses its front lidar to match the shape of the station against the previously recorded station using ICP and get its pose. Then, that pose will be send to the built-in controller until charger is detected.
• Undocking: just move back and rotate.
• Record: Store the station in pcl format and the initial estimate / base pose in a config file.

The main cons our system are that is detached from navigation. On one hand, you need to undock the robot before send it to a navigation goal. On the other hand, the approaching is an open loop and the robot don't see the obstacles between the base pose and the dock.

I think those problems can be solved with the new BT in navigation2 because that FSM can be splitted in differents BT:

• a pluginizable docking controller that gets the pose of the dock. (ICP, apriltags, etc)
• the controller already implemented.
• an undocking subtree with the already implemented behaviors.

We've already implemented that last part with the BT in the PR.

And our robot:

Questions? Thoughts?

[jwallace42]

Hey @ajtudela!

Here are some of my thoughts/questions.

Docking: Checks if there is a near free station (we have three stations in our lab), go to the base point of that station, search the station and approach to the station. Our robots uses its front lidar to match the shape of the station against the previously recorded station using ICP and get its pose. Then, that pose will be send to the built-in controller until charger is detected.

How do you check if there is a free station near? Do you have a higher level autonomy system?

I would recommend implementing a behavior in the behavior server rather than a controller for docking. The controllers expect a path which you don't need. (A lot has changed in two year :)).

The main cons our system are that is detached from navigation. On one hand, you need to undock the robot before send it to a navigation goal. On the other hand, the approaching is an open loop and the robot don't see the obstacles between the base pose and the dock.

I am not quite sure what you mean by your system is detached from navigation. Why can you not see obstacles between the base and dock? Are there no sensors looking down?

I think those problems can be solved with the new BT in navigation2 because that FSM can be splitted in differents BT:

a pluginizable docking controller that gets the pose of the dock. (ICP, apriltags, etc) the controller already implemented. an undocking subtree with the already implemented behaviors.

Splitting out the docking and undocking behaviors into separate behavior trees is great.

The undock behavior tree looks good.

Can you talk a bit more about the combined behavior tree. I don't think I quite understand what you are doing.

[ajtudela]

Thanks for the thoughts @jwallace42

How do you check if there is a free station near? Do you have a higher level autonomy system?

The main idea was to implement a long-running node that keeps tracks of the stations status (free/occupied) across both of our robots but due to my lack of time, it was half-baked inside te docking server in this way: at startup the docking server marks all stations as free, when a robot tries to dock and gets a FAILURE goes to the next station.

I would recommend implementing a behavior in the behavior server rather than a controller for docking. The controllers expect a path which you don't need. (A lot has changed in two year :)).

Mmm, could a "fake" path between the base and the dock pose be sent to the controller?

I am not quite sure what you mean by your system is detached from navigation. Why can you not see obstacles between the base and dock? Are there no sensors looking down?

Yes, sorry. What I mean is, once the dock is found and the robot gets the dock pose, it sends velocity commands until the dif between the two poses are zero. So, no maps and not obstacles are used.

Can you talk a bit more about the combined behavior tree. I don't think I quite understand what you are doing.

I sent that picture to illustrate the undocking subtree in the whole navigatio tree. It just one of the BT tree examples that comes within the BT navigator package with the undocking subtree at first.

[jwallace42]

Mmm, could a "fake" path between the base and the dock pose be sent to the controller?

You could do this. However, it is not ideal and likely something that we would not be able to accept into nav2. The controller server also has progress checkers and goal checkers that aren't applicable to a dock controller. Implementing it as a plugin in the behavior server will actually be easier.

Yes, sorry. What I mean is, once the dock is found and the robot gets the dock pose, it sends velocity commands until the dif between the two poses are zero. So, no maps and not obstacles are used.

np! The behavior server has access to the local and global costmap. See https://github.com/ros-planning/navigation2/blob/main/nav2_behaviors/plugins/drive_on_heading.hpp#L163 for an example of collision checking.

I sent that picture to illustrate the undocking subtree in the whole navigatio tree. It just one of the BT tree examples that comes within the BT navigator package with the undocking subtree at first.

Alright. Just to note, you don't want to trigger recovery behaviors if undocking fails a few times which will happen in your behavior tree above.

I would suggest focusing on completing the sub trees for docking and undocking. After that, we can worry about full integration :).

If you have a branch ready for the undock sub tree, you can put up a pr and I would gladly review it for you.

[ajtudela]

You could do this. However, it is not ideal and likely something that we would not be able to accept into nav2. The controller server also has progress checkers and goal checkers that aren't applicable to a dock controller. Implementing it as a plugin in the behavior server will actually be easier.

So, if I understood right, you're saying to implement a kind of controller to send command velocities using the behavior server, right?

Alright. Just to note, you don't want to trigger recovery behaviors if undocking fails a few times which will happen in your behavior tree above.

I'm a little newbie with the BTs but in this case if, for example, BackUp returns FAILURE, the sequence and the fallback will return FAILURE too and then, the whole subtree will fail. I'm not exactly sure what are you trying to tell me here.

If you have a branch ready for the undock sub tree, you can put up a pr and I would gladly review it for you.

Ok. How I do that? I mean, I copy the default "navigate_to_pose_w_replanning_and_recovery.xml" and add the subtree to it or I add a new xml with the subtree in the same folder, you tell me...

But before that, @SteveMacenski and I we discussed in the #3553 the possibility to replace the "IsBatteryCharging" condition with new type of message + BT node (IsDocked) for this because we saw different use case were "IsBatteryCharging" is not suitable for docking.

[jwallace42]

So, if I understood right, you're saying to implement a kind of controller to send command velocities using the behavior server, right?

Yeah, behaviors are set up to publish velocity commands. Spin and backup are good examples of this.

Ok. How I do that? I mean, I copy the default "navigate_to_pose_w_replanning_and_recovery.xml" and add the subtree to it or I add a new xml with the subtree in the same folder, you tell me...

Create a new file undock.xml which contains the undock tree.

I will let steve get back to you :).

[SteveMacenski]

Sorry for the delay, starting a new business has alot of overhead to get over.

The FSM for your docking system seems good from a first glance. I initially thought about simply setting this up with a single docking station dedicated to a particular robot to start with, but we could just jump directly to a more complete system if we had one that folks were interested in contributing! I usually like incremental movements, but if there's a working and reliable system that's being donated / contributed, I see no reason to argue with that :smile: Though it sounds like from other messages it might not be fully baked, so maybe we do start with something incremental and design the behavior with multiple docks in mind for later adjustment?

Wrt the recording/detection - does it handle adjusting the docking pose if the dock has shifted about (or localization is not totally on the ball)? Something I had thought about related to this was to create a traditional machine learning model to detect docks from the lidar scans when the dock shapes are unique and at lidar height. When not at lidar height or not unique, then other detectors would need to be provided (and we'd have an interface to support that e.g. QR codes or visual segmentation). The record thing probably works well as long as the dock doesn't move w.r.t. the static environment since a simple ICP on the scan snap-shot doesn't differentiate the dock vs the background.

The main cons our system are that is detached from navigation. On one hand, you need to undock the robot before send it to a navigation goal.

I don't necessarily see that as a con, its just a separate "behavior" from the general free-space navigation for a specific case. That can be just a dockRobot and undockRobot BT node actions to call and once they're complete, then you can send it on its way to normal navigation using the BT. Not much different than if you were to call ComputePathToPose - that has to complete before we can FollowPath :smile: The BT control flow nodes, when properly selected, will make sure they're mutually exclusive.

On the other hand, the approaching is an open loop and the robot don't see the obstacles between the base pose and the dock.

I would want to add something along these lines, but I think the laser scan detector would be a good first-order feature for that. If the laser scanner can see the dock, that means at least in that slice of the plane the dock is free. Dealing with things below the laser scan plane (or above it) would be good too, but that could be made into future work or address it once someone finds it to be a tangible problem that they're willing to dedicate resources to work with us to solve it.

I think those problems can be solved with the new BT

Certainly, but I would call "dock my robot" a well defined behavior in it of itself. This could be a behavior plugin that does all that logic using the FSM as a stand-alone specific behavior. We could also define it as a subtree for easier customization, that is true. But that wasn't my original thinking - but I'm flexible. Instead, I was thinking we create a Behavior that is Docking (instead of a Controller) which combines the full maneuver.

The last remark that wasn't addressed here was the actual controller that is used once you have the robot at its staging point and want to bring it to the detected docking pose (when its not a trivial "go straight back"). What do you use in this case? I'm aware of Ferg's Graceful controller based on spirals which is used in Fetch's Open Autodock framework https://github.com/mikeferguson/graceful_controller -- which might be also worth a glance.

[ajtudela]

Wrt the recording/detection - does it handle adjusting the docking pose if the dock has shifted about (or localization is not totally on the ball)?

The dock detection was linked to the odometry of the robot. So, in every callback the dock pose is updated.

The record thing probably works well as long as the dock doesn't move w.r.t. the static environment since a simple ICP on the scan snap-shot doesn't differentiate the dock vs the background.

Yeap, in our case, we have three docks (two of them having the same shape and the other one is different) fixed in our lab. The record action stores a PCD with the dock points (previously segmented) from the lidar and the position of the dock in the map. So, later we can use this position as an estimate for the ICP matcher.

Certainly, but I would call "dock my robot" a well defined behavior in it of itself. This could be a behavior plugin that does all that logic using the FSM as a stand-alone specific behavior. We could also define it as a subtree for easier customization, that is true. But that wasn't my original thinking - but I'm flexible. Instead, I was thinking we create a Behavior that is Docking (instead of a Controller) which combines the full maneuver.

Behaviors can be pluginizable? I asked because I was thinking more on an action server at the same level that controller, planner, etc just focus to find the dock with different plugins (ML, ICP, QR, ...) depending the needs of the users.

The last remark that wasn't addressed here was the actual controller that is used once you have the robot at its staging point and want to bring it to the detected docking pose (when its not a trivial "go straight back"). What do you use in this case?

First, I made my own version of a controller (pan and straight forward) but it didn't work in some cases. Then, I discovered the ICRA article about "A Smooth Control Law for Graceful Motion.." and I implemented. Later, I find out that was already implemented by Mike 😅. Honestly, it was the best choice. It works flawlessly.

[SteveMacenski]

Then, I discovered the ICRA article about "A Smooth Control Law for Graceful Motion.." and I implemented.

... wait, you did? Did you happen to implement it without referencing Fergs' implementation? That is actually very valuable to us. Fergs ripped that out of the Fetch autodocking code which is under LGPL licenses which we don't want to insert into Nav2. If you have an implementation that you'd be open to contributing under a permissive license (optimally Apache 2.0 just for consistency reasons, but MIT or BSD work just as fine) that could be mighty useful both in Docking but also as another controller option in Nav2.

Even separately of Docking, we could start another thread on introducing that as a controller in Nav2's core if you're game to work with me on refining it! Also might be a good first-step anyway for the docking capability since it'll leverage it.

The dock detection was linked to the odometry of the robot.

I got that, I meant more what if someone came by and kicked the dock physically 5 inches to the right or something. So the general area looks the same, but the dock is physically not where it was before. That was a big issue for some of the other robots I've deployed, but perhaps I'm overthinking it to start with. How are you segmenting the points? I suppose that's what I'm suggesting to do with traditional ML, but curious what your thoughts are on it. I haven't done alot of traditional segmentation-esk tasks, especially on lidar data - so I'm genuinely open ears.

Behaviors can be pluginizable? I asked because I was thinking more on an action server at the same level that controller, planner, etc just focus to find the dock with different plugins (ML, ICP, QR, ...) depending the needs of the users.

I suppose I'd build this as a framework where we have plugin slots for things like findDock which could use the lidar segment + ICP or QR code or what have you so it can be customized.

[ajtudela]

... wait, you did? Did you happen to implement it without referencing Fergs' implementation? That is actually very valuable to us. Fergs ripped that out of the Fetch autodocking code which is under LGPL licenses which we don't want to insert into Nav2. If you have an implementation that you'd be open to contributing under a permissive license (optimally Apache 2.0 just for consistency reasons, but MIT or BSD work just as fine) that could be mighty useful both in Docking but also as another controller option in Nav2.

I implemented the main algorithm using exactly the formulas that the article mentions. If I remembered correctly there were two or three formulas at most. I allways mess with the different licenses but if it's ok, I think we can wrap a code around the article formulas into a new controller and start from that.

Even separately of Docking, we could start another thread on introducing that as a controller in Nav2's core if you're game to work with me on refining it! Also might be a good first-step anyway for the docking capability since it'll leverage it.

I'm in!

I got that, I meant more what if someone came by and kicked the dock physically 5 inches to the right or something. So the general area looks the same, but the dock is physically not where it was before.

In that case, as the robot rotates from the base point, if the dock is far enough from the wall and if the dock is facing that point, I think the dock could be matched. But there're too much ifs and too much thinking, hahaha

How are you segmenting the points? I suppose that's what I'm suggesting to do with traditional ML, but curious what your thoughts are on it. I haven't done alot of traditional segmentation-esk tasks, especially on lidar data - so I'm genuinely open ears.

Using a little piece of code I made some time ago: laser_segmentation. It implements several point segmentation algorithms on planar lidar. Works pretty well if fine-tuned.

[SteveMacenski]

What's the license its currently under and are you able to potentially relicense it? Wrapping the code doesn't intrinsically allow us change the licenses for the use of it later, if they are more restrictive like GPL/LGPL.

In all honesty after talking with Fergs at an event a couple of years ago, I wouldn't have made RPP if I knew about that algorithm. It doesn't do 100% what RPP does, but it has the same foundations for the same kind of behavior to be built upon it. If/when we have both in Nav2, it becomes a little fuzzy to me if we should keep RPP or put it on a medium-term deprecation cycle (or make it really specialized only for ackermann vehicles)

Sounds good! :smile:

:+1: I suppose the detection stuff we're talking about making a plugin anyway, so that can be customized or added later. I don't think dock-kicking needs to be a ground-zero requirement unless we already had a system capable of doing it by means of separate dock vs background segmentation. In that case, I think it would be worth the extra couple of days to do it.

I'll take a look at it today!

[ajtudela]

What's the license its currently under and are you able to potentially relicense it? Wrapping the code doesn't intrinsically allow us change the licenses for the use of it later, if they are more restrictive like GPL/LGPL.

I've asked and there is no problem relicensing our code.

[SteveMacenski]

Is this posted somewhere in open source? What do you think is the best way to proceed for us together on the controller addition to lead into docking?

[ajtudela]

Is this posted somewhere in open source? What do you think is the best way to proceed for us together on the controller addition to lead into docking?

We have it in our private repos.

I can write a controller plugin using the graceful approach in our "navigation2" repo and make a pull request. Ww can start from there.

[wvergouwenavular]

@ajtudela Have you ever had the time to start developing this controller plugin? :smile:

[ajtudela]

@ajtudela Have you ever had the time to start developing this controller plugin? 😄

Sorry, I had a lot of work last few months. I have an alpha version of the controller right now and I expect to finish it and make the first pull request in a few weeks. Hopefully before Christmas ;)

[wvergouwenavular]

@ajtudela That's amazing! I'll keep an eye out for this PR!

[jcarlosgm30]

@SteveMacenski what is the status of the docking framework? can I help in any way?

[SteveMacenski]

Wait a few weeks and you'll see something pop up about it :-) Can't say much more until the announcement :wink: