├── .github └── workflows │ └── main.yaml ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── include └── mh_amcl │ ├── Correcter.hpp │ ├── LaserCorrecter.hpp │ ├── MH_AMCL.hpp │ ├── MapMatcher.hpp │ ├── MapMatcherBase.hpp │ ├── ParticlesDistribution.hpp │ ├── PointCloudCorrecter.hpp │ ├── Types.hpp │ └── Utils.hpp ├── launch ├── bringup_launch.py ├── kobuki_launch.py ├── localization_launch.py ├── navigation_launch.py ├── summit_launch.py ├── tb3_simulation_launch.py ├── tiago_launch.py └── tiago_real_exp_mh3d_launch.py ├── maps ├── home.pgm ├── home.yaml ├── home_ele.pgm ├── home_octo.ot ├── house_uneven.pgm ├── house_uneven.yaml ├── house_uneven_ele.pgm ├── house_uneven_octo.ot ├── lab.pgm ├── lab.yaml ├── lab_3d.pgm ├── lab_3d.yaml ├── lab_3d_ele.pgm ├── lab_3d_octo.ot ├── simple_uneven_house.pgm ├── simple_uneven_house.yaml ├── simple_uneven_house_ele.pgm ├── simple_uneven_house_octo.ot ├── turtlebot3_world.pgm ├── turtlebot3_world.yaml ├── uneven_floor.pgm ├── uneven_floor.yaml ├── uneven_floor_ele.pgm └── uneven_floor_octo.ot ├── package.xml ├── params ├── nav2_params_kobuki.yaml ├── nav2_params_summit.yaml ├── nav2_params_summit_lab.yaml ├── nav2_params_tb3_sim.yaml ├── nav2_params_tiago.yaml └── nav2_params_tiago_real_exp_mh3d.yaml ├── src ├── mh_amcl │ ├── LaserCorrecter.cpp │ ├── MH_AMCL.cpp │ ├── MapMatcher.cpp │ ├── MapMatcherBase.cpp │ ├── ParticlesDistribution.cpp │ ├── PointCloudCorrecter.cpp │ └── Utils.cpp └── mh_amcl_program.cpp └── tests ├── CMakeLists.txt ├── rosbag2_2022_09_01-11_42_10 ├── metadata.yaml └── rosbag2_2022_09_01-11_42_10_0.db3 ├── test_map_matcher.cpp ├── test_mh_amcl.cpp └── test_particles.cpp /.github/workflows/main.yaml: -------------------------------------------------------------------------------- 1 | name: main 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - main 7 | push: 8 | branches: 9 | - main 10 | 11 | jobs: 12 | build-and-test: 13 | runs-on: ${{ matrix.os }} 14 | strategy: 15 | matrix: 16 | os: [ubuntu-22.04] 17 | fail-fast: false 18 | steps: 19 | - name: Setup ROS 2 20 | uses: ros-tooling/setup-ros@0.3.4 21 | with: 22 | required-ros-distributions: humble 23 | - name: build and test 24 | uses: ros-tooling/action-ros-ci@0.2.6 25 | with: 26 | package-name: mh_amcl 27 | target-ros2-distro: humble 28 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mh_amcl) 3 | 4 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | 6 | find_package(ament_cmake REQUIRED) 7 | find_package(rclcpp) 8 | find_package(rclcpp_lifecycle) 9 | find_package(nav2_costmap_2d) 10 | find_package(sensor_msgs) 11 | find_package(visualization_msgs) 12 | find_package(tf2_geometry_msgs) 13 | find_package(geometry_msgs) 14 | find_package(tf2_ros) 15 | find_package(tf2) 16 | find_package(nav2_util) 17 | find_package(nav2_msgs) 18 | find_package(grid_map_msgs) 19 | find_package(grid_map_ros) 20 | find_package(octomap_msgs) 21 | find_package(octomap_ros) 22 | find_package(pcl_conversions REQUIRED) 23 | find_package(pcl_ros REQUIRED) 24 | find_package(mh_amcl_msgs) 25 | find_package(builtin_interfaces REQUIRED) 26 | 27 | set(dependencies 28 | rclcpp 29 | rclcpp_lifecycle 30 | nav2_costmap_2d 31 | sensor_msgs 32 | visualization_msgs 33 | tf2_geometry_msgs 34 | geometry_msgs 35 | tf2_ros 36 | tf2 37 | nav2_util 38 | nav2_msgs 39 | grid_map_msgs 40 | grid_map_ros 41 | octomap_msgs 42 | octomap_ros 43 | pcl_conversions 44 | pcl_ros 45 | mh_amcl_msgs 46 | builtin_interfaces 47 | ) 48 | 49 | include_directories( 50 | include 51 | ) 52 | 53 | add_library(${PROJECT_NAME} 54 | src/${PROJECT_NAME}/MH_AMCL.cpp 55 | src/${PROJECT_NAME}/MapMatcherBase.cpp 56 | src/${PROJECT_NAME}/MapMatcher.cpp 57 | src/${PROJECT_NAME}/ParticlesDistribution.cpp 58 | src/${PROJECT_NAME}/LaserCorrecter.cpp 59 | src/${PROJECT_NAME}/PointCloudCorrecter.cpp 60 | src/${PROJECT_NAME}/Utils.cpp 61 | ) 62 | ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 63 | 64 | add_executable(mh_amcl_program 65 | src/mh_amcl_program.cpp 66 | ) 67 | ament_target_dependencies(mh_amcl_program ${dependencies}) 68 | target_link_libraries(mh_amcl_program ${PROJECT_NAME}) 69 | 70 | install(TARGETS ${PROJECT_NAME} 71 | ARCHIVE DESTINATION lib 72 | LIBRARY DESTINATION lib 73 | RUNTIME DESTINATION bin 74 | ) 75 | 76 | install(TARGETS mh_amcl_program 77 | RUNTIME DESTINATION lib/${PROJECT_NAME} 78 | ) 79 | 80 | install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) 81 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 82 | install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) 83 | 84 | if(BUILD_TESTING) 85 | find_package(ament_lint_auto REQUIRED) 86 | ament_lint_auto_find_test_dependencies() 87 | 88 | find_package(ament_cmake_gtest REQUIRED) 89 | add_subdirectory(tests) 90 | endif() 91 | 92 | ament_export_include_directories(include) 93 | ament_export_libraries(${PROJECT_NAME}) 94 | ament_export_dependencies(${dependencies}) 95 | ament_package() 96 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contribution Guidelines 2 | 3 | As an open-source project, we welcome and encourage the community to submit patches directly to the Multi-Hypotesis AMCL(MH-AMCL). In our collaborative open source environment, standards and methods for submitting changes help reduce the chaos that can result from an active development community. 4 | 5 | This document explains how to participate in project conversations, log and track bugs and enhancement requests, and submit patches to the project so your patch will be accepted quickly in the codebase. 6 | 7 | Licensing 8 | Licensing is very important to open source projects. It helps ensure the software continues to be available under the terms that the author desired. 9 | 10 | Any contribution that you make to this repository will 11 | be under the Apache 2 License, as dictated by that 12 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 13 | 14 | ~~~ 15 | 5. Submission of Contributions. Unless You explicitly state otherwise, 16 | any Contribution intentionally submitted for inclusion in the Work 17 | by You to the Licensor shall be under the terms and conditions of 18 | this License, without any additional terms or conditions. 19 | Notwithstanding the above, nothing herein shall supersede or modify 20 | the terms of any separate license agreement you may have executed 21 | with Licensor regarding such Contributions. 22 | ~~~ 23 | 24 | A license tells you what rights you have as a developer, as provided by the copyright holder. It is important that the contributor fully understands the licensing rights and agrees to them. Sometimes the copyright holder isn’t the contributor, such as when the contributor is doing work on behalf of a company. 25 | 26 | ## Developer Certification of Origin (DCO) 27 | 28 | To make a good faith effort to ensure licensing criteria are met,MH-AMCL requires the Developer Certificate of Origin (DCO) process to be followed. 29 | 30 | The DCO is an attestation attached to every contribution made by every developer. In the commit message of the contribution, (described more fully later in this document), the developer simply adds a `Signed-off-by` statement and thereby agrees to the DCO. 31 | 32 | When a developer submits a patch, it is a commitment that the contributor has the right to submit the patch per the license. The DCO agreement is shown below and at [http://developercertificate.org/](http://developercertificate.org/). 33 | 34 | ``` txt 35 | Developer's Certificate of Origin 1.1 36 | 37 | By making a contribution to this project, I certify that: 38 | 39 | (a) The contribution was created in whole or in part by me and I 40 | have the right to submit it under the open source license 41 | indicated in the file; or 42 | 43 | (b) The contribution is based upon previous work that, to the 44 | best of my knowledge, is covered under an appropriate open 45 | source license and I have the right under that license to 46 | submit that work with modifications, whether created in whole 47 | or in part by me, under the same open source license (unless 48 | I am permitted to submit under a different license), as 49 | Indicated in the file; or 50 | 51 | (c) The contribution was provided directly to me by some other 52 | person who certified (a), (b) or (c) and I have not modified 53 | it. 54 | 55 | (d) I understand and agree that this project and the contribution 56 | are public and that a record of the contribution (including 57 | all personal information I submit with it, including my 58 | sign-off) is maintained indefinitely and may be redistributed 59 | consistent with this project or the open source license(s) 60 | involved. 61 | ``` 62 | 63 | ## DCO Sign-Off Methods 64 | 65 | The DCO requires that a sign-off message, in the following format, appears on each commit in the pull request: 66 | 67 | ``` txt 68 | Signed-off-by: Sofforus Jones 69 | ``` 70 | 71 | The DCO text can either be manually added to your commit body, or you can add either `-s` or `--signoff` to your usual Git commit commands. If you forget to add the sign-off you can also amend a previous commit with the sign-off by running `git commit --amend -s`. If you’ve pushed your changes to GitHub already you’ll need to force push your branch after this with `git push -f`. 72 | 73 | Note: The name and email address of the account you use to submit your PR must match the name and email address on the Signed-off-by line in your commit message. 74 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![main](https://github.com/navigation-gridmap/mh_amcl/actions/workflows/main.yaml/badge.svg)](https://github.com/navigation-gridmap/mh_amcl/actions/workflows/main.yaml) 2 | 3 | # Multi-Hypothesis AMCL (MH-AMCL) in non-planar environments 4 | 5 | [![](https://img.youtube.com/vi/GkCaiirI8f8/0.jpg)](https://www.youtube.com/watch?v=GkCaiirI8f8&feature=youtu.be "Click to play on You Tube") 6 | [![](https://img.youtube.com/vi/bIf0aU-H2yg/0.jpg)](https://www.youtube.com/watch?v=bIf0aU-H2yg&feature=youtu.be "Click to play on You Tube") 7 | 8 | MH-AMCL is a fully functional localization algorithm implementation with [Nav2](https://navigation.ros.org/). The main feature is that it maintains several hypotheses about the robot's position. The algorithm periodically generates new hypotheses on positions where the robot could be, based on the latest laser and map reading. This allows: 9 | * Total unknown the position of the robot. 10 | * Recover from erroneous estimates and hijacks. 11 | 12 | **The version in this repo is for non-planar environments, using the [Extended Map Server](https://github.com/navigation-gridmap/extended_map_server)** 13 | 14 | ## Build 15 | 16 | This package requires the Rolling distribution, as it is in sync with Nav2, whose `main` branch uses Rolling. 17 | 18 | Just clone this repo in the workspace and build as usual: 19 | 20 | ``` 21 | colcon build --symlink-install 22 | ``` 23 | 24 | ## Run 25 | 26 | Coomin Soon 27 | 28 | ## Details 29 | 30 | ### Suscribed Topics: 31 | * `grid_map_map` (`grid_map_msgs/msg/GridMap`): The environmen gridmap containig elevation and occupancy layers. 32 | * `octomap_map` (`octomap_msgs/msg/Octomap`): The environmen octomap. 33 | * `initialpose` (`geometry_msgs/msg/PoseWithCovarianceStamped`): Used for reset the robot's position from Rviz2. 34 | 35 | * `sensor_msgs/msg/LaserScan` and `sensor_msgs/msg/PointCloud2` depending on the configuration 36 | 37 | ### Published Topics: 38 | * `amcl_pose` (`geometry_msgs::msg::PoseWithCovarianceStamped`): The robot's pose with the covariance associated from the best hypothesis. 39 | * `particle_cloud` (`nav2_msgs::msg::ParticleCloud`): The particles from the best hypothesis. 40 | * `poses` (`visualization_msgs::msg::MarkerArray`): All the particles from all the hypotheses, each one with a different color. 41 | 42 | ### Parameters: 43 | * `use_sim_time` (bool, False): Use the robot's clock or the one coming from the `/clock` topic. 44 | * `max_particles` (int, 200): The maximum number of particles for each hypothesis. 45 | * `min_particles` (int, 200): The minimum number of particles for each hypothesis. 46 | * `particles_step` (int, 30): Particles' variation increases `particles_step` when the estimation is bad and decreases when it is good. 47 | * `init_pos_x` (double): The initial X position of the robot, if known. 48 | * `init_pos_y` (double): The initial Y position of the robot, if known. 49 | * `init_pos_yaw` (double): The initial Yaw position of the robot, if known. 50 | * `init_error_x` (double): The initial X uncertainty of the robot. 51 | * `init_error_y` (double): The initial Y uncertainty of the robot. 52 | * `init_error_yaw` (double): The initial Yaw uncertainty of the robot. 53 | * `translation_noise` (double, 10%): The error percentage coming from the translation component. 54 | * `rotation_noise` (double, 10%): The error percentage from the rotational component. 55 | * `rotation_noise` (double, 10%): The error percentage from the rotational component. 56 | * `distance_perception_error` (double, 0.01): The error in meters of the sensor when reading distances. 57 | * `reseed_percentage_losers` (double, 90%): The percentage of particles to be replaced when reseeding. 58 | * `reseed_percentage_winners` (double, 3%): The percentage of particles that generate new particles when reseeding. 59 | * `multihypothesis` (bool, true): Use multiples hypothesis, or only one - the created initially. 60 | * `max_hypotheses` (int, 5): Maximum number of concurrent hypotheses. 61 | * `min_candidate_weight` (float, 0.5): Minimum quality of a candidate to be considered for a new hypothesis. 62 | * `min_candidate_distance` (double, 1.0): Minimum distance to an existing hypothesis to be considered for a new hypothesis. 63 | * `min_candidate_angle` (double, PI/2): Minimum angle to an existing hypothesis to be considered for a new hypothesis. 64 | * `low_q_hypo_thereshold` (float, 0.25): Under this threshold, a hypothesis is considered low quality and should be removed if there is a better candidate. 65 | * `very_low_q_hypo_thereshold` (float, 0.10): A hypothesis is considered very low quality and should be removed under this threshold. 66 | * `hypo_merge_distance` (double, 0.3): Distance under consideration to merge two hypotesese (angle and distance shpuld meet). 67 | * `hypo_merge_angle` (double, 0.5): Angle to consider merging two hypotheses (angle and distance should meet). 68 | * `good_hypo_thereshold` (double, 0.5): Threshold to consider a hypothesis to be selected as the newly selected hypothesis for the algorithm's output. 69 | * `min_hypo_diff_winner` (double, 0.3): An hypothesis should have a quality `min_hypo_diff_winner` better than the currently selected hypothesis to be the newly selected hypothesis. Low values could lead to continuing changes between the two hypotheses. High values could make it impossible to consider other hypotheses. 70 | * `matchers` (String[], ""): List of matchers, which provides hypothesis of other prabable robot positions. 71 | * `type` (String): type of matcher. 72 | * For type == `matcher2d`: 73 | * `topic` (String): Topic of the perception source used 74 | * `correction_sources` (String[], ""): List of perception sources. 75 | * `type` (String): Type of the corrector in [`laser`, `pointcloud`] 76 | * For type == `laser`: 77 | * `topic` (String, `/perception`: Topic of the perception source 78 | * `distance_perception_error` (double, 0.05): Mean of the expected perception error. 79 | * For type == `pointcloud`: 80 | * `topic` (String, `/perception`: Topic of the perception source 81 | * `distance_perception_error` (double, 0.05): Mean of the expected perception error. 82 | * `max_perception_distance` (double, 10.0): Maximun range of the sensor. 83 | * `point_step` (double, 1): The point step for a sensor reading. 1 means that all the readings will be used. 50 means that only one point out of 50 is used. This is used to reduce the CPU load. 84 | 85 | Example: 86 | 87 | ``` 88 | mh_amcl: 89 | ros__parameters: 90 | use_sim_time: True 91 | max_particles: 200 92 | min_particles: 30 93 | particles_step: 30 94 | init_pos_x: 0.0 95 | init_pos_y: 0.0 96 | init_pos_z: 0.3 97 | init_pos_yaw: 0.0 98 | init_pos_pitch: 0.0 99 | init_pos_roll: 0.0 100 | init_error_x: 0.1 101 | init_error_y: 0.1 102 | init_error_z: 0.1 103 | init_error_yaw: 0.05 104 | init_error_pitch: 0.01 105 | init_error_roll: 0.01 106 | translation_noise: 0.1 107 | rotation_noise: 0.1 108 | reseed_percentage_losers: 0.9 109 | reseed_percentage_winners: 0.03 110 | multihypothesis: False 111 | max_hypotheses: 5 112 | min_candidate_weight: 0.5 113 | min_candidate_distance: 1.0 114 | min_candidate_angle: 1.57 115 | low_q_hypo_thereshold: 0.25 116 | very_low_q_hypo_thereshold: 0.10 117 | hypo_merge_distance: 0.3 118 | hypo_merge_angle: 0.5 119 | good_hypo_thereshold: 0.5 120 | min_hypo_diff_winner: 0.3 121 | matchers: ['matcher_0'] 122 | matcher_0: 123 | type: matcher2d 124 | topic: /front_laser/scan 125 | correction_sources: ['front_laser'] 126 | front_laser: 127 | type: laser 128 | topic: /front_laser/scan 129 | distance_perception_error: 0.05 130 | debug: true 131 | rear_laser: 132 | type: laser 133 | topic: /rear_laser/scan 134 | distance_perception_error: 0.05 135 | pc_0: 136 | type: pointcloud 137 | topic: /velodyne_points 138 | distance_perception_error: 0.1 139 | max_perception_distance: 10.0 140 | point_step: 50 141 | ``` 142 | 143 | ## Benchmarking 144 | 145 | The bottleneck is in the octomap function RayTrace. Build octomap with (OpenMPI)[https://www.open-mpi.org/] support to improve it. 146 | 147 | ![Captura desde 2023-08-26 08-42-04](https://github.com/navigation-gridmap/mh_amcl/assets/3810011/de4cb8c0-b778-485a-85c4-ba59f2a708a9) 148 | 149 | 150 | ## Citing 151 | 152 | ``` 153 | @article{https://doi.org/10.1002/rob.22353, 154 | author = {Rico, Francisco Martín and Hernández, José Miguel Guerrero and Pérez-Rodríguez, Rodrigo and Peña-Narvaez, Juan Diego and Gómez-Jacinto, Alberto García}, 155 | title = {Open source robot localization for nonplanar environments}, 156 | journal = {Journal of Field Robotics}, 157 | volume = {n/a}, 158 | number = {n/a}, 159 | pages = {}, 160 | keywords = {field robotics, localization, mobile robotics, navigation, ROS}, 161 | doi = {https://doi.org/10.1002/rob.22353}, 162 | url = {https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.22353}, 163 | eprint = {https://onlinelibrary.wiley.com/doi/pdf/10.1002/rob.22353}, 164 | } 165 | 166 | ``` 167 | 168 | ## Acknowledgment: SELF-AIR Project 169 | 170 | Supporting Extensive Livestock Farming with the use of Autonomous Intelligent Robots 171 | 172 | Grant TED2021-132356B-I00 funded by MCIN/AEI/10.13039/501100011033 and by the “European Union NextGenerationEU/PRTR" 173 | 174 | SELF_AIR_EU eu_logo 175 | -------------------------------------------------------------------------------- /include/mh_amcl/Correcter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef MH_AMCL__CORRECTER_HPP_ 17 | #define MH_AMCL__CORRECTER_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "mh_amcl/Types.hpp" 24 | 25 | #include "nav2_util/lifecycle_node.hpp" 26 | #include "rclcpp/rclcpp.hpp" 27 | 28 | namespace mh_amcl 29 | { 30 | 31 | using std::placeholders::_1; 32 | 33 | class CorrecterBase { 34 | public: 35 | virtual ~CorrecterBase() {}; 36 | std::string type_; 37 | virtual void clear_perception() = 0; 38 | }; 39 | 40 | template 41 | class Correcter : public CorrecterBase 42 | { 43 | public: 44 | Correcter(const std::string & name, nav2_util::LifecycleNode::SharedPtr node, typename std::shared_ptr & map) 45 | : name_(name), 46 | node_(node), 47 | map_(map), 48 | tf_buffer_(), 49 | tf_listener_(tf_buffer_) 50 | { 51 | std::string topic; 52 | if (!node->has_parameter(name + ".topic")) { 53 | node->declare_parameter(name + ".topic", "/perception"); 54 | } 55 | node->get_parameter(name + ".topic", topic); 56 | 57 | percept_sub_ = node->create_subscription( 58 | topic, rclcpp::QoS(100).best_effort(), std::bind(&Correcter::perception_callback, this, _1)); 59 | } 60 | 61 | void set_last_perception(T & last_perception) { 62 | if (last_perception_ == nullptr) { 63 | last_perception_ = std::make_unique(); 64 | } 65 | *last_perception_ = last_perception; 66 | } 67 | 68 | virtual void correct(std::vector & particles, rclcpp::Time & update_time) = 0; 69 | virtual void clear_perception() { 70 | last_perception_ = nullptr; 71 | } 72 | 73 | public: 74 | typename T::UniquePtr last_perception_; 75 | 76 | protected: 77 | const std::string name_; 78 | rclcpp_lifecycle::LifecycleNode::SharedPtr node_; 79 | 80 | typename std::shared_ptr & map_; 81 | typename rclcpp::Subscription::SharedPtr percept_sub_; 82 | 83 | tf2::BufferCore tf_buffer_; 84 | tf2_ros::TransformListener tf_listener_; 85 | 86 | void perception_callback(typename T::UniquePtr msg) { 87 | last_perception_ = std::move(msg); 88 | } 89 | }; 90 | 91 | } // namespace mh_amcl 92 | 93 | #endif // MH_AMCL__CORRECTER_HPP_ 94 | -------------------------------------------------------------------------------- /include/mh_amcl/LaserCorrecter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MH_AMCL__LASERCORRECTER_HPP_ 16 | #define MH_AMCL__LASERCORRECTER_HPP_ 17 | 18 | #include "octomap/octomap.h" 19 | 20 | #include "sensor_msgs/msg/laser_scan.hpp" 21 | 22 | #include "mh_amcl/Types.hpp" 23 | #include "mh_amcl/Correcter.hpp" 24 | 25 | #include "nav2_util/lifecycle_node.hpp" 26 | #include "rclcpp/rclcpp.hpp" 27 | 28 | 29 | namespace mh_amcl 30 | { 31 | 32 | class LaserCorrecter : public Correcter 33 | { 34 | public: 35 | LaserCorrecter( 36 | const std::string & name, nav2_util::LifecycleNode::SharedPtr node, 37 | std::shared_ptr & map); 38 | 39 | void correct(std::vector & particles, rclcpp::Time & update_time) override; 40 | 41 | protected: 42 | tf2::Transform get_tranform_to_read(const sensor_msgs::msg::LaserScan & scan, int index) const; 43 | double get_distance_to_obstacle( 44 | const tf2::Transform & map2bf, const tf2::Transform & bf2laser, 45 | const tf2::Vector3 unit_vector, const sensor_msgs::msg::LaserScan & scan, 46 | const octomap::OcTree & costmap) const; 47 | double get_occupancy( 48 | const tf2::Transform & transform, 49 | const octomap::OcTree & costmap) const; 50 | tf2::Vector3 get_perception_unit_vector(const sensor_msgs::msg::LaserScan & scan, int index) const; 51 | 52 | private: 53 | double distance_perception_error_; 54 | }; 55 | 56 | } // namespace mh_amcl 57 | 58 | #endif // MH_AMCL__LASERCORRECTER_HPP_ 59 | -------------------------------------------------------------------------------- /include/mh_amcl/MH_AMCL.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MH_AMCL__MH_AMCL_HPP_ 16 | #define MH_AMCL__MH_AMCL_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "tf2_ros/buffer.h" 27 | #include "tf2_ros/transform_listener.h" 28 | #include "tf2_ros/transform_broadcaster.h" 29 | #include "tf2/LinearMath/Transform.h" 30 | #include "tf2/transform_datatypes.h" 31 | 32 | #include "geometry_msgs/msg/pose_array.hpp" 33 | 34 | #include "sensor_msgs/msg/laser_scan.hpp" 35 | #include "nav_msgs/msg/occupancy_grid.hpp" 36 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 37 | #include "nav2_msgs/msg/particle_cloud.hpp" 38 | #include "mh_amcl_msgs/msg/info.hpp" 39 | 40 | #include "nav2_costmap_2d/costmap_2d.hpp" 41 | #include "mh_amcl/ParticlesDistribution.hpp" 42 | 43 | #include "grid_map_msgs/msg/grid_map.hpp" 44 | #include "grid_map_ros/grid_map_ros.hpp" 45 | 46 | #include "octomap_msgs/msg/octomap.hpp" 47 | #include "octomap_msgs/conversions.h" 48 | #include "octomap_ros/conversions.hpp" 49 | #include "octomap/octomap.h" 50 | 51 | #include "mh_amcl/MapMatcher.hpp" 52 | #include "mh_amcl/Correcter.hpp" 53 | 54 | #include "rclcpp/rclcpp.hpp" 55 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 56 | #include "nav2_util/lifecycle_node.hpp" 57 | 58 | namespace mh_amcl 59 | { 60 | 61 | class MH_AMCL_Node : public nav2_util::LifecycleNode 62 | { 63 | public: 64 | explicit MH_AMCL_Node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 65 | 66 | void init(); 67 | 68 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 69 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 70 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 71 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 72 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 73 | nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; 74 | 75 | protected: 76 | void predict(); 77 | void correct(); 78 | void reseed(); 79 | void publish_particles(); 80 | void publish_position(); 81 | void publish_position_tf(); 82 | void manage_hypotesis(); 83 | 84 | unsigned char get_cost(const geometry_msgs::msg::Pose & pose); 85 | geometry_msgs::msg::Pose toMsg(const tf2::Transform & tf); 86 | 87 | private: 88 | rclcpp::Subscription::SharedPtr sub_gridmap_; 89 | rclcpp::Subscription::SharedPtr sub_octomap_; 90 | 91 | rclcpp::Subscription::SharedPtr sub_init_pose_; 92 | rclcpp::Publisher::SharedPtr pose_pub_; 93 | rclcpp::Publisher::SharedPtr particles_pub_; 94 | rclcpp::Publisher::SharedPtr info_pub_; 95 | 96 | 97 | rclcpp::CallbackGroup::SharedPtr reentrant_1_cg_; 98 | rclcpp::CallbackGroup::SharedPtr other_cg_; 99 | std::mutex correct_mutex_; 100 | 101 | rclcpp::TimerBase::SharedPtr predict_timer_; 102 | rclcpp::TimerBase::SharedPtr correct_timer_; 103 | rclcpp::TimerBase::SharedPtr reseed_timer_; 104 | rclcpp::TimerBase::SharedPtr hypotesis_timer_; 105 | rclcpp::TimerBase::SharedPtr publish_particles_timer_; 106 | rclcpp::TimerBase::SharedPtr publish_position_timer_; 107 | rclcpp::TimerBase::SharedPtr publish_position_tf_timer_; 108 | 109 | int max_hypotheses_; 110 | bool multihypothesis_; 111 | float min_candidate_weight_; 112 | double min_candidate_distance_; 113 | double min_candidate_angle_; 114 | float low_q_hypo_thereshold_; 115 | float very_low_q_hypo_thereshold_; 116 | double hypo_merge_distance_; 117 | double hypo_merge_angle_; 118 | float good_hypo_thereshold_; 119 | float min_hypo_diff_winner_; 120 | 121 | rclcpp::Time last_time_; 122 | mh_amcl_msgs::msg::Info info_; 123 | 124 | std::list> particles_population_; 125 | std::shared_ptr current_amcl_; 126 | float current_amcl_q_; 127 | 128 | tf2::BufferCore tf_buffer_; 129 | tf2_ros::TransformListener tf_listener_; 130 | std::shared_ptr tf_broadcaster_; 131 | 132 | tf2::Stamped odom2prevbf_; 133 | bool valid_prev_odom2bf_ {false}; 134 | 135 | std::shared_ptr gridmap_; 136 | std::shared_ptr octomap_; 137 | 138 | std::list matchers_; 139 | std::list correcters_; 140 | 141 | void gridmap_callback(const grid_map_msgs::msg::GridMap::ConstSharedPtr & msg); 142 | void octomap_callback(const octomap_msgs::msg::Octomap::ConstSharedPtr & msg); 143 | 144 | void initpose_callback( 145 | const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & pose_msg); 146 | int counter_ {0}; 147 | }; 148 | 149 | } // namespace mh_amcl 150 | 151 | #endif // MH_AMCL__MH_AMCL_HPP_ 152 | -------------------------------------------------------------------------------- /include/mh_amcl/MapMatcher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MH_AMCL__MAPMATCHER_HPP_ 16 | #define MH_AMCL__MAPMATCHER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "tf2/transform_datatypes.h" 23 | #include "tf2/LinearMath/Transform.h" 24 | 25 | #include "sensor_msgs/msg/laser_scan.hpp" 26 | #include "nav_msgs/msg/occupancy_grid.hpp" 27 | 28 | #include "nav2_costmap_2d/costmap_2d.hpp" 29 | 30 | #include "mh_amcl/MapMatcherBase.hpp" 31 | 32 | #include "rclcpp/rclcpp.hpp" 33 | 34 | 35 | namespace mh_amcl 36 | { 37 | 38 | class MapMatcher : public MapMatcherBase 39 | { 40 | public: 41 | explicit MapMatcher( 42 | const std::string & name, 43 | std::shared_ptr parent_node); 44 | std::list get_matchs(); 45 | 46 | protected: 47 | static const int NUM_LEVEL_SCALE_COSTMAP = 4; 48 | 49 | std::shared_ptr 50 | half_scale(std::shared_ptr costmap_in); 51 | std::vector laser2points(const sensor_msgs::msg::LaserScan & scan); 52 | std::list get_matchs( 53 | int scale, const std::vector & scan, 54 | float min_x, float min_y, float max_y, float max_x); 55 | float match( 56 | int scale, const nav2_costmap_2d::Costmap2D & costmap, 57 | const std::vector & scan, tf2::Transform & transform); 58 | 59 | std::vector> costmaps_; 60 | std::shared_ptr parent_node_; 61 | 62 | void laser_callback(sensor_msgs::msg::LaserScan::UniquePtr msg); 63 | void map_callback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & msg); 64 | 65 | rclcpp::Subscription::SharedPtr laser_sub_; 66 | rclcpp::Subscription::SharedPtr sub_map_; 67 | 68 | sensor_msgs::msg::LaserScan::UniquePtr last_perception_; 69 | std::shared_ptr costmap_; 70 | }; 71 | 72 | nav_msgs::msg::OccupancyGrid toMsg(const nav2_costmap_2d::Costmap2D & costmap); 73 | 74 | } // namespace mh_amcl 75 | 76 | #endif // MH_AMCL__MAPMATCHER_HPP_ 77 | -------------------------------------------------------------------------------- /include/mh_amcl/MapMatcherBase.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef MH_AMCL__MAPMATCHERBASE_HPP_ 17 | #define MH_AMCL__MAPMATCHERBASE_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "mh_amcl/Types.hpp" 24 | 25 | #include "nav2_util/lifecycle_node.hpp" 26 | #include "rclcpp/rclcpp.hpp" 27 | 28 | namespace mh_amcl 29 | { 30 | 31 | typedef struct 32 | { 33 | float weight; 34 | tf2::Transform transform; 35 | } TransformWeighted; 36 | 37 | bool operator<(const TransformWeighted & tw1, const TransformWeighted & tw2); 38 | 39 | using std::placeholders::_1; 40 | 41 | class MapMatcherBase { 42 | public: 43 | virtual ~MapMatcherBase() {}; 44 | 45 | virtual std::list get_matchs() = 0; 46 | 47 | std::string type_; 48 | }; 49 | 50 | } // namespace mh_amcl 51 | 52 | #endif // MH_AMCL__MAPMATCHERBASE_HPP_ 53 | -------------------------------------------------------------------------------- /include/mh_amcl/ParticlesDistribution.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef MH_AMCL__PARTICLESDISTRIBUTION_HPP_ 17 | #define MH_AMCL__PARTICLESDISTRIBUTION_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include "sensor_msgs/msg/laser_scan.hpp" 27 | 28 | #include "geometry_msgs/msg/transform_stamped.hpp" 29 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 30 | #include "std_msgs/msg/color_rgba.hpp" 31 | #include "visualization_msgs/msg/marker_array.hpp" 32 | 33 | #include "mh_amcl_msgs/msg/hypo_info.hpp" 34 | 35 | #include "grid_map_msgs/msg/grid_map.hpp" 36 | #include "grid_map_ros/grid_map_ros.hpp" 37 | 38 | #include "mh_amcl/Types.hpp" 39 | #include "mh_amcl/Correcter.hpp" 40 | 41 | #include "rclcpp/rclcpp.hpp" 42 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 43 | 44 | namespace mh_amcl 45 | { 46 | 47 | std_msgs::msg::ColorRGBA 48 | getColor(Color color_id, double alpha = 1.0); 49 | 50 | class ParticlesDistribution 51 | { 52 | public: 53 | explicit ParticlesDistribution(rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node); 54 | 55 | void init(const tf2::Transform & pose_init); 56 | void predict(const tf2::Transform & movement, std::shared_ptr gridmap); 57 | void correct_once(const std::list & correcters, rclcpp::Time & update_time); 58 | void reseed(); 59 | const std::vector & get_particles() const {return particles_;} 60 | 61 | using CallbackReturnT = 62 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 63 | 64 | CallbackReturnT on_configure(const rclcpp_lifecycle::State & state); 65 | CallbackReturnT on_activate(const rclcpp_lifecycle::State & state); 66 | CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state); 67 | CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state); 68 | 69 | void publish_particles(int base_idx, const std_msgs::msg::ColorRGBA & color) const; 70 | tf2::WithCovarianceStamped get_pose() const {return pose_;} 71 | float get_quality() {return quality_;} 72 | void merge(ParticlesDistribution & other); 73 | 74 | const mh_amcl_msgs::msg::HypoInfo & get_info() const {return info_;} 75 | 76 | protected: 77 | rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node_; 78 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr 79 | pub_particles_; 80 | 81 | tf2::Transform add_noise(const tf2::Transform & dm); 82 | 83 | void normalize(); 84 | double normalize_angle(double angle); 85 | void update_pose(tf2::WithCovarianceStamped & pose); 86 | void update_covariance(tf2::WithCovarianceStamped & pose); 87 | 88 | tf2::WithCovarianceStamped pose_; 89 | 90 | std::random_device rd_; 91 | std::mt19937 generator_; 92 | 93 | std::vector particles_; 94 | float quality_; 95 | 96 | tf2::BufferCore tf_buffer_; 97 | tf2_ros::TransformListener tf_listener_; 98 | 99 | tf2::Stamped bf2laser_; 100 | bool bf2laser_init_ {false}; 101 | 102 | // AMCL Parameters 103 | int max_particles_; 104 | int min_particles_; 105 | double init_pos_x_; 106 | double init_pos_y_; 107 | double init_pos_z_; 108 | double init_pos_yaw_; 109 | double init_pos_pitch_; 110 | double init_pos_roll_; 111 | double init_error_x_; 112 | double init_error_y_; 113 | double init_error_z_; 114 | double init_error_yaw_; 115 | double init_error_pitch_; 116 | double init_error_roll_; 117 | double translation_noise_; 118 | double rotation_noise_; 119 | double reseed_percentage_losers_; 120 | double reseed_percentage_winners_; 121 | float good_hypo_thereshold_; 122 | float low_q_hypo_thereshold_; 123 | int particles_step_; 124 | 125 | // Experiments 126 | mh_amcl_msgs::msg::HypoInfo info_; 127 | }; 128 | 129 | double weighted_mean(const std::vector & v, const std::vector & w); 130 | double angle_weighted_mean(const std::vector & v, const std::vector & w); 131 | double mean(const std::vector & v); 132 | double angle_mean(const std::vector & v); 133 | double covariance( 134 | const std::vector & v1, const std::vector & v2, 135 | bool v1_is_angle = false, bool v2_is_angle = false); 136 | 137 | } // namespace mh_amcl 138 | 139 | #endif // MH_AMCL__PARTICLESDISTRIBUTION_HPP_ 140 | -------------------------------------------------------------------------------- /include/mh_amcl/PointCloudCorrecter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MH_AMCL__POINTCLOUDCORRECTER_HPP_ 16 | #define MH_AMCL__POINTCLOUDCORRECTER_HPP_ 17 | 18 | #include "octomap/octomap.h" 19 | 20 | #include "sensor_msgs/msg/point_cloud2.hpp" 21 | 22 | #include "mh_amcl/Types.hpp" 23 | #include "mh_amcl/Correcter.hpp" 24 | 25 | #include "pcl/point_types.h" 26 | #include "pcl_conversions/pcl_conversions.h" 27 | #include "pcl/point_types_conversion.h" 28 | 29 | #include "nav2_util/lifecycle_node.hpp" 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | 33 | namespace mh_amcl 34 | { 35 | 36 | class PointCloudCorrecter : public Correcter 37 | { 38 | public: 39 | PointCloudCorrecter( 40 | const std::string & name, nav2_util::LifecycleNode::SharedPtr node, 41 | std::shared_ptr & map); 42 | 43 | void correct(std::vector & particles, rclcpp::Time & update_time) override; 44 | 45 | protected: 46 | tf2::Transform get_tranform_to_read(const sensor_msgs::msg::PointCloud2 & scan, int index) const; 47 | double get_distance_to_obstacle( 48 | const tf2::Transform & map2bf, const tf2::Transform & bf2laser, 49 | const tf2::Vector3 unit_vector, const sensor_msgs::msg::PointCloud2 & scan, 50 | const octomap::OcTree & costmap) const; 51 | double get_occupancy( 52 | const tf2::Transform & transform, 53 | const octomap::OcTree & costmap) const; 54 | 55 | private: 56 | double distance_perception_error_; 57 | double max_perception_distance_; 58 | int point_step_; 59 | 60 | pcl::PointCloud pointcloud_; 61 | }; 62 | 63 | } // namespace mh_amcl 64 | 65 | #endif // MH_AMCL__POINTCLOUDCORRECTER_HPP_ 66 | -------------------------------------------------------------------------------- /include/mh_amcl/Types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef MH_AMCL__TYPES_HPP_ 17 | #define MH_AMCL__TYPES_HPP_ 18 | 19 | #include 20 | 21 | namespace mh_amcl 22 | { 23 | 24 | typedef struct 25 | { 26 | tf2::Transform pose; 27 | double prob {0.0}; 28 | float hits {0.0}; 29 | float possible_hits {0.0}; 30 | } Particle; 31 | 32 | typedef enum TColor 33 | { 34 | RED, GREEN, BLUE, WHITE, GREY, DARK_GREY, BLACK, YELLOW, ORANGE, BROWN, PINK, 35 | LIME_GREEN, PURPLE, CYAN, MAGENTA, NUM_COLORS 36 | } Color; 37 | 38 | } // namespace mh_amcl 39 | 40 | #endif // MH_AMCL__TYPES_HPP_ 41 | -------------------------------------------------------------------------------- /include/mh_amcl/Utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef MH_AMCL__UTILS_HPP_ 17 | #define MH_AMCL__UTILS_HPP_ 18 | 19 | namespace mh_amcl 20 | { 21 | 22 | } // namespace mh_amcl 23 | 24 | #endif // MH_AMCL__UTILS_HPP_ 25 | -------------------------------------------------------------------------------- /launch/bringup_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import (DeclareLaunchArgument, GroupAction, 21 | IncludeLaunchDescription, SetEnvironmentVariable) 22 | from launch.conditions import IfCondition 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch.substitutions import LaunchConfiguration, PythonExpression 25 | from launch_ros.actions import Node 26 | from launch_ros.actions import PushRosNamespace 27 | from nav2_common.launch import RewrittenYaml 28 | 29 | 30 | def generate_launch_description(): 31 | # Get the launch directory 32 | bringup_dir = get_package_share_directory('mh_amcl') 33 | launch_dir = os.path.join(bringup_dir, 'launch') 34 | 35 | # Create the launch configuration variables 36 | namespace = LaunchConfiguration('namespace') 37 | use_namespace = LaunchConfiguration('use_namespace') 38 | slam = LaunchConfiguration('slam') 39 | map_yaml_file = LaunchConfiguration('map') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | params_file = LaunchConfiguration('params_file') 42 | autostart = LaunchConfiguration('autostart') 43 | use_composition = LaunchConfiguration('use_composition') 44 | use_respawn = LaunchConfiguration('use_respawn') 45 | 46 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 47 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 48 | # https://github.com/ros/geometry2/issues/32 49 | # https://github.com/ros/robot_state_publisher/pull/30 50 | # TODO(orduno) Substitute with `PushNodeRemapping` 51 | # https://github.com/ros2/launch_ros/issues/56 52 | remappings = [('/tf', 'tf'), 53 | ('/tf_static', 'tf_static')] 54 | 55 | # Create our own temporary YAML files that include substitutions 56 | param_substitutions = { 57 | 'use_sim_time': use_sim_time, 58 | 'yaml_filename': map_yaml_file} 59 | 60 | configured_params = RewrittenYaml( 61 | source_file=params_file, 62 | root_key=namespace, 63 | param_rewrites=param_substitutions, 64 | convert_types=True) 65 | 66 | stdout_linebuf_envvar = SetEnvironmentVariable( 67 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 68 | 69 | declare_namespace_cmd = DeclareLaunchArgument( 70 | 'namespace', 71 | default_value='', 72 | description='Top-level namespace') 73 | 74 | declare_use_namespace_cmd = DeclareLaunchArgument( 75 | 'use_namespace', 76 | default_value='false', 77 | description='Whether to apply a namespace to the navigation stack') 78 | 79 | declare_slam_cmd = DeclareLaunchArgument( 80 | 'slam', 81 | default_value='False', 82 | description='Whether run a SLAM') 83 | 84 | declare_map_yaml_cmd = DeclareLaunchArgument( 85 | 'map', 86 | description='Full path to map yaml file to load') 87 | 88 | declare_use_sim_time_cmd = DeclareLaunchArgument( 89 | 'use_sim_time', 90 | default_value='false', 91 | description='Use simulation (Gazebo) clock if true') 92 | 93 | declare_params_file_cmd = DeclareLaunchArgument( 94 | 'params_file', 95 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 96 | description='Full path to the ROS2 parameters file to use for all launched nodes') 97 | 98 | declare_autostart_cmd = DeclareLaunchArgument( 99 | 'autostart', default_value='true', 100 | description='Automatically startup the nav2 stack') 101 | 102 | declare_use_composition_cmd = DeclareLaunchArgument( 103 | 'use_composition', default_value='False', 104 | description='Whether to use composed bringup') 105 | 106 | declare_use_respawn_cmd = DeclareLaunchArgument( 107 | 'use_respawn', default_value='False', 108 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 109 | 110 | # Specify the actions 111 | bringup_cmd_group = GroupAction([ 112 | PushRosNamespace( 113 | condition=IfCondition(use_namespace), 114 | namespace=namespace), 115 | 116 | Node( 117 | condition=IfCondition(use_composition), 118 | name='nav2_container', 119 | package='rclcpp_components', 120 | executable='component_container_isolated', 121 | parameters=[configured_params, {'autostart': autostart}], 122 | remappings=remappings, 123 | output='screen'), 124 | 125 | IncludeLaunchDescription( 126 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), 127 | condition=IfCondition(slam), 128 | launch_arguments={'namespace': namespace, 129 | 'use_sim_time': use_sim_time, 130 | 'autostart': autostart, 131 | 'use_respawn': use_respawn, 132 | 'params_file': params_file}.items()), 133 | 134 | IncludeLaunchDescription( 135 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 136 | 'localization_launch.py')), 137 | condition=IfCondition(PythonExpression(['not ', slam])), 138 | launch_arguments={'namespace': namespace, 139 | 'map': map_yaml_file, 140 | 'use_sim_time': use_sim_time, 141 | 'autostart': autostart, 142 | 'params_file': params_file, 143 | 'use_composition': use_composition, 144 | 'use_respawn': use_respawn, 145 | 'container_name': 'nav2_container'}.items()), 146 | 147 | IncludeLaunchDescription( 148 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), 149 | launch_arguments={'namespace': namespace, 150 | 'use_sim_time': use_sim_time, 151 | 'autostart': autostart, 152 | 'params_file': params_file, 153 | 'use_composition': use_composition, 154 | 'use_respawn': use_respawn, 155 | 'container_name': 'nav2_container'}.items()), 156 | ]) 157 | 158 | # Create the launch description and populate 159 | ld = LaunchDescription() 160 | 161 | # Set environment variables 162 | ld.add_action(stdout_linebuf_envvar) 163 | 164 | # Declare the launch options 165 | ld.add_action(declare_namespace_cmd) 166 | ld.add_action(declare_use_namespace_cmd) 167 | ld.add_action(declare_slam_cmd) 168 | ld.add_action(declare_map_yaml_cmd) 169 | ld.add_action(declare_use_sim_time_cmd) 170 | ld.add_action(declare_params_file_cmd) 171 | ld.add_action(declare_autostart_cmd) 172 | ld.add_action(declare_use_composition_cmd) 173 | ld.add_action(declare_use_respawn_cmd) 174 | 175 | # Add the actions to launch all of the navigation nodes 176 | ld.add_action(bringup_cmd_group) 177 | 178 | return ld 179 | -------------------------------------------------------------------------------- /launch/kobuki_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 23 | from launch.conditions import IfCondition 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import LaunchConfiguration 26 | 27 | 28 | def generate_launch_description(): 29 | # Get the launch directory 30 | bringup_dir = get_package_share_directory('mh_amcl') 31 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 32 | launch_dir = os.path.join(bringup_dir, 'launch') 33 | nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch') 34 | 35 | # Create the launch configuration variables 36 | slam = LaunchConfiguration('slam') 37 | namespace = LaunchConfiguration('namespace') 38 | use_namespace = LaunchConfiguration('use_namespace') 39 | map_yaml_file = LaunchConfiguration('map') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | params_file = LaunchConfiguration('params_file') 42 | autostart = LaunchConfiguration('autostart') 43 | use_composition = LaunchConfiguration('use_composition') 44 | use_respawn = LaunchConfiguration('use_respawn') 45 | 46 | # Launch configuration variables specific to simulation 47 | rviz_config_file = LaunchConfiguration('rviz_config_file') 48 | use_rviz = LaunchConfiguration('use_rviz') 49 | 50 | # Declare the launch arguments 51 | declare_namespace_cmd = DeclareLaunchArgument( 52 | 'namespace', 53 | default_value='', 54 | description='Top-level namespace') 55 | 56 | declare_use_namespace_cmd = DeclareLaunchArgument( 57 | 'use_namespace', 58 | default_value='false', 59 | description='Whether to apply a namespace to the navigation stack') 60 | 61 | declare_slam_cmd = DeclareLaunchArgument( 62 | 'slam', 63 | default_value='False', 64 | description='Whether run a SLAM') 65 | 66 | declare_map_yaml_cmd = DeclareLaunchArgument( 67 | 'map', 68 | default_value=os.path.join( 69 | bringup_dir, 'maps', 'home.yaml'), 70 | description='Full path to map file to load') 71 | 72 | declare_use_sim_time_cmd = DeclareLaunchArgument( 73 | 'use_sim_time', 74 | default_value='true', 75 | description='Use simulation (Gazebo) clock if true') 76 | 77 | declare_params_file_cmd = DeclareLaunchArgument( 78 | 'params_file', 79 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params_kobuki.yaml'), 80 | description='Full path to the ROS2 parameters file to use for all launched nodes') 81 | 82 | declare_autostart_cmd = DeclareLaunchArgument( 83 | 'autostart', default_value='true', 84 | description='Automatically startup the nav2 stack') 85 | 86 | declare_use_composition_cmd = DeclareLaunchArgument( 87 | 'use_composition', default_value='False', 88 | description='Whether to use composed bringup') 89 | 90 | declare_use_respawn_cmd = DeclareLaunchArgument( 91 | 'use_respawn', default_value='False', 92 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 93 | 94 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 95 | 'rviz_config_file', 96 | default_value=os.path.join( 97 | nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 98 | description='Full path to the RVIZ config file to use') 99 | 100 | declare_use_rviz_cmd = DeclareLaunchArgument( 101 | 'use_rviz', 102 | default_value='False', 103 | description='Whether to start RVIZ') 104 | 105 | rviz_cmd = IncludeLaunchDescription( 106 | PythonLaunchDescriptionSource( 107 | os.path.join(nav2_launch_dir, 'rviz_launch.py')), 108 | condition=IfCondition(use_rviz), 109 | launch_arguments={'namespace': namespace, 110 | 'use_namespace': use_namespace, 111 | 'rviz_config': rviz_config_file}.items()) 112 | 113 | bringup_cmd = IncludeLaunchDescription( 114 | PythonLaunchDescriptionSource( 115 | os.path.join(launch_dir, 'bringup_launch.py')), 116 | launch_arguments={'namespace': namespace, 117 | 'use_namespace': use_namespace, 118 | 'slam': slam, 119 | 'map': map_yaml_file, 120 | 'use_sim_time': use_sim_time, 121 | 'params_file': params_file, 122 | 'autostart': autostart, 123 | 'use_composition': use_composition, 124 | 'use_respawn': use_respawn}.items()) 125 | 126 | # Create the launch description and populate 127 | ld = LaunchDescription() 128 | 129 | # Declare the launch options 130 | ld.add_action(declare_namespace_cmd) 131 | ld.add_action(declare_use_namespace_cmd) 132 | ld.add_action(declare_slam_cmd) 133 | ld.add_action(declare_map_yaml_cmd) 134 | ld.add_action(declare_use_sim_time_cmd) 135 | ld.add_action(declare_params_file_cmd) 136 | ld.add_action(declare_autostart_cmd) 137 | ld.add_action(declare_use_composition_cmd) 138 | 139 | ld.add_action(declare_rviz_config_file_cmd) 140 | ld.add_action(declare_use_rviz_cmd) 141 | ld.add_action(declare_use_respawn_cmd) 142 | 143 | # Add the actions to launch all of the navigation nodes 144 | ld.add_action(rviz_cmd) 145 | ld.add_action(bringup_cmd) 146 | 147 | return ld 148 | -------------------------------------------------------------------------------- /launch/localization_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable 21 | from launch.conditions import IfCondition 22 | from launch.substitutions import LaunchConfiguration, PythonExpression 23 | from launch_ros.actions import LoadComposableNodes 24 | from launch_ros.actions import Node 25 | from launch_ros.descriptions import ComposableNode 26 | from nav2_common.launch import RewrittenYaml 27 | 28 | 29 | def generate_launch_description(): 30 | # Get the launch directory 31 | bringup_dir = get_package_share_directory('mh_amcl') 32 | 33 | namespace = LaunchConfiguration('namespace') 34 | map_yaml_file = LaunchConfiguration('map') 35 | use_sim_time = LaunchConfiguration('use_sim_time') 36 | autostart = LaunchConfiguration('autostart') 37 | params_file = LaunchConfiguration('params_file') 38 | use_composition = LaunchConfiguration('use_composition') 39 | container_name = LaunchConfiguration('container_name') 40 | use_respawn = LaunchConfiguration('use_respawn') 41 | 42 | lifecycle_nodes = ['map_server', 'mh_amcl'] 43 | 44 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 45 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 46 | # https://github.com/ros/geometry2/issues/32 47 | # https://github.com/ros/robot_state_publisher/pull/30 48 | # TODO(orduno) Substitute with `PushNodeRemapping` 49 | # https://github.com/ros2/launch_ros/issues/56 50 | remappings = [('/tf', 'tf'), 51 | ('/tf_static', 'tf_static')] 52 | 53 | # Create our own temporary YAML files that include substitutions 54 | param_substitutions = { 55 | 'use_sim_time': use_sim_time, 56 | 'yaml_filename': map_yaml_file} 57 | 58 | configured_params = RewrittenYaml( 59 | source_file=params_file, 60 | root_key=namespace, 61 | param_rewrites=param_substitutions, 62 | convert_types=True) 63 | 64 | stdout_linebuf_envvar = SetEnvironmentVariable( 65 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 66 | 67 | declare_namespace_cmd = DeclareLaunchArgument( 68 | 'namespace', 69 | default_value='', 70 | description='Top-level namespace') 71 | 72 | declare_map_yaml_cmd = DeclareLaunchArgument( 73 | 'map', 74 | description='Full path to map yaml file to load') 75 | 76 | declare_use_sim_time_cmd = DeclareLaunchArgument( 77 | 'use_sim_time', 78 | default_value='false', 79 | description='Use simulation (Gazebo) clock if true') 80 | 81 | declare_params_file_cmd = DeclareLaunchArgument( 82 | 'params_file', 83 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 84 | description='Full path to the ROS2 parameters file to use for all launched nodes') 85 | 86 | declare_autostart_cmd = DeclareLaunchArgument( 87 | 'autostart', default_value='true', 88 | description='Automatically startup the nav2 stack') 89 | 90 | declare_use_composition_cmd = DeclareLaunchArgument( 91 | 'use_composition', default_value='False', 92 | description='Use composed bringup if True') 93 | 94 | declare_container_name_cmd = DeclareLaunchArgument( 95 | 'container_name', default_value='nav2_container', 96 | description='the name of conatiner that nodes will load in if use composition') 97 | 98 | declare_use_respawn_cmd = DeclareLaunchArgument( 99 | 'use_respawn', default_value='False', 100 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 101 | 102 | load_nodes = GroupAction( 103 | condition=IfCondition(PythonExpression(['not ', use_composition])), 104 | actions=[ 105 | Node( 106 | package='extended_map_server', 107 | executable='map_server', 108 | name='map_server', 109 | output='screen', 110 | respawn=use_respawn, 111 | respawn_delay=2.0, 112 | parameters=[configured_params], 113 | remappings=remappings), 114 | Node( 115 | package='mh_amcl', 116 | executable='mh_amcl_program', 117 | name='mh_amcl', 118 | output='screen', 119 | respawn=use_respawn, 120 | respawn_delay=2.0, 121 | parameters=[configured_params], 122 | remappings=remappings, 123 | # prefix=['xterm -e gdb -ex run --args'], 124 | # arguments=['--ros-args', '--log-level', 'mh_amcl:=debug'], 125 | ), 126 | Node( 127 | package='nav2_lifecycle_manager', 128 | executable='lifecycle_manager', 129 | name='lifecycle_manager_localization', 130 | output='screen', 131 | parameters=[{'use_sim_time': use_sim_time}, 132 | {'autostart': autostart}, 133 | {'node_names': lifecycle_nodes}]) 134 | ] 135 | ) 136 | 137 | load_composable_nodes = LoadComposableNodes( 138 | condition=IfCondition(use_composition), 139 | target_container=container_name, 140 | composable_node_descriptions=[ 141 | ComposableNode( 142 | package='extended_map_server', 143 | plugin='extended_map_server::MapServer', 144 | name='map_server', 145 | parameters=[configured_params], 146 | remappings=remappings), 147 | ComposableNode( 148 | package='mh_amcl', 149 | plugin='mh_amcl::AmclNode', 150 | name='mh_amcl', 151 | parameters=[configured_params], 152 | remappings=remappings), 153 | ComposableNode( 154 | package='nav2_lifecycle_manager', 155 | plugin='nav2_lifecycle_manager::LifecycleManager', 156 | name='lifecycle_manager_localization', 157 | parameters=[{'use_sim_time': use_sim_time, 158 | 'autostart': autostart, 159 | 'node_names': lifecycle_nodes}]), 160 | ], 161 | ) 162 | 163 | # Create the launch description and populate 164 | ld = LaunchDescription() 165 | 166 | # Set environment variables 167 | ld.add_action(stdout_linebuf_envvar) 168 | 169 | # Declare the launch options 170 | ld.add_action(declare_namespace_cmd) 171 | ld.add_action(declare_map_yaml_cmd) 172 | ld.add_action(declare_use_sim_time_cmd) 173 | ld.add_action(declare_params_file_cmd) 174 | ld.add_action(declare_autostart_cmd) 175 | ld.add_action(declare_use_composition_cmd) 176 | ld.add_action(declare_container_name_cmd) 177 | ld.add_action(declare_use_respawn_cmd) 178 | 179 | # Add the actions to launch all of the localiztion nodes 180 | ld.add_action(load_nodes) 181 | ld.add_action(load_composable_nodes) 182 | 183 | return ld 184 | -------------------------------------------------------------------------------- /launch/navigation_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable 21 | from launch.conditions import IfCondition 22 | from launch.substitutions import LaunchConfiguration, PythonExpression 23 | from launch_ros.actions import LoadComposableNodes 24 | from launch_ros.actions import Node 25 | from launch_ros.descriptions import ComposableNode 26 | from nav2_common.launch import RewrittenYaml 27 | 28 | 29 | def generate_launch_description(): 30 | # Get the launch directory 31 | bringup_dir = get_package_share_directory('mh_amcl') 32 | 33 | namespace = LaunchConfiguration('namespace') 34 | use_sim_time = LaunchConfiguration('use_sim_time') 35 | autostart = LaunchConfiguration('autostart') 36 | params_file = LaunchConfiguration('params_file') 37 | use_composition = LaunchConfiguration('use_composition') 38 | container_name = LaunchConfiguration('container_name') 39 | use_respawn = LaunchConfiguration('use_respawn') 40 | 41 | lifecycle_nodes = ['controller_server', 42 | 'smoother_server', 43 | 'planner_server', 44 | 'behavior_server', 45 | 'bt_navigator', 46 | 'waypoint_follower', 47 | 'velocity_smoother'] 48 | 49 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 50 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 51 | # https://github.com/ros/geometry2/issues/32 52 | # https://github.com/ros/robot_state_publisher/pull/30 53 | # TODO(orduno) Substitute with `PushNodeRemapping` 54 | # https://github.com/ros2/launch_ros/issues/56 55 | remappings = [('/tf', 'tf'), 56 | ('/tf_static', 'tf_static')] 57 | 58 | # Create our own temporary YAML files that include substitutions 59 | param_substitutions = { 60 | 'use_sim_time': use_sim_time, 61 | 'autostart': autostart} 62 | 63 | configured_params = RewrittenYaml( 64 | source_file=params_file, 65 | root_key=namespace, 66 | param_rewrites=param_substitutions, 67 | convert_types=True) 68 | 69 | stdout_linebuf_envvar = SetEnvironmentVariable( 70 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 71 | 72 | declare_namespace_cmd = DeclareLaunchArgument( 73 | 'namespace', 74 | default_value='', 75 | description='Top-level namespace') 76 | 77 | declare_use_sim_time_cmd = DeclareLaunchArgument( 78 | 'use_sim_time', 79 | default_value='false', 80 | description='Use simulation (Gazebo) clock if true') 81 | 82 | declare_params_file_cmd = DeclareLaunchArgument( 83 | 'params_file', 84 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 85 | description='Full path to the ROS2 parameters file to use for all launched nodes') 86 | 87 | declare_autostart_cmd = DeclareLaunchArgument( 88 | 'autostart', default_value='true', 89 | description='Automatically startup the nav2 stack') 90 | 91 | declare_use_composition_cmd = DeclareLaunchArgument( 92 | 'use_composition', default_value='False', 93 | description='Use composed bringup if True') 94 | 95 | declare_container_name_cmd = DeclareLaunchArgument( 96 | 'container_name', default_value='nav2_container', 97 | description='the name of conatiner that nodes will load in if use composition') 98 | 99 | declare_use_respawn_cmd = DeclareLaunchArgument( 100 | 'use_respawn', default_value='False', 101 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 102 | 103 | load_nodes = GroupAction( 104 | condition=IfCondition(PythonExpression(['not ', use_composition])), 105 | actions=[ 106 | Node( 107 | package='nav2_controller', 108 | executable='controller_server', 109 | output='screen', 110 | respawn=use_respawn, 111 | respawn_delay=2.0, 112 | parameters=[configured_params], 113 | remappings=remappings + [('robot/cmd_vel', 'cmd_vel_nav')]), 114 | Node( 115 | package='nav2_smoother', 116 | executable='smoother_server', 117 | name='smoother_server', 118 | output='screen', 119 | respawn=use_respawn, 120 | respawn_delay=2.0, 121 | parameters=[configured_params], 122 | remappings=remappings), 123 | 124 | Node( 125 | package='nav2_planner', 126 | executable='planner_server', 127 | name='planner_server', 128 | output='screen', 129 | respawn=use_respawn, 130 | respawn_delay=2.0, 131 | parameters=[configured_params], 132 | # prefix=['xterm -e gdb -ex run --args'], 133 | remappings=remappings), 134 | Node( 135 | package='nav2_behaviors', 136 | executable='behavior_server', 137 | name='behavior_server', 138 | output='screen', 139 | respawn=use_respawn, 140 | respawn_delay=2.0, 141 | parameters=[configured_params], 142 | remappings=remappings), 143 | Node( 144 | package='nav2_bt_navigator', 145 | executable='bt_navigator', 146 | name='bt_navigator', 147 | output='screen', 148 | respawn=use_respawn, 149 | respawn_delay=2.0, 150 | parameters=[configured_params], 151 | remappings=remappings), 152 | Node( 153 | package='nav2_waypoint_follower', 154 | executable='waypoint_follower', 155 | name='waypoint_follower', 156 | output='screen', 157 | respawn=use_respawn, 158 | respawn_delay=2.0, 159 | parameters=[configured_params], 160 | remappings=remappings), 161 | Node( 162 | package='nav2_velocity_smoother', 163 | executable='velocity_smoother', 164 | name='velocity_smoother', 165 | output='screen', 166 | respawn=use_respawn, 167 | respawn_delay=2.0, 168 | parameters=[configured_params], 169 | remappings=remappings + 170 | [('robot/cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'robot/cmd_vel')]), 171 | Node( 172 | package='nav2_lifecycle_manager', 173 | executable='lifecycle_manager', 174 | name='lifecycle_manager_navigation', 175 | output='screen', 176 | parameters=[{'use_sim_time': use_sim_time}, 177 | {'autostart': autostart}, 178 | {'node_names': lifecycle_nodes}]), 179 | ] 180 | ) 181 | 182 | load_composable_nodes = LoadComposableNodes( 183 | condition=IfCondition(use_composition), 184 | target_container=container_name, 185 | composable_node_descriptions=[ 186 | ComposableNode( 187 | package='nav2_controller', 188 | plugin='nav2_controller::ControllerServer', 189 | name='controller_server', 190 | parameters=[configured_params], 191 | remappings=remappings + [('robot/cmd_vel', 'cmd_vel_nav')]), 192 | ComposableNode( 193 | package='nav2_smoother', 194 | plugin='nav2_smoother::SmootherServer', 195 | name='smoother_server', 196 | parameters=[configured_params], 197 | remappings=remappings), 198 | ComposableNode( 199 | package='nav2_planner', 200 | plugin='nav2_planner::PlannerServer', 201 | name='planner_server', 202 | parameters=[configured_params], 203 | remappings=remappings), 204 | ComposableNode( 205 | package='nav2_behaviors', 206 | plugin='behavior_server::BehaviorServer', 207 | name='behavior_server', 208 | parameters=[configured_params], 209 | remappings=remappings), 210 | ComposableNode( 211 | package='nav2_bt_navigator', 212 | plugin='nav2_bt_navigator::BtNavigator', 213 | name='bt_navigator', 214 | parameters=[configured_params], 215 | remappings=remappings), 216 | ComposableNode( 217 | package='nav2_waypoint_follower', 218 | plugin='nav2_waypoint_follower::WaypointFollower', 219 | name='waypoint_follower', 220 | parameters=[configured_params], 221 | remappings=remappings), 222 | ComposableNode( 223 | package='nav2_velocity_smoother', 224 | plugin='nav2_velocity_smoother::VelocitySmoother', 225 | name='velocity_smoother', 226 | parameters=[configured_params], 227 | remappings=remappings + 228 | [('robot/cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'robot/cmd_vel')]), 229 | ComposableNode( 230 | package='nav2_lifecycle_manager', 231 | plugin='nav2_lifecycle_manager::LifecycleManager', 232 | name='lifecycle_manager_navigation', 233 | parameters=[{'use_sim_time': use_sim_time, 234 | 'autostart': autostart, 235 | 'node_names': lifecycle_nodes}]), 236 | ], 237 | ) 238 | 239 | # Create the launch description and populate 240 | ld = LaunchDescription() 241 | 242 | # Set environment variables 243 | ld.add_action(stdout_linebuf_envvar) 244 | 245 | # Declare the launch options 246 | ld.add_action(declare_namespace_cmd) 247 | ld.add_action(declare_use_sim_time_cmd) 248 | ld.add_action(declare_params_file_cmd) 249 | ld.add_action(declare_autostart_cmd) 250 | ld.add_action(declare_use_composition_cmd) 251 | ld.add_action(declare_container_name_cmd) 252 | ld.add_action(declare_use_respawn_cmd) 253 | 254 | # Add the actions to launch all of the navigation nodes 255 | ld.add_action(load_nodes) 256 | ld.add_action(load_composable_nodes) 257 | 258 | return ld 259 | -------------------------------------------------------------------------------- /launch/summit_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 23 | from launch.conditions import IfCondition 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import LaunchConfiguration 26 | 27 | 28 | def generate_launch_description(): 29 | # Get the launch directory 30 | mh_bringup_dir = get_package_share_directory('mh_amcl') 31 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 32 | mh_launch_dir = os.path.join(mh_bringup_dir, 'launch') 33 | nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch') 34 | 35 | # Create the launch configuration variables 36 | slam = LaunchConfiguration('slam') 37 | namespace = LaunchConfiguration('namespace') 38 | use_namespace = LaunchConfiguration('use_namespace') 39 | map_yaml_file = LaunchConfiguration('map') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | params_file = LaunchConfiguration('params_file') 42 | autostart = LaunchConfiguration('autostart') 43 | use_composition = LaunchConfiguration('use_composition') 44 | use_respawn = LaunchConfiguration('use_respawn') 45 | 46 | # Launch configuration variables specific to simulation 47 | rviz_config_file = LaunchConfiguration('rviz_config_file') 48 | use_rviz = LaunchConfiguration('use_rviz') 49 | 50 | # Declare the launch arguments 51 | declare_namespace_cmd = DeclareLaunchArgument( 52 | 'namespace', 53 | default_value='', 54 | description='Top-level namespace') 55 | 56 | declare_use_namespace_cmd = DeclareLaunchArgument( 57 | 'use_namespace', 58 | default_value='false', 59 | description='Whether to apply a namespace to the navigation stack') 60 | 61 | declare_slam_cmd = DeclareLaunchArgument( 62 | 'slam', 63 | default_value='False', 64 | description='Whether run a SLAM') 65 | 66 | declare_map_yaml_cmd = DeclareLaunchArgument( 67 | 'map', 68 | default_value=os.path.join( 69 | mh_bringup_dir, 'maps', 'simple_uneven_house.yaml'), 70 | description='Full path to map file to load') 71 | 72 | declare_use_sim_time_cmd = DeclareLaunchArgument( 73 | 'use_sim_time', 74 | default_value='true', 75 | description='Use simulation (Gazebo) clock if true') 76 | 77 | declare_params_file_cmd = DeclareLaunchArgument( 78 | 'params_file', 79 | default_value=os.path.join(mh_bringup_dir, 'params', 'nav2_params_summit.yaml'), 80 | description='Full path to the ROS2 parameters file to use for all launched nodes') 81 | 82 | declare_autostart_cmd = DeclareLaunchArgument( 83 | 'autostart', default_value='true', 84 | description='Automatically startup the nav2 stack') 85 | 86 | declare_use_composition_cmd = DeclareLaunchArgument( 87 | 'use_composition', default_value='False', 88 | description='Whether to use composed bringup') 89 | 90 | declare_use_respawn_cmd = DeclareLaunchArgument( 91 | 'use_respawn', default_value='False', 92 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 93 | 94 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 95 | 'rviz_config_file', 96 | default_value=os.path.join( 97 | nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 98 | description='Full path to the RVIZ config file to use') 99 | 100 | declare_use_rviz_cmd = DeclareLaunchArgument( 101 | 'use_rviz', 102 | default_value='False', 103 | description='Whether to start RVIZ') 104 | 105 | rviz_cmd = IncludeLaunchDescription( 106 | PythonLaunchDescriptionSource( 107 | os.path.join(nav2_launch_dir, 'rviz_launch.py')), 108 | condition=IfCondition(use_rviz), 109 | launch_arguments={'namespace': namespace, 110 | 'use_namespace': use_namespace, 111 | 'rviz_config': rviz_config_file}.items()) 112 | 113 | bringup_cmd = IncludeLaunchDescription( 114 | PythonLaunchDescriptionSource( 115 | os.path.join(mh_launch_dir, 'bringup_launch.py')), 116 | launch_arguments={'namespace': namespace, 117 | 'use_namespace': use_namespace, 118 | 'slam': slam, 119 | 'map': map_yaml_file, 120 | 'use_sim_time': use_sim_time, 121 | 'params_file': params_file, 122 | 'autostart': autostart, 123 | 'use_composition': use_composition, 124 | 'use_respawn': use_respawn}.items()) 125 | 126 | # Create the launch description and populate 127 | ld = LaunchDescription() 128 | 129 | # Declare the launch options 130 | ld.add_action(declare_namespace_cmd) 131 | ld.add_action(declare_use_namespace_cmd) 132 | ld.add_action(declare_slam_cmd) 133 | ld.add_action(declare_map_yaml_cmd) 134 | ld.add_action(declare_use_sim_time_cmd) 135 | ld.add_action(declare_params_file_cmd) 136 | ld.add_action(declare_autostart_cmd) 137 | ld.add_action(declare_use_composition_cmd) 138 | 139 | ld.add_action(declare_rviz_config_file_cmd) 140 | ld.add_action(declare_use_rviz_cmd) 141 | ld.add_action(declare_use_respawn_cmd) 142 | 143 | # Add the actions to launch all of the navigation nodes 144 | ld.add_action(rviz_cmd) 145 | ld.add_action(bringup_cmd) 146 | 147 | return ld 148 | -------------------------------------------------------------------------------- /launch/tb3_simulation_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 23 | from launch.conditions import IfCondition 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import LaunchConfiguration, PythonExpression 26 | from launch_ros.actions import Node 27 | 28 | 29 | def generate_launch_description(): 30 | # Get the launch directory 31 | bringup_dir = get_package_share_directory('mh_amcl') 32 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 33 | launch_dir = os.path.join(bringup_dir, 'launch') 34 | nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch') 35 | 36 | # Create the launch configuration variables 37 | slam = LaunchConfiguration('slam') 38 | namespace = LaunchConfiguration('namespace') 39 | use_namespace = LaunchConfiguration('use_namespace') 40 | map_yaml_file = LaunchConfiguration('map') 41 | use_sim_time = LaunchConfiguration('use_sim_time') 42 | params_file = LaunchConfiguration('params_file') 43 | autostart = LaunchConfiguration('autostart') 44 | use_composition = LaunchConfiguration('use_composition') 45 | use_respawn = LaunchConfiguration('use_respawn') 46 | 47 | # Launch configuration variables specific to simulation 48 | rviz_config_file = LaunchConfiguration('rviz_config_file') 49 | use_simulator = LaunchConfiguration('use_simulator') 50 | use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') 51 | use_rviz = LaunchConfiguration('use_rviz') 52 | headless = LaunchConfiguration('headless') 53 | world = LaunchConfiguration('world') 54 | pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), 55 | 'y': LaunchConfiguration('y_pose', default='-0.50'), 56 | 'z': LaunchConfiguration('z_pose', default='0.01'), 57 | 'R': LaunchConfiguration('roll', default='0.00'), 58 | 'P': LaunchConfiguration('pitch', default='0.00'), 59 | 'Y': LaunchConfiguration('yaw', default='0.00')} 60 | robot_name = LaunchConfiguration('robot_name') 61 | robot_sdf = LaunchConfiguration('robot_sdf') 62 | 63 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 64 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 65 | # https://github.com/ros/geometry2/issues/32 66 | # https://github.com/ros/robot_state_publisher/pull/30 67 | # TODO(orduno) Substitute with `PushNodeRemapping` 68 | # https://github.com/ros2/launch_ros/issues/56 69 | remappings = [('/tf', 'tf'), 70 | ('/tf_static', 'tf_static')] 71 | 72 | # Declare the launch arguments 73 | declare_namespace_cmd = DeclareLaunchArgument( 74 | 'namespace', 75 | default_value='', 76 | description='Top-level namespace') 77 | 78 | declare_use_namespace_cmd = DeclareLaunchArgument( 79 | 'use_namespace', 80 | default_value='false', 81 | description='Whether to apply a namespace to the navigation stack') 82 | 83 | declare_slam_cmd = DeclareLaunchArgument( 84 | 'slam', 85 | default_value='False', 86 | description='Whether run a SLAM') 87 | 88 | declare_map_yaml_cmd = DeclareLaunchArgument( 89 | 'map', 90 | default_value=os.path.join( 91 | nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml'), 92 | description='Full path to map file to load') 93 | 94 | declare_use_sim_time_cmd = DeclareLaunchArgument( 95 | 'use_sim_time', 96 | default_value='true', 97 | description='Use simulation (Gazebo) clock if true') 98 | 99 | declare_params_file_cmd = DeclareLaunchArgument( 100 | 'params_file', 101 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params_tb3_sim.yaml'), 102 | description='Full path to the ROS2 parameters file to use for all launched nodes') 103 | 104 | declare_autostart_cmd = DeclareLaunchArgument( 105 | 'autostart', default_value='true', 106 | description='Automatically startup the nav2 stack') 107 | 108 | declare_use_composition_cmd = DeclareLaunchArgument( 109 | 'use_composition', default_value='False', 110 | description='Whether to use composed bringup') 111 | 112 | declare_use_respawn_cmd = DeclareLaunchArgument( 113 | 'use_respawn', default_value='False', 114 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 115 | 116 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 117 | 'rviz_config_file', 118 | default_value=os.path.join( 119 | nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 120 | description='Full path to the RVIZ config file to use') 121 | 122 | declare_use_simulator_cmd = DeclareLaunchArgument( 123 | 'use_simulator', 124 | default_value='True', 125 | description='Whether to start the simulator') 126 | 127 | declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 128 | 'use_robot_state_pub', 129 | default_value='True', 130 | description='Whether to start the robot state publisher') 131 | 132 | declare_use_rviz_cmd = DeclareLaunchArgument( 133 | 'use_rviz', 134 | default_value='True', 135 | description='Whether to start RVIZ') 136 | 137 | declare_simulator_cmd = DeclareLaunchArgument( 138 | 'headless', 139 | default_value='True', 140 | description='Whether to execute gzclient)') 141 | 142 | declare_world_cmd = DeclareLaunchArgument( 143 | 'world', 144 | # TODO(orduno) Switch back once ROS argument passing has been fixed upstream 145 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 146 | # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), 147 | # worlds/turtlebot3_worlds/waffle.model') 148 | default_value=os.path.join(nav2_bringup_dir, 'worlds', 'world_only.model'), 149 | description='Full path to world model file to load') 150 | 151 | declare_robot_name_cmd = DeclareLaunchArgument( 152 | 'robot_name', 153 | default_value='turtlebot3_waffle', 154 | description='name of the robot') 155 | 156 | declare_robot_sdf_cmd = DeclareLaunchArgument( 157 | 'robot_sdf', 158 | default_value=os.path.join(nav2_bringup_dir, 'worlds', 'waffle.model'), 159 | description='Full path to robot sdf file to spawn the robot in gazebo') 160 | 161 | # Specify the actions 162 | start_gazebo_server_cmd = ExecuteProcess( 163 | condition=IfCondition(use_simulator), 164 | cmd=['gzserver', '-s', 'libgazebo_ros_init.so', 165 | '-s', 'libgazebo_ros_factory.so', world], 166 | cwd=[nav2_launch_dir], output='screen') 167 | 168 | start_gazebo_client_cmd = ExecuteProcess( 169 | condition=IfCondition(PythonExpression( 170 | [use_simulator, ' and not ', headless])), 171 | cmd=['gzclient'], 172 | cwd=[nav2_launch_dir], output='screen') 173 | 174 | urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') 175 | with open(urdf, 'r') as infp: 176 | robot_description = infp.read() 177 | 178 | start_robot_state_publisher_cmd = Node( 179 | condition=IfCondition(use_robot_state_pub), 180 | package='robot_state_publisher', 181 | executable='robot_state_publisher', 182 | name='robot_state_publisher', 183 | namespace=namespace, 184 | output='screen', 185 | parameters=[{'use_sim_time': use_sim_time, 186 | 'robot_description': robot_description}], 187 | remappings=remappings) 188 | 189 | start_gazebo_spawner_cmd = Node( 190 | package='gazebo_ros', 191 | executable='spawn_entity.py', 192 | output='screen', 193 | arguments=[ 194 | '-entity', robot_name, 195 | '-file', robot_sdf, 196 | '-robot_namespace', namespace, 197 | '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], 198 | '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]) 199 | 200 | rviz_cmd = IncludeLaunchDescription( 201 | PythonLaunchDescriptionSource( 202 | os.path.join(nav2_launch_dir, 'rviz_launch.py')), 203 | condition=IfCondition(use_rviz), 204 | launch_arguments={'namespace': namespace, 205 | 'use_namespace': use_namespace, 206 | 'rviz_config': rviz_config_file}.items()) 207 | 208 | bringup_cmd = IncludeLaunchDescription( 209 | PythonLaunchDescriptionSource( 210 | os.path.join(launch_dir, 'bringup_launch.py')), 211 | launch_arguments={'namespace': namespace, 212 | 'use_namespace': use_namespace, 213 | 'slam': slam, 214 | 'map': map_yaml_file, 215 | 'use_sim_time': use_sim_time, 216 | 'params_file': params_file, 217 | 'autostart': autostart, 218 | 'use_composition': use_composition, 219 | 'use_respawn': use_respawn}.items()) 220 | 221 | # Create the launch description and populate 222 | ld = LaunchDescription() 223 | 224 | # Declare the launch options 225 | ld.add_action(declare_namespace_cmd) 226 | ld.add_action(declare_use_namespace_cmd) 227 | ld.add_action(declare_slam_cmd) 228 | ld.add_action(declare_map_yaml_cmd) 229 | ld.add_action(declare_use_sim_time_cmd) 230 | ld.add_action(declare_params_file_cmd) 231 | ld.add_action(declare_autostart_cmd) 232 | ld.add_action(declare_use_composition_cmd) 233 | 234 | ld.add_action(declare_rviz_config_file_cmd) 235 | ld.add_action(declare_use_simulator_cmd) 236 | ld.add_action(declare_use_robot_state_pub_cmd) 237 | ld.add_action(declare_use_rviz_cmd) 238 | ld.add_action(declare_simulator_cmd) 239 | ld.add_action(declare_world_cmd) 240 | ld.add_action(declare_robot_name_cmd) 241 | ld.add_action(declare_robot_sdf_cmd) 242 | ld.add_action(declare_use_respawn_cmd) 243 | 244 | # Add any conditioned actions 245 | ld.add_action(start_gazebo_server_cmd) 246 | ld.add_action(start_gazebo_client_cmd) 247 | ld.add_action(start_gazebo_spawner_cmd) 248 | 249 | # Add the actions to launch all of the navigation nodes 250 | ld.add_action(start_robot_state_publisher_cmd) 251 | ld.add_action(rviz_cmd) 252 | ld.add_action(bringup_cmd) 253 | 254 | return ld 255 | -------------------------------------------------------------------------------- /launch/tiago_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 23 | from launch.conditions import IfCondition 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import LaunchConfiguration 26 | 27 | 28 | def generate_launch_description(): 29 | # Get the launch directory 30 | bringup_dir = get_package_share_directory('mh_amcl') 31 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 32 | launch_dir = os.path.join(bringup_dir, 'launch') 33 | nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch') 34 | 35 | # Create the launch configuration variables 36 | slam = LaunchConfiguration('slam') 37 | namespace = LaunchConfiguration('namespace') 38 | use_namespace = LaunchConfiguration('use_namespace') 39 | map_yaml_file = LaunchConfiguration('map') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | params_file = LaunchConfiguration('params_file') 42 | autostart = LaunchConfiguration('autostart') 43 | use_composition = LaunchConfiguration('use_composition') 44 | use_respawn = LaunchConfiguration('use_respawn') 45 | 46 | # Launch configuration variables specific to simulation 47 | rviz_config_file = LaunchConfiguration('rviz_config_file') 48 | use_rviz = LaunchConfiguration('use_rviz') 49 | 50 | # Declare the launch arguments 51 | declare_namespace_cmd = DeclareLaunchArgument( 52 | 'namespace', 53 | default_value='', 54 | description='Top-level namespace') 55 | 56 | declare_use_namespace_cmd = DeclareLaunchArgument( 57 | 'use_namespace', 58 | default_value='false', 59 | description='Whether to apply a namespace to the navigation stack') 60 | 61 | declare_slam_cmd = DeclareLaunchArgument( 62 | 'slam', 63 | default_value='False', 64 | description='Whether run a SLAM') 65 | 66 | declare_map_yaml_cmd = DeclareLaunchArgument( 67 | 'map', 68 | default_value=os.path.join( 69 | bringup_dir, 'maps', 'lab.yaml'), 70 | description='Full path to map file to load') 71 | 72 | declare_use_sim_time_cmd = DeclareLaunchArgument( 73 | 'use_sim_time', 74 | default_value='false', 75 | description='Use simulation (Gazebo) clock if true') 76 | 77 | declare_params_file_cmd = DeclareLaunchArgument( 78 | 'params_file', 79 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params_tiago.yaml'), 80 | description='Full path to the ROS2 parameters file to use for all launched nodes') 81 | 82 | declare_autostart_cmd = DeclareLaunchArgument( 83 | 'autostart', default_value='true', 84 | description='Automatically startup the nav2 stack') 85 | 86 | declare_use_composition_cmd = DeclareLaunchArgument( 87 | 'use_composition', default_value='False', 88 | description='Whether to use composed bringup') 89 | 90 | declare_use_respawn_cmd = DeclareLaunchArgument( 91 | 'use_respawn', default_value='False', 92 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 93 | 94 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 95 | 'rviz_config_file', 96 | default_value=os.path.join( 97 | nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 98 | description='Full path to the RVIZ config file to use') 99 | 100 | declare_use_rviz_cmd = DeclareLaunchArgument( 101 | 'use_rviz', 102 | default_value='True', 103 | description='Whether to start RVIZ') 104 | 105 | rviz_cmd = IncludeLaunchDescription( 106 | PythonLaunchDescriptionSource( 107 | os.path.join(nav2_launch_dir, 'rviz_launch.py')), 108 | condition=IfCondition(use_rviz), 109 | launch_arguments={'namespace': namespace, 110 | 'use_namespace': use_namespace, 111 | 'rviz_config': rviz_config_file}.items()) 112 | 113 | bringup_cmd = IncludeLaunchDescription( 114 | PythonLaunchDescriptionSource( 115 | os.path.join(launch_dir, 'bringup_launch.py')), 116 | launch_arguments={'namespace': namespace, 117 | 'use_namespace': use_namespace, 118 | 'slam': slam, 119 | 'map': map_yaml_file, 120 | 'use_sim_time': use_sim_time, 121 | 'params_file': params_file, 122 | 'autostart': autostart, 123 | 'use_composition': use_composition, 124 | 'use_respawn': use_respawn}.items()) 125 | 126 | # Create the launch description and populate 127 | ld = LaunchDescription() 128 | 129 | # Declare the launch options 130 | ld.add_action(declare_namespace_cmd) 131 | ld.add_action(declare_use_namespace_cmd) 132 | ld.add_action(declare_slam_cmd) 133 | ld.add_action(declare_map_yaml_cmd) 134 | ld.add_action(declare_use_sim_time_cmd) 135 | ld.add_action(declare_params_file_cmd) 136 | ld.add_action(declare_autostart_cmd) 137 | ld.add_action(declare_use_composition_cmd) 138 | 139 | ld.add_action(declare_rviz_config_file_cmd) 140 | ld.add_action(declare_use_rviz_cmd) 141 | ld.add_action(declare_use_respawn_cmd) 142 | 143 | # Add the actions to launch all of the navigation nodes 144 | ld.add_action(rviz_cmd) 145 | ld.add_action(bringup_cmd) 146 | 147 | return ld 148 | -------------------------------------------------------------------------------- /launch/tiago_real_exp_mh3d_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 23 | from launch.conditions import IfCondition 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import LaunchConfiguration 26 | from launch_ros.actions import Node 27 | 28 | 29 | def generate_launch_description(): 30 | # Get the launch directory 31 | mh_bringup_dir = get_package_share_directory('mh_amcl') 32 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 33 | mh_launch_dir = os.path.join(mh_bringup_dir, 'launch') 34 | nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch') 35 | 36 | # Create the launch configuration variables 37 | slam = LaunchConfiguration('slam') 38 | namespace = LaunchConfiguration('namespace') 39 | use_namespace = LaunchConfiguration('use_namespace') 40 | map_yaml_file = LaunchConfiguration('map') 41 | use_sim_time = LaunchConfiguration('use_sim_time') 42 | params_file = LaunchConfiguration('params_file') 43 | autostart = LaunchConfiguration('autostart') 44 | use_composition = LaunchConfiguration('use_composition') 45 | use_respawn = LaunchConfiguration('use_respawn') 46 | 47 | # Launch configuration variables specific to simulation 48 | rviz_config_file = LaunchConfiguration('rviz_config_file') 49 | use_rviz = LaunchConfiguration('use_rviz') 50 | 51 | # Declare the launch arguments 52 | declare_namespace_cmd = DeclareLaunchArgument( 53 | 'namespace', 54 | default_value='', 55 | description='Top-level namespace') 56 | 57 | declare_use_namespace_cmd = DeclareLaunchArgument( 58 | 'use_namespace', 59 | default_value='false', 60 | description='Whether to apply a namespace to the navigation stack') 61 | 62 | declare_slam_cmd = DeclareLaunchArgument( 63 | 'slam', 64 | default_value='False', 65 | description='Whether run a SLAM') 66 | 67 | declare_map_yaml_cmd = DeclareLaunchArgument( 68 | 'map', 69 | default_value=os.path.join( 70 | mh_bringup_dir, 'maps', 'lab_3d.yaml'), 71 | description='Full path to map file to load') 72 | 73 | declare_use_sim_time_cmd = DeclareLaunchArgument( 74 | 'use_sim_time', 75 | default_value='true', 76 | description='Use simulation (Gazebo) clock if true') 77 | 78 | declare_params_file_cmd = DeclareLaunchArgument( 79 | 'params_file', 80 | default_value=os.path.join(mh_bringup_dir, 'params', 'nav2_params_tiago_real_exp_mh3d.yaml'), 81 | description='Full path to the ROS2 parameters file to use for all launched nodes') 82 | 83 | declare_autostart_cmd = DeclareLaunchArgument( 84 | 'autostart', default_value='true', 85 | description='Automatically startup the nav2 stack') 86 | 87 | declare_use_composition_cmd = DeclareLaunchArgument( 88 | 'use_composition', default_value='False', 89 | description='Whether to use composed bringup') 90 | 91 | declare_use_respawn_cmd = DeclareLaunchArgument( 92 | 'use_respawn', default_value='False', 93 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 94 | 95 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 96 | 'rviz_config_file', 97 | default_value=os.path.join( 98 | nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 99 | description='Full path to the RVIZ config file to use') 100 | 101 | declare_use_rviz_cmd = DeclareLaunchArgument( 102 | 'use_rviz', 103 | default_value='False', 104 | description='Whether to start RVIZ') 105 | 106 | rviz_cmd = IncludeLaunchDescription( 107 | PythonLaunchDescriptionSource( 108 | os.path.join(nav2_launch_dir, 'rviz_launch.py')), 109 | condition=IfCondition(use_rviz), 110 | launch_arguments={'namespace': namespace, 111 | 'use_namespace': use_namespace, 112 | 'rviz_config': rviz_config_file}.items()) 113 | 114 | bringup_cmd = IncludeLaunchDescription( 115 | PythonLaunchDescriptionSource( 116 | os.path.join(mh_launch_dir, 'bringup_launch.py')), # Change to select amcl or mh_amcl 117 | # os.path.join(nav2_launch_dir, 'bringup_launch.py')), # Change to select amcl or mh_amcl 118 | launch_arguments={'namespace': namespace, 119 | 'use_namespace': use_namespace, 120 | 'slam': slam, 121 | 'map': map_yaml_file, 122 | 'use_sim_time': use_sim_time, 123 | 'params_file': params_file, 124 | 'autostart': autostart, 125 | 'use_composition': use_composition, 126 | 'use_respawn': use_respawn}.items()) 127 | 128 | start_mocap_marker_rviz = Node( 129 | package='mocap_marker_viz', 130 | executable='mocap_marker_viz', 131 | output='both', 132 | emulate_tty=True, 133 | parameters=[{'mocap_system': 'test'}], 134 | ) 135 | 136 | # Create the launch description and populate 137 | ld = LaunchDescription() 138 | 139 | # Declare the launch options 140 | ld.add_action(declare_namespace_cmd) 141 | ld.add_action(declare_use_namespace_cmd) 142 | ld.add_action(declare_slam_cmd) 143 | ld.add_action(declare_map_yaml_cmd) 144 | ld.add_action(declare_use_sim_time_cmd) 145 | ld.add_action(declare_params_file_cmd) 146 | ld.add_action(declare_autostart_cmd) 147 | ld.add_action(declare_use_composition_cmd) 148 | 149 | ld.add_action(declare_rviz_config_file_cmd) 150 | ld.add_action(declare_use_rviz_cmd) 151 | ld.add_action(declare_use_respawn_cmd) 152 | 153 | ld.add_action(start_mocap_marker_rviz) 154 | 155 | # Add the actions to launch all of the navigation nodes 156 | ld.add_action(rviz_cmd) 157 | ld.add_action(bringup_cmd) 158 | 159 | return ld 160 | -------------------------------------------------------------------------------- /maps/home.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/home.pgm -------------------------------------------------------------------------------- /maps/home.yaml: -------------------------------------------------------------------------------- 1 | image: home.pgm 2 | mode: trinary 3 | resolution: 0.1 4 | origin: [-12, -15, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | # elevation layer 9 | elevation_image: home_ele.pgm 10 | min_height: -0.01 11 | max_height: 0 12 | # octomap 13 | octomap_file: home_octo.ot 14 | binary: false -------------------------------------------------------------------------------- /maps/home_ele.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/home_ele.pgm -------------------------------------------------------------------------------- /maps/home_octo.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/home_octo.ot -------------------------------------------------------------------------------- /maps/house_uneven.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/house_uneven.pgm -------------------------------------------------------------------------------- /maps/house_uneven.yaml: -------------------------------------------------------------------------------- 1 | image: house_uneven.pgm 2 | mode: trinary 3 | resolution: 0.1 4 | origin: [-15, -26, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | # elevation layer 9 | elevation_image: house_uneven_ele.pgm 10 | min_height: -2.69 11 | max_height: 1.58 12 | # octomap 13 | octomap_file: house_uneven_octo.ot 14 | binary: false -------------------------------------------------------------------------------- /maps/house_uneven_ele.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/house_uneven_ele.pgm -------------------------------------------------------------------------------- /maps/house_uneven_octo.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/house_uneven_octo.ot -------------------------------------------------------------------------------- /maps/lab.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/lab.pgm -------------------------------------------------------------------------------- /maps/lab.yaml: -------------------------------------------------------------------------------- 1 | image: lab.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-7.09, -4.64, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /maps/lab_3d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/lab_3d.pgm -------------------------------------------------------------------------------- /maps/lab_3d.yaml: -------------------------------------------------------------------------------- 1 | image: lab_3d.pgm 2 | mode: trinary 3 | resolution: 0.1 4 | origin: [-5.3, -5.2, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | # elevation layer 9 | elevation_image: lab_3d_ele.pgm 10 | min_height: -1.45 11 | max_height: 0.25 12 | # octomap 13 | octomap_file: lab_3d_octo.ot 14 | binary: false -------------------------------------------------------------------------------- /maps/lab_3d_ele.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/lab_3d_ele.pgm -------------------------------------------------------------------------------- /maps/lab_3d_octo.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/lab_3d_octo.ot -------------------------------------------------------------------------------- /maps/simple_uneven_house.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/simple_uneven_house.pgm -------------------------------------------------------------------------------- /maps/simple_uneven_house.yaml: -------------------------------------------------------------------------------- 1 | image: simple_uneven_house.pgm 2 | mode: trinary 3 | resolution: 0.1 4 | origin: [-39, -39, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | # elevation layer 9 | elevation_image: simple_uneven_house_ele.pgm 10 | min_height: -2.61 11 | max_height: 3.09 12 | # octomap 13 | octomap_file: simple_uneven_house_octo.ot 14 | binary: false -------------------------------------------------------------------------------- /maps/simple_uneven_house_ele.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/simple_uneven_house_ele.pgm -------------------------------------------------------------------------------- /maps/simple_uneven_house_octo.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/simple_uneven_house_octo.ot -------------------------------------------------------------------------------- /maps/turtlebot3_world.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/turtlebot3_world.pgm -------------------------------------------------------------------------------- /maps/turtlebot3_world.yaml: -------------------------------------------------------------------------------- 1 | image: turtlebot3_world.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/uneven_floor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/uneven_floor.pgm -------------------------------------------------------------------------------- /maps/uneven_floor.yaml: -------------------------------------------------------------------------------- 1 | image: uneven_floor.pgm 2 | mode: trinary 3 | resolution: 0.1 4 | origin: [-9.5, -8.5, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | # elevation layer 9 | elevation_image: uneven_floor_ele.pgm 10 | min_height: -0.492 11 | max_height: 2.67 12 | # octomap 13 | octomap_file: uneven_floor_octo.ot 14 | binary: false -------------------------------------------------------------------------------- /maps/uneven_floor_ele.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/uneven_floor_ele.pgm -------------------------------------------------------------------------------- /maps/uneven_floor_octo.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/maps/uneven_floor_octo.ot -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mh_amcl 5 | 0.1.0 6 | Multi-Hypothesys AMCL 7 | Francisco Martín 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_lifecycle 14 | nav2_costmap_2d 15 | sensor_msgs 16 | visualization_msgs 17 | tf2_geometry_msgs 18 | geometry_msgs 19 | tf2_ros 20 | tf2 21 | nav2_util 22 | nav2_msgs 23 | grid_map_msgs 24 | grid_map_ros 25 | octomap_msgs 26 | octomap_ros 27 | pcl_conversions 28 | pcl_ros 29 | mh_amcl_msgs 30 | 31 | ament_lint_common 32 | ament_lint_auto 33 | ament_cmake_gtest 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /params/nav2_params_kobuki.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | mh_amcl: 43 | ros__parameters: 44 | use_sim_time: True 45 | max_particles: 200 46 | min_particles: 30 47 | particles_step: 30 48 | init_pos_x: 0.0 49 | init_pos_y: 0.0 50 | init_pos_yaw: 0.0 51 | init_error_x: 0.1 52 | init_error_y: 0.1 53 | init_error_yaw: 0.05 54 | translation_noise: 0.1 55 | rotation_noise: 0.1 56 | reseed_percentage_losers: 0.9 57 | reseed_percentage_winners: 0.03 58 | multihypothesis: False 59 | max_hypotheses: 5 60 | min_candidate_weight: 0.5 61 | min_candidate_distance: 1.0 62 | min_candidate_angle: 1.57 63 | low_q_hypo_thereshold: 0.25 64 | very_low_q_hypo_thereshold: 0.10 65 | hypo_merge_distance: 0.3 66 | hypo_merge_angle: 0.5 67 | good_hypo_thereshold: 0.5 68 | min_hypo_diff_winner: 0.3 69 | matchers: ['matcher_0'] 70 | matcher_0: 71 | type: matcher2d 72 | topic: /scan 73 | correction_sources: ['pc_0'] 74 | laser_0: 75 | type: laser 76 | topic: /scan 77 | distance_perception_error: 0.05 78 | pc_0: 79 | type: pointcloud 80 | topic: /velodyne_points 81 | distance_perception_error: 0.1 82 | max_perception_distance: 10.0 83 | point_step: 50 84 | 85 | bt_navigator: 86 | ros__parameters: 87 | use_sim_time: True 88 | global_frame: map 89 | robot_base_frame: base_link 90 | odom_topic: /odom 91 | bt_loop_duration: 10 92 | default_server_timeout: 20 93 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 94 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 95 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 96 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 97 | plugin_lib_names: 98 | - nav2_compute_path_to_pose_action_bt_node 99 | - nav2_compute_path_through_poses_action_bt_node 100 | - nav2_smooth_path_action_bt_node 101 | - nav2_follow_path_action_bt_node 102 | - nav2_spin_action_bt_node 103 | - nav2_wait_action_bt_node 104 | - nav2_back_up_action_bt_node 105 | - nav2_drive_on_heading_bt_node 106 | - nav2_clear_costmap_service_bt_node 107 | - nav2_is_stuck_condition_bt_node 108 | - nav2_goal_reached_condition_bt_node 109 | - nav2_goal_updated_condition_bt_node 110 | - nav2_globally_updated_goal_condition_bt_node 111 | - nav2_is_path_valid_condition_bt_node 112 | - nav2_initial_pose_received_condition_bt_node 113 | - nav2_reinitialize_global_localization_service_bt_node 114 | - nav2_rate_controller_bt_node 115 | - nav2_distance_controller_bt_node 116 | - nav2_speed_controller_bt_node 117 | - nav2_truncate_path_action_bt_node 118 | - nav2_truncate_path_local_action_bt_node 119 | - nav2_goal_updater_node_bt_node 120 | - nav2_recovery_node_bt_node 121 | - nav2_pipeline_sequence_bt_node 122 | - nav2_round_robin_node_bt_node 123 | - nav2_transform_available_condition_bt_node 124 | - nav2_time_expired_condition_bt_node 125 | - nav2_path_expiring_timer_condition 126 | - nav2_distance_traveled_condition_bt_node 127 | - nav2_single_trigger_bt_node 128 | - nav2_goal_updated_controller_bt_node 129 | - nav2_is_battery_low_condition_bt_node 130 | - nav2_navigate_through_poses_action_bt_node 131 | - nav2_navigate_to_pose_action_bt_node 132 | - nav2_remove_passed_goals_action_bt_node 133 | - nav2_planner_selector_bt_node 134 | - nav2_controller_selector_bt_node 135 | - nav2_goal_checker_selector_bt_node 136 | - nav2_controller_cancel_bt_node 137 | - nav2_path_longer_on_approach_bt_node 138 | - nav2_wait_cancel_bt_node 139 | - nav2_spin_cancel_bt_node 140 | - nav2_back_up_cancel_bt_node 141 | - nav2_drive_on_heading_cancel_bt_node 142 | 143 | bt_navigator_navigate_through_poses_rclcpp_node: 144 | ros__parameters: 145 | use_sim_time: True 146 | 147 | bt_navigator_navigate_to_pose_rclcpp_node: 148 | ros__parameters: 149 | use_sim_time: True 150 | 151 | controller_server: 152 | ros__parameters: 153 | use_sim_time: True 154 | controller_frequency: 20.0 155 | min_x_velocity_threshold: 0.001 156 | min_y_velocity_threshold: 0.5 157 | min_theta_velocity_threshold: 0.001 158 | failure_tolerance: 0.3 159 | progress_checker_plugin: "progress_checker" 160 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 161 | controller_plugins: ["FollowPath"] 162 | 163 | # Progress checker parameters 164 | progress_checker: 165 | plugin: "nav2_controller::SimpleProgressChecker" 166 | required_movement_radius: 0.5 167 | movement_time_allowance: 10.0 168 | # Goal checker parameters 169 | #precise_goal_checker: 170 | # plugin: "nav2_controller::SimpleGoalChecker" 171 | # xy_goal_tolerance: 0.25 172 | # yaw_goal_tolerance: 0.25 173 | # stateful: True 174 | general_goal_checker: 175 | stateful: True 176 | plugin: "nav2_controller::SimpleGoalChecker" 177 | xy_goal_tolerance: 0.25 178 | yaw_goal_tolerance: 0.25 179 | # DWB parameters 180 | FollowPath: 181 | plugin: "dwb_core::DWBLocalPlanner" 182 | debug_trajectory_details: True 183 | min_vel_x: 0.0 184 | min_vel_y: 0.0 185 | max_vel_x: 0.5 186 | max_vel_y: 0.0 187 | max_vel_theta: 1.0 188 | min_speed_xy: 0.0 189 | max_speed_xy: 0.5 190 | min_speed_theta: 0.0 191 | # Add high threshold velocity for turtlebot 3 issue. 192 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 193 | acc_lim_x: 2.5 194 | acc_lim_y: 0.0 195 | acc_lim_theta: 3.2 196 | decel_lim_x: -2.5 197 | decel_lim_y: 0.0 198 | decel_lim_theta: -3.2 199 | vx_samples: 20 200 | vy_samples: 5 201 | vtheta_samples: 20 202 | sim_time: 1.7 203 | linear_granularity: 0.05 204 | angular_granularity: 0.025 205 | transform_tolerance: 0.2 206 | xy_goal_tolerance: 0.25 207 | trans_stopped_velocity: 0.25 208 | short_circuit_trajectory_evaluation: True 209 | stateful: True 210 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 211 | BaseObstacle.scale: 0.02 212 | PathAlign.scale: 32.0 213 | PathAlign.forward_point_distance: 0.1 214 | GoalAlign.scale: 24.0 215 | GoalAlign.forward_point_distance: 0.1 216 | PathDist.scale: 32.0 217 | GoalDist.scale: 24.0 218 | RotateToGoal.scale: 32.0 219 | RotateToGoal.slowing_factor: 5.0 220 | RotateToGoal.lookahead_time: -1.0 221 | 222 | local_costmap: 223 | local_costmap: 224 | ros__parameters: 225 | update_frequency: 5.0 226 | publish_frequency: 2.0 227 | global_frame: odom 228 | robot_base_frame: base_link 229 | use_sim_time: True 230 | rolling_window: true 231 | width: 3 232 | height: 3 233 | resolution: 0.05 234 | robot_radius: 0.22 235 | plugins: ["voxel_layer", "inflation_layer"] 236 | inflation_layer: 237 | plugin: "nav2_costmap_2d::InflationLayer" 238 | cost_scaling_factor: 3.0 239 | inflation_radius: 0.55 240 | voxel_layer: 241 | plugin: "nav2_costmap_2d::VoxelLayer" 242 | enabled: True 243 | publish_voxel_map: True 244 | origin_z: 0.0 245 | z_resolution: 0.05 246 | z_voxels: 16 247 | max_obstacle_height: 2.0 248 | mark_threshold: 0 249 | observation_sources: scan 250 | scan: 251 | topic: /scan 252 | max_obstacle_height: 2.0 253 | clearing: True 254 | marking: True 255 | data_type: "LaserScan" 256 | raytrace_max_range: 3.0 257 | raytrace_min_range: 0.0 258 | obstacle_max_range: 2.5 259 | obstacle_min_range: 0.0 260 | static_layer: 261 | plugin: "nav2_costmap_2d::StaticLayer" 262 | map_subscribe_transient_local: True 263 | always_send_full_costmap: True 264 | 265 | global_costmap: 266 | global_costmap: 267 | ros__parameters: 268 | update_frequency: 1.0 269 | publish_frequency: 1.0 270 | global_frame: map 271 | robot_base_frame: base_link 272 | use_sim_time: True 273 | robot_radius: 0.22 274 | resolution: 0.05 275 | track_unknown_space: true 276 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 277 | obstacle_layer: 278 | plugin: "nav2_costmap_2d::ObstacleLayer" 279 | enabled: True 280 | observation_sources: scan 281 | scan: 282 | topic: /scan 283 | max_obstacle_height: 2.0 284 | clearing: True 285 | marking: True 286 | data_type: "LaserScan" 287 | raytrace_max_range: 3.0 288 | raytrace_min_range: 0.0 289 | obstacle_max_range: 2.5 290 | obstacle_min_range: 0.0 291 | static_layer: 292 | plugin: "nav2_costmap_2d::StaticLayer" 293 | map_subscribe_transient_local: True 294 | inflation_layer: 295 | plugin: "nav2_costmap_2d::InflationLayer" 296 | cost_scaling_factor: 3.0 297 | inflation_radius: 0.55 298 | always_send_full_costmap: True 299 | 300 | map_server: 301 | ros__parameters: 302 | use_sim_time: True 303 | yaml_filename: "turtlebot3_world.yaml" 304 | 305 | map_saver: 306 | ros__parameters: 307 | use_sim_time: True 308 | save_map_timeout: 5.0 309 | free_thresh_default: 0.25 310 | occupied_thresh_default: 0.65 311 | map_subscribe_transient_local: True 312 | 313 | planner_server: 314 | ros__parameters: 315 | expected_planner_frequency: 20.0 316 | use_sim_time: True 317 | planner_plugins: ["GridBased"] 318 | GridBased: 319 | plugin: "nav2_navfn_planner/NavfnPlanner" 320 | tolerance: 0.5 321 | use_astar: false 322 | allow_unknown: true 323 | 324 | smoother_server: 325 | ros__parameters: 326 | use_sim_time: True 327 | smoother_plugins: ["simple_smoother"] 328 | simple_smoother: 329 | plugin: "nav2_smoother::SimpleSmoother" 330 | tolerance: 1.0e-10 331 | max_its: 1000 332 | do_refinement: True 333 | 334 | behavior_server: 335 | ros__parameters: 336 | costmap_topic: local_costmap/costmap_raw 337 | footprint_topic: local_costmap/published_footprint 338 | cycle_frequency: 10.0 339 | behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] 340 | spin: 341 | plugin: "nav2_behaviors/Spin" 342 | backup: 343 | plugin: "nav2_behaviors/BackUp" 344 | drive_on_heading: 345 | plugin: "nav2_behaviors/DriveOnHeading" 346 | wait: 347 | plugin: "nav2_behaviors/Wait" 348 | global_frame: odom 349 | robot_base_frame: base_link 350 | transform_tolerance: 0.1 351 | use_sim_time: true 352 | simulate_ahead_time: 2.0 353 | max_rotational_vel: 1.0 354 | min_rotational_vel: 0.4 355 | rotational_acc_lim: 3.2 356 | 357 | robot_state_publisher: 358 | ros__parameters: 359 | use_sim_time: True 360 | 361 | waypoint_follower: 362 | ros__parameters: 363 | use_sim_time: True 364 | loop_rate: 20 365 | stop_on_failure: false 366 | waypoint_task_executor_plugin: "wait_at_waypoint" 367 | wait_at_waypoint: 368 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 369 | enabled: True 370 | waypoint_pause_duration: 200 371 | 372 | velocity_smoother: 373 | ros__parameters: 374 | use_sim_time: True 375 | smoothing_frequency: 20.0 376 | scale_velocities: False 377 | feedback: "OPEN_LOOP" 378 | max_velocity: [0.5, 0.0, 1.0] 379 | min_velocity: [-0.5, 0.0, -1.0] 380 | max_accel: [2.5, 0.0, 3.2] 381 | max_decel: [-2.5, 0.0, -3.2] 382 | odom_topic: "odom" 383 | odom_duration: 0.1 384 | deadband_velocity: [0.0, 0.0, 0.0] 385 | velocity_timeout: 1.0 386 | -------------------------------------------------------------------------------- /params/nav2_params_summit.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | mh_amcl: 43 | ros__parameters: 44 | use_sim_time: True 45 | max_particles: 200 46 | min_particles: 60 47 | particles_step: 30 48 | init_pos_x: 0.0 49 | init_pos_y: 0.0 50 | init_pos_z: 0.3 51 | init_pos_yaw: 0.0 52 | init_pos_pitch: 0.0 53 | init_pos_roll: 0.0 54 | init_error_x: 0.1 55 | init_error_y: 0.1 56 | init_error_z: 0.1 57 | init_error_yaw: 0.02 58 | init_error_pitch: 0.01 59 | init_error_roll: 0.01 60 | translation_noise: 0.1 61 | rotation_noise: 0.1 62 | reseed_percentage_losers: 0.9 63 | reseed_percentage_winners: 0.03 64 | multihypothesis: False 65 | max_hypotheses: 5 66 | min_candidate_weight: 0.5 67 | min_candidate_distance: 1.0 68 | min_candidate_angle: 1.57 69 | low_q_hypo_thereshold: 0.25 70 | very_low_q_hypo_thereshold: 0.10 71 | hypo_merge_distance: 0.3 72 | hypo_merge_angle: 0.5 73 | good_hypo_thereshold: 0.5 74 | min_hypo_diff_winner: 0.3 75 | matchers: ['matcher_0'] 76 | matcher_0: 77 | type: matcher2d 78 | topic: /front_laser/scan 79 | correction_sources: ['front_laser', 'pc_0'] 80 | front_laser: 81 | type: laser 82 | topic: /front_laser/scan 83 | distance_perception_error: 0.15 84 | debug: true 85 | rear_laser: 86 | type: laser 87 | topic: /rear_laser/scan 88 | distance_perception_error: 0.15 89 | pc_0: 90 | type: pointcloud 91 | topic: /velodyne_points 92 | distance_perception_error: 0.15 93 | max_perception_distance: 10.0 94 | point_step: 50 95 | 96 | bt_navigator: 97 | ros__parameters: 98 | use_sim_time: True 99 | global_frame: map 100 | robot_base_frame: base_link 101 | odom_topic: /odom 102 | bt_loop_duration: 10 103 | default_server_timeout: 20 104 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 105 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 106 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 107 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 108 | plugin_lib_names: 109 | - nav2_compute_path_to_pose_action_bt_node 110 | - nav2_compute_path_through_poses_action_bt_node 111 | - nav2_smooth_path_action_bt_node 112 | - nav2_follow_path_action_bt_node 113 | - nav2_spin_action_bt_node 114 | - nav2_wait_action_bt_node 115 | - nav2_back_up_action_bt_node 116 | - nav2_drive_on_heading_bt_node 117 | - nav2_clear_costmap_service_bt_node 118 | - nav2_is_stuck_condition_bt_node 119 | - nav2_goal_reached_condition_bt_node 120 | - nav2_goal_updated_condition_bt_node 121 | - nav2_globally_updated_goal_condition_bt_node 122 | - nav2_is_path_valid_condition_bt_node 123 | - nav2_initial_pose_received_condition_bt_node 124 | - nav2_reinitialize_global_localization_service_bt_node 125 | - nav2_rate_controller_bt_node 126 | - nav2_distance_controller_bt_node 127 | - nav2_speed_controller_bt_node 128 | - nav2_truncate_path_action_bt_node 129 | - nav2_truncate_path_local_action_bt_node 130 | - nav2_goal_updater_node_bt_node 131 | - nav2_recovery_node_bt_node 132 | - nav2_pipeline_sequence_bt_node 133 | - nav2_round_robin_node_bt_node 134 | - nav2_transform_available_condition_bt_node 135 | - nav2_time_expired_condition_bt_node 136 | - nav2_path_expiring_timer_condition 137 | - nav2_distance_traveled_condition_bt_node 138 | - nav2_single_trigger_bt_node 139 | - nav2_goal_updated_controller_bt_node 140 | - nav2_is_battery_low_condition_bt_node 141 | - nav2_navigate_through_poses_action_bt_node 142 | - nav2_navigate_to_pose_action_bt_node 143 | - nav2_remove_passed_goals_action_bt_node 144 | - nav2_planner_selector_bt_node 145 | - nav2_controller_selector_bt_node 146 | - nav2_goal_checker_selector_bt_node 147 | - nav2_controller_cancel_bt_node 148 | - nav2_path_longer_on_approach_bt_node 149 | - nav2_wait_cancel_bt_node 150 | - nav2_spin_cancel_bt_node 151 | - nav2_back_up_cancel_bt_node 152 | - nav2_drive_on_heading_cancel_bt_node 153 | 154 | bt_navigator_navigate_through_poses_rclcpp_node: 155 | ros__parameters: 156 | use_sim_time: True 157 | 158 | bt_navigator_navigate_to_pose_rclcpp_node: 159 | ros__parameters: 160 | use_sim_time: True 161 | 162 | controller_server: 163 | ros__parameters: 164 | use_sim_time: True 165 | controller_frequency: 20.0 166 | min_x_velocity_threshold: 0.001 167 | min_y_velocity_threshold: 0.5 168 | min_theta_velocity_threshold: 0.001 169 | failure_tolerance: 0.3 170 | progress_checker_plugin: "progress_checker" 171 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 172 | controller_plugins: ["FollowPath"] 173 | 174 | # Progress checker parameters 175 | progress_checker: 176 | plugin: "nav2_controller::SimpleProgressChecker" 177 | required_movement_radius: 0.5 178 | movement_time_allowance: 10.0 179 | # Goal checker parameters 180 | #precise_goal_checker: 181 | # plugin: "nav2_controller::SimpleGoalChecker" 182 | # xy_goal_tolerance: 0.25 183 | # yaw_goal_tolerance: 0.25 184 | # stateful: True 185 | general_goal_checker: 186 | stateful: True 187 | plugin: "nav2_controller::SimpleGoalChecker" 188 | xy_goal_tolerance: 0.25 189 | yaw_goal_tolerance: 0.25 190 | # DWB parameters 191 | FollowPath: 192 | plugin: "dwb_core::DWBLocalPlanner" 193 | debug_trajectory_details: True 194 | min_vel_x: 0.0 195 | min_vel_y: 0.0 196 | max_vel_x: 0.5 197 | max_vel_y: 0.0 198 | max_vel_theta: 1.0 199 | min_speed_xy: 0.0 200 | max_speed_xy: 0.5 201 | min_speed_theta: 0.0 202 | # Add high threshold velocity for turtlebot 3 issue. 203 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 204 | acc_lim_x: 2.5 205 | acc_lim_y: 0.0 206 | acc_lim_theta: 3.2 207 | decel_lim_x: -2.5 208 | decel_lim_y: 0.0 209 | decel_lim_theta: -3.2 210 | vx_samples: 20 211 | vy_samples: 5 212 | vtheta_samples: 20 213 | sim_time: 1.7 214 | linear_granularity: 0.05 215 | angular_granularity: 0.025 216 | transform_tolerance: 0.2 217 | xy_goal_tolerance: 0.25 218 | trans_stopped_velocity: 0.25 219 | short_circuit_trajectory_evaluation: True 220 | stateful: True 221 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 222 | BaseObstacle.scale: 0.02 223 | PathAlign.scale: 32.0 224 | PathAlign.forward_point_distance: 0.1 225 | GoalAlign.scale: 24.0 226 | GoalAlign.forward_point_distance: 0.1 227 | PathDist.scale: 32.0 228 | GoalDist.scale: 24.0 229 | RotateToGoal.scale: 32.0 230 | RotateToGoal.slowing_factor: 5.0 231 | RotateToGoal.lookahead_time: -1.0 232 | 233 | local_costmap: 234 | local_costmap: 235 | ros__parameters: 236 | update_frequency: 5.0 237 | publish_frequency: 2.0 238 | global_frame: odom 239 | robot_base_frame: base_link 240 | use_sim_time: True 241 | rolling_window: true 242 | width: 3 243 | height: 3 244 | resolution: 0.05 245 | robot_radius: 0.22 246 | plugins: ["inflation_layer"] 247 | inflation_layer: 248 | plugin: "nav2_costmap_2d::InflationLayer" 249 | cost_scaling_factor: 3.0 250 | inflation_radius: 0.55 251 | voxel_layer: 252 | plugin: "nav2_costmap_2d::VoxelLayer" 253 | enabled: True 254 | publish_voxel_map: True 255 | origin_z: 0.0 256 | z_resolution: 0.05 257 | z_voxels: 16 258 | max_obstacle_height: 2.0 259 | mark_threshold: 0 260 | observation_sources: front_scan 261 | # rear_scan: 262 | # topic: /rear_scan/scan 263 | # max_obstacle_height: 2.0 264 | # clearing: True 265 | # marking: True 266 | # data_type: "LaserScan" 267 | # raytrace_max_range: 3.0 268 | # raytrace_min_range: 0.0 269 | # obstacle_max_range: 2.5 270 | # obstacle_min_range: 0.0 271 | front_scan: 272 | topic: /front_laser/scan 273 | max_obstacle_height: 2.0 274 | clearing: True 275 | marking: True 276 | data_type: "LaserScan" 277 | raytrace_max_range: 3.0 278 | raytrace_min_range: 0.0 279 | obstacle_max_range: 2.5 280 | obstacle_min_range: 0.0 281 | static_layer: 282 | plugin: "nav2_costmap_2d::StaticLayer" 283 | map_subscribe_transient_local: True 284 | always_send_full_costmap: True 285 | 286 | global_costmap: 287 | global_costmap: 288 | ros__parameters: 289 | update_frequency: 1.0 290 | publish_frequency: 1.0 291 | global_frame: map 292 | robot_base_frame: base_link 293 | use_sim_time: True 294 | robot_radius: 0.22 295 | resolution: 0.05 296 | track_unknown_space: true 297 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 298 | obstacle_layer: 299 | plugin: "non_planar_obstacle_layer/NonPlanarObstacleLayer" 300 | enabled: True 301 | gridmap_topic: /grid_map_map 302 | min_z: -20.0 303 | max_z: 20.0 304 | min_obstacle_height: 0.4 305 | max_obstacle_height: 2.0 306 | observation_sources: ["front_scan"] 307 | front_scan: 308 | topic: "/front_laser/scan" 309 | type: "LaserScan" 310 | static_layer: 311 | plugin: "nav2_costmap_2d::StaticLayer" 312 | map_subscribe_transient_local: True 313 | inflation_layer: 314 | plugin: "nav2_costmap_2d::InflationLayer" 315 | cost_scaling_factor: 3.0 316 | inflation_radius: 0.55 317 | always_send_full_costmap: True 318 | 319 | map_server: 320 | ros__parameters: 321 | use_sim_time: True 322 | yaml_filename: "turtlebot3_world.yaml" 323 | 324 | map_saver: 325 | ros__parameters: 326 | use_sim_time: True 327 | save_map_timeout: 5.0 328 | free_thresh_default: 0.25 329 | occupied_thresh_default: 0.65 330 | map_subscribe_transient_local: True 331 | 332 | planner_server: 333 | ros__parameters: 334 | expected_planner_frequency: 20.0 335 | use_sim_time: True 336 | planner_plugins: ["GridBased"] 337 | GridBased: 338 | plugin: "nav2_navfn_planner/NavfnPlanner" 339 | tolerance: 0.5 340 | use_astar: false 341 | allow_unknown: true 342 | 343 | smoother_server: 344 | ros__parameters: 345 | use_sim_time: True 346 | smoother_plugins: ["simple_smoother"] 347 | simple_smoother: 348 | plugin: "nav2_smoother::SimpleSmoother" 349 | tolerance: 1.0e-10 350 | max_its: 1000 351 | do_refinement: True 352 | 353 | behavior_server: 354 | ros__parameters: 355 | costmap_topic: local_costmap/costmap_raw 356 | footprint_topic: local_costmap/published_footprint 357 | cycle_frequency: 10.0 358 | behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] 359 | spin: 360 | plugin: "nav2_behaviors/Spin" 361 | backup: 362 | plugin: "nav2_behaviors/BackUp" 363 | drive_on_heading: 364 | plugin: "nav2_behaviors/DriveOnHeading" 365 | wait: 366 | plugin: "nav2_behaviors/Wait" 367 | global_frame: odom 368 | robot_base_frame: base_link 369 | transform_tolerance: 0.1 370 | use_sim_time: true 371 | simulate_ahead_time: 2.0 372 | max_rotational_vel: 1.0 373 | min_rotational_vel: 0.4 374 | rotational_acc_lim: 3.2 375 | 376 | robot_state_publisher: 377 | ros__parameters: 378 | use_sim_time: True 379 | 380 | waypoint_follower: 381 | ros__parameters: 382 | use_sim_time: True 383 | loop_rate: 20 384 | stop_on_failure: false 385 | waypoint_task_executor_plugin: "wait_at_waypoint" 386 | wait_at_waypoint: 387 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 388 | enabled: True 389 | waypoint_pause_duration: 200 390 | 391 | velocity_smoother: 392 | ros__parameters: 393 | use_sim_time: True 394 | smoothing_frequency: 20.0 395 | scale_velocities: False 396 | feedback: "OPEN_LOOP" 397 | max_velocity: [0.5, 0.0, 1.0] 398 | min_velocity: [-0.5, 0.0, -1.0] 399 | max_accel: [2.5, 0.0, 3.2] 400 | max_decel: [-2.5, 0.0, -3.2] 401 | odom_topic: "odom" 402 | odom_duration: 0.1 403 | deadband_velocity: [0.0, 0.0, 0.0] 404 | velocity_timeout: 1.0 405 | -------------------------------------------------------------------------------- /params/nav2_params_summit_lab.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | mh_amcl: 43 | ros__parameters: 44 | use_sim_time: True 45 | max_particles: 200 46 | min_particles: 60 47 | particles_step: 30 48 | init_pos_x: 0.0 49 | init_pos_y: 0.0 50 | init_pos_z: 0.3 51 | init_pos_yaw: 0.0 52 | init_pos_pitch: 0.0 53 | init_pos_roll: 0.0 54 | init_error_x: 0.1 55 | init_error_y: 0.1 56 | init_error_z: 0.1 57 | init_error_yaw: 0.02 58 | init_error_pitch: 0.01 59 | init_error_roll: 0.01 60 | translation_noise: 0.05 61 | rotation_noise: 0.05 62 | reseed_percentage_losers: 0.9 63 | reseed_percentage_winners: 0.03 64 | multihypothesis: False 65 | max_hypotheses: 5 66 | min_candidate_weight: 0.5 67 | min_candidate_distance: 1.0 68 | min_candidate_angle: 1.57 69 | low_q_hypo_thereshold: 0.25 70 | very_low_q_hypo_thereshold: 0.10 71 | hypo_merge_distance: 0.3 72 | hypo_merge_angle: 0.5 73 | good_hypo_thereshold: 0.5 74 | min_hypo_diff_winner: 0.3 75 | matchers: ['matcher_0'] 76 | matcher_0: 77 | type: matcher2d 78 | topic: /front_laser/scan 79 | correction_sources: ['front_laser', 'pc_0'] 80 | front_laser: 81 | type: laser 82 | topic: /front_laser/scan 83 | distance_perception_error: 0.2 84 | debug: true 85 | rear_laser: 86 | type: laser 87 | topic: /rear_laser/scan 88 | distance_perception_error: 0.2 89 | pc_0: 90 | type: pointcloud 91 | topic: /velodyne_points 92 | distance_perception_error: 0.2 93 | max_perception_distance: 10.0 94 | point_step: 50 95 | 96 | bt_navigator: 97 | ros__parameters: 98 | use_sim_time: True 99 | global_frame: map 100 | robot_base_frame: base_link 101 | odom_topic: /odom 102 | bt_loop_duration: 10 103 | default_server_timeout: 20 104 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 105 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 106 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 107 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 108 | plugin_lib_names: 109 | - nav2_compute_path_to_pose_action_bt_node 110 | - nav2_compute_path_through_poses_action_bt_node 111 | - nav2_smooth_path_action_bt_node 112 | - nav2_follow_path_action_bt_node 113 | - nav2_spin_action_bt_node 114 | - nav2_wait_action_bt_node 115 | - nav2_back_up_action_bt_node 116 | - nav2_drive_on_heading_bt_node 117 | - nav2_clear_costmap_service_bt_node 118 | - nav2_is_stuck_condition_bt_node 119 | - nav2_goal_reached_condition_bt_node 120 | - nav2_goal_updated_condition_bt_node 121 | - nav2_globally_updated_goal_condition_bt_node 122 | - nav2_is_path_valid_condition_bt_node 123 | - nav2_initial_pose_received_condition_bt_node 124 | - nav2_reinitialize_global_localization_service_bt_node 125 | - nav2_rate_controller_bt_node 126 | - nav2_distance_controller_bt_node 127 | - nav2_speed_controller_bt_node 128 | - nav2_truncate_path_action_bt_node 129 | - nav2_truncate_path_local_action_bt_node 130 | - nav2_goal_updater_node_bt_node 131 | - nav2_recovery_node_bt_node 132 | - nav2_pipeline_sequence_bt_node 133 | - nav2_round_robin_node_bt_node 134 | - nav2_transform_available_condition_bt_node 135 | - nav2_time_expired_condition_bt_node 136 | - nav2_path_expiring_timer_condition 137 | - nav2_distance_traveled_condition_bt_node 138 | - nav2_single_trigger_bt_node 139 | - nav2_goal_updated_controller_bt_node 140 | - nav2_is_battery_low_condition_bt_node 141 | - nav2_navigate_through_poses_action_bt_node 142 | - nav2_navigate_to_pose_action_bt_node 143 | - nav2_remove_passed_goals_action_bt_node 144 | - nav2_planner_selector_bt_node 145 | - nav2_controller_selector_bt_node 146 | - nav2_goal_checker_selector_bt_node 147 | - nav2_controller_cancel_bt_node 148 | - nav2_path_longer_on_approach_bt_node 149 | - nav2_wait_cancel_bt_node 150 | - nav2_spin_cancel_bt_node 151 | - nav2_back_up_cancel_bt_node 152 | - nav2_drive_on_heading_cancel_bt_node 153 | 154 | bt_navigator_navigate_through_poses_rclcpp_node: 155 | ros__parameters: 156 | use_sim_time: True 157 | 158 | bt_navigator_navigate_to_pose_rclcpp_node: 159 | ros__parameters: 160 | use_sim_time: True 161 | 162 | controller_server: 163 | ros__parameters: 164 | use_sim_time: True 165 | controller_frequency: 20.0 166 | min_x_velocity_threshold: 0.001 167 | min_y_velocity_threshold: 0.5 168 | min_theta_velocity_threshold: 0.001 169 | failure_tolerance: 0.3 170 | progress_checker_plugin: "progress_checker" 171 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 172 | controller_plugins: ["FollowPath"] 173 | 174 | # Progress checker parameters 175 | progress_checker: 176 | plugin: "nav2_controller::SimpleProgressChecker" 177 | required_movement_radius: 0.5 178 | movement_time_allowance: 10.0 179 | # Goal checker parameters 180 | #precise_goal_checker: 181 | # plugin: "nav2_controller::SimpleGoalChecker" 182 | # xy_goal_tolerance: 0.25 183 | # yaw_goal_tolerance: 0.25 184 | # stateful: True 185 | general_goal_checker: 186 | stateful: True 187 | plugin: "nav2_controller::SimpleGoalChecker" 188 | xy_goal_tolerance: 0.25 189 | yaw_goal_tolerance: 0.25 190 | # DWB parameters 191 | FollowPath: 192 | plugin: "dwb_core::DWBLocalPlanner" 193 | debug_trajectory_details: True 194 | min_vel_x: 0.0 195 | min_vel_y: 0.0 196 | max_vel_x: 0.5 197 | max_vel_y: 0.0 198 | max_vel_theta: 1.0 199 | min_speed_xy: 0.0 200 | max_speed_xy: 0.5 201 | min_speed_theta: 0.0 202 | # Add high threshold velocity for turtlebot 3 issue. 203 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 204 | acc_lim_x: 2.5 205 | acc_lim_y: 0.0 206 | acc_lim_theta: 3.2 207 | decel_lim_x: -2.5 208 | decel_lim_y: 0.0 209 | decel_lim_theta: -3.2 210 | vx_samples: 20 211 | vy_samples: 5 212 | vtheta_samples: 20 213 | sim_time: 1.7 214 | linear_granularity: 0.05 215 | angular_granularity: 0.025 216 | transform_tolerance: 0.2 217 | xy_goal_tolerance: 0.25 218 | trans_stopped_velocity: 0.25 219 | short_circuit_trajectory_evaluation: True 220 | stateful: True 221 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 222 | BaseObstacle.scale: 0.02 223 | PathAlign.scale: 32.0 224 | PathAlign.forward_point_distance: 0.1 225 | GoalAlign.scale: 24.0 226 | GoalAlign.forward_point_distance: 0.1 227 | PathDist.scale: 32.0 228 | GoalDist.scale: 24.0 229 | RotateToGoal.scale: 32.0 230 | RotateToGoal.slowing_factor: 5.0 231 | RotateToGoal.lookahead_time: -1.0 232 | 233 | local_costmap: 234 | local_costmap: 235 | ros__parameters: 236 | update_frequency: 5.0 237 | publish_frequency: 2.0 238 | global_frame: odom 239 | robot_base_frame: base_link 240 | use_sim_time: True 241 | rolling_window: true 242 | width: 3 243 | height: 3 244 | resolution: 0.05 245 | robot_radius: 0.22 246 | plugins: ["inflation_layer"] 247 | inflation_layer: 248 | plugin: "nav2_costmap_2d::InflationLayer" 249 | cost_scaling_factor: 3.0 250 | inflation_radius: 0.55 251 | voxel_layer: 252 | plugin: "nav2_costmap_2d::VoxelLayer" 253 | enabled: True 254 | publish_voxel_map: True 255 | origin_z: 0.0 256 | z_resolution: 0.05 257 | z_voxels: 16 258 | max_obstacle_height: 2.0 259 | mark_threshold: 0 260 | observation_sources: front_scan 261 | # rear_scan: 262 | # topic: /rear_scan/scan 263 | # max_obstacle_height: 2.0 264 | # clearing: True 265 | # marking: True 266 | # data_type: "LaserScan" 267 | # raytrace_max_range: 3.0 268 | # raytrace_min_range: 0.0 269 | # obstacle_max_range: 2.5 270 | # obstacle_min_range: 0.0 271 | front_scan: 272 | topic: /front_laser/scan 273 | max_obstacle_height: 2.0 274 | clearing: True 275 | marking: True 276 | data_type: "LaserScan" 277 | raytrace_max_range: 3.0 278 | raytrace_min_range: 0.0 279 | obstacle_max_range: 2.5 280 | obstacle_min_range: 0.0 281 | static_layer: 282 | plugin: "nav2_costmap_2d::StaticLayer" 283 | map_subscribe_transient_local: True 284 | always_send_full_costmap: True 285 | 286 | global_costmap: 287 | global_costmap: 288 | ros__parameters: 289 | update_frequency: 1.0 290 | publish_frequency: 1.0 291 | global_frame: map 292 | robot_base_frame: base_link 293 | use_sim_time: True 294 | robot_radius: 0.22 295 | resolution: 0.05 296 | track_unknown_space: true 297 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 298 | obstacle_layer: 299 | plugin: "non_planar_obstacle_layer/NonPlanarObstacleLayer" 300 | enabled: True 301 | gridmap_topic: /grid_map_map 302 | min_z: -20.0 303 | max_z: 20.0 304 | min_obstacle_height: 0.4 305 | max_obstacle_height: 2.0 306 | observation_sources: ["front_scan"] 307 | front_scan: 308 | topic: "/front_laser/scan" 309 | type: "LaserScan" 310 | static_layer: 311 | plugin: "nav2_costmap_2d::StaticLayer" 312 | map_subscribe_transient_local: True 313 | inflation_layer: 314 | plugin: "nav2_costmap_2d::InflationLayer" 315 | cost_scaling_factor: 3.0 316 | inflation_radius: 0.55 317 | always_send_full_costmap: True 318 | 319 | map_server: 320 | ros__parameters: 321 | use_sim_time: True 322 | yaml_filename: "turtlebot3_world.yaml" 323 | 324 | map_saver: 325 | ros__parameters: 326 | use_sim_time: True 327 | save_map_timeout: 5.0 328 | free_thresh_default: 0.25 329 | occupied_thresh_default: 0.65 330 | map_subscribe_transient_local: True 331 | 332 | planner_server: 333 | ros__parameters: 334 | expected_planner_frequency: 20.0 335 | use_sim_time: True 336 | planner_plugins: ["GridBased"] 337 | GridBased: 338 | plugin: "nav2_navfn_planner/NavfnPlanner" 339 | tolerance: 0.5 340 | use_astar: false 341 | allow_unknown: true 342 | 343 | smoother_server: 344 | ros__parameters: 345 | use_sim_time: True 346 | smoother_plugins: ["simple_smoother"] 347 | simple_smoother: 348 | plugin: "nav2_smoother::SimpleSmoother" 349 | tolerance: 1.0e-10 350 | max_its: 1000 351 | do_refinement: True 352 | 353 | behavior_server: 354 | ros__parameters: 355 | costmap_topic: local_costmap/costmap_raw 356 | footprint_topic: local_costmap/published_footprint 357 | cycle_frequency: 10.0 358 | behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] 359 | spin: 360 | plugin: "nav2_behaviors/Spin" 361 | backup: 362 | plugin: "nav2_behaviors/BackUp" 363 | drive_on_heading: 364 | plugin: "nav2_behaviors/DriveOnHeading" 365 | wait: 366 | plugin: "nav2_behaviors/Wait" 367 | global_frame: odom 368 | robot_base_frame: base_link 369 | transform_tolerance: 0.1 370 | use_sim_time: true 371 | simulate_ahead_time: 2.0 372 | max_rotational_vel: 1.0 373 | min_rotational_vel: 0.4 374 | rotational_acc_lim: 3.2 375 | 376 | robot_state_publisher: 377 | ros__parameters: 378 | use_sim_time: True 379 | 380 | waypoint_follower: 381 | ros__parameters: 382 | use_sim_time: True 383 | loop_rate: 20 384 | stop_on_failure: false 385 | waypoint_task_executor_plugin: "wait_at_waypoint" 386 | wait_at_waypoint: 387 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 388 | enabled: True 389 | waypoint_pause_duration: 200 390 | 391 | velocity_smoother: 392 | ros__parameters: 393 | use_sim_time: True 394 | smoothing_frequency: 20.0 395 | scale_velocities: False 396 | feedback: "OPEN_LOOP" 397 | max_velocity: [0.5, 0.0, 1.0] 398 | min_velocity: [-0.5, 0.0, -1.0] 399 | max_accel: [2.5, 0.0, 3.2] 400 | max_decel: [-2.5, 0.0, -3.2] 401 | odom_topic: "odom" 402 | odom_duration: 0.1 403 | deadband_velocity: [0.0, 0.0, 0.0] 404 | velocity_timeout: 1.0 405 | -------------------------------------------------------------------------------- /params/nav2_params_tb3_sim.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | mh_amcl: 43 | ros__parameters: 44 | use_sim_time: True 45 | max_particles: 200 46 | min_particles: 30 47 | init_pos_x: -2.0 48 | init_pos_y: -0.3 49 | init_pos_yaw: 0.0 50 | init_error_x: 0.1 51 | init_error_y: 0.1 52 | init_error_yaw: 0.05 53 | translation_noise: 0.1 54 | rotation_noise: 0.05 55 | distance_perception_error: 0.01 56 | reseed_percentage_losers: 0.9 57 | reseed_percentage_winners: 0.03 58 | multihypothesis: True 59 | max_hypotheses: 5 60 | 61 | bt_navigator: 62 | ros__parameters: 63 | use_sim_time: True 64 | global_frame: map 65 | robot_base_frame: base_link 66 | odom_topic: /odom 67 | bt_loop_duration: 10 68 | default_server_timeout: 20 69 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 70 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 71 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 72 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 73 | plugin_lib_names: 74 | - nav2_compute_path_to_pose_action_bt_node 75 | - nav2_compute_path_through_poses_action_bt_node 76 | - nav2_smooth_path_action_bt_node 77 | - nav2_follow_path_action_bt_node 78 | - nav2_spin_action_bt_node 79 | - nav2_wait_action_bt_node 80 | - nav2_back_up_action_bt_node 81 | - nav2_drive_on_heading_bt_node 82 | - nav2_clear_costmap_service_bt_node 83 | - nav2_is_stuck_condition_bt_node 84 | - nav2_goal_reached_condition_bt_node 85 | - nav2_goal_updated_condition_bt_node 86 | - nav2_globally_updated_goal_condition_bt_node 87 | - nav2_is_path_valid_condition_bt_node 88 | - nav2_initial_pose_received_condition_bt_node 89 | - nav2_reinitialize_global_localization_service_bt_node 90 | - nav2_rate_controller_bt_node 91 | - nav2_distance_controller_bt_node 92 | - nav2_speed_controller_bt_node 93 | - nav2_truncate_path_action_bt_node 94 | - nav2_truncate_path_local_action_bt_node 95 | - nav2_goal_updater_node_bt_node 96 | - nav2_recovery_node_bt_node 97 | - nav2_pipeline_sequence_bt_node 98 | - nav2_round_robin_node_bt_node 99 | - nav2_transform_available_condition_bt_node 100 | - nav2_time_expired_condition_bt_node 101 | - nav2_path_expiring_timer_condition 102 | - nav2_distance_traveled_condition_bt_node 103 | - nav2_single_trigger_bt_node 104 | - nav2_goal_updated_controller_bt_node 105 | - nav2_is_battery_low_condition_bt_node 106 | - nav2_navigate_through_poses_action_bt_node 107 | - nav2_navigate_to_pose_action_bt_node 108 | - nav2_remove_passed_goals_action_bt_node 109 | - nav2_planner_selector_bt_node 110 | - nav2_controller_selector_bt_node 111 | - nav2_goal_checker_selector_bt_node 112 | - nav2_controller_cancel_bt_node 113 | - nav2_path_longer_on_approach_bt_node 114 | - nav2_wait_cancel_bt_node 115 | - nav2_spin_cancel_bt_node 116 | - nav2_back_up_cancel_bt_node 117 | - nav2_drive_on_heading_cancel_bt_node 118 | 119 | bt_navigator_navigate_through_poses_rclcpp_node: 120 | ros__parameters: 121 | use_sim_time: True 122 | 123 | bt_navigator_navigate_to_pose_rclcpp_node: 124 | ros__parameters: 125 | use_sim_time: True 126 | 127 | controller_server: 128 | ros__parameters: 129 | use_sim_time: True 130 | controller_frequency: 20.0 131 | min_x_velocity_threshold: 0.001 132 | min_y_velocity_threshold: 0.5 133 | min_theta_velocity_threshold: 0.001 134 | failure_tolerance: 0.3 135 | progress_checker_plugin: "progress_checker" 136 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 137 | controller_plugins: ["FollowPath"] 138 | 139 | # Progress checker parameters 140 | progress_checker: 141 | plugin: "nav2_controller::SimpleProgressChecker" 142 | required_movement_radius: 0.5 143 | movement_time_allowance: 10.0 144 | # Goal checker parameters 145 | #precise_goal_checker: 146 | # plugin: "nav2_controller::SimpleGoalChecker" 147 | # xy_goal_tolerance: 0.25 148 | # yaw_goal_tolerance: 0.25 149 | # stateful: True 150 | general_goal_checker: 151 | stateful: True 152 | plugin: "nav2_controller::SimpleGoalChecker" 153 | xy_goal_tolerance: 0.25 154 | yaw_goal_tolerance: 0.25 155 | # DWB parameters 156 | FollowPath: 157 | plugin: "dwb_core::DWBLocalPlanner" 158 | debug_trajectory_details: True 159 | min_vel_x: 0.0 160 | min_vel_y: 0.0 161 | max_vel_x: 0.2 162 | max_vel_y: 0.0 163 | max_vel_theta: 0.6 164 | min_speed_xy: 0.0 165 | max_speed_xy: 0.26 166 | min_speed_theta: 0.0 167 | # Add high threshold velocity for turtlebot 3 issue. 168 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 169 | acc_lim_x: 2.5 170 | acc_lim_y: 0.0 171 | acc_lim_theta: 3.2 172 | decel_lim_x: -2.5 173 | decel_lim_y: 0.0 174 | decel_lim_theta: -3.2 175 | vx_samples: 20 176 | vy_samples: 5 177 | vtheta_samples: 20 178 | sim_time: 1.7 179 | linear_granularity: 0.05 180 | angular_granularity: 0.025 181 | transform_tolerance: 0.2 182 | xy_goal_tolerance: 0.25 183 | trans_stopped_velocity: 0.25 184 | short_circuit_trajectory_evaluation: True 185 | stateful: True 186 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 187 | BaseObstacle.scale: 0.02 188 | PathAlign.scale: 32.0 189 | PathAlign.forward_point_distance: 0.1 190 | GoalAlign.scale: 24.0 191 | GoalAlign.forward_point_distance: 0.1 192 | PathDist.scale: 32.0 193 | GoalDist.scale: 24.0 194 | RotateToGoal.scale: 32.0 195 | RotateToGoal.slowing_factor: 5.0 196 | RotateToGoal.lookahead_time: -1.0 197 | 198 | local_costmap: 199 | local_costmap: 200 | ros__parameters: 201 | update_frequency: 5.0 202 | publish_frequency: 2.0 203 | global_frame: odom 204 | robot_base_frame: base_link 205 | use_sim_time: True 206 | rolling_window: true 207 | width: 3 208 | height: 3 209 | resolution: 0.05 210 | robot_radius: 0.22 211 | plugins: ["voxel_layer", "inflation_layer"] 212 | inflation_layer: 213 | plugin: "nav2_costmap_2d::InflationLayer" 214 | cost_scaling_factor: 3.0 215 | inflation_radius: 0.55 216 | voxel_layer: 217 | plugin: "nav2_costmap_2d::VoxelLayer" 218 | enabled: True 219 | publish_voxel_map: True 220 | origin_z: 0.0 221 | z_resolution: 0.05 222 | z_voxels: 16 223 | max_obstacle_height: 2.0 224 | mark_threshold: 0 225 | observation_sources: scan 226 | scan: 227 | topic: /scan 228 | max_obstacle_height: 2.0 229 | clearing: True 230 | marking: True 231 | data_type: "LaserScan" 232 | raytrace_max_range: 3.0 233 | raytrace_min_range: 0.0 234 | obstacle_max_range: 2.5 235 | obstacle_min_range: 0.0 236 | static_layer: 237 | plugin: "nav2_costmap_2d::StaticLayer" 238 | map_subscribe_transient_local: True 239 | always_send_full_costmap: True 240 | 241 | global_costmap: 242 | global_costmap: 243 | ros__parameters: 244 | update_frequency: 1.0 245 | publish_frequency: 1.0 246 | global_frame: map 247 | robot_base_frame: base_link 248 | use_sim_time: True 249 | robot_radius: 0.22 250 | resolution: 0.05 251 | track_unknown_space: true 252 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 253 | obstacle_layer: 254 | plugin: "nav2_costmap_2d::ObstacleLayer" 255 | enabled: True 256 | observation_sources: scan 257 | scan: 258 | topic: /scan 259 | max_obstacle_height: 2.0 260 | clearing: True 261 | marking: True 262 | data_type: "LaserScan" 263 | raytrace_max_range: 3.0 264 | raytrace_min_range: 0.0 265 | obstacle_max_range: 2.5 266 | obstacle_min_range: 0.0 267 | static_layer: 268 | plugin: "nav2_costmap_2d::StaticLayer" 269 | map_subscribe_transient_local: True 270 | inflation_layer: 271 | plugin: "nav2_costmap_2d::InflationLayer" 272 | cost_scaling_factor: 3.0 273 | inflation_radius: 0.55 274 | always_send_full_costmap: True 275 | 276 | map_server: 277 | ros__parameters: 278 | use_sim_time: True 279 | yaml_filename: "turtlebot3_world.yaml" 280 | 281 | map_saver: 282 | ros__parameters: 283 | use_sim_time: True 284 | save_map_timeout: 5.0 285 | free_thresh_default: 0.25 286 | occupied_thresh_default: 0.65 287 | map_subscribe_transient_local: True 288 | 289 | planner_server: 290 | ros__parameters: 291 | expected_planner_frequency: 20.0 292 | use_sim_time: True 293 | planner_plugins: ["GridBased"] 294 | GridBased: 295 | plugin: "nav2_navfn_planner/NavfnPlanner" 296 | tolerance: 0.5 297 | use_astar: false 298 | allow_unknown: true 299 | 300 | smoother_server: 301 | ros__parameters: 302 | use_sim_time: True 303 | smoother_plugins: ["simple_smoother"] 304 | simple_smoother: 305 | plugin: "nav2_smoother::SimpleSmoother" 306 | tolerance: 1.0e-10 307 | max_its: 1000 308 | do_refinement: True 309 | 310 | behavior_server: 311 | ros__parameters: 312 | costmap_topic: local_costmap/costmap_raw 313 | footprint_topic: local_costmap/published_footprint 314 | cycle_frequency: 10.0 315 | behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] 316 | spin: 317 | plugin: "nav2_behaviors/Spin" 318 | backup: 319 | plugin: "nav2_behaviors/BackUp" 320 | drive_on_heading: 321 | plugin: "nav2_behaviors/DriveOnHeading" 322 | wait: 323 | plugin: "nav2_behaviors/Wait" 324 | global_frame: odom 325 | robot_base_frame: base_link 326 | transform_tolerance: 0.1 327 | use_sim_time: true 328 | simulate_ahead_time: 2.0 329 | max_rotational_vel: 1.0 330 | min_rotational_vel: 0.4 331 | rotational_acc_lim: 3.2 332 | 333 | robot_state_publisher: 334 | ros__parameters: 335 | use_sim_time: True 336 | 337 | waypoint_follower: 338 | ros__parameters: 339 | use_sim_time: True 340 | loop_rate: 20 341 | stop_on_failure: false 342 | waypoint_task_executor_plugin: "wait_at_waypoint" 343 | wait_at_waypoint: 344 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 345 | enabled: True 346 | waypoint_pause_duration: 200 347 | 348 | velocity_smoother: 349 | ros__parameters: 350 | use_sim_time: True 351 | smoothing_frequency: 20.0 352 | scale_velocities: False 353 | feedback: "OPEN_LOOP" 354 | max_velocity: [0.26, 0.0, 1.0] 355 | min_velocity: [-0.26, 0.0, -1.0] 356 | max_accel: [2.5, 0.0, 3.2] 357 | max_decel: [-2.5, 0.0, -3.2] 358 | odom_topic: "odom" 359 | odom_duration: 0.1 360 | deadband_velocity: [0.0, 0.0, 0.0] 361 | velocity_timeout: 1.0 362 | -------------------------------------------------------------------------------- /params/nav2_params_tiago.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: False 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | mh_amcl: 43 | ros__parameters: 44 | use_sim_time: False 45 | max_particles: 200 46 | min_particles: 30 47 | particles_step: 30 48 | init_pos_x: -2.0 49 | init_pos_y: 2.0 50 | init_pos_yaw: 0.0 51 | init_error_x: 0.1 52 | init_error_y: 0.1 53 | init_error_yaw: 0.05 54 | translation_noise: 0.1 55 | rotation_noise: 0.1 56 | distance_perception_error: 0.01 57 | reseed_percentage_losers: 0.9 58 | reseed_percentage_winners: 0.03 59 | multihypothesis: True 60 | max_hypotheses: 5 61 | min_candidate_weight: 0.5 62 | min_candidate_distance: 1.0 63 | min_candidate_angle: 1.57 64 | low_q_hypo_thereshold: 0.25 65 | very_low_q_hypo_thereshold: 0.10 66 | hypo_merge_distance: 0.3 67 | hypo_merge_angle: 0.5 68 | good_hypo_thereshold: 0.5 69 | min_hypo_diff_winner: 0.3 70 | 71 | bt_navigator: 72 | ros__parameters: 73 | use_sim_time: False 74 | global_frame: map 75 | robot_base_frame: base_link 76 | odom_topic: /odom 77 | bt_loop_duration: 10 78 | default_server_timeout: 20 79 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 80 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 81 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 82 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 83 | plugin_lib_names: 84 | - nav2_compute_path_to_pose_action_bt_node 85 | - nav2_compute_path_through_poses_action_bt_node 86 | - nav2_smooth_path_action_bt_node 87 | - nav2_follow_path_action_bt_node 88 | - nav2_spin_action_bt_node 89 | - nav2_wait_action_bt_node 90 | - nav2_back_up_action_bt_node 91 | - nav2_drive_on_heading_bt_node 92 | - nav2_clear_costmap_service_bt_node 93 | - nav2_is_stuck_condition_bt_node 94 | - nav2_goal_reached_condition_bt_node 95 | - nav2_goal_updated_condition_bt_node 96 | - nav2_globally_updated_goal_condition_bt_node 97 | - nav2_is_path_valid_condition_bt_node 98 | - nav2_initial_pose_received_condition_bt_node 99 | - nav2_reinitialize_global_localization_service_bt_node 100 | - nav2_rate_controller_bt_node 101 | - nav2_distance_controller_bt_node 102 | - nav2_speed_controller_bt_node 103 | - nav2_truncate_path_action_bt_node 104 | - nav2_truncate_path_local_action_bt_node 105 | - nav2_goal_updater_node_bt_node 106 | - nav2_recovery_node_bt_node 107 | - nav2_pipeline_sequence_bt_node 108 | - nav2_round_robin_node_bt_node 109 | - nav2_transform_available_condition_bt_node 110 | - nav2_time_expired_condition_bt_node 111 | - nav2_path_expiring_timer_condition 112 | - nav2_distance_traveled_condition_bt_node 113 | - nav2_single_trigger_bt_node 114 | - nav2_goal_updated_controller_bt_node 115 | - nav2_is_battery_low_condition_bt_node 116 | - nav2_navigate_through_poses_action_bt_node 117 | - nav2_navigate_to_pose_action_bt_node 118 | - nav2_remove_passed_goals_action_bt_node 119 | - nav2_planner_selector_bt_node 120 | - nav2_controller_selector_bt_node 121 | - nav2_goal_checker_selector_bt_node 122 | - nav2_controller_cancel_bt_node 123 | - nav2_path_longer_on_approach_bt_node 124 | - nav2_wait_cancel_bt_node 125 | - nav2_spin_cancel_bt_node 126 | - nav2_back_up_cancel_bt_node 127 | - nav2_drive_on_heading_cancel_bt_node 128 | 129 | bt_navigator_navigate_through_poses_rclcpp_node: 130 | ros__parameters: 131 | use_sim_time: False 132 | 133 | bt_navigator_navigate_to_pose_rclcpp_node: 134 | ros__parameters: 135 | use_sim_time: False 136 | 137 | controller_server: 138 | ros__parameters: 139 | use_sim_time: False 140 | controller_frequency: 20.0 141 | min_x_velocity_threshold: 0.001 142 | min_y_velocity_threshold: 0.5 143 | min_theta_velocity_threshold: 0.001 144 | failure_tolerance: 0.3 145 | progress_checker_plugin: "progress_checker" 146 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 147 | controller_plugins: ["FollowPath"] 148 | 149 | # Progress checker parameters 150 | progress_checker: 151 | plugin: "nav2_controller::SimpleProgressChecker" 152 | required_movement_radius: 0.5 153 | movement_time_allowance: 10.0 154 | # Goal checker parameters 155 | #precise_goal_checker: 156 | # plugin: "nav2_controller::SimpleGoalChecker" 157 | # xy_goal_tolerance: 0.25 158 | # yaw_goal_tolerance: 0.25 159 | # stateful: True 160 | general_goal_checker: 161 | stateful: True 162 | plugin: "nav2_controller::SimpleGoalChecker" 163 | xy_goal_tolerance: 0.25 164 | yaw_goal_tolerance: 0.25 165 | # DWB parameters 166 | FollowPath: 167 | plugin: "dwb_core::DWBLocalPlanner" 168 | debug_trajectory_details: True 169 | min_vel_x: 0.0 170 | min_vel_y: 0.0 171 | max_vel_x: 0.2 172 | max_vel_y: 0.0 173 | max_vel_theta: 0.6 174 | min_speed_xy: 0.0 175 | max_speed_xy: 0.26 176 | min_speed_theta: 0.0 177 | # Add high threshold velocity for turtlebot 3 issue. 178 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 179 | acc_lim_x: 2.5 180 | acc_lim_y: 0.0 181 | acc_lim_theta: 3.2 182 | decel_lim_x: -2.5 183 | decel_lim_y: 0.0 184 | decel_lim_theta: -3.2 185 | vx_samples: 20 186 | vy_samples: 5 187 | vtheta_samples: 20 188 | sim_time: 1.7 189 | linear_granularity: 0.05 190 | angular_granularity: 0.025 191 | transform_tolerance: 0.2 192 | xy_goal_tolerance: 0.25 193 | trans_stopped_velocity: 0.25 194 | short_circuit_trajectory_evaluation: True 195 | stateful: True 196 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 197 | BaseObstacle.scale: 0.02 198 | PathAlign.scale: 32.0 199 | PathAlign.forward_point_distance: 0.1 200 | GoalAlign.scale: 24.0 201 | GoalAlign.forward_point_distance: 0.1 202 | PathDist.scale: 32.0 203 | GoalDist.scale: 24.0 204 | RotateToGoal.scale: 32.0 205 | RotateToGoal.slowing_factor: 5.0 206 | RotateToGoal.lookahead_time: -1.0 207 | 208 | local_costmap: 209 | local_costmap: 210 | ros__parameters: 211 | update_frequency: 5.0 212 | publish_frequency: 2.0 213 | global_frame: odom 214 | robot_base_frame: base_link 215 | use_sim_time: False 216 | rolling_window: true 217 | width: 3 218 | height: 3 219 | resolution: 0.05 220 | robot_radius: 0.22 221 | plugins: ["voxel_layer", "inflation_layer"] 222 | inflation_layer: 223 | plugin: "nav2_costmap_2d::InflationLayer" 224 | cost_scaling_factor: 3.0 225 | inflation_radius: 0.55 226 | voxel_layer: 227 | plugin: "nav2_costmap_2d::VoxelLayer" 228 | enabled: True 229 | publish_voxel_map: True 230 | origin_z: 0.0 231 | z_resolution: 0.05 232 | z_voxels: 16 233 | max_obstacle_height: 2.0 234 | mark_threshold: 0 235 | observation_sources: scan 236 | scan: 237 | topic: /scan 238 | max_obstacle_height: 2.0 239 | clearing: True 240 | marking: True 241 | data_type: "LaserScan" 242 | raytrace_max_range: 3.0 243 | raytrace_min_range: 0.0 244 | obstacle_max_range: 2.5 245 | obstacle_min_range: 0.0 246 | static_layer: 247 | plugin: "nav2_costmap_2d::StaticLayer" 248 | map_subscribe_transient_local: True 249 | always_send_full_costmap: True 250 | 251 | global_costmap: 252 | global_costmap: 253 | ros__parameters: 254 | update_frequency: 1.0 255 | publish_frequency: 1.0 256 | global_frame: map 257 | robot_base_frame: base_link 258 | use_sim_time: False 259 | robot_radius: 0.22 260 | resolution: 0.05 261 | track_unknown_space: true 262 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 263 | obstacle_layer: 264 | plugin: "nav2_costmap_2d::ObstacleLayer" 265 | enabled: True 266 | observation_sources: scan 267 | scan: 268 | topic: /scan 269 | max_obstacle_height: 2.0 270 | clearing: True 271 | marking: True 272 | data_type: "LaserScan" 273 | raytrace_max_range: 3.0 274 | raytrace_min_range: 0.0 275 | obstacle_max_range: 2.5 276 | obstacle_min_range: 0.0 277 | static_layer: 278 | plugin: "nav2_costmap_2d::StaticLayer" 279 | map_subscribe_transient_local: True 280 | inflation_layer: 281 | plugin: "nav2_costmap_2d::InflationLayer" 282 | cost_scaling_factor: 3.0 283 | inflation_radius: 0.55 284 | always_send_full_costmap: True 285 | 286 | map_server: 287 | ros__parameters: 288 | use_sim_time: False 289 | yaml_filename: "turtlebot3_world.yaml" 290 | 291 | map_saver: 292 | ros__parameters: 293 | use_sim_time: False 294 | save_map_timeout: 5.0 295 | free_thresh_default: 0.25 296 | occupied_thresh_default: 0.65 297 | map_subscribe_transient_local: True 298 | 299 | planner_server: 300 | ros__parameters: 301 | expected_planner_frequency: 20.0 302 | use_sim_time: False 303 | planner_plugins: ["GridBased"] 304 | GridBased: 305 | plugin: "nav2_navfn_planner/NavfnPlanner" 306 | tolerance: 0.5 307 | use_astar: false 308 | allow_unknown: true 309 | 310 | smoother_server: 311 | ros__parameters: 312 | use_sim_time: False 313 | smoother_plugins: ["simple_smoother"] 314 | simple_smoother: 315 | plugin: "nav2_smoother::SimpleSmoother" 316 | tolerance: 1.0e-10 317 | max_its: 1000 318 | do_refinement: True 319 | 320 | behavior_server: 321 | ros__parameters: 322 | costmap_topic: local_costmap/costmap_raw 323 | footprint_topic: local_costmap/published_footprint 324 | cycle_frequency: 10.0 325 | behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] 326 | spin: 327 | plugin: "nav2_behaviors/Spin" 328 | backup: 329 | plugin: "nav2_behaviors/BackUp" 330 | drive_on_heading: 331 | plugin: "nav2_behaviors/DriveOnHeading" 332 | wait: 333 | plugin: "nav2_behaviors/Wait" 334 | global_frame: odom 335 | robot_base_frame: base_link 336 | transform_tolerance: 0.1 337 | use_sim_time: true 338 | simulate_ahead_time: 2.0 339 | max_rotational_vel: 1.0 340 | min_rotational_vel: 0.4 341 | rotational_acc_lim: 3.2 342 | 343 | robot_state_publisher: 344 | ros__parameters: 345 | use_sim_time: False 346 | 347 | waypoint_follower: 348 | ros__parameters: 349 | use_sim_time: False 350 | loop_rate: 20 351 | stop_on_failure: false 352 | waypoint_task_executor_plugin: "wait_at_waypoint" 353 | wait_at_waypoint: 354 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 355 | enabled: True 356 | waypoint_pause_duration: 200 357 | 358 | velocity_smoother: 359 | ros__parameters: 360 | use_sim_time: False 361 | smoothing_frequency: 20.0 362 | scale_velocities: False 363 | feedback: "OPEN_LOOP" 364 | max_velocity: [0.26, 0.0, 1.0] 365 | min_velocity: [-0.26, 0.0, -1.0] 366 | max_accel: [2.5, 0.0, 3.2] 367 | max_decel: [-2.5, 0.0, -3.2] 368 | odom_topic: "odom" 369 | odom_duration: 0.1 370 | deadband_velocity: [0.0, 0.0, 0.0] 371 | velocity_timeout: 1.0 372 | -------------------------------------------------------------------------------- /src/mh_amcl/LaserCorrecter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include "mh_amcl/Types.hpp" 17 | #include "mh_amcl/Utils.hpp" 18 | #include "mh_amcl/LaserCorrecter.hpp" 19 | 20 | #include 21 | #include 22 | #include 23 | #include "geometry_msgs/msg/transform_stamped.hpp" 24 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 25 | 26 | #include "octomap/octomap.h" 27 | 28 | #include "nav2_util/lifecycle_node.hpp" 29 | #include "rclcpp/rclcpp.hpp" 30 | 31 | namespace mh_amcl 32 | { 33 | 34 | LaserCorrecter::LaserCorrecter( 35 | const std::string & name, nav2_util::LifecycleNode::SharedPtr node, 36 | std::shared_ptr & map) 37 | : Correcter(name, node, map) 38 | { 39 | if (!node->has_parameter(name + ".distance_perception_error")) { 40 | node->declare_parameter(name + ".distance_perception_error", 0.05); 41 | } 42 | 43 | node->get_parameter(name + ".distance_perception_error", distance_perception_error_); 44 | } 45 | 46 | void 47 | LaserCorrecter::correct(std::vector & particles, rclcpp::Time & update_time) 48 | { 49 | if (last_perception_ == nullptr) { 50 | return; 51 | } 52 | 53 | if (map_ == nullptr) { 54 | return; 55 | } 56 | 57 | tf2::Stamped bf2laser; 58 | std::string error; 59 | if (tf_buffer_.canTransform( 60 | last_perception_->header.frame_id, "base_footprint", 61 | tf2_ros::fromMsg(last_perception_->header.stamp), &error)) 62 | { 63 | auto bf2laser_msg = tf_buffer_.lookupTransform( 64 | "base_footprint", last_perception_->header.frame_id, 65 | tf2_ros::fromMsg(last_perception_->header.stamp)); 66 | tf2::fromMsg(bf2laser_msg, bf2laser); 67 | } else { 68 | RCLCPP_WARN( 69 | node_->get_logger(), "Timeout while waiting TF %s -> base_footprint [%s]", 70 | last_perception_->header.frame_id.c_str(), error.c_str()); 71 | return; 72 | } 73 | 74 | const double o = distance_perception_error_; 75 | 76 | static const float inv_sqrt_2pi = 0.3989422804014327; 77 | const double normal_comp_1 = inv_sqrt_2pi / o; 78 | 79 | // for (auto & p : particles) { 80 | // p.possible_hits += static_cast(last_perception_->ranges.size()); 81 | // } 82 | 83 | for (int j = 0; j < last_perception_->ranges.size(); j++) { 84 | if (std::isnan(last_perception_->ranges[j])) {continue;} 85 | 86 | tf2::Vector3 laser2point_u = get_perception_unit_vector(*last_perception_, j); 87 | 88 | for (int i = 0; i < particles.size(); i++) { 89 | auto & p = particles[i]; 90 | 91 | double calculated_distance = get_distance_to_obstacle( 92 | p.pose, bf2laser, laser2point_u, *last_perception_, *map_); 93 | 94 | if (std::isinf(last_perception_->ranges[j]) && std::isinf(calculated_distance)) { 95 | p.prob = std::max(p.prob * map_->getProbHit(), 0.000001); 96 | 97 | p.possible_hits += 1.0; 98 | p.hits += 1.0; 99 | } else { 100 | double diff = abs(last_perception_->ranges[j] - calculated_distance); 101 | const double a = diff / o; 102 | const double normal_comp_2 = std::exp(-0.5 * a * a); 103 | 104 | double prob = std::clamp(normal_comp_1 * normal_comp_2, 0.0, 1.0); 105 | p.prob = std::max(p.prob + prob, 0.000001); 106 | 107 | p.possible_hits += 1.0; 108 | // p.hits += 1.0; 109 | 110 | // if (prob > map_->getProbHit()) { 111 | if (prob > map_->getProbMiss()) { 112 | p.hits += 1.0; 113 | } 114 | } 115 | } 116 | } 117 | 118 | update_time = last_perception_->header.stamp; 119 | // last_perception_ = nullptr; 120 | } 121 | 122 | tf2::Transform 123 | LaserCorrecter::get_tranform_to_read(const sensor_msgs::msg::LaserScan & scan, int index) const 124 | { 125 | double dist = scan.ranges[index]; 126 | double angle = scan.angle_min + static_cast(index) * scan.angle_increment; 127 | 128 | tf2::Transform ret; 129 | 130 | double x = dist * cos(angle); 131 | double y = dist * sin(angle); 132 | 133 | ret.setOrigin({x, y, 0.0}); 134 | ret.setRotation({0.0, 0.0, 0.0, 1.0}); 135 | 136 | return ret; 137 | } 138 | 139 | tf2::Vector3 140 | LaserCorrecter::get_perception_unit_vector(const sensor_msgs::msg::LaserScan & scan, int index) const 141 | { 142 | double dist = 1.0; 143 | double angle = scan.angle_min + static_cast(index) * scan.angle_increment; 144 | 145 | tf2::Transform ret; 146 | 147 | double x = dist * cos(angle); 148 | double y = dist * sin(angle); 149 | 150 | return {x, y, 0.0}; 151 | } 152 | 153 | double 154 | LaserCorrecter::get_distance_to_obstacle( 155 | const tf2::Transform & map2bf, const tf2::Transform & bf2laser, 156 | const tf2::Vector3 unit_vector, const sensor_msgs::msg::LaserScan & scan, 157 | const octomap::OcTree & octomap) const 158 | { 159 | tf2::Transform map2laser = map2bf * bf2laser; 160 | tf2::Transform map2laser_rot = map2laser; 161 | map2laser_rot.setOrigin({0.0, 0.0, 0.0}); 162 | 163 | tf2::Vector3 map2point_unit = map2laser_rot * unit_vector; 164 | 165 | tf2::Vector3 & laser_pos = map2laser.getOrigin(); 166 | 167 | octomap::point3d hit; 168 | bool is_obstacle = octomap.castRay( 169 | {static_cast(laser_pos.x()), static_cast(laser_pos.y()), static_cast(laser_pos.z())}, 170 | {static_cast(map2point_unit.x()), static_cast(map2point_unit.y()), static_cast(map2point_unit.z())}, 171 | hit, true, 172 | scan.range_max); 173 | 174 | if (!is_obstacle) { 175 | return std::numeric_limits::infinity(); 176 | } else { 177 | return hit.distance( 178 | { 179 | static_cast(map2laser.getOrigin().x()), 180 | static_cast(map2laser.getOrigin().y()), 181 | static_cast(map2laser.getOrigin().z()) 182 | }); 183 | } 184 | } 185 | 186 | double 187 | LaserCorrecter::get_occupancy( 188 | const tf2::Transform & transform, const octomap::OcTree & octomap) const 189 | { 190 | const auto & transl = transform.getOrigin(); 191 | octomap::OcTreeNode * node = octomap.search(transl.x(), transl.y(), transl.z()); 192 | 193 | if (node == NULL) { 194 | return octomap.getClampingThresMin(); 195 | } 196 | 197 | return node->getOccupancy(); 198 | } 199 | 200 | } // namespace mh_amcl 201 | -------------------------------------------------------------------------------- /src/mh_amcl/MapMatcher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include "nav2_costmap_2d/cost_values.hpp" 17 | #include "nav2_costmap_2d/costmap_2d.hpp" 18 | 19 | #include "mh_amcl/MapMatcher.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | namespace mh_amcl 23 | { 24 | 25 | using std::placeholders::_1; 26 | 27 | MapMatcher::MapMatcher( 28 | const std::string & name, 29 | std::shared_ptr parent_node) 30 | : parent_node_(parent_node) 31 | { 32 | sub_map_ = parent_node->create_subscription( 33 | "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), 34 | std::bind(&MapMatcher::map_callback, this, _1)); 35 | 36 | std::string scan_topic("/scan"); 37 | if (!parent_node->has_parameter(name + ".topic")) { 38 | parent_node->declare_parameter(name + ".topic", scan_topic); 39 | } 40 | parent_node->get_parameter(name + ".topic", scan_topic); 41 | 42 | laser_sub_ = parent_node->create_subscription( 43 | scan_topic, rclcpp::SensorDataQoS(), 44 | std::bind(&MapMatcher::laser_callback, this, _1)); 45 | } 46 | 47 | void 48 | MapMatcher::laser_callback(sensor_msgs::msg::LaserScan::UniquePtr msg) 49 | { 50 | last_perception_ = std::move(msg); 51 | } 52 | 53 | void 54 | MapMatcher::map_callback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & msg) 55 | { 56 | costmap_ = std::make_shared(*msg); 57 | 58 | costmaps_.resize(NUM_LEVEL_SCALE_COSTMAP); 59 | costmaps_[0] = std::make_shared(*costmap_); 60 | 61 | for (int i = 1; i < NUM_LEVEL_SCALE_COSTMAP; i++) { 62 | costmaps_[i] = half_scale(costmaps_[i - 1]); 63 | } 64 | } 65 | 66 | std::shared_ptr 67 | MapMatcher::half_scale(std::shared_ptr costmap_in) 68 | { 69 | auto costmap = std::make_shared( 70 | costmap_in->getSizeInCellsX() / 2, costmap_in->getSizeInCellsY() / 2, 71 | costmap_in->getResolution() * 2.0, costmap_in->getOriginX(), 72 | costmap_in->getOriginY(), costmap_in->getDefaultValue()); 73 | 74 | for (unsigned int i = 0; i < costmap->getSizeInCellsX(); i++) { 75 | for (unsigned int j = 0; j < costmap->getSizeInCellsY(); j++) { 76 | unsigned int ri = i * 2; 77 | unsigned int rj = j * 2; 78 | 79 | auto cost1 = costmap_in->getCost(ri, rj); 80 | auto cost2 = costmap_in->getCost(ri + 1, rj); 81 | auto cost3 = costmap_in->getCost(ri, rj + 1); 82 | auto cost4 = costmap_in->getCost(ri + 1, rj + 1); 83 | 84 | if (cost1 == nav2_costmap_2d::LETHAL_OBSTACLE || 85 | cost2 == nav2_costmap_2d::LETHAL_OBSTACLE || 86 | cost3 == nav2_costmap_2d::LETHAL_OBSTACLE || 87 | cost4 == nav2_costmap_2d::LETHAL_OBSTACLE) 88 | { 89 | costmap->setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); 90 | } else { 91 | if (cost1 == nav2_costmap_2d::FREE_SPACE || 92 | cost2 == nav2_costmap_2d::FREE_SPACE || 93 | cost3 == nav2_costmap_2d::FREE_SPACE || 94 | cost4 == nav2_costmap_2d::FREE_SPACE) 95 | { 96 | costmap->setCost(i, j, nav2_costmap_2d::FREE_SPACE); 97 | } else { 98 | if (cost1 == nav2_costmap_2d::NO_INFORMATION && 99 | cost2 == nav2_costmap_2d::NO_INFORMATION && 100 | cost3 == nav2_costmap_2d::NO_INFORMATION && 101 | cost4 == nav2_costmap_2d::NO_INFORMATION) 102 | { 103 | costmap->setCost(i, j, nav2_costmap_2d::NO_INFORMATION); 104 | } else { 105 | costmap->setCost(i, j, cost1); 106 | } 107 | } 108 | } 109 | } 110 | } 111 | 112 | return costmap; 113 | } 114 | 115 | std::list 116 | MapMatcher::get_matchs() 117 | { 118 | if (last_perception_ == nullptr || costmap_ == nullptr) { 119 | RCLCPP_WARN(parent_node_->get_logger(), "No data available for map matching"); 120 | return {}; 121 | } 122 | 123 | std::vector laser_poins = laser2points(*last_perception_); 124 | 125 | int start_level = NUM_LEVEL_SCALE_COSTMAP - 2; 126 | int min_level = 2; 127 | 128 | double min_x, max_x, min_y, max_y; 129 | costmaps_[start_level]->mapToWorld(0, 0, min_x, min_y); 130 | costmaps_[start_level]->mapToWorld( 131 | costmaps_[start_level]->getSizeInCellsX(), 132 | costmaps_[start_level]->getSizeInCellsY(), max_x, max_y); 133 | 134 | std::list candidates[NUM_LEVEL_SCALE_COSTMAP]; 135 | candidates[start_level] = get_matchs(start_level, laser_poins, min_x, min_y, max_x, max_y); 136 | candidates[start_level].sort(); 137 | 138 | for (int level = start_level; level >= min_level; level--) { 139 | for (const auto & candidate : candidates[level]) { 140 | double min_x, max_x, min_y, max_y; 141 | min_x = candidate.transform.getOrigin().x() - costmaps_[level]->getResolution() / 2.0; 142 | max_x = candidate.transform.getOrigin().x() + costmaps_[level]->getResolution() / 2.0; 143 | min_y = candidate.transform.getOrigin().y() - costmaps_[level]->getResolution() / 2.0; 144 | max_y = candidate.transform.getOrigin().y() + costmaps_[level]->getResolution() / 2.0; 145 | 146 | auto new_candidates = get_matchs(level - 1, laser_poins, min_x, min_y, max_x, max_y); 147 | 148 | candidates[level - 1].insert( 149 | candidates[level - 1].end(), new_candidates.begin(), new_candidates.end()); 150 | } 151 | } 152 | 153 | candidates[min_level].sort(); 154 | 155 | return candidates[min_level]; 156 | } 157 | 158 | std::list 159 | MapMatcher::get_matchs( 160 | int scale, const std::vector & scan, 161 | float min_x, float min_y, float max_y, float max_x) 162 | { 163 | std::list ret; 164 | const auto & costmap = costmaps_[scale]; 165 | 166 | int init_i, init_j, end_i, end_j; 167 | costmap->worldToMapEnforceBounds(min_x, min_y, init_i, init_j); 168 | costmap->worldToMapEnforceBounds(max_x, max_y, end_i, end_j); 169 | 170 | for (unsigned int i = init_i; i < end_i; i++) { 171 | for (unsigned int j = init_j; j < end_j; j++) { 172 | auto cost = costmap->getCost(i, j); 173 | if (cost == nav2_costmap_2d::FREE_SPACE) { 174 | double inc_theta = (M_PI / 4.0); 175 | for (double theta = 0; theta < 1.9 * M_PI; theta = theta + inc_theta) { 176 | double x, y; 177 | costmap->mapToWorld(i, j, x, y); 178 | TransformWeighted tw; 179 | 180 | tf2::Quaternion q; 181 | q.setRPY(0.0, 0.0, theta); 182 | tw.transform = tf2::Transform(q, {x, y, 0.0}); 183 | 184 | tw.weight = match(scale, *costmap, scan, tw.transform); 185 | 186 | if (tw.weight > 0.5) { 187 | ret.push_back(tw); 188 | } 189 | } 190 | } 191 | } 192 | } 193 | return ret; 194 | } 195 | 196 | float 197 | MapMatcher::match( 198 | int scale, const nav2_costmap_2d::Costmap2D & costmap, 199 | const std::vector & scan, tf2::Transform & transform) 200 | { 201 | int hits = 0; 202 | int total = 0; 203 | 204 | for (int i = 0; i < scan.size(); i = i + scale) { 205 | tf2::Vector3 test_point = transform * scan[i]; 206 | int gi, gj; 207 | 208 | costmap.worldToMapNoBounds(test_point.x(), test_point.y(), gi, gj); 209 | 210 | 211 | if (gi > 0 && gj > 0 && gi < costmap.getSizeInCellsX() && gj < costmap.getSizeInCellsY() && 212 | costmap.getCost(gi, gj) == nav2_costmap_2d::LETHAL_OBSTACLE) 213 | { 214 | hits++; 215 | } 216 | total++; 217 | } 218 | 219 | return static_cast(hits) / static_cast(total); 220 | } 221 | 222 | std::vector 223 | MapMatcher::laser2points(const sensor_msgs::msg::LaserScan & scan) 224 | { 225 | std::list points; 226 | for (auto i = 0; i < scan.ranges.size(); i++) { 227 | if (std::isnan(scan.ranges[i]) || std::isinf(scan.ranges[i])) {continue;} 228 | 229 | tf2::Vector3 p; 230 | float dist = scan.ranges[i]; 231 | float theta = scan.angle_min + i * scan.angle_increment; 232 | p.setX(dist * cos(theta)); 233 | p.setY(dist * sin(theta)); 234 | p.setZ(0.0); 235 | points.push_back(p); 236 | } 237 | return std::vector(points.begin(), points.end()); 238 | } 239 | 240 | nav_msgs::msg::OccupancyGrid 241 | toMsg(const nav2_costmap_2d::Costmap2D & costmap) 242 | { 243 | nav_msgs::msg::OccupancyGrid grid; 244 | 245 | grid.info.resolution = costmap.getResolution(); 246 | grid.info.width = costmap.getSizeInCellsX(); 247 | grid.info.height = costmap.getSizeInCellsY(); 248 | 249 | double wx, wy; 250 | costmap.mapToWorld(0, 0, wx, wy); 251 | grid.info.origin.position.x = wx - costmap.getResolution() / 2; 252 | grid.info.origin.position.y = wy - costmap.getResolution() / 2; 253 | grid.info.origin.position.z = 0.0; 254 | grid.info.origin.orientation.w = 1.0; 255 | 256 | grid.data.resize(grid.info.width * grid.info.height); 257 | 258 | std::vector cost_translation_table(256); 259 | 260 | // special values: 261 | cost_translation_table[0] = 0; // NO obstacle 262 | cost_translation_table[253] = 99; // INSCRIBED obstacle 263 | cost_translation_table[254] = 100; // LETHAL obstacle 264 | cost_translation_table[255] = -1; // UNKNOWN 265 | 266 | // regular cost values scale the range 1 to 252 (inclusive) to fit 267 | // into 1 to 98 (inclusive). 268 | for (int i = 1; i < 253; i++) { 269 | cost_translation_table[i] = static_cast(1 + (97 * (i - 1)) / 251); 270 | } 271 | 272 | unsigned char * data = costmap.getCharMap(); 273 | for (unsigned int i = 0; i < grid.data.size(); i++) { 274 | grid.data[i] = cost_translation_table[data[i]]; 275 | } 276 | 277 | return grid; 278 | } 279 | 280 | } // namespace mh_amcl 281 | -------------------------------------------------------------------------------- /src/mh_amcl/MapMatcherBase.cpp: -------------------------------------------------------------------------------- 1 | 2 | // Copyright 2022 Intelligent Robotics Lab 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "mh_amcl/MapMatcherBase.hpp" 17 | 18 | namespace mh_amcl 19 | { 20 | 21 | bool operator<(const TransformWeighted & tw1, const TransformWeighted & tw2) 22 | { 23 | // To sort incremental 24 | return tw1.weight > tw2.weight; 25 | } 26 | 27 | } // namespace mh_amcl 28 | -------------------------------------------------------------------------------- /src/mh_amcl/PointCloudCorrecter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include "mh_amcl/Types.hpp" 17 | #include "mh_amcl/Utils.hpp" 18 | #include "mh_amcl/PointCloudCorrecter.hpp" 19 | 20 | #include 21 | #include 22 | #include 23 | #include "geometry_msgs/msg/transform_stamped.hpp" 24 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 25 | 26 | #include "pcl/point_types.h" 27 | #include "pcl_conversions/pcl_conversions.h" 28 | #include "pcl/point_types_conversion.h" 29 | 30 | #include "octomap/octomap.h" 31 | 32 | #include "nav2_util/lifecycle_node.hpp" 33 | #include "rclcpp/rclcpp.hpp" 34 | 35 | 36 | namespace mh_amcl 37 | { 38 | 39 | PointCloudCorrecter::PointCloudCorrecter( 40 | const std::string & name, nav2_util::LifecycleNode::SharedPtr node, 41 | std::shared_ptr & map) 42 | : Correcter(name, node, map) 43 | { 44 | if (!node->has_parameter(name + ".distance_perception_error")) { 45 | node->declare_parameter(name + ".distance_perception_error", 0.05); 46 | } 47 | if (!node->has_parameter(name + ".max_perception_distance")) { 48 | node->declare_parameter(name + ".max_perception_distance", 2.0); 49 | } 50 | if (!node->has_parameter(name + ".point_step")) { 51 | node->declare_parameter(name + ".point_step", 1); 52 | } 53 | node->get_parameter(name + ".distance_perception_error", distance_perception_error_); 54 | node->get_parameter(name + ".max_perception_distance", max_perception_distance_); 55 | node->get_parameter(name + ".point_step", point_step_); 56 | } 57 | 58 | void 59 | PointCloudCorrecter::correct(std::vector & particles, rclcpp::Time & update_time) 60 | { 61 | if (last_perception_ == nullptr) { 62 | return; 63 | } 64 | if (map_ == nullptr) { 65 | return; 66 | } 67 | 68 | tf2::Stamped bf2sensor; 69 | std::string error; 70 | if (tf_buffer_.canTransform( 71 | last_perception_->header.frame_id, "base_footprint", 72 | tf2_ros::fromMsg(last_perception_->header.stamp), &error)) 73 | { 74 | auto bf2sensor_msg = tf_buffer_.lookupTransform( 75 | "base_footprint", last_perception_->header.frame_id, 76 | tf2_ros::fromMsg(last_perception_->header.stamp)); 77 | tf2::fromMsg(bf2sensor_msg, bf2sensor); 78 | } else { 79 | RCLCPP_WARN( 80 | node_->get_logger(), "Timeout while waiting TF %s -> base_footprint [%s]", 81 | last_perception_->header.frame_id.c_str(), error.c_str()); 82 | return; 83 | } 84 | 85 | const double o = distance_perception_error_; 86 | 87 | static const float inv_sqrt_2pi = 0.3989422804014327; 88 | const double normal_comp_1 = inv_sqrt_2pi / o; 89 | 90 | pointcloud_.clear(); 91 | pcl::fromROSMsg(*last_perception_, pointcloud_); 92 | 93 | int point_count = 0; 94 | 95 | for (const auto & point : pointcloud_.points) { 96 | if (std::isnan(point.x)) {continue;} 97 | 98 | if (point_count++ % point_step_ != 0) { 99 | continue; 100 | } 101 | 102 | tf2::Vector3 sensor2point(point.x, point.y, point.z); 103 | tf2::Vector3 sensor2point_u = sensor2point / sensor2point.length(); 104 | 105 | for (int i = 0; i < particles.size(); i++) { 106 | auto & p = particles[i]; 107 | 108 | double calculated_distance = get_distance_to_obstacle( 109 | p.pose, bf2sensor, sensor2point_u, *last_perception_, *map_); 110 | 111 | if (std::isinf(point.x) && std::isinf(calculated_distance)) { 112 | p.prob = std::max(p.prob + map_->getProbHit(), 0.000001); 113 | 114 | p.possible_hits += 1.0; 115 | p.hits += 1.0; 116 | } else { 117 | double diff = abs(sensor2point.length() - calculated_distance); 118 | const double a = diff / o; 119 | const double normal_comp_2 = std::exp(-0.5 * a * a); 120 | 121 | double prob = std::clamp(normal_comp_1 * normal_comp_2, 0.0, 1.0); 122 | p.prob = std::max(p.prob + prob, 0.000001); 123 | 124 | p.possible_hits += 1.0; 125 | // p.hits += 1.0; 126 | 127 | // if (prob > map_->getProbHit()) { 128 | if (prob > map_->getProbMiss()) { 129 | p.hits += 1.0; 130 | } 131 | } 132 | } 133 | } 134 | 135 | update_time = last_perception_->header.stamp; 136 | } 137 | 138 | 139 | double 140 | PointCloudCorrecter::get_distance_to_obstacle( 141 | const tf2::Transform & map2bf, const tf2::Transform & bf2sensor, 142 | const tf2::Vector3 unit_vector, const sensor_msgs::msg::PointCloud2 & scan, 143 | const octomap::OcTree & octomap) const 144 | { 145 | tf2::Transform map2sensor = map2bf * bf2sensor; 146 | tf2::Transform map2sensor_rot = map2sensor; 147 | map2sensor_rot.setOrigin({0.0, 0.0, 0.0}); 148 | 149 | tf2::Vector3 map2point_unit = map2sensor_rot * unit_vector; 150 | 151 | tf2::Vector3 & sensor_pos = map2sensor.getOrigin(); 152 | 153 | octomap::point3d hit; 154 | bool is_obstacle = octomap.castRay( 155 | {static_cast(sensor_pos.x()), static_cast(sensor_pos.y()), static_cast(sensor_pos.z())}, 156 | {static_cast(map2point_unit.x()), static_cast(map2point_unit.y()), static_cast(map2point_unit.z())}, 157 | hit, true, 158 | max_perception_distance_); 159 | 160 | if (!is_obstacle) { 161 | return std::numeric_limits::infinity(); 162 | } else { 163 | return hit.distance( 164 | { 165 | static_cast(map2sensor.getOrigin().x()), 166 | static_cast(map2sensor.getOrigin().y()), 167 | static_cast(map2sensor.getOrigin().z()) 168 | }); 169 | } 170 | } 171 | 172 | double 173 | PointCloudCorrecter::get_occupancy( 174 | const tf2::Transform & transform, const octomap::OcTree & octomap) const 175 | { 176 | const auto & transl = transform.getOrigin(); 177 | octomap::OcTreeNode * node = octomap.search(transl.x(), transl.y(), transl.z()); 178 | 179 | if (node == NULL) { 180 | return octomap.getClampingThresMin(); 181 | } 182 | 183 | return node->getOccupancy(); 184 | } 185 | 186 | } // namespace mh_amcl 187 | -------------------------------------------------------------------------------- /src/mh_amcl/Utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | namespace mh_amcl 18 | { 19 | 20 | 21 | } // namespace mh_amcl 22 | 23 | -------------------------------------------------------------------------------- /src/mh_amcl_program.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mh_amcl/MH_AMCL.hpp" 16 | 17 | #include "lifecycle_msgs/msg/transition.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | 24 | auto mh_amcl = std::make_shared(); 25 | auto executor = rclcpp::executors::MultiThreadedExecutor(rclcpp::ExecutorOptions(), 10); 26 | executor.add_node(mh_amcl->get_node_base_interface()); 27 | 28 | executor.spin(); 29 | 30 | rclcpp::shutdown(); 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_particles test_particles.cpp TIMEOUT 300) 2 | target_link_libraries(test_particles ${PROJECT_NAME}) 3 | 4 | ament_add_gtest(test_map_matcher test_map_matcher.cpp TIMEOUT 300) 5 | ament_target_dependencies(test_map_matcher ${dependencies}) 6 | target_link_libraries(test_map_matcher ${PROJECT_NAME}) 7 | 8 | ament_add_gtest(test_mh_amcl test_mh_amcl.cpp TIMEOUT 300) 9 | ament_target_dependencies(test_mh_amcl ${dependencies}) 10 | target_link_libraries(test_mh_amcl ${PROJECT_NAME}) 11 | -------------------------------------------------------------------------------- /tests/rosbag2_2022_09_01-11_42_10/metadata.yaml: -------------------------------------------------------------------------------- 1 | rosbag2_bagfile_information: 2 | version: 6 3 | storage_identifier: sqlite3 4 | duration: 5 | nanoseconds: 113542004462 6 | starting_time: 7 | nanoseconds_since_epoch: 1662025331280700187 8 | message_count: 10926 9 | topics_with_message_count: 10 | - topic_metadata: 11 | name: /scan 12 | type: sensor_msgs/msg/LaserScan 13 | serialization_format: cdr 14 | offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" 15 | message_count: 1703 16 | - topic_metadata: 17 | name: /tf 18 | type: tf2_msgs/msg/TFMessage 19 | serialization_format: cdr 20 | offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" 21 | message_count: 9222 22 | - topic_metadata: 23 | name: /tf_static 24 | type: tf2_msgs/msg/TFMessage 25 | serialization_format: cdr 26 | offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" 27 | message_count: 1 28 | compression_format: "" 29 | compression_mode: "" 30 | relative_file_paths: 31 | - rosbag2_2022_09_01-11_42_10_0.db3 32 | files: 33 | - path: rosbag2_2022_09_01-11_42_10_0.db3 34 | starting_time: 35 | nanoseconds_since_epoch: 1662025331280700187 36 | duration: 37 | nanoseconds: 113542004462 38 | message_count: 10926 39 | custom_data: ~ -------------------------------------------------------------------------------- /tests/rosbag2_2022_09_01-11_42_10/rosbag2_2022_09_01-11_42_10_0.db3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navigation-gridmap/mh_amcl/af809379b91afe67e52485f73699aeb86eb90b9e/tests/rosbag2_2022_09_01-11_42_10/rosbag2_2022_09_01-11_42_10_0.db3 -------------------------------------------------------------------------------- /tests/test_map_matcher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include "gtest/gtest.h" 17 | 18 | #include "mh_amcl/MapMatcher.hpp" 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | using namespace std::chrono_literals; 23 | 24 | /* 25 | class MapMatcherTest : public mh_amcl::MapMatcher 26 | { 27 | public: 28 | MapMatcherTest(const nav_msgs::msg::OccupancyGrid & map, rclcpp::Node::SharedPtr node) 29 | : node_(node), MapMatcher(map) 30 | { 31 | pubs_.resize(NUM_LEVEL_SCALE_COSTMAP); 32 | for (int i = 0; i < NUM_LEVEL_SCALE_COSTMAP; i++) { 33 | pubs_[i] = node_->create_publisher( 34 | "grid_" + std::to_string(i), 100); 35 | } 36 | } 37 | 38 | void publish_maps() 39 | { 40 | for (int i = 0; i < NUM_LEVEL_SCALE_COSTMAP; i++) { 41 | if (pubs_[i]->get_subscription_count() > 0) { 42 | nav_msgs::msg::OccupancyGrid grid = mh_amcl::toMsg(*costmaps_[i]); 43 | grid.header.frame_id = "map"; 44 | grid.header.stamp = node_->now(); 45 | pubs_[i]->publish(grid); 46 | } 47 | } 48 | } 49 | 50 | rclcpp::Node::SharedPtr node_; 51 | std::vector::SharedPtr> pubs_; 52 | }; 53 | */ 54 | /*TEST(test1, test_match) 55 | { 56 | auto test_node = rclcpp::Node::make_shared("test_node"); 57 | sensor_msgs::msg::LaserScan scan; 58 | nav_msgs::msg::OccupancyGrid grid; 59 | 60 | auto scan_sub = test_node->create_subscription( 61 | "scan", 100, [&scan](const sensor_msgs::msg::LaserScan::SharedPtr msg) { 62 | scan = *msg; 63 | }); 64 | auto grid_sub = test_node->create_subscription( 65 | "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), 66 | [&grid](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { 67 | grid = *msg; 68 | }); 69 | 70 | auto start = test_node->now(); 71 | rclcpp::Rate rate(100ms); 72 | while ((test_node->now() - start).seconds() < 5.0) { 73 | rclcpp::spin_some(test_node); 74 | rate.sleep(); 75 | } 76 | 77 | ASSERT_FALSE(scan.ranges.empty()); 78 | ASSERT_FALSE(grid.data.empty()); 79 | 80 | MapMatcherTest map_matcher(grid, test_node); 81 | const auto & tfs = map_matcher.get_matchs(scan); 82 | 83 | for (const auto & transform : tfs) { 84 | const auto & x = transform.transform.getOrigin().x(); 85 | const auto & y = transform.transform.getOrigin().y(); 86 | double roll, pitch, yaw; 87 | tf2::Matrix3x3(transform.transform.getRotation()).getRPY(roll, pitch, yaw); 88 | std::cerr << "(" << x << ", " << y << ", " << yaw << ")" << std::endl; 89 | } 90 | // start = test_node->now(); 91 | // while ((test_node->now() - start).seconds() < 30.0) { 92 | // map_matcher.publish_maps(); 93 | // rclcpp::spin_some(test_node); 94 | // rate.sleep(); 95 | // } 96 | }*/ 97 | 98 | int main(int argc, char * argv[]) 99 | { 100 | testing::InitGoogleTest(&argc, argv); 101 | rclcpp::init(argc, argv); 102 | return RUN_ALL_TESTS(); 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /tests/test_mh_amcl.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | 18 | #include "gtest/gtest.h" 19 | 20 | #include "mh_amcl/MH_AMCL.hpp" 21 | 22 | #include "nav2_costmap_2d/cost_values.hpp" 23 | #include "tf2_ros/static_transform_broadcaster.h" 24 | #include "sensor_msgs/msg/laser_scan.hpp" 25 | #include "lifecycle_msgs/msg/state.hpp" 26 | 27 | #include "nav2_costmap_2d/costmap_2d.hpp" 28 | 29 | #include "rclcpp/rclcpp.hpp" 30 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 31 | 32 | 33 | class MH_AMCL_NodeTest : public mh_amcl::MH_AMCL_Node 34 | { 35 | public: 36 | MH_AMCL_NodeTest() 37 | : MH_AMCL_Node() {} 38 | }; 39 | 40 | 41 | TEST(test1, test_aux) 42 | { 43 | } 44 | 45 | int main(int argc, char * argv[]) 46 | { 47 | testing::InitGoogleTest(&argc, argv); 48 | rclcpp::init(argc, argv); 49 | return RUN_ALL_TESTS(); 50 | return 0; 51 | } 52 | --------------------------------------------------------------------------------