-
Notifications
You must be signed in to change notification settings - Fork 69
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges #145
base: main
Are you sure you want to change the base?
Conversation
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
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]>
ros2 run free_fleet_examples test_navigate_to_pose.py \ | ||
--frame-id map \ | ||
--namespace turtlebot3_1 \ | ||
-x 1.808 | ||
-y 0.503 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
### 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, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Spelling
source /opt/ros/galactic/setup.bash | ||
colcon build | ||
``` | ||
sudo apt install ros-jazzy-nav2-bringup |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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)
|
||
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
|
||
```bash | ||
[INFO] [1636706001.275082185] [turtlebot3_fleet_server_node]: registered a new robot: [ros1_tb3_0] | ||
source ~/ff_ws/install/setup.bash |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Might want to add a comment reminding users to source their RMF workspace if it is separate from the free fleet workspace
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
|
||
### 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). |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Discussed offline to move this to setup section
else: | ||
self.node.get_logger().error( | ||
f'Navigation goal {self.nav_goal_id} status ' | ||
f'{rep.status}') | ||
return True |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
) | ||
|
||
def _make_random_goal_id(self): | ||
return np.random.randint(0, 255, size=(16)).astype('uint8').tolist() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nit, though its random and a size 16 list, I wonder if there's a more reliable way to prevent overlapping goal IDs.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
self.node.get_logger().error( | ||
f'Navigation goal {nav_goal_id} was rejected' | ||
) | ||
self.nav_goal_id = None | ||
return |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
What happens to self.execution
in this scenario?
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.easy_full_control
, based off thefleet_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 #141zenoh
as the main way of communicating with robotszenoh-bridge-ros2dds
as a drop-in method of bridging required ROS 2 topics and actions between robot and fleet adapter/navigation_to_pose
action, ROS 2 navigation stack client #84pycdr2
to serialize and deserialize messages, with required message definitions fleshed out, Question on message generation #129nav2_bringup
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
'stb3_simulation
Two robots example using official
nav2_bringup
'sunique_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)
There are plenty of TODOs left, but this is the bare minimum testable and usable state