Multirobot SLAM ros2 #592
Multirobot SLAM ros2 #592
Conversation
|
@acachathuranga this looks great. Could you go into more detail about how you set up the two robot scenario and run the multirobot_slam_toolbox_node? FYI, I am trying to replicate your demo using two Turtlebot3 robot platforms in Gazebo. I have set up the namespaces according to your diagram but when launching everything I can't seem to get the correct /robot1/map and /robot2/map topics to publish and the /localized_scan topic shows no published messages. Appreciate any pointers. |
|
@Daniel-Meza I have added the launch and parameter files for multi-robot mapping. The parameters file is identical to mapper_params_online_async.yaml. I have also updated the ReadMe on how to run multi-robot slam with turtlebot3 multirobot simulation (provided with navigation2 nav2_bringup package). In short, you can follow the installation instructions in navigation2 Getting started tutorial. Once you've installed navigation2, nav2_bringup and turtlebot3 packages, source the environment variables as described and run the following commands in separate terminals. ros2 launch nav2_bringup multi_tb3_simulation_launch.py headless:=False
ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot1
ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot2Then you can use teleop_twist_keyboard to drive the robots around using following commands. When driving the robots around, the merged map will be displayed on both rviz windows. ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot1
ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot2 |
|
@acachathuranga Thank you, I was able to replicate the demo and also get it working with a 3rd robot. Going a bit further, do you have some insights on the following: Maximum number of robots. Since every robot/sensor runs its own slam_toolbox node, do you foresee any limitation on the number of robots that can be used simultaneously? Assuming of course there is enough computing power and bandwidth to handle all of the data. Running on real robots. In Gazebo simulation, I believe the "map" and "odom" frame is always placed at a global (0, 0, 0) regardless of where the robots are spawned. This means that every slam_toolbox node has their "map" located based on the same initial point. This is not the case when booting up real robots since the map frame is placed on the current physical location of the robot itself. So, would it be necessary to provide an additional transform to ensure that the "map" frame of real robots align on top of each other? Or is this transformation handled automatically by the multirobot nodes? |
|
@Daniel-Meza nice to hear you got it working! Maximum number of robots: Given there is enough computing power and bandwidth, I do not see any limitation on the number of robots (Unless there are underlying SLAM limitations within slam_toolbox on number of graph nodes etc). But nothing specific to 'multi-robot slam'. However, when the robot count increases in real world scenarios (at least during our testing), you're likely to run into bandwidth problems before anything else :) Running on real robots: This is explained in the PR notes as well as the updated ReadMe. To summarize; gazebo simulation odometry is indeed in a global frame. So everything will automatically work. When you run on actual robots, you need to start the robots very close to each other, and also in the same orientation. Then the scan matcher will handle the initial offsets. In our testing, what we did was, 1) turn on one robot at a marked location, 2) move the robot away from the location 3) place another robot at the marked location and repeat. Taking initial poses from input args /parameters will be added in near future. |
|
Good Morning, Are you able to incoperate the recently added /reset feature with the multi-robot solution? |
|
@SteveMacenski any thoughts or comments on this? I noticed many people find this multi-robot implementation useful and there have also been some recent duplicate efforts (PR #727) on this multi-robot slam topic. This work has been tested extensively with real robot fleets (2 robot, 4 robot, 8 robot configurations) and has provided very good results. The use of '/localized_scan' topic instead of sharing all laser scans among all robots provides real-life benefits in terms of network bandwidth. We also did extensive tests with different types of network (Infrastructure based, mesh .etc) and found out sharing all laser messages is indeed not practical for real situations. We already have done further extensions on this work, in terms of bandwidth optimizations for large robot fleets. I will push these updates once our work is published. |
|
Is it possible to share robots initial poses (relative poses) before multi-robot mapping starts, so that the robots do not need to start from the same location? |
|
Sorry for the delays here - this is something I'm going to prioritize responses and a resolution to now. Expect more responses from me now :-) There's another PR open by @Daniel-Meza which adds this into I would like to setup a call with Daniel, you @acachathuranga and myself to chat about steps forward here so we can get some solution into I possibly see a solution being 2 fold: your new node here and Daniel's work to embed the optionality into the single node for less networking complex situations / simulation. Or if we all agree one way in particular is the right way to go for all cases, then we can go that route and work together to make that happen. |
|
Sounds good, @SteveMacenski. |
|
Mid day in which time zone 😉 @acachathuranga what about you? |
|
Good point 😅 |
|
Sorry for the late reply! Thanks for your time on this @SteveMacenski. I'm 12hrs ahead of your timezone haha, but actually I'm okay with any time this month. Let me know a convenient time for you, thanks! |
|
@Daniel-Meza could you do 10am CDT? That would be 8am my time, 10am your time, and 8pm @acachathuranga time -- which seems like the most reasonable cross section for everyone involved if you could do a little earlier. I'd be open Nov 20, 22, 26 (to name a few) |
|
Yes, I can make 10am CDT on Nov 20 or 26. |
|
@Daniel-Meza you can find my email on my github page. Please email me so I have your email to set up the meeting. I see @acachathuranga has an email on his account that I can use |
|
Invites were sent out! |
|
Notes from our conversation:
|
| @@ -64,6 +64,7 @@ set(libraries | |||
| toolbox_common | |||
| SlamToolboxPlugin | |||
| ceres_solver_plugin | |||
| multirobot_slam_toolbox | |||
There was a problem hiding this comment.
For this + the Class + file names: I think we need to make this more specific since we'll have both nodes in the project.
Suggestion: Decentralized Multi-robot?
|
|
||
| *multirobot_slam_toolbox_node* extends slam_toolbox for multi-robot mapping and localization. Slam_toolbox instances are run on each robot under unique namespaces and pose graph data are shared between the nodes through */localized_scan* topic. Each slam_toolbox instance publishes transform of host robot and peers in the network. Peer transform frame names are prefixed with the namespace of the peer. | ||
|
|
||
| Initially the robots should start close to each other, in the same orientation, as scan matching is used to find the connection between different pose graphs. This however is not required in simulation environments such as gazebo, as all robots' odometry is referenced to a gazebo's global frame. |
There was a problem hiding this comment.
As discussed: expand on this. If you can show an example project setting up the shared frame, that would be immensely helpful for those that want to use your work
| # Multi-Robot SLAM | ||
|  | ||
|
|
||
| *multirobot_slam_toolbox_node* extends slam_toolbox for multi-robot mapping and localization. Slam_toolbox instances are run on each robot under unique namespaces and pose graph data are shared between the nodes through */localized_scan* topic. Each slam_toolbox instance publishes transform of host robot and peers in the network. Peer transform frame names are prefixed with the namespace of the peer. |
There was a problem hiding this comment.
I'd expand on this description about how the data is shared (localized scans published once processed by a robot's host node to sync up with others), why (network limitations). Enough so that when someone reads the software, they know what theyre looking at
| @@ -80,10 +80,12 @@ class LaserAssistant | |||
| const std::string & base_frame); | |||
| ~LaserAssistant(); | |||
| LaserMetadata toLaserMetadata(sensor_msgs::msg::LaserScan scan); | |||
There was a problem hiding this comment.
Should the single version be removed and uses switched to the new one?
There was a problem hiding this comment.
It looks like the single version just wraps the multi-version now
| @@ -144,34 +153,39 @@ karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw) | |||
|
|
|||
| bool LaserAssistant::isInverted(double & mountingYaw) | |||
| { | |||
| geometry_msgs::msg::TransformStamped laser_ident; | |||
There was a problem hiding this comment.
why was this method updated?
src/slam_toolbox_multirobot.cpp
Outdated
| : SlamToolbox(options), localized_scan_topic_("/localized_scan") | ||
| /*****************************************************************************/ | ||
| { | ||
| current_ns_ = this->get_namespace() + 1; |
There was a problem hiding this comment.
get_namespace returns a string, I'm not sure what this intends to do
There was a problem hiding this comment.
After reviewing the file, the use of current_ns_ and its modification to the published localized range scan should be documented / explained further
|
|
||
| /*****************************************************************************/ | ||
| MultiRobotSlamToolbox::MultiRobotSlamToolbox(rclcpp::NodeOptions options) | ||
| : SlamToolbox(options), localized_scan_topic_("/localized_scan") |
There was a problem hiding this comment.
This should be a parameter (the topic to use)
|
|
||
| sensor_msgs::msg::LaserScan::ConstSharedPtr scan = | ||
| std::make_shared<sensor_msgs::msg::LaserScan>(localized_scan->scan); | ||
| Pose2 pose; |
There was a problem hiding this comment.
There's a x,y,heading constructor for Pose2 that would make this more concise https://github.com/SteveMacenski/slam_toolbox/blob/ros2/lib/karto_sdk/include/karto_sdk/Karto.h#L2063
src/slam_toolbox_multirobot.cpp
Outdated
| } | ||
| LocalizedRangeScan * range_scan = addExternalScan(laser, scan, pose); | ||
|
|
||
| if (range_scan != nullptr) |
There was a problem hiding this comment.
This is also publishing - aren't we having some confliction if this is also publishing?
https://github.com/SteveMacenski/slam_toolbox/blob/ros2/src/slam_toolbox_common.cpp#L472-L498
acachathuranga
force-pushed
the
multirobot_ros2
branch
from
f595f0c
to
90f5fb3
Compare
|
Sorry, it had been a very busy couple weeks trying to wrap up work things before holidays, so couldn't finish all the changes. But the linting issues and the conflicts with ros2 branch are fixed now.
Sorry again for the delays as I'll be traveling starting tomorrow, so will take a while for me to finish the items. But will try my best to finish all before new year! |
|
No worries at all, its a busy time of year :-) Let us know when you're ready |
Basic Info
An extension of slam_toolbox for ROS2 multi-robot distributed SLAM. This work was done as part of a research at Singapore University of Technology and Design.
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points