├── .github └── FUNDING.yml ├── .gitignore ├── Doxyfile ├── LICENSE ├── README.md ├── docs ├── contributing.md ├── images │ ├── .gitkeep │ ├── Donate-PayPal-green-brl.png │ ├── Donate-PayPal-green-usd.png │ ├── agreements.png │ ├── capes.png │ ├── cnpq.png │ ├── dcc.png │ ├── dcist.png │ ├── example_exploration.png │ ├── fapemig.jpg │ ├── forestworld.png │ ├── forestworld2.png │ ├── forestworld3.png │ ├── glass_sandbox_project_logo.png │ ├── penn.png │ ├── pioneer3at.png │ ├── rosgraph.png │ ├── run_simulation.png │ ├── rviz.png │ ├── rviz_trajectory.png │ ├── rvizexploration.png │ ├── sandbox_project_logo.png │ ├── shared_controls_treated.png │ ├── spread.png │ ├── stack.png │ ├── stack_components.png │ ├── stack_components_high.png │ ├── tf.png │ ├── tmux.png │ ├── trajectories before.png │ ├── trajectoriesafter.png │ ├── ufmg-logo.png │ └── verlab_logo.png ├── maps.md ├── multi-robot-exploration.md ├── multi-robot-guidelines.md ├── multi-robot-simulations.md ├── nodes │ ├── .gitkeep │ ├── alysson2024_node.md │ ├── average_velocity_node.md │ ├── cspace_node.md │ ├── frontier_discovery_node.md │ ├── gmapping_pose_node.md │ ├── integrated_global_planner_node.md │ ├── laser_to_world_node.md │ ├── local_cspace_node.md │ ├── local_planner_node.md │ ├── map_stitching_node.md │ ├── mock_communication_model_node.md │ ├── randomized_social_welfare_node.md │ ├── relative_pose_estimator_node.md │ └── yamauchi1999_node.md ├── references.md ├── robots.md ├── usage.md └── working_environment.md ├── gazebo_resources ├── models │ ├── .gitkeep │ └── testing_area │ │ ├── materials │ │ └── textures │ │ │ └── heightmap.png │ │ ├── model.config │ │ └── model.sdf └── worlds │ ├── .gitkeep │ ├── boxes.world │ └── forest.world └── src ├── multirobotexploration ├── CMakeLists.txt ├── config │ ├── .gitkeep │ ├── gazebo_robots_start_pose.yaml │ ├── rendezvous_footprint.yaml │ └── rendezvous_plan.yaml ├── include │ ├── common │ │ ├── .gitkeep │ │ ├── Common.h │ │ ├── RendezvousPlan.h │ │ └── SearchAlgorithms.h │ └── nodes │ │ ├── .gitkeep │ │ ├── Alysson2024Node.h │ │ ├── AverageVelocityEstimatorNode.h │ │ ├── CSpaceNode.h │ │ ├── FrontierDiscoveryNode.h │ │ ├── GmappingPoseNode.h │ │ ├── IntegratedGlobalPlannerNode.h │ │ ├── LaserToWorldNode.h │ │ ├── LocalCSpaceNode.h │ │ ├── LocalPlannerNode.h │ │ ├── MapStitchingNode.h │ │ ├── MockCommunicationModelNode.h │ │ ├── RandomizedSocialWelfareNode.h │ │ ├── RelativePoseEstimatorNode.h │ │ └── Yamauchi1999Node.h ├── launch │ ├── .gitkeep │ ├── exploration_stack_bringup.launch │ └── gmapping.launch ├── package.xml ├── rviz │ ├── .gitkeep │ ├── robot_0.rviz │ ├── robot_0_full.rviz │ ├── robot_1.rviz │ ├── robot_2.rviz │ └── robot_3.rviz ├── setup.py └── source │ ├── .gitkeep │ ├── common │ ├── .gitkeep │ ├── RendezvousPlan.cpp │ └── SearchAlgorithms.cpp │ ├── communication │ ├── .gitkeep │ └── MockCommunicationModelNode.cpp │ ├── frontier │ ├── .gitkeep │ └── FrontierDiscoveryNode.cpp │ ├── laser │ ├── .gitkeep │ └── LaserToWorldNode.cpp │ ├── localization │ ├── .gitkeep │ ├── AverageVelocityEstimatorNode.cpp │ ├── GmappingPoseNode.cpp │ └── RelativePoseEstimatorNode.cpp │ ├── map │ ├── .gitkeep │ ├── CSpaceNode.cpp │ ├── LocalCSpaceNode.cpp │ └── MapStitchingNode.cpp │ ├── navigation │ ├── .gitkeep │ ├── IntegratedGlobalPlannerNode.cpp │ └── LocalPlannerNode.cpp │ └── policies │ ├── .gitkeep │ ├── Alysson2024Node.cpp │ ├── RandomizedSocialWelfareNode.cpp │ └── Yamauchi1999Node.cpp ├── multirobotsimulations ├── CMakeLists.txt ├── launch │ ├── .gitkeep │ ├── gazebo_add_robot.launch │ └── gazebo_multi_robot_bringup.launch ├── msg │ ├── .gitkeep │ ├── CustomImage.msg │ ├── CustomOcc.msg │ ├── CustomPose.msg │ ├── Frontiers.msg │ ├── MockPackage.msg │ └── rendezvous.msg ├── package.xml ├── robots │ ├── .gitkeep │ └── pioneer3at │ │ ├── meshes │ │ ├── Coordinates │ │ ├── axle.stl │ │ ├── back_sonar.stl │ │ ├── chassis.stl │ │ ├── front_sonar.stl │ │ ├── left_hubcap.stl │ │ ├── right_hubcap.stl │ │ ├── top.stl │ │ └── wheel.stl │ │ └── urdf │ │ └── pioneer3at.urdf └── setup.py └── scripts ├── .gitkeep ├── launch_robots.sh ├── run_simulation.sh ├── start_exploration.sh └── stop_forced.sh /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | github: [Ophien, multirobotplayground] 2 | custom: ["https://www.paypal.com/donate/?business=YWAAG5LVWXBQC&no_recurring=0&item_name=Support+Open+Source+mobile+robots+projects+for+search+and+rescue+in+natural+disasters.+Your+donation+can+change+lives%21¤cy_code=USD"] -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | install/ 7 | log/ 8 | msg_gen/ 9 | srv_gen/ 10 | msg/*Action.msg 11 | msg/*ActionFeedback.msg 12 | msg/*ActionGoal.msg 13 | msg/*ActionResult.msg 14 | msg/*Feedback.msg 15 | msg/*Goal.msg 16 | msg/*Result.msg 17 | msg/_*.py 18 | build_isolated/ 19 | devel_isolated/ 20 | 21 | # Generated by dynamic reconfigure 22 | *.cfgc 23 | /cfg/cpp/ 24 | /cfg/*.py 25 | 26 | # Ignore generated docs 27 | *.dox 28 | *.wikidoc 29 | 30 | # eclipse stuff 31 | .project 32 | .cproject 33 | 34 | # qcreator stuff 35 | CMakeLists.txt.user 36 | 37 | srv/_*.py 38 | *.pcd 39 | *.pyc 40 | qtcreator-* 41 | *.user 42 | 43 | /planning/cfg 44 | /planning/docs 45 | /planning/src 46 | 47 | *~ 48 | 49 | # Emacs 50 | .#* 51 | 52 | # Catkin custom files 53 | CATKIN_IGNORE 54 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | Copyright (C) 2020 Alysson Ribeiro da Silva 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | drawing 3 |

4 | 5 | # [Table of Contents](#table-of-contents) 6 | 7 | - [Setup](docs/working_environment.md) 8 | - [Usage](docs/usage.md) 9 | - [Robots](docs/robots.md) 10 | - [Maps](docs/maps.md) 11 | - [Nodes](docs/multi-robot-exploration.md) 12 | - [Multi-robot Guidelines](docs/multi-robot-guidelines.md) 13 | - [Documentation](https://github.com/multirobotplayground/Noetic-Sandbox-Docs) 14 | - [Relevant Literature in Multi-robot Exploration](docs/references.md) 15 | - [Contributing](docs/contributing.md) 16 | 17 | ## [ROS-Noetic-Multi-robot-Sandbox](#ros-noetic-multi-robot-sandbox) 18 | 19 | This workspace is a sandbox for multi-robot research. 20 | 21 | Initially, it includes a fully opperational multi-robot exploration stack for ROS Noetic and Ubuntu 20.04 that allows them to keep intermittent communication in Gazebo 11 simulations. It contains an implementation and integration of the structures from this research: [https://arxiv.org/abs/2309.13494](https://arxiv.org/abs/2309.13494). 22 | 23 | The deployment features components highlighted in red in the diagram, allowing robots to explore and maintain intermittent connectivity through a rendezvous plan. Communication is facilitated through topics with a mock communication model. 24 | 25 |

26 | drawing 27 |

28 | 29 | ## [What to Expect?](#what-to-expect) 30 | 31 | ### Fully opperational multi-robot exploration with communication constraints to accelerate your research and development. 32 | 33 |

34 | drawing 35 |

36 | 37 | ### Complex stack integrated with customized nodes for ROS Noetic, with SLAM, map stitching, trajectory optimization, global and local planning, and more, to simulate a realistic multi-robot exploration application with communication constraints, visualization, and configurations for Gazebo 11 and rviz. 38 | 39 |

40 | drawing 41 |

42 | 43 | ### Intermittent Communication Policy to share information at rendezvous locations spread dynamically while exploration happens. 44 | 45 |

46 | drawing 47 |

48 | 49 | ## [Disclaimer](#disclaimer) 50 | 51 | This workspace is not based on the ```move_base``` and the ```nav``` stack for navigation, because they had many drawbacks when I first tried to used them for multi-robot exploration research. Therefore, this project contains its own ```sub-goal``` navigation node with ```global``` and ```local planners``` to handle situations posed by ```multi-robot exploration``` applications when robots do ```SLAM``` individually, have to deal with traffic, and share maps. Furthermore, I'm currently refactoring all nodes to make them more friendly with ROS 2 standards. 52 | 53 | 54 | ## [Features Roadmap](#features-roadmap) 55 | 56 | | Feature | Status | 57 | |-|-| 58 | | Enhance Documentation | in-progress | 59 | | [ROS 2 Jazzy Jalisco Integration](https://github.com/Ophien/ROS-Jazzy-Multi-robot-Sandbox) | in-progress | 60 | | [Add Real World Deployment](https://youtu.be/8FP3UK7kpRA?feature=shared) | | 61 | | Youtube Channel | | 62 | | Add Robust Testing | | 63 | | Add Probabilistic-based Global Planner | | 64 | | Add Pose Graph-base Mapping | | 65 | | Add Pose Graph Merging | | 66 | | Add Heterogeneous Robot Teams Support | | 67 | | Add Human Computer Interfaces for Ground Teams | | 68 | | Add Mission Assignment and Inspector | | 69 | | Add Other Baselines and Algorithms | | 70 | | Add Other Coordination Methods | | 71 | | [Add More Realistic Maps and Environments for Fuel](https://app.gazebosim.org/fuel/models) | | 72 | | [Add Safety Mechanisms for Navigation and Traffic Avoidance](https://youtu.be/HSFmGdI0TAY?si=gvVgttymSCMpddl7) | | 73 | | Unity3D integration | | 74 | 75 | ## [Publications](#publications) 76 | 77 | If this workspace is somehow useful to you, consider mentioning this paper accepted on IROS. 78 | 79 | > A. R. da Silva, L. Chaimowicz, T. C. Silva, and A. Hsieh, Communication-Constrained Multi-Robot Exploration with Intermittent Rendezvous. 2024. 80 | 81 | ```text 82 | @misc{dasilva2024communicationconstrained, 83 | title={Communication-Constrained Multi-Robot Exploration with Intermittent Rendezvous}, 84 | author={Alysson Ribeiro da Silva and Luiz Chaimowicz and Thales Costa Silva and Ani Hsieh}, 85 | year={2024}, 86 | eprint={2309.13494}, 87 | archivePrefix={arXiv}, 88 | primaryClass={cs.RO} 89 | } 90 | ``` 91 | 92 | ## [Acknowledgements](#acknowledge) 93 | 94 | The research paper [https://arxiv.org/abs/2309.13494](https://arxiv.org/abs/2309.13494) was supported by CAPES, FAPEMIG, CNPQ, and ARL DCIST CRA W911NF-17-2-0181. 95 | 96 | ## [Support this Project](#support-this-project) 97 | 98 | Support Open Source mobile robots projects for search and rescue in natural disasters. 99 | 100 | [![Donate](docs/images/Donate-PayPal-green-usd.png)](https://www.paypal.com/donate/?business=YWAAG5LVWXBQC&no_recurring=0&item_name=Support+Open+Source+mobile+robots+projects+for+search+and+rescue+in+natural+disasters.+Your+donation+can+change+lives%21¤cy_code=USD) 101 | [![Donate](docs/images/Donate-PayPal-green-brl.png)](https://www.paypal.com/donate/?business=YWAAG5LVWXBQC&no_recurring=0&item_name=Support+Open+Source+mobile+robots+projects+for+search+and+rescue+in+natural+disasters.+Your+donation+can+change+lives%21¤cy_code=BRL) 102 | 103 | ## [License](#license) 104 | 105 | All content from this repository is released under a [GPLv3 license](LICENSE). 106 | 107 | Author/Maintainer: 108 | 109 | - [Alysson Ribeiro da Silva](https://alysson.thegeneralsolution.com/) 110 | 111 | emails: 112 | 113 | - 114 | 115 | 116 | ## [Bug & Feature Requests](#bug--feature-requests) 117 | 118 | Please report bugs and do your requests to add new features through the [Issue Tracker](https://github.com/multirobotplayground/ROS-Noetic-Multi-robot-Sandbox/issues). 119 | -------------------------------------------------------------------------------- /docs/contributing.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | Write commits with two components: 4 | 5 | 1. description in imperative mood without ending with a period. 6 | 2. sequence of structured paragraphs with detailed explanation of what you have changed. 7 | 8 | Example: 9 | 10 | ```text 11 | Remove unused function 12 | 13 | The function was unecessary, because of this and that. 14 | Something will be added according to the new roadmap as specified during the meeting. 15 | ``` 16 | -------------------------------------------------------------------------------- /docs/images/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/.gitkeep -------------------------------------------------------------------------------- /docs/images/Donate-PayPal-green-brl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/Donate-PayPal-green-brl.png -------------------------------------------------------------------------------- /docs/images/Donate-PayPal-green-usd.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/Donate-PayPal-green-usd.png -------------------------------------------------------------------------------- /docs/images/agreements.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/agreements.png -------------------------------------------------------------------------------- /docs/images/capes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/capes.png -------------------------------------------------------------------------------- /docs/images/cnpq.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/cnpq.png -------------------------------------------------------------------------------- /docs/images/dcc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/dcc.png -------------------------------------------------------------------------------- /docs/images/dcist.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/dcist.png -------------------------------------------------------------------------------- /docs/images/example_exploration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/example_exploration.png -------------------------------------------------------------------------------- /docs/images/fapemig.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/fapemig.jpg -------------------------------------------------------------------------------- /docs/images/forestworld.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/forestworld.png -------------------------------------------------------------------------------- /docs/images/forestworld2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/forestworld2.png -------------------------------------------------------------------------------- /docs/images/forestworld3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/forestworld3.png -------------------------------------------------------------------------------- /docs/images/glass_sandbox_project_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/glass_sandbox_project_logo.png -------------------------------------------------------------------------------- /docs/images/penn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/penn.png -------------------------------------------------------------------------------- /docs/images/pioneer3at.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/pioneer3at.png -------------------------------------------------------------------------------- /docs/images/rosgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/rosgraph.png -------------------------------------------------------------------------------- /docs/images/run_simulation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/run_simulation.png -------------------------------------------------------------------------------- /docs/images/rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/rviz.png -------------------------------------------------------------------------------- /docs/images/rviz_trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/rviz_trajectory.png -------------------------------------------------------------------------------- /docs/images/rvizexploration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/rvizexploration.png -------------------------------------------------------------------------------- /docs/images/sandbox_project_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/sandbox_project_logo.png -------------------------------------------------------------------------------- /docs/images/shared_controls_treated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/shared_controls_treated.png -------------------------------------------------------------------------------- /docs/images/spread.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/spread.png -------------------------------------------------------------------------------- /docs/images/stack.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/stack.png -------------------------------------------------------------------------------- /docs/images/stack_components.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/stack_components.png -------------------------------------------------------------------------------- /docs/images/stack_components_high.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/stack_components_high.png -------------------------------------------------------------------------------- /docs/images/tf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/tf.png -------------------------------------------------------------------------------- /docs/images/tmux.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/tmux.png -------------------------------------------------------------------------------- /docs/images/trajectories before.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/trajectories before.png -------------------------------------------------------------------------------- /docs/images/trajectoriesafter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/trajectoriesafter.png -------------------------------------------------------------------------------- /docs/images/ufmg-logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/ufmg-logo.png -------------------------------------------------------------------------------- /docs/images/verlab_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/images/verlab_logo.png -------------------------------------------------------------------------------- /docs/maps.md: -------------------------------------------------------------------------------- 1 | # [Table of Contents](#table-of-contents) 2 | 3 | - [Table of Contents](#table-of-contents) 4 | - [Box World](#box-world) 5 | - [Forest](#forest) 6 | 7 | ## [Box World](#box-world) 8 | 9 | I've configured the box world to run on low-spec computers, such as old laptops. Feel free to use it if you find it suitable. If you want to run this world, you must change the [run_simulation.sh](../src/scripts/run_simulation.sh) world parameters. 10 | 11 | **World File:** [boxes.world](../gazebo_resources/worlds/boxes.world) 12 | 13 |

14 | drawing 15 |

16 | 17 | ## [Forest](#forest) 18 | 19 | Alternatively, I've designed the forest environment to include irregular obstacles. However, you will need a high-end computer to run it smoothly. By default, this world is launched by the [run_simulation.sh](../src/scripts/run_simulation.sh) script. 20 | 21 | **World File:** [forest.world](../gazebo_resources/worlds/forest.world) 22 | 23 |

24 | drawing 25 |
26 | drawing 27 |
28 | drawing 29 |

-------------------------------------------------------------------------------- /docs/multi-robot-exploration.md: -------------------------------------------------------------------------------- 1 | # Multi-robot Exploration 2 | 3 | It contains a multi-robot exploration stack and an implementation and integration of this [research](https://arxiv.org/abs/2309.13494) that allow you to start your development out-of-the-box. 4 | 5 | ## Nodes 6 | 7 | - [Policies](../docs/multi-robot-exploration.md) 8 | - [Yamauchi1999 node](../docs/nodes/yamauchi1999_node.md) 9 | - [RandomizedSocialWelfare node](../docs/nodes/randomized_social_welfare_node.md) 10 | - [Alysson2024 node](../docs/nodes/alysson2024_node.md) 11 | - [Navigation](../docs/multi-robot-exploration.md) 12 | - [LocalPlannerNode](../docs/nodes/local_planner_node.md) 13 | - [IntegratedGlobalPlannerNode](../docs/nodes/integrated_global_planner_node.md) 14 | - [Mapping](../docs/multi-robot-exploration.md) 15 | - [CSpaceNode](../docs/nodes/cspace_node.md) 16 | - [MapStitchingNode](../docs/nodes/map_stitching_node.md) 17 | - [LocalCSpaceNode](../docs/nodes/local_cspace_node.md) 18 | - [Localization](../docs/multi-robot-exploration.md) 19 | - [AverageVelocityEstimatorNode](../docs/nodes/average_velocity_node.md) 20 | - [GmappingPoseNode](../docs/nodes/gmapping_pose_node.md) 21 | - [RelativePoseEstimatorNode](../docs/nodes/relative_pose_estimator_node.md) 22 | - [Lidar](../docs/multi-robot-exploration.md) 23 | - [LaserToWorldNode](../docs/nodes/laser_to_world_node.md) 24 | - [Frontiers](../docs/multi-robot-exploration.md) 25 | - [FrontierDiscoveryNode](../docs/nodes/frontier_discovery_node.md) 26 | - [Communication](../docs/multi-robot-exploration.md) 27 | - [MockCommunicationModelNode](../docs/nodes/mock_communication_model_node.md) 28 | - [Objects and Algorithms](../docs/multi-robot-exploration.md) 29 | - [RendezvosPlan](../src/multirobotexploration/source/common/RendezvousPlan.cpp) 30 | - [SearchAlgorithms](../src/multirobotexploration/source/common/SearchAlgorithms.cpp) 31 | - [Common](../src/multirobotexploration/include/common/Common.h) -------------------------------------------------------------------------------- /docs/multi-robot-guidelines.md: -------------------------------------------------------------------------------- 1 | # [Table of Contents](#table-of-contents) 2 | 3 | - [Table of Contents](#table-of-contents) 4 | - [Tmux and Session Manager](#tmux-and-session-manager) 5 | - [Traffic Avoidance](#traffic-avoidance) 6 | - [Subscriptions Through Lambda](#subscriptions-through-lambda) 7 | 8 | ## [Tmux and Session Manager](#tmux-and-session-manager) 9 | 10 | Many students and professionals I've encountered do not rely on tools such as ```tmux``` for visualization and organizational purposes when working with ROS. Consequently, they end up running complex systems with numerous messages, warnings, and errors in their terminal, which degrades debugging and visualization quality. In the multi-robot case, which is considerably more complicated, the situations is even worse. Consequently, I highly encourage the use of ```tmux``` to manage ROS-based multi-robot systems. 11 | 12 | For example, the following terminal session, 13 | 14 |

15 | drawing 16 |

17 | 18 | can be created by simply calling ```tmux``` with the ```new-session``` command. 19 | 20 | ```bash 21 | tmux new-session -n session_manager -s simulation -d "roslaunch multirobotsimulations gazebo_multi_robot_bringup.launch world_name:=forest.world paused:=true" 22 | ``` 23 | 24 | Breaking this command down, the ```-n``` flag specifies the window name, which is ```session_manager```, the ```-s``` command specifies the session ID, which is ```simulation```, and the ```-d``` command specifies the command line that will be run, which is just a ros command ```roslaunch multirobotsimulations gazebo_multi_robot_bringup.launch world_name:=forest.world paused:=true```. 25 | 26 | To launch the created session in a terminal, just open a terminal manager, such as GNOME Terminal, and tell tmux to attach to the created session ID. 27 | 28 | ```bash 29 | gnome-terminal -t simulation_terminal --geometry=150x20 --hide-menubar -- tmux attach-session -t simulation 30 | ``` 31 | 32 | You can run as many windows as you need for any number of components, attach them to the same session ID, and keep things clean. See the [launch_robots.sh](../src/scripts/launch_robots.sh) scripts for more examples. 33 | 34 | ## [Traffic Avoidance](#traffic-avoidance) 35 | 36 | A huge problem in multi-robot systems is traffic management. There are a lot of solutions to cope with this, such as using [level sets](https://en.wikipedia.org/wiki/Level_set) when optimizing trajectories. In this project 37 | I'm using a simple mechanism, where robots share their controls ```n``` time steps in the future to all robots they can communicate with when enhance their global and local configuration spaces for planning. The system is fully distributed, however, robots can act as fully rational agents without sharing their controls by replacing this mechanism with anything that predicts others' trajectories. 38 | 39 | In the RViz window, the controls of other robots should appear as ```polygon``` obstacles. The following image highlights them as orange and blue striped blobs. 40 | 41 |

42 | drawing 43 |

44 | 45 | ## [Subscriptions Through Lambda](#sumbscriptions-through-lambda) 46 | 47 | When I started working with multiple robots, I encountered a problem that many students also face: the lack of a robust mechanism to create dynamic subscriptions given a variable number of robots. Fortunately, C++11 provides a mechanism for declaring lambda functions, which can help address this issue. In this workspace, you will find some nodes where I have declared subscriptions to other robots' nodes using lambda functions. This approach allows for a variable number of callbacks without explicitly declaring the functions in an object's body. 48 | 49 | Below is an example of how to dynamically subscribe to a variable number of robots' ```/robot_/fusion``` topics, which receive maps from each of them, from the [MapStitchingNode.cpp](../src/multirobotexploration/source/map/MapStitchingNode.cpp). 50 | 51 | ```c++ 52 | for(int robot = 0; robot < aRobots; ++robot) { 53 | if(robot == aId) continue; 54 | 55 | nav_msgs::OccupancyGrid* occPtr = &aRobotsOcc[robot]; 56 | std_msgs::Int8MultiArray* commPtr = &aRobotsInCommMsg; 57 | std::vector* receivedFlagPtr = &aReceivedOccs; 58 | aSubscribers.push_back( 59 | node_handle.subscribe( 60 | "/robot_" + std::to_string(robot) + "/fusion", 61 | aQueueSize, 62 | [occPtr, receivedFlagPtr, commPtr, robot](nav_msgs::OccupancyGrid::ConstPtr msg){ 63 | // mock communication to merge maps 64 | // if this flag is 0, then the 'robot' cannot 65 | // share its map 66 | if(commPtr->data[robot] == 0) return; 67 | 68 | // otherwise, update the last received map 69 | occPtr->data.assign(msg->data.begin(), msg->data.end()); 70 | occPtr->info = msg->info; 71 | occPtr->header = msg->header; 72 | 73 | // set received flag to true 74 | receivedFlagPtr->at(robot) = true; 75 | })); 76 | } 77 | ``` 78 | -------------------------------------------------------------------------------- /docs/multi-robot-simulations.md: -------------------------------------------------------------------------------- 1 | # Multi-robot-Simulations 2 | 3 | It contains simulations that allow you to start your development out-of-the-box. There is a main launch file that takes care of everything for you, bringing up all robots, transforms, and topics organized by robot namespace. 4 | -------------------------------------------------------------------------------- /docs/nodes/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/docs/nodes/.gitkeep -------------------------------------------------------------------------------- /docs/nodes/alysson2024_node.md: -------------------------------------------------------------------------------- 1 | # Alysson2024Node 2 | 3 | This node uses the ```Randomized Social Welfare``` and implements the Intermittent Communication policy from this [research](https://arxiv.org/abs/2309.13494). 4 | 5 | Source: [Alysson2024Node.cpp](../../src/multirobotexploration/source/policies/Alysson2024Node.cpp) 6 | 7 | ## Parameters 8 | 9 | * ```/robots``` 10 | 11 | Number of robots in the pack. This is a global parameter. 12 | 13 | * ```id``` 14 | 15 | Id of this robot. 16 | 17 | * ```rate``` 18 | 19 | Main loop rate in hertz. 20 | 21 | * ```queue_size``` 22 | 23 | Queue size of publishers and subscribers. 24 | 25 | * ```/first_rendezvous``` 26 | 27 | This is a global parameter with the pose, ```{x: 0, y: 0, z: 0}``` , with the first rendezvous location for all sub-teams. 28 | 29 | * ```/footprint_robot_``` 30 | 31 | This is a global parameter with the pose that represents the position of this robot in the rendezvous footprint, where `````` is the id of this robot. 32 | 33 | ## Subscribed Topics 34 | 35 | * ```/c_space``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 36 | 37 | The configuration space with inflated obstacles for navigation. It must handle other robots, dynamic and static obstacles. The `````` specifies this robot's namespace. 38 | 39 | * ```/frontier_discovery/frontiers_clusters``` ([multirobotsimulations::Frontiers](../../src/multirobotsimulations/msg/Frontiers.msg)) 40 | 41 | Frontiers from the frontiers node. They must be filtered and are visibile only for this robot. The `````` specifies this robot's namespace. 42 | 43 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 44 | 45 | Custom pose used throughout the system, it contains the robot id and a pose. The `````` specifies this robot's namespace. 46 | 47 | * ```/sub_goal_nav/finish``` ([std_msgs::String](../../src/multirobotsimulations/msg/CustomPose.msg)) 48 | 49 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 50 | 51 | * ```/explorer/set_idle``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 52 | 53 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 54 | 55 | * ```/explorer/set_exploring``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 56 | 57 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 58 | 59 | * ```/global_explorer/back_to_base``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 60 | 61 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 62 | 63 | * ```/global_explorer/set_exploring``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 64 | 65 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 66 | 67 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Int8MultiArray.html)) 68 | 69 | This topic receives an integer array from the communication handler, where each position i is 1 if this robot can communicate with i and 0 otherwise. The `````` specifies this robot's namespace. 70 | 71 | * ```/robot_/realizing_plan``` ([multirobotsimulations::rendezvous](../../src/multirobotsimulations/msg/rendezvous.msg)) 72 | 73 | This is a broadcast channel used to receive messages from others that are trying to realize rendezvous plan and that can communicate with this robot. In particullar, this robot creates a subscriber for each robot available during the exploration and identify each subscriber by the `````` in the topic's name. 74 | 75 | * ```/robot_/plan_updater``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 76 | 77 | This is a broadcast channel used by any robot to receive rendezvous locations updates for its sub-team. 78 | 79 | ## Published Topics 80 | 81 | * ```/sub_goal_nav/goal``` ([geometry_msgs::Pose](https://docs.ros.org/en/api/geometry_msgs/html/msg/Pose.html)) 82 | 83 | The goal location to explore to the sub goal navigation module. 84 | 85 | * ```/frontier_discovery/compute``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 86 | 87 | Communication channel with the frontier discovery module. Used to ask for frontiers. 88 | 89 | * ```/realizing_plan``` ([multirobotsimulations::rendezvous](../../src/multirobotsimulations/msg/rendezvous.msg)) 90 | 91 | This topic is used to tell other robots which rendezvous agreement this robot is realizing. It holds this robot's ```id``` and an unique identifier associated with a rendezvous agreement. The `````` specifies this robot's namespace. 92 | 93 | * ```/plan_updater``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 94 | 95 | This robot uses this topic to send a new rendezvous location for the sub-team it is doing the rendezvous, if it is the consensus robot. The consensus robot is the robot if the lowest unique identifier from the sub-team that is doing the rendezvous. Rendezvous also have unique identifiers. 96 | 97 | 98 | 101 | -------------------------------------------------------------------------------- /docs/nodes/average_velocity_node.md: -------------------------------------------------------------------------------- 1 | # AverageDisplacement 2 | 3 | Source: [AverageVelocityNode.cpp](../../src/multirobotexploration/source/localization/AverageVelocityNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```id``` 8 | 9 | * ```rate``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```count``` 14 | 15 | ## Subscribed Topics 16 | 17 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 18 | 19 | ## Published Topics 20 | 21 | * ```/average_velocity`` ([std_msgs::Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html)) 22 | 23 | 26 | -------------------------------------------------------------------------------- /docs/nodes/cspace_node.md: -------------------------------------------------------------------------------- 1 | # CSpaceNode 2 | 3 | Source: [CSpaceNode.cpp](../../src/multirobotexploration/source/map/CSpaceNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```id``` 8 | 9 | * ```rate``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```max_lidar_range``` 14 | 15 | * ```free_inflation_radius``` 16 | 17 | * ```ocu_inflation_radius``` 18 | 19 | * ```lidar_sources``` 20 | 21 | ## Subscribed Topics 22 | 23 | * ```/map``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 24 | 25 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 26 | 27 | * ```/laser_to_world/lidar_occ_``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 28 | 29 | * ```/robot_/local_planner/optimal_poses``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 30 | 31 | ## Published Topics 32 | 33 | * ```/c_space``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 34 | 35 | 38 | -------------------------------------------------------------------------------- /docs/nodes/frontier_discovery_node.md: -------------------------------------------------------------------------------- 1 | # FrontierDiscoveryNode 2 | 3 | Source: [FrontierDiscoveryNode.cpp](../../src/multirobotexploration/source/frontier/FrontierDiscoveryNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```id``` 8 | 9 | * ```rate``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```max_lidar_range``` 14 | 15 | ## Subscribed Topics 16 | 17 | * ```/c_space``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 18 | 19 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 20 | 21 | * ```/frontier_discovery/compute``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 22 | 23 | ## Published Topics 24 | 25 | * ```/frontier_discovery/frontiers_clusters_markers``` ([visualization_msgs::Marker](https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html)) 26 | 27 | * ```/frontier_discovery/frontiers``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 28 | 29 | * ```/frontier_discovery/frontiers_clusters``` ([multirobotsimulations::Frontiers](../../src/multirobotsimulations/msg/Frontiers.msg)) 30 | 31 | 34 | -------------------------------------------------------------------------------- /docs/nodes/gmapping_pose_node.md: -------------------------------------------------------------------------------- 1 | # GmappingPoseNode 2 | 3 | Source: [GmappingPoseNode.cpp](../../src/multirobotexploration/source/localization/GmappingPoseNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```id``` 8 | 9 | * ```rate``` 10 | 11 | * ```queue_size``` 12 | 13 | ## Subscribed Topics 14 | 15 | * ```/map``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 16 | 17 | ## Published Topics 18 | 19 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 20 | 21 | * ```/gmapping_pose/pose_stamped``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 22 | 23 | * ```/path``` ([nav_msgs::Path](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html)) 24 | 25 | 28 | -------------------------------------------------------------------------------- /docs/nodes/integrated_global_planner_node.md: -------------------------------------------------------------------------------- 1 | # IntegratedGlobalPlannerNode 2 | 3 | Source: [IntegratedGlobalPlannerNode.cpp](../../src/multirobotexploration/source/navigation/IntegratedGlobalPlannerNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```id``` 8 | 9 | Id of this robot. 10 | 11 | * ```rate``` 12 | 13 | Main loop rate in hertz. 14 | 15 | * ```queue_size``` 16 | 17 | Queue size of publishers and subscribers. 18 | 19 | * ```reach_threshold``` 20 | 21 | * ```stuck_time_threshold``` 22 | 23 | ## Subscribed Topics 24 | 25 | * ```/c_space``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 26 | 27 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 28 | 29 | * ```/integrated_global_planner/goal``` ([geometry_msgs::Pose](https://docs.ros.org/en/api/geometry_msgs/html/msg/Pose.html)) 30 | 31 | * ```/integrated_global_planner/stop``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 32 | 33 | * ```/average_velocity``` ([std_msgs::Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html)) 34 | 35 | ## Published Topics 36 | 37 | * ```/integrated_global_planner/path``` ([visualization_msgs::Marker](https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html)) 38 | 39 | * ```/integrated_global_planner/finish``` ([std_msgs::String](https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html)) 40 | 41 | * ```/integrated_global_planner/current_path``` ([nav_msgs::Path](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html)) 42 | 43 | 46 | -------------------------------------------------------------------------------- /docs/nodes/laser_to_world_node.md: -------------------------------------------------------------------------------- 1 | # LaserToWorldNode 2 | 3 | Source: [LaserToWorldNode.cpp](../../src/multirobotexploration/source/laser/LaserToWorldNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```rate``` 8 | 9 | * ```queue_size``` 10 | 11 | * ```x``` 12 | 13 | * ```y``` 14 | 15 | * ```z``` 16 | 17 | * ```r``` 18 | 19 | * ```p``` 20 | 21 | * ```y``` 22 | 23 | 24 | ## Subscribed Topics 25 | 26 | * ```/scan``` ([sensor_msgs::LaserScan](https://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html)) 27 | 28 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 29 | 30 | * ```/map``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 31 | 32 | ## Published Topics 33 | 34 | * ```/laser_to_world/laser_world``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 35 | 36 | * ```/laser_to_world/laser_occ``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 37 | 38 | 41 | -------------------------------------------------------------------------------- /docs/nodes/local_cspace_node.md: -------------------------------------------------------------------------------- 1 | # LocalCSpaceNode 2 | 3 | Source: [LocalCSpaceNode.cpp](../../src/multirobotexploration/source/map/LocalCSpaceNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```id``` 8 | 9 | * ```rate``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```max_lidar_range``` 14 | 15 | * ```free_inflation_radius``` 16 | 17 | * ```ocu_inflation_radius``` 18 | 19 | * ```lidar_sources``` 20 | 21 | * ```local_view_size``` 22 | 23 | ## Subscribed Topics 24 | 25 | * ```/map``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 26 | 27 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 28 | 29 | * ```/laser_to_world/lidar_occ_``` ([geometry_msgs::PoseArray](ttps://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 30 | 31 | * ```/robot_/local_planner/optimal_poses``` ([geometry_msgs::PoseArray](ttps://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 32 | 33 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](ttps://docs.ros.org/en/api/std_msgs/html/msg/Int8MultiArray.html)) 34 | 35 | ## Published Topics 36 | 37 | * ```/c_space_local``` ([nav_msgs::OccupancyGrid](ttps://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 38 | 39 | * ```/local_occupied_poses``` ([geometry_msgs::PoseArray](ttps://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 40 | 41 | * ```/local_free_poses``` ([geometry_msgs::PoseArray](ttps://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 42 | 43 | 46 | -------------------------------------------------------------------------------- /docs/nodes/local_planner_node.md: -------------------------------------------------------------------------------- 1 | # LocalPlannerNode 2 | 3 | Source: [LocalPlannerNode.cpp](../../src/multirobotexploration/source/navigation/LocalPlannerNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```/robots``` 8 | 9 | * ```rate``` 10 | 11 | * ```controls_to_share``` 12 | 13 | * ```waypoints_to_use``` 14 | 15 | * ```via_points_increment``` 16 | 17 | * ```use_priority_stop_behavior``` 18 | 19 | ## Subscribed Topics 20 | 21 | * ```/costmap_converter/obstacles``` ([costmap_converter::ObstacleArrayMsg]) 22 | 23 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 24 | 25 | * ```/sub_goal_nav/current_path``` ([nav_msgs::Path](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html)) 26 | 27 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Int8MultiArray.html)) 28 | 29 | ## Published Topics 30 | 31 | * ```/cmd_vel``` ([geometry_msgs::Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html)) 32 | 33 | * ```/local_planner/optimal_poses``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 34 | 35 | * ```/mre_local_planner/global_via_points``` ([visualization_msgs::Marker](https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html)) 36 | 37 | 40 | -------------------------------------------------------------------------------- /docs/nodes/map_stitching_node.md: -------------------------------------------------------------------------------- 1 | # MapStitchingNode 2 | 3 | Source: [MapStitchingNode.cpp](../../src/multirobotexploration/source/map/MapStitchingNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```/robots``` 8 | 9 | * ```id``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```rate``` 14 | 15 | ## Subscribed Topics 16 | 17 | * ```/relative_pose_estimator/relative_start``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 18 | 19 | * ```/map``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 20 | 21 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Int8MultiArray.html)) 22 | 23 | * ```/robot_/fusion``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 24 | 25 | ## Published Topics 26 | 27 | * ```/fusion``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 28 | 29 | 32 | -------------------------------------------------------------------------------- /docs/nodes/mock_communication_model_node.md: -------------------------------------------------------------------------------- 1 | # MockCommunicationModelNode 2 | 3 | Source: [MockCommunicationModelNode.cpp](../../src/multirobotexploration/source/communication/MockCommunicationModelNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```/robots``` 8 | 9 | * ```id``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```comm_dist``` 14 | 15 | * ```rate``` 16 | 17 | * ```/start_pose_robot_``` 18 | 19 | ## Subscribed Topics 20 | 21 | * ```/robot_/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 22 | 23 | * ```/frontier_discovery/frontiers_clusters``` ([multirobotsimulations::Frontiers](../../src/multirobotsimulations/msg/Frontiers.msg)) 24 | 25 | ## Published Topics 26 | 27 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Int8MultiArray.html)) 28 | 29 | 32 | -------------------------------------------------------------------------------- /docs/nodes/randomized_social_welfare_node.md: -------------------------------------------------------------------------------- 1 | # RandomizedSocialWelfareNode 2 | 3 | This node implements a basic ```Utility Theory``` concept of randomizing the Social Welfare into Yamauchi's method for simple coordination. It relies in a configuration space, frontier discovery services, and a sub goal navigation module. Some of its functionality should be coded as services. 4 | 5 | Source: [RandomizedSocialWelfareNode.cpp](../../src/multirobotexploration/source/policies/RandomizedSocialWelfareNode.cpp) 6 | 7 | ## Parameters 8 | 9 | * ```/robots``` 10 | 11 | Number of robots in the pack. This is a global parameter. 12 | 13 | * ```id``` 14 | 15 | Id of this robot. 16 | 17 | * ```rate``` 18 | 19 | Main loop rate in hertz. 20 | 21 | * ```queue_size``` 22 | 23 | Queue size of publishers and subscribers. 24 | 25 | ## Subscribed Topics 26 | 27 | * ```/c_space``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 28 | 29 | The configuration space with inflated obstacles for navigation. It must handle other robots, dynamic and static obstacles. The `````` specifies this robot's namespace. 30 | 31 | * ```/frontier_discovery/frontiers_clusters``` ([multirobotsimulations::Frontiers](../../src/multirobotsimulations/msg/Frontiers.msg)) 32 | 33 | Frontiers from the frontiers node. They must be filtered and are visibile only for this robot. The `````` specifies this robot's namespace. 34 | 35 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 36 | 37 | Custom pose used throughout the system, it contains the robot id and a pose. The `````` specifies this robot's namespace. 38 | 39 | * ```/sub_goal_nav/finish``` ([std_msgs::String](../../src/multirobotsimulations/msg/CustomPose.msg)) 40 | 41 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 42 | 43 | * ```/explorer/set_idle``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 44 | 45 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 46 | 47 | * ```/explorer/set_exploring``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 48 | 49 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 50 | 51 | * ```/global_explorer/back_to_base``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 52 | 53 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 54 | 55 | * ```/global_explorer/set_exploring``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 56 | 57 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 58 | 59 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Int8MultiArray.html)) 60 | 61 | This topic receives an integer array from the communication handler, where each position i is 1 if this robot can communicate with i and 0 otherwise. The `````` specifies this robot's namespace. 62 | 63 | ## Published Topics 64 | 65 | * ```/sub_goal_nav/goal``` ([geometry_msgs::Pose](https://docs.ros.org/en/api/geometry_msgs/html/msg/Pose.html)) 66 | 67 | The goal location to explore to the sub goal navigation module. 68 | 69 | * ```/frontier_discovery/compute``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 70 | 71 | Communication channel with the frontier discovery module. Used to ask for frontiers. 72 | 73 | 76 | -------------------------------------------------------------------------------- /docs/nodes/relative_pose_estimator_node.md: -------------------------------------------------------------------------------- 1 | # RelativePoseEstimatorNode 2 | 3 | Source: [RelativePoseEstimatorNode.cpp](../../src/multirobotexploration/source/localization/RelativePoseEstimatorNode.cpp) 4 | 5 | ## Parameters 6 | 7 | * ```/robots``` 8 | 9 | * ```id``` 10 | 11 | * ```queue_size``` 12 | 13 | * ```rate``` 14 | 15 | * ```/start_pose_robot_``` 16 | 17 | ## Subscribed Topics 18 | 19 | * ```/robot_/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 20 | 21 | * ```/mock_communication_model/robots_in_comm``` ([std_msgs::Int8MultiArray](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 22 | 23 | ## Published Topics 24 | 25 | * ```/relative_pose_estimator/relative_start``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 26 | 27 | * ```/relative_pose_estimator/relative_poses``` ([geometry_msgs::PoseArray](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseArray.html)) 28 | 29 | * ```/relative_pose_estimator/distances``` ([std_msgs::Float64MultiArray](https://docs.ros.org/en/api/std_msgs/html/msg/Float64MultiArray.html)) 30 | 31 | * ```/relative_pose_estimator/pose_markers``` ([visualization_msgs::Marker](https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html)) 32 | 33 | * ```/relative_pose_estimator/pose_far_markers``` ([visualization_msgs::Marker](https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html)) 34 | 35 | 38 | -------------------------------------------------------------------------------- /docs/nodes/yamauchi1999_node.md: -------------------------------------------------------------------------------- 1 | # Yamauchi1999 2 | 3 | This node implements Yamauchi's method from 1999. It relies in a configuration space, frontier discovery services, and a sub goal navigation module. Some of its functionality should be coded as services. 4 | 5 | Source: [Yamauchi1999Node.cpp](../../src/multirobotexploration/source/policies/Yamauchi1999Node.cpp) 6 | 7 | ## Parameters 8 | 9 | * ```/robots``` 10 | 11 | Number of robots in the pack. This is a global parameter. 12 | 13 | * ```id``` 14 | 15 | Id of this robot. 16 | 17 | * ```rate``` 18 | 19 | Main loop rate in hertz. 20 | 21 | * ```queue_size``` 22 | 23 | Queue size of publishers and subscribers. 24 | 25 | ## Subscribed Topics 26 | 27 | * ```/c_space``` ([nav_msgs::OccupancyGrid](https://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html)) 28 | 29 | The configuration space with inflated obstacles for navigation. It must handle other robots, dynamic and static obstacles. The `````` specifies this robot's namespace. 30 | 31 | * ```/frontier_discovery/frontiers_clusters``` ([multirobotsimulations::Frontiers](../../src/multirobotsimulations/msg/Frontiers.msg)) 32 | 33 | Frontiers from the frontiers node. They must be filtered and are visibile only for this robot. The `````` specifies this robot's namespace. 34 | 35 | * ```/gmapping_pose/world_pose``` ([multirobotsimulations::CustomPose](../../src/multirobotsimulations/msg/CustomPose.msg)) 36 | 37 | Custom pose used throughout the system, it contains the robot id and a pose. The `````` specifies this robot's namespace. 38 | 39 | * ```/sub_goal_nav/finish``` ([std_msgs::String](../../src/multirobotsimulations/msg/CustomPose.msg)) 40 | 41 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 42 | 43 | * ```/explorer/set_idle``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 44 | 45 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 46 | 47 | * ```/explorer/set_exploring``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 48 | 49 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 50 | 51 | * ```/global_explorer/back_to_base``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 52 | 53 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 54 | 55 | * ```/global_explorer/set_exploring``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 56 | 57 | This topic is used to check wether the sub goal navigation module reached a goal. The `````` specifies this robot's namespace. 58 | 59 | ## Published Topics 60 | 61 | * ```/sub_goal_nav/goal``` ([geometry_msgs::Pose](https://docs.ros.org/en/api/geometry_msgs/html/msg/Pose.html)) 62 | 63 | The goal location to explore to the sub goal navigation module. 64 | 65 | * ```/frontier_discovery/compute``` ([std_msgs::String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) 66 | 67 | Communication channel with the frontier discovery module. Used to ask for frontiers. 68 | 69 | 72 | -------------------------------------------------------------------------------- /docs/references.md: -------------------------------------------------------------------------------- 1 | # Related Works 2 | 3 | This is a complementary literature review for the research paper: [https://arxiv.org/abs/2309.13494](https://arxiv.org/abs/2309.13494) 4 | 5 | There are several strategies to solve the Multi-robot Exploration and share information with other agents. For example, one might let the robots explore and meet opportunistically [[1](https://www.sciencedirect.com/science/article/abs/pii/S0921889099000469),[2](https://dl.acm.org/doi/10.5555/647288.723404),[3](https://ieeexplore.ieee.org/document/844100),[4](https://ieeexplore.ieee.org/document/1013690),[5](https://ieeexplore.ieee.org/document/10341485)]. Differently, a base station can be used to ensure that the information is gathered and shared at a specific rate [[6](https://ieeexplore.ieee.org/document/5980179), [7](https://link.springer.com/chapter/10.1007/978-3-662-43645-5_5), [8](https://ieeexplore.ieee.org/document/9361138)]. Another approach would be to let the robots remain connected and move as a structure that can adapt to accomplish tasks [[9](https://ieeexplore.ieee.org/document/7889035), [10](https://ieeexplore.ieee.org/document/8610112), [11](https://ieeexplore.ieee.org/document/9197109), [12](https://ieeexplore.ieee.org/document/9683512)]. Additionally, a relay network can be used, where some robots act as data mules or static network relays [[13](https://ieeexplore.ieee.org/document/9837416)]. As an alternative, robots can plan their trajectories to be intermittently connected [[14](https://ieeexplore.ieee.org/document/6177277), [15](https://ieeexplore.ieee.org/document/8612974), [16](https://ieeexplore.ieee.org/document/9341636)]. These strategies have their advantages in meeting some requirements. However, they can also introduce constraints into the robots’ motion, which is what we try to avoid. 6 | 7 | # References 8 | 9 | [[1] B. Yamauchi, “Decentralized coordination for multirobot exploration,” Robotics and Autonomous Systems, vol. 29, no. 2, pp. 111–118, 1999.[Online]. Available: https://www.sciencedirect.com/science/article/pii/S0921889099000469](https://www.sciencedirect.com/science/article/abs/pii/S0921889099000469) 10 | 11 | [[2] R. Simmons, D. Apfelbaum, W. Burgard, M. Fox, D. an Moors, S. Thrun, and H. Younes, “Coordination for multi-robot exploration and mapping,” in Proceedings of the AAAI National Conference on Artificial Intelligence. Austin, TX: AAAI, 2000.](https://dl.acm.org/doi/10.5555/647288.723404) 12 | 13 | [[3] W. Burgard, M. Moors, D. Fox, R. Simmons, and S. Thrun, “Collaborative multi-robot exploration,” in Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), vol. 1, 2000, pp. 476–481 vol.1.](https://ieeexplore.ieee.org/document/844100) 14 | 15 | [[4] R. Zlot, A. Stentz, M. Dias, and S. Thayer, “Multi-robot exploration controlled by a market economy,” in Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292), vol. 3, 2002, pp. 3016–3023 vol.3.](https://ieeexplore.ieee.org/document/1013690) 16 | 17 | [[5] S. Bone, L. Bartolomei, F. Kennel-Maushart, and M. Chli, “Decentralised multi-robot exploration using monte carlo tree search,” in 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2023, pp. 7354–7361.](https://ieeexplore.ieee.org/document/10341485) 18 | 19 | [[6] E. Stump, N. Michael, V. Kumar, and V. Isler, “Visibility-based deployment of robot formations for communication maintenance,” in 2011 IEEE International Conference on Robotics and Automation, 2011, pp. 4498–4505.](https://ieeexplore.ieee.org/document/5980179) 20 | 21 | [[7] V. Spirin, S. Cameron, and J. de Hoog, “Time preference for information in multi-agent exploration with limited communication,” in Towards Autonomous Robotic Systems, A. Natraj, S. Cameron, C. Melhuish, and M. Witkowski, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 2014, pp. 34–45.](https://link.springer.com/chapter/10.1007/978-3-662-43645-5_5) 22 | 23 | [[8] L. Clark, J. Galante, B. Krishnamachari, and K. Psounis, “A queue stabilizing framework for networked multi-robot exploration,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 2091–2098, 2021.](https://ieeexplore.ieee.org/document/9361138) 24 | 25 | [[9] A. Gasparri, L. Sabattini, and G. Ulivi, “Bounded control law for global connectivity maintenance in cooperative multirobot systems,” IEEE Transactions on Robotics, vol. 33, no. 3, pp. 700–717, 2017.](https://ieeexplore.ieee.org/document/7889035) 26 | 27 | [[10] K. Khateri, M. Pourgholi, M. Montazeri, and L. Sabattini, “A comparison between decentralized local and global methods for connectivity maintenance of multi-robot networks,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 633–640, 2019.](https://ieeexplore.ieee.org/document/8610112) 28 | 29 | [[11] B. Capelli and L. Sabattini, “Connectivity maintenance: Global and optimized approach through control barrier functions,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 5590–5596.](https://ieeexplore.ieee.org/document/9197109) 30 | 31 | [[12] P. Ong, B. Capelli, L. Sabattini, and J. Cort´es, “Network connectivity maintenance via nonsmooth control barrier functions,” in 2021 60th IEEE CDC, 2021, pp. 4786–4791.](https://ieeexplore.ieee.org/document/9683512) 32 | 33 | [[13] M. Saboia, L. Clark, V. Thangavelu, J. A. Edlund, K. Otsu, G. J. Correa, V. S. Varadharajan, A. Santamaria-Navarro, T. Touma, A. Bouman, H. Melikyan, T. Pailevanian, S.-K. Kim, A. Archanian, T. S. Vaquero, G. Beltrame, N. Napp, G. Pessin, and A.-a. Agha-mohammadi, “Achord: Communication-aware multi-robot coordination with intermittent connectivity,” IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 10 184–10 191, 2022.](https://ieeexplore.ieee.org/document/9837416) 34 | 35 | [[14] G. A. Hollinger and S. Singh, “Multirobot coordination with periodic connectivity: Theory and experiments,” IEEE Transactions on Robotics, vol. 28, no. 4, pp. 967–973, 2012.](https://ieeexplore.ieee.org/document/6177277) 36 | 37 | [[15] Y. Kantaros, M. Guo, and M. M. Zavlanos, “Temporal logic task planning and intermittent connectivity control of mobile robot networks,” IEEE Trans. on Automatic Control, vol. 64, no. 10, pp. 4105–4120, 2019.](https://ieeexplore.ieee.org/document/8612974) 38 | 39 | [[16] H. Rovina, T. Salam, Y. Kantaros, and M. Ani Hsieh, “Asynchronous adaptive sampling and reduced-order modeling of dynamic processes by robot teams via intermittently connected networks,” in 2020 IEEE/RSJ IROS, 2020, pp. 4798–4805.](https://ieeexplore.ieee.org/document/9341636) -------------------------------------------------------------------------------- /docs/robots.md: -------------------------------------------------------------------------------- 1 | # Table of Contents 2 | 3 | - [Table of Contents](#table-of-contents) 4 | - [Available Robots](#available-robots) 5 | - [Transform trees and sensors](#transform-trees-and-sensors) 6 | - [Pioneer3At](#pioneer3at) 7 | 8 | ## [Available Robots](#available-robots) 9 | 10 | The robots were initially obtained from [here](https://wiki.ros.org/Robots/AMR_Pioneer_Compatible) and modified to attend the specifications needed for the research. The adaptation were necessary to add more robustness to the robots' behaviors when exploring the environment, such as avoiding other robots and yet building a clean map. 11 | 12 | ## [Transform trees and sensors](#transform-trees-and-sensors) 13 | 14 | The transformation tree is a crucial part of the system, and improper configuration can disrupt your experiments. Unfortunately, ROS still has inconsistencies regarding how namespaces are handled by sensors and transforms. 15 | 16 | To improve usability, I have configured all robots in this project with a transformation tree from a parameterized namespace assigned during launch. This setup makes them ready for experimentation in complex scenarios. 17 | 18 | ## [Pioneer3At](#clearpath-husky) 19 | 20 | I've configured the Pioneer robot with: 21 | 22 | - IMU 23 | - 4 RGBD Cameras for dynamic obstacles 24 | - 1 360 Lidar for static obstacles 25 | 26 |

27 | drawing 28 |

29 | 30 | Its transformation tree should looks like the following. 31 | 32 |

33 | drawing 34 |

35 | 36 | I've also provided a basic rviz file to visualize this robot, its transforms, and model. 37 | 38 |

39 | drawing 40 |

41 | -------------------------------------------------------------------------------- /gazebo_resources/models/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/gazebo_resources/models/.gitkeep -------------------------------------------------------------------------------- /gazebo_resources/models/testing_area/materials/textures/heightmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/gazebo_resources/models/testing_area/materials/textures/heightmap.png -------------------------------------------------------------------------------- /gazebo_resources/models/testing_area/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | testing_area 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Alysson 10 | Alysson.ribeiro.silva@gmail.com 11 | 12 | 13 | 14 | Demo for multi-robot exploration. 15 | 16 | 17 | -------------------------------------------------------------------------------- /gazebo_resources/models/testing_area/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://testing_area/materials/textures/heightmap.png 10 | 129 129 10 11 | 0 0 0 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | file://media/materials/textures/dirt_diffusespecular.png 21 | file://media/materials/textures/flat_normal.png 22 | 1 23 | 24 | 25 | file://media/materials/textures/grass_diffusespecular.png 26 | file://media/materials/textures/flat_normal.png 27 | 1 28 | 29 | 30 | file://media/materials/textures/fungus_diffusespecular.png 31 | file://media/materials/textures/flat_normal.png 32 | 1 33 | 34 | 35 | 0 36 | 5 37 | 38 | 39 | 10 40 | 5 41 | 42 | model://testing_area/materials/textures/heightmap.png 43 | 129 129 10 44 | 0 0 0 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /gazebo_resources/worlds/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/gazebo_resources/worlds/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(multirobotexploration) 3 | add_compile_options(-std=c++17) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | rospy 8 | std_msgs 9 | sensor_msgs 10 | nav_msgs 11 | tf2_ros 12 | roslib 13 | message_generation 14 | message_runtime 15 | dynamic_reconfigure 16 | teb_local_planner 17 | multirobotsimulations 18 | ) 19 | 20 | catkin_python_setup() 21 | 22 | catkin_package( 23 | CATKIN_DEPENDS roscpp 24 | rospy 25 | std_msgs 26 | sensor_msgs 27 | nav_msgs 28 | tf2_ros 29 | roslib 30 | message_generation 31 | message_runtime 32 | dynamic_reconfigure 33 | teb_local_planner 34 | multirobotsimulations 35 | ) 36 | 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | 42 | include_directories( 43 | include/common 44 | include/nodes 45 | ${catkin_INCLUDE_DIRS} 46 | ${GAZEBO_INCLUDE_DIRS} 47 | ${OpenCV_INCLUDE_DIRS} 48 | ${Eigen3_INCLUDE_DIRS} 49 | ) 50 | 51 | add_library( 52 | ${PROJECT_NAME} 53 | source/common/SearchAlgorithms.cpp 54 | source/common/RendezvousPlan.cpp 55 | ) 56 | 57 | set(CMAKE_CXX_STANDARD 17) 58 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 59 | set(CMAKE_CXX_EXTENSIONS OFF) 60 | set(DENABLE_PRECOMPILED_HEADERS OFF) 61 | 62 | ################### 63 | ## communication ## 64 | ################### 65 | 66 | add_executable(mockcommunicationmodelnode source/communication/MockCommunicationModelNode.cpp) 67 | target_link_libraries(mockcommunicationmodelnode multirobotexploration ${catkin_LIBRARIES}) 68 | 69 | ################## 70 | ## localization ## 71 | ################## 72 | add_executable(gmappingposenode source/localization/GmappingPoseNode.cpp) 73 | target_link_libraries(gmappingposenode ${catkin_LIBRARIES}) 74 | 75 | add_executable(averagevelocityestimatornode source/localization/AverageVelocityEstimatorNode.cpp) 76 | target_link_libraries(averagevelocityestimatornode multirobotexploration ${catkin_LIBRARIES}) 77 | 78 | add_executable(relativeposeestimatornode source/localization/RelativePoseEstimatorNode.cpp) 79 | target_link_libraries(relativeposeestimatornode multirobotexploration ${catkin_LIBRARIES}) 80 | 81 | ################ 82 | ## navigation ## 83 | ################ 84 | add_executable(integratedglobalplannernode source/navigation/IntegratedGlobalPlannerNode.cpp) 85 | target_link_libraries(integratedglobalplannernode multirobotexploration ${catkin_LIBRARIES}) 86 | 87 | add_executable(localplannernode source/navigation/LocalPlannerNode.cpp) 88 | target_link_libraries(localplannernode multirobotexploration ${catkin_LIBRARIES}) 89 | 90 | ############### 91 | ## frontiers ## 92 | ############### 93 | add_executable(frontierdiscoverynode source/frontier/FrontierDiscoveryNode.cpp) 94 | target_link_libraries(frontierdiscoverynode multirobotexploration ${catkin_LIBRARIES}) 95 | 96 | ############# 97 | ## mapping ## 98 | ############# 99 | add_executable(cspacenode source/map/CSpaceNode.cpp) 100 | target_link_libraries(cspacenode multirobotexploration ${catkin_LIBRARIES}) 101 | 102 | add_executable(localcspacenode source/map/LocalCSpaceNode.cpp) 103 | target_link_libraries(localcspacenode multirobotexploration ${catkin_LIBRARIES}) 104 | 105 | add_executable(mapstitchingnode source/map/MapStitchingNode.cpp) 106 | target_link_libraries(mapstitchingnode multirobotexploration ${OpenCV_LIBS} ${catkin_LIBRARIES}) 107 | 108 | ############# 109 | ## sensors ## 110 | ############# 111 | add_executable(lasertoworldnode source/laser/LaserToWorldNode.cpp) 112 | target_link_libraries(lasertoworldnode multirobotexploration ${catkin_LIBRARIES}) 113 | 114 | ############## 115 | ## policies ## 116 | ############## 117 | add_executable(alysson2024 source/policies/Alysson2024Node.cpp) 118 | target_link_libraries(alysson2024 multirobotexploration ${catkin_LIBRARIES}) 119 | 120 | add_executable(randomizedsocialwelfare source/policies/RandomizedSocialWelfareNode.cpp) 121 | target_link_libraries(randomizedsocialwelfare multirobotexploration ${catkin_LIBRARIES}) 122 | 123 | add_executable(yamauchi1999 source/policies/Yamauchi1999Node.cpp) 124 | target_link_libraries(yamauchi1999 multirobotexploration ${catkin_LIBRARIES}) 125 | 126 | ############# 127 | ## Install ## 128 | ############# 129 | catkin_install_python(PROGRAMS 130 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 131 | ) -------------------------------------------------------------------------------- /src/multirobotexploration/config/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/config/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/config/gazebo_robots_start_pose.yaml: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | start_pose_robot_0: {x: 0, y: 0, z: 0, theta: 0} 18 | start_pose_robot_1: {x: 0, y: 0, z: 0, theta: 0} 19 | start_pose_robot_2: {x: 0, y: 0, z: 0, theta: 0} 20 | start_pose_robot_3: {x: 0, y: 0, z: 0, theta: 0} 21 | start_pose_robot_4: {x: 0, y: 0, z: 0, theta: 0} 22 | start_pose_robot_5: {x: 0, y: 0, z: 0, theta: 0} 23 | -------------------------------------------------------------------------------- /src/multirobotexploration/config/rendezvous_footprint.yaml: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | footprint_robot_0: {x: -1, y: 1, z: 0, theta: 0} 18 | footprint_robot_1: {x: 1, y: 1, z: 0, theta: 0} 19 | footprint_robot_2: {x: 1, y: -1, z: 0, theta: 0} 20 | -------------------------------------------------------------------------------- /src/multirobotexploration/config/rendezvous_plan.yaml: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | # Width of the plan is used to get the number of agreements 18 | # and iterate over the matrix. 19 | width: 3 20 | 21 | # The k matrix represents the rendezvous plan, where each row is an agreement 22 | # and columns represent robots. 23 | K: [1,1,0, 24 | 0,1,1, 25 | 1,1,0, 26 | 0,1,1, 27 | 1,1,0, 28 | 1,1,0, 29 | 1,1,0, 30 | 0,1,1] 31 | 32 | # The W matrix represents the agreements activation weights. 33 | # 34 | # The entire plan can be represented by the W matrix alone, 35 | # however, this would imply in loosing the number 0 from the representation. 36 | # 37 | # For example, you can have an agreement [1,1,1] and the weights [0,0,0], which 38 | # in abstract math would imply in an instant agreement fulfillment activation. 39 | W: [60 , 60 , 0 , 40 | 0 , 60 , 120, 41 | 120, 60 , 0 , 42 | 0 , 60 , 120, 43 | 120, 120, 0 , 44 | 120, 120, 0 , 45 | 120, 120, 0 , 46 | 0 , 120, 300] 47 | 48 | # The first rendezvous location is where all sub-teams 49 | # are supposed to meet first. 50 | first_rendezvous: {x: 6.5, y: 5.5, z: 0.0} 51 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/common/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/include/common/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/include/common/RendezvousPlan.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2023 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include 20 | #include "tf/tf.h" 21 | #include "ros/ros.h" 22 | 23 | struct agreement_el { 24 | int participate; 25 | double timer; 26 | }; 27 | 28 | class RendezvousPlan { 29 | public: 30 | RendezvousPlan(ros::NodeHandle& nodeHandle, const int& id); 31 | ~RendezvousPlan(); 32 | 33 | int GetCurrentAgreement(); 34 | double GetCurrentAgreementTimer(); 35 | bool CheckConsensusCurrentPlan(); 36 | 37 | void InitializeLocation(tf::Vector3 location); 38 | void Update(const double& deltaTime); 39 | bool ShouldFulfillAgreement(); 40 | void SetNextAgreement(); 41 | void UpdateCurrentAgreementLocation(tf::Vector3 newLocation); 42 | tf::Vector3 GetCurrentAgreementLocation(); 43 | void PrintLocations(); 44 | void Print(); 45 | void PrintCurrent(); 46 | void PrintRealization(); 47 | int GetCurrentAgreementUniqueID(); 48 | int GetCurrentAgreementConsensusID(); 49 | 50 | // relization 51 | void RealizePlan(const int& robotId); 52 | void ResetPlanRealization(); 53 | bool WasPlanRealized(); 54 | 55 | std::vector* GetPlanPtr(); 56 | 57 | private: 58 | std::string GenerateAgreementKey(const int& index); 59 | 60 | int id, width; 61 | int current_agreement; 62 | double current_timer; 63 | std::vector< std::pair> > agreements; 64 | std::map agreements_locations; 65 | 66 | std::vector> agreementsIParticipate; 67 | 68 | std::vector currentPlanRealization; 69 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/common/SearchAlgorithms.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #ifndef SEARCH_ALGORITHMS_H 20 | #define SEARCH_ALGORITHMS_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include "nav_msgs/OccupancyGrid.h" 31 | #include "Common.h" 32 | 33 | namespace sa { 34 | void InitOccFrom(nav_msgs::OccupancyGrid& rInput, nav_msgs::OccupancyGrid& rOutput); 35 | bool IsInBounds(nav_msgs::OccupancyGrid& rInput, Vec2i& rPos); 36 | bool CheckAny(nav_msgs::OccupancyGrid& rInput, const Vec2i& rStart, const Vec2i& rEnd, const int& rVal); 37 | void ComputePath(nav_msgs::OccupancyGrid& rOcc, 38 | const Vec2i& rStart, 39 | const Vec2i& rEnd, 40 | std::list& rOutPath); 41 | void ComputePathWavefront( 42 | nav_msgs::OccupancyGrid& rInput, 43 | const Vec2i& rStart, 44 | const Vec2i& rEnd, 45 | std::list& rOutPath); 46 | void ComputeFrontiers(nav_msgs::OccupancyGrid& rInput, 47 | nav_msgs::OccupancyGrid& rOutput, 48 | std::vector& rFrontiers); 49 | void ComputeClusters(nav_msgs::OccupancyGrid& rFrontiersMap, 50 | std::vector& rFrontiers, 51 | std::vector>& rOutClusters); 52 | Vec2i ClosestFrontierCluster(const Vec2i& rPos, std::vector& rCluster); 53 | Vec2i MedianFrontierCluster(const Vec2i& rPos, std::vector& rCluster); 54 | void ComputeCentroids(const Vec2i& rPos, 55 | std::vector>& rClusters, 56 | std::vector& rOutCentroids); 57 | void ComputeAverageCentroids(const Vec2i& rPos, 58 | std::vector>& rClusters, 59 | std::vector& rOutCentroids); 60 | }; 61 | #endif -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/include/nodes/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/AverageVelocityEstimatorNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include 24 | #include "ros/ros.h" 25 | #include "tf/tf.h" 26 | 27 | /* 28 | * Messages 29 | */ 30 | #include "std_msgs/Float32.h" 31 | #include "geometry_msgs/Pose.h" 32 | #include "multirobotsimulations/CustomPose.h" 33 | 34 | /* 35 | * Helpers 36 | */ 37 | #include "Common.h" 38 | 39 | /* 40 | * AverageVelocityEstimatorNode class 41 | */ 42 | class AverageVelocityEstimatorNode { 43 | public: 44 | AverageVelocityEstimatorNode(); 45 | ~AverageVelocityEstimatorNode(); 46 | 47 | private: 48 | double ComputeAverageVelocity(std::deque& speedArray); 49 | void WorldPoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 50 | void Update(); 51 | 52 | /* 53 | * Control variables 54 | */ 55 | int aId; 56 | int aQueueSize; 57 | int aCount; 58 | bool aReceivedPosition; 59 | double aRate; 60 | std::string aNamespace; 61 | tf::Vector3 aLastWorldPos; 62 | tf::Vector3 aWorldPos; 63 | 64 | /* 65 | * Routines 66 | */ 67 | std::vector aTimers; 68 | 69 | /* 70 | * Subscribers 71 | */ 72 | std::vector aSubscribers; 73 | 74 | /* 75 | * Advertisers 76 | */ 77 | ros::Publisher aAverageVelocityPublisher; 78 | 79 | /* 80 | * Messages 81 | */ 82 | std_msgs::Float32 aAverageVelocityMsg; 83 | 84 | /* 85 | * Helpers 86 | */ 87 | std::deque aVelocityArray; 88 | }; 89 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/CSpaceNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and systems 21 | */ 22 | #include 23 | #include 24 | #include 25 | #include "tf/tf.h" 26 | #include "string.h" 27 | 28 | /* 29 | * Messages 30 | */ 31 | #include "multirobotsimulations/CustomOcc.h" 32 | #include "nav_msgs/OccupancyGrid.h" 33 | #include "geometry_msgs/PoseArray.h" 34 | #include "geometry_msgs/Pose.h" 35 | #include "costmap_converter/ObstacleArrayMsg.h" 36 | #include "std_msgs/Int8MultiArray.h" 37 | #include "multirobotsimulations/CustomPose.h" 38 | 39 | /* 40 | * Helpers 41 | */ 42 | #include "Common.h" 43 | 44 | /* 45 | * CSpaceNode class 46 | */ 47 | class CSpaceNode { 48 | public: 49 | CSpaceNode(); 50 | ~CSpaceNode(); 51 | 52 | private: 53 | void Inflate(nav_msgs::OccupancyGrid& occ, 54 | nav_msgs::OccupancyGrid& free, 55 | nav_msgs::OccupancyGrid& occupied, 56 | const double& freeInflationRadius, 57 | const double& occupiedInflationRadius, 58 | const int8_t& occupancyThreshold = 90, 59 | const int8_t& freeThreshold = 50, 60 | const int8_t& occupiedValue = 100, 61 | const int8_t& freeVal = 1); 62 | 63 | void ApplyDynamicData(nav_msgs::OccupancyGrid& occ, 64 | nav_msgs::OccupancyGrid& dynamicOcc, 65 | std::vector& lidarSources, 66 | const double& maxLidarRange = 10.0, 67 | const int8_t& occupiedValue = 100); 68 | 69 | void ApplyDynamicData(nav_msgs::OccupancyGrid& occ, 70 | nav_msgs::OccupancyGrid& dynamicOcc, 71 | std::vector& lidarSources, 72 | std::vector& otherSources, 73 | const double& maxLidarRange = 10.0, 74 | const int8_t& occupiedValue = 100); 75 | 76 | void GenerateCSpace(nav_msgs::OccupancyGrid& free, 77 | nav_msgs::OccupancyGrid& occupied, 78 | nav_msgs::OccupancyGrid& cspace, 79 | tf::Vector3& occ_pose, 80 | const int8_t& unknownVal = -1); 81 | 82 | void InflatePoseForPlanner(nav_msgs::OccupancyGrid& cspace, 83 | const double& freeInflationRadius, 84 | const int& x, 85 | const int& y, 86 | const int8_t& occupancyThreshold = 90, 87 | const int8_t& freeVal = 1); 88 | 89 | void ClearLocalTrajectories(std::vector& local, 90 | std_msgs::Int8MultiArray& comm); 91 | 92 | void RobotsInCommCallback(std_msgs::Int8MultiArray::ConstPtr msg); 93 | void OccCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 94 | void WorldPoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 95 | 96 | void Update(); 97 | 98 | 99 | /* 100 | * Control variables 101 | */ 102 | int aQueueSize; 103 | int aId; 104 | int aLidarSources; 105 | int aRobots; 106 | bool aHasOcc; 107 | bool aHasPose; 108 | bool aReceivedComm; 109 | double aRate; 110 | double aLidarRange; 111 | double aFreeInflateRadius; 112 | double aOccuInflateRadius; 113 | tf::Vector3 aOccPose; 114 | std::string aNamespace; 115 | 116 | /* 117 | * Routines 118 | */ 119 | std::vector aTimers; 120 | 121 | /* 122 | * Subscribers 123 | */ 124 | std::vector aSubscribers; 125 | 126 | /* 127 | * Advertisers 128 | */ 129 | ros::Publisher aCspacePublisher; 130 | 131 | /* 132 | * Messages 133 | */ 134 | nav_msgs::OccupancyGrid aOccMsg; 135 | nav_msgs::OccupancyGrid aFreeCellsMsg; 136 | nav_msgs::OccupancyGrid aOccupiedCellsMsg; 137 | nav_msgs::OccupancyGrid aOccWithDynamicDataMsg; 138 | nav_msgs::OccupancyGrid aCspaceMsg; 139 | multirobotsimulations::CustomPose aWorldPoseMsg; 140 | std_msgs::Int8MultiArray aRobotsInCommMsg; 141 | 142 | /* 143 | * Helpers 144 | */ 145 | std::vector aTrajectoriesArray; 146 | std::vector aLidarsArray; 147 | }; 148 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/FrontierDiscoveryNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include "ros/ros.h" 24 | #include "tf/tf.h" 25 | 26 | /* 27 | * Messages 28 | */ 29 | #include "nav_msgs/OccupancyGrid.h" 30 | #include "geometry_msgs/Point.h" 31 | #include "geometry_msgs/PoseArray.h" 32 | #include "std_msgs/String.h" 33 | #include "multirobotsimulations/CustomPose.h" 34 | #include "multirobotsimulations/Frontiers.h" 35 | #include "visualization_msgs/Marker.h" 36 | #include "visualization_msgs/MarkerArray.h" 37 | 38 | /* 39 | * Helpers 40 | */ 41 | #include "Common.h" 42 | #include "SearchAlgorithms.h" 43 | 44 | /* 45 | * FrontierDiscoveryNode states 46 | */ 47 | typedef enum{ 48 | IDLE = 0, 49 | PROCESSING = 1, 50 | FINISHED = 2 51 | }FrontierState; 52 | 53 | /* 54 | * Frontier discovery node class 55 | */ 56 | class FrontierDiscoveryNode { 57 | public: 58 | FrontierDiscoveryNode(); 59 | ~FrontierDiscoveryNode(); 60 | 61 | private: 62 | void Update(); 63 | void CSpaceCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 64 | void EstimatePoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 65 | void ComputeCallback(std_msgs::String::ConstPtr msg); 66 | void CreateMarker(visualization_msgs::Marker& input, const char* ns, const int& id, const int& seq); 67 | void SetPoseArr(geometry_msgs::PoseArray& arr, const int& seq); 68 | void ResetFrontierMsg(multirobotsimulations::Frontiers& msg); 69 | double ComputeCentroidValue(nav_msgs::OccupancyGrid& occ, Vec2i& centroid, const double& lidarRange); 70 | 71 | /* 72 | * Control variables 73 | */ 74 | int aQueueSize; 75 | int aId; 76 | int aSeq; 77 | int aClusterDetectionMin; 78 | bool aReceivedCSpace; 79 | bool aHasPose; 80 | double aRate; 81 | double aYaw; 82 | double aMaxLidarRange; 83 | Vec2i aPos; 84 | FrontierState aState; 85 | std::string aNamespace; 86 | 87 | /* 88 | * Routines 89 | */ 90 | std::vector aTimers; 91 | 92 | /* 93 | * Subscribers 94 | */ 95 | std::vector aSubscribers; 96 | 97 | /* 98 | * Advertisers 99 | */ 100 | ros::Publisher aClusterMarkerPub; 101 | ros::Publisher aFrontiersMapPub; 102 | ros::Publisher aFrontiersClustersPub; 103 | 104 | /* 105 | * Messages 106 | */ 107 | multirobotsimulations::Frontiers aFrontierMsg; 108 | geometry_msgs::PoseArray aPoseArrMsg; 109 | geometry_msgs::Pose aWorldPos; 110 | nav_msgs::OccupancyGrid aOcc; 111 | nav_msgs::OccupancyGrid aFrontiersMap; 112 | visualization_msgs::Marker aClusterMarkerMsg; 113 | 114 | /* 115 | * Helpers 116 | */ 117 | std::list aPath; 118 | std::vector aFrontiers; 119 | std::vector aCentroids; 120 | std::vector aFilteredCentroids; 121 | std::vector> aClusters; 122 | std::vector> aFilteredClusters; 123 | }; 124 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/GmappingPoseNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include 24 | #include 25 | #include "ros/ros.h" 26 | #include "tf2_ros/transform_listener.h" 27 | #include "tf/LinearMath/Matrix3x3.h" 28 | #include "tf/tf.h" 29 | 30 | /* 31 | * Messages 32 | */ 33 | #include "geometry_msgs/TransformStamped.h" 34 | #include "geometry_msgs/Pose.h" 35 | #include "geometry_msgs/Point.h" 36 | #include "nav_msgs/MapMetaData.h" 37 | #include "nav_msgs/OccupancyGrid.h" 38 | #include "multirobotsimulations/CustomPose.h" 39 | #include "geometry_msgs/PoseStamped.h" 40 | #include "nav_msgs/Path.h" 41 | 42 | /* 43 | * Helpers 44 | */ 45 | #include "Common.h" 46 | 47 | class GmappingPoseNode { 48 | public: 49 | GmappingPoseNode(); 50 | ~GmappingPoseNode(); 51 | 52 | private: 53 | void OccCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 54 | void Update(); 55 | 56 | /* 57 | * Control variables 58 | */ 59 | int aId; 60 | int aQueueSize; 61 | bool aHasOcc; 62 | double aRate; 63 | std::string aNamespace; 64 | std::string aTFBaseLink; 65 | std::string aTFMap; 66 | 67 | /* 68 | * Routines 69 | */ 70 | std::vector aTimers; 71 | 72 | /* 73 | * Subscribers 74 | */ 75 | std::vector aSubscribers; 76 | 77 | /* 78 | * Advertisers 79 | */ 80 | ros::Publisher aPosePublisher; 81 | ros::Publisher aPoseStampedPublisher; 82 | ros::Publisher aPathPublisher; 83 | 84 | /* 85 | * Messages 86 | */ 87 | nav_msgs::OccupancyGrid aOcc; 88 | multirobotsimulations::CustomPose aPose; 89 | geometry_msgs::PoseStamped aPoseStamped; 90 | nav_msgs::Path aPath; 91 | 92 | /* 93 | * Helpers 94 | */ 95 | std::shared_ptr aTFBuffer; 96 | std::shared_ptr aTFListener; 97 | 98 | }; 99 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/IntegratedGlobalPlannerNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2023 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include 24 | #include "ros/ros.h" 25 | #include "tf/tf.h" 26 | 27 | /* 28 | * Messages 29 | */ 30 | #include "std_msgs/Bool.h" 31 | #include "std_msgs/String.h" 32 | #include "std_msgs/Float32.h" 33 | #include "nav_msgs/Path.h" 34 | #include "nav_msgs/OccupancyGrid.h" 35 | #include "visualization_msgs/Marker.h" 36 | #include "geometry_msgs/PoseStamped.h" 37 | #include "multirobotsimulations/CustomPose.h" 38 | 39 | /* 40 | * Helpers 41 | */ 42 | #include "SearchAlgorithms.h" 43 | #include "Common.h" 44 | 45 | typedef enum { 46 | state_idle = 0, 47 | state_executing_path = 1 48 | } SubGoalState; 49 | 50 | class IntegratedGlobalPlannerNode { 51 | public: 52 | IntegratedGlobalPlannerNode(); 53 | ~IntegratedGlobalPlannerNode(); 54 | 55 | private: 56 | void CreateMarker(visualization_msgs::Marker& input, const char* ns, const int& id, const int& seq); 57 | void DepthFirstSearchFreePath( 58 | nav_msgs::OccupancyGrid& cspace, 59 | Vec2i& occpos, 60 | Vec2i& source, 61 | Vec2i& closest, 62 | std::list& outpath); 63 | void ChangeState(const SubGoalState& newState); 64 | void CSpaceCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 65 | void PoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 66 | void AverageVelocityCallback(std_msgs::Float32::ConstPtr msg); 67 | void GoalCallback(geometry_msgs::Pose::ConstPtr msg); 68 | void StopCallBack(std_msgs::String::ConstPtr msg); 69 | void Update(); 70 | 71 | /* 72 | * Control variables 73 | */ 74 | int aQueueSize; 75 | int aId; 76 | int aSeq; 77 | int aDeltaTimeSec; 78 | bool aHasPose; 79 | bool aHasOcc; 80 | bool aHasAverageVelocity; 81 | double aRate; 82 | double aDistance; 83 | double aSubGoalReachThreshold; 84 | double aStuckTimeThreshold; 85 | double aStuckTime; 86 | double aAverageVelocity; 87 | std::string aNamespace; 88 | 89 | /* 90 | * Routines 91 | */ 92 | std::vector aTimers; 93 | 94 | /* 95 | * Subscribers 96 | */ 97 | std::vector aSubscribers; 98 | 99 | /* 100 | * Advertisers 101 | */ 102 | ros::Publisher aPathMarkerPublisher; 103 | ros::Publisher aFinishEventPublisher; 104 | ros::Publisher aCurrentPathPublisher; 105 | 106 | /* 107 | * Messages 108 | */ 109 | nav_msgs::Path aPathMsg; 110 | std_msgs::String aStrMsg; 111 | visualization_msgs::Marker aPathMarkerMsg; 112 | nav_msgs::OccupancyGrid aCspace; 113 | 114 | /* 115 | * Helpers 116 | */ 117 | ros::Time last_time; 118 | std::list aWaypoints; 119 | tf::Vector3 aLastPos; 120 | tf::Vector3 aWorldPos; 121 | tf::Vector3 aCurrentGoal; 122 | Vec2i aOccPos; 123 | SubGoalState aCurrentState; 124 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/LaserToWorldNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include "ros/ros.h" 24 | #include "tf/tf.h" 25 | 26 | /* 27 | * Messages 28 | */ 29 | #include "sensor_msgs/LaserScan.h" 30 | #include "geometry_msgs/Pose.h" 31 | #include "geometry_msgs/PoseArray.h" 32 | #include "nav_msgs/OccupancyGrid.h" 33 | #include "nav_msgs/MapMetaData.h" 34 | #include "multirobotsimulations/CustomPose.h" 35 | 36 | /* 37 | * Helpers 38 | */ 39 | #include "Common.h" 40 | 41 | class LaserToWorldNode { 42 | public: 43 | LaserToWorldNode(); 44 | ~LaserToWorldNode(); 45 | 46 | private: 47 | void EstimatePoseWorldCallback(multirobotsimulations::CustomPose::ConstPtr msg); 48 | void OccupancyGridCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 49 | void LaserCapture(sensor_msgs::LaserScan::ConstPtr msg); 50 | void Update(); 51 | 52 | /* 53 | * Control variables 54 | */ 55 | int aQueueSize; 56 | bool aHasLidar; 57 | bool aHasPose; 58 | bool aHasOccInfo; 59 | double aRate; 60 | double aRobotYaw; 61 | double aLidarError; 62 | tf::Vector3 aRobotWorldPosition; 63 | tf::Vector3 aLidarPosition; 64 | tf::Quaternion aLidarOrientation; 65 | std::string aNamespace; 66 | 67 | /* 68 | * Routines 69 | */ 70 | std::vector aTimers; 71 | 72 | /* 73 | * Subscribers 74 | */ 75 | std::vector aSubscribers; 76 | 77 | /* 78 | * Advertisers 79 | */ 80 | ros::Publisher aLidarPublisher; 81 | ros::Publisher aOccLidarPublisher; 82 | 83 | /* 84 | * Messages 85 | */ 86 | geometry_msgs::PoseArray aWorldLidarMsg; 87 | geometry_msgs::PoseArray aOccLidarMsg; 88 | nav_msgs::OccupancyGrid aOccInfo; 89 | 90 | /* 91 | * Helpers 92 | */ 93 | std::vector aWorldReadings; 94 | std::vector aOccReadings; 95 | }; 96 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/LocalCSpaceNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and systems 21 | */ 22 | #include 23 | #include 24 | #include 25 | #include "tf/tf.h" 26 | #include "string.h" 27 | 28 | /* 29 | * Messages 30 | */ 31 | #include "multirobotsimulations/CustomOcc.h" 32 | #include "nav_msgs/OccupancyGrid.h" 33 | #include "geometry_msgs/PoseArray.h" 34 | #include "geometry_msgs/Pose.h" 35 | #include "costmap_converter/ObstacleArrayMsg.h" 36 | #include "std_msgs/Int8MultiArray.h" 37 | #include "multirobotsimulations/CustomPose.h" 38 | 39 | /* 40 | * Helpers 41 | */ 42 | #include "Common.h" 43 | 44 | /* 45 | * LocalCSpaceNode class 46 | */ 47 | class LocalCSpaceNode { 48 | public: 49 | LocalCSpaceNode(); 50 | ~LocalCSpaceNode(); 51 | 52 | private: 53 | void CreateLocal(nav_msgs::OccupancyGrid& dynamicOcc, 54 | nav_msgs::OccupancyGrid& localMap, 55 | geometry_msgs::PoseArray& occupiedPoses, 56 | geometry_msgs::PoseArray& freePoses, 57 | tf::Vector3& worldPose, 58 | tf::Vector3& occPose, 59 | const double& freeInflationRadius, 60 | const double& occupiedInflationRadius, 61 | const int& windws_size_meters, 62 | const int8_t& occupancyThreshold = 90, 63 | const int8_t& freeThreshold = 50, 64 | const int8_t& occupiedValue = 100, 65 | const int8_t& freeVal = 1, 66 | const int8_t& unknownVal = -1); 67 | 68 | void ApplyDynamicData(nav_msgs::OccupancyGrid& occ, 69 | nav_msgs::OccupancyGrid& dynamicOcc, 70 | std::vector& lidarSources, 71 | std::vector& otherSources, 72 | const double& maxLidarRange = 10.0, 73 | const int8_t& occupiedValue = 100); 74 | void ClearLocalTrajectories(std::vector& local, 75 | std_msgs::Int8MultiArray& comm); 76 | 77 | void RobotsInCommCallback(std_msgs::Int8MultiArray::ConstPtr msg); 78 | void OccCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 79 | void WorldPoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 80 | 81 | void Update(); 82 | 83 | 84 | /* 85 | * Control variables 86 | */ 87 | int aQueueSize; 88 | int aId; 89 | int aLidarSources; 90 | int aRobots; 91 | int aLocalViewSize; 92 | bool aHasOcc; 93 | bool aHasPose; 94 | bool aReceivedComm; 95 | double aRate; 96 | double aLidarRange; 97 | double aFreeInflateRadius; 98 | double aOccuInflateRadius; 99 | tf::Vector3 aOccPose; 100 | std::string aNamespace; 101 | 102 | 103 | /* 104 | * Routines 105 | */ 106 | std::vector aTimers; 107 | 108 | /* 109 | * Subscribers 110 | */ 111 | std::vector aSubscribers; 112 | 113 | /* 114 | * Advertisers 115 | */ 116 | ros::Publisher aObstaclesPublisher; 117 | ros::Publisher aLocalCSpacePublisher; 118 | ros::Publisher aOccupiedPositionsPublisher; 119 | ros::Publisher aFreePositionsPublisher; 120 | 121 | /* 122 | * Messages 123 | */ 124 | nav_msgs::OccupancyGrid aOccMsg; 125 | nav_msgs::OccupancyGrid aOccWithDynamicDataMsg; 126 | nav_msgs::OccupancyGrid aLocalCspaceMsg; 127 | std_msgs::Int8MultiArray aRobotsInCommMsg; 128 | multirobotsimulations::CustomPose aWorldPoseMsg; 129 | geometry_msgs::PoseArray aOccupiedPosesMsg; 130 | geometry_msgs::PoseArray aFreePosesMsg; 131 | 132 | /* 133 | * Helpers 134 | */ 135 | std::vector aTrajectoriesArray; 136 | std::vector aLidarsArray; 137 | }; 138 | -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/LocalPlannerNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include "ros/ros.h" 23 | #include "teb_local_planner/teb_local_planner_ros.h" 24 | 25 | /* 26 | * Messages 27 | */ 28 | #include "std_msgs/Int8MultiArray.h" 29 | #include "std_msgs/Float32.h" 30 | #include "multirobotsimulations/CustomPose.h" 31 | #include "multirobotsimulations/MockPackage.h" 32 | #include "std_msgs/Float64MultiArray.h" 33 | #include "geometry_msgs/PoseArray.h" 34 | 35 | /* 36 | * 37 | */ 38 | #include "Common.h" 39 | 40 | class LocalPlannerNode { 41 | public: 42 | LocalPlannerNode(); 43 | ~LocalPlannerNode(); 44 | 45 | private: 46 | void CreateMarker(visualization_msgs::Marker& marker, const char* ns, const int& id, const int& seq); 47 | void AssembleSparsePath(nav_msgs::Path& currentPath, nav_msgs::Path& filteredPath, const int& viaIncrement, visualization_msgs::Marker& globalPathMaker); 48 | bool CheckNearPriority(); 49 | void ObstacleArrayCallback(costmap_converter::ObstacleArrayConstPtr msg); 50 | void PoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 51 | void SubgoalPathCallback(nav_msgs::Path::ConstPtr msg); 52 | void RobotInCommCallback(std_msgs::Int8MultiArray::ConstPtr msg); 53 | void Update(); 54 | 55 | /* 56 | * Control variables 57 | */ 58 | int aQueueSize; 59 | int aSeq; 60 | int aId; 61 | int aRobots; 62 | int aMaxWaypoints; 63 | int aViaIncrement; 64 | int aIncrement; 65 | int aControlsToShare; 66 | bool aUsePriorityBehavior; 67 | bool aReceivedComm; 68 | double aRate; 69 | std::string aNamespace; 70 | std::string aName; 71 | 72 | /* 73 | * Routines 74 | */ 75 | std::vector aTimers; 76 | 77 | /* 78 | * Subscribers 79 | */ 80 | std::vector aSubscribers; 81 | 82 | /* 83 | * Advertisers 84 | */ 85 | ros::Publisher aVelocityPublisher; 86 | ros::Publisher aTebPosesPublisher; 87 | ros::Publisher aViaPointsPublisher; 88 | 89 | /* 90 | * Messages 91 | */ 92 | geometry_msgs::Twist aTwistVelMsg; 93 | geometry_msgs::PoseStamped aPrevPoseMsg; 94 | geometry_msgs::PoseStamped aLastPoseMsg; 95 | geometry_msgs::PoseArray aTebPosesMsg; 96 | visualization_msgs::Marker aGlobalPathMsg; 97 | std_msgs::Int8MultiArray aRobotsInCommMsg; 98 | nav_msgs::Path aCurrentPathMsg; 99 | nav_msgs::Path aFilteredPathMsg; 100 | multirobotsimulations::CustomPose aPose; 101 | 102 | /* 103 | * Helpers 104 | */ 105 | teb_local_planner::TebConfig aTebConfig; 106 | teb_local_planner::ViaPointContainer aViaPoints; 107 | teb_local_planner::TebVisualizationPtr aVisual; 108 | teb_local_planner::RobotFootprintModelPtr aRobotFootprint; 109 | std::vector aObstacleArray; 110 | std::shared_ptr aPlanner; 111 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/MapStitchingNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "ros/ros.h" 27 | #include "ros/package.h" 28 | 29 | /* 30 | * Messages 31 | */ 32 | #include "geometry_msgs/Pose.h" 33 | #include "nav_msgs/OccupancyGrid.h" 34 | #include "geometry_msgs/PoseArray.h" 35 | #include "multirobotsimulations/CustomPose.h" 36 | #include "multirobotsimulations/MockPackage.h" 37 | #include "std_msgs/Float64MultiArray.h" 38 | #include "std_msgs/Int8MultiArray.h" 39 | #include "visualization_msgs/Marker.h" 40 | #include "visualization_msgs/MarkerArray.h" 41 | 42 | /* 43 | * Helpers 44 | */ 45 | #include "Common.h" 46 | 47 | class MapStitchingNode { 48 | public: 49 | MapStitchingNode(); 50 | ~MapStitchingNode(); 51 | 52 | private: 53 | void CommunicationsCallback(std_msgs::Int8MultiArray::ConstPtr msg); 54 | void RelativeStartingPosesCallback(geometry_msgs::PoseArray::ConstPtr msg); 55 | void OccCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 56 | void Fusemaps(nav_msgs::OccupancyGrid& occ, 57 | nav_msgs::OccupancyGrid& other, 58 | geometry_msgs::Pose& relative, 59 | const bool& replace); 60 | void Update(); 61 | 62 | /* 63 | * Control variables 64 | */ 65 | int aQueueSize; 66 | int aId; 67 | int aRobots; 68 | bool aDirty; 69 | double aRate; 70 | std::string aNamespace; 71 | 72 | /* 73 | * Routines 74 | */ 75 | std::vector aTimers; 76 | 77 | /* 78 | * Subscribers 79 | */ 80 | std::vector aSubscribers; 81 | 82 | /* 83 | * Advertisers 84 | */ 85 | ros::Publisher aFusionPublisher; 86 | 87 | /* 88 | * Messages 89 | */ 90 | nav_msgs::OccupancyGrid aFusionMsg; 91 | std_msgs::Int8MultiArray aRobotsInCommMsg; 92 | geometry_msgs::PoseArray aRobotsRelativePosesMsg; 93 | 94 | /* 95 | * Helpers 96 | */ 97 | std::vector aRobotsOcc; 98 | std::vector aReceivedOccs; 99 | bool aReceivedRelativePoses; 100 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/MockCommunicationModelNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2023 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * System and ROS 21 | */ 22 | #include 23 | #include 24 | #include "ros/ros.h" 25 | #include "tf/tf.h" 26 | 27 | /* 28 | * Messages 29 | */ 30 | #include "std_msgs/Float64MultiArray.h" 31 | #include "geometry_msgs/Pose.h" 32 | #include "geometry_msgs/PoseArray.h" 33 | #include "std_msgs/Int8MultiArray.h" 34 | #include "visualization_msgs/Marker.h" 35 | #include "visualization_msgs/MarkerArray.h" 36 | #include "multirobotsimulations/CustomPose.h" 37 | 38 | /* 39 | * Helpers 40 | */ 41 | #include "Common.h" 42 | 43 | class MockCommunicationModelNode { 44 | public: 45 | MockCommunicationModelNode(); 46 | ~MockCommunicationModelNode(); 47 | 48 | private: 49 | void LoadRelativePoses(ros::NodeHandle& nodeHandle); 50 | void Update(); 51 | 52 | /* 53 | * Control variables 54 | */ 55 | int aRobots; 56 | int aId; 57 | int aQueueSize; 58 | double aRate; 59 | double aCommDist; 60 | std::string aNamespace; 61 | 62 | /* 63 | * Routines 64 | */ 65 | std::vector aTimers; 66 | 67 | /* 68 | * Subscribers 69 | */ 70 | std::vector aSubscribers; 71 | 72 | /* 73 | * Advertisers 74 | */ 75 | ros::Publisher aCommunicationModelBroadcaster; 76 | 77 | /* 78 | * Messages 79 | */ 80 | std_msgs::Int8MultiArray aRobotsInComm; 81 | std::vector aRobotsWorldPoses; 82 | 83 | /* 84 | * Helpers 85 | */ 86 | std::vector aRelativePoses; 87 | std::vector aReceivedPoses; 88 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/RandomizedSocialWelfareNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include 24 | #include "ros/ros.h" 25 | 26 | /* 27 | * Messages 28 | */ 29 | #include "nav_msgs/OccupancyGrid.h" 30 | #include "std_msgs/String.h" 31 | #include "std_msgs/Int8MultiArray.h" 32 | #include "multirobotsimulations/CustomPose.h" 33 | #include "multirobotsimulations/Frontiers.h" 34 | #include "visualization_msgs/Marker.h" 35 | 36 | /* 37 | * Helpers 38 | */ 39 | #include "Common.h" 40 | 41 | typedef enum { 42 | state_select_frontier = 2, 43 | state_exploring = 3, 44 | state_idle = 11, 45 | state_exploration_finished = 12, 46 | state_back_to_base = 25, 47 | state_back_to_base_finished = 26, 48 | state_compute_centroids = 27, 49 | state_waiting_centroids = 28, 50 | state_set_back_to_base = 30, 51 | state_planning = 31, 52 | } ExplorerState; 53 | 54 | class RandomizedSocialWelfareNode { 55 | public: 56 | RandomizedSocialWelfareNode(); 57 | ~RandomizedSocialWelfareNode(); 58 | 59 | private: 60 | void EstimatePoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 61 | void ClustersCallback(multirobotsimulations::Frontiers::ConstPtr msg); 62 | void SubGoalFinishCallback(std_msgs::String::ConstPtr msg); 63 | void CSpaceCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 64 | void SetExploringCallback(std_msgs::String::ConstPtr msg); 65 | void SetBasestationCallback(std_msgs::String::ConstPtr msg); 66 | void SetIdleCallback(std_msgs::String::ConstPtr msg); 67 | 68 | void ChangeState(const ExplorerState& newState); 69 | int SelectFrontier(multirobotsimulations::Frontiers& centroids, tf::Vector3& selectFrontierWorld); 70 | void CreateMarker(visualization_msgs::Marker& input, const char* ns, const int& id, const int& seq); 71 | void SetGoal(const tf::Vector3& goal); 72 | void Update(); 73 | 74 | /* 75 | * Control variables 76 | */ 77 | int aQueueSize; 78 | int aRate; 79 | int aRobots; 80 | int aId; 81 | bool aHasPose; 82 | bool aHasOcc; 83 | bool aDirty; 84 | bool aFirst; 85 | double aDeltaTime; 86 | Vec2i aOccPos; 87 | ros::Time aLastTime; 88 | tf::Vector3 aWorldPos; 89 | tf::Vector3 aGoalFrontier; 90 | tf::Vector3 aGoalBasestation; 91 | std::string aNamespace; 92 | ExplorerState aCurrentState; 93 | 94 | /* 95 | * Routines 96 | */ 97 | std::vector aTimers; 98 | 99 | /* 100 | * Subscribers 101 | */ 102 | std::vector aSubscribers; 103 | 104 | /* 105 | * Advertisers 106 | */ 107 | ros::Publisher aGoalPublisher; 108 | ros::Publisher aFrontierComputePublisher; 109 | 110 | /* 111 | * Messages 112 | */ 113 | multirobotsimulations::Frontiers aFrontierCentroidsMsg; 114 | nav_msgs::OccupancyGrid aCSpaceMsg; 115 | 116 | /* 117 | * Extension from Yamauchi-based policy 118 | */ 119 | bool CheckNear(); 120 | int RandomizedFrontierSelection(multirobotsimulations::Frontiers& centroids, tf::Vector3& selectFrontierWorld); 121 | void CommCallback(std_msgs::Int8MultiArray::ConstPtr msg); 122 | 123 | // extension control 124 | bool aHasComm; 125 | 126 | // extension messages 127 | std_msgs::Int8MultiArray aCommMsg; 128 | 129 | // extension helpers 130 | std::unique_ptr aRandomNumberGenerator; 131 | std::random_device aRandomNumberDevice; 132 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/RelativePoseEstimatorNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * ROS and system 21 | */ 22 | #include 23 | #include 24 | #include "ros/ros.h" 25 | #include "tf/tf.h" 26 | 27 | /* 28 | * Messages 29 | */ 30 | #include "std_msgs/Float64MultiArray.h" 31 | #include "geometry_msgs/Pose.h" 32 | #include "geometry_msgs/PoseArray.h" 33 | #include "std_msgs/Int8MultiArray.h" 34 | #include "visualization_msgs/Marker.h" 35 | #include "visualization_msgs/MarkerArray.h" 36 | #include "multirobotsimulations/CustomPose.h" 37 | #include "multirobotsimulations/MockPackage.h" 38 | 39 | /* 40 | * Helpers 41 | */ 42 | #include "Common.h" 43 | 44 | class RelativePoseEstimatorNode { 45 | public: 46 | RelativePoseEstimatorNode(); 47 | ~RelativePoseEstimatorNode(); 48 | 49 | private: 50 | void LoadRelativePoses(ros::NodeHandle& nodeHandle); 51 | void PrepareMarkers(visualization_msgs::Marker& input, const char* ns, const int& id, const int& seq); 52 | void SetNear(visualization_msgs::Marker& input); 53 | void SetFar(visualization_msgs::Marker& input); 54 | void CommCallback(std_msgs::Int8MultiArray::ConstPtr msg); 55 | void Update(); 56 | 57 | /* 58 | * Control variables 59 | */ 60 | int aRobots; 61 | int aId; 62 | int aQueueSize; 63 | int aSeq; 64 | double aRate; 65 | std::string aNamespace; 66 | 67 | /* 68 | * Routines 69 | */ 70 | std::vector aTimers; 71 | 72 | /* 73 | * Subscribers 74 | */ 75 | std::vector aSubscribers; 76 | 77 | /* 78 | * Advertisers 79 | */ 80 | ros::Publisher aStartRelativePosesPublisher; 81 | ros::Publisher aRelativePosesPublisher; 82 | ros::Publisher aDistancesPublisher; 83 | ros::Publisher aNearMarkerPublisher; 84 | ros::Publisher aFarMarkerPublisher; 85 | 86 | /* 87 | * Messages 88 | */ 89 | geometry_msgs::PoseArray aRobotsRelStartingPosMsg; 90 | geometry_msgs::PoseArray aRobotsRelativePosesMsg; 91 | visualization_msgs::Marker aClusterMarkerMsg; 92 | visualization_msgs::Marker aClusterMarkerFarMsg; 93 | std_msgs::Float64MultiArray aRobotsRelativeDistancesMsg; 94 | std_msgs::Int8MultiArray aRobotsInCommMsg; 95 | 96 | /* 97 | * Helpers 98 | */ 99 | std::vector aRobotsWorldPoses; 100 | std::vector aReceivedPoses; 101 | std::vector aRelativePoses; 102 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/include/nodes/Yamauchi1999Node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | /* 20 | * Ros and system 21 | */ 22 | #include 23 | #include "ros/ros.h" 24 | 25 | /* 26 | * Messages 27 | */ 28 | #include "nav_msgs/OccupancyGrid.h" 29 | #include "std_msgs/String.h" 30 | #include "multirobotsimulations/CustomPose.h" 31 | #include "multirobotsimulations/Frontiers.h" 32 | #include "visualization_msgs/Marker.h" 33 | 34 | /* 35 | * Helpers 36 | */ 37 | #include "Common.h" 38 | 39 | typedef enum { 40 | state_select_frontier = 2, 41 | state_exploring = 3, 42 | state_idle = 11, 43 | state_exploration_finished = 12, 44 | state_back_to_base = 25, 45 | state_back_to_base_finished = 26, 46 | state_compute_centroids = 27, 47 | state_waiting_centroids = 28, 48 | state_set_back_to_base = 30, 49 | state_planning = 31, 50 | } ExplorerState; 51 | 52 | class Yamauchi1999Node { 53 | public: 54 | Yamauchi1999Node(); 55 | ~Yamauchi1999Node(); 56 | 57 | private: 58 | void EstimatePoseCallback(multirobotsimulations::CustomPose::ConstPtr msg); 59 | void ClustersCallback(multirobotsimulations::Frontiers::ConstPtr msg); 60 | void SubGoalFinishCallback(std_msgs::String::ConstPtr msg); 61 | void CSpaceCallback(nav_msgs::OccupancyGrid::ConstPtr msg); 62 | void SetExploringCallback(std_msgs::String::ConstPtr msg); 63 | void SetBasestationCallback(std_msgs::String::ConstPtr msg); 64 | void SetIdleCallback(std_msgs::String::ConstPtr msg); 65 | 66 | void ChangeState(const ExplorerState& newState); 67 | int SelectFrontier(multirobotsimulations::Frontiers& centroids, tf::Vector3& selectFrontierWorld); 68 | void CreateMarker(visualization_msgs::Marker& input, const char* ns, const int& id, const int& seq); 69 | void SetGoal(const tf::Vector3& goal); 70 | void Update(); 71 | 72 | /* 73 | * Control variables 74 | */ 75 | int aQueueSize; 76 | int aRate; 77 | int aRobots; 78 | int aId; 79 | bool aHasPose; 80 | bool aHasOcc; 81 | bool aDirty; 82 | bool aFirst; 83 | double aDeltaTime; 84 | Vec2i aOccPos; 85 | ros::Time aLastTime; 86 | tf::Vector3 aWorldPos; 87 | tf::Vector3 aGoalFrontier; 88 | tf::Vector3 aGoalBasestation; 89 | std::string aNamespace; 90 | ExplorerState aCurrentState; 91 | 92 | /* 93 | * Routines 94 | */ 95 | std::vector aTimers; 96 | 97 | /* 98 | * Subscribers 99 | */ 100 | std::vector aSubscribers; 101 | 102 | /* 103 | * Advertisers 104 | */ 105 | ros::Publisher aGoalPublisher; 106 | ros::Publisher aFrontierComputePublisher; 107 | 108 | /* 109 | * Messages 110 | */ 111 | multirobotsimulations::Frontiers aFrontierCentroidsMsg; 112 | nav_msgs::OccupancyGrid aCSpaceMsg; 113 | }; -------------------------------------------------------------------------------- /src/multirobotexploration/launch/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/launch/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/launch/gmapping.launch: -------------------------------------------------------------------------------- 1 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /src/multirobotexploration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multirobotexploration 4 | 0.0.1 5 | The multi-robot exploration package 6 | Alysson Ribeiro da Silva 7 | GPLv3 8 | http://alysson.thegeneralsolution.com 9 | Alysson Ribeiro da Silva 10 | 11 | 12 | catkin 13 | 14 | 16 | roscpp 17 | rospy 18 | std_msgs 19 | sensor_msgs 20 | gazebo_plugins 21 | nav_msgs 22 | tf2_ros 23 | roslib 24 | message_generation 25 | message_runtime 26 | dynamic_reconfigure 27 | teb_local_planner 28 | multirobotsimulations 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/multirobotexploration/rviz/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/rviz/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/rviz/robot_2.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 303 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: "" 27 | Preferences: 28 | PromptSaveOnExit: true 29 | Toolbars: 30 | toolButtonStyle: 2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 1 35 | Class: rviz/Axes 36 | Enabled: true 37 | Length: 0.4000000059604645 38 | Name: Axes 39 | Radius: 0.10000000149011612 40 | Reference Frame: robot_2/base_link 41 | Show Trail: false 42 | Value: true 43 | - Class: rviz/Marker 44 | Enabled: false 45 | Marker Topic: /robot_2/frontier_discovery/frontiers_clusters_markers 46 | Name: frontiers_centroids 47 | Namespaces: 48 | {} 49 | Queue Size: 100 50 | Value: false 51 | - Alpha: 0.699999988079071 52 | Class: rviz/Map 53 | Color Scheme: costmap 54 | Draw Behind: true 55 | Enabled: false 56 | Name: frontiers 57 | Topic: /robot_2/frontier_discovery/frontiers 58 | Unreliable: false 59 | Use Timestamp: false 60 | Value: false 61 | - Alpha: 0.5 62 | Class: rviz/Map 63 | Color Scheme: map 64 | Draw Behind: false 65 | Enabled: false 66 | Name: c_space 67 | Topic: /robot_2/c_space 68 | Unreliable: false 69 | Use Timestamp: false 70 | Value: false 71 | - Alpha: 0.20000000298023224 72 | Class: rviz/Map 73 | Color Scheme: map 74 | Draw Behind: false 75 | Enabled: true 76 | Name: occ 77 | Topic: /robot_2/fusion 78 | Unreliable: false 79 | Use Timestamp: false 80 | Value: true 81 | - Alpha: 0.30000001192092896 82 | Class: rviz/Map 83 | Color Scheme: costmap 84 | Draw Behind: false 85 | Enabled: false 86 | Name: local 87 | Topic: /robot_2/c_space_local 88 | Unreliable: false 89 | Use Timestamp: false 90 | Value: false 91 | - Class: rviz/Marker 92 | Enabled: false 93 | Marker Topic: /robot_2/sub_goal_nav/path 94 | Name: sub_goal 95 | Namespaces: 96 | {} 97 | Queue Size: 100 98 | Value: false 99 | - Class: rviz/Marker 100 | Enabled: false 101 | Marker Topic: /robot_2/relative_pose_estimator/pose_markers 102 | Name: near_robots 103 | Namespaces: 104 | {} 105 | Queue Size: 100 106 | Value: false 107 | - Class: rviz/Marker 108 | Enabled: false 109 | Marker Topic: /robot_2/relative_pose_estimator/pose_far_markers 110 | Name: far_robots 111 | Namespaces: 112 | {} 113 | Queue Size: 100 114 | Value: false 115 | - Alpha: 1 116 | Buffer Length: 1 117 | Class: rviz/Path 118 | Color: 114; 159; 207 119 | Enabled: true 120 | Head Diameter: 0.30000001192092896 121 | Head Length: 0.20000000298023224 122 | Length: 0.30000001192092896 123 | Line Style: Billboards 124 | Line Width: 0.5 125 | Name: trajetory 126 | Offset: 127 | X: 0 128 | Y: 0 129 | Z: 0 130 | Pose Color: 255; 85; 255 131 | Pose Style: None 132 | Queue Size: 10 133 | Radius: 0.029999999329447746 134 | Shaft Diameter: 0.10000000149011612 135 | Shaft Length: 0.10000000149011612 136 | Topic: /robot_2/path 137 | Unreliable: false 138 | Value: true 139 | Enabled: true 140 | Global Options: 141 | Background Color: 48; 48; 48 142 | Default Light: true 143 | Fixed Frame: robot_2/map 144 | Frame Rate: 30 145 | Name: root 146 | Tools: 147 | - Class: rviz/Interact 148 | Hide Inactive Objects: true 149 | - Class: rviz/MoveCamera 150 | - Class: rviz/Select 151 | - Class: rviz/Measure 152 | - Class: rviz/SetInitialPose 153 | Theta std deviation: 0.2617993950843811 154 | Topic: /initialpose 155 | X std deviation: 0.5 156 | Y std deviation: 0.5 157 | - Class: rviz/SetGoal 158 | Topic: /move_base_simple/goal 159 | - Class: rviz/PublishPoint 160 | Single click: true 161 | Topic: /clicked_point 162 | Value: true 163 | Views: 164 | Current: 165 | Class: rviz/Orbit 166 | Distance: 45.10614776611328 167 | Enable Stereo Rendering: 168 | Stereo Eye Separation: 0.05999999865889549 169 | Stereo Focal Distance: 1 170 | Swap Stereo Eyes: false 171 | Value: false 172 | Field of View: 0.7853981852531433 173 | Focal Point: 174 | X: -10.929421424865723 175 | Y: -7.0816969871521 176 | Z: 3.339012861251831 177 | Focal Shape Fixed Size: true 178 | Focal Shape Size: 0.05000000074505806 179 | Invert Z Axis: false 180 | Name: Current View 181 | Near Clip Distance: 0.009999999776482582 182 | Pitch: 1.5697963237762451 183 | Target Frame: 184 | Yaw: 4.713412761688232 185 | Saved: ~ 186 | Window Geometry: 187 | Displays: 188 | collapsed: true 189 | Height: 420 190 | Hide Left Dock: true 191 | Hide Right Dock: true 192 | QMainWindow State: 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 193 | Selection: 194 | collapsed: false 195 | Time: 196 | collapsed: false 197 | Tool Properties: 198 | collapsed: false 199 | Views: 200 | collapsed: true 201 | Width: 439 202 | X: 555 203 | Y: 142 204 | -------------------------------------------------------------------------------- /src/multirobotexploration/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | pkg_config = generate_distutils_setup( 6 | packages=['multirobotexploration'], 7 | scripts=[], 8 | package_dir={'': 'scripts'} 9 | ) 10 | 11 | setup(**pkg_config) -------------------------------------------------------------------------------- /src/multirobotexploration/source/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/common/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/common/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/common/RendezvousPlan.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2023 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | 20 | #include "RendezvousPlan.h" 21 | #include "ros/ros.h" 22 | #include 23 | 24 | RendezvousPlan::RendezvousPlan(ros::NodeHandle& nodeHandle, const int& id) { 25 | this->id = id; 26 | 27 | std::vector K; 28 | std::vector W; 29 | int width, height; 30 | 31 | nodeHandle.getParam("/width", width); 32 | nodeHandle.getParam("/K", K); 33 | nodeHandle.getParam("/W", W); 34 | 35 | height = static_cast(K.size() / width); 36 | this->width = width; 37 | 38 | // as a preference, agreements are being stored as rows instead 39 | // of a single memory block 40 | for(int y = 0; y < height; ++y) { 41 | std::vector row; 42 | 43 | if(K[width * y + id] == 1) { 44 | for(int x = 0; x < width; ++x) { 45 | int index = width * y + x; 46 | 47 | agreement_el el; 48 | el.participate = K[index]; 49 | el.timer = W[index]; 50 | 51 | row.push_back(el); 52 | } 53 | this->agreements.push_back(std::pair>(y, row)); 54 | } 55 | } 56 | 57 | current_agreement = 0; 58 | current_timer = GetCurrentAgreementTimer(); 59 | currentPlanRealization.assign(width,0); 60 | } 61 | 62 | RendezvousPlan::~RendezvousPlan() { 63 | 64 | } 65 | 66 | void RendezvousPlan::PrintRealization() { 67 | printf("[RendezvousPlan] Realization: "); 68 | for(size_t robot=0; robotwidth, 0); 85 | } 86 | 87 | std::vector* RendezvousPlan::GetPlanPtr() { 88 | return ¤tPlanRealization; 89 | } 90 | 91 | void RendezvousPlan::RealizePlan(const int& robotId) { 92 | if(robotId < 0 || robotId >= currentPlanRealization.size()) 93 | throw std::out_of_range("robot id out of range in RealizePlan."); 94 | currentPlanRealization[robotId] = 1; 95 | } 96 | 97 | void RendezvousPlan::Print() { 98 | std::string to_print = "Rendezvous plan\n\t"; 99 | for(size_t row = 0; row < agreements.size(); ++row) { 100 | for(size_t col = 0; col < agreements[row].second.size(); ++col) { 101 | to_print += "[" + std::to_string(agreements[row].second[col].participate) + "," + std::to_string(agreements[row].second[col].timer) + "]"; 102 | } 103 | to_print += "\n\t"; 104 | } 105 | ROS_INFO("[RendezvousPlan] %s", to_print.c_str()); 106 | } 107 | 108 | void RendezvousPlan::PrintCurrent() { 109 | int unique_id = GetCurrentAgreementUniqueID(); 110 | ROS_INFO("[RendezvousPlan] id: %d key: %s - timer: %f/%f", 111 | unique_id, GenerateAgreementKey(current_agreement).c_str(), current_timer, agreements[current_agreement].second[id].timer); 112 | } 113 | 114 | void RendezvousPlan::PrintLocations() { 115 | std::map::iterator it = agreements_locations.begin(); 116 | for(;it != agreements_locations.end(); ++it) { 117 | ROS_INFO("Key %s x: %f y: %f z: %f", it->first.c_str(), it->second.getX(), it->second.getY(), it->second.getZ()); 118 | } 119 | } 120 | 121 | int RendezvousPlan::GetCurrentAgreement() { 122 | return current_agreement; 123 | } 124 | 125 | int RendezvousPlan::GetCurrentAgreementConsensusID() { 126 | for(size_t i = 0; i < agreements[current_agreement].second.size(); ++i) { 127 | int el = agreements[current_agreement].second[i].participate; 128 | if(el == 1) return i; 129 | } 130 | return -1; 131 | } 132 | 133 | bool RendezvousPlan::CheckConsensusCurrentPlan() { 134 | // this should be pre-computed as some of the other stuff 135 | int consensus = 0; 136 | for(size_t i = 0; i < agreements[current_agreement].second.size(); ++i) { 137 | int el = agreements[current_agreement].second[i].participate; 138 | if(el == 1) { 139 | consensus = i; 140 | break; 141 | } 142 | } 143 | printf("[RendezvousPlan] Consensus id: %d my id: %d\n", consensus, id); 144 | return (id == consensus); 145 | } 146 | 147 | double RendezvousPlan::GetCurrentAgreementTimer() { 148 | return agreements[current_agreement].second[this->id].timer; 149 | } 150 | 151 | void RendezvousPlan::SetNextAgreement() { 152 | current_agreement += 1; 153 | if(current_agreement >= agreements.size()) 154 | current_agreement = 0; 155 | current_timer = GetCurrentAgreementTimer(); 156 | } 157 | 158 | std::string RendezvousPlan::GenerateAgreementKey(const int& index) { 159 | if(index < 0 || index >= agreements.size()) throw std::out_of_range("Index out of range in GenerateAgreementKey."); 160 | std::string key = ""; 161 | for(auto& val : agreements[index].second) { 162 | key += std::to_string(val.participate); 163 | } 164 | return key; 165 | } 166 | 167 | void RendezvousPlan::InitializeLocation(tf::Vector3 location) { 168 | std::string key = ""; 169 | for(size_t agreement = 0; agreement < agreements.size(); ++agreement) { 170 | key = GenerateAgreementKey(agreement); 171 | agreements_locations[key] = location; 172 | } 173 | } 174 | 175 | void RendezvousPlan::UpdateCurrentAgreementLocation(tf::Vector3 newLocation) { 176 | if(current_agreement < 0 || current_agreement >= agreements.size()) throw std::out_of_range("Index out of range in UpdateAgreementLocation."); 177 | std::string key = GenerateAgreementKey(current_agreement); 178 | agreements_locations[key] = newLocation; 179 | } 180 | 181 | tf::Vector3 RendezvousPlan::GetCurrentAgreementLocation() { 182 | if(current_agreement < 0 || current_agreement >= agreements.size()) throw std::out_of_range("Index out of range in GetCurrentAgreementLocation."); 183 | std::string key = GenerateAgreementKey(current_agreement); 184 | if(agreements_locations.find(key)!=agreements_locations.end()) { 185 | return agreements_locations[key]; 186 | } 187 | throw std::runtime_error("Cannot locate key in GetCurrentAgreementLocation"); 188 | } 189 | 190 | bool RendezvousPlan::ShouldFulfillAgreement() { 191 | if(current_timer <= 0.0) return true; 192 | return false; 193 | } 194 | 195 | void RendezvousPlan::Update(const double& deltaTime) { 196 | current_timer -= deltaTime; 197 | } 198 | 199 | int RendezvousPlan::GetCurrentAgreementUniqueID() { 200 | if(current_agreement < 0 || current_agreement >= agreements.size()) throw std::out_of_range("Index out of range in GetCurrentAgreementUniqueID."); 201 | return agreements[current_agreement].first; 202 | } -------------------------------------------------------------------------------- /src/multirobotexploration/source/communication/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/communication/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/communication/MockCommunicationModelNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2023 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include "MockCommunicationModelNode.h" 20 | 21 | /* 22 | * Node implementation 23 | */ 24 | MockCommunicationModelNode::MockCommunicationModelNode() { 25 | ros::NodeHandle node_handle("~"); 26 | 27 | // load all parameters 28 | if(!node_handle.getParam("/robots", aRobots)) throw std::runtime_error("Could not retrieve /robots."); 29 | if(!node_handle.getParam("id", aId)) throw std::runtime_error("Could not retrieve id."); 30 | if(!node_handle.getParam("queue_size", aQueueSize)) aQueueSize = 2; 31 | if(!node_handle.getParam("comm_dist", aCommDist)) throw std::runtime_error("Could not retrieve comm_dist."); 32 | if(!node_handle.getParam("rate", aRate)) aRate = 2.0; 33 | aNamespace = ros::this_node::getNamespace(); 34 | 35 | // initialize communication containers 36 | aReceivedPoses.assign(aRobots, false); 37 | aRobotsWorldPoses.assign(aRobots, geometry_msgs::Pose()); 38 | aRelativePoses.assign(aRobots, tf::Vector3()); 39 | aRobotsInComm.data.assign(aRobots, 0); 40 | LoadRelativePoses(node_handle); 41 | 42 | /* 43 | * I use lambda in the multi-robot context subscriptions, because it can help 44 | * having multiple callbacks, one for each robot, without explicitly 45 | * defining them, which would required some engineering. 46 | */ 47 | std::vector* receivedPosesPtr = &aReceivedPoses; 48 | std::vector* robotsWorldPosesPtr = &aRobotsWorldPoses; 49 | for(int robot = 0; robot < aRobots; ++robot) { 50 | aSubscribers.push_back(node_handle.subscribe("/robot_" + std::to_string(robot) + "/gmapping_pose/world_pose", aQueueSize, 51 | [robot, receivedPosesPtr, robotsWorldPosesPtr](multirobotsimulations::CustomPose::ConstPtr msg) { 52 | robotsWorldPosesPtr->at(robot).position = msg->pose.position; 53 | robotsWorldPosesPtr->at(robot).orientation = msg->pose.orientation; 54 | receivedPosesPtr->at(robot) = true; 55 | })); 56 | } 57 | 58 | // Advertisers 59 | aCommunicationModelBroadcaster = node_handle.advertise(aNamespace + "/mock_communication_model/robots_in_comm", aQueueSize); 60 | 61 | // Node's routines 62 | double update_period = PeriodToFreqAndFreqToPeriod(aRate); 63 | aTimers.push_back(node_handle.createTimer(ros::Duration(update_period), std::bind(&MockCommunicationModelNode::Update, this))); 64 | } 65 | 66 | MockCommunicationModelNode::~MockCommunicationModelNode() { 67 | 68 | } 69 | 70 | void MockCommunicationModelNode::LoadRelativePoses( ros::NodeHandle& nodeHandle) { 71 | // read relative start poses parameters 72 | std::vector> poses; 73 | std::string key = ""; 74 | for (int robot = 0; robot < aRobots; ++robot) { 75 | key = "/start_pose_robot_" + std::to_string(robot); 76 | std::map pose; 77 | nodeHandle.getParam(key, pose); 78 | poses.push_back(pose); 79 | ROS_INFO("[MockCommunicationModelNode]: %s: %f %f %f", key.c_str(), pose["x"], pose["y"], pose["z"]); 80 | } 81 | 82 | // compute relative poses from file 83 | tf::Vector3 my_pose(poses[aId]["x"], poses[aId]["y"], poses[aId]["z"]); 84 | for(int robot = 0; robot < aRobots; ++robot) { 85 | if(robot!=aId) { 86 | tf::Vector3 other_pose(poses[robot]["x"], poses[robot]["y"], poses[robot]["z"]); 87 | tf::Vector3 dir = other_pose - my_pose; 88 | aRelativePoses[robot] = dir; 89 | ROS_INFO("[MockCommunicationModelNode] relative to %d: %f %f %f", robot, dir.getX(), dir.getY(), dir.getZ()); 90 | } else { 91 | aRelativePoses[robot].setX(0.0); 92 | aRelativePoses[robot].setY(0.0); 93 | aRelativePoses[robot].setZ(0.0); 94 | ROS_INFO("[MockCommunicationModelNode] relative to self: %f %f %f", aRelativePoses[robot].getX(), aRelativePoses[robot].getY(), aRelativePoses[robot].getZ()); 95 | } 96 | } 97 | } 98 | 99 | void MockCommunicationModelNode::Update() { 100 | // translate all the poses here 101 | tf::Vector3 relative_pose; 102 | tf::Vector3 my_pose; 103 | PoseToVector3(aRobotsWorldPoses[aId], my_pose); 104 | 105 | double distance = 0.0; 106 | for(int robot = 0; robot < aRobots; ++robot) { 107 | if(aReceivedPoses[robot] == true) { 108 | PoseToVector3(aRobotsWorldPoses[robot], relative_pose); 109 | relative_pose += aRelativePoses[robot]; 110 | 111 | // check distance to me to see if this the relative positions should be updated 112 | distance = relative_pose.distance(my_pose); 113 | 114 | if(distance < aCommDist) { 115 | // set nearby robots 116 | aRobotsInComm.data[robot] = 1; 117 | } else { 118 | aRobotsInComm.data[robot] = 0; 119 | } 120 | } 121 | } 122 | 123 | // send data to the system 124 | aCommunicationModelBroadcaster.publish(aRobotsInComm); 125 | } 126 | 127 | /* 128 | * Node's main function 129 | */ 130 | int main(int argc, char* argv[]) { 131 | ros::init(argc, argv, "mockcommunicationmodelnode"); 132 | std::unique_ptr mockCommunicationModelNode = std::make_unique(); 133 | ros::spin(); 134 | } -------------------------------------------------------------------------------- /src/multirobotexploration/source/frontier/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/frontier/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/laser/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/laser/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/laser/LaserToWorldNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include "LaserToWorldNode.h" 20 | 21 | LaserToWorldNode::LaserToWorldNode() { 22 | ros::NodeHandle node_handle("~"); 23 | aHasLidar = false; 24 | aHasOccInfo = false; 25 | aHasPose = false; 26 | 27 | // load all parameters 28 | double x,y,z,roll,pitch,yaw; 29 | if(!node_handle.getParam("x", x)) x = 0.0; 30 | if(!node_handle.getParam("y", y)) y = 0.0; 31 | if(!node_handle.getParam("z", z)) z = 0.0; 32 | if(!node_handle.getParam("roll", roll)) roll = 0.0; 33 | if(!node_handle.getParam("pitch", pitch)) pitch = 0.0; 34 | if(!node_handle.getParam("yaw", yaw)) yaw = 0.0; 35 | if(!node_handle.getParam("rate", aRate)) aRate = 5; 36 | if(!node_handle.getParam("queue_size", aQueueSize)) aQueueSize = 2; 37 | aNamespace = ros::this_node::getNamespace(); 38 | 39 | aLidarPosition = tf::Vector3(x,y,0.0); 40 | aLidarOrientation = tf::createQuaternionFromRPY(roll,pitch,yaw); 41 | 42 | // subscriptions 43 | aSubscribers.push_back( 44 | node_handle.subscribe( 45 | aNamespace + "/scan", 46 | aQueueSize, 47 | std::bind(&LaserToWorldNode::LaserCapture, this, std::placeholders::_1))); 48 | 49 | aSubscribers.push_back( 50 | node_handle.subscribe( 51 | aNamespace + "/gmapping_pose/world_pose", 52 | aQueueSize, 53 | std::bind(&LaserToWorldNode::EstimatePoseWorldCallback, this, std::placeholders::_1))); 54 | 55 | aSubscribers.push_back( 56 | node_handle.subscribe( 57 | aNamespace + "/map", 58 | aQueueSize, 59 | std::bind(&LaserToWorldNode::OccupancyGridCallback, this, std::placeholders::_1))); 60 | 61 | // advertisers 62 | aLidarPublisher = node_handle.advertise(aNamespace + "/laser_to_world/laser_world", aQueueSize); 63 | aOccLidarPublisher = node_handle.advertise(aNamespace + "/laser_to_world/laser_occ", aQueueSize); 64 | 65 | // node's routines 66 | double update_period = PeriodToFreqAndFreqToPeriod(aRate); 67 | aTimers.push_back(node_handle.createTimer(ros::Duration(update_period), std::bind(&LaserToWorldNode::Update, this))); 68 | } 69 | 70 | LaserToWorldNode::~LaserToWorldNode() { 71 | 72 | } 73 | 74 | void LaserToWorldNode::EstimatePoseWorldCallback(multirobotsimulations::CustomPose::ConstPtr msg) { 75 | aRobotWorldPosition.setX(msg->pose.position.x); 76 | aRobotWorldPosition.setY(msg->pose.position.y); 77 | aRobotWorldPosition.setZ(0.0); 78 | aRobotYaw = tf::getYaw(msg->pose.orientation); 79 | aHasPose = true; 80 | } 81 | 82 | void LaserToWorldNode::OccupancyGridCallback(nav_msgs::OccupancyGrid::ConstPtr msg) { 83 | aOccInfo.info = msg->info; 84 | aHasOccInfo = true; 85 | } 86 | 87 | void LaserToWorldNode::LaserCapture(sensor_msgs::LaserScan::ConstPtr msg) { 88 | if(!aHasPose || !aHasOccInfo) return 89 | 90 | // clear previous readings 91 | aWorldReadings.clear(); 92 | aOccReadings.clear(); 93 | double increment = msg->angle_increment; 94 | double theta = msg->angle_min; 95 | tf::Vector3 rot_axis(0,0,1); 96 | 97 | // compute laser robot frame exact positions 98 | for(size_t beam = 0; beam < msg->ranges.size(); ++beam) { 99 | double range = msg->ranges[beam]; 100 | 101 | // crop lasers with threshold 102 | if(range > msg->range_max - aLidarError) { 103 | theta += increment; 104 | continue; 105 | } 106 | 107 | // do rotation on YZ plane normal X 108 | tf::Vector3 laser_vec(1,0,0); 109 | 110 | // rotate arround Z 111 | laser_vec = laser_vec.rotate(rot_axis, theta); 112 | 113 | // extend vector with range in meters 114 | tf::Vector3 laser_world = laser_vec * range + tf::Vector3(0.3,0.0,0.0); 115 | laser_world = laser_world.rotate(tf::Vector3(0,0,1), aRobotYaw); 116 | laser_world = laser_world.rotate(tf::Vector3(0,0,1), tf::getYaw(aLidarOrientation)); 117 | laser_world += aRobotWorldPosition; 118 | 119 | geometry_msgs::Pose laser_pose_world; 120 | laser_pose_world.position.x = laser_world.getX(); 121 | laser_pose_world.position.y = laser_world.getY(); 122 | laser_pose_world.position.z = range; 123 | 124 | // convert world laser coordinates into occ laser coordinates 125 | tf::Vector3 world_occ; 126 | WorldToMap(aOccInfo, laser_world, world_occ); 127 | geometry_msgs::Pose laser_pose_occ; 128 | laser_pose_occ.position.x = world_occ.getX(); 129 | laser_pose_occ.position.y = world_occ.getY(); 130 | laser_pose_occ.position.z = range; 131 | 132 | // add laser point to readings array 133 | aWorldReadings.push_back(laser_pose_world); 134 | aOccReadings.push_back(laser_pose_occ); 135 | 136 | // increment ray angle 137 | theta += increment; 138 | } 139 | 140 | aHasLidar = true; 141 | } 142 | 143 | void LaserToWorldNode::Update() { 144 | if(!aHasLidar) return; 145 | 146 | aWorldLidarMsg.poses.assign(aWorldReadings.begin(), aWorldReadings.end()); 147 | aOccLidarMsg.poses.assign(aOccReadings.begin(), aOccReadings.end()); 148 | 149 | aLidarPublisher.publish(aWorldLidarMsg); 150 | aOccLidarPublisher.publish(aOccLidarMsg); 151 | } 152 | 153 | int main(int argc, char* argv[]) { 154 | ros::init(argc, argv, "lasertoworldnode"); 155 | std::unique_ptr laserToWorldNode = std::make_unique(); 156 | ros::spin(); 157 | } -------------------------------------------------------------------------------- /src/multirobotexploration/source/localization/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/localization/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/localization/AverageVelocityEstimatorNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include "AverageVelocityEstimatorNode.h" 20 | 21 | AverageVelocityEstimatorNode::AverageVelocityEstimatorNode() { 22 | ros::NodeHandle node_handle("~"); 23 | 24 | // load all parameters 25 | if(!node_handle.getParam("id", aId)) throw std::runtime_error("Could not retrieve id."); 26 | if(!node_handle.getParam("rate", aRate)) aRate = 2.0;; 27 | if(!node_handle.getParam("queue_size", aQueueSize)) aQueueSize = 2; 28 | if(!node_handle.getParam("count", aCount)) aCount = 10; 29 | aNamespace = ros::this_node::getNamespace(); 30 | 31 | // subscriptions 32 | aSubscribers.push_back(node_handle.subscribe( 33 | aNamespace + "/gmapping_pose/world_pose", 34 | aQueueSize, 35 | std::bind(&AverageVelocityEstimatorNode::WorldPoseCallback, this, std::placeholders::_1))); 36 | 37 | // advertisers 38 | aAverageVelocityPublisher = node_handle.advertise(aNamespace + "/average_velocity", aQueueSize); 39 | 40 | // node's routines 41 | double update_period = PeriodToFreqAndFreqToPeriod(aRate); 42 | aTimers.push_back(node_handle.createTimer(ros::Duration(update_period), std::bind(&AverageVelocityEstimatorNode::Update, this))); 43 | } 44 | 45 | AverageVelocityEstimatorNode::~AverageVelocityEstimatorNode() { 46 | 47 | } 48 | 49 | void AverageVelocityEstimatorNode::WorldPoseCallback(multirobotsimulations::CustomPose::ConstPtr msg) { 50 | aWorldPos.setX(msg->pose.position.x); 51 | aWorldPos.setY(msg->pose.position.y); 52 | 53 | // initialize last pos, once as soon 54 | // as a pose is received 55 | if(!aReceivedPosition) { 56 | aLastWorldPos = aWorldPos; 57 | aReceivedPosition = true; 58 | } 59 | } 60 | 61 | double AverageVelocityEstimatorNode::ComputeAverageVelocity(std::deque& speedArray) { 62 | double average = 0.0; 63 | double velocities = static_cast(aVelocityArray.size()); 64 | 65 | if(velocities > 0) { 66 | for(std::deque::iterator i = aVelocityArray.begin(); i != aVelocityArray.end(); ++i) average += (*i); 67 | average /= velocities; 68 | } 69 | 70 | return average; 71 | } 72 | 73 | void AverageVelocityEstimatorNode::Update() { 74 | // compute moving average 75 | if(aReceivedPosition) { 76 | aAverageVelocityMsg.data = ComputeAverageVelocity(aVelocityArray); 77 | aAverageVelocityPublisher.publish(aAverageVelocityMsg); 78 | } 79 | 80 | // update velocities array 81 | aVelocityArray.push_back(aWorldPos.distance(aLastWorldPos)); 82 | if(aVelocityArray.size() > aCount) aVelocityArray.pop_front(); 83 | aLastWorldPos = aWorldPos; 84 | } 85 | 86 | int main(int argc, char* argv[]) { 87 | ros::init(argc, argv, "averagespeedestimatornode"); 88 | std::unique_ptr averagePoseEstimatorNode = std::make_unique(); 89 | ros::spin(); 90 | } -------------------------------------------------------------------------------- /src/multirobotexploration/source/localization/GmappingPoseNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include "GmappingPoseNode.h" 20 | 21 | GmappingPoseNode::GmappingPoseNode() { 22 | ros::NodeHandle node_handle("~"); 23 | 24 | // load all parameters 25 | if(!node_handle.getParam("id", aId)) throw std::runtime_error("Could not retrieve id."); 26 | if(!node_handle.getParam("rate", aRate)) aRate = 2.0; 27 | if(!node_handle.getParam("queue_size", aQueueSize)) aQueueSize = 2; 28 | aNamespace = ros::this_node::getNamespace(); 29 | 30 | // transform listener 31 | aTFBuffer = std::make_shared(); 32 | aTFListener = std::make_shared(*aTFBuffer); 33 | 34 | // transform frame names 35 | std::string ns_clean = aNamespace; 36 | ns_clean.erase(remove(ns_clean.begin(), ns_clean.end(), '/'), ns_clean.end()); 37 | ROS_INFO("[gmapping_pose] lookup transform name: %s from: %s", ns_clean.c_str(), aNamespace.c_str()); 38 | 39 | if(ns_clean.size() == 0) { 40 | aTFMap = "map"; 41 | aTFBaseLink = "base_link"; 42 | } else { 43 | aTFMap = ns_clean + "/map"; 44 | aTFBaseLink = ns_clean + "/base_link"; 45 | } 46 | ROS_INFO("[gmapping_pose] looking for tf: %s", aTFMap.c_str()); 47 | 48 | // subscriptions 49 | aSubscribers.push_back( 50 | node_handle.subscribe( 51 | aNamespace + "/map", 52 | aQueueSize, 53 | std::bind(&GmappingPoseNode::OccCallback, this, std::placeholders::_1))); 54 | 55 | // advertisers 56 | aPosePublisher = node_handle.advertise(aNamespace + "/gmapping_pose/world_pose", aQueueSize); 57 | aPoseStampedPublisher = node_handle.advertise(aNamespace + "/gmapping_pose/pose_stamped", aQueueSize); 58 | aPathPublisher = node_handle.advertise(aNamespace + "/path", aQueueSize); 59 | 60 | // node's routines 61 | double update_period = PeriodToFreqAndFreqToPeriod(aRate); 62 | aTimers.push_back(node_handle.createTimer(ros::Duration(update_period), std::bind(&GmappingPoseNode::Update, this))); 63 | } 64 | 65 | GmappingPoseNode::~GmappingPoseNode() { 66 | 67 | } 68 | 69 | void GmappingPoseNode::OccCallback(nav_msgs::OccupancyGrid::ConstPtr msg) { 70 | aOcc.info = msg->info; 71 | } 72 | 73 | void GmappingPoseNode::Update() { 74 | /* 75 | * get current estimated pose from gmapping 76 | */ 77 | geometry_msgs::TransformStamped map_base_link; 78 | try{ 79 | /* 80 | * Transform lookup do the transformation between the map reference frame 81 | * and the base_link reference frame 82 | */ 83 | map_base_link = aTFBuffer->lookupTransform(aTFMap, aTFBaseLink, ros::Time(0)); 84 | 85 | /* 86 | * Process captured transform 87 | */ 88 | aPoseStamped.header = map_base_link.header; 89 | aPoseStamped.pose.position.x = map_base_link.transform.translation.x; 90 | aPoseStamped.pose.position.y = map_base_link.transform.translation.y; 91 | aPoseStamped.pose.position.z = map_base_link.transform.translation.z; 92 | 93 | /* 94 | * Prepare pose to be published with captured transformation data 95 | */ 96 | aPose.pose.position.x = map_base_link.transform.translation.x; 97 | aPose.pose.position.y = map_base_link.transform.translation.y; 98 | aPose.pose.position.z = map_base_link.transform.translation.z; 99 | aPose.pose.orientation = map_base_link.transform.rotation; 100 | aPose.robot_id = aId; 101 | 102 | /* 103 | * Assemble path 104 | */ 105 | aPath.header = map_base_link.header; 106 | aPath.poses.push_back(aPoseStamped); 107 | 108 | /* 109 | * Broadcast all data 110 | */ 111 | aPoseStampedPublisher.publish(aPoseStamped); 112 | aPosePublisher.publish(aPose); 113 | aPathPublisher.publish(aPath); 114 | } catch(tf2::TransformException &ex) { 115 | ROS_INFO("[gmapping_pose] %s", ex.what()); 116 | } 117 | } 118 | 119 | int main(int argc, char* argv[]) { 120 | ros::init(argc, argv, "gmappingposenode"); 121 | std::unique_ptr gmappingPoseNode = std::make_unique(); 122 | ros::spin(); 123 | } -------------------------------------------------------------------------------- /src/multirobotexploration/source/localization/RelativePoseEstimatorNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include "RelativePoseEstimatorNode.h" 20 | 21 | RelativePoseEstimatorNode::RelativePoseEstimatorNode() { 22 | ros::NodeHandle node_handle("~"); 23 | 24 | // load all parameters 25 | if(!node_handle.getParam("/robots", aRobots)) throw std::runtime_error("Could not retrieve robots."); 26 | if(!node_handle.getParam("id", aId)) throw std::runtime_error("Coult not retrieve id."); 27 | if(!node_handle.getParam("queue_size", aQueueSize)) aQueueSize = 2; 28 | if(!node_handle.getParam("rate", aRate)) aRate = 2.0; 29 | aNamespace = ros::this_node::getNamespace(); 30 | 31 | // variables initialization 32 | aSeq = 0; 33 | 34 | // initialize all containers to hold robots' information 35 | aRobotsWorldPoses.assign(aRobots, geometry_msgs::Pose()); 36 | aRelativePoses.assign(aRobots, tf::Vector3()); 37 | aReceivedPoses.assign(aRobots, false); 38 | aRobotsRelativePosesMsg.poses.assign(aRobots, geometry_msgs::Pose()); 39 | aRobotsRelStartingPosMsg.poses.assign(aRobots, geometry_msgs::Pose()); 40 | aRobotsRelativeDistancesMsg.data.assign(aRobots, 0.0); 41 | aRobotsInCommMsg.data.assign(aRobots, 0); 42 | 43 | // load relative poses container 44 | LoadRelativePoses(node_handle); 45 | 46 | // subscriptions 47 | std::vector* robotsWorldPosesPtr = &aRobotsWorldPoses; 48 | std::vector* receivedPosesPtr = &aReceivedPoses; 49 | for(int robot = 0; robot < aRobots; ++robot) { 50 | aSubscribers.push_back(node_handle.subscribe("/robot_" + std::to_string(robot) +"/gmapping_pose/world_pose", aQueueSize, 51 | [receivedPosesPtr, robotsWorldPosesPtr, robot](multirobotsimulations::CustomPose::ConstPtr msg) { 52 | robotsWorldPosesPtr->at(robot).position = msg->pose.position; 53 | robotsWorldPosesPtr->at(robot).orientation = msg->pose.orientation; 54 | receivedPosesPtr->at(robot) = true; 55 | })); 56 | } 57 | 58 | aSubscribers.push_back( 59 | node_handle.subscribe( 60 | aNamespace + "/mock_communication_model/robots_in_comm", 61 | aQueueSize, 62 | std::bind(&RelativePoseEstimatorNode::CommCallback, this, std::placeholders::_1))); 63 | 64 | // advertisers 65 | aStartRelativePosesPublisher = node_handle.advertise(aNamespace + "/relative_pose_estimator/relative_start", aQueueSize); 66 | aRelativePosesPublisher = node_handle.advertise(aNamespace + "/relative_pose_estimator/relative_poses", aQueueSize); 67 | aDistancesPublisher = node_handle.advertise(aNamespace + "/relative_pose_estimator/distances", aQueueSize); 68 | aNearMarkerPublisher = node_handle.advertise(aNamespace + "/relative_pose_estimator/pose_markers", aQueueSize); 69 | aFarMarkerPublisher = node_handle.advertise(aNamespace + "/relative_pose_estimator/pose_far_markers", aQueueSize); 70 | 71 | // node's routines 72 | double update_period = PeriodToFreqAndFreqToPeriod(aRate); 73 | aTimers.push_back(node_handle.createTimer(ros::Duration(update_period), std::bind(&RelativePoseEstimatorNode::Update, this))); 74 | } 75 | 76 | RelativePoseEstimatorNode::~RelativePoseEstimatorNode() { 77 | } 78 | 79 | void RelativePoseEstimatorNode::PrepareMarkers(visualization_msgs::Marker& input, const char* ns, const int& id, const int& seq) { 80 | input.id = id; 81 | input.header.frame_id = std::string("robot_") + std::to_string(id) + std::string("/map"); 82 | input.header.stamp = ros::Time().now(); 83 | input.header.seq = seq; 84 | input.ns = ns; 85 | input.points.clear(); 86 | input.type = visualization_msgs::Marker::CUBE_LIST; 87 | input.action = visualization_msgs::Marker::MODIFY; 88 | input.pose.orientation.x = 0.0; 89 | input.pose.orientation.y = 0.0; 90 | input.pose.orientation.z = 0.0; 91 | input.pose.orientation.w = 1.0; 92 | input.scale.x = 0.5; 93 | input.scale.y = 0.5; 94 | input.scale.z = 0.5; 95 | input.color.a = 1.0; 96 | input.color.r = 1.0; 97 | input.color.g = 0.3; 98 | input.color.b = 0.1; 99 | input.lifetime = ros::Duration(60); 100 | } 101 | 102 | void RelativePoseEstimatorNode::SetNear(visualization_msgs::Marker& input) { 103 | input.color.a = 1.0; 104 | input.color.r = 0.0; 105 | input.color.g = 1.0; 106 | input.color.b = 0.1; 107 | } 108 | 109 | void RelativePoseEstimatorNode::SetFar(visualization_msgs::Marker& input) { 110 | input.color.a = 1.0; 111 | input.color.r = 1.0; 112 | input.color.g = 0.0; 113 | input.color.b = 0.1; 114 | } 115 | 116 | void RelativePoseEstimatorNode::CommCallback(std_msgs::Int8MultiArray::ConstPtr msg) { 117 | aRobotsInCommMsg.data.assign(msg->data.begin(), msg->data.end()); 118 | } 119 | 120 | void RelativePoseEstimatorNode::LoadRelativePoses(ros::NodeHandle& nodeHandle) { 121 | // read relative start poses parameters 122 | std::vector> poses; 123 | std::string key = ""; 124 | for (int robot = 0; robot < aRobots; ++robot) { 125 | key = "/start_pose_robot_" + std::to_string(robot); 126 | std::map pose; 127 | nodeHandle.getParam(key, pose); 128 | poses.push_back(pose); 129 | ROS_INFO("[Relative_pose_estimator]: %s: %f %f %f", key.c_str(), pose["x"], pose["y"], pose["z"]); 130 | } 131 | 132 | // compute relative poses from file 133 | tf::Vector3 my_pose(poses[aId]["x"], poses[aId]["y"], poses[aId]["z"]); 134 | for(int robot = 0; robot < aRobots; ++robot) { 135 | if(robot!=aId) { 136 | tf::Vector3 other_pose(poses[robot]["x"], poses[robot]["y"], poses[robot]["z"]); 137 | tf::Vector3 dir = other_pose - my_pose; 138 | aRelativePoses[robot] = dir; 139 | Vector3ToPose(dir, aRobotsRelStartingPosMsg.poses[robot]); 140 | ROS_INFO("[Relative_pose_estimator] relative to %d: %f %f %f", robot, dir.getX(), dir.getY(), dir.getZ()); 141 | } else { 142 | aRelativePoses[robot].setX(0.0); 143 | aRelativePoses[robot].setY(0.0); 144 | aRelativePoses[robot].setZ(0.0); 145 | ROS_INFO("[Relative_pose_estimator] relative to self: %f %f %f", aRelativePoses[robot].getX(), aRelativePoses[robot].getY(), aRelativePoses[robot].getZ()); 146 | } 147 | } 148 | } 149 | 150 | void RelativePoseEstimatorNode::Update() { 151 | // create a new cluster marker msg to be sent 152 | PrepareMarkers(aClusterMarkerMsg, aNamespace.c_str(), aId, aSeq); 153 | SetNear(aClusterMarkerMsg); 154 | 155 | PrepareMarkers(aClusterMarkerFarMsg, aNamespace.c_str(), aId, aSeq); 156 | SetFar(aClusterMarkerFarMsg); 157 | 158 | // translate all the poses here 159 | tf::Vector3 relative_pose; 160 | tf::Vector3 mine; 161 | PoseToVector3(aRobotsWorldPoses[aId], mine); 162 | double distance = 0.0; 163 | for(int robot = 0; robot < aRobots; ++robot) { 164 | if(robot == aId) continue; 165 | 166 | if(aReceivedPoses[robot] == true) { 167 | PoseToVector3(aRobotsWorldPoses[robot], relative_pose); 168 | relative_pose += aRelativePoses[robot]; 169 | 170 | geometry_msgs::Point p; 171 | 172 | if(aRobotsInCommMsg.data[robot] == 1) { 173 | // store into pose array to be sent 174 | Vector3ToPose(relative_pose, aRobotsRelativePosesMsg.poses[robot]); 175 | p.z = 0.25; 176 | p.x = aRobotsRelativePosesMsg.poses[robot].position.x; 177 | p.y = aRobotsRelativePosesMsg.poses[robot].position.y; 178 | 179 | // update marker array for relative poses 180 | // and do it only for the other robots 181 | aClusterMarkerMsg.points.push_back(p); 182 | 183 | // check distance to me to see if this the relative positions should be updated 184 | distance = relative_pose.distance(mine); 185 | } else { 186 | p.z = 0.25; 187 | p.x = aRobotsRelativePosesMsg.poses[robot].position.x; 188 | p.y = aRobotsRelativePosesMsg.poses[robot].position.y; 189 | 190 | aClusterMarkerFarMsg.points.push_back(p); 191 | distance = 10000000.0; 192 | } 193 | 194 | // also set the distances array 195 | aRobotsRelativeDistancesMsg.data[robot] = distance; 196 | } 197 | } 198 | 199 | // send data to the system 200 | aStartRelativePosesPublisher.publish(aRobotsRelStartingPosMsg); 201 | aRelativePosesPublisher.publish(aRobotsRelativePosesMsg); 202 | aDistancesPublisher.publish(aRobotsRelativeDistancesMsg); 203 | aNearMarkerPublisher.publish(aClusterMarkerMsg); 204 | aFarMarkerPublisher.publish(aClusterMarkerFarMsg); 205 | 206 | aSeq += 1; 207 | } 208 | 209 | int main(int argc, char* argv[]) { 210 | ros::init(argc, argv, "relativeposeestimatornode"); 211 | std::unique_ptr relativePoseEstimatorNode = std::make_unique(); 212 | ros::spin(); 213 | } -------------------------------------------------------------------------------- /src/multirobotexploration/source/map/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/map/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/map/MapStitchingNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 3 | * Copyright (C) 2020 Alysson Ribeiro da Silva 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | 20 | #include "MapStitchingNode.h" 21 | 22 | MapStitchingNode::MapStitchingNode() { 23 | ros::NodeHandle node_handle("~"); 24 | 25 | // load all parameters 26 | if(!node_handle.getParam("/robots", aRobots)) throw std::runtime_error("Could not retrieve /robots."); 27 | if(!node_handle.getParam("id", aId)) throw std::runtime_error("Could not retrieve id."); 28 | if(!node_handle.getParam("rate", aRate)) aRate = 2.0; 29 | if(!node_handle.getParam("queue_size", aQueueSize)) aQueueSize = 2; 30 | aNamespace = ros::this_node::getNamespace(); 31 | 32 | aReceivedRelativePoses = false; 33 | aDirty = true; 34 | 35 | // containers initialization 36 | aRobotsInCommMsg.data.assign(aRobots, 0); 37 | aRobotsOcc.assign(aRobots, nav_msgs::OccupancyGrid()); 38 | aReceivedOccs.assign(aRobots, false); 39 | 40 | // subscriptions 41 | for(int robot = 0; robot < aRobots; ++robot) { 42 | if(robot == aId) continue; 43 | 44 | nav_msgs::OccupancyGrid* occPtr = &aRobotsOcc[robot]; 45 | std_msgs::Int8MultiArray* commPtr = &aRobotsInCommMsg; 46 | std::vector* receivedFlagPtr = &aReceivedOccs; 47 | aSubscribers.push_back( 48 | node_handle.subscribe( 49 | "/robot_" + std::to_string(robot) + "/fusion", 50 | aQueueSize, 51 | [occPtr, receivedFlagPtr, commPtr, robot](nav_msgs::OccupancyGrid::ConstPtr msg){ 52 | // mock communication to merge maps 53 | // if this flag is 0, then the 'robot' cannot 54 | // share its map 55 | if(commPtr->data[robot] == 0) return; 56 | 57 | // otherwise, update the last received map 58 | occPtr->data.assign(msg->data.begin(), msg->data.end()); 59 | occPtr->info = msg->info; 60 | occPtr->header = msg->header; 61 | 62 | // set received flag to true 63 | receivedFlagPtr->at(robot) = true; 64 | })); 65 | } 66 | 67 | aSubscribers.push_back( 68 | node_handle.subscribe( 69 | aNamespace + "/mock_communication_model/robots_in_comm", 70 | aQueueSize, 71 | std::bind(&MapStitchingNode::CommunicationsCallback, this, std::placeholders::_1))); 72 | 73 | aSubscribers.push_back( 74 | node_handle.subscribe( 75 | aNamespace + "/relative_pose_estimator/relative_start", 76 | aQueueSize, 77 | std::bind(&MapStitchingNode::RelativeStartingPosesCallback, this, std::placeholders::_1))); 78 | 79 | aSubscribers.push_back( 80 | node_handle.subscribe( 81 | aNamespace + "/map", 82 | aQueueSize, 83 | std::bind(&MapStitchingNode::OccCallback, this, std::placeholders::_1))); 84 | 85 | // advertisers 86 | aFusionPublisher = node_handle.advertise(aNamespace + "/fusion", aQueueSize); 87 | 88 | // node's routines 89 | double update_period = PeriodToFreqAndFreqToPeriod(aRate); 90 | aTimers.push_back(node_handle.createTimer(ros::Duration(update_period), std::bind(&MapStitchingNode::Update, this))); 91 | } 92 | 93 | MapStitchingNode::~MapStitchingNode() { 94 | 95 | } 96 | 97 | void MapStitchingNode::CommunicationsCallback(std_msgs::Int8MultiArray::ConstPtr msg) { 98 | aRobotsInCommMsg.data.assign(msg->data.begin(), msg->data.end()); 99 | } 100 | 101 | void MapStitchingNode::RelativeStartingPosesCallback(geometry_msgs::PoseArray::ConstPtr msg) { 102 | aRobotsRelativePosesMsg.header = msg->header; 103 | aRobotsRelativePosesMsg.poses.assign(msg->poses.begin(), msg->poses.end()); 104 | aReceivedRelativePoses = true; 105 | } 106 | 107 | void MapStitchingNode::OccCallback(nav_msgs::OccupancyGrid::ConstPtr msg) { 108 | aRobotsOcc[aId].data.assign(msg->data.begin(), msg->data.end()); 109 | aRobotsOcc[aId].info = msg->info; 110 | aRobotsOcc[aId].header = msg->header; 111 | aReceivedOccs[aId] = true; 112 | } 113 | 114 | void MapStitchingNode::Fusemaps(nav_msgs::OccupancyGrid& occ, 115 | nav_msgs::OccupancyGrid& other, 116 | geometry_msgs::Pose& relative, 117 | const bool& replace) { 118 | // transformation does not invert indexes 119 | int x, y; 120 | int8_t val; 121 | int8_t self_val; 122 | tf::Vector3 world_relative; 123 | tf::Vector3 world_origin; 124 | tf::Vector3 map_origin; 125 | tf::Vector3 map_relative; 126 | tf::Vector3 relative_transform; 127 | 128 | PoseToVector3(relative, world_relative); 129 | WorldToMap(occ, world_origin, map_origin); 130 | WorldToMap(occ, world_relative, map_relative); 131 | relative_transform = map_relative - map_origin; 132 | 133 | for(size_t index = 0; index < occ.data.size(); ++index) { 134 | x = index % occ.info.width; 135 | y = index / occ.info.width; 136 | 137 | int res_x = x + relative_transform.getX(); 138 | int res_y = y + relative_transform.getY(); 139 | 140 | int other_index = res_y * occ.info.width + res_x; 141 | 142 | self_val = occ.data[index]; 143 | val = other.data[index]; 144 | if(res_x > 0 && res_x < occ.info.width && res_y > 0 && res_y < occ.info.height) { 145 | if(replace == true) { 146 | if(val != -1) { 147 | occ.data[other_index] = val; 148 | } 149 | } else { 150 | if(self_val == -1) { 151 | if(val != -1) { 152 | occ.data[other_index] = val; 153 | } 154 | } 155 | } 156 | } 157 | } 158 | } 159 | 160 | void MapStitchingNode::Update() { 161 | if(aReceivedOccs[aId] == false || aReceivedRelativePoses == false) return; 162 | 163 | // initialize first occupancy grid 164 | if(aDirty) { 165 | aFusionMsg.info = aRobotsOcc[aId].info; 166 | aFusionMsg.header = aRobotsOcc[aId].header; 167 | aFusionMsg.data.assign(aRobotsOcc[aId].data.size(), -1); 168 | aDirty = false; 169 | } 170 | 171 | // do the map transformation here 172 | // other adjustments can be made 173 | // but for the purpose of several experiments, 174 | // initial translation should be enough 175 | for(int robot = 0; robot < aRobots; ++robot) { 176 | // do not process my map 177 | if(robot == aId || aReceivedOccs[robot] == false) continue; 178 | 179 | // just fuse the maps 180 | Fusemaps(aFusionMsg, aRobotsOcc[robot], aRobotsRelativePosesMsg.poses[robot], false); 181 | } 182 | 183 | // copy my map on top of everyone 184 | Fusemaps(aFusionMsg, aRobotsOcc[aId], aRobotsRelativePosesMsg.poses[aId], true); 185 | 186 | // publish fusion, this robot's map will always be on top 187 | aFusionPublisher.publish(aFusionMsg); 188 | } 189 | 190 | int main(int argc, char* argv[]) { 191 | ros::init(argc, argv, "mapstitchingnode"); 192 | std::unique_ptr mapStitchingNode = std::make_unique(); 193 | ros::spin(); 194 | } 195 | -------------------------------------------------------------------------------- /src/multirobotexploration/source/navigation/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/navigation/.gitkeep -------------------------------------------------------------------------------- /src/multirobotexploration/source/policies/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotexploration/source/policies/.gitkeep -------------------------------------------------------------------------------- /src/multirobotsimulations/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(multirobotsimulations) 3 | 4 | ## Compile as C++11 supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++17) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | sensor_msgs 12 | nav_msgs 13 | tf2_ros 14 | roslib 15 | message_generation 16 | message_runtime 17 | ) 18 | 19 | add_message_files( 20 | FILES 21 | CustomImage.msg 22 | CustomPose.msg 23 | CustomOcc.msg 24 | Frontiers.msg 25 | rendezvous.msg 26 | MockPackage.msg 27 | ) 28 | 29 | generate_messages( 30 | DEPENDENCIES 31 | std_msgs 32 | sensor_msgs 33 | nav_msgs 34 | geometry_msgs 35 | ) 36 | 37 | ################################### 38 | ## catkin specific configuration ## 39 | ################################### 40 | catkin_package( 41 | CATKIN_DEPENDS roscpp 42 | rospy 43 | std_msgs 44 | sensor_msgs 45 | nav_msgs 46 | tf2_ros 47 | roslib 48 | message_generation 49 | message_runtime 50 | ) 51 | 52 | ########### 53 | ## Build ## 54 | ########### 55 | 56 | include_directories( 57 | include 58 | ${catkin_INCLUDE_DIRS} 59 | ${GAZEBO_INCLUDE_DIRS} 60 | ${OpenCV_INCLUDE_DIRS} 61 | ) 62 | 63 | set(CMAKE_CXX_STANDARD 17) 64 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 65 | set(CMAKE_CXX_EXTENSIONS OFF) -------------------------------------------------------------------------------- /src/multirobotsimulations/launch/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/launch/.gitkeep -------------------------------------------------------------------------------- /src/multirobotsimulations/launch/gazebo_add_robot.launch: -------------------------------------------------------------------------------- 1 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/multirobotsimulations/launch/gazebo_multi_robot_bringup.launch: -------------------------------------------------------------------------------- 1 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/msg/.gitkeep -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/CustomImage.msg: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint8 robot_id 18 | sensor_msgs/Image image -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/CustomOcc.msg: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint8 robot_id 18 | nav_msgs/OccupancyGrid occ -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/CustomPose.msg: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint8 robot_id 18 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/Frontiers.msg: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | geometry_msgs/PoseArray centroids 18 | std_msgs/Float64MultiArray costs 19 | std_msgs/Float64MultiArray values 20 | std_msgs/Float64MultiArray utilities 21 | 22 | uint8 highest_cost_index 23 | float32 highest_cost 24 | 25 | uint8 lowest_cost_index 26 | float32 lowest_cost 27 | 28 | uint8 highest_value_index 29 | float32 highest_value 30 | 31 | uint8 lowest_value_index 32 | float32 lowest_value 33 | 34 | uint8 highest_utility_index 35 | float32 highest_utility 36 | 37 | uint8 lowest_utility_index 38 | float32 lowest_utility -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/MockPackage.msg: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint8 sender 18 | uint8 destination 19 | nav_msgs/OccupancyGrid occ 20 | float32 average_velocity 21 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /src/multirobotsimulations/msg/rendezvous.msg: -------------------------------------------------------------------------------- 1 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 2 | # Copyright (C) 2023 Alysson Ribeiro da Silva 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint8 robot_id 18 | uint8 plan -------------------------------------------------------------------------------- /src/multirobotsimulations/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multirobotsimulations 4 | 0.0.1 5 | The Multirobot Simulations Package 6 | Alysson Ribeiro da Silva 7 | GPLv3 8 | http://alysson.thegeneralsolution.com 9 | Alysson Ribeiro da Silva 10 | 11 | 12 | catkin 13 | 14 | 16 | roscpp 17 | rospy 18 | std_msgs 19 | sensor_msgs 20 | nav_msgs 21 | tf2_ros 22 | roslib 23 | message_generation 24 | message_runtime 25 | 26 | -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/.gitkeep -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/Coordinates: -------------------------------------------------------------------------------- 1 | =================================== 2 | Pioneer3AT Model Locations (x y z) 3 | =================================== 4 | 5 | Chassis : 0 0 0.177 6 | Top : 0.003 0 0.274 7 | 8 | Front Sonar : 0.193 0 0.25 9 | Back Sonar : -0.187 0 0.247 10 | 11 | FR Axle : 0.135 -0.156 0.111 12 | FR Hub : 0.135 -0.199 0.111 13 | FR wheel : 0.135 -0.199 0.111 14 | 15 | FL Axle : 0.135 0.156 0.111 16 | FL Hub : 0.135 0.199 0.111 17 | FL wheel : 0.135 0.199 0.111 18 | 19 | BR Axle : -0.134 -0.156 0.111 20 | BR Hub : -0.134 -0.199 0.111 21 | BR Wheel : -0.134 -0.199 0.111 22 | 23 | BL Axle : -0.134 0.156 0.111 24 | BL Hub : -0.134 0.199 0.111 25 | BL Wheel : -0.134 0.199 0.111 26 | -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/axle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/axle.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/back_sonar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/back_sonar.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/chassis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/chassis.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/front_sonar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/front_sonar.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/left_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/left_hubcap.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/right_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/right_hubcap.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/top.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/robots/pioneer3at/meshes/wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/multirobotsimulations/robots/pioneer3at/meshes/wheel.stl -------------------------------------------------------------------------------- /src/multirobotsimulations/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | pkg_config = generate_distutils_setup( 6 | packages=['multirobotsimulations'], 7 | scripts=[], 8 | package_dir={'': 'scripts'} 9 | ) 10 | 11 | setup(**pkg_config) -------------------------------------------------------------------------------- /src/scripts/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/multirobotplayground/Noetic-Multi-Robot-Sandbox/2d01225b533a2a75f072f26e44bd40fc30c2cf7b/src/scripts/.gitkeep -------------------------------------------------------------------------------- /src/scripts/launch_robots.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 4 | # Copyright (C) 2023 Alysson Ribeiro da Silva 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU General Public License 17 | # along with this program. If not, see . 18 | 19 | # run each stack part in a separate session 20 | tmux new-window -n robot_0_mre_stack -t simulation -d "roslaunch multirobotexploration exploration_stack_bringup.launch robot_id:=0 robot_count:=3" 21 | tmux new-window -n robot_0_gmapping -t simulation -d "roslaunch multirobotexploration gmapping.launch robot_id:=0" 22 | 23 | tmux new-window -n robot_1_mre_stack -t simulation -d "roslaunch multirobotexploration exploration_stack_bringup.launch robot_id:=1 robot_count:=3" 24 | tmux new-window -n robot_1_gmapping -t simulation -d "roslaunch multirobotexploration gmapping.launch robot_id:=1" 25 | 26 | tmux new-window -n robot_2_mre_stack -t simulation -d "roslaunch multirobotexploration exploration_stack_bringup.launch robot_id:=2 robot_count:=3" 27 | tmux new-window -n robot_2_gmapping -t simulation -d "roslaunch multirobotexploration gmapping.launch robot_id:=2" -------------------------------------------------------------------------------- /src/scripts/run_simulation.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 4 | # Copyright (C) 2023 Alysson Ribeiro da Silva 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU General Public License 17 | # along with this program. If not, see . 18 | 19 | # create session with robot_$1 name and in detached mode 20 | # tmux new-session -n global_terminal -s motherbase -d "/bin/bash" 21 | tmux new-session -n session_manager -s simulation -d "roslaunch multirobotsimulations gazebo_multi_robot_bringup.launch world_name:=forest.world paused:=true" 22 | 23 | # create new gnome-terminal and attach it to this session 24 | # gnome-terminal -t motherbase_terminal --geometry=100x100 --hide-menubar -- tmux attach-session -t motherbase 25 | gnome-terminal -t simulation_terminal --geometry=150x20 --hide-menubar -- tmux attach-session -t simulation -------------------------------------------------------------------------------- /src/scripts/start_exploration.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 4 | # Copyright (C) 2023 Alysson Ribeiro da Silva 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU General Public License 17 | # along with this program. If not, see . 18 | 19 | rostopic pub -1 /global_explorer/set_exploring std_msgs/String "data: ''" -------------------------------------------------------------------------------- /src/scripts/stop_forced.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Noetic-Multi-Robot-Sandbox for multi-robot research using ROS Noetic 4 | # Copyright (C) 2023 Alysson Ribeiro da Silva 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU General Public License 17 | # along with this program. If not, see . 18 | 19 | clear 20 | pkill -f rviz 21 | pkill -f gazebo 22 | pkill -f robot_state_publisher 23 | pkill -f gzclient 24 | pkill -f rosout 25 | pkill -f roslaunch 26 | pkill -f rosmaster 27 | pkill -f gzserver 28 | pkill -f node_manager_da 29 | pkill -f master_discover 30 | pkill -f master_sync 31 | pkill -f map_merge 32 | pkill -f node_manager 33 | pkill -f scan_to_scan_fi 34 | pkill -f frontier 35 | pkill -f gmappingpose 36 | pkill -f slam_gmapping 37 | pkill -f static_transfor 38 | pkill -f desaicontroller 39 | pkill -f lasertoworld 40 | pkill -f explorer 41 | pkill -f lasertoworld 42 | pkill -f joy_node 43 | pkill -f teleop_node 44 | pkill -f potentialfieldl 45 | pkill -f celldecompositi 46 | pkill -f frontierdiscove 47 | pkill -f configurationsp 48 | pkill -f ekf_localizatio 49 | pkill -f amcl 50 | pkill -f rqt_tf_tree 51 | pkill -f subgoalnav 52 | pkill -f screen 53 | pkill -f node_manager_da 54 | pkill -f rendezvous 55 | pkill -f frontierfromocc 56 | pkill -f relativedirecti 57 | pkill -f occimageconvert 58 | pkill -f PureStrategySyn 59 | pkill -f StatisticsProfi 60 | pkill -f FBMapFusion.py 61 | pkill -f FusionPose.py 62 | pkill -f dynamiccspace 63 | pkill -f lasernoiser 64 | pkill -f fbmapfusion 65 | pkill -f static_transfor 66 | pkill -f mapproc 67 | pkill -f fbmapfusionnew 68 | pkill -f explorerzones 69 | pkill -f globalmetrics 70 | pkill -f fbmapfusionrela 71 | pkill -f relativeposeest 72 | pkill -f nodelet 73 | pkill -f disparity_view 74 | pkill -f stereo_image_pr 75 | pkill -f tsdf_server 76 | pkill -f kworker 77 | pkill -f esdf_server 78 | pkill -f explorerrendezv 79 | pkill -f pointcloud_to_l 80 | pkill -f mrelocalplanner 81 | pkill -f localdynamicmap 82 | pkill -f cspace 83 | pkill -f python3 84 | pkill -f mockcommunicati 85 | pkill -f standalone_conv 86 | pkill -f averagedisplace 87 | pkill -f alysson2024 88 | pkill -f yamauchi1999 --------------------------------------------------------------------------------