├── .github └── workflows │ └── main.yaml ├── CHANGELOG.rst ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── colcon.pkg ├── dependency_repos.repos ├── env-hooks └── gazebo_plugins.sh.in ├── include └── gazebo_plugins │ └── gazebo_ros_gridmap.hpp ├── package.xml ├── src └── gazebo_ros_gridmap.cpp ├── test ├── CMakeLists.txt ├── rviz │ └── octomap_gridmap_plugin.rviz ├── test_gazebo_ros_gridmap.cpp └── worlds │ ├── gazebo_ros_ackermann_drive.world │ ├── gazebo_ros_bumper.world │ ├── gazebo_ros_camera.world │ ├── gazebo_ros_camera_16bit.world │ ├── gazebo_ros_camera_distortion_barrel.world │ ├── gazebo_ros_camera_distortion_pincushion.world │ ├── gazebo_ros_camera_triggered.world │ ├── gazebo_ros_depth_camera.world │ ├── gazebo_ros_diff_drive.world │ ├── gazebo_ros_elevator.world │ ├── gazebo_ros_force.world │ ├── gazebo_ros_force_link.world │ ├── gazebo_ros_ft_sensor.world │ ├── gazebo_ros_gps_sensor.world │ ├── gazebo_ros_hand_of_god.world │ ├── gazebo_ros_harness.world │ ├── gazebo_ros_imu_sensor.world │ ├── gazebo_ros_joint_pose_trajectory.world │ ├── gazebo_ros_joint_state_publisher.world │ ├── gazebo_ros_multi_camera.world │ ├── gazebo_ros_multicamera.world │ ├── gazebo_ros_p3d.world │ ├── gazebo_ros_planar_move.world │ ├── gazebo_ros_projector.world │ ├── gazebo_ros_ray_sensor.world │ ├── gazebo_ros_skid_steer_drive.world │ ├── gazebo_ros_tricycle_drive.world │ ├── gazebo_ros_vacuum_gripper.world │ ├── gazebo_ros_video.world │ ├── gazebo_ros_wheel_slip.world │ ├── gazebo_ros_wheel_slip_at_rest.world │ └── wheelslip_worlds │ ├── gazebo_ros_wheelslip_1.world │ ├── gazebo_ros_wheelslip_2.world │ ├── gazebo_ros_wheelslip_3.world │ ├── gazebo_ros_wheelslip_4.world │ └── gazebo_ros_wheelslip_5.world └── worlds ├── gazebo_ros_gridmap.world └── gazebo_ros_gridmap_apollo.world /.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.4.1 21 | with: 22 | required-ros-distributions: rolling 23 | - name: build and test 24 | uses: ros-tooling/action-ros-ci@0.2.6 25 | with: 26 | package-name: gazebo_gridmap_plugin 27 | import-token: ${{secrets.GITHUB_TOKEN}} 28 | target-ros2-distro: rolling 29 | colcon-defaults: | 30 | { 31 | "test": { 32 | "ctest-args": [ 33 | "-LE", "copyright" 34 | ] 35 | } 36 | } 37 | vcs-repo-file-url: https://raw.githubusercontent.com/ivrolan/gazebo_gridmap_plugin/testing-ci/dependency_repos.repos 38 | 39 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Void 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(gazebo_gridmap_plugin) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | # we dont use add_compile_options with pedantic in message packages 10 | # because the Python C extensions dont comply with it 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") 12 | endif() 13 | 14 | if(WIN32) 15 | add_compile_definitions( 16 | # For math constants 17 | _USE_MATH_DEFINES 18 | # Minimize Windows namespace collision 19 | NOMINMAX 20 | WIN32_LEAN_AND_MEAN 21 | ) 22 | endif() 23 | 24 | find_package(ament_cmake REQUIRED) 25 | find_package(camera_info_manager REQUIRED) 26 | find_package(gazebo_dev REQUIRED) 27 | find_package(gazebo_msgs REQUIRED) 28 | find_package(gazebo_ros REQUIRED) 29 | find_package(geometry_msgs REQUIRED) 30 | find_package(image_transport REQUIRED) 31 | find_package(cv_bridge REQUIRED) 32 | find_package(nav_msgs REQUIRED) 33 | find_package(nav2_msgs REQUIRED) 34 | find_package(rclcpp REQUIRED) 35 | find_package(sensor_msgs REQUIRED) 36 | find_package(std_msgs REQUIRED) 37 | find_package(std_srvs REQUIRED) 38 | find_package(tf2 REQUIRED) 39 | find_package(tf2_geometry_msgs REQUIRED) 40 | find_package(tf2_ros REQUIRED) 41 | find_package(trajectory_msgs REQUIRED) 42 | find_package(grid_map_ros REQUIRED) 43 | find_package(grid_map_msgs REQUIRED) 44 | find_package(octomap REQUIRED) 45 | find_package(octomap_msgs REQUIRED) 46 | find_package(octomap_ros REQUIRED) 47 | 48 | link_directories(${gazebo_dev_LIBRARY_DIRS}) 49 | 50 | option(ENABLE_PROFILER "Enable Ignition Profiler" FALSE) 51 | if(ENABLE_PROFILER) 52 | find_package(ignition-common3 QUIET COMPONENTS profiler) 53 | if(ignition-common3_FOUND) 54 | add_definitions("-DIGN_PROFILER_ENABLE=1" "-DIGN_PROFILER_REMOTERY=1") 55 | message(STATUS "Profiler is active") 56 | else() 57 | message(WARNING "Can't find Ignition common3. Profiler will not be actived") 58 | add_definitions("-DIGN_PROFILER_ENABLE=0" "-DIGN_PROFILER_REMOTERY=0") 59 | endif() 60 | endif() 61 | 62 | # gazebo_ros_gridmap 63 | add_library(gazebo_ros_gridmap SHARED 64 | src/gazebo_ros_gridmap.cpp 65 | ) 66 | target_include_directories(gazebo_ros_gridmap PUBLIC include) 67 | ament_target_dependencies(gazebo_ros_gridmap 68 | "gazebo_dev" 69 | "gazebo_ros" 70 | "rclcpp" 71 | "grid_map_ros" 72 | "grid_map_msgs" 73 | "grid_map_core" 74 | "octomap_msgs" 75 | "octomap_ros" 76 | "octomap" 77 | ) 78 | ament_export_libraries(gazebo_ros_gridmap) 79 | 80 | 81 | ament_export_include_directories(include) 82 | ament_export_dependencies(rclcpp) 83 | ament_export_dependencies(gazebo_dev) 84 | ament_export_dependencies(gazebo_msgs) 85 | ament_export_dependencies(gazebo_ros) 86 | 87 | if(BUILD_TESTING) 88 | add_subdirectory(test) 89 | find_package(ament_lint_auto REQUIRED) 90 | set(ament_cmake_copyright_FOUND TRUE) 91 | ament_lint_auto_find_test_dependencies() 92 | endif() 93 | 94 | if(NOT WIN32) 95 | if(NOT APPLE) 96 | set( 97 | AMENT_CMAKE_ENVIRONMENT_HOOKS_DESC_gazebo_plugins 98 | "prepend-non-duplicate;LD_LIBRARY_PATH;${GAZEBO_PLUGIN_PATH}") 99 | else() 100 | set( 101 | AMENT_CMAKE_ENVIRONMENT_HOOKS_DESC_gazebo_plugins 102 | "prepend-non-duplicate;DYLD_LIBRARY_PATH;${GAZEBO_PLUGIN_PATH}") 103 | endif() 104 | endif() 105 | ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/gazebo_plugins.sh.in") 106 | 107 | ament_package() 108 | 109 | install(DIRECTORY include/ 110 | DESTINATION include) 111 | 112 | install(TARGETS 113 | gazebo_ros_gridmap 114 | ARCHIVE DESTINATION lib 115 | LIBRARY DESTINATION lib 116 | RUNTIME DESTINATION bin) 117 | 118 | install(DIRECTORY 119 | worlds 120 | DESTINATION share/${PROJECT_NAME}/ 121 | ) 122 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contribution Guidelines 2 | 3 | As an open-source project, we welcome and encourage the community to submit patches directly to this package. 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 | Contributions should be made under the predominant license of that package. Entirely new packages should be made available under the [Apache 2.0 license](https://www.apache.org/licenses/LICENSE-2.0). 11 | 12 | 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. 13 | 14 | ## Developer Certification of Origin (DCO) 15 | 16 | To make a good faith effort to ensure licensing criteria are met, it is required the Developer Certificate of Origin (DCO) process to be followed. 17 | 18 | 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. 19 | 20 | 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/). 21 | 22 | ``` txt 23 | Developer's Certificate of Origin 1.1 24 | 25 | By making a contribution to this project, I certify that: 26 | 27 | (a) The contribution was created in whole or in part by me and I 28 | have the right to submit it under the open source license 29 | indicated in the file; or 30 | 31 | (b) The contribution is based upon previous work that, to the 32 | best of my knowledge, is covered under an appropriate open 33 | source license and I have the right under that license to 34 | submit that work with modifications, whether created in whole 35 | or in part by me, under the same open source license (unless 36 | I am permitted to submit under a different license), as 37 | Indicated in the file; or 38 | 39 | (c) The contribution was provided directly to me by some other 40 | person who certified (a), (b) or (c) and I have not modified 41 | it. 42 | 43 | (d) I understand and agree that this project and the contribution 44 | are public and that a record of the contribution (including 45 | all personal information I submit with it, including my 46 | sign-off) is maintained indefinitely and may be redistributed 47 | consistent with this project or the open source license(s) 48 | involved. 49 | ``` 50 | 51 | ## DCO Sign-Off Methods 52 | 53 | The DCO requires that a sign-off message, in the following format, appears on each commit in the pull request: 54 | 55 | ``` txt 56 | Signed-off-by: Sofforus Jones 57 | ``` 58 | 59 | 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`. 60 | 61 | 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. 62 | -------------------------------------------------------------------------------- /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 | [![GitHub Action 2 | Status](https://github.com/fmrico/gazebo_gridmap_plugin/workflows/main/badge.svg)](https://github.com/fmrico/gazebo_gridmap_plugin) 3 | 4 | # gazebo_gridmap_plugin 5 | 6 | This gazebo plugin creates and publishes to [ROS2 (Rolling)](https://docs.ros.org/en/rolling/index.html) a [grid_map](https://github.com/ANYbotics/grid_map) with elevation and occupancy and a [octomap](https://github.com/OctoMap/octomap) representing the 3D space 7 | 8 | ## Usage: 9 | 10 | ``` 11 | 12 | 13 | demo 14 | 15 | 0.0 16 | 0.0 17 | -10.0 18 | -10.0 19 | -10.0 20 | 10.0 21 | 10.0 22 | 10.0 23 | 0.2 24 | 1.5 25 | 0.1 26 | 27 | ``` 28 | 29 | ## captures 30 | 31 | ![Captura de pantalla 2022-02-16 20:00:46](https://user-images.githubusercontent.com/3810011/154469799-23fbca08-cc6d-4d6c-8398-eb8dda94342b.png) 32 | ![Captura de pantalla 2022-02-16 19:59:42](https://user-images.githubusercontent.com/3810011/154469801-a73b0ada-d3db-439d-bc9e-1995696227d5.png) 33 | ![Captura de pantalla 2022-02-16 19:58:26](https://user-images.githubusercontent.com/3810011/154469802-2878e765-d718-4a81-b234-2b61c3b674c4.png) 34 | 35 | ### Octomap example 36 | 37 | ![octomap_example](https://user-images.githubusercontent.com/22964725/199963628-993dd17b-dc56-4f3b-a9cb-8b3f510d9f65.png) 38 | 39 | Provided by [@miggsant](https://twitter.com/miggsant/status/1494434063838547977) 40 | 41 | ![FL1LukfXIAkf0my](https://user-images.githubusercontent.com/3810011/154728065-d001c324-ba64-4537-bae7-e840178d6c4d.jpeg) 42 | ![FL1KTaJXEAM-Vd7](https://user-images.githubusercontent.com/3810011/154728082-f7103b80-71d2-4d3e-85a7-59b0777f60b1.jpeg) 43 | 44 | -------------------------------------------------------------------------------- /colcon.pkg: -------------------------------------------------------------------------------- 1 | # Configuration file for colcon (https://colcon.readthedocs.io). 2 | # 3 | # Please see the doc for the details of the spec: 4 | # - https://colcon.readthedocs.io/en/released/user/configuration.html#colcon-pkg-files 5 | 6 | { 7 | "dependencies": ["Gazebo"], 8 | } 9 | -------------------------------------------------------------------------------- /dependency_repos.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | grid_map: 3 | type: git 4 | url: https://github.com/ANYbotics/grid_map.git 5 | version: rolling 6 | navigation2: 7 | type: git 8 | url: https://github.com/ros-planning/navigation2.git 9 | version: main 10 | -------------------------------------------------------------------------------- /env-hooks/gazebo_plugins.sh.in: -------------------------------------------------------------------------------- 1 | # generated from gazebo_plugins/env-hooks/gazebo_plugins.sh.in 2 | 3 | # detect if running on Darwin platform 4 | _UNAME=`uname -s` 5 | _IS_DARWIN=0 6 | if [ "$_UNAME" = "Darwin" ]; then 7 | _IS_DARWIN=1 8 | fi 9 | unset _UNAME 10 | 11 | if [ $_IS_DARWIN -eq 0 ]; then 12 | ament_prepend_unique_value LD_LIBRARY_PATH @GAZEBO_PLUGIN_PATH@ 13 | else 14 | ament_prepend_unique_value DYLD_LIBRARY_PATH @GAZEBO_PLUGIN_PATH@ 15 | fi 16 | unset _IS_DARWIN 17 | 18 | -------------------------------------------------------------------------------- /include/gazebo_plugins/gazebo_ros_gridmap.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 GAZEBO_PLUGINS__GAZEBO_ROS_GRIDMAP_HPP_ 16 | #define GAZEBO_PLUGINS__GAZEBO_ROS_GRIDMAP_HPP_ 17 | 18 | // For std::unique_ptr, could be removed 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include "gazebo/physics/physics.hh" 25 | #include "gazebo/common/common.hh" 26 | #include "gazebo/gazebo.hh" 27 | 28 | #include "grid_map_ros/grid_map_ros.hpp" 29 | 30 | namespace gazebo_plugins 31 | { 32 | // Forward declaration of private data class. 33 | class GazeboRosGridmapPrivate; 34 | 35 | typedef Eigen::Matrix MatrixXf; 36 | 37 | class GazeboRosGridmap : public gazebo::WorldPlugin 38 | { 39 | public: 40 | /// Constructor 41 | GazeboRosGridmap(); 42 | 43 | /// Destructor 44 | virtual ~GazeboRosGridmap(); 45 | 46 | /// Gazebo calls this when the plugin is loaded. 47 | /// \param[in] model Pointer to parent model. Other plugin types will expose different entities, 48 | /// such as `gazebo::sensors::SensorPtr`, `gazebo::physics::WorldPtr`, 49 | /// `gazebo::rendering::VisualPtr`, etc. 50 | /// \param[in] sdf SDF element containing user-defined parameters. 51 | void Load(gazebo::physics::WorldPtr model, sdf::ElementPtr sdf) override; 52 | 53 | protected: 54 | /// Optional callback to be called at every simulation iteration. 55 | virtual void OnUpdate(); 56 | 57 | bool get_surface( 58 | const ignition::math::Vector3d & central_point, 59 | const double min_z, const double max_z, 60 | const double resolution, gazebo::physics::RayShapePtr ray, 61 | double & height); 62 | void process_neighbour( 63 | const grid_map::Index & pos, double current_height, 64 | double resolution, std::stack & pending_poses, 65 | gazebo::physics::RayShapePtr ray, MatrixXf & em); 66 | 67 | void create_gridmap(); 68 | void create_octomap(); 69 | bool is_obstacle( 70 | const ignition::math::Vector3d & central_point, double surface, 71 | const double min_z, const double max_z, 72 | const double resolution, 73 | gazebo::physics::RayShapePtr ray); 74 | bool voxel_is_obstacle( 75 | const ignition::math::Vector3d & central_point, 76 | const double resolution, 77 | gazebo::physics::RayShapePtr ray); 78 | 79 | void flood( 80 | const grid_map::Position & current_pos, double current_height, 81 | double resolution, gazebo::physics::RayShapePtr ray); 82 | void create_occupancy(); 83 | 84 | private: 85 | /// Recommended PIMPL pattern. This variable should hold all private 86 | /// data members. 87 | std::unique_ptr impl_; 88 | }; 89 | } // namespace gazebo_plugins 90 | 91 | #endif // GAZEBO_PLUGINS__GAZEBO_ROS_GRIDMAP_HPP_ 92 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gazebo_gridmap_plugin 5 | 0.0.0 6 | 7 | Gazebo plugins for creating gridmaps 8 | 9 | 10 | Francisco Martín Rico 11 | 12 | Apache 2.0 13 | 14 | Francisco Martín Rico 15 | 16 | ament_cmake 17 | 18 | camera_info_manager 19 | cv_bridge 20 | geometry_msgs 21 | image_transport 22 | nav_msgs 23 | octomap 24 | octomap_msgs 25 | octomap_ros 26 | nav2_msgs 27 | sensor_msgs 28 | std_msgs 29 | std_srvs 30 | tf2_geometry_msgs 31 | tf2_ros 32 | trajectory_msgs 33 | grid_map_ros 34 | grid_map_msgs 35 | grid_map_core 36 | 37 | gazebo_dev 38 | gazebo_msgs 39 | gazebo_ros 40 | rclcpp 41 | 42 | gazebo_dev 43 | gazebo_msgs 44 | gazebo_ros 45 | rclcpp 46 | 47 | ament_cmake_gtest 48 | ament_lint_auto 49 | ament_lint_common 50 | cv_bridge 51 | 52 | 53 | ament_cmake 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/gazebo_ros_gridmap.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 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "grid_map_ros/grid_map_ros.hpp" 31 | #include "grid_map_msgs/msg/grid_map.hpp" 32 | 33 | #include 34 | #include "octomap_msgs/conversions.h" 35 | #include "octomap_ros/conversions.hpp" 36 | 37 | namespace gazebo_plugins 38 | { 39 | 40 | typedef struct 41 | { 42 | grid_map::Position pos; 43 | double last_height {0.0}; 44 | } TFloodCell; 45 | 46 | /// Class to hold private data members (PIMPL pattern) 47 | class GazeboRosGridmapPrivate 48 | { 49 | public: 50 | GazeboRosGridmapPrivate() 51 | : gridmap_() {} 52 | 53 | /// Connection to world update event. Callback is called while this is alive. 54 | gazebo::event::ConnectionPtr update_connection_; 55 | 56 | /// Node for ROS communication. 57 | gazebo_ros::Node::SharedPtr ros_node_; 58 | 59 | gazebo::physics::WorldPtr world_; 60 | bool surface_started_{false}; 61 | bool surface_finished_{false}; 62 | bool occupancy_started_{false}; 63 | bool occupancy_finished_{false}; 64 | bool octomap_started_{false}; 65 | bool octomap_finished_{false}; 66 | 67 | grid_map::GridMap gridmap_; 68 | bool octomap_created_{false}; 69 | std::unique_ptr octomap_; 70 | 71 | double center_x_; 72 | double center_y_; 73 | double center_z_; 74 | double min_scan_x_; 75 | double min_scan_y_; 76 | double min_scan_z_; 77 | double max_scan_x_; 78 | double max_scan_y_; 79 | double max_scan_z_; 80 | double min_height_; 81 | double max_height_; 82 | double resolution_; 83 | 84 | rclcpp::Publisher::SharedPtr gridmap_pub_; 85 | rclcpp::Publisher::SharedPtr octomap_pub_; 86 | }; 87 | 88 | GazeboRosGridmap::GazeboRosGridmap() 89 | : impl_(std::make_unique()) 90 | { 91 | } 92 | 93 | GazeboRosGridmap::~GazeboRosGridmap() 94 | { 95 | } 96 | 97 | void GazeboRosGridmap::Load(gazebo::physics::WorldPtr _parent, sdf::ElementPtr sdf) 98 | { 99 | // Create a GazeboRos node instead of a common ROS node. 100 | // Pass it SDF parameters so common options like namespace and remapping 101 | // can be handled. 102 | impl_->ros_node_ = gazebo_ros::Node::Get(sdf); 103 | 104 | // The model pointer gives you direct access to the physics object, 105 | // for example: 106 | RCLCPP_INFO(impl_->ros_node_->get_logger(), "Loaded %s", _parent->Name().c_str()); 107 | 108 | // Create a connection so the OnUpdate function is called at every simulation 109 | // iteration. Remove this call, the connection and the callback if not needed. 110 | impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( 111 | std::bind(&GazeboRosGridmap::OnUpdate, this)); 112 | 113 | impl_->gridmap_pub_ = impl_->ros_node_->create_publisher( 114 | "grid_map", rclcpp::QoS(1).transient_local()); 115 | 116 | impl_->octomap_pub_ = impl_->ros_node_->create_publisher( 117 | "octomap", rclcpp::QoS(1).transient_local()); 118 | 119 | auto logger = impl_->ros_node_->get_logger(); 120 | 121 | if (!sdf->HasElement("center_x")) { 122 | RCLCPP_INFO( 123 | logger, "Gidmap plugin missing wasn't set," 124 | "therefore it's been set as '0.0'."); 125 | impl_->center_x_ = 0.0; 126 | } else { 127 | impl_->center_x_ = sdf->GetElement("center_x")->Get(); 128 | } 129 | 130 | if (!sdf->HasElement("center_y")) { 131 | RCLCPP_INFO( 132 | logger, "Gidmap plugin missing wasn't set," 133 | "therefore it's been set as '0.0'."); 134 | impl_->center_y_ = 0.0; 135 | } else { 136 | impl_->center_y_ = sdf->GetElement("center_y")->Get(); 137 | } 138 | if (!sdf->HasElement("center_z")) { 139 | RCLCPP_INFO( 140 | logger, "Gidmap plugin missing wasn't set," 141 | "therefore it's been set as '0.0'."); 142 | impl_->center_z_ = 0.0; 143 | } else { 144 | impl_->center_z_ = sdf->GetElement("center_z")->Get(); 145 | } 146 | 147 | if (!sdf->HasElement("min_scan_x")) { 148 | RCLCPP_INFO( 149 | logger, "Gidmap plugin missing wasn't set," 150 | "therefore it's been set as '-5.0'."); 151 | impl_->min_scan_x_ = -5.0; 152 | } else { 153 | impl_->min_scan_x_ = sdf->GetElement("min_scan_x")->Get(); 154 | } 155 | 156 | if (!sdf->HasElement("min_scan_y")) { 157 | RCLCPP_INFO( 158 | logger, "Gidmap plugin missing wasn't set," 159 | "therefore it's been set as '-5.0'."); 160 | impl_->min_scan_y_ = -5.0; 161 | } else { 162 | impl_->min_scan_y_ = sdf->GetElement("min_scan_y")->Get(); 163 | } 164 | 165 | if (!sdf->HasElement("min_scan_z")) { 166 | RCLCPP_INFO( 167 | logger, "Gidmap plugin missing wasn't set," 168 | "therefore it's been set as '-5.0'."); 169 | impl_->min_scan_z_ = -5.0; 170 | } else { 171 | impl_->min_scan_z_ = sdf->GetElement("min_scan_z")->Get(); 172 | } 173 | 174 | if (!sdf->HasElement("max_scan_x")) { 175 | RCLCPP_INFO( 176 | logger, "Gidmap plugin missing wasn't set," 177 | "therefore it's been set as '5.0'."); 178 | impl_->max_scan_x_ = 5.0; 179 | } else { 180 | impl_->max_scan_x_ = sdf->GetElement("max_scan_x")->Get(); 181 | } 182 | 183 | if (!sdf->HasElement("max_scan_y")) { 184 | RCLCPP_INFO( 185 | logger, "Gidmap plugin missing wasn't set," 186 | "therefore it's been set as '5.0'."); 187 | impl_->max_scan_y_ = 5.0; 188 | } else { 189 | impl_->max_scan_y_ = sdf->GetElement("max_scan_y")->Get(); 190 | } 191 | 192 | if (!sdf->HasElement("max_scan_z")) { 193 | RCLCPP_INFO( 194 | logger, "Gidmap plugin missing wasn't set," 195 | "therefore it's been set as '5.0'."); 196 | impl_->max_scan_z_ = 5.0; 197 | } else { 198 | impl_->max_scan_z_ = sdf->GetElement("max_scan_z")->Get(); 199 | } 200 | 201 | if (!sdf->HasElement("min_height")) { 202 | RCLCPP_INFO( 203 | logger, "Gidmap plugin missing wasn't set," 204 | "therefore it's been set as '0.1'."); 205 | impl_->min_height_ = 0.1; 206 | } else { 207 | impl_->min_height_ = sdf->GetElement("min_height")->Get(); 208 | } 209 | 210 | if (!sdf->HasElement("max_height")) { 211 | RCLCPP_INFO( 212 | logger, "Gidmap plugin missing wasn't set," 213 | "therefore it's been set as '1.0'."); 214 | impl_->max_height_ = 1.0; 215 | } else { 216 | impl_->max_height_ = sdf->GetElement("max_height")->Get(); 217 | } 218 | 219 | if (!sdf->HasElement("resolution")) { 220 | RCLCPP_INFO( 221 | logger, "Gidmap plugin missing wasn't set," 222 | "therefore it's been set as '0.1'."); 223 | impl_->resolution_ = 0.1; 224 | } else { 225 | impl_->resolution_ = sdf->GetElement("resolution")->Get(); 226 | } 227 | impl_->world_ = _parent; 228 | } 229 | 230 | void GazeboRosGridmap::OnUpdate() 231 | { 232 | if (!impl_->surface_started_) { 233 | impl_->surface_started_ = true; 234 | create_gridmap(); 235 | impl_->surface_finished_ = true; 236 | } 237 | 238 | if (impl_->surface_finished_ && !impl_->octomap_started_) { 239 | impl_->octomap_started_ = true; 240 | create_octomap(); 241 | impl_->octomap_finished_ = true; 242 | } 243 | 244 | if (impl_->octomap_finished_ && !impl_->occupancy_started_) { 245 | impl_->occupancy_started_ = true; 246 | create_occupancy(); 247 | impl_->occupancy_finished_ = true; 248 | 249 | std::unique_ptr message; 250 | message = grid_map::GridMapRosConverter::toMessage(impl_->gridmap_); 251 | impl_->gridmap_pub_->publish(std::move(message)); 252 | 253 | octomap_msgs::msg::Octomap octo_message; 254 | octomap_msgs::fullMapToMsg(*(impl_->octomap_), octo_message); 255 | octo_message.header.frame_id = "map"; 256 | octo_message.header.stamp = impl_->ros_node_->get_clock()->now(); 257 | impl_->octomap_pub_->publish(octo_message); 258 | } 259 | } 260 | 261 | 262 | bool GazeboRosGridmap::get_surface( 263 | const ignition::math::Vector3d & central_point, 264 | const double min_z, const double max_z, 265 | const double resolution, gazebo::physics::RayShapePtr ray, 266 | double & height) 267 | { 268 | ignition::math::Vector3d start_point = central_point; 269 | ignition::math::Vector3d end_point = central_point; 270 | start_point.Z() = max_z; 271 | end_point.Z() = min_z; 272 | 273 | double dist = 0.0; 274 | std::string entity_name; 275 | std::string current_entity; 276 | 277 | ray->SetPoints(start_point, end_point); 278 | ray->GetIntersection(dist, current_entity); 279 | 280 | if (current_entity == "") { 281 | return false; 282 | } else { 283 | height = start_point.Z() - dist; 284 | return true; 285 | } 286 | } 287 | 288 | 289 | bool GazeboRosGridmap::is_obstacle( 290 | const ignition::math::Vector3d & central_point, double surface, 291 | const double min_z, const double max_z, 292 | const double resolution, 293 | gazebo::physics::RayShapePtr ray) 294 | { 295 | ignition::math::Vector3d start_point = central_point; 296 | ignition::math::Vector3d end_point = central_point; 297 | 298 | std::string entity; 299 | double dist; 300 | 301 | start_point.Z() = surface + min_z; 302 | 303 | end_point.Z() = surface + max_z; 304 | 305 | ray->SetPoints(start_point, end_point); 306 | ray->GetIntersection(dist, entity); 307 | 308 | if (dist < max_z - min_z) { 309 | return true; 310 | } 311 | 312 | return false; 313 | } 314 | 315 | void GazeboRosGridmap::create_gridmap() 316 | { 317 | double & resolution = impl_->resolution_; 318 | double & max_height = impl_->max_height_; 319 | double & min_height = impl_->min_height_; 320 | double & center_x = impl_->center_x_; 321 | double & center_y = impl_->center_y_; 322 | double & min_x = impl_->min_scan_x_; 323 | double & min_y = impl_->min_scan_y_; 324 | double & min_z = impl_->min_scan_z_; 325 | double & max_x = impl_->max_scan_x_; 326 | double & max_y = impl_->max_scan_y_; 327 | double & max_z = impl_->max_scan_z_; 328 | 329 | double size_x = max_x - min_x; 330 | double size_y = max_y - min_y; 331 | 332 | std::cerr << "Creating surface" << std::endl; 333 | impl_->gridmap_.setFrameId("map"); 334 | impl_->gridmap_.setGeometry( 335 | grid_map::Length(size_x, size_y), resolution, 336 | grid_map::Position(center_x, center_y)); 337 | 338 | gazebo::physics::PhysicsEnginePtr engine = impl_->world_->Physics(); 339 | engine->InitForThread(); 340 | gazebo::physics::RayShapePtr ray = 341 | boost::dynamic_pointer_cast( 342 | engine->CreateShape("ray", gazebo::physics::CollisionPtr())); 343 | 344 | impl_->gridmap_.add("elevation"); 345 | impl_->gridmap_.add("occupancy"); 346 | grid_map::Position current_pos(center_x, center_y); 347 | 348 | double height = 0.0; 349 | ignition::math::Vector3d point(current_pos.x(), current_pos.y(), 0); 350 | if (get_surface(point, min_z, max_z, resolution, ray, height)) { 351 | impl_->gridmap_.atPosition("elevation", current_pos) = height; 352 | } 353 | 354 | flood(current_pos, height, resolution, ray); 355 | 356 | std::cout << "Surface completed " << std::endl; 357 | } 358 | 359 | void 360 | GazeboRosGridmap::process_neighbour( 361 | const grid_map::Index & pos, double current_height, 362 | double resolution, std::stack & pending_poses, 363 | gazebo::physics::RayShapePtr ray, MatrixXf & em) 364 | { 365 | const double thr = resolution * 1.0; 366 | double height; 367 | 368 | grid_map::Position metric_pos; 369 | 370 | if (!impl_->gridmap_.getPosition(pos, metric_pos)) { 371 | return; 372 | } 373 | 374 | grid_map::Index debug_idx; 375 | grid_map::Position debug_pos(-11.0, 13.10); 376 | impl_->gridmap_.getIndex(debug_pos, debug_idx); 377 | 378 | ignition::math::Vector3d point(metric_pos.x(), metric_pos.y(), 0.0); 379 | 380 | bool there_is_surface = get_surface( 381 | point, current_height - (thr), current_height + (thr), resolution, ray, height); 382 | bool already_visited = !std::isnan(em(pos(0), pos(1))); 383 | double diff = height - em(pos(0), pos(1)); 384 | 385 | 386 | if (there_is_surface && (!already_visited)) { 387 | em(pos(0), pos(1)) = height; 388 | pending_poses.push(pos); 389 | } else if (there_is_surface && diff > thr) { 390 | em(pos(0), pos(1)) = height; 391 | 392 | grid_map::Index n_pos(pos); 393 | n_pos(0) = n_pos(0) + 1; 394 | pending_poses.push(n_pos); 395 | 396 | grid_map::Index w_pos(pos); 397 | w_pos(1) = w_pos(1) + 1; 398 | pending_poses.push(w_pos); 399 | 400 | grid_map::Index e_pos(pos); 401 | e_pos(1) = e_pos(1) - 1; 402 | pending_poses.push(e_pos); 403 | 404 | grid_map::Index s_pos(pos); 405 | s_pos(0) = s_pos(0) - 1; 406 | pending_poses.push(s_pos); 407 | } 408 | } 409 | 410 | void 411 | GazeboRosGridmap::flood( 412 | const grid_map::Position & current_pos, 413 | double current_height, double resolution, gazebo::physics::RayShapePtr ray) 414 | { 415 | MatrixXf em(impl_->gridmap_.getSize()(0), impl_->gridmap_.getSize()(1)); 416 | for (auto i = 0; i < impl_->gridmap_.getSize()(0); i++) { 417 | for (auto j = 0; j < impl_->gridmap_.getSize()(1); j++) { 418 | em(i, j) = NAN; 419 | } 420 | } 421 | 422 | std::stack pending_poses; 423 | 424 | grid_map::Index init_index; 425 | if (!impl_->gridmap_.getIndex(current_pos, init_index)) { 426 | return; 427 | } 428 | 429 | pending_poses.push(init_index); 430 | em(init_index(0), init_index(1)) = current_height; 431 | 432 | int counter_flood = 0; 433 | while (!pending_poses.empty()) { 434 | grid_map::Index cell = pending_poses.top(); 435 | pending_poses.pop(); 436 | 437 | process_neighbour(cell, current_height, resolution, pending_poses, ray, em); 438 | current_height = em(cell(0), cell(1)); 439 | 440 | // Check N cell 441 | grid_map::Index n_pos(cell); 442 | n_pos(0) = n_pos(0) + 1; 443 | process_neighbour(n_pos, current_height, resolution, pending_poses, ray, em); 444 | 445 | grid_map::Index w_pos(cell); 446 | w_pos(1) = w_pos(1) + 1; 447 | process_neighbour(w_pos, current_height, resolution, pending_poses, ray, em); 448 | 449 | grid_map::Index e_pos(cell); 450 | e_pos(1) = e_pos(1) - 1; 451 | process_neighbour(e_pos, current_height, resolution, pending_poses, ray, em); 452 | 453 | grid_map::Index s_pos(cell); 454 | s_pos(0) = s_pos(0) - 1; 455 | process_neighbour(s_pos, current_height, resolution, pending_poses, ray, em); 456 | 457 | counter_flood++; 458 | } 459 | 460 | for (auto i = 0; i < impl_->gridmap_.getSize()(0); i++) { 461 | for (auto j = 0; j < impl_->gridmap_.getSize()(1); j++) { 462 | grid_map::Index map_index(i, j); 463 | impl_->gridmap_.at("elevation", map_index) = em(i, j); 464 | } 465 | } 466 | 467 | // Fill holes 468 | double height = 0.0; 469 | for (grid_map::GridMapIterator grid_iterator(impl_->gridmap_); !grid_iterator.isPastEnd(); 470 | ++grid_iterator) 471 | { 472 | grid_map::Position pos; 473 | impl_->gridmap_.getPosition(*grid_iterator, pos); 474 | 475 | double cell_height = impl_->gridmap_.atPosition("elevation", pos); 476 | if (cell_height == NAN) { 477 | impl_->gridmap_.atPosition("elevation", pos) = height; 478 | } else { 479 | height = cell_height; 480 | } 481 | } 482 | } 483 | 484 | void 485 | GazeboRosGridmap::create_occupancy() 486 | { 487 | std::cerr << "Creating occupancy" << std::endl; 488 | 489 | float res = static_cast(impl_->resolution_); 490 | double & max_height = impl_->max_height_; 491 | double & min_height = impl_->min_height_; 492 | 493 | gazebo::physics::PhysicsEnginePtr engine = impl_->world_->Physics(); 494 | engine->InitForThread(); 495 | gazebo::physics::RayShapePtr ray = 496 | boost::dynamic_pointer_cast( 497 | engine->CreateShape("ray", gazebo::physics::CollisionPtr())); 498 | 499 | 500 | for (auto i = 0; i < impl_->gridmap_.getSize()(0); i++) { 501 | for (auto j = 0; j < impl_->gridmap_.getSize()(1); j++) { 502 | grid_map::Index idx(i, j); 503 | impl_->gridmap_.at("occupancy", idx) = 1.0; 504 | } 505 | } 506 | 507 | for (auto i = 0; i < impl_->gridmap_.getSize()(0); i++) { 508 | for (auto j = 0; j < impl_->gridmap_.getSize()(1); j++) { 509 | grid_map::Index idx(i, j); 510 | float height = impl_->gridmap_.at("elevation", idx); 511 | 512 | float diff_n(0.0), diff_s(0.0), diff_w(0.0), diff_e(0.0); 513 | 514 | // Check N cell 515 | grid_map::Index n_pos(idx); 516 | n_pos(0) = n_pos(0) + 1; 517 | if (impl_->gridmap_.isValid(n_pos, "elevation")) { 518 | diff_n = abs(impl_->gridmap_.at("elevation", n_pos) - height); 519 | } 520 | 521 | grid_map::Index w_pos(idx); 522 | w_pos(1) = w_pos(1) + 1; 523 | if (impl_->gridmap_.isValid(w_pos, "elevation")) { 524 | diff_w = abs(impl_->gridmap_.at("elevation", w_pos) - height); 525 | } 526 | 527 | grid_map::Index e_pos(idx); 528 | e_pos(1) = e_pos(1) - 1; 529 | if (impl_->gridmap_.isValid(e_pos, "elevation")) { 530 | diff_e = abs(impl_->gridmap_.at("elevation", e_pos) - height); 531 | } 532 | 533 | grid_map::Index s_pos(idx); 534 | s_pos(0) = s_pos(0) - 1; 535 | if (impl_->gridmap_.isValid(s_pos, "elevation")) { 536 | diff_s = abs(impl_->gridmap_.at("elevation", s_pos) - height); 537 | } 538 | 539 | grid_map::Position pos; 540 | impl_->gridmap_.getPosition(n_pos, pos); 541 | ignition::math::Vector3d point(pos(0), pos(1), 0.0); 542 | 543 | bool obstacle = is_obstacle(point, height, min_height, max_height, res, ray); 544 | 545 | if (obstacle || diff_n > res || diff_s > res || diff_w > res || diff_e > res) { 546 | impl_->gridmap_.at("occupancy", idx) = 254.0; 547 | } 548 | } 549 | } 550 | 551 | for (auto it = impl_->octomap_->begin_leafs(); it != impl_->octomap_->end_leafs(); ++it) { 552 | if (it->getOccupancy() > 0.0) { 553 | double wx = it.getX(); 554 | double wy = it.getY(); 555 | double wz = it.getZ(); 556 | 557 | grid_map::Position position(wx, wy); 558 | grid_map::Index idx; 559 | impl_->gridmap_.getIndex(position, idx); 560 | 561 | float height = impl_->gridmap_.at("elevation", idx); 562 | 563 | if ((wz > (impl_->min_height_ + height)) && (wz < (impl_->max_height_ + height))) { 564 | impl_->gridmap_.at("occupancy", idx) = 254.0; 565 | } 566 | } 567 | } 568 | 569 | std::cerr << "Occupancy created" << std::endl; 570 | } 571 | 572 | 573 | bool ray_collision( 574 | const ignition::math::Vector3d & start, 575 | const ignition::math::Vector3d & end, 576 | const double min_dist, 577 | gazebo::physics::RayShapePtr ray) 578 | { 579 | std::string entity; 580 | double dist; 581 | 582 | ray->SetPoints(start, end); 583 | ray->GetIntersection(dist, entity); 584 | 585 | return dist < min_dist; 586 | } 587 | 588 | // @brief a ray is casted from the bottom of the cube to the top by the center 589 | // to check occupancy 590 | // @param central_point center of the voxel to check 591 | // @param resolution size of the cube to check 592 | // @returns true if the cube centered in central_point and size resolution 593 | // is occupied, false otherwise 594 | bool GazeboRosGridmap::voxel_is_obstacle( 595 | const ignition::math::Vector3d & central_point, 596 | const double resolution, 597 | gazebo::physics::RayShapePtr ray) 598 | { 599 | ignition::math::Vector3d start_point_x = central_point; 600 | ignition::math::Vector3d end_point_x = central_point; 601 | ignition::math::Vector3d start_point_y = central_point; 602 | ignition::math::Vector3d end_point_y = central_point; 603 | ignition::math::Vector3d start_point_z = central_point; 604 | ignition::math::Vector3d end_point_z = central_point; 605 | 606 | // X line 607 | start_point_x.X() = start_point_x.X() - resolution / 2; 608 | end_point_x.X() = end_point_x.X() + resolution / 2; 609 | // Y line 610 | start_point_y.Y() = start_point_y.Y() - resolution / 2; 611 | end_point_y.Y() = end_point_y.Y() + resolution / 2; 612 | // Z line 613 | start_point_z.Z() = start_point_z.Z() - resolution / 2; 614 | end_point_z.Z() = end_point_z.Z() + resolution / 2; 615 | 616 | bool collision_x = ray_collision(start_point_x, end_point_x, resolution, ray); 617 | bool collision_xi = ray_collision(end_point_x, start_point_x, resolution, ray); 618 | bool collision_y = ray_collision(start_point_y, end_point_y, resolution, ray); 619 | bool collision_yi = ray_collision(end_point_y, start_point_y, resolution, ray); 620 | bool collision_z = ray_collision(start_point_z, end_point_z, resolution, ray); 621 | bool collision_zi = ray_collision(end_point_z, start_point_z, resolution, ray); 622 | // returns true if is there a collision in any axis 623 | return collision_x || collision_y || collision_z || collision_xi || collision_yi || collision_zi; 624 | } 625 | 626 | void GazeboRosGridmap::create_octomap() 627 | { 628 | gazebo::physics::PhysicsEnginePtr engine = impl_->world_->Physics(); 629 | engine->InitForThread(); 630 | gazebo::physics::RayShapePtr ray = 631 | boost::dynamic_pointer_cast( 632 | engine->CreateShape("ray", gazebo::physics::CollisionPtr())); 633 | 634 | double & resolution = impl_->resolution_; 635 | // double & max_height = impl_->max_height_; 636 | // double & min_height = impl_->min_height_; 637 | double & min_x = impl_->min_scan_x_; 638 | double & min_y = impl_->min_scan_y_; 639 | double & min_z = impl_->min_scan_z_; 640 | double & max_x = impl_->max_scan_x_; 641 | double & max_y = impl_->max_scan_y_; 642 | double & max_z = impl_->max_scan_z_; 643 | double filter_min; 644 | 645 | impl_->octomap_ = std::make_unique(resolution); 646 | 647 | std::cout << "Creating octomap" << std::endl; 648 | 649 | for (double i = min_x; i < max_x; i += resolution) { 650 | for (double j = min_y; j < max_y; j += resolution) { 651 | for (double k = min_z; k < max_z; k += resolution) { 652 | if (voxel_is_obstacle(ignition::math::Vector3d(i, j, k), resolution, ray)) { 653 | impl_->octomap_->updateNode(i, j, k, true); 654 | } 655 | } 656 | } 657 | } 658 | 659 | std::cout << "Octomap completed" << std::endl; 660 | } 661 | 662 | // Register this plugin with the simulator 663 | GZ_REGISTER_WORLD_PLUGIN(GazeboRosGridmap) 664 | } // namespace gazebo_plugins 665 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake_gtest REQUIRED) 2 | find_package(cv_bridge REQUIRED) 3 | 4 | option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF) 5 | 6 | # Worlds 7 | file(GLOB_RECURSE worlds RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "worlds/*.world") 8 | foreach(world ${worlds}) 9 | # Use configure_file so this is rerun each time make is invoked (as opposed to just cmake) 10 | configure_file(${world} ${world} COPYONLY) 11 | endforeach() 12 | 13 | # Tests 14 | set(tests 15 | test_gazebo_ros_gridmap 16 | ) 17 | 18 | foreach(test ${tests}) 19 | ament_add_gtest(${test} 20 | ${test}.cpp 21 | WORKING_DIRECTORY 22 | ${CMAKE_CURRENT_BINARY_DIR} 23 | # Long timeout because has to run gazebo several times 24 | TIMEOUT 25 | 120 26 | ) 27 | target_link_libraries(${test} 28 | gazebo_test_fixture 29 | ) 30 | ament_target_dependencies(${test} 31 | "cv_bridge" 32 | "gazebo_dev" 33 | "gazebo_msgs" 34 | "gazebo_ros" 35 | "geometry_msgs" 36 | "image_transport" 37 | "nav_msgs" 38 | "rclcpp" 39 | "sensor_msgs" 40 | "std_msgs" 41 | "std_srvs" 42 | "tf2" 43 | "tf2_geometry_msgs" 44 | "tf2_ros" 45 | "trajectory_msgs" 46 | ) 47 | if(ENABLE_PROFILER) 48 | target_include_directories(${test} PUBLIC ${ignition-common3_INCLUDE_DIRS}) 49 | target_link_libraries(${test} ${ignition-common3_LIBRARIES}) 50 | endif() 51 | endforeach() 52 | -------------------------------------------------------------------------------- /test/rviz/octomap_gridmap_plugin.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /GridMap1/Topic1 10 | - /GridMap2 11 | - /GridMap2/Topic1 12 | - /OccupancyGrid1 13 | - /OccupancyGrid1/Topic1 14 | Splitter Ratio: 0.5 15 | Tree Height: 719 16 | - Class: rviz_common/Selection 17 | Name: Selection 18 | - Class: rviz_common/Tool Properties 19 | Expanded: 20 | - /2D Goal Pose1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz_common/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz_common/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz_default_plugins/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Class: grid_map_rviz_plugin/GridMap 58 | Color: 200; 200; 200 59 | Color Layer: elevation 60 | Color Transformer: GridMapLayer 61 | Enabled: false 62 | Height Layer: elevation 63 | Height Transformer: GridMapLayer 64 | History Length: 1 65 | Invert Rainbow: false 66 | Max Color: 255; 255; 255 67 | Max Intensity: 10 68 | Min Color: 0; 0; 0 69 | Min Intensity: 0 70 | Name: GridMap 71 | Show Grid Lines: true 72 | Topic: 73 | Depth: 5 74 | Durability Policy: Transient Local 75 | Filter size: 10 76 | History Policy: System Default 77 | Reliability Policy: Reliable 78 | Value: /demo/grid_map 79 | Use Rainbow: true 80 | Value: false 81 | - Alpha: 1 82 | Autocompute Intensity Bounds: true 83 | Class: grid_map_rviz_plugin/GridMap 84 | Color: 200; 200; 200 85 | Color Layer: occupancy 86 | Color Transformer: IntensityLayer 87 | Enabled: false 88 | Height Layer: occupancy 89 | Height Transformer: Flat 90 | History Length: 1 91 | Invert Rainbow: false 92 | Max Color: 255; 255; 255 93 | Max Intensity: 10 94 | Min Color: 0; 0; 0 95 | Min Intensity: 0 96 | Name: GridMap 97 | Show Grid Lines: true 98 | Topic: 99 | Depth: 5 100 | Durability Policy: Transient Local 101 | Filter size: 10 102 | History Policy: Keep Last 103 | Reliability Policy: Reliable 104 | Value: /demo/grid_map 105 | Use Rainbow: false 106 | Value: false 107 | - Class: octomap_rviz_plugins/OccupancyGrid 108 | Enabled: true 109 | Max. Height Display: 3.4028234663852886e+38 110 | Max. Octree Depth: 16 111 | Min. Height Display: -3.4028234663852886e+38 112 | Name: OccupancyGrid 113 | Topic: 114 | Depth: 5 115 | Durability Policy: Transient Local 116 | Filter size: 10 117 | History Policy: Keep Last 118 | Reliability Policy: Reliable 119 | Value: /demo/octomap 120 | Value: true 121 | Voxel Alpha: 1 122 | Voxel Coloring: Z-Axis 123 | Voxel Rendering: Occupied Voxels 124 | Enabled: true 125 | Global Options: 126 | Background Color: 48; 48; 48 127 | Fixed Frame: map 128 | Frame Rate: 30 129 | Name: root 130 | Tools: 131 | - Class: rviz_default_plugins/Interact 132 | Hide Inactive Objects: true 133 | - Class: rviz_default_plugins/MoveCamera 134 | - Class: rviz_default_plugins/Select 135 | - Class: rviz_default_plugins/FocusCamera 136 | - Class: rviz_default_plugins/Measure 137 | Line color: 128; 128; 0 138 | - Class: rviz_default_plugins/SetInitialPose 139 | Covariance x: 0.25 140 | Covariance y: 0.25 141 | Covariance yaw: 0.06853891909122467 142 | Topic: 143 | Depth: 5 144 | Durability Policy: Volatile 145 | History Policy: Keep Last 146 | Reliability Policy: Reliable 147 | Value: /initialpose 148 | - Class: rviz_default_plugins/SetGoal 149 | Topic: 150 | Depth: 5 151 | Durability Policy: Volatile 152 | History Policy: Keep Last 153 | Reliability Policy: Reliable 154 | Value: /goal_pose 155 | - Class: rviz_default_plugins/PublishPoint 156 | Single click: true 157 | Topic: 158 | Depth: 5 159 | Durability Policy: Volatile 160 | History Policy: Keep Last 161 | Reliability Policy: Reliable 162 | Value: /clicked_point 163 | Transformation: 164 | Current: 165 | Class: rviz_default_plugins/TF 166 | Value: true 167 | Views: 168 | Current: 169 | Class: rviz_default_plugins/Orbit 170 | Distance: 24.252487182617188 171 | Enable Stereo Rendering: 172 | Stereo Eye Separation: 0.05999999865889549 173 | Stereo Focal Distance: 1 174 | Swap Stereo Eyes: false 175 | Value: false 176 | Focal Point: 177 | X: -0.09614761918783188 178 | Y: -0.4160982072353363 179 | Z: -0.9541629552841187 180 | Focal Shape Fixed Size: true 181 | Focal Shape Size: 0.05000000074505806 182 | Invert Z Axis: false 183 | Name: Current View 184 | Near Clip Distance: 0.009999999776482582 185 | Pitch: 0.404796838760376 186 | Target Frame: 187 | Value: Orbit (rviz) 188 | Yaw: 5.4953932762146 189 | Saved: ~ 190 | Window Geometry: 191 | Displays: 192 | collapsed: false 193 | Height: 1016 194 | Hide Left Dock: false 195 | Hide Right Dock: false 196 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000051e0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 197 | Selection: 198 | collapsed: false 199 | Time: 200 | collapsed: false 201 | Tool Properties: 202 | collapsed: false 203 | Views: 204 | collapsed: false 205 | Width: 1920 206 | X: 0 207 | Y: 27 208 | -------------------------------------------------------------------------------- /test/test_gazebo_ros_gridmap.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 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | using namespace std::chrono_literals; 28 | 29 | 30 | int main(int argc, char ** argv) 31 | { 32 | rclcpp::init(argc, argv); 33 | ::testing::InitGoogleTest(&argc, argv); 34 | return RUN_ALL_TESTS(); 35 | } 36 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_ackermann_drive.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | 0 0 0.03 0 0 0 10 | 11 | 0 0 0 0 0 1.57 12 | 13 | 1326.0 14 | 0 -0.266 0.48 0 0 0 15 | 16 | 2581.13354740 17 | 0.0 18 | 591.30846112 19 | 0.0 20 | 0.0 21 | 2681.95008628 22 | 23 | 24 | 25 | 26 | 27 | 28 | model://prius_hybrid/meshes/Hybrid.obj 29 | 0.01 0.01 0.01 30 | 31 | Hybrid 32 |
false
33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | model://prius_hybrid/meshes/Hybrid.obj 42 | 0.01 0.01 0.01 43 | 44 | Hybrid_Interior 45 |
false
46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | model://prius_hybrid/meshes/Hybrid.obj 55 | 0.01 0.01 0.01 56 | 57 | Hybrid_Windows 58 |
false
59 | 60 | 61 | 62 | 63 | 64 | 65 | 0.0 0.05 0.625 0 0 0 66 | 67 | 68 | 1.7526 2.1 0.95 69 | 70 | 71 | 72 | 73 | 74 | 0.0 -2.0 0.458488 0.0 0 0 75 | 76 | 77 | 1.337282 0.48 0.566691 78 | 79 | 80 | 81 | 82 | 83 | 0.0 -1.900842 0.676305 0.341247 0 0 84 | 85 | 86 | 1.597968 0.493107 0.265468 87 | 88 | 89 | 90 | 91 | 92 | 0.0 -0.875105 1.032268 0.335476 0 0 93 | 94 | 95 | 1.168381 1.654253 0.272347 96 | 97 | 98 | 99 | 100 | 101 | 0.0 0.161236 1.386042 0.135030 0 0 102 | 103 | 104 | 1.279154 0.625988 0.171868 105 | 106 | 107 | 108 | 109 | 110 | 0.0 0.817696 1.360069 -0.068997 0 0 111 | 112 | 113 | 1.285130 0.771189 0.226557 114 | 115 | 116 | 117 | 118 | 119 | 0.0 1.640531 1.175126 -0.262017 0 0 120 | 121 | 122 | 1.267845 1.116344 0.244286 123 | 124 | 125 | 126 | 127 | 128 | 0.0 1.637059 0.888180 0.0 0 0 129 | 130 | 131 | 1.788064 1.138988 0.482746 132 | 133 | 134 | 135 | 136 | 137 | 0.0 2.054454 0.577870 0.0 0 0 138 | 139 | 140 | 1.781650 0.512093 0.581427 141 | 142 | 143 | 144 | 145 | 146 | 147 | 0.627868 0.357734 0.988243 -1.302101 0 1.57 148 | 149 | 1.0 150 | 151 | 0.14583300 152 | 0 153 | 0 154 | 0.14583300 155 | 0 156 | 0.12500000 157 | 158 | 159 | 160 | 161 | 162 | 0.178172 163 | 0.041845 164 | 165 | 166 | 167 | 168 | 169 | 0.003 170 | 171 | 172 | 173 | 174 | 175 | 0 0 0 1.302101 0 0 176 | 177 | 178 | model://prius_hybrid/meshes/Hybrid.obj 179 | 0.01 0.01 0.01 180 | 181 | Steering_Wheel 182 |
true
183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 1.41 0.76 0.3 0 0 1.57 191 | 192 | 11 193 | 194 | 0.58631238 195 | 0.0 196 | 0.33552910 197 | 0.0 198 | 0.0 199 | 0.33552910 200 | 201 | 202 | 203 | 204 | 0.04 0.0 0.0 0 0 0 205 | 206 | 207 | model://prius_hybrid/meshes/Hybrid.obj 208 | 0.01 0.01 0.01 209 | 210 | Wheel_Front_Left_ 211 |
true
212 | 213 | 214 | 215 | 216 | 217 | 218 | 0.0 0.0 0.0 0 1.5707963267948966 0 219 | 220 | 221 | 0.31265 222 | 223 | 224 | 225 | 226 | 227 | 0.9 228 | 0.9 229 | 0.0 230 | 0.0 231 | 232 | 233 | 234 | 235 | 0.001 236 | 1e9 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 1.41 -0.76 0.3 0 0 1.57 246 | 247 | 11 248 | 249 | 0.58631238 250 | 0.0 251 | 0.33552910 252 | 0.0 253 | 0.0 254 | 0.33552910 255 | 256 | 257 | 258 | -0.04 0.0 0.0 0 0 0 259 | 0 0 0.0 0 0 0 260 | 261 | 262 | model://prius_hybrid/meshes/Hybrid.obj 263 | 0.01 0.01 0.01 264 | 265 | Wheel_Front_Right 266 |
true
267 | 268 | 269 | 270 | 271 | 272 | 273 | 0.0 0.0 0.0 0 1.5707963267948966 0 274 | 275 | 276 | 0.31265 277 | 278 | 279 | 280 | 281 | 282 | 0.9 283 | 0.9 284 | 0.0 285 | 0.0 286 | 287 | 288 | 289 | 290 | 0.001 291 | 1e9 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | -1.45 0.786 0.3 0 0 1.57 302 | 303 | 11 304 | 305 | 0.58631238 306 | 0.0 307 | 0.33552910 308 | 0.0 309 | 0.0 310 | 0.33552910 311 | 312 | 313 | 314 | 315 | 0.04 0.0 0.0 0 0 0 316 | 317 | 318 | model://prius_hybrid/meshes/Hybrid.obj 319 | 0.01 0.01 0.01 320 | 321 | Wheel_Front_Left_ 322 |
true
323 | 324 | 325 | 326 | 327 | 328 | 329 | 0.0 0.0 0.0 0 1.5707963267948966 0 330 | 331 | 332 | 0.31265 333 | 334 | 335 | 336 | 337 | 338 | 1.1 339 | 1.1 340 | 0.0 341 | 0.0 342 | 343 | 344 | 345 | 346 | 0.001 347 | 1e9 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | -1.45 -0.786 0.3 0 0 1.57 357 | 358 | 11 359 | 360 | 0.58631238 361 | 0.0 362 | 0.33552910 363 | 0.0 364 | 0.0 365 | 0.33552910 366 | 367 | 368 | 369 | -0.04 0.0 0.0 0 0 0 370 | 371 | 372 | model://prius_hybrid/meshes/Hybrid.obj 373 | 0.01 0.01 0.01 374 | 375 | Wheel_Front_Right 376 |
true
377 | 378 | 379 | 380 | 381 | 382 | 383 | 0.0 0.0 0.0 0 1.5707963267948966 0 384 | 385 | 386 | 0.31265 387 | 388 | 389 | 390 | 391 | 392 | 1.1 393 | 1.1 394 | 0.0 395 | 0.0 396 | 397 | 398 | 399 | 400 | 0.001 401 | 1e9 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 0 0 0 -0.08726646259971647 0 0 410 | front_left_wheel 411 | chassis 412 | 413 | 0 0 1 414 | 415 | 416 | -0.8727 417 | 0.8727 418 | 419 | 420 | 421 | 1 0 0 422 | 423 | 18.0474092253 424 | 425 | 426 | 427 | 428 | 429 | 0 0 0 -0.08726646259971647 0 0 430 | front_right_wheel 431 | chassis 432 | 433 | 0 0 1 434 | 435 | 436 | -0.8727 437 | 0.8727 438 | 439 | 440 | 441 | 1 0 0 442 | 443 | 18.0474092253 444 | 445 | 446 | 447 | 448 | 449 | -1.45 0 0.3 0 0 0 450 | 451 | 30.0 452 | 453 | 0.08437499999999999 454 | 0.0 455 | 4.64581 456 | 0.0 457 | 0.0 458 | 4.64581 459 | 460 | 461 | 462 | 0.0 0.0 0.0 0 1.5707963267948966 0 463 | 464 | 465 | 1.357 466 | 0.075 467 | 468 | 469 | 470 | 474 | 475 | 476 | 477 | 478 | 479 | rear_axle 480 | chassis 481 | 482 | 0 1 0 483 | 484 | -0.05089058524173028 485 | 0.05089058524173028 486 | 487 | 488 | 20000.0 489 | 2000.0 490 | 491 | 492 | 493 | 494 | rear_left_wheel 495 | rear_axle 496 | 497 | 1 0 0 498 | 499 | 12.031606150200002 500 | 501 | 502 | 503 | 504 | rear_right_wheel 505 | rear_axle 506 | 507 | 1 0 0 508 | 509 | 12.031606150200002 510 | 511 | 512 | 513 | 514 | 515 | -0.002 0 0 0 0 0 516 | chassis 517 | steering_wheel 518 | 519 | -0.964118 0.000768 0.265556 520 | 521 | -7.85 522 | 7.85 523 | 524 | 525 | 1.0 526 | 527 | true 528 | 529 | 530 | 531 | 1 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | test 540 | cmd_vel:=cmd_test 541 | odom:=odom_test 542 | distance:=distance_test 543 | 544 | 545 | 100.0 546 | 547 | 548 | front_left_combined_joint 549 | front_right_combined_joint 550 | rear_left_wheel_joint 551 | rear_right_wheel_joint 552 | front_left_combined_joint 553 | front_right_combined_joint 554 | steering_joint 555 | 556 | 557 | 558 | 0.6458 559 | 560 | 561 | 7.85 562 | 563 | 564 | 20 565 | 566 | 567 | 1500 0 1 568 | 0 0 569 | 1500 0 1 570 | 0 0 571 | 1000 0 1 572 | 0 0 573 | 574 | 575 | true 576 | true 577 | true 578 | true 579 | 580 | odom_test 581 | chassis 582 | 583 | 584 | 585 | 586 | 587 | 588 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_bumper.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://ground_plane 9 | 10 | 11 | 12 | 13 | 0.026 14 | 15 | 16 | 1.664e-5 17 | 1.664e-5 18 | 1.664e-5 19 | 0 20 | 0 21 | 0 22 | 23 | 24 | 25 | 26 | 27 | 0.04 28 | 29 | 30 | 31 | 32 | 33 | 34 | 0.04 35 | 36 | 37 | 38 | 39 | -1 40 | 41 | collision 42 | 43 | 44 | 45 | test 46 | bumper_states:=bumper_test 47 | 48 | world 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_camera.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 0.5 9 | 10 | 1.3962634 11 | 12 | 640 13 | 480 14 | R8G8B8 15 | 16 | 17 | 0.02 18 | 300 19 | 20 | 21 | gaussian 22 | 25 | 0.0 26 | 0.007 27 | 28 | 29 | 30 | 31 | test_cam 32 | camera1/image_raw:=camera/image_test 33 | camera_info:=camera_info_test 34 | 35 | 36 | 37 | 0.07 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_camera_16bit.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 0.5 9 | 10 | 1.3962634 11 | 12 | 640 13 | 480 14 | R16G16B16 15 | 16 | 17 | 0.02 18 | 300 19 | 20 | 21 | gaussian 22 | 25 | 0.0 26 | 0.007 27 | 28 | 29 | 30 | 31 | test_cam_16bit 32 | test_camera_name/image_raw:=image_test_16bit 33 | camera_info:=camera_info_test_16bit 34 | 35 | test_camera_name 36 | 37 | 0.07 38 | 0.0 39 | 0.0 40 | 0.0 41 | 0.0 42 | 0.0 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_camera_distortion_barrel.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://sun 8 | 9 | 10 | 11 | true 12 | 0.0 0.0 2.5 0.0 1.5707 0.0 13 | 14 | 15 | 30.0 16 | 17 | 0.927295218 18 | 19 | 1000 20 | 1000 21 | R8G8B8 22 | 23 | 24 | 0.02 25 | 300 26 | 27 | 31 | 32 | -0.1 33 | -0.1 34 | 0 35 | 0 36 | -0.1 37 |
0.5 0.5
38 | 39 | 40 | 41 | 42 | distorted_camera/image_raw:=distorted_image 43 | distorted_camera/camera_info:=distorted_info 44 | 45 | distorted_camera 46 | frame_name 47 | 0.07 48 | false 49 | 50 | 51 | 52 | 53 | 54 | 55 | true 56 | 0.0 0.0 2.5 0.0 1.5707 0.0 57 | 58 | 59 | 30.0 60 | 61 | 0.927295218 62 | 63 | 1000 64 | 1000 65 | R8G8B8 66 | 67 | 68 | 0.02 69 | 300 70 | 71 | 72 | 0.0 73 | 0.0 74 | 0.0 75 | 0.0 76 | 0.0 77 |
0.5 0.5
78 | 79 | 80 | 81 | 82 | undistorted_camera/image_raw:=undistorted_image 83 | 84 | undistorted_camera 85 | frame_name 86 | 87 | 88 | 89 | 90 | 91 | 92 | model://checkerboard_plane 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_camera_distortion_pincushion.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://sun 8 | 9 | 10 | 11 | true 12 | 0.0 0.0 2.5 0.0 1.5707 0.0 13 | 14 | 15 | 30.0 16 | 17 | 0.927295218 18 | 19 | 1000 20 | 1000 21 | R8G8B8 22 | 23 | 24 | 0.02 25 | 300 26 | 27 | 31 | 32 | 0.1 33 | 0.1 34 | 0 35 | 0 36 | 0.1 37 |
0.5 0.5
38 | 39 | 40 | 41 | 42 | distorted_camera/image_raw:=distorted_image 43 | distorted_camera/camera_info:=distorted_info 44 | 45 | distorted_camera 46 | frame_name 47 | 0.07 48 | false 49 | 50 | 51 | 52 | 53 | 54 | 55 | true 56 | 0.0 0.0 2.5 0.0 1.5707 0.0 57 | 58 | 59 | 30.0 60 | 61 | 0.927295218 62 | 63 | 1000 64 | 1000 65 | R8G8B8 66 | 67 | 68 | 0.02 69 | 300 70 | 71 | 72 | 0.0 73 | 0.0 74 | 0.0 75 | 0.0 76 | 0.0 77 |
0.5 0.5
78 | 79 | 80 | 81 | 82 | undistorted_camera/image_raw:=undistorted_image 83 | 84 | undistorted_camera 85 | frame_name 86 | 87 | 88 | 89 | 90 | 91 | 92 | model://checkerboard_plane 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_camera_triggered.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | true 11 | 0.0 0.0 0.5 0.0 0.0 0.0 12 | 13 | 14 | 0 15 | 1 16 | 17 | 1.3962634 18 | 19 | 640 20 | 480 21 | R8G8B8 22 | 23 | 24 | 0.02 25 | 300 26 | 27 | 28 | gaussian 29 | 32 | 0.0 33 | 0.007 34 | 35 | 36 | 37 | 38 | test_triggered_cam 39 | camera1/image_raw:=image_raw_test 40 | camera1/image_trigger:=image_trigger_test 41 | 42 | true 43 | 0.07 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_depth_camera.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 0.5 9 | 10 | 1.3962634 11 | 12 | 640 13 | 480 14 | R8G8B8 15 | 16 | 17 | 0.02 18 | 300 19 | 20 | 21 | gaussian 22 | 25 | 0.0 26 | 0.007 27 | 28 | 29 | 30 | 31 | test_cam 32 | camera1/image_raw:=camera/raw_image_test 33 | camera1/depth/image_raw:=camera/depth_image_test 34 | camera1/points:=camera/points_test 35 | 36 | 37 | 38 | 0.07 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_diff_drive.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | 0 0 0.325 0 -0 0 15 | 16 | 17 | -0.151427 -0 0.175 0 -0 0 18 | 19 | 1.14395 20 | 21 | 0.126164 22 | 0 23 | 0 24 | 0.416519 25 | 0 26 | 0.481014 27 | 28 | 29 | 30 | 31 | 32 | 2.01142 1 0.568726 33 | 34 | 35 | 36 | 37 | 38 | 39 | 2.01142 1 0.568726 40 | 41 | 42 | 43 | 44 | 45 | 46 | 0.554283 0.625029 -0.025 -1.5707 0 0 47 | 48 | 2 49 | 50 | 0.145833 51 | 0 52 | 0 53 | 0.145833 54 | 0 55 | 0.125 56 | 57 | 58 | 59 | 60 | 61 | 0.3 62 | 63 | 64 | 65 | 66 | 67 | 68 | 0.3 69 | 70 | 71 | 72 | 73 | 74 | 1 75 | 1 76 | 0 77 | 0 78 | 79 | 80 | 81 | 82 | 0 83 | 0.2 84 | 1e+13 85 | 1 86 | 0.01 87 | 0.01 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 0.554282 -0.625029 -0.025 -1.5707 0 0 96 | 97 | 2 98 | 99 | 0.145833 100 | 0 101 | 0 102 | 0.145833 103 | 0 104 | 0.125 105 | 106 | 107 | 108 | 109 | 110 | 0.3 111 | 112 | 113 | 114 | 115 | 116 | 117 | 0.3 118 | 119 | 120 | 121 | 122 | 123 | 1 124 | 1 125 | 0 126 | 0 127 | 128 | 129 | 130 | 131 | 0 132 | 0.2 133 | 1e+13 134 | 1 135 | 0.01 136 | 0.01 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | -0.957138 -0 -0.125 0 -0 0 145 | 146 | 1 147 | 148 | 0.1 149 | 0 150 | 0 151 | 0.1 152 | 0 153 | 0.1 154 | 155 | 156 | 157 | 158 | 159 | 0.2 160 | 161 | 162 | 163 | 164 | 165 | 166 | 0.2 167 | 168 | 169 | 170 | 171 | 172 | 173 | chassis 174 | left_wheel 175 | 176 | 0 0 1 177 | 178 | -1.79769e+308 179 | 1.79769e+308 180 | 181 | 182 | 183 | 184 | 185 | chassis 186 | right_wheel 187 | 188 | 0 0 1 189 | 190 | -1.79769e+308 191 | 1.79769e+308 192 | 193 | 194 | 195 | 196 | 197 | chassis 198 | caster 199 | 200 | 201 | 202 | 203 | /test 204 | cmd_vel:=cmd_test 205 | odom:=odom_test 206 | 207 | 208 | left_wheel_joint 209 | right_wheel_joint 210 | 1.25 211 | 0.6 212 | 20 213 | 1.0 214 | true 215 | true 216 | true 217 | odom_frame_test 218 | chassis 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_elevator.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | 0 0 0.075 0 0 0 10 | 11 | 12 | 800 13 | 14 | 15 | 16 | 17 | 18 | 2.25 2.25 0.15 19 | 20 | 21 | 22 | 23 | 24 | 25 | 2.25 2.25 0.15 26 | 27 | 28 | 29 | 30 | 31 | 1.0745 0.5725 1.125 0 0 0 32 | 33 | 34 | 0.1 1.15 2.25 35 | 36 | 37 | 38 | 39 | 1.0745 0.5725 1.125 0 0 0 40 | 41 | 42 | 0.1 1.15 2.25 43 | 44 | 45 | 46 | 49 | 50 | 51 | 52 | 53 | 1.0745 -1.0625 1.125 0 0 0 54 | 55 | 56 | 0.1 0.125 2.25 57 | 58 | 59 | 60 | 61 | 1.0745 -1.0625 1.125 0 0 0 62 | 63 | 64 | 0.1 0.125 2.25 65 | 66 | 67 | 68 | 71 | 72 | 73 | 74 | 75 | 76 | 1.0745 -0.5 1.125 0 0 0 77 | 78 | 79 | 80 | 0.08 1.0 2.25 81 | 82 | 83 | 84 | 85 | 86 | 87 | 0.08 1.0 2.25 88 | 89 | 90 | 91 | 92 | 93 | 94 | link 95 | door 96 | 97 | 0 1 0 98 | 99 | 0 100 | 1 101 | 10 102 | 103 | 104 | 105 | 2 106 | 107 | 108 | 109 | 110 | 111 | world 112 | link 113 | 114 | 0 0 1 115 | 116 | 0 117 | 10 118 | 100000 119 | 120 | 121 | 122 | 50 123 | 124 | 125 | 126 | 127 | 1 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | test 136 | elevator:=elevator_test 137 | 138 | 0 139 | 1 140 | 141 | elevator::lift 142 | elevator::door 143 | 3.075 144 | 145 | 146 | 10 147 | 148 | 149 | 150 | 151 | 152 | true 153 | 154 | 155 | 0 1.25 3 0 0 0 156 | 157 | 158 | 2.5 0.15 6 159 | 160 | 161 | 162 | 163 | 0 1.25 3 0 0 0 164 | 165 | 166 | 2.5 0.15 6 167 | 168 | 169 | 170 | 171 | 172 | 0 -1.25 3 0 0 0 173 | 174 | 175 | 2.5 0.15 6 176 | 177 | 178 | 179 | 180 | 0 -1.25 3 0 0 0 181 | 182 | 183 | 2.5 0.15 6 184 | 185 | 186 | 187 | 188 | 189 | -1.25 0 3 0 0 0 190 | 191 | 192 | 0.15 2.7 6 193 | 194 | 195 | 196 | 197 | -1.25 0 3 0 0 0 198 | 199 | 200 | 0.15 2.7 6 201 | 202 | 203 | 204 | 205 | 206 | 1.19 0 0.075 0 0 0 207 | 208 | 209 | 0.12 2.5 0.15 210 | 211 | 212 | 213 | 214 | 1.19 0 0.075 0 0 0 215 | 216 | 217 | 0.12 2.5 0.15 218 | 219 | 220 | 221 | 222 | 223 | 1.19 0 3.075 0 0 0 224 | 225 | 226 | 0.12 2.5 0.15 227 | 228 | 229 | 230 | 231 | 1.19 0 3.075 0 0 0 232 | 233 | 234 | 0.12 2.5 0.15 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 2.25 0 0.075 0 0 0 243 | true 244 | 245 | 246 | 247 | 248 | 2.0 5.0 0.15 249 | 250 | 251 | 252 | 253 | 254 | 255 | 2.0 5.0 0.15 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 2.25 0 3.075 0 0 0 264 | true 265 | 266 | 267 | 268 | 269 | 2.0 5.0 0.15 270 | 271 | 272 | 273 | 274 | 275 | 276 | 2.0 5.0 0.15 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_force.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 0 0 0.5 0 0 0 12 | 13 | 14 | 15 | 16 | 1 1 1 17 | 18 | 19 | 20 | 21 | 22 | 23 | 1 1 1 24 | 25 | 26 | 27 | 28 | 29 | 30 | /test 31 | gazebo_ros_force:=force_test 32 | 33 | box_link 34 | world 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_force_link.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 0 0 0.5 0 0 -1.5707963267948966 12 | 13 | 14 | 15 | 16 | 1 1 1 17 | 18 | 19 | 20 | 21 | 22 | 23 | 1 1 1 24 | 25 | 26 | 27 | 28 | 29 | 30 | /test 31 | gazebo_ros_force:=force_test 32 | 33 | box_link 34 | link 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_ft_sensor.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | 0 0 0 0 0 0 9 | 10 | 11 | 12 | 13 | 1 1 1 14 | 15 | 16 | 17 | 18 | 19 | 20 | 1 1 1 21 | 22 | 23 | 24 | 25 | 26 | 27 | /test 28 | wrench:=ft_sensor_test 29 | 30 | link 31 | test_world 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_gps_sensor.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 14 | 180 15 | 16 | 17 | 0 0 0.0 0 0 0 18 | 19 | 20 | 21 | 22 | 1 1 1 23 | 24 | 25 | 26 | 27 | 28 | 29 | 1 1 1 30 | 31 | 32 | 33 | 34 | true 35 | 30 36 | 37 | 38 | 39 | 40 | 0.0 41 | 2e-4 42 | 43 | 44 | 45 | 46 | 0.0 47 | 2e-4 48 | 49 | 50 | 51 | 52 | 53 | 54 | /gps 55 | ~/out:=data 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_hand_of_god.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 0 0 0.5 0 0 0 9 | 10 | 11 | 12 | 13 | 1 1 1 14 | 15 | 16 | 17 | 18 | 19 | 20 | 1 1 1 21 | 22 | 23 | 24 | 25 | 26 | 27 | /test 28 | 29 | link 30 | link 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_harness.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | 0 0 3.0 0 0 0 10 | 11 | 12 | 13 | 14 | 1 1 1 15 | 16 | 17 | 18 | 19 | 20 | 21 | 1 1 1 22 | 23 | 24 | 25 | 26 | 27 | 28 | test 29 | box/harness/velocity:=velocity_test 30 | box/harness/detach:=detach_test 31 | 32 | 33 | 0 0 0 0 0 0 34 | world 35 | link 36 | 37 | 0 0 1 38 | 39 | 10 40 | 41 | 42 | -1.5 43 | 1.5 44 | 10000 45 | -1 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | 1 53 | 54 | 0.0 55 | 0.0 56 | 57 | 58 | 59 | 60 | 61 | 62 | joint1 63 | 64 |

1000000

65 | 0 66 | 0 67 | 0 68 | 0 69 | -10000 70 | 10000 71 | 72 | 73 |

10000

74 | 0 75 | 0 76 | 0 77 | 0 78 | 0 79 | 10000 80 | 81 | 82 | 83 | joint1 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_imu_sensor.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 0 0 0.0 0 0 0 12 | 13 | 14 | 15 | 16 | 1 1 1 17 | 18 | 19 | 20 | 21 | 22 | 23 | 1 1 1 24 | 25 | 26 | 27 | 28 | true 29 | 30 30 | 31 | 32 | 33 | 34 | 0.0 35 | 2e-4 36 | 37 | 38 | 39 | 40 | 0.0 41 | 2e-4 42 | 43 | 44 | 45 | 46 | 0.0 47 | 2e-4 48 | 49 | 50 | 51 | 52 | 53 | 54 | 0.0 55 | 1.7e-2 56 | 57 | 58 | 59 | 60 | 0.0 61 | 1.7e-2 62 | 63 | 64 | 65 | 66 | 0.0 67 | 1.7e-2 68 | 69 | 70 | 71 | 72 | 73 | 74 | /imu 75 | ~/out:=data 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_joint_pose_trajectory.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 5 | 6 | model://ground_plane 7 | 8 | 9 | model://sun 10 | 11 | 12 | 13 | model://double_pendulum_with_base 14 | 15 | 17 | 18 | /test 19 | set_joint_trajectory:=set_trajectory_test 20 | 21 | 2 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_joint_state_publisher.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 12 | 0.5 0.5 0.5 0 0 0 13 | 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 18 | 19 | 0.5 1 1 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0.5 1 1 27 | 28 | 29 | 30 | 31 | 32 | 33 | 0.25 0 -0.2 0 -0 0 34 | 35 | 36 | 37 | 0.05 38 | 0.5 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 0.05 47 | 0.5 48 | 49 | 50 | 51 | 52 | 53 | 54 | base 55 | needle 56 | 57 | 0 0 0 0 0 0 58 | 59 | 1 0 0 60 | 61 | 62 | 63 | 64 | world 65 | base 66 | 67 | 68 | 70 | 71 | /test 72 | joint_states:=joint_states_test 73 | 74 | 100 75 | hinge 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_multi_camera.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 0.5 9 | 10 | 1.3962634 11 | 12 | 640 13 | 480 14 | R8G8B8 15 | 16 | 17 | 0.02 18 | 300 19 | 20 | 21 | gaussian 22 | 25 | 0.0 26 | 0.007 27 | 28 | 29 | 30 | 1.3962634 31 | 32 | 640 33 | 480 34 | R8G8B8 35 | 36 | 37 | 0.02 38 | 300 39 | 40 | 41 | gaussian 42 | 45 | 0.0 46 | 0.007 47 | 48 | 49 | 50 | 51 | test_cam 52 | camera1/left/image_raw:=camera/left/image_test 53 | camera1/right/image_raw:=camera/right/image_test 54 | camera1/left/camera_info:=camera/left/camera_info_test 55 | camera1/right/camera_info:=camera/right/camera_info_test 56 | 57 | 58 | 59 | 0.07 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_multicamera.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 0.5 9 | 10 | 1.3962634 11 | 12 | 640 13 | 480 14 | R8G8B8 15 | 16 | 17 | 0.02 18 | 300 19 | 20 | 21 | gaussian 22 | 25 | 0.0 26 | 0.007 27 | 28 | 29 | 30 | 1.3962634 31 | 32 | 640 33 | 480 34 | R8G8B8 35 | 36 | 37 | 0.02 38 | 300 39 | 40 | 41 | gaussian 42 | 45 | 0.0 46 | 0.007 47 | 48 | 49 | 50 | 51 | test_cam 52 | camera1/left/image_raw:=camera/left/image_test 53 | camera1/right/image_raw:=camera/right/image_test 54 | 55 | 56 | 57 | 0.07 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_p3d.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 0 0 0.5 0 0 0 12 | 13 | 14 | 15 | 16 | 1 1 1 17 | 18 | 19 | 20 | 21 | 22 | 23 | 1 1 1 24 | 25 | 26 | 27 | 28 | 29 | 3 0 0 0 0 0 30 | 31 | 32 | 33 | 0.5 34 | 35 | 36 | 37 | 38 | 39 | 40 | 0.5 41 | 42 | 43 | 44 | 45 | 46 | 47 | /test 48 | odom:=p3d_test 49 | 50 | box_link 51 | sphere_link 52 | 0 53 | 10 10 10 54 | 0 0 0 55 | 0 56 | 57 | 58 | 59 | /test_deprecated 60 | odom:=p3d_test_deprecated 61 | 62 | box_link 63 | sphere_link 64 | 0 65 | 5 5 5 66 | 0 0 0 67 | 0 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_planar_move.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | 0 0 0.5 0 0 0 11 | 12 | 13 | 14 | 15 | 1 1 1 16 | 17 | 18 | 19 | 20 | 21 | 22 | 1 1 1 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | /test 31 | cmd_vel:=cmd_test 32 | odom:=odom_test 33 | 34 | 35 | 100 36 | 100 37 | 38 | true 39 | true 40 | odom_frame_test 41 | link 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_projector.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | 0 0 1 0.0 0.0 0 11 | 12 | 13 | 14 | 15 | 0.1 16 | 17 | 18 | 19 | 20 | 21 | 22 | 0.1 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 0 0 0 0 0 0 31 | bricks.png 32 | 0.959931088597 33 | 0.1 34 | 10 35 | 36 | 37 | true 38 | 39 | 40 | 41 | /test 42 | switch:=switch_test 43 | 44 | projector_link 45 | texture_projector 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_ray_sensor.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 5 | 6 | 7 | model://sun 8 | 9 | 10 | 0.0 0.0 0.0 0.0 0.0 0.0 11 | 12 | 13 | 14 | 15 | 0.05 0.05 0.05 16 | 17 | 18 | 19 | 20 | 21 | 22 | 0.05 0.05 0.05 23 | 24 | 25 | 100.0 26 | 27 | 28 | 0.0 0.0 0.0 0.0 0.0 0.0 29 | 30 | 31 | 32 | 300 33 | -0.5236 34 | 0.5236 35 | 36 | 37 | 100 38 | -0.5236 39 | 0.5236 40 | 41 | 42 | 43 | 0.05 44 | 50.0 45 | 46 | 47 | true 48 | 3.0 49 | 50 | 51 | 52 | /ray 53 | ~/out:=pointcloud2 54 | 55 | sensor_msgs/PointCloud2 56 | 57 | 58 | 59 | /ray 60 | ~/out:=pointcloud 61 | 62 | sensor_msgs/PointCloud 63 | 64 | 65 | 66 | /ray 67 | ~/out:=laserscan 68 | 69 | sensor_msgs/LaserScan 70 | 71 | 72 | 73 | /ray 74 | ~/out:=range 75 | 76 | sensor_msgs/Range 77 | 78 | 79 | 110 | 111 | 112 | 113 | 1.0 0.0 0.0 0.0 0.0 114 | 115 | 116 | 117 | 118 | 0.05 119 | 120 | 121 | 122 | 123 | 124 | 125 | 0.05 126 | 127 | 128 | 80.0 129 | 130 | 131 | 132 | 133 | 5.0 0.9 0.0 0.0 0.0 134 | 135 | 136 | 137 | 138 | 0.05 139 | 140 | 141 | 142 | 143 | 144 | 145 | 0.05 146 | 147 | 148 | 80.0 149 | 150 | 151 | 152 | 153 | 5.0 -1.2 0.0 0.0 0.0 154 | 155 | 156 | 157 | 158 | 0.05 159 | 160 | 161 | 162 | 163 | 164 | 165 | 0.05 166 | 167 | 168 | 80.0 169 | 170 | 171 | 172 | 173 | 3.0 -1.5 1.0 0.0 0.0 174 | 175 | 176 | 177 | 178 | 0.05 179 | 180 | 181 | 182 | 183 | 184 | 185 | 0.05 186 | 187 | 188 | 80.0 189 | 190 | 191 | 192 | 193 | 3.0 1.3 -0.8 0.0 0.0 194 | 195 | 196 | 197 | 198 | 0.05 199 | 200 | 201 | 202 | 203 | 204 | 205 | 0.05 206 | 207 | 208 | 80.0 209 | 210 | 211 | 212 | 213 | 214 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_skid_steer_drive.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | 0 0 0.325 0 -0 0 15 | 16 | 17 | -0.151427 -0 0.175 0 -0 0 18 | 19 | 1.14395 20 | 21 | 0.126164 22 | 0 23 | 0 24 | 0.416519 25 | 0 26 | 0.481014 27 | 28 | 29 | 30 | 31 | 32 | 2.01142 1 0.568726 33 | 34 | 35 | 36 | 37 | 38 | 39 | 2.01142 1 0.568726 40 | 41 | 42 | 43 | 44 | 45 | 46 | 0.554283 0.625029 -0.025 -1.5707 0 0 47 | 48 | 2 49 | 50 | 0.145833 51 | 0 52 | 0 53 | 0.145833 54 | 0 55 | 0.125 56 | 57 | 58 | 59 | 60 | 61 | 0.3 62 | 63 | 64 | 65 | 66 | 67 | 68 | 0.3 69 | 70 | 71 | 72 | 73 | 74 | 1 75 | 1 76 | 0 77 | 0 78 | 79 | 80 | 81 | 82 | 0 83 | 0.2 84 | 1e+13 85 | 1 86 | 0.01 87 | 0.01 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 0.554282 -0.625029 -0.025 -1.5707 0 0 96 | 97 | 2 98 | 99 | 0.145833 100 | 0 101 | 0 102 | 0.145833 103 | 0 104 | 0.125 105 | 106 | 107 | 108 | 109 | 110 | 0.3 111 | 112 | 113 | 114 | 115 | 116 | 117 | 0.3 118 | 119 | 120 | 121 | 122 | 123 | 1 124 | 1 125 | 0 126 | 0 127 | 128 | 129 | 130 | 131 | 0 132 | 0.2 133 | 1e+13 134 | 1 135 | 0.01 136 | 0.01 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | -0.554283 0.625029 -0.025 -1.5707 0 0 145 | 146 | 2 147 | 148 | 0.145833 149 | 0 150 | 0 151 | 0.145833 152 | 0 153 | 0.125 154 | 155 | 156 | 157 | 158 | 159 | 0.5 160 | 161 | 162 | 163 | 164 | 165 | 166 | 0.5 167 | 168 | 169 | 170 | 171 | 172 | 1 173 | 1 174 | 0 175 | 0 176 | 177 | 178 | 179 | 180 | 0 181 | 0.2 182 | 1e+13 183 | 1 184 | 0.01 185 | 0.01 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | -0.554282 -0.625029 -0.025 -1.5707 0 0 194 | 195 | 2 196 | 197 | 0.145833 198 | 0 199 | 0 200 | 0.145833 201 | 0 202 | 0.125 203 | 204 | 205 | 206 | 207 | 208 | 0.5 209 | 210 | 211 | 212 | 213 | 214 | 215 | 0.5 216 | 217 | 218 | 219 | 220 | 221 | 1 222 | 1 223 | 0 224 | 0 225 | 226 | 227 | 228 | 229 | 0 230 | 0.2 231 | 1e+13 232 | 1 233 | 0.01 234 | 0.01 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | -0.957138 -0 -0.125 0 -0 0 243 | 244 | 1 245 | 246 | 0.1 247 | 0 248 | 0 249 | 0.1 250 | 0 251 | 0.1 252 | 253 | 254 | 255 | 256 | 257 | 0.2 258 | 259 | 260 | 261 | 262 | 263 | 264 | 0.2 265 | 266 | 267 | 268 | 269 | 270 | 271 | chassis 272 | left_wheel0 273 | 274 | 0 0 1 275 | 276 | -1.79769e+308 277 | 1.79769e+308 278 | 279 | 280 | 281 | 282 | 283 | chassis 284 | right_wheel0 285 | 286 | 0 0 1 287 | 288 | -1.79769e+308 289 | 1.79769e+308 290 | 291 | 292 | 293 | 294 | 295 | chassis 296 | left_wheel1 297 | 298 | 0 0 1 299 | 300 | -1.79769e+308 301 | 1.79769e+308 302 | 303 | 304 | 305 | 306 | 307 | chassis 308 | right_wheel1 309 | 310 | 0 0 1 311 | 312 | -1.79769e+308 313 | 1.79769e+308 314 | 315 | 316 | 317 | 318 | 319 | chassis 320 | caster 321 | 322 | 323 | 324 | 325 | /test 326 | cmd_vel:=cmd_test 327 | odom:=odom_test 328 | 329 | 330 | 2 331 | left_wheel_joint0 332 | right_wheel_joint0 333 | left_wheel_joint1 334 | right_wheel_joint1 335 | 1.25 336 | 1.25 337 | 0.6 338 | 1.0 339 | 20 340 | 1.0 341 | true 342 | true 343 | true 344 | odom_frame_test 345 | chassis 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_tricycle_drive.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | 15 | -0.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 16 | 17 | 0.0 0 0 0 0 0 18 | 10 19 | 20 | 0.22799999999999998 21 | 0.7435210984814149 22 | 0.9655210984814149 23 | 0 24 | 0 25 | 0 26 | 27 | 28 | 29 | -0.4713346258704366 0 0 1.5707963267948966 0 0 30 | 31 | 32 | 1.0392304845413263 33 | 0.03 34 | 35 | 36 | 37 | 38 | -0.4713346258704366 0 0 1.5707963267948966 0 0 39 | 40 | 41 | 1.0392304845413263 42 | 0.03 43 | 44 | 45 | 46 | 50 | 51 | 52 | 53 | 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 54 | 55 | 56 | 1.0031676644991372 57 | 0.03 58 | 59 | 60 | 61 | 62 | 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 63 | 64 | 65 | 1.0031676644991372 66 | 0.03 67 | 68 | 69 | 70 | 74 | 75 | 76 | 77 | 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 78 | 79 | 80 | 1.0031676644991372 81 | 0.03 82 | 83 | 84 | 85 | 86 | 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 87 | 88 | 89 | 1.0031676644991372 90 | 0.03 91 | 92 | 93 | 94 | 98 | 99 | 100 | 101 | 102 | 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 103 | 104 | 3 105 | 106 | 0.15820312499999997 107 | 0.058359374999999984 108 | 0.10265624999999999 109 | 0 110 | 0 111 | 0 112 | 113 | 114 | 115 | 0 0 0.397747564417433 1.5707963267948966 0 0 116 | 117 | 118 | 0.6363961030678927 119 | 0.0375 120 | 121 | 122 | 123 | 124 | 0 0 0.397747564417433 1.5707963267948966 0 0 125 | 126 | 127 | 0.6363961030678927 128 | 0.0375 129 | 130 | 131 | 132 | 136 | 137 | 138 | 139 | 0 0 0.2386485386504598 0 0 0 140 | 141 | 142 | 0.31819805153394637 143 | 0.0375 144 | 145 | 146 | 147 | 148 | 0 0 0.2386485386504598 0 0 0 149 | 150 | 151 | 0.31819805153394637 152 | 0.0375 153 | 154 | 155 | 156 | 160 | 161 | 162 | 163 | 0 0 -0.23864853865045976 1.5707963267948966 0 0 164 | 165 | 166 | 0.6363961030678927 167 | 0.015 168 | 169 | 170 | 171 | 172 | 0 0 -0.23864853865045976 1.5707963267948966 0 0 173 | 174 | 175 | 0.6363961030678927 176 | 0.015 177 | 178 | 179 | 180 | 184 | 185 | 186 | 187 | 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 188 | 189 | 190 | 0.45 191 | 0.0375 192 | 193 | 194 | 195 | 196 | 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 197 | 198 | 199 | 0.45 200 | 0.0375 201 | 202 | 203 | 204 | 208 | 209 | 210 | 211 | 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 212 | 213 | 214 | 0.45 215 | 0.0375 216 | 217 | 218 | 219 | 220 | 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 221 | 222 | 223 | 0.45 224 | 0.0375 225 | 226 | 227 | 228 | 232 | 233 | 234 | 235 | 236 | 0.08288176767905665 0 0.15 0 0 0 237 | 238 | 0.5 239 | 240 | 0.0045 241 | 0.0045 242 | 0.0045 243 | 0 244 | 0 245 | 0 246 | 247 | 248 | 249 | 250 | 251 | 0.15 252 | 253 | 254 | 255 | 256 | 257 | 100000000.0 258 | 1 259 | 0.0005 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 0.15 268 | 269 | 270 | 271 | 275 | 276 | 277 | 278 | 279 | frame 280 | fork 281 | 282 | 0 0 1 283 | 284 | -0.9599310885968813 285 | 0.9599310885968813 286 | 287 | 288 | 289 | 290 | fork 291 | wheel_front 292 | 293 | 0 1 0 294 | 295 | 296 | 297 | -0.8171182323209433 0.5196152422706631 0.15 0 0 0 298 | 299 | 0.5 300 | 301 | 0.0045 302 | 0.0045 303 | 0.0045 304 | 0 305 | 0 306 | 0 307 | 308 | 309 | 310 | 311 | 312 | 0.15 313 | 314 | 315 | 316 | 317 | 318 | 100000000.0 319 | 1 320 | 0.0005 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 0.15 329 | 330 | 331 | 332 | 336 | 337 | 338 | 339 | 340 | frame 341 | wheel_rear_left 342 | 343 | 0 1 0 344 | 345 | 346 | 347 | -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 348 | 349 | 0.5 350 | 351 | 0.0045 352 | 0.0045 353 | 0.0045 354 | 0 355 | 0 356 | 0 357 | 358 | 359 | 360 | 361 | 362 | 0.15 363 | 364 | 365 | 366 | 367 | 368 | 100000000.0 369 | 1 370 | 0.0005 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 0.15 379 | 380 | 381 | 382 | 386 | 387 | 388 | 389 | 390 | frame 391 | wheel_rear_right 392 | 393 | 0 1 0 394 | 395 | 396 | 397 | 398 | 399 | 400 | /test 401 | cmd_vel:=cmd_test 402 | odom:=odom_test 403 | 404 | 405 | 406 | wheel_front_steer 407 | wheel_front_spin 408 | wheel_rear_left_spin 409 | wheel_rear_right_spin 410 | 411 | 412 | 1.0 413 | 0.3 414 | 0.3 415 | 416 | 417 | 20 418 | 5.0 419 | 1.0 420 | 421 | 422 | true 423 | true 424 | true 425 | 426 | odom_frame_test 427 | fork 428 | 429 | 430 | 431 | 432 | 433 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_vacuum_gripper.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | 0 0 0.04 0 0 0 11 | 12 | 13 | 14 | 15 | 0.08 0.08 0.08 16 | 17 | 18 | 19 | 20 | 21 | 22 | 0.08 0.08 0.08 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | /test 32 | switch:=switch_test 33 | grasping:=grasping_test 34 | 35 | 36 | link 37 | 38 | 5.0 39 | 40 | ground_plane 41 | 42 | 43 | 44 | 45 | 46 | 0.6 0 0.04 0 0 0 47 | 48 | 49 | 0.026 50 | 51 | 1.664e-5 52 | 1.664e-5 53 | 1.664e-5 54 | 0 55 | 0 56 | 0 57 | 58 | 59 | 60 | 61 | 62 | 0.04 63 | 64 | 65 | 66 | 67 | 68 | 69 | 0.04 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -0.6 0 0.04 0 0 0 79 | 80 | 81 | 0.026 82 | 83 | 1.664e-5 84 | 1.664e-5 85 | 1.664e-5 86 | 0 87 | 0 88 | 0 89 | 90 | 91 | 92 | 93 | 94 | 0.04 95 | 96 | 97 | 98 | 99 | 100 | 101 | 0.04 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_video.world: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | model://sun 12 | 13 | 14 | 15 | model://ground_plane 16 | 17 | 18 | 19 | 0.0 0.0 0.0 0.0 0.0 0.0 20 | 21 | 22 | 23 | 24 | 0.5 0.5 0.5 25 | 26 | 27 | 28 | 29 | 30 | /test 31 | ~/image_raw:=video_test 32 | 33 | 120 34 | 160 35 | 36 | 37 | 38 | 39 | 40 | 0.5 0.5 0.5 41 | 42 | 43 | 100.0 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_wheel_slip.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -2 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip 16 | 0 2 0 0 0 0 17 | 18 | 19 | trisphere_cycle_slip 20 | 21 | 22 | 1 23 | 1 24 | 32 25 | 26 | 27 | 1 28 | 1 29 | 32 30 | 31 | 32 | 1 33 | 1 34 | 32 35 | 36 | 37 | 38 | 39 | wheel_front_steer 40 | 0 41 | 9 0 0.1 42 | 43 | 44 | wheel_front_spin 45 | wheel_rear_left_spin 46 | wheel_rear_right_spin 47 | 6.0 48 | 9 0 0 49 | 50 | 51 | wheel_front_spin 52 | wheel_rear_left_spin 53 | wheel_rear_right_spin 54 | 2.15 55 | 56 | 57 | 58 | 59 | 60 | 61 | 1.5 -4 2.5 0 0.5 1.6 62 | orbit 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /test/worlds/gazebo_ros_wheel_slip_at_rest.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip 16 | 0 2 0 0 0 0 17 | 18 | 19 | trisphere_cycle_slip 20 | 21 | 22 | 1 23 | 1 24 | 32 25 | 26 | 27 | 1 28 | 1 29 | 32 30 | 31 | 32 | 1 33 | 1 34 | 32 35 | 36 | 37 | 38 | 39 | 40 | 41 | 1.5 -4 2.5 0 0.5 1.6 42 | orbit 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /test/worlds/wheelslip_worlds/gazebo_ros_wheelslip_1.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -2 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip0 16 | 0 0 0 0 0 0 17 | 18 | 19 | trisphere_cycle_slip0 20 | 21 | 22 | -0.3 23 | 0.4 24 | 32 25 | 26 | 27 | 0.5 28 | 0.6 29 | 32 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/worlds/wheelslip_worlds/gazebo_ros_wheelslip_2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -2 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip0 16 | 0 0 0 0 0 0 17 | 18 | 19 | 20 | trisphere_cycle_slip0 21 | 0.3 22 | 0.4 23 | 24 | 25 | 26 | 0.1 27 | 0.2 28 | 77 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /test/worlds/wheelslip_worlds/gazebo_ros_wheelslip_3.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -2 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip0 16 | 0 0 0 0 0 0 17 | 18 | 19 | 20 | trisphere_cycle_slip0 21 | 0.3 22 | 0.4 23 | 24 | 25 | 26 | 77 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /test/worlds/wheelslip_worlds/gazebo_ros_wheelslip_4.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -2 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip0 16 | 0 0 0 0 0 0 17 | 18 | 19 | 20 | trisphere_cycle_slip0 21 | 0.3 22 | 0.4 23 | 24 | 25 | 26 | 0.1 27 | 0.2 28 | 77 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /test/worlds/wheelslip_worlds/gazebo_ros_wheelslip_5.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -2 0 -9.8 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | model://trisphere_cycle 15 | trisphere_cycle_slip0 16 | 0 0 0 0 0 0 17 | 18 | 19 | 20 | trisphere_cycle_slip0 21 | 0.1 22 | 0.2 23 | 0.6 24 | 0.7 25 | 26 | 27 | 28 | -0.3 29 | 0.4 30 | 32 31 | 32 | 33 | 0.5 34 | 0.6 35 | 32 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /worlds/gazebo_ros_gridmap_apollo.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 0 17 | 0 18 | 0 19 | 20 | 21 | 0 0 -9.8 22 | 6e-06 2.3e-05 -4.2e-05 23 | 24 | 25 | 0.001 26 | 1 27 | 1000 28 | 29 | 30 | 0.4 0.4 0.4 1 31 | 0.7 0.7 0.7 1 32 | 1 33 | 34 | 35 | 36 | EARTH_WGS84 37 | 0 38 | 0 39 | 0 40 | 0 41 | 42 | 43 | 1 44 | 45 | 46 | 47 | 48 | 49 | 0.2 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 1 58 | 100000 59 | 1 60 | 1e-06 61 | 0.02 62 | 63 | 64 | 65 | 66 | 67 | 68 | model://apollo15_landing_site_1000x1000/materials/textures/NAC_DTM_APOLLO15_E261N0036_257x257+7472+2152.png 69 | 514 514 49 70 | 0 0 -45 71 | 72 | 10 73 | __default__ 74 | __default__ 75 | 76 | 77 | 0 78 | 0 79 | 80 | 81 | 82 | 10 83 | 84 | 85 | 86 | 87 | 88 | model://apollo15_landing_site_1000x1000/materials/textures/AS16-110-18026HR-512x512.jpg 89 | file://media/materials/textures/flat_normal.png 90 | 2 91 | 92 | model://apollo15_landing_site_1000x1000/materials/textures/NAC_DTM_APOLLO15_E261N0036_257x257+7472+2152.png 93 | 514 514 49 94 | 0 0 -45 95 | 96 | 0 97 | 0 98 | 99 | 100 | 101 | 102 | 0 103 | 0 104 | 0 105 | 106 | 7.91106 -1.29896 0 0 -0 0 107 | 108 | 109 | 1 110 | 111 | 112 | 0 -0.06945 0.47293 0 -0 0 113 | 114 | 115 | 0.87974 0.8821 0.02602 116 | 117 | 118 | 10 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 0 -0.05 0.31475 0 -0 0 134 | 135 | 136 | 0.49522 0.67401 0.28915 137 | 138 | 139 | 10 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 0.30107 0.3835 0.38976 0 -0 0 155 | 156 | 157 | 0.04442 0.38897 0.07918 158 | 159 | 160 | 10 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | -0.30107 0.3835 0.38976 0 -0 0 176 | 177 | 178 | 0.04442 0.38897 0.07918 179 | 180 | 181 | 10 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 0 0.59414 0.38976 0 -0 0 197 | 198 | 199 | 0.56656 0.04442 0.07918 200 | 201 | 202 | 10 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 0 0.47 0.32114 0.69137 -0 0 218 | 219 | 220 | 0.43774 0.212366 0.05173 221 | 222 | 223 | 10 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 0.22615 0.32897 0.33193 0 -0 0 239 | 240 | 241 | 0.01116 0.19338 0.16304 242 | 243 | 244 | 10 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 0.22615 0.471 0.39308 0 -0 0 260 | 261 | 262 | 0.01116 0.15 0.04 263 | 264 | 265 | 10 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 0.22615 0.45 0.35308 0 -0 0 281 | 282 | 283 | 0.01116 0.15 0.04 284 | 285 | 286 | 10 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 0.22615 0.39308 0.31308 0 -0 0 302 | 303 | 304 | 0.01116 0.15 0.04 305 | 306 | 307 | 10 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | -0.22615 0.32897 0.33193 0 -0 0 323 | 324 | 325 | 0.01116 0.19338 0.16304 326 | 327 | 328 | 10 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | -0.22615 0.471 0.39308 0 -0 0 344 | 345 | 346 | 0.01116 0.15 0.04 347 | 348 | 349 | 10 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | -0.22615 0.45 0.35308 0 -0 0 365 | 366 | 367 | 0.01116 0.15 0.04 368 | 369 | 370 | 10 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | -0.22615 0.39308 0.31308 0 -0 0 386 | 387 | 388 | 0.01116 0.15 0.04 389 | 390 | 391 | 10 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 0 0.30047 0.32114 2.26207 -0 0 407 | 408 | 409 | 0.43774 0.212366 0.05173 410 | 411 | 412 | 10 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 0.31103 -0.32115 0.1358 1.5707 0 1.5707 428 | 429 | 430 | 0.1358 431 | 0.1641 432 | 433 | 434 | 10 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | -0.31103 -0.32115 0.1358 1.5707 0 1.5707 450 | 451 | 452 | 0.1358 453 | 0.1641 454 | 455 | 456 | 10 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 0.31103 0.28049 0.1358 1.5707 0 1.5707 472 | 473 | 474 | 0.1358 475 | 0.1641 476 | 477 | 478 | 10 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | -0.31103 0.28049 0.1358 1.5707 0 1.5707 494 | 495 | 496 | 0.1358 497 | 0.1641 498 | 499 | 500 | 10 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | model://mars_rover/meshes/rover.dae 518 | 519 | 520 | 521 | 0 522 | 0 523 | 0 524 | 525 | -5.61524 -1.10816 0 0 -0 0 526 | 527 | 528 | 321 772000000 529 | 158 239802880 530 | 1645002817 346167988 531 | 155166 532 | 533 | 0 0 0 0 -0 0 534 | 1 1 1 535 | 536 | 0 0 0 0 -0 0 537 | 0 0 0 0 -0 0 538 | 0 0 0 0 -0 0 539 | 0 0 0 0 -0 0 540 | 541 | 542 | 543 | -5.61524 -1.10816 0.227165 0 -0 0 544 | 1 1 1 545 | 546 | -5.61524 -1.10816 0.227165 0 -0 0 547 | 0 0 0 0 -0 0 548 | 0 0 0 0 -0 0 549 | 0 0 0 0 -0 0 550 | 551 | 552 | 553 | 0 0 10 0 -0 0 554 | 555 | 556 | 557 | 558 | 41.8663 -11.6825 30.3578 0 0.679454 2.60419 559 | orbit 560 | perspective 561 | 562 | 563 | 564 | 565 | demo 566 | 567 | 0.0 568 | 0.0 569 | -30.0 570 | -30.0 571 | -10.0 572 | 30.0 573 | 30.0 574 | 10.0 575 | 0.12 576 | 2.0 577 | 0.2 578 | 579 | 580 | 581 | --------------------------------------------------------------------------------