[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges #145

aaronchongth · 2024-09-30T02:45:15Z

New feature implementation

A large motivation of this re-write, is to fully utilize the well tested tools already available in the ROS ecosystem and Zenoh, and also take advantage of python bindings as much as possible, to allow for easier iterations, testing and contributions.

The use of Zenoh bridges will hopefully also minimize disruption to any existing robot navigation stack, and can be spun up with just a standalone binary with the appropriate configuration file.

The legacy implementation can still be found at the legacy branch.

re-implemented free_fleet using easy_full_control, based off the fleet_adapter_template, will allow setting up of perform actions, Support perform_action feature of the fleet adapter #116, [Feature request]: Add /robot_state topic published from free_fleet_server #141
fleet adapter to use zenoh as the main way of communicating with robots
change approach to use zenoh-bridge-ros2dds as a drop-in method of bridging required ROS 2 topics and actions between robot and fleet adapter
supports the nav2 stack with the simple /navigation_to_pose action, ROS 2 navigation stack client #84
using pycdr2 to serialize and deserialize messages, with required message definitions fleshed out, Question on message generation #129
examples using existing simulations from nav2_bringup
updated README, https://github.com/open-rmf/free_fleet/blob/easy-full-control/README.md
supports Jazzy (other distros not tested yet), What are the plans on moving to humble (Ubuntu 22.04)? #138
linting, Add style linting #100

Architecture

This architecture and the use of Zenoh bridges will ideally improve the integration experience, with just setting up the zenoh-bridge-ros2dds and its configuration, instead of building and running a separate node.

This PR does not support the ROS 1 navigation stack, the diagram is just for illustrating the proposed architecture once it is implemented down the road.

Single robot example using official `nav2_bringup`'s `tb3_simulation`

Two robots example using official `nav2_bringup`'s `unique_multi_tb3_simulation`

Although the two robot simulation example is just for testing purposes as it uses a different architecture as intended

Next TODOs (over subsequent PRs)

hardware testing
attempt to optimize tf messages (not all are needed)
ROS 1 nav support
map switching support
docking support
update to use zenoh 1.0
static testing
end-to-end testing
support for Rolling
docker images
releases
testing and support for other RMW implementations

There are plenty of TODOs left, but this is the bare minimum testable and usable state

Signed-off-by: Aaron Chong <[email protected]>

* Pytest setup Signed-off-by: Aaron Chong <[email protected]> * Add workflow Signed-off-by: Aaron Chong <[email protected]> * On push for testing Signed-off-by: Aaron Chong <[email protected]> * Remove branch override Signed-off-by: Aaron Chong <[email protected]> * Use workflow directory Signed-off-by: Aaron Chong <[email protected]> * Basic testing ci without RMF Signed-off-by: Aaron Chong <[email protected]> * Use containers Signed-off-by: Aaron Chong <[email protected]> --------- Signed-off-by: Aaron Chong <[email protected]>

xiyuoh · 2024-10-16T03:30:33Z

README.md

+ros2 run free_fleet_examples test_navigate_to_pose.py \
+    --frame-id map \
+    --namespace turtlebot3_1 \
+    -x 1.808
+    -y 0.503


Suggested change

ros2 run free_fleet_examples test_navigate_to_pose.py \

--frame-id map \

--namespace turtlebot3_1 \

-x 1.808

-y 0.503

ros2 run free_fleet_examples test_navigate_to_pose.py \

--frame-id map \

--namespace turtlebot3_1 \

-x 1.808 \

-y 0.503

Missing backslash, copy and paste doesn't work

xiyuoh

Thank you for revamping free fleet! Tried out the examples and things are looking fantastic. I was able to bring up both single and multiple TB3 tests by following the instructions on README.

Left some minor comments for the README that are mostly about typos and clarity. For the fleet/robot adapter, I was primarily looking out for edge cases where self.nav_goal_id might fall out of sync with self.execution and catching any scenarios where the execution object is left unattended if self.nav_goal_id is set to None. This may lead to RMF not sending any subsequent commands, or sending outdated nav commands if replans are not being called prompty.

xiyuoh · 2024-10-16T03:32:11Z

README.md

-### Message Generation
-
-Message generation via `FleetMessages.idl` is done using `dds_idlc` from `CycloneDDS`. For convenience, the generated mesasges and files has been done offline and committed into the code base. They can be found [here](./free_fleet/src/messages/FleetMessages.idl).
+Other system depenendencies,


xiyuoh · 2024-10-16T03:56:33Z

README.md

-source /opt/ros/galactic/setup.bash
-colcon build
+```
+sudo apt install ros-jazzy-nav2-bringup


Do we need ros-jazzy-navigation2? Also suggest to add a line of sudo apt update && sudo apt upgrade somewhere (speaking from experience, I didn't realize my nav2 was outdated and sending goals didn't work)

xiyuoh · 2024-10-16T04:02:58Z

README.md


-This launch file starts the simulation in `gazebo`, visualization in `rviz`, as well as the simulated navigation stack of the single turtlebot3. Once the simulation and visualization show up, the robot can be commanded as per normal through `rviz` with `2D Nav Goal`.
+ros2 launch free_fleet_examples tb3_simulation_fleet_adapter.launch.xml


Are we able to wrap all these commands up in a single launch file for optional use? Apart from zenoh bridge and test_navigate_goal_to_pose. I think it's great to have these steps provided to explain what each node does, but for someone who just wants to bring it up after the first time it feels like a lot of new terminals to be opening and repeated setting of environmental variables. Let me know what you think

Hmmm, I definitely see the merit in that, however I was hoping to get users more familiar with the architecture of RMF, rather than launching and killing everything in one go, with this setup, they can jumble it together if they want to, or they can kill just the fleet adapter, fix something, and launch it again, without killing the entire stack

I'll keep this in mind and monitor the UX for a bit, but I'd probably like to maintain these set of steps for now

xiyuoh · 2024-10-16T04:06:19Z

README.md


 ```bash
-[INFO] [1636706001.275082185] [turtlebot3_fleet_server_node]: registered a new robot: [ros1_tb3_0]
+source ~/ff_ws/install/setup.bash


Might want to add a comment reminding users to source their RMF workspace if it is separate from the free fleet workspace

I've currently added a note during the build step, https://github.com/open-rmf/free_fleet/pull/145/files#diff-b335630551682c19a781afebcf4d07bf978fb1f8ac04c6bf87428ed5106870f5R50, instructions assume folks are building everything in this workspace
also I believe if an overlay workspace is used for building just free_fleet, sourcing the child workspace should allow access to the parent workspace too

README.md

xiyuoh · 2024-10-16T05:34:26Z

README.md


-### Prerequisites
+We recommend setting up `zenoh-bridge-ros2dds` with the standalone binaries. After downloading the appropriate released version and platform, extract and use the standalone binaries as is. For source builds of `zenoh-bridge-ros2dds`, please follow the [official guides](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds).


Discussed offline to move this to setup section

xiyuoh · 2024-10-16T06:14:26Z

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

+                else:
+                    self.node.get_logger().error(
+                        f'Navigation goal {self.nav_goal_id} status '
+                        f'{rep.status}')
+                    return True


In cases where the navigation goal has failed and the robot did not reach the goal waypoint successfully, _is_navigation_done() returns True. This might cause RMF to misunderstand that the robot is ready for the subsequent waypoint. It might help to either add a retry mechanism for the same goal waypoint, or trigger a replan.

xiyuoh · 2024-10-16T06:18:35Z

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

+        )
+
+    def _make_random_goal_id(self):
+        return np.random.randint(0, 255, size=(16)).astype('uint8').tolist()


Nit, though its random and a size 16 list, I wonder if there's a more reliable way to prevent overlapping goal IDs.

I think this should be random enough, it'll be 256^16 combinations, but I'll check out how nav2 usually does it and use a similar method perhaps

xiyuoh · 2024-10-16T06:20:55Z

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

+        if destination.map != self.map:
+            self.node.get_logger().error(
+                f'Destination is on map [{destination.map}], while robot '
+                f'[{self.name}] is on map [{self.map}]'
+            )
+            return


While this is unlikely, we could trigger a replan instead of returning with nothing to ensure that RMF sends in the next reasonable command. Otherwise the execution object remains waiting to be marked finished() without acknowledging that this command is invalid.

xiyuoh · 2024-10-16T06:22:25Z

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

+                self.node.get_logger().error(
+                    f'Navigation goal {nav_goal_id} was rejected'
+                )
+                self.nav_goal_id = None
+                return


What happens to self.execution in this scenario?

aaronchongth added 15 commits August 9, 2024 16:30

Clean up

5fb1270

Signed-off-by: Aaron Chong <[email protected]>

Turtlebot3 world map

73b008a

Signed-off-by: Aaron Chong <[email protected]>

Basic example of navigate_to_pose

da8c3f3

Signed-off-by: Aaron Chong <[email protected]>

test tf

2058661

Signed-off-by: Aaron Chong <[email protected]>

Basic tests and README updated, started with fleet adapter template

292be14

Signed-off-by: Aaron Chong <[email protected]>

Tweaking results and got cancellation

c6e8330

Signed-off-by: Aaron Chong <[email protected]>

Using tf_transformations, flesh out more implementation

a29278c

Signed-off-by: Aaron Chong <[email protected]>

Basic launch done

b76ab27

Signed-off-by: Aaron Chong <[email protected]>

Basic working with patrol tasks

552e7e6

Signed-off-by: Aaron Chong <[email protected]>

Update configs

c2cf257

Signed-off-by: Aaron Chong <[email protected]>

Battery state, update readme

5fc16db

Signed-off-by: Aaron Chong <[email protected]>

readme

cf71e16

Signed-off-by: Aaron Chong <[email protected]>

Update README, reorganized and renamed configs, testing unique tb3 sim

de67ea8

Signed-off-by: Aaron Chong <[email protected]>

Setup tested

65f1505

Signed-off-by: Aaron Chong <[email protected]>

Updated README

e7273c2

Signed-off-by: Aaron Chong <[email protected]>

aaronchongth marked this pull request as ready for review September 30, 2024 17:45

aaronchongth marked this pull request as draft September 30, 2024 17:46

aaronchongth added 2 commits October 1, 2024 10:46

Style workflow on push and PR, lint

b2a4c26

Signed-off-by: Aaron Chong <[email protected]>

Linter syntax

82e10a0

Signed-off-by: Aaron Chong <[email protected]>

aaronchongth marked this pull request as ready for review October 1, 2024 02:50

aaronchongth requested review from mxgrey and xiyuoh October 1, 2024 02:57

aaronchongth changed the title ~~[Re-write] Free fleet adapter using easy full control and zenoh bridges~~ [Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges Oct 1, 2024

aaronchongth mentioned this pull request Oct 1, 2024

develop: Add field to RobotState to track command completion #114

Closed

aaronchongth mentioned this pull request Oct 1, 2024

Develop #76

Closed

xiyuoh reviewed Oct 16, 2024

View reviewed changes

aaronchongth mentioned this pull request Oct 16, 2024

Build Error #147

Closed

1 task

xiyuoh requested changes Oct 16, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges #145

[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges #145

aaronchongth commented Sep 30, 2024 •

edited

Loading

xiyuoh Oct 16, 2024

xiyuoh left a comment

xiyuoh Oct 16, 2024

xiyuoh Oct 16, 2024

xiyuoh Oct 16, 2024

aaronchongth Oct 16, 2024

xiyuoh Oct 16, 2024

aaronchongth Oct 16, 2024

xiyuoh Oct 16, 2024

xiyuoh Oct 16, 2024

xiyuoh Oct 16, 2024

aaronchongth Oct 16, 2024

xiyuoh Oct 16, 2024

xiyuoh Oct 16, 2024


		This launch file starts the simulation in `gazebo`, visualization in `rviz`, as well as the simulated navigation stack of the single turtlebot3. Once the simulation and visualization show up, the robot can be commanded as per normal through `rviz` with `2D Nav Goal`.
		ros2 launch free_fleet_examples tb3_simulation_fleet_adapter.launch.xml


		### Prerequisites
		We recommend setting up `zenoh-bridge-ros2dds` with the standalone binaries. After downloading the appropriate released version and platform, extract and use the standalone binaries as is. For source builds of `zenoh-bridge-ros2dds`, please follow the [official guides](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds).

[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges #145

Are you sure you want to change the base?

[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges #145

Conversation

aaronchongth commented Sep 30, 2024 • edited Loading

New feature implementation

Architecture

Single robot example using official nav2_bringup's tb3_simulation

Two robots example using official nav2_bringup's unique_multi_tb3_simulation

Next TODOs (over subsequent PRs)

Choose a reason for hiding this comment

xiyuoh left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aaronchongth commented Sep 30, 2024 •

edited

Loading

Single robot example using official `nav2_bringup`'s `tb3_simulation`

Two robots example using official `nav2_bringup`'s `unique_multi_tb3_simulation`