├── .gitignore ├── LICENSE ├── README.md ├── assets ├── exploration.png ├── load-map-1.png ├── load-map.png ├── localization-1.png ├── localization-2.png ├── localization-3.png ├── localization-4.png ├── localization.png ├── mapping.png ├── navigation-1.png ├── navigation-2.png ├── navigation-slam.png ├── navigation.png ├── rqt-graph-1.png ├── rqt-graph.png ├── save-map.png ├── starter-package.png ├── tf-tree-1.png ├── tf-tree.png ├── waypoint-1.png ├── waypoint-2.png ├── waypoint-3.png ├── waypoint-4.png ├── waypoint.png ├── youtube-exploration.png ├── youtube-navigation-slam.png ├── youtube-navigation.png ├── youtube-waypoint-node.png └── youtube-waypoints.png ├── bme_ros2_navigation ├── CMakeLists.txt ├── config │ ├── amcl_localization.yaml │ ├── ekf.yaml │ ├── gz_bridge.yaml │ ├── navigation.yaml │ ├── slam_toolbox_localization.yaml │ ├── slam_toolbox_mapping.yaml │ └── waypoints.yaml ├── launch │ ├── check_urdf.launch.py │ ├── localization.launch.py │ ├── localization_slam_toolbox.launch.py │ ├── mapping.launch.py │ ├── navigation.launch.py │ ├── navigation_with_slam.launch.py │ ├── spawn_robot.launch.py │ └── world.launch.py ├── maps │ ├── my_map.pgm │ ├── my_map.yaml │ ├── serialized.data │ └── serialized.posegraph ├── meshes │ ├── lidar.dae │ ├── mogi_bot.dae │ └── wheel.dae ├── package.xml ├── rviz │ ├── localization.rviz │ ├── mapping.rviz │ ├── navigation.rviz │ ├── rviz.rviz │ └── urdf.rviz ├── urdf │ ├── materials.xacro │ ├── mogi_bot.gazebo │ └── mogi_bot.urdf └── worlds │ ├── empty.sdf │ └── home.sdf └── bme_ros2_navigation_py ├── bme_ros2_navigation_py ├── __init__.py ├── follow_waypoints.py ├── send_initialpose.py └── slam_toolbox_load_map.py ├── package.xml ├── resource └── bme_ros2_navigation_py ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | **/.DS_Store 2 | **/Thumbs.db 3 | -------------------------------------------------------------------------------- /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 2024-2025, David Dudas 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 | [//]: # (Image References) 2 | 3 | [image1]: ./assets/starter-package.png "Starter package" 4 | [image2]: ./assets/rqt-graph.png "rqt_graph" 5 | [image3]: ./assets/tf-tree.png "rqt_tf_tree" 6 | [image4]: ./assets/tf-tree-1.png "rqt_tf_tree" 7 | [image5]: ./assets/mapping.png "mapping" 8 | [image6]: ./assets/save-map.png "Save map" 9 | [image7]: ./assets/load-map.png "Load map" 10 | [image8]: ./assets/load-map-1.png "Load map" 11 | [image9]: ./assets/localization.png "Localization" 12 | [image10]: ./assets/localization-1.png "Localization" 13 | [image11]: ./assets/localization-2.png "Localization" 14 | [image12]: ./assets/localization-3.png "Localization" 15 | [image13]: ./assets/localization-4.png "Localization" 16 | [image14]: ./assets/navigation.png "Navigation" 17 | [image15]: ./assets/navigation-1.png "Navigation" 18 | [image16]: ./assets/navigation-2.png "Navigation" 19 | [image17]: ./assets/waypoint.png "Waypoint navigation" 20 | [image18]: ./assets/waypoint-1.png "Waypoint navigation" 21 | [image19]: ./assets/waypoint-2.png "Waypoint navigation" 22 | [image20]: ./assets/waypoint-3.png "Waypoint navigation" 23 | [image21]: ./assets/waypoint-4.png "Waypoint navigation" 24 | [image22]: ./assets/navigation-slam.png "Navigation with SLAM" 25 | [image23]: ./assets/exploration.png "Exploration" 26 | [image24]: ./assets/rqt-graph-1.png "rqt_graph" 27 | 28 | # Week 7-8: Navigation 29 | 30 | ## This is how far we will get by the end of this lesson: 31 | 32 | 33 | 34 | # Table of Contents 35 | 1. [Introduction](#introduction) 36 | 1.1. [Download ROS package](#download-ros-package) 37 | 1.2. [Test the starter package](#test-the-starter-package) 38 | 2. [Mapping](#mapping) 39 | 2.1. [SLAM toolbox](#slam-toolbox) 40 | 3. [Localization](#localization) 41 | 3.1. [Localization with AMCL](#localization-with-amcl) 42 | 3.2. [Localization with SLAM toolbox](#localization-with-slam-toolbox) 43 | 4. [Navigation](#navigation) 44 | 4.1. [Waypoint navigation](#waypoint-navigation) 45 | 4.2. [Navigation with SLAM](#navigation-with-slam) 46 | 5. [Exploration](#exploration) 47 | 48 | # Introduction 49 | 50 | In this lesson we'll lear how to map the robot's environment, how to do localization on an existing map and we'll learn to use ROS2's navigation stack. 51 | 52 | ## Download ROS package 53 | 54 | To download the starter package, clone the following git repo with the `starter-branch` (using the `-b branch` flag) into your colcon workspace: 55 | ```bash 56 | git clone -b starter-branch https://github.com/MOGI-ROS/Week-7-8-ROS2-Navigation 57 | ``` 58 | 59 | Let's take a look what's inside the `bme_ros2_navigation` package with the `tree` command! 60 | 61 | 62 | ```bash 63 | . 64 | ├── CMakeLists.txt 65 | ├── package.xml 66 | ├── config 67 | │   ├── amcl_localization.yaml 68 | │   ├── ekf.yaml 69 | │   ├── gz_bridge.yaml 70 | │   ├── navigation.yaml 71 | │   ├── slam_toolbox_localization.yaml 72 | │   ├── slam_toolbox_mapping.yaml 73 | │   └── waypoints.yaml 74 | ├── launch 75 | │   ├── check_urdf.launch.py 76 | │   ├── spawn_robot.launch.py 77 | │   └── world.launch.py 78 | ├── maps 79 | │   ├── my_map.pgm 80 | │   ├── my_map.yaml 81 | │   ├── serialized.data 82 | │   └── serialized.posegraph 83 | ├── meshes 84 | │   ├── lidar.dae 85 | │   ├── mogi_bot.dae 86 | │   └── wheel.dae 87 | ├── rviz 88 | │   ├── localization.rviz 89 | │   ├── mapping.rviz 90 | │   ├── navigation.rviz 91 | │   ├── rviz.rviz 92 | │   └── urdf.rviz 93 | ├── urdf 94 | │   ├── materials.xacro 95 | │   ├── mogi_bot.gazebo 96 | │   └── mogi_bot.urdf 97 | └── worlds 98 | ├── empty.sdf 99 | └── home.sdf 100 | ``` 101 | 102 | Let's see what will we do with the existing files and folders: 103 | - `config`: As we saw previously, we usually store parameters and large configuration files for ROS packages which aren't comfortable to handle from the launchfiles directly. In this lesson we will use more configuration files from this folder. 104 | - `launch`: Default launch files are already part of the starting package, we can test the package with `spawn_robot.launch.py`. 105 | - `maps`: Offline map files for the Gazebo world 106 | - `meshes`: this folder contains the 3D models in `dae` format (collada mesh) that we use for our robot's body, wheels and lidar sensor. 107 | - `rviz`: Pre-configured RViz2 layouts 108 | - `urdf`: The URDF models of our robot, we'll extend the `mogi_bot.urdf` and `gazebo` files through this lesson 109 | - `worlds`: default Gazebo worlds that we'll use in the simulations. 110 | 111 | We have another package `bme_ros2_navigation_py` for our python scripts: 112 | ```bash 113 | . 114 | ├── bme_ros2_navigation_py 115 | │   ├── __init__.py 116 | │   ├── send_initialpose.py 117 | │   └── slam_toolbox_load_map.py 118 | ├── package.xml 119 | ├── resource 120 | │   └── bme_ros2_navigation_py 121 | ├── setup.cfg 122 | └── setup.py 123 | ``` 124 | 125 | ## Test the starter package 126 | 127 | After we downloaded the `starter-branch` from GitHub, let's rebuild the workspace and source the `install/setup.bash` file to make sure ROS and its tools are aware about the new package. Before we test it let's take a look on the `spawn_robot.launch.py`, we'll notice a few important changes! 128 | - This package uses ekf sensor fusion by default, we don't forward the `tf` transformations from Gazebo, because it's done by the `robot_localization` package. 129 | - We moved all the `parameter_bridge` topics into a yaml config file `gz_bridge.yaml`, we don't describe them in the launch file anymore, this is more comfortable when we forward many topics. 130 | - There is a new node in the launch file `marker_server` from the `interactive-marker-twist-server` this can be used to move and rotate our robot directly from RViz. Let's install it with `sudo apt install ros-jazzy-interactive-marker-twist-server`! 131 | 132 | Now we are ready to test the package with the usual launch file: 133 | ```bash 134 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 135 | ``` 136 | 137 | ![alt text][image1] 138 | 139 | Let's see the `rqt_graph` tool to see the connections between the nodes: 140 | ![alt text][image2] 141 | 142 | And also try the `rqt_tf_tree`: 143 | 144 | ```bash 145 | ros2 run rqt_tf_tree rqt_tf_tree 146 | ``` 147 | 148 | ![alt text][image3] 149 | 150 | # Mapping 151 | 152 | Let's learn how to create the map of the robot's surrounding. In practice we are using SLAM algorithms, SLAM stands for Simultaneous Localization and Mapping. It is a fundamental technique in robotics (and other fields) that allows a robot to: 153 | 1. Build a map of an unknown environment (mapping). 154 | 2. Track its own pose (position and orientation) within that map at the same time (localization). 155 | 156 | Usually SLAM algorithms consists of 4 core functionalities: 157 | 1. Sensor inputs 158 | SLAM typically uses sensor data (e.g., LIDAR scans, camera images, or depth sensor measurements) to detect features or landmarks in the environment. 159 | 2. State estimation 160 | An internal state (the robot’s pose, including x, y, yaw) is estimated using algorithms like Extended Kalman Filters, Particle Filters, or Graph Optimization. 161 | 3. Map building 162 | As the robot moves, it accumulates new sensor data. The SLAM algorithm integrates that data into a global map (2D grid map, 3D point cloud, or other representations). 163 | 4. Loop closure 164 | When the robot revisits a previously mapped area, the SLAM algorithm detects that it’s the same place (loop closure). This knowledge is used to reduce accumulated drift and refine both the map and pose estimates. 165 | 166 | ## SLAM Toolbox 167 | 168 | In this lesson we will use the `slam_toolbox` package that has to be installed first: 169 | 170 | ```bash 171 | sudo apt install ros-jazzy-slam-toolbox 172 | ``` 173 | 174 | After installing it we will create a new launch file for mapping, the `slam_toolbox` parameters are already in the `config` folder (`slam_toolbox_mapping.yaml`) we'll use these parameters. Let's also move the `interactive_twist_marker` and RViz related functions into the new launch file from `spawn_robot.launch.py`. 175 | 176 | ```python 177 | #launchDescriptionObject.add_action(rviz_launch_arg) 178 | #launchDescriptionObject.add_action(rviz_config_arg) 179 | launchDescriptionObject.add_action(world_arg) 180 | launchDescriptionObject.add_action(model_arg) 181 | launchDescriptionObject.add_action(x_arg) 182 | launchDescriptionObject.add_action(y_arg) 183 | launchDescriptionObject.add_action(yaw_arg) 184 | launchDescriptionObject.add_action(sim_time_arg) 185 | launchDescriptionObject.add_action(world_launch) 186 | #launchDescriptionObject.add_action(rviz_node) 187 | launchDescriptionObject.add_action(spawn_urdf_node) 188 | launchDescriptionObject.add_action(gz_bridge_node) 189 | launchDescriptionObject.add_action(gz_image_bridge_node) 190 | launchDescriptionObject.add_action(relay_camera_info_node) 191 | launchDescriptionObject.add_action(robot_state_publisher_node) 192 | launchDescriptionObject.add_action(trajectory_node) 193 | launchDescriptionObject.add_action(ekf_node) 194 | #launchDescriptionObject.add_action(interactive_marker_twist_server_node) 195 | ``` 196 | 197 | Also change the `reference_frame_id` of `mogi_trajectory_server` from `odom` to `map` because this will be our new reference frame when we have a map! 198 | 199 | ```python 200 | trajectory_node = Node( 201 | package='mogi_trajectory_server', 202 | executable='mogi_trajectory_server', 203 | name='mogi_trajectory_server', 204 | parameters=[{'reference_frame_id': 'map'}] 205 | ) 206 | ``` 207 | 208 | Let's create `mapping.launch.py`: 209 | 210 | ```python 211 | import os 212 | from launch import LaunchDescription 213 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 214 | from launch.conditions import IfCondition 215 | from launch.launch_description_sources import PythonLaunchDescriptionSource 216 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 217 | from launch_ros.actions import Node 218 | from ament_index_python.packages import get_package_share_directory 219 | 220 | def generate_launch_description(): 221 | 222 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 223 | 224 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 225 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 226 | 227 | rviz_launch_arg = DeclareLaunchArgument( 228 | 'rviz', default_value='true', 229 | description='Open RViz' 230 | ) 231 | 232 | rviz_config_arg = DeclareLaunchArgument( 233 | 'rviz_config', default_value='mapping.rviz', 234 | description='RViz config file' 235 | ) 236 | 237 | sim_time_arg = DeclareLaunchArgument( 238 | 'use_sim_time', default_value='True', 239 | description='Flag to enable use_sim_time' 240 | ) 241 | 242 | # Generate path to config file 243 | interactive_marker_config_file_path = os.path.join( 244 | get_package_share_directory('interactive_marker_twist_server'), 245 | 'config', 246 | 'linear.yaml' 247 | ) 248 | 249 | # Path to the Slam Toolbox launch file 250 | slam_toolbox_launch_path = os.path.join( 251 | get_package_share_directory('slam_toolbox'), 252 | 'launch', 253 | 'online_async_launch.py' 254 | ) 255 | 256 | slam_toolbox_params_path = os.path.join( 257 | get_package_share_directory('bme_ros2_navigation'), 258 | 'config', 259 | 'slam_toolbox_mapping.yaml' 260 | ) 261 | 262 | # Launch rviz 263 | rviz_node = Node( 264 | package='rviz2', 265 | executable='rviz2', 266 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 267 | condition=IfCondition(LaunchConfiguration('rviz')), 268 | parameters=[ 269 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 270 | ] 271 | ) 272 | 273 | interactive_marker_twist_server_node = Node( 274 | package='interactive_marker_twist_server', 275 | executable='marker_server', 276 | name='twist_server_node', 277 | parameters=[interactive_marker_config_file_path], 278 | output='screen', 279 | ) 280 | 281 | slam_toolbox_launch = IncludeLaunchDescription( 282 | PythonLaunchDescriptionSource(slam_toolbox_launch_path), 283 | launch_arguments={ 284 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 285 | 'slam_params_file': slam_toolbox_params_path, 286 | }.items() 287 | ) 288 | 289 | launchDescriptionObject = LaunchDescription() 290 | 291 | launchDescriptionObject.add_action(rviz_launch_arg) 292 | launchDescriptionObject.add_action(rviz_config_arg) 293 | launchDescriptionObject.add_action(sim_time_arg) 294 | launchDescriptionObject.add_action(rviz_node) 295 | launchDescriptionObject.add_action(interactive_marker_twist_server_node) 296 | launchDescriptionObject.add_action(slam_toolbox_launch) 297 | 298 | return launchDescriptionObject 299 | ``` 300 | 301 | Build the workspace and we'll need 2 terminals, in the first we launch the simulation like before (but this time it won't open RViz): 302 | ```bash 303 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 304 | ``` 305 | 306 | And in another terminal we launch the new `mapping.launch.py`: 307 | 308 | ```bash 309 | ros2 launch bme_ros2_navigation mapping.launch.py 310 | ``` 311 | 312 | Let's take a look first on `rqt_tf_tree`: 313 | 314 | ```bash 315 | ros2 run rqt_tf_tree rqt_tf_tree 316 | ``` 317 | 318 | ![alt text][image4] 319 | 320 | We can see an additional frame `map` over the `odom` odometry frame. We can also visualize this transformation in RViz: 321 | ![alt text][image5] 322 | 323 | The difference between the `odom` and `map` frames show the accumulated drift of our odometry over time. don't forget that this is already improved a lot thanks to the sensor fusion between the raw odometry and the IMU. 324 | 325 | With SLAM Toolbox we can also save the maps, we have two options: 326 | 1) `Save Map`: The map is saved as a `.pgm` file and a `.yaml` file. This is a black and white image file that can be used with other ROS nodes for localization as we will see later. Since it's only an image file it's impossible to continue the mapping with such a file because SLAM Toolbox handles the map in the background as a graph that cannot be restored from an image. 327 | 2) `Serialize Map`: With this feature we can serialize and later deserialize SLAM Toolbox's graph, so it can be loaded and the mapping can be continued. Although other ROS nodes won't be able to read or use it for localization. 328 | 329 | ![alt text][image6] 330 | 331 | After saving a serialized map next time we can load (deserialize it): 332 | ![alt text][image7] 333 | 334 | And we can also load the map that is in the starter package of this lesson: 335 | ![alt text][image8] 336 | 337 | We can also use a custom node to deserialize the already saved map, it implements the same functionality as the `Deserialize Map` button in RViz: 338 | 339 | ```bash 340 | ros2 run bme_ros2_navigation_py slam_toolbox_load_map 341 | ``` 342 | 343 | Another way to save the `.pgm` and `.yaml` map for later use is the `map_saver_cli` tool of the navigation stack. 344 | ```bash 345 | ros2 run nav2_map_server map_saver_cli -f my_map 346 | ``` 347 | 348 | To use it, `map_server` has to be installed: 349 | 350 | ```bash 351 | sudo apt install ros-jazzy-nav2-map-server 352 | ``` 353 | 354 | # Localization 355 | 356 | While mapping was the process of creating a representation (a map) of an environment. Localization is the process by which a robot determines its own position and orientation within a known environment (map). In other words: 357 | - The environment or map is typically already available or pre-built. 358 | - The robot’s task is to figure out “Where am I?” or “Which direction am I facing?” using sensor data, often by matching its current perceptions to the known map. 359 | 360 | ## Localization with AMCL 361 | 362 | AMCL (Adaptive Monte Carlo Localization) is a particle filter–based 2D localization algorithm. The robot’s possible poses (position + orientation in 2D) are represented by a set of particles. It adaptively samples the robot’s possible poses according to sensor readings and motion updates, converging on an accurate estimate of where the robot is within a known map. 363 | 364 | AMCL is part of the ROS2 navigation stack, let's install it first: 365 | 366 | ```bash 367 | sudo apt install ros-jazzy-nav2-bringup 368 | sudo apt install ros-jazzy-nav2-amcl 369 | ``` 370 | 371 | And then let's create a new launch file `localization.launch.py`: 372 | 373 | ```python 374 | import os 375 | from launch import LaunchDescription 376 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 377 | from launch.conditions import IfCondition 378 | from launch.launch_description_sources import PythonLaunchDescriptionSource 379 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 380 | from launch_ros.actions import Node 381 | from ament_index_python.packages import get_package_share_directory 382 | 383 | def generate_launch_description(): 384 | 385 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 386 | 387 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 388 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 389 | 390 | rviz_launch_arg = DeclareLaunchArgument( 391 | 'rviz', default_value='true', 392 | description='Open RViz' 393 | ) 394 | 395 | rviz_config_arg = DeclareLaunchArgument( 396 | 'rviz_config', default_value='localization.rviz', 397 | description='RViz config file' 398 | ) 399 | 400 | sim_time_arg = DeclareLaunchArgument( 401 | 'use_sim_time', default_value='True', 402 | description='Flag to enable use_sim_time' 403 | ) 404 | 405 | # Generate path to config file 406 | interactive_marker_config_file_path = os.path.join( 407 | get_package_share_directory('interactive_marker_twist_server'), 408 | 'config', 409 | 'linear.yaml' 410 | ) 411 | 412 | # Path to the Slam Toolbox launch file 413 | nav2_localization_launch_path = os.path.join( 414 | get_package_share_directory('nav2_bringup'), 415 | 'launch', 416 | 'localization_launch.py' 417 | ) 418 | 419 | localization_params_path = os.path.join( 420 | get_package_share_directory('bme_ros2_navigation'), 421 | 'config', 422 | 'amcl_localization.yaml' 423 | ) 424 | 425 | map_file_path = os.path.join( 426 | get_package_share_directory('bme_ros2_navigation'), 427 | 'maps', 428 | 'my_map.yaml' 429 | ) 430 | 431 | # Launch rviz 432 | rviz_node = Node( 433 | package='rviz2', 434 | executable='rviz2', 435 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 436 | condition=IfCondition(LaunchConfiguration('rviz')), 437 | parameters=[ 438 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 439 | ] 440 | ) 441 | 442 | interactive_marker_twist_server_node = Node( 443 | package='interactive_marker_twist_server', 444 | executable='marker_server', 445 | name='twist_server_node', 446 | parameters=[interactive_marker_config_file_path], 447 | output='screen', 448 | ) 449 | 450 | localization_launch = IncludeLaunchDescription( 451 | PythonLaunchDescriptionSource(nav2_localization_launch_path), 452 | launch_arguments={ 453 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 454 | 'params_file': localization_params_path, 455 | 'map': map_file_path, 456 | }.items() 457 | ) 458 | 459 | launchDescriptionObject = LaunchDescription() 460 | 461 | launchDescriptionObject.add_action(rviz_launch_arg) 462 | launchDescriptionObject.add_action(rviz_config_arg) 463 | launchDescriptionObject.add_action(sim_time_arg) 464 | launchDescriptionObject.add_action(rviz_node) 465 | launchDescriptionObject.add_action(interactive_marker_twist_server_node) 466 | launchDescriptionObject.add_action(localization_launch) 467 | 468 | return launchDescriptionObject 469 | ``` 470 | 471 | Build the workspace and let's try it, we'll need 2 terminals again, one for the simulation: 472 | ```bash 473 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 474 | ``` 475 | 476 | And another for the new launch file: 477 | 478 | ```bash 479 | ros2 launch bme_ros2_navigation localization.launch.py 480 | ``` 481 | 482 | To start using AMCL we have to provide an initial pose to the `/initialpose` topic. We can use RViz's built in tool for that. 483 | 484 | ![alt text][image9] 485 | 486 | This will initialize AMCL's particles around the initial pose which can be displayed in RViz as a particle cloud where each particle rerpresnts a pose (position + orientation in 2D). 487 | 488 | ![alt text][image10] 489 | 490 | The initial distribution and spread of the partciles depends on the covariance matrix of the initial pose that cannot be changed in RViz, but we can publish our own `/initialpose` topic by a custom node which is in the package of this lesson, `send_initialpose.py`. 491 | 492 | ![alt text][image12] 493 | 494 | The main purpose of the localization algorithm is establishing the transformation between the fixed map and the robot's odometry frame based on real time sensor data. We can visualize this in RViz as we saw it during mapping: 495 | 496 | ![alt text][image11] 497 | 498 | ## Localization with SLAM toolbox 499 | 500 | It's possible to use SLAM toolbox in localization mode, it requires a small adjustment on the parameters which is already part of this lesson, `slam_toolbox_localization.yaml`. This requires the path to the serialized map file. 501 | 502 | > Make sure the `map_file_name` parameter is changed to the path on your machine to the serialized map file! 503 | 504 | Let's create the launch file for localization, `localization_slam_toolbox.launch.py`, it's very similar to the SLAM toolbox mapping launch file: 505 | 506 | ```python 507 | import os 508 | from launch import LaunchDescription 509 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction 510 | from launch.conditions import IfCondition 511 | from launch.launch_description_sources import PythonLaunchDescriptionSource 512 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 513 | from launch_ros.actions import Node 514 | from ament_index_python.packages import get_package_share_directory 515 | 516 | def generate_launch_description(): 517 | 518 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 519 | 520 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 521 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 522 | 523 | rviz_launch_arg = DeclareLaunchArgument( 524 | 'rviz', default_value='true', 525 | description='Open RViz' 526 | ) 527 | 528 | rviz_config_arg = DeclareLaunchArgument( 529 | 'rviz_config', default_value='mapping.rviz', 530 | description='RViz config file' 531 | ) 532 | 533 | sim_time_arg = DeclareLaunchArgument( 534 | 'use_sim_time', default_value='True', 535 | description='Flag to enable use_sim_time' 536 | ) 537 | 538 | # Generate path to config file 539 | interactive_marker_config_file_path = os.path.join( 540 | get_package_share_directory('interactive_marker_twist_server'), 541 | 'config', 542 | 'linear.yaml' 543 | ) 544 | 545 | # Path to the Slam Toolbox launch file 546 | slam_toolbox_launch_path = os.path.join( 547 | get_package_share_directory('slam_toolbox'), 548 | 'launch', 549 | 'localization_launch.py' 550 | ) 551 | 552 | slam_toolbox_params_path = os.path.join( 553 | get_package_share_directory('bme_ros2_navigation'), 554 | 'config', 555 | 'slam_toolbox_localization.yaml' 556 | ) 557 | 558 | # Launch rviz 559 | rviz_node = Node( 560 | package='rviz2', 561 | executable='rviz2', 562 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 563 | condition=IfCondition(LaunchConfiguration('rviz')), 564 | parameters=[ 565 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 566 | ] 567 | ) 568 | 569 | interactive_marker_twist_server_node = Node( 570 | package='interactive_marker_twist_server', 571 | executable='marker_server', 572 | name='twist_server_node', 573 | parameters=[interactive_marker_config_file_path], 574 | output='screen', 575 | ) 576 | 577 | slam_toolbox_launch = IncludeLaunchDescription( 578 | PythonLaunchDescriptionSource(slam_toolbox_launch_path), 579 | launch_arguments={ 580 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 581 | 'slam_params_file': slam_toolbox_params_path, 582 | }.items() 583 | ) 584 | 585 | launchDescriptionObject = LaunchDescription() 586 | 587 | launchDescriptionObject.add_action(rviz_launch_arg) 588 | launchDescriptionObject.add_action(rviz_config_arg) 589 | launchDescriptionObject.add_action(sim_time_arg) 590 | launchDescriptionObject.add_action(rviz_node) 591 | launchDescriptionObject.add_action(interactive_marker_twist_server_node) 592 | launchDescriptionObject.add_action(slam_toolbox_launch) 593 | 594 | return launchDescriptionObject 595 | 596 | ``` 597 | 598 | Rebuild the workspace and try it, we'll see that the map keeps updating unlike with AMCL, this is the normal behavior of the localization with SLAM toolbox. According to [this paper](https://joss.theoj.org/papers/10.21105/joss.02783), the localization mode does continue to update the pose-graph with new constraints and nodes, but the updated map expires over some time. SLAM toolbox describes it as "elastic", which means it holds the updated graph for some amount of time, but does not add it to the permanent graph. 599 | 600 | ![alt text][image13] 601 | 602 | # Navigation 603 | 604 | Navigation in robotics is the overall process that enables a robot to move from one location to another in a safe, efficient, and autonomous manner. It typically involves: 605 | 1. Knowing where the robot is (localization or SLAM), 606 | 2. Knowing where it needs to go (a goal pose or waypoint), 607 | 3. Planning a path to reach that goal (path planning), and 608 | 4. Moving along that path while avoiding dynamic and static obstacles (motion control and obstacle avoidance). 609 | 610 | ROS's nav2 navigation stack implements the above points 2 to 4, for the first point we already met several possible solutions. 611 | 612 | Let's create a launch file that uses both AMCL and the nav2 navigation stack, `navigation.launch.py`: 613 | 614 | ```python 615 | import os 616 | from launch import LaunchDescription 617 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 618 | from launch.conditions import IfCondition 619 | from launch.launch_description_sources import PythonLaunchDescriptionSource 620 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 621 | from launch_ros.actions import Node 622 | from ament_index_python.packages import get_package_share_directory 623 | 624 | def generate_launch_description(): 625 | 626 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 627 | 628 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 629 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 630 | 631 | rviz_launch_arg = DeclareLaunchArgument( 632 | 'rviz', default_value='true', 633 | description='Open RViz' 634 | ) 635 | 636 | rviz_config_arg = DeclareLaunchArgument( 637 | 'rviz_config', default_value='navigation.rviz', 638 | description='RViz config file' 639 | ) 640 | 641 | sim_time_arg = DeclareLaunchArgument( 642 | 'use_sim_time', default_value='True', 643 | description='Flag to enable use_sim_time' 644 | ) 645 | 646 | # Generate path to config file 647 | interactive_marker_config_file_path = os.path.join( 648 | get_package_share_directory('interactive_marker_twist_server'), 649 | 'config', 650 | 'linear.yaml' 651 | ) 652 | 653 | # Path to the Slam Toolbox launch file 654 | nav2_localization_launch_path = os.path.join( 655 | get_package_share_directory('nav2_bringup'), 656 | 'launch', 657 | 'localization_launch.py' 658 | ) 659 | 660 | nav2_navigation_launch_path = os.path.join( 661 | get_package_share_directory('nav2_bringup'), 662 | 'launch', 663 | 'navigation_launch.py' 664 | ) 665 | 666 | localization_params_path = os.path.join( 667 | get_package_share_directory('bme_ros2_navigation'), 668 | 'config', 669 | 'amcl_localization.yaml' 670 | ) 671 | 672 | navigation_params_path = os.path.join( 673 | get_package_share_directory('bme_ros2_navigation'), 674 | 'config', 675 | 'navigation.yaml' 676 | ) 677 | 678 | map_file_path = os.path.join( 679 | get_package_share_directory('bme_ros2_navigation'), 680 | 'maps', 681 | 'my_map.yaml' 682 | ) 683 | 684 | # Launch rviz 685 | rviz_node = Node( 686 | package='rviz2', 687 | executable='rviz2', 688 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 689 | condition=IfCondition(LaunchConfiguration('rviz')), 690 | parameters=[ 691 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 692 | ] 693 | ) 694 | 695 | interactive_marker_twist_server_node = Node( 696 | package='interactive_marker_twist_server', 697 | executable='marker_server', 698 | name='twist_server_node', 699 | parameters=[interactive_marker_config_file_path], 700 | output='screen', 701 | ) 702 | 703 | localization_launch = IncludeLaunchDescription( 704 | PythonLaunchDescriptionSource(nav2_localization_launch_path), 705 | launch_arguments={ 706 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 707 | 'params_file': localization_params_path, 708 | 'map': map_file_path, 709 | }.items() 710 | ) 711 | 712 | navigation_launch = IncludeLaunchDescription( 713 | PythonLaunchDescriptionSource(nav2_navigation_launch_path), 714 | launch_arguments={ 715 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 716 | 'params_file': navigation_params_path, 717 | }.items() 718 | ) 719 | 720 | launchDescriptionObject = LaunchDescription() 721 | 722 | launchDescriptionObject.add_action(rviz_launch_arg) 723 | launchDescriptionObject.add_action(rviz_config_arg) 724 | launchDescriptionObject.add_action(sim_time_arg) 725 | launchDescriptionObject.add_action(rviz_node) 726 | #launchDescriptionObject.add_action(interactive_marker_twist_server_node) 727 | launchDescriptionObject.add_action(localization_launch) 728 | launchDescriptionObject.add_action(navigation_launch) 729 | 730 | return launchDescriptionObject 731 | ``` 732 | 733 | We'll need 2 terminals as before, one for the simulation: 734 | ```bash 735 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 736 | ``` 737 | 738 | And in another terminal we launch the new `navigation.launch.py`: 739 | 740 | ```bash 741 | ros2 launch bme_ros2_navigation navigation.launch.py 742 | ``` 743 | 744 | We are using AMCL, so first we'll have to publish an initial pose, then we have to tell the pose goal to the navigation stack. For that we can also use RViz's other built-in feature: 745 | 746 | ![alt text][image14] 747 | 748 | As soon as the pose goal is received the navigation stack plans a global path to the goal and the controller ensures locally that the robot follows the global path while it avoids dynamic obstacles. The controller calculates a cost map around the robot that determines the ideal trajectory of the robot. If there aren't any obstacles around the robot this cost map weighs the global plan. 749 | 750 | ![alt text][image15] 751 | 752 | If obstacles are detected around the robot those can be visualized as a cost map too: 753 | 754 | ![alt text][image16] 755 | 756 | ## Waypoint navigation 757 | 758 | We can use the navigation stack for waypoint navigation, this can be done through the GUI of RViz or writing a custom node. Let's start with the first one. 759 | 760 | We'll need 2 terminals as before, one for the simulation: 761 | ```bash 762 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 763 | ``` 764 | 765 | And in another terminal we launch the `navigation.launch.py`: 766 | 767 | ```bash 768 | ros2 launch bme_ros2_navigation navigation.launch.py 769 | ``` 770 | 771 | First, we have to make sure that the `Nav2 Goal` toolbar is added to RViz! If not, we can add it under the `+` sign. 772 | ![alt text][image17] 773 | 774 | Then we have to switch nav2 to waypoint following mode: 775 | ![alt text][image18] 776 | 777 | Using the `Nav2 Goal` tool we can define the waypoints: 778 | ![alt text][image19] 779 | 780 | And when we are done with the waypoints we can start the navigation through them: 781 | ![alt text][image20] 782 | ![alt text][image21] 783 | 784 | It's possible to run multiple loops through the waypoints and it's also possible to save and load the waypoints. One example is already in the `config` folder: `waypoints.yaml`. You can see a video about it here: 785 | 786 | 787 | 788 | It's also possible to follow waypoints through the nav2 navigation stack's API with a custom node. Let's create `follow_waypoints.py` in the `bme_ros2_navigation_py` package: 789 | 790 | ```python 791 | import rclpy 792 | from rclpy.node import Node 793 | from geometry_msgs.msg import PoseStamped 794 | from nav2_msgs.action import FollowWaypoints 795 | from rclpy.action import ActionClient 796 | from tf_transformations import quaternion_from_euler 797 | 798 | class WaypointFollower(Node): 799 | def __init__(self): 800 | super().__init__('waypoint_follower') 801 | self._action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') 802 | 803 | def define_waypoints(self): 804 | waypoints = [] 805 | 806 | # Waypoint 1 807 | wp1 = PoseStamped() 808 | wp1.header.frame_id = 'map' 809 | wp1.pose.position.x = 6.0 810 | wp1.pose.position.y = 1.5 811 | q = quaternion_from_euler(0, 0, 0) 812 | wp1.pose.orientation.x = q[0] 813 | wp1.pose.orientation.y = q[1] 814 | wp1.pose.orientation.z = q[2] 815 | wp1.pose.orientation.w = q[3] 816 | waypoints.append(wp1) 817 | 818 | return waypoints 819 | 820 | def send_goal(self): 821 | waypoints = self.define_waypoints() 822 | goal_msg = FollowWaypoints.Goal() 823 | goal_msg.poses = waypoints 824 | 825 | self._action_client.wait_for_server() 826 | self._send_goal_future = self._action_client.send_goal_async( 827 | goal_msg, feedback_callback=self.feedback_callback 828 | ) 829 | self._send_goal_future.add_done_callback(self.goal_response_callback) 830 | 831 | def goal_response_callback(self, future): 832 | goal_handle = future.result() 833 | if not goal_handle.accepted: 834 | self.get_logger().info('Goal rejected.') 835 | return 836 | 837 | self.get_logger().info('Goal accepted.') 838 | self._get_result_future = goal_handle.get_result_async() 839 | self._get_result_future.add_done_callback(self.get_result_callback) 840 | 841 | def feedback_callback(self, feedback_msg): 842 | feedback = feedback_msg.feedback 843 | current_waypoint = feedback.current_waypoint 844 | self.get_logger().info(f'Navigating to waypoint {current_waypoint}') 845 | 846 | def get_result_callback(self, future): 847 | result = future.result().result 848 | self.get_logger().info('Waypoint following completed.') 849 | rclpy.shutdown() 850 | 851 | def main(args=None): 852 | rclpy.init(args=args) 853 | waypoint_follower = WaypointFollower() 854 | waypoint_follower.send_goal() 855 | rclpy.spin(waypoint_follower) 856 | 857 | if __name__ == '__main__': 858 | main() 859 | ``` 860 | 861 | The definition of the waypoints are identical to the config file we have, we define the `x`, `y` and `z` position and the orientation in a quaternion. 862 | 863 | Let's add the entry point to the `setup.py`: 864 | ```python 865 | entry_points={ 866 | 'console_scripts': [ 867 | 'send_initialpose = bme_ros2_navigation_py.send_initialpose:main', 868 | 'slam_toolbox_load_map = bme_ros2_navigation_py.slam_toolbox_load_map:main', 869 | 'follow_waypoints = bme_ros2_navigation_py.follow_waypoints:main', 870 | ], 871 | }, 872 | ``` 873 | 874 | Build the workspace, and we'll need 3 terminals this time, one for the simulation: 875 | ```bash 876 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 877 | ``` 878 | 879 | Another terminal to launch the `navigation.launch.py`: 880 | 881 | ```bash 882 | ros2 launch bme_ros2_navigation navigation.launch.py 883 | ``` 884 | 885 | And in the third one: 886 | ```bash 887 | ros2 run bme_ros2_navigation_py follow_waypoints 888 | ``` 889 | 890 | We can add more waypoints easily: 891 | ```python 892 | ... 893 | # Waypoint 2 894 | wp2 = PoseStamped() 895 | wp2.header.frame_id = 'map' 896 | wp2.pose.position.x = -2.0 897 | wp2.pose.position.y = -8.0 898 | q = quaternion_from_euler(0, 0, 1.57) 899 | wp2.pose.orientation.x = q[0] 900 | wp2.pose.orientation.y = q[1] 901 | wp2.pose.orientation.z = q[2] 902 | wp2.pose.orientation.w = q[3] 903 | waypoints.append(wp2) 904 | 905 | # Waypoint 3 906 | wp3 = PoseStamped() 907 | wp3.header.frame_id = 'map' 908 | wp3.pose.position.x = 0.0 909 | wp3.pose.position.y = 0.0 910 | q = quaternion_from_euler(0, 0, 0) 911 | wp3.pose.orientation.x = q[0] 912 | wp3.pose.orientation.y = q[1] 913 | wp3.pose.orientation.z = q[2] 914 | wp3.pose.orientation.w = q[3] 915 | waypoints.append(wp3) 916 | 917 | # Add more waypoints as needed 918 | ... 919 | ``` 920 | 921 | 922 | 923 | ## Navigation with SLAM 924 | 925 | As we saw to navigate a mobile robot we need to know 2 things: 926 | 1. Knowing where the robot is (localization or SLAM), 927 | 2. Knowing where it needs to go (a goal pose or waypoint) 928 | 929 | In the previous examples we used localization on a know map, but it's also possible to navigate together with an online SLAM. It means we don't know the complete environment around the robot but we can already navigate in the known surrounding. 930 | 931 | Let's create a `navigation_with_slam.launch.py` launch file where we start both the SLAM toolbox and the navigation stack: 932 | ```python 933 | import os 934 | from launch import LaunchDescription 935 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 936 | from launch.conditions import IfCondition 937 | from launch.launch_description_sources import PythonLaunchDescriptionSource 938 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 939 | from launch_ros.actions import Node 940 | from ament_index_python.packages import get_package_share_directory 941 | 942 | def generate_launch_description(): 943 | 944 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 945 | 946 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 947 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 948 | 949 | rviz_launch_arg = DeclareLaunchArgument( 950 | 'rviz', default_value='true', 951 | description='Open RViz' 952 | ) 953 | 954 | rviz_config_arg = DeclareLaunchArgument( 955 | 'rviz_config', default_value='navigation.rviz', 956 | description='RViz config file' 957 | ) 958 | 959 | sim_time_arg = DeclareLaunchArgument( 960 | 'use_sim_time', default_value='True', 961 | description='Flag to enable use_sim_time' 962 | ) 963 | 964 | # Generate path to config file 965 | interactive_marker_config_file_path = os.path.join( 966 | get_package_share_directory('interactive_marker_twist_server'), 967 | 'config', 968 | 'linear.yaml' 969 | ) 970 | 971 | nav2_navigation_launch_path = os.path.join( 972 | get_package_share_directory('nav2_bringup'), 973 | 'launch', 974 | 'navigation_launch.py' 975 | ) 976 | 977 | navigation_params_path = os.path.join( 978 | get_package_share_directory('bme_ros2_navigation'), 979 | 'config', 980 | 'navigation.yaml' 981 | ) 982 | 983 | slam_toolbox_params_path = os.path.join( 984 | get_package_share_directory('bme_ros2_navigation'), 985 | 'config', 986 | 'slam_toolbox_mapping.yaml' 987 | ) 988 | 989 | # Launch rviz 990 | rviz_node = Node( 991 | package='rviz2', 992 | executable='rviz2', 993 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 994 | condition=IfCondition(LaunchConfiguration('rviz')), 995 | parameters=[ 996 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 997 | ] 998 | ) 999 | 1000 | interactive_marker_twist_server_node = Node( 1001 | package='interactive_marker_twist_server', 1002 | executable='marker_server', 1003 | name='twist_server_node', 1004 | parameters=[interactive_marker_config_file_path], 1005 | output='screen', 1006 | ) 1007 | 1008 | # Path to the Slam Toolbox launch file 1009 | slam_toolbox_launch_path = os.path.join( 1010 | get_package_share_directory('slam_toolbox'), 1011 | 'launch', 1012 | 'online_async_launch.py' 1013 | ) 1014 | 1015 | slam_toolbox_launch = IncludeLaunchDescription( 1016 | PythonLaunchDescriptionSource(slam_toolbox_launch_path), 1017 | launch_arguments={ 1018 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 1019 | 'slam_params_file': slam_toolbox_params_path, 1020 | }.items() 1021 | ) 1022 | 1023 | navigation_launch = IncludeLaunchDescription( 1024 | PythonLaunchDescriptionSource(nav2_navigation_launch_path), 1025 | launch_arguments={ 1026 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 1027 | 'params_file': navigation_params_path, 1028 | }.items() 1029 | ) 1030 | 1031 | launchDescriptionObject = LaunchDescription() 1032 | 1033 | launchDescriptionObject.add_action(rviz_launch_arg) 1034 | launchDescriptionObject.add_action(rviz_config_arg) 1035 | launchDescriptionObject.add_action(sim_time_arg) 1036 | launchDescriptionObject.add_action(rviz_node) 1037 | #launchDescriptionObject.add_action(interactive_marker_twist_server_node) 1038 | launchDescriptionObject.add_action(slam_toolbox_launch) 1039 | launchDescriptionObject.add_action(navigation_launch) 1040 | 1041 | return launchDescriptionObject 1042 | ``` 1043 | 1044 | Build the workspace and let's try it! 1045 | 1046 | We'll need 2 terminals as before, one for the simulation: 1047 | ```bash 1048 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 1049 | ``` 1050 | 1051 | And in another terminal we launch the new `navigation_with_slam.launch.py`: 1052 | 1053 | ```bash 1054 | ros2 launch bme_ros2_navigation navigation_with_slam.launch.py 1055 | ``` 1056 | 1057 | ![alt text][image22] 1058 | 1059 | 1060 | 1061 | # Exploration 1062 | 1063 | Exploration is a process by which a robot operating in an unknown or partially known environment actively searches the space to gather new information. This is a real life use case to use SLAM together with the navigation stack. 1064 | 1065 | There is a simple exploration package that we will use in this lesson, you can download it from the GitHub: 1066 | ```bash 1067 | git clone https://github.com/MOGI-ROS/m-explore-ros2.git 1068 | ``` 1069 | 1070 | Build the workspace and source the `setup.bash` to make sure ROS is aware about the new package! 1071 | 1072 | We'll need 3 terminals, one for the simulation: 1073 | ```bash 1074 | ros2 launch bme_ros2_navigation spawn_robot.launch.py 1075 | ``` 1076 | 1077 | In another terminal we launch the `navigation_with_slam.launch.py`: 1078 | ```bash 1079 | ros2 launch bme_ros2_navigation navigation_with_slam.launch.py 1080 | ``` 1081 | 1082 | And in the third one we launch the exploration: 1083 | ```bash 1084 | ros2 launch explore_lite explore.launch.py 1085 | ``` 1086 | 1087 | The exploration node will identify the boundaries of the know surrounding and will navigate the robot until it finds all the physical boundaries of the environment. 1088 | ![alt text][image23] 1089 | 1090 | We can take a look on the `rqt_graph` of the simulation, but we'll see it's quite big! We can find the exploration node and we can see that it subscribes to the `/map` and to the action status and feedback of the navigation stack. 1091 | ![alt text][image24] 1092 | 1093 | And finally here is a video about exploration: 1094 | 1095 | -------------------------------------------------------------------------------- /assets/exploration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/exploration.png -------------------------------------------------------------------------------- /assets/load-map-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/load-map-1.png -------------------------------------------------------------------------------- /assets/load-map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/load-map.png -------------------------------------------------------------------------------- /assets/localization-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/localization-1.png -------------------------------------------------------------------------------- /assets/localization-2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/localization-2.png -------------------------------------------------------------------------------- /assets/localization-3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/localization-3.png -------------------------------------------------------------------------------- /assets/localization-4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/localization-4.png -------------------------------------------------------------------------------- /assets/localization.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/localization.png -------------------------------------------------------------------------------- /assets/mapping.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/mapping.png -------------------------------------------------------------------------------- /assets/navigation-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/navigation-1.png -------------------------------------------------------------------------------- /assets/navigation-2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/navigation-2.png -------------------------------------------------------------------------------- /assets/navigation-slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/navigation-slam.png -------------------------------------------------------------------------------- /assets/navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/navigation.png -------------------------------------------------------------------------------- /assets/rqt-graph-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/rqt-graph-1.png -------------------------------------------------------------------------------- /assets/rqt-graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/rqt-graph.png -------------------------------------------------------------------------------- /assets/save-map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/save-map.png -------------------------------------------------------------------------------- /assets/starter-package.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/starter-package.png -------------------------------------------------------------------------------- /assets/tf-tree-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/tf-tree-1.png -------------------------------------------------------------------------------- /assets/tf-tree.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/tf-tree.png -------------------------------------------------------------------------------- /assets/waypoint-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/waypoint-1.png -------------------------------------------------------------------------------- /assets/waypoint-2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/waypoint-2.png -------------------------------------------------------------------------------- /assets/waypoint-3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/waypoint-3.png -------------------------------------------------------------------------------- /assets/waypoint-4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/waypoint-4.png -------------------------------------------------------------------------------- /assets/waypoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/waypoint.png -------------------------------------------------------------------------------- /assets/youtube-exploration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/youtube-exploration.png -------------------------------------------------------------------------------- /assets/youtube-navigation-slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/youtube-navigation-slam.png -------------------------------------------------------------------------------- /assets/youtube-navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/youtube-navigation.png -------------------------------------------------------------------------------- /assets/youtube-waypoint-node.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/youtube-waypoint-node.png -------------------------------------------------------------------------------- /assets/youtube-waypoints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/assets/youtube-waypoints.png -------------------------------------------------------------------------------- /bme_ros2_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(bme_ros2_navigation) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | if(BUILD_TESTING) 15 | find_package(ament_lint_auto REQUIRED) 16 | # the following line skips the linter which checks for copyrights 17 | # comment the line when a copyright and license is added to all source files 18 | set(ament_cmake_copyright_FOUND TRUE) 19 | # the following line skips cpplint (only works in a git repo) 20 | # comment the line when this package is in a git repo and when 21 | # a copyright and license is added to all source files 22 | set(ament_cmake_cpplint_FOUND TRUE) 23 | ament_lint_auto_find_test_dependencies() 24 | endif() 25 | 26 | install(DIRECTORY 27 | launch 28 | worlds 29 | rviz 30 | urdf 31 | meshes 32 | config 33 | maps 34 | DESTINATION share/${PROJECT_NAME} 35 | ) 36 | 37 | ament_package() 38 | -------------------------------------------------------------------------------- /bme_ros2_navigation/config/amcl_localization.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | alpha1: 0.2 4 | alpha2: 0.2 5 | alpha3: 0.2 6 | alpha4: 0.2 7 | alpha5: 0.2 8 | base_frame_id: "base_footprint" 9 | beam_skip_distance: 0.5 10 | beam_skip_error_threshold: 0.9 11 | beam_skip_threshold: 0.3 12 | do_beamskip: false 13 | global_frame_id: "map" 14 | lambda_short: 0.1 15 | laser_likelihood_max_dist: 2.0 16 | laser_max_range: 100.0 17 | laser_min_range: -1.0 18 | laser_model_type: "likelihood_field" 19 | max_beams: 60 20 | max_particles: 2000 21 | min_particles: 500 22 | odom_frame_id: "odom" 23 | pf_err: 0.05 24 | pf_z: 0.99 25 | recovery_alpha_fast: 0.0 26 | recovery_alpha_slow: 0.0 27 | resample_interval: 1 28 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 29 | save_pose_rate: 0.5 30 | sigma_hit: 0.2 31 | tf_broadcast: true 32 | transform_tolerance: 1.0 33 | update_min_a: 0.2 34 | update_min_d: 0.25 35 | z_hit: 0.5 36 | z_max: 0.05 37 | z_rand: 0.5 38 | z_short: 0.05 39 | scan_topic: scan 40 | map_topic: map 41 | set_initial_pose: true 42 | always_reset_initial_pose: false 43 | first_map_only: false 44 | initial_pose: 45 | x: 0.0 46 | y: 0.0 47 | z: 0.0 48 | yaw: 0.0 49 | 50 | -------------------------------------------------------------------------------- /bme_ros2_navigation/config/ekf.yaml: -------------------------------------------------------------------------------- 1 | ### ekf config file ### 2 | ekf_filter_node: 3 | ros__parameters: 4 | # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin 5 | # computation until it receives at least one message from one of theinputs. It will then run continuously at the 6 | # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. 7 | frequency: 30.0 8 | 9 | # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is 10 | # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar 11 | # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected 12 | # by, for example, an IMU. Defaults to false if unspecified. 13 | two_d_mode: true 14 | 15 | # Whether to publish the acceleration state. Defaults to false if unspecified. 16 | publish_acceleration: false 17 | 18 | # Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified. 19 | publish_tf: true 20 | 21 | # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. 22 | # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame. 23 | # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame" 24 | # to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. 25 | # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark 26 | # observations) then: 27 | # 3a. Set your "world_frame" to your map_frame value 28 | # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node 29 | # from robot_localization! However, that instance should *not* fuse the global data. 30 | map_frame: map # Defaults to "map" if unspecified 31 | odom_frame: odom # Defaults to "odom" if unspecified 32 | base_link_frame: base_footprint # Defaults to "base_link" ifunspecified 33 | world_frame: odom # Defaults to the value of odom_frame if unspecified 34 | 35 | odom0: odom 36 | odom0_config: [false, false, false, 37 | false, false, false, 38 | true, true, false, 39 | false, false, true, 40 | false, false, false] 41 | 42 | imu0: imu 43 | imu0_config: [false, false, false, 44 | true, true, true, 45 | false, false, false, 46 | false, false, true, 47 | true, false, false] 48 | 49 | imu0_differential: false 50 | 51 | 52 | # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is 53 | # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each 54 | # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. 55 | # However, if users find that a given variable is slow to converge, one approach is to increase the 56 | # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error 57 | # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are 58 | # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if 59 | # unspecified. 60 | # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support 61 | # both full specification or specification of only the diagonal values. 62 | process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 63 | 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 64 | 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 65 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 66 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 67 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 68 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 69 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 70 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 71 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 72 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 73 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 74 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 75 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 76 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] 77 | 78 | # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal 79 | # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in 80 | # question. Users should take care not to use large values for variables that will not be measured directly. The values 81 | # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below 82 | # if unspecified. In this example, we specify only the diagonal of the matrix. 83 | initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] -------------------------------------------------------------------------------- /bme_ros2_navigation/config/gz_bridge.yaml: -------------------------------------------------------------------------------- 1 | - ros_topic_name: "clock" 2 | gz_topic_name: "clock" 3 | ros_type_name: "rosgraph_msgs/msg/Clock" 4 | gz_type_name: "gz.msgs.Clock" 5 | direction: "GZ_TO_ROS" 6 | 7 | - ros_topic_name: "joint_states" 8 | gz_topic_name: "joint_states" 9 | ros_type_name: "sensor_msgs/msg/JointState" 10 | gz_type_name: "gz.msgs.Model" 11 | direction: "GZ_TO_ROS" 12 | 13 | - ros_topic_name: "odom" 14 | gz_topic_name: "odom" 15 | ros_type_name: "nav_msgs/msg/Odometry" 16 | gz_type_name: "gz.msgs.Odometry" 17 | direction: "GZ_TO_ROS" 18 | 19 | - ros_topic_name: "cmd_vel" 20 | gz_topic_name: "cmd_vel" 21 | ros_type_name: "geometry_msgs/msg/Twist" 22 | gz_type_name: "gz.msgs.Twist" 23 | direction: "ROS_TO_GZ" 24 | 25 | - ros_topic_name: "camera/camera_info" 26 | gz_topic_name: "camera/camera_info" 27 | ros_type_name: "sensor_msgs/msg/CameraInfo" 28 | gz_type_name: "gz.msgs.CameraInfo" 29 | direction: "GZ_TO_ROS" 30 | 31 | - ros_topic_name: "scan" 32 | gz_topic_name: "scan" 33 | ros_type_name: "sensor_msgs/msg/LaserScan" 34 | gz_type_name: "gz.msgs.LaserScan" 35 | direction: "GZ_TO_ROS" 36 | 37 | - ros_topic_name: "scan/points" 38 | gz_topic_name: "scan/points" 39 | ros_type_name: "sensor_msgs/msg/PointCloud2" 40 | gz_type_name: "gz.msgs.PointCloudPacked" 41 | direction: "GZ_TO_ROS" 42 | 43 | - ros_topic_name: "imu" 44 | gz_topic_name: "imu" 45 | ros_type_name: "sensor_msgs/msg/Imu" 46 | gz_type_name: "gz.msgs.IMU" 47 | direction: "GZ_TO_ROS" 48 | -------------------------------------------------------------------------------- /bme_ros2_navigation/config/navigation.yaml: -------------------------------------------------------------------------------- 1 | bt_navigator: 2 | ros__parameters: 3 | global_frame: map 4 | robot_base_frame: base_link 5 | odom_topic: /odom 6 | bt_loop_duration: 10 7 | default_server_timeout: 20 8 | wait_for_service_timeout: 1000 9 | action_server_result_timeout: 900.0 10 | navigators: ["navigate_to_pose", "navigate_through_poses"] 11 | navigate_to_pose: 12 | plugin: "nav2_bt_navigator::NavigateToPoseNavigator" 13 | navigate_through_poses: 14 | plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" 15 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 16 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 17 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 18 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 19 | 20 | # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). 21 | # Built-in plugins are added automatically 22 | # plugin_lib_names: [] 23 | 24 | error_code_names: 25 | - compute_path_error_code 26 | - follow_path_error_code 27 | 28 | controller_server: 29 | ros__parameters: 30 | controller_frequency: 20.0 31 | costmap_update_timeout: 0.30 32 | min_x_velocity_threshold: 0.001 33 | min_y_velocity_threshold: 0.5 34 | min_theta_velocity_threshold: 0.001 35 | failure_tolerance: 0.3 36 | progress_checker_plugins: ["progress_checker"] 37 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 38 | controller_plugins: ["FollowPath"] 39 | use_realtime_priority: false 40 | 41 | # Progress checker parameters 42 | progress_checker: 43 | plugin: "nav2_controller::SimpleProgressChecker" 44 | required_movement_radius: 0.5 45 | movement_time_allowance: 10.0 46 | # Goal checker parameters 47 | #precise_goal_checker: 48 | # plugin: "nav2_controller::SimpleGoalChecker" 49 | # xy_goal_tolerance: 0.25 50 | # yaw_goal_tolerance: 0.25 51 | # stateful: True 52 | general_goal_checker: 53 | stateful: True 54 | plugin: "nav2_controller::SimpleGoalChecker" 55 | xy_goal_tolerance: 0.25 56 | yaw_goal_tolerance: 0.25 57 | # DWB parameters 58 | FollowPath: 59 | plugin: "dwb_core::DWBLocalPlanner" 60 | debug_trajectory_details: True 61 | min_vel_x: 0.0 62 | min_vel_y: 0.0 63 | max_vel_x: 0.22 64 | max_vel_y: 0.0 65 | max_vel_theta: 2.84 66 | min_speed_xy: 0.0 67 | max_speed_xy: 0.22 68 | min_speed_theta: 0.0 69 | # Add high threshold velocity for turtlebot 3 issue. 70 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 71 | acc_lim_x: 2.5 72 | acc_lim_y: 0.0 73 | acc_lim_theta: 3.2 74 | decel_lim_x: -2.5 75 | decel_lim_y: 0.0 76 | decel_lim_theta: -3.2 77 | vx_samples: 20 78 | vy_samples: 5 79 | vtheta_samples: 20 80 | sim_time: 1.7 81 | linear_granularity: 0.05 82 | angular_granularity: 0.025 83 | transform_tolerance: 0.2 84 | xy_goal_tolerance: 0.25 85 | trans_stopped_velocity: 0.25 86 | short_circuit_trajectory_evaluation: True 87 | stateful: True 88 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 89 | BaseObstacle.scale: 0.02 90 | PathAlign.scale: 32.0 91 | PathAlign.forward_point_distance: 0.1 92 | GoalAlign.scale: 24.0 93 | GoalAlign.forward_point_distance: 0.1 94 | PathDist.scale: 32.0 95 | GoalDist.scale: 24.0 96 | RotateToGoal.scale: 32.0 97 | RotateToGoal.slowing_factor: 5.0 98 | RotateToGoal.lookahead_time: -1.0 99 | publish_cost_grid_pc: True 100 | # MPPI parameters, default controller is DWB above 101 | FollowPathMPPI: 102 | plugin: "nav2_mppi_controller::MPPIController" 103 | time_steps: 56 104 | model_dt: 0.05 105 | batch_size: 2000 106 | ax_max: 3.0 107 | ax_min: -3.0 108 | ay_max: 3.0 109 | az_max: 3.5 110 | vx_std: 0.2 111 | vy_std: 0.2 112 | wz_std: 0.4 113 | vx_max: 0.5 114 | vx_min: -0.35 115 | vy_max: 0.5 116 | wz_max: 1.9 117 | iteration_count: 1 118 | prune_distance: 1.7 119 | transform_tolerance: 0.1 120 | temperature: 0.3 121 | gamma: 0.015 122 | motion_model: "DiffDrive" 123 | visualize: true 124 | regenerate_noises: true 125 | TrajectoryVisualizer: 126 | trajectory_step: 5 127 | time_step: 3 128 | AckermannConstraints: 129 | min_turning_r: 0.2 130 | critics: [ 131 | "ConstraintCritic", "CostCritic", "GoalCritic", 132 | "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", 133 | "PathAngleCritic", "PreferForwardCritic"] 134 | ConstraintCritic: 135 | enabled: true 136 | cost_power: 1 137 | cost_weight: 4.0 138 | GoalCritic: 139 | enabled: true 140 | cost_power: 1 141 | cost_weight: 5.0 142 | threshold_to_consider: 1.4 143 | GoalAngleCritic: 144 | enabled: true 145 | cost_power: 1 146 | cost_weight: 3.0 147 | threshold_to_consider: 0.5 148 | PreferForwardCritic: 149 | enabled: true 150 | cost_power: 1 151 | cost_weight: 5.0 152 | threshold_to_consider: 0.5 153 | CostCritic: 154 | enabled: true 155 | cost_power: 1 156 | cost_weight: 3.81 157 | critical_cost: 300.0 158 | consider_footprint: true 159 | collision_cost: 1000000.0 160 | near_goal_distance: 1.0 161 | trajectory_point_step: 2 162 | PathAlignCritic: 163 | enabled: true 164 | cost_power: 1 165 | cost_weight: 14.0 166 | max_path_occupancy_ratio: 0.05 167 | trajectory_point_step: 4 168 | threshold_to_consider: 0.5 169 | offset_from_furthest: 20 170 | use_path_orientations: false 171 | PathFollowCritic: 172 | enabled: true 173 | cost_power: 1 174 | cost_weight: 5.0 175 | offset_from_furthest: 5 176 | threshold_to_consider: 1.4 177 | PathAngleCritic: 178 | enabled: true 179 | cost_power: 1 180 | cost_weight: 2.0 181 | offset_from_furthest: 4 182 | threshold_to_consider: 0.5 183 | max_angle_to_furthest: 1.0 184 | mode: 0 185 | # TwirlingCritic: 186 | # enabled: true 187 | # twirling_cost_power: 1 188 | # twirling_cost_weight: 10.0 189 | 190 | local_costmap: 191 | local_costmap: 192 | ros__parameters: 193 | update_frequency: 5.0 194 | publish_frequency: 2.0 195 | global_frame: odom 196 | robot_base_frame: base_link 197 | rolling_window: true 198 | width: 3 199 | height: 3 200 | resolution: 0.05 201 | robot_radius: 0.22 202 | plugins: ["voxel_layer", "inflation_layer"] 203 | inflation_layer: 204 | plugin: "nav2_costmap_2d::InflationLayer" 205 | cost_scaling_factor: 3.0 206 | inflation_radius: 0.70 207 | voxel_layer: 208 | plugin: "nav2_costmap_2d::VoxelLayer" 209 | enabled: True 210 | publish_voxel_map: True 211 | origin_z: 0.0 212 | z_resolution: 0.05 213 | z_voxels: 16 214 | max_obstacle_height: 2.0 215 | mark_threshold: 0 216 | observation_sources: scan 217 | scan: 218 | topic: /scan 219 | max_obstacle_height: 2.0 220 | clearing: True 221 | marking: True 222 | data_type: "LaserScan" 223 | raytrace_max_range: 3.0 224 | raytrace_min_range: 0.0 225 | obstacle_max_range: 2.5 226 | obstacle_min_range: 0.0 227 | static_layer: 228 | plugin: "nav2_costmap_2d::StaticLayer" 229 | map_subscribe_transient_local: True 230 | always_send_full_costmap: True 231 | 232 | global_costmap: 233 | global_costmap: 234 | ros__parameters: 235 | update_frequency: 1.0 236 | publish_frequency: 1.0 237 | global_frame: map 238 | robot_base_frame: base_link 239 | robot_radius: 0.22 240 | resolution: 0.05 241 | track_unknown_space: true 242 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 243 | obstacle_layer: 244 | plugin: "nav2_costmap_2d::ObstacleLayer" 245 | enabled: True 246 | observation_sources: scan 247 | scan: 248 | topic: /scan 249 | max_obstacle_height: 2.0 250 | clearing: True 251 | marking: True 252 | data_type: "LaserScan" 253 | raytrace_max_range: 3.0 254 | raytrace_min_range: 0.0 255 | obstacle_max_range: 2.5 256 | obstacle_min_range: 0.0 257 | static_layer: 258 | plugin: "nav2_costmap_2d::StaticLayer" 259 | map_subscribe_transient_local: True 260 | inflation_layer: 261 | plugin: "nav2_costmap_2d::InflationLayer" 262 | cost_scaling_factor: 3.0 263 | inflation_radius: 0.7 264 | always_send_full_costmap: True 265 | 266 | 267 | planner_server: 268 | ros__parameters: 269 | expected_planner_frequency: 20.0 270 | planner_plugins: ["GridBased"] 271 | costmap_update_timeout: 1.0 272 | GridBased: 273 | plugin: "nav2_navfn_planner::NavfnPlanner" 274 | tolerance: 0.5 275 | use_astar: false 276 | allow_unknown: true 277 | 278 | smoother_server: 279 | ros__parameters: 280 | smoother_plugins: ["simple_smoother"] 281 | simple_smoother: 282 | plugin: "nav2_smoother::SimpleSmoother" 283 | tolerance: 1.0e-10 284 | max_its: 1000 285 | do_refinement: True 286 | 287 | behavior_server: 288 | ros__parameters: 289 | local_costmap_topic: local_costmap/costmap_raw 290 | global_costmap_topic: global_costmap/costmap_raw 291 | local_footprint_topic: local_costmap/published_footprint 292 | global_footprint_topic: global_costmap/published_footprint 293 | cycle_frequency: 10.0 294 | behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] 295 | spin: 296 | plugin: "nav2_behaviors::Spin" 297 | backup: 298 | plugin: "nav2_behaviors::BackUp" 299 | drive_on_heading: 300 | plugin: "nav2_behaviors::DriveOnHeading" 301 | wait: 302 | plugin: "nav2_behaviors::Wait" 303 | assisted_teleop: 304 | plugin: "nav2_behaviors::AssistedTeleop" 305 | local_frame: odom 306 | global_frame: map 307 | robot_base_frame: base_link 308 | transform_tolerance: 0.1 309 | simulate_ahead_time: 2.0 310 | max_rotational_vel: 1.0 311 | min_rotational_vel: 0.4 312 | rotational_acc_lim: 3.2 313 | 314 | waypoint_follower: 315 | ros__parameters: 316 | loop_rate: 20 317 | stop_on_failure: false 318 | action_server_result_timeout: 900.0 319 | waypoint_task_executor_plugin: "wait_at_waypoint" 320 | wait_at_waypoint: 321 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 322 | enabled: True 323 | waypoint_pause_duration: 200 324 | 325 | velocity_smoother: 326 | ros__parameters: 327 | smoothing_frequency: 20.0 328 | scale_velocities: False 329 | feedback: "OPEN_LOOP" 330 | max_velocity: [0.5, 0.0, 2.0] 331 | min_velocity: [-0.5, 0.0, -2.0] 332 | max_accel: [2.5, 0.0, 3.2] 333 | max_decel: [-2.5, 0.0, -3.2] 334 | odom_topic: "odom" 335 | odom_duration: 0.1 336 | deadband_velocity: [0.0, 0.0, 0.0] 337 | velocity_timeout: 1.0 338 | 339 | collision_monitor: 340 | ros__parameters: 341 | base_frame_id: "base_footprint" 342 | odom_frame_id: "odom" 343 | cmd_vel_in_topic: "cmd_vel_smoothed" 344 | cmd_vel_out_topic: "cmd_vel" 345 | state_topic: "collision_monitor_state" 346 | transform_tolerance: 0.2 347 | source_timeout: 1.0 348 | base_shift_correction: True 349 | stop_pub_timeout: 2.0 350 | # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, 351 | # and robot footprint for "approach" action type. 352 | polygons: ["FootprintApproach"] 353 | FootprintApproach: 354 | type: "polygon" 355 | action_type: "approach" 356 | footprint_topic: "/local_costmap/published_footprint" 357 | time_before_collision: 1.2 358 | simulation_time_step: 0.1 359 | min_points: 6 360 | visualize: False 361 | enabled: True 362 | observation_sources: ["scan"] 363 | scan: 364 | type: "scan" 365 | topic: "scan" 366 | min_height: 0.15 367 | max_height: 2.0 368 | enabled: True 369 | 370 | docking_server: 371 | ros__parameters: 372 | controller_frequency: 50.0 373 | initial_perception_timeout: 5.0 374 | wait_charge_timeout: 5.0 375 | dock_approach_timeout: 30.0 376 | undock_linear_tolerance: 0.05 377 | undock_angular_tolerance: 0.1 378 | max_retries: 3 379 | base_frame: "base_link" 380 | fixed_frame: "odom" 381 | dock_backwards: false 382 | dock_prestaging_tolerance: 0.5 383 | 384 | # Types of docks 385 | dock_plugins: ['simple_charging_dock'] 386 | simple_charging_dock: 387 | plugin: 'opennav_docking::SimpleChargingDock' 388 | docking_threshold: 0.05 389 | staging_x_offset: -0.7 390 | use_external_detection_pose: true 391 | use_battery_status: false # true 392 | use_stall_detection: false # true 393 | 394 | external_detection_timeout: 1.0 395 | external_detection_translation_x: -0.18 396 | external_detection_translation_y: 0.0 397 | external_detection_rotation_roll: -1.57 398 | external_detection_rotation_pitch: -1.57 399 | external_detection_rotation_yaw: 0.0 400 | filter_coef: 0.1 401 | 402 | # Dock instances 403 | # The following example illustrates configuring dock instances. 404 | # docks: ['home_dock'] # Input your docks here 405 | # home_dock: 406 | # type: 'simple_charging_dock' 407 | # frame: map 408 | # pose: [0.0, 0.0, 0.0] 409 | 410 | controller: 411 | k_phi: 3.0 412 | k_delta: 2.0 413 | v_linear_min: 0.15 414 | v_linear_max: 0.15 415 | 416 | -------------------------------------------------------------------------------- /bme_ros2_navigation/config/slam_toolbox_localization.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # Plugin params 5 | solver_plugin: solver_plugins::CeresSolver 6 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 7 | ceres_preconditioner: SCHUR_JACOBI 8 | ceres_trust_strategy: LEVENBERG_MARQUARDT 9 | ceres_dogleg_type: TRADITIONAL_DOGLEG 10 | ceres_loss_function: None 11 | 12 | # ROS Parameters 13 | odom_frame: odom 14 | map_frame: map 15 | base_frame: base_footprint 16 | scan_topic: /scan 17 | use_map_saver: true 18 | mode: localization 19 | 20 | # if you'd like to immediately start continuing a map at a given pose 21 | # or at the dock, but they are mutually exclusive, if pose is given 22 | # will use pose 23 | map_file_name: /home/david/ros2_ws/src/ROS2-lessons/Week-7-8-ROS2-Navigation/bme_ros2_navigation/maps/serialized 24 | # map_start_pose: [0.0, 0.0, 0.0] 25 | map_start_at_dock: true 26 | 27 | debug_logging: true 28 | throttle_scans: 1 29 | transform_publish_period: 0.02 #if 0 never publishes odometry 30 | map_update_interval: 5.0 31 | resolution: 0.05 32 | min_laser_range: 0.0 #for rastering images 33 | max_laser_range: 20.0 #for rastering images 34 | minimum_time_interval: 0.5 35 | transform_timeout: 0.2 36 | tf_buffer_duration: 30.0 37 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 38 | enable_interactive_mode: true 39 | 40 | # General Parameters 41 | use_scan_matching: true 42 | use_scan_barycenter: true 43 | minimum_travel_distance: 0.5 44 | minimum_travel_heading: 0.5 45 | scan_buffer_size: 10 46 | scan_buffer_maximum_scan_distance: 10.0 47 | link_match_minimum_response_fine: 0.1 48 | link_scan_maximum_distance: 1.5 49 | loop_search_maximum_distance: 3.0 50 | do_loop_closing: true 51 | loop_match_minimum_chain_size: 10 52 | loop_match_maximum_variance_coarse: 3.0 53 | loop_match_minimum_response_coarse: 0.35 54 | loop_match_minimum_response_fine: 0.45 55 | 56 | # Correlation Parameters - Correlation Parameters 57 | correlation_search_space_dimension: 0.5 58 | correlation_search_space_resolution: 0.01 59 | correlation_search_space_smear_deviation: 0.1 60 | 61 | # Correlation Parameters - Loop Closure Parameters 62 | loop_search_space_dimension: 8.0 63 | loop_search_space_resolution: 0.05 64 | loop_search_space_smear_deviation: 0.03 65 | 66 | # Scan Matcher Parameters 67 | distance_variance_penalty: 0.5 68 | angle_variance_penalty: 1.0 69 | 70 | fine_search_angle_offset: 0.00349 71 | coarse_search_angle_offset: 0.349 72 | coarse_angle_resolution: 0.0349 73 | minimum_angle_penalty: 0.9 74 | minimum_distance_penalty: 0.5 75 | use_response_expansion: true -------------------------------------------------------------------------------- /bme_ros2_navigation/config/slam_toolbox_mapping.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # Plugin params 5 | solver_plugin: solver_plugins::CeresSolver 6 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 7 | ceres_preconditioner: SCHUR_JACOBI 8 | ceres_trust_strategy: LEVENBERG_MARQUARDT 9 | ceres_dogleg_type: TRADITIONAL_DOGLEG 10 | ceres_loss_function: None 11 | 12 | # ROS Parameters 13 | odom_frame: odom 14 | map_frame: map 15 | base_frame: base_footprint 16 | scan_topic: /scan 17 | use_map_saver: true 18 | mode: mapping #localization 19 | 20 | # if you'd like to immediately start continuing a map at a given pose 21 | # or at the dock, but they are mutually exclusive, if pose is given 22 | # will use pose 23 | #map_file_name: test_steve 24 | # map_start_pose: [0.0, 0.0, 0.0] 25 | #map_start_at_dock: true 26 | 27 | debug_logging: true 28 | throttle_scans: 1 29 | transform_publish_period: 0.02 #if 0 never publishes odometry 30 | map_update_interval: 5.0 31 | resolution: 0.05 32 | min_laser_range: 0.0 #for rastering images 33 | max_laser_range: 20.0 #for rastering images 34 | minimum_time_interval: 0.5 35 | transform_timeout: 0.2 36 | tf_buffer_duration: 30.0 37 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 38 | enable_interactive_mode: true 39 | 40 | # General Parameters 41 | use_scan_matching: true 42 | use_scan_barycenter: true 43 | minimum_travel_distance: 0.5 44 | minimum_travel_heading: 0.5 45 | scan_buffer_size: 10 46 | scan_buffer_maximum_scan_distance: 10.0 47 | link_match_minimum_response_fine: 0.1 48 | link_scan_maximum_distance: 1.5 49 | loop_search_maximum_distance: 3.0 50 | do_loop_closing: true 51 | loop_match_minimum_chain_size: 10 52 | loop_match_maximum_variance_coarse: 3.0 53 | loop_match_minimum_response_coarse: 0.35 54 | loop_match_minimum_response_fine: 0.45 55 | 56 | # Correlation Parameters - Correlation Parameters 57 | correlation_search_space_dimension: 0.5 58 | correlation_search_space_resolution: 0.01 59 | correlation_search_space_smear_deviation: 0.1 60 | 61 | # Correlation Parameters - Loop Closure Parameters 62 | loop_search_space_dimension: 8.0 63 | loop_search_space_resolution: 0.05 64 | loop_search_space_smear_deviation: 0.03 65 | 66 | # Scan Matcher Parameters 67 | distance_variance_penalty: 0.5 68 | angle_variance_penalty: 1.0 69 | 70 | fine_search_angle_offset: 0.00349 71 | coarse_search_angle_offset: 0.349 72 | coarse_angle_resolution: 0.0349 73 | minimum_angle_penalty: 0.9 74 | minimum_distance_penalty: 0.5 75 | use_response_expansion: true -------------------------------------------------------------------------------- /bme_ros2_navigation/config/waypoints.yaml: -------------------------------------------------------------------------------- 1 | waypoints: 2 | waypoint0: 3 | pose: 4 | - 0.89327335357666016 5 | - 1.1334505081176758 6 | - 0 7 | orientation: 8 | - 0.92752015501641683 9 | - 0 10 | - 0 11 | - 0.37377314247859234 12 | waypoint1: 13 | pose: 14 | - 0.81780672073364258 15 | - -0.94519948959350586 16 | - 0 17 | orientation: 18 | - 0.90411574593179533 19 | - 0 20 | - 0 21 | - -0.42728762907226014 22 | waypoint2: 23 | pose: 24 | - -0.7626488208770752 25 | - -0.098996639251708984 26 | - 0 27 | orientation: 28 | - 0.0072688065351738836 29 | - 0 30 | - 0 31 | - 0.99997358187681851 -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/check_urdf.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 5 | from launch_ros.substitutions import FindPackageShare 6 | 7 | 8 | def generate_launch_description(): 9 | 10 | pkg_bme_ros2_navigation = FindPackageShare('bme_ros2_navigation') 11 | default_rviz_config_path = PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', 'urdf.rviz']) 12 | 13 | # Show joint state publisher GUI for joints 14 | gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'], 15 | description='Flag to enable joint_state_publisher_gui') 16 | 17 | # RViz config file path 18 | rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, 19 | description='Absolute path to rviz config file') 20 | 21 | 22 | # URDF model path within the bme_ros2_navigation package 23 | model_arg = DeclareLaunchArgument( 24 | 'model', default_value='mogi_bot.urdf', 25 | description='Name of the URDF description to load' 26 | ) 27 | 28 | # Use built-in ROS2 URDF launch package with our own arguments 29 | urdf = IncludeLaunchDescription( 30 | PathJoinSubstitution([FindPackageShare('urdf_launch'), 'launch', 'display.launch.py']), 31 | launch_arguments={ 32 | 'urdf_package': 'bme_ros2_navigation', 33 | 'urdf_package_path': PathJoinSubstitution(['urdf', LaunchConfiguration('model')]), 34 | 'rviz_config': LaunchConfiguration('rvizconfig'), 35 | 'jsp_gui': LaunchConfiguration('gui')}.items() 36 | ) 37 | 38 | launchDescriptionObject = LaunchDescription() 39 | 40 | launchDescriptionObject.add_action(gui_arg) 41 | launchDescriptionObject.add_action(rviz_arg) 42 | launchDescriptionObject.add_action(model_arg) 43 | launchDescriptionObject.add_action(urdf) 44 | 45 | return launchDescriptionObject 46 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/localization.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 13 | 14 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 15 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 16 | 17 | rviz_launch_arg = DeclareLaunchArgument( 18 | 'rviz', default_value='true', 19 | description='Open RViz' 20 | ) 21 | 22 | rviz_config_arg = DeclareLaunchArgument( 23 | 'rviz_config', default_value='localization.rviz', 24 | description='RViz config file' 25 | ) 26 | 27 | sim_time_arg = DeclareLaunchArgument( 28 | 'use_sim_time', default_value='True', 29 | description='Flag to enable use_sim_time' 30 | ) 31 | 32 | # Generate path to config file 33 | interactive_marker_config_file_path = os.path.join( 34 | get_package_share_directory('interactive_marker_twist_server'), 35 | 'config', 36 | 'linear.yaml' 37 | ) 38 | 39 | # Path to the Slam Toolbox launch file 40 | nav2_localization_launch_path = os.path.join( 41 | get_package_share_directory('nav2_bringup'), 42 | 'launch', 43 | 'localization_launch.py' 44 | ) 45 | 46 | localization_params_path = os.path.join( 47 | get_package_share_directory('bme_ros2_navigation'), 48 | 'config', 49 | 'amcl_localization.yaml' 50 | ) 51 | 52 | map_file_path = os.path.join( 53 | get_package_share_directory('bme_ros2_navigation'), 54 | 'maps', 55 | 'my_map.yaml' 56 | ) 57 | 58 | # Launch rviz 59 | rviz_node = Node( 60 | package='rviz2', 61 | executable='rviz2', 62 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 63 | condition=IfCondition(LaunchConfiguration('rviz')), 64 | parameters=[ 65 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 66 | ] 67 | ) 68 | 69 | interactive_marker_twist_server_node = Node( 70 | package='interactive_marker_twist_server', 71 | executable='marker_server', 72 | name='twist_server_node', 73 | parameters=[interactive_marker_config_file_path], 74 | output='screen', 75 | ) 76 | 77 | localization_launch = IncludeLaunchDescription( 78 | PythonLaunchDescriptionSource(nav2_localization_launch_path), 79 | launch_arguments={ 80 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 81 | 'params_file': localization_params_path, 82 | 'map': map_file_path, 83 | }.items() 84 | ) 85 | 86 | launchDescriptionObject = LaunchDescription() 87 | 88 | launchDescriptionObject.add_action(rviz_launch_arg) 89 | launchDescriptionObject.add_action(rviz_config_arg) 90 | launchDescriptionObject.add_action(sim_time_arg) 91 | launchDescriptionObject.add_action(rviz_node) 92 | launchDescriptionObject.add_action(interactive_marker_twist_server_node) 93 | launchDescriptionObject.add_action(localization_launch) 94 | 95 | return launchDescriptionObject 96 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/localization_slam_toolbox.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 13 | 14 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 15 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 16 | 17 | rviz_launch_arg = DeclareLaunchArgument( 18 | 'rviz', default_value='true', 19 | description='Open RViz' 20 | ) 21 | 22 | rviz_config_arg = DeclareLaunchArgument( 23 | 'rviz_config', default_value='mapping.rviz', 24 | description='RViz config file' 25 | ) 26 | 27 | sim_time_arg = DeclareLaunchArgument( 28 | 'use_sim_time', default_value='True', 29 | description='Flag to enable use_sim_time' 30 | ) 31 | 32 | # Generate path to config file 33 | interactive_marker_config_file_path = os.path.join( 34 | get_package_share_directory('interactive_marker_twist_server'), 35 | 'config', 36 | 'linear.yaml' 37 | ) 38 | 39 | # Path to the Slam Toolbox launch file 40 | slam_toolbox_launch_path = os.path.join( 41 | get_package_share_directory('slam_toolbox'), 42 | 'launch', 43 | 'localization_launch.py' 44 | ) 45 | 46 | slam_toolbox_params_path = os.path.join( 47 | get_package_share_directory('bme_ros2_navigation'), 48 | 'config', 49 | 'slam_toolbox_localization.yaml' 50 | ) 51 | 52 | # Launch rviz 53 | rviz_node = Node( 54 | package='rviz2', 55 | executable='rviz2', 56 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 57 | condition=IfCondition(LaunchConfiguration('rviz')), 58 | parameters=[ 59 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 60 | ] 61 | ) 62 | 63 | interactive_marker_twist_server_node = Node( 64 | package='interactive_marker_twist_server', 65 | executable='marker_server', 66 | name='twist_server_node', 67 | parameters=[interactive_marker_config_file_path], 68 | output='screen', 69 | ) 70 | 71 | slam_toolbox_launch = IncludeLaunchDescription( 72 | PythonLaunchDescriptionSource(slam_toolbox_launch_path), 73 | launch_arguments={ 74 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 75 | 'slam_params_file': slam_toolbox_params_path, 76 | }.items() 77 | ) 78 | 79 | launchDescriptionObject = LaunchDescription() 80 | 81 | launchDescriptionObject.add_action(rviz_launch_arg) 82 | launchDescriptionObject.add_action(rviz_config_arg) 83 | launchDescriptionObject.add_action(sim_time_arg) 84 | launchDescriptionObject.add_action(rviz_node) 85 | launchDescriptionObject.add_action(interactive_marker_twist_server_node) 86 | launchDescriptionObject.add_action(slam_toolbox_launch) 87 | 88 | return launchDescriptionObject 89 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/mapping.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 13 | 14 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 15 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 16 | 17 | rviz_launch_arg = DeclareLaunchArgument( 18 | 'rviz', default_value='true', 19 | description='Open RViz' 20 | ) 21 | 22 | rviz_config_arg = DeclareLaunchArgument( 23 | 'rviz_config', default_value='mapping.rviz', 24 | description='RViz config file' 25 | ) 26 | 27 | sim_time_arg = DeclareLaunchArgument( 28 | 'use_sim_time', default_value='True', 29 | description='Flag to enable use_sim_time' 30 | ) 31 | 32 | # Generate path to config file 33 | interactive_marker_config_file_path = os.path.join( 34 | get_package_share_directory('interactive_marker_twist_server'), 35 | 'config', 36 | 'linear.yaml' 37 | ) 38 | 39 | # Path to the Slam Toolbox launch file 40 | slam_toolbox_launch_path = os.path.join( 41 | get_package_share_directory('slam_toolbox'), 42 | 'launch', 43 | 'online_async_launch.py' 44 | ) 45 | 46 | slam_toolbox_params_path = os.path.join( 47 | get_package_share_directory('bme_ros2_navigation'), 48 | 'config', 49 | 'slam_toolbox_mapping.yaml' 50 | ) 51 | 52 | # Launch rviz 53 | rviz_node = Node( 54 | package='rviz2', 55 | executable='rviz2', 56 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 57 | condition=IfCondition(LaunchConfiguration('rviz')), 58 | parameters=[ 59 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 60 | ] 61 | ) 62 | 63 | interactive_marker_twist_server_node = Node( 64 | package='interactive_marker_twist_server', 65 | executable='marker_server', 66 | name='twist_server_node', 67 | parameters=[interactive_marker_config_file_path], 68 | output='screen', 69 | ) 70 | 71 | slam_toolbox_launch = IncludeLaunchDescription( 72 | PythonLaunchDescriptionSource(slam_toolbox_launch_path), 73 | launch_arguments={ 74 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 75 | 'slam_params_file': slam_toolbox_params_path, 76 | }.items() 77 | ) 78 | 79 | launchDescriptionObject = LaunchDescription() 80 | 81 | launchDescriptionObject.add_action(rviz_launch_arg) 82 | launchDescriptionObject.add_action(rviz_config_arg) 83 | launchDescriptionObject.add_action(sim_time_arg) 84 | launchDescriptionObject.add_action(rviz_node) 85 | launchDescriptionObject.add_action(interactive_marker_twist_server_node) 86 | launchDescriptionObject.add_action(slam_toolbox_launch) 87 | 88 | return launchDescriptionObject 89 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/navigation.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 13 | 14 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 15 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 16 | 17 | rviz_launch_arg = DeclareLaunchArgument( 18 | 'rviz', default_value='true', 19 | description='Open RViz' 20 | ) 21 | 22 | rviz_config_arg = DeclareLaunchArgument( 23 | 'rviz_config', default_value='navigation.rviz', 24 | description='RViz config file' 25 | ) 26 | 27 | sim_time_arg = DeclareLaunchArgument( 28 | 'use_sim_time', default_value='True', 29 | description='Flag to enable use_sim_time' 30 | ) 31 | 32 | # Generate path to config file 33 | interactive_marker_config_file_path = os.path.join( 34 | get_package_share_directory('interactive_marker_twist_server'), 35 | 'config', 36 | 'linear.yaml' 37 | ) 38 | 39 | # Path to the Slam Toolbox launch file 40 | nav2_localization_launch_path = os.path.join( 41 | get_package_share_directory('nav2_bringup'), 42 | 'launch', 43 | 'localization_launch.py' 44 | ) 45 | 46 | nav2_navigation_launch_path = os.path.join( 47 | get_package_share_directory('nav2_bringup'), 48 | 'launch', 49 | 'navigation_launch.py' 50 | ) 51 | 52 | localization_params_path = os.path.join( 53 | get_package_share_directory('bme_ros2_navigation'), 54 | 'config', 55 | 'amcl_localization.yaml' 56 | ) 57 | 58 | navigation_params_path = os.path.join( 59 | get_package_share_directory('bme_ros2_navigation'), 60 | 'config', 61 | 'navigation.yaml' 62 | ) 63 | 64 | map_file_path = os.path.join( 65 | get_package_share_directory('bme_ros2_navigation'), 66 | 'maps', 67 | 'my_map.yaml' 68 | ) 69 | 70 | # Launch rviz 71 | rviz_node = Node( 72 | package='rviz2', 73 | executable='rviz2', 74 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 75 | condition=IfCondition(LaunchConfiguration('rviz')), 76 | parameters=[ 77 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 78 | ] 79 | ) 80 | 81 | interactive_marker_twist_server_node = Node( 82 | package='interactive_marker_twist_server', 83 | executable='marker_server', 84 | name='twist_server_node', 85 | parameters=[interactive_marker_config_file_path], 86 | output='screen', 87 | ) 88 | 89 | localization_launch = IncludeLaunchDescription( 90 | PythonLaunchDescriptionSource(nav2_localization_launch_path), 91 | launch_arguments={ 92 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 93 | 'params_file': localization_params_path, 94 | 'map': map_file_path, 95 | }.items() 96 | ) 97 | 98 | navigation_launch = IncludeLaunchDescription( 99 | PythonLaunchDescriptionSource(nav2_navigation_launch_path), 100 | launch_arguments={ 101 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 102 | 'params_file': navigation_params_path, 103 | }.items() 104 | ) 105 | 106 | launchDescriptionObject = LaunchDescription() 107 | 108 | launchDescriptionObject.add_action(rviz_launch_arg) 109 | launchDescriptionObject.add_action(rviz_config_arg) 110 | launchDescriptionObject.add_action(sim_time_arg) 111 | launchDescriptionObject.add_action(rviz_node) 112 | #launchDescriptionObject.add_action(interactive_marker_twist_server_node) 113 | launchDescriptionObject.add_action(localization_launch) 114 | launchDescriptionObject.add_action(navigation_launch) 115 | 116 | return launchDescriptionObject 117 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/navigation_with_slam.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 13 | 14 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 15 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 16 | 17 | rviz_launch_arg = DeclareLaunchArgument( 18 | 'rviz', default_value='true', 19 | description='Open RViz' 20 | ) 21 | 22 | rviz_config_arg = DeclareLaunchArgument( 23 | 'rviz_config', default_value='navigation.rviz', 24 | description='RViz config file' 25 | ) 26 | 27 | sim_time_arg = DeclareLaunchArgument( 28 | 'use_sim_time', default_value='True', 29 | description='Flag to enable use_sim_time' 30 | ) 31 | 32 | # Generate path to config file 33 | interactive_marker_config_file_path = os.path.join( 34 | get_package_share_directory('interactive_marker_twist_server'), 35 | 'config', 36 | 'linear.yaml' 37 | ) 38 | 39 | nav2_navigation_launch_path = os.path.join( 40 | get_package_share_directory('nav2_bringup'), 41 | 'launch', 42 | 'navigation_launch.py' 43 | ) 44 | 45 | navigation_params_path = os.path.join( 46 | get_package_share_directory('bme_ros2_navigation'), 47 | 'config', 48 | 'navigation.yaml' 49 | ) 50 | 51 | slam_toolbox_params_path = os.path.join( 52 | get_package_share_directory('bme_ros2_navigation'), 53 | 'config', 54 | 'slam_toolbox_mapping.yaml' 55 | ) 56 | 57 | # Launch rviz 58 | rviz_node = Node( 59 | package='rviz2', 60 | executable='rviz2', 61 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 62 | condition=IfCondition(LaunchConfiguration('rviz')), 63 | parameters=[ 64 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 65 | ] 66 | ) 67 | 68 | interactive_marker_twist_server_node = Node( 69 | package='interactive_marker_twist_server', 70 | executable='marker_server', 71 | name='twist_server_node', 72 | parameters=[interactive_marker_config_file_path], 73 | output='screen', 74 | ) 75 | 76 | # Path to the Slam Toolbox launch file 77 | slam_toolbox_launch_path = os.path.join( 78 | get_package_share_directory('slam_toolbox'), 79 | 'launch', 80 | 'online_async_launch.py' 81 | ) 82 | 83 | slam_toolbox_launch = IncludeLaunchDescription( 84 | PythonLaunchDescriptionSource(slam_toolbox_launch_path), 85 | launch_arguments={ 86 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 87 | 'slam_params_file': slam_toolbox_params_path, 88 | }.items() 89 | ) 90 | 91 | navigation_launch = IncludeLaunchDescription( 92 | PythonLaunchDescriptionSource(nav2_navigation_launch_path), 93 | launch_arguments={ 94 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 95 | 'params_file': navigation_params_path, 96 | }.items() 97 | ) 98 | 99 | launchDescriptionObject = LaunchDescription() 100 | 101 | launchDescriptionObject.add_action(rviz_launch_arg) 102 | launchDescriptionObject.add_action(rviz_config_arg) 103 | launchDescriptionObject.add_action(sim_time_arg) 104 | launchDescriptionObject.add_action(rviz_node) 105 | #launchDescriptionObject.add_action(interactive_marker_twist_server_node) 106 | launchDescriptionObject.add_action(slam_toolbox_launch) 107 | launchDescriptionObject.add_action(navigation_launch) 108 | 109 | return launchDescriptionObject 110 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/spawn_robot.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 13 | 14 | gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation) 15 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 16 | 17 | rviz_launch_arg = DeclareLaunchArgument( 18 | 'rviz', default_value='true', 19 | description='Open RViz' 20 | ) 21 | 22 | rviz_config_arg = DeclareLaunchArgument( 23 | 'rviz_config', default_value='rviz.rviz', 24 | description='RViz config file' 25 | ) 26 | 27 | world_arg = DeclareLaunchArgument( 28 | 'world', default_value='home.sdf', 29 | description='Name of the Gazebo world file to load' 30 | ) 31 | 32 | model_arg = DeclareLaunchArgument( 33 | 'model', default_value='mogi_bot.urdf', 34 | description='Name of the URDF description to load' 35 | ) 36 | 37 | x_arg = DeclareLaunchArgument( 38 | 'x', default_value='2.5', 39 | description='x coordinate of spawned robot' 40 | ) 41 | 42 | y_arg = DeclareLaunchArgument( 43 | 'y', default_value='1.5', 44 | description='y coordinate of spawned robot' 45 | ) 46 | 47 | yaw_arg = DeclareLaunchArgument( 48 | 'yaw', default_value='-1.5707', 49 | description='yaw angle of spawned robot' 50 | ) 51 | 52 | sim_time_arg = DeclareLaunchArgument( 53 | 'use_sim_time', default_value='True', 54 | description='Flag to enable use_sim_time' 55 | ) 56 | 57 | # Define the path to your URDF or Xacro file 58 | urdf_file_path = PathJoinSubstitution([ 59 | pkg_bme_ros2_navigation, # Replace with your package name 60 | "urdf", 61 | LaunchConfiguration('model') # Replace with your URDF or Xacro file 62 | ]) 63 | 64 | gz_bridge_params_path = os.path.join( 65 | get_package_share_directory('bme_ros2_navigation'), 66 | 'config', 67 | 'gz_bridge.yaml' 68 | ) 69 | 70 | # Generate path to config file 71 | interactive_marker_config_file_path = os.path.join( 72 | get_package_share_directory('interactive_marker_twist_server'), 73 | 'config', 74 | 'linear.yaml' 75 | ) 76 | 77 | world_launch = IncludeLaunchDescription( 78 | PythonLaunchDescriptionSource( 79 | os.path.join(pkg_bme_ros2_navigation, 'launch', 'world.launch.py'), 80 | ), 81 | launch_arguments={ 82 | 'world': LaunchConfiguration('world'), 83 | }.items() 84 | ) 85 | 86 | # Launch rviz 87 | rviz_node = Node( 88 | package='rviz2', 89 | executable='rviz2', 90 | arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])], 91 | condition=IfCondition(LaunchConfiguration('rviz')), 92 | parameters=[ 93 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 94 | ] 95 | ) 96 | 97 | # Spawn the URDF model using the `/world//create` service 98 | spawn_urdf_node = Node( 99 | package="ros_gz_sim", 100 | executable="create", 101 | arguments=[ 102 | "-name", "mogi_bot", 103 | "-topic", "robot_description", 104 | "-x", LaunchConfiguration('x'), "-y", LaunchConfiguration('y'), "-z", "0.5", "-Y", LaunchConfiguration('yaw') # Initial spawn position 105 | ], 106 | output="screen", 107 | parameters=[ 108 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 109 | ] 110 | ) 111 | 112 | # Node to bridge /cmd_vel and /odom 113 | gz_bridge_node = Node( 114 | package="ros_gz_bridge", 115 | executable="parameter_bridge", 116 | arguments=[ 117 | '--ros-args', '-p', 118 | f'config_file:={gz_bridge_params_path}' 119 | ], 120 | output="screen", 121 | parameters=[ 122 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 123 | ] 124 | ) 125 | 126 | # Node to bridge /cmd_vel and /odom 127 | gz_image_bridge_node = Node( 128 | package="ros_gz_image", 129 | executable="image_bridge", 130 | arguments=[ 131 | "/camera/image", 132 | ], 133 | output="screen", 134 | parameters=[ 135 | {'use_sim_time': LaunchConfiguration('use_sim_time'), 136 | 'camera.image.compressed.jpeg_quality': 75}, 137 | ], 138 | ) 139 | 140 | # Relay node to republish camera_info to /camera_info 141 | relay_camera_info_node = Node( 142 | package='topic_tools', 143 | executable='relay', 144 | name='relay_camera_info', 145 | output='screen', 146 | arguments=['camera/camera_info', 'camera/image/camera_info'], 147 | parameters=[ 148 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 149 | ] 150 | ) 151 | 152 | robot_state_publisher_node = Node( 153 | package='robot_state_publisher', 154 | executable='robot_state_publisher', 155 | name='robot_state_publisher', 156 | output='screen', 157 | parameters=[ 158 | {'robot_description': Command(['xacro', ' ', urdf_file_path]), 159 | 'use_sim_time': LaunchConfiguration('use_sim_time')}, 160 | ], 161 | remappings=[ 162 | ('/tf', 'tf'), 163 | ('/tf_static', 'tf_static') 164 | ] 165 | ) 166 | 167 | trajectory_node = Node( 168 | package='mogi_trajectory_server', 169 | executable='mogi_trajectory_server', 170 | name='mogi_trajectory_server', 171 | parameters=[{'reference_frame_id': 'map'}] 172 | ) 173 | 174 | ekf_node = Node( 175 | package='robot_localization', 176 | executable='ekf_node', 177 | name='ekf_filter_node', 178 | output='screen', 179 | parameters=[ 180 | os.path.join(pkg_bme_ros2_navigation, 'config', 'ekf.yaml'), 181 | {'use_sim_time': LaunchConfiguration('use_sim_time')}, 182 | ] 183 | ) 184 | 185 | interactive_marker_twist_server_node = Node( 186 | package='interactive_marker_twist_server', 187 | executable='marker_server', 188 | name='twist_server_node', 189 | parameters=[interactive_marker_config_file_path], 190 | output='screen', 191 | ) 192 | 193 | launchDescriptionObject = LaunchDescription() 194 | 195 | #launchDescriptionObject.add_action(rviz_launch_arg) 196 | #launchDescriptionObject.add_action(rviz_config_arg) 197 | launchDescriptionObject.add_action(world_arg) 198 | launchDescriptionObject.add_action(model_arg) 199 | launchDescriptionObject.add_action(x_arg) 200 | launchDescriptionObject.add_action(y_arg) 201 | launchDescriptionObject.add_action(yaw_arg) 202 | launchDescriptionObject.add_action(sim_time_arg) 203 | launchDescriptionObject.add_action(world_launch) 204 | #launchDescriptionObject.add_action(rviz_node) 205 | launchDescriptionObject.add_action(spawn_urdf_node) 206 | launchDescriptionObject.add_action(gz_bridge_node) 207 | launchDescriptionObject.add_action(gz_image_bridge_node) 208 | launchDescriptionObject.add_action(relay_camera_info_node) 209 | launchDescriptionObject.add_action(robot_state_publisher_node) 210 | launchDescriptionObject.add_action(trajectory_node) 211 | launchDescriptionObject.add_action(ekf_node) 212 | #launchDescriptionObject.add_action(interactive_marker_twist_server_node) 213 | 214 | return launchDescriptionObject 215 | -------------------------------------------------------------------------------- /bme_ros2_navigation/launch/world.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | world_arg = DeclareLaunchArgument( 12 | 'world', default_value='world.sdf', 13 | description='Name of the Gazebo world file to load' 14 | ) 15 | 16 | pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation') 17 | pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 18 | 19 | # Add your own gazebo library path here 20 | gazebo_models_path = "/home/david/gazebo_models" 21 | os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path 22 | 23 | 24 | gazebo_launch = IncludeLaunchDescription( 25 | PythonLaunchDescriptionSource( 26 | os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), 27 | ), 28 | launch_arguments={'gz_args': [PathJoinSubstitution([ 29 | pkg_bme_ros2_navigation, 30 | 'worlds', 31 | LaunchConfiguration('world') 32 | ]), 33 | TextSubstitution(text=' -r -v -v1')], 34 | #TextSubstitution(text=' -r -v -v1 --render-engine ogre --render-engine-gui-api-backend opengl')], 35 | 'on_exit_shutdown': 'true'}.items() 36 | ) 37 | 38 | launchDescriptionObject = LaunchDescription() 39 | 40 | launchDescriptionObject.add_action(world_arg) 41 | launchDescriptionObject.add_action(gazebo_launch) 42 | 43 | return launchDescriptionObject -------------------------------------------------------------------------------- /bme_ros2_navigation/maps/my_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/bme_ros2_navigation/maps/my_map.pgm -------------------------------------------------------------------------------- /bme_ros2_navigation/maps/my_map.yaml: -------------------------------------------------------------------------------- 1 | image: my_map.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-4.64, -10, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /bme_ros2_navigation/maps/serialized.data: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/bme_ros2_navigation/maps/serialized.data -------------------------------------------------------------------------------- /bme_ros2_navigation/maps/serialized.posegraph: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/bme_ros2_navigation/maps/serialized.posegraph -------------------------------------------------------------------------------- /bme_ros2_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bme_ros2_navigation 5 | 1.0.0 6 | Using slam, localization and navigation with Gazebo Harmonic and ROS Jazzy for BME MOGI ROS2 course 7 | David Dudas 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /bme_ros2_navigation/rviz/localization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 158 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /Imu1/Axes properties1 9 | - /Imu1/Acceleration properties1 10 | - /ParticleCloud1/Topic1 11 | Splitter Ratio: 0.5 12 | Tree Height: 2062 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz_default_plugins/RobotModel 54 | Collision Enabled: false 55 | Description File: "" 56 | Description Source: Topic 57 | Description Topic: 58 | Depth: 5 59 | Durability Policy: Volatile 60 | History Policy: Keep Last 61 | Reliability Policy: Reliable 62 | Value: /robot_description 63 | Enabled: true 64 | Links: 65 | All Links Enabled: true 66 | Expand Joint Details: false 67 | Expand Link Details: false 68 | Expand Tree: false 69 | Link Tree Style: Links in Alphabetic Order 70 | base_footprint: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | base_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | camera_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | camera_link_optical: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | imu_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | left_wheel: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | right_wheel: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | scan_link: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | Mass Properties: 108 | Inertia: false 109 | Mass: false 110 | Name: RobotModel 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Class: rviz_default_plugins/TF 116 | Enabled: true 117 | Filter (blacklist): "" 118 | Filter (whitelist): "" 119 | Frame Timeout: 15 120 | Frames: 121 | All Enabled: false 122 | base_footprint: 123 | Value: false 124 | base_link: 125 | Value: false 126 | camera_link: 127 | Value: false 128 | camera_link_optical: 129 | Value: false 130 | imu_link: 131 | Value: false 132 | left_wheel: 133 | Value: false 134 | map: 135 | Value: true 136 | odom: 137 | Value: true 138 | right_wheel: 139 | Value: false 140 | scan_link: 141 | Value: false 142 | Marker Scale: 1 143 | Name: TF 144 | Show Arrows: true 145 | Show Axes: true 146 | Show Names: true 147 | Tree: 148 | map: 149 | odom: 150 | base_footprint: 151 | base_link: 152 | camera_link: 153 | camera_link_optical: 154 | {} 155 | imu_link: 156 | {} 157 | left_wheel: 158 | {} 159 | right_wheel: 160 | {} 161 | scan_link: 162 | {} 163 | Update Interval: 0 164 | Value: true 165 | - Alpha: 1 166 | Buffer Length: 1 167 | Class: rviz_default_plugins/Path 168 | Color: 0; 0; 255 169 | Enabled: true 170 | Head Diameter: 0.30000001192092896 171 | Head Length: 0.20000000298023224 172 | Length: 0.30000001192092896 173 | Line Style: Lines 174 | Line Width: 0.029999999329447746 175 | Name: Odometry filtered 176 | Offset: 177 | X: 0 178 | Y: 0 179 | Z: 0 180 | Pose Color: 255; 85; 255 181 | Pose Style: None 182 | Radius: 0.029999999329447746 183 | Shaft Diameter: 0.10000000149011612 184 | Shaft Length: 0.10000000149011612 185 | Topic: 186 | Depth: 5 187 | Durability Policy: Volatile 188 | Filter size: 10 189 | History Policy: Keep Last 190 | Reliability Policy: Reliable 191 | Value: /trajectory 192 | Value: true 193 | - Class: rviz_default_plugins/Camera 194 | Enabled: false 195 | Far Plane Distance: 100 196 | Image Rendering: background and overlay 197 | Name: Camera 198 | Overlay Alpha: 0.5 199 | Topic: 200 | Depth: 5 201 | Durability Policy: Volatile 202 | History Policy: Keep Last 203 | Reliability Policy: Reliable 204 | Value: /camera/image/compressed 205 | Value: false 206 | Visibility: 207 | Grid: true 208 | Imu: true 209 | InteractiveMarkers: true 210 | LaserScan: true 211 | Map: true 212 | Odometry filtered: true 213 | ParticleCloud: true 214 | RobotModel: true 215 | TF: true 216 | Value: true 217 | Zoom Factor: 1 218 | - Alpha: 1 219 | Autocompute Intensity Bounds: true 220 | Autocompute Value Bounds: 221 | Max Value: 10 222 | Min Value: -10 223 | Value: true 224 | Axis: Z 225 | Channel Name: intensity 226 | Class: rviz_default_plugins/LaserScan 227 | Color: 255; 255; 255 228 | Color Transformer: Intensity 229 | Decay Time: 0 230 | Enabled: true 231 | Invert Rainbow: false 232 | Max Color: 255; 255; 255 233 | Max Intensity: 1 234 | Min Color: 0; 0; 0 235 | Min Intensity: 0 236 | Name: LaserScan 237 | Position Transformer: XYZ 238 | Selectable: true 239 | Size (Pixels): 3 240 | Size (m): 0.009999999776482582 241 | Style: Flat Squares 242 | Topic: 243 | Depth: 5 244 | Durability Policy: Volatile 245 | Filter size: 10 246 | History Policy: Keep Last 247 | Reliability Policy: Reliable 248 | Value: /scan 249 | Use Fixed Frame: true 250 | Use rainbow: true 251 | Value: true 252 | - Acceleration properties: 253 | Acc. vector alpha: 0.30000001192092896 254 | Acc. vector color: 0; 0; 255 255 | Acc. vector scale: 0.05000000074505806 256 | Derotate acceleration: false 257 | Enable acceleration: true 258 | Axes properties: 259 | Axes scale: 0.30000001192092896 260 | Enable axes: true 261 | Box properties: 262 | Box alpha: 1 263 | Box color: 255; 0; 0 264 | Enable box: false 265 | x_scale: 1 266 | y_scale: 1 267 | z_scale: 1 268 | Class: rviz_imu_plugin/Imu 269 | Enabled: true 270 | Name: Imu 271 | Topic: 272 | Depth: 5 273 | Durability Policy: Volatile 274 | Filter size: 10 275 | History Policy: Keep Last 276 | Reliability Policy: Reliable 277 | Value: /imu 278 | Value: true 279 | fixed_frame_orientation: true 280 | - Class: rviz_default_plugins/InteractiveMarkers 281 | Enable Transparency: true 282 | Enabled: true 283 | Interactive Markers Namespace: /twist_server 284 | Name: InteractiveMarkers 285 | Show Axes: false 286 | Show Descriptions: false 287 | Show Visual Aids: false 288 | Value: true 289 | - Alpha: 0.699999988079071 290 | Binary representation: false 291 | Binary threshold: 100 292 | Class: rviz_default_plugins/Map 293 | Color Scheme: map 294 | Draw Behind: false 295 | Enabled: true 296 | Name: Map 297 | Topic: 298 | Depth: 5 299 | Durability Policy: Transient Local 300 | Filter size: 10 301 | History Policy: Keep Last 302 | Reliability Policy: Reliable 303 | Value: /map 304 | Update Topic: 305 | Depth: 5 306 | Durability Policy: Volatile 307 | History Policy: Keep Last 308 | Reliability Policy: Reliable 309 | Value: /map_updates 310 | Use Timestamp: false 311 | Value: true 312 | - Alpha: 1 313 | Class: nav2_rviz_plugins/ParticleCloud 314 | Color: 255; 25; 0 315 | Enabled: true 316 | Max Arrow Length: 0.30000001192092896 317 | Min Arrow Length: 0.10000000149011612 318 | Name: ParticleCloud 319 | Shape: Arrow (Flat) 320 | Topic: 321 | Depth: 5 322 | Durability Policy: Volatile 323 | Filter size: 10 324 | History Policy: Keep Last 325 | Reliability Policy: Best Effort 326 | Value: /particle_cloud 327 | Value: true 328 | Enabled: true 329 | Global Options: 330 | Background Color: 48; 48; 48 331 | Fixed Frame: map 332 | Frame Rate: 30 333 | Name: root 334 | Tools: 335 | - Class: rviz_default_plugins/Interact 336 | Hide Inactive Objects: true 337 | - Class: rviz_default_plugins/MoveCamera 338 | - Class: rviz_default_plugins/Select 339 | - Class: rviz_default_plugins/FocusCamera 340 | - Class: rviz_default_plugins/Measure 341 | Line color: 128; 128; 0 342 | - Class: rviz_default_plugins/SetInitialPose 343 | Covariance x: 0.25 344 | Covariance y: 0.25 345 | Covariance yaw: 0.06853891909122467 346 | Topic: 347 | Depth: 5 348 | Durability Policy: Volatile 349 | History Policy: Keep Last 350 | Reliability Policy: Reliable 351 | Value: /initialpose 352 | - Class: rviz_default_plugins/SetGoal 353 | Topic: 354 | Depth: 5 355 | Durability Policy: Volatile 356 | History Policy: Keep Last 357 | Reliability Policy: Reliable 358 | Value: /goal_pose 359 | - Class: rviz_default_plugins/PublishPoint 360 | Single click: true 361 | Topic: 362 | Depth: 5 363 | Durability Policy: Volatile 364 | History Policy: Keep Last 365 | Reliability Policy: Reliable 366 | Value: /clicked_point 367 | Transformation: 368 | Current: 369 | Class: rviz_default_plugins/TF 370 | Value: true 371 | Views: 372 | Current: 373 | Class: rviz_default_plugins/Orbit 374 | Distance: 8.036380767822266 375 | Enable Stereo Rendering: 376 | Stereo Eye Separation: 0.05999999865889549 377 | Stereo Focal Distance: 1 378 | Swap Stereo Eyes: false 379 | Value: false 380 | Focal Point: 381 | X: 2.292283296585083 382 | Y: -1.3710719347000122 383 | Z: 0.6975705027580261 384 | Focal Shape Fixed Size: true 385 | Focal Shape Size: 0.05000000074505806 386 | Invert Z Axis: false 387 | Name: Current View 388 | Near Clip Distance: 0.009999999776482582 389 | Pitch: 1.0697966814041138 390 | Target Frame: 391 | Value: Orbit (rviz) 392 | Yaw: 2.645399808883667 393 | Saved: ~ 394 | Window Geometry: 395 | Camera: 396 | collapsed: false 397 | Displays: 398 | collapsed: false 399 | Height: 2600 400 | Hide Left Dock: false 401 | Hide Right Dock: false 402 | QMainWindow State: 000000ff00000000fd0000000400000000000001f40000091efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000700000091e0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000593000001730000002800fffffffb0000000c00430061006d006500720061010000027a000000c20000000000000000fb0000000c00430061006d006500720061010000041b0000023c0000000000000000fb0000000c00430061006d006500720061010000056e000000e90000000000000000fb0000000c00430061006d0065007200610100000366000001230000000000000000000000010000015d0000091efc0200000006fb0000000a0056006900650077007301000000700000091e0000013800fffffffb0000000c00430061006d00650072006101000003eb0000026c0000000000000000fb0000000a0049006d006100670065000000022c000000f90000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000c00430061006d00650072006101000001c10000017b0000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000014000000005efc0100000002fb0000000800540069006d00650100000000000014000000047a00fffffffb0000000800540069006d00650100000000000004500000000000000000000010970000091e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 403 | Selection: 404 | collapsed: false 405 | Time: 406 | collapsed: false 407 | Tool Properties: 408 | collapsed: false 409 | Views: 410 | collapsed: false 411 | Width: 5120 412 | X: 0 413 | Y: 64 414 | -------------------------------------------------------------------------------- /bme_ros2_navigation/rviz/mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 158 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | - /Imu1/Axes properties1 10 | - /Imu1/Acceleration properties1 11 | Splitter Ratio: 0.5 12 | Tree Height: 1201 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | - Class: slam_toolbox::SlamToolboxPlugin 32 | Name: SlamToolboxPlugin 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz_default_plugins/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz_default_plugins/RobotModel 56 | Collision Enabled: false 57 | Description File: "" 58 | Description Source: Topic 59 | Description Topic: 60 | Depth: 5 61 | Durability Policy: Volatile 62 | History Policy: Keep Last 63 | Reliability Policy: Reliable 64 | Value: /robot_description 65 | Enabled: true 66 | Links: 67 | All Links Enabled: true 68 | Expand Joint Details: false 69 | Expand Link Details: false 70 | Expand Tree: false 71 | Link Tree Style: Links in Alphabetic Order 72 | base_footprint: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | base_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | camera_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | camera_link_optical: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | imu_link: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | left_wheel: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | right_wheel: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | scan_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | Mass Properties: 110 | Inertia: false 111 | Mass: false 112 | Name: RobotModel 113 | TF Prefix: "" 114 | Update Interval: 0 115 | Value: true 116 | Visual Enabled: true 117 | - Class: rviz_default_plugins/TF 118 | Enabled: true 119 | Filter (blacklist): "" 120 | Filter (whitelist): "" 121 | Frame Timeout: 15 122 | Frames: 123 | All Enabled: false 124 | base_footprint: 125 | Value: false 126 | base_link: 127 | Value: false 128 | camera_link: 129 | Value: false 130 | camera_link_optical: 131 | Value: false 132 | imu_link: 133 | Value: false 134 | left_wheel: 135 | Value: false 136 | map: 137 | Value: true 138 | odom: 139 | Value: true 140 | right_wheel: 141 | Value: false 142 | scan_link: 143 | Value: false 144 | Marker Scale: 1 145 | Name: TF 146 | Show Arrows: true 147 | Show Axes: true 148 | Show Names: true 149 | Tree: 150 | map: 151 | odom: 152 | base_footprint: 153 | base_link: 154 | camera_link: 155 | camera_link_optical: 156 | {} 157 | imu_link: 158 | {} 159 | left_wheel: 160 | {} 161 | right_wheel: 162 | {} 163 | scan_link: 164 | {} 165 | Update Interval: 0 166 | Value: true 167 | - Alpha: 1 168 | Buffer Length: 1 169 | Class: rviz_default_plugins/Path 170 | Color: 0; 0; 255 171 | Enabled: true 172 | Head Diameter: 0.30000001192092896 173 | Head Length: 0.20000000298023224 174 | Length: 0.30000001192092896 175 | Line Style: Lines 176 | Line Width: 0.029999999329447746 177 | Name: Odometry filtered 178 | Offset: 179 | X: 0 180 | Y: 0 181 | Z: 0 182 | Pose Color: 255; 85; 255 183 | Pose Style: None 184 | Radius: 0.029999999329447746 185 | Shaft Diameter: 0.10000000149011612 186 | Shaft Length: 0.10000000149011612 187 | Topic: 188 | Depth: 5 189 | Durability Policy: Volatile 190 | Filter size: 10 191 | History Policy: Keep Last 192 | Reliability Policy: Reliable 193 | Value: /trajectory 194 | Value: true 195 | - Class: rviz_default_plugins/Camera 196 | Enabled: false 197 | Far Plane Distance: 100 198 | Image Rendering: background and overlay 199 | Name: Camera 200 | Overlay Alpha: 0.5 201 | Topic: 202 | Depth: 5 203 | Durability Policy: Volatile 204 | History Policy: Keep Last 205 | Reliability Policy: Reliable 206 | Value: /camera/image/compressed 207 | Value: false 208 | Visibility: 209 | Grid: true 210 | Imu: true 211 | InteractiveMarkers: true 212 | LaserScan: true 213 | Map: true 214 | Odometry filtered: true 215 | RobotModel: true 216 | TF: true 217 | Value: true 218 | Zoom Factor: 1 219 | - Alpha: 1 220 | Autocompute Intensity Bounds: true 221 | Autocompute Value Bounds: 222 | Max Value: 10 223 | Min Value: -10 224 | Value: true 225 | Axis: Z 226 | Channel Name: intensity 227 | Class: rviz_default_plugins/LaserScan 228 | Color: 255; 255; 255 229 | Color Transformer: Intensity 230 | Decay Time: 0 231 | Enabled: true 232 | Invert Rainbow: false 233 | Max Color: 255; 255; 255 234 | Max Intensity: 1 235 | Min Color: 0; 0; 0 236 | Min Intensity: 0 237 | Name: LaserScan 238 | Position Transformer: XYZ 239 | Selectable: true 240 | Size (Pixels): 3 241 | Size (m): 0.009999999776482582 242 | Style: Flat Squares 243 | Topic: 244 | Depth: 5 245 | Durability Policy: Volatile 246 | Filter size: 10 247 | History Policy: Keep Last 248 | Reliability Policy: Reliable 249 | Value: /scan 250 | Use Fixed Frame: true 251 | Use rainbow: true 252 | Value: true 253 | - Acceleration properties: 254 | Acc. vector alpha: 0.30000001192092896 255 | Acc. vector color: 0; 0; 255 256 | Acc. vector scale: 0.05000000074505806 257 | Derotate acceleration: false 258 | Enable acceleration: true 259 | Axes properties: 260 | Axes scale: 0.30000001192092896 261 | Enable axes: true 262 | Box properties: 263 | Box alpha: 1 264 | Box color: 255; 0; 0 265 | Enable box: false 266 | x_scale: 1 267 | y_scale: 1 268 | z_scale: 1 269 | Class: rviz_imu_plugin/Imu 270 | Enabled: true 271 | Name: Imu 272 | Topic: 273 | Depth: 5 274 | Durability Policy: Volatile 275 | Filter size: 10 276 | History Policy: Keep Last 277 | Reliability Policy: Reliable 278 | Value: /imu 279 | Value: true 280 | fixed_frame_orientation: true 281 | - Class: rviz_default_plugins/InteractiveMarkers 282 | Enable Transparency: true 283 | Enabled: true 284 | Interactive Markers Namespace: /twist_server 285 | Name: InteractiveMarkers 286 | Show Axes: false 287 | Show Descriptions: false 288 | Show Visual Aids: false 289 | Value: true 290 | - Alpha: 0.699999988079071 291 | Binary representation: false 292 | Binary threshold: 100 293 | Class: rviz_default_plugins/Map 294 | Color Scheme: map 295 | Draw Behind: false 296 | Enabled: true 297 | Name: Map 298 | Topic: 299 | Depth: 5 300 | Durability Policy: Volatile 301 | Filter size: 10 302 | History Policy: Keep Last 303 | Reliability Policy: Reliable 304 | Value: /map 305 | Update Topic: 306 | Depth: 5 307 | Durability Policy: Volatile 308 | History Policy: Keep Last 309 | Reliability Policy: Reliable 310 | Value: /map_updates 311 | Use Timestamp: false 312 | Value: true 313 | Enabled: true 314 | Global Options: 315 | Background Color: 48; 48; 48 316 | Fixed Frame: map 317 | Frame Rate: 30 318 | Name: root 319 | Tools: 320 | - Class: rviz_default_plugins/Interact 321 | Hide Inactive Objects: true 322 | - Class: rviz_default_plugins/MoveCamera 323 | - Class: rviz_default_plugins/Select 324 | - Class: rviz_default_plugins/FocusCamera 325 | - Class: rviz_default_plugins/Measure 326 | Line color: 128; 128; 0 327 | - Class: rviz_default_plugins/SetInitialPose 328 | Covariance x: 0.25 329 | Covariance y: 0.25 330 | Covariance yaw: 0.06853891909122467 331 | Topic: 332 | Depth: 5 333 | Durability Policy: Volatile 334 | History Policy: Keep Last 335 | Reliability Policy: Reliable 336 | Value: /initialpose 337 | - Class: rviz_default_plugins/SetGoal 338 | Topic: 339 | Depth: 5 340 | Durability Policy: Volatile 341 | History Policy: Keep Last 342 | Reliability Policy: Reliable 343 | Value: /goal_pose 344 | - Class: rviz_default_plugins/PublishPoint 345 | Single click: true 346 | Topic: 347 | Depth: 5 348 | Durability Policy: Volatile 349 | History Policy: Keep Last 350 | Reliability Policy: Reliable 351 | Value: /clicked_point 352 | Transformation: 353 | Current: 354 | Class: rviz_default_plugins/TF 355 | Value: true 356 | Views: 357 | Current: 358 | Class: rviz_default_plugins/Orbit 359 | Distance: 13.755589485168457 360 | Enable Stereo Rendering: 361 | Stereo Eye Separation: 0.05999999865889549 362 | Stereo Focal Distance: 1 363 | Swap Stereo Eyes: false 364 | Value: false 365 | Focal Point: 366 | X: 2.2494306564331055 367 | Y: -0.527195394039154 368 | Z: 0.06637868285179138 369 | Focal Shape Fixed Size: true 370 | Focal Shape Size: 0.05000000074505806 371 | Invert Z Axis: false 372 | Name: Current View 373 | Near Clip Distance: 0.009999999776482582 374 | Pitch: 0.9697970151901245 375 | Target Frame: 376 | Value: Orbit (rviz) 377 | Yaw: 2.7053985595703125 378 | Saved: ~ 379 | Window Geometry: 380 | Camera: 381 | collapsed: false 382 | Displays: 383 | collapsed: false 384 | Height: 2600 385 | Hide Left Dock: false 386 | Hide Right Dock: false 387 | QMainWindow State: 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 388 | Selection: 389 | collapsed: false 390 | SlamToolboxPlugin: 391 | collapsed: false 392 | Time: 393 | collapsed: false 394 | Tool Properties: 395 | collapsed: false 396 | Views: 397 | collapsed: false 398 | Width: 5120 399 | X: 0 400 | Y: 64 401 | -------------------------------------------------------------------------------- /bme_ros2_navigation/rviz/navigation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /Imu1/Axes properties1 9 | - /Imu1/Acceleration properties1 10 | - /Map1/Topic1 11 | - /Local costmap1/Topic1 12 | - /Local costmap1/Update Topic1 13 | Splitter Ratio: 0.5298672318458557 14 | Tree Height: 964 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /2D Goal Pose1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz_common/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: LaserScan 33 | - Class: nav2_rviz_plugins/Navigation 2 34 | Name: Navigation 2 35 | - Class: nav2_rviz_plugins/Selector 36 | Name: Selector 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz_default_plugins/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Class: rviz_default_plugins/RobotModel 60 | Collision Enabled: false 61 | Description File: "" 62 | Description Source: Topic 63 | Description Topic: 64 | Depth: 5 65 | Durability Policy: Volatile 66 | History Policy: Keep Last 67 | Reliability Policy: Reliable 68 | Value: /robot_description 69 | Enabled: true 70 | Links: 71 | All Links Enabled: true 72 | Expand Joint Details: false 73 | Expand Link Details: false 74 | Expand Tree: false 75 | Link Tree Style: Links in Alphabetic Order 76 | base_footprint: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | base_link: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | camera_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | camera_link_optical: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | imu_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | left_wheel: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | right_wheel: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | scan_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | Mass Properties: 114 | Inertia: false 115 | Mass: false 116 | Name: RobotModel 117 | TF Prefix: "" 118 | Update Interval: 0 119 | Value: true 120 | Visual Enabled: true 121 | - Class: rviz_default_plugins/TF 122 | Enabled: true 123 | Filter (blacklist): "" 124 | Filter (whitelist): "" 125 | Frame Timeout: 15 126 | Frames: 127 | All Enabled: false 128 | base_footprint: 129 | Value: false 130 | base_link: 131 | Value: false 132 | camera_link: 133 | Value: false 134 | camera_link_optical: 135 | Value: false 136 | imu_link: 137 | Value: false 138 | left_wheel: 139 | Value: false 140 | map: 141 | Value: true 142 | odom: 143 | Value: true 144 | right_wheel: 145 | Value: false 146 | scan_link: 147 | Value: false 148 | Marker Scale: 1 149 | Name: TF 150 | Show Arrows: true 151 | Show Axes: true 152 | Show Names: true 153 | Tree: 154 | map: 155 | odom: 156 | base_footprint: 157 | base_link: 158 | camera_link: 159 | camera_link_optical: 160 | {} 161 | imu_link: 162 | {} 163 | left_wheel: 164 | {} 165 | right_wheel: 166 | {} 167 | scan_link: 168 | {} 169 | Update Interval: 0 170 | Value: true 171 | - Alpha: 1 172 | Buffer Length: 1 173 | Class: rviz_default_plugins/Path 174 | Color: 0; 0; 255 175 | Enabled: true 176 | Head Diameter: 0.30000001192092896 177 | Head Length: 0.20000000298023224 178 | Length: 0.30000001192092896 179 | Line Style: Lines 180 | Line Width: 0.029999999329447746 181 | Name: Odometry filtered 182 | Offset: 183 | X: 0 184 | Y: 0 185 | Z: 0 186 | Pose Color: 255; 85; 255 187 | Pose Style: None 188 | Radius: 0.029999999329447746 189 | Shaft Diameter: 0.10000000149011612 190 | Shaft Length: 0.10000000149011612 191 | Topic: 192 | Depth: 5 193 | Durability Policy: Volatile 194 | Filter size: 10 195 | History Policy: Keep Last 196 | Reliability Policy: Reliable 197 | Value: /trajectory 198 | Value: true 199 | - Alpha: 1 200 | Autocompute Intensity Bounds: true 201 | Autocompute Value Bounds: 202 | Max Value: 10 203 | Min Value: -10 204 | Value: true 205 | Axis: Z 206 | Channel Name: intensity 207 | Class: rviz_default_plugins/LaserScan 208 | Color: 255; 255; 255 209 | Color Transformer: Intensity 210 | Decay Time: 0 211 | Enabled: true 212 | Invert Rainbow: false 213 | Max Color: 255; 255; 255 214 | Max Intensity: 1 215 | Min Color: 0; 0; 0 216 | Min Intensity: 0 217 | Name: LaserScan 218 | Position Transformer: XYZ 219 | Selectable: true 220 | Size (Pixels): 3 221 | Size (m): 0.009999999776482582 222 | Style: Flat Squares 223 | Topic: 224 | Depth: 5 225 | Durability Policy: Volatile 226 | Filter size: 10 227 | History Policy: Keep Last 228 | Reliability Policy: Best Effort 229 | Value: /scan 230 | Use Fixed Frame: true 231 | Use rainbow: true 232 | Value: true 233 | - Acceleration properties: 234 | Acc. vector alpha: 0.30000001192092896 235 | Acc. vector color: 0; 0; 255 236 | Acc. vector scale: 0.05000000074505806 237 | Derotate acceleration: false 238 | Enable acceleration: true 239 | Axes properties: 240 | Axes scale: 0.30000001192092896 241 | Enable axes: true 242 | Box properties: 243 | Box alpha: 1 244 | Box color: 255; 0; 0 245 | Enable box: false 246 | x_scale: 1 247 | y_scale: 1 248 | z_scale: 1 249 | Class: rviz_imu_plugin/Imu 250 | Enabled: true 251 | Name: Imu 252 | Topic: 253 | Depth: 5 254 | Durability Policy: Volatile 255 | Filter size: 10 256 | History Policy: Keep Last 257 | Reliability Policy: Reliable 258 | Value: /imu 259 | Value: true 260 | fixed_frame_orientation: true 261 | - Class: rviz_default_plugins/Camera 262 | Enabled: false 263 | Far Plane Distance: 100 264 | Image Rendering: background and overlay 265 | Name: Camera 266 | Overlay Alpha: 0.5 267 | Topic: 268 | Depth: 5 269 | Durability Policy: Volatile 270 | History Policy: Keep Last 271 | Reliability Policy: Reliable 272 | Value: /camera/image/compressed 273 | Value: false 274 | Visibility: 275 | AMCL ParticleCloud: true 276 | DWB total cost cloud: true 277 | Frontiers: true 278 | Global costmap: true 279 | Global plan: true 280 | Goal Pose: true 281 | Grid: true 282 | Imu: true 283 | LaserScan: true 284 | Local costmap: true 285 | Map: true 286 | Odometry filtered: true 287 | Robot footprint: true 288 | RobotModel: true 289 | TF: true 290 | Value: true 291 | Waypoints: true 292 | Zoom Factor: 1 293 | - Alpha: 0.699999988079071 294 | Binary representation: false 295 | Binary threshold: 100 296 | Class: rviz_default_plugins/Map 297 | Color Scheme: map 298 | Draw Behind: true 299 | Enabled: true 300 | Name: Map 301 | Topic: 302 | Depth: 5 303 | Durability Policy: Transient Local 304 | Filter size: 10 305 | History Policy: Keep Last 306 | Reliability Policy: Reliable 307 | Value: /map 308 | Update Topic: 309 | Depth: 5 310 | Durability Policy: Volatile 311 | History Policy: Keep Last 312 | Reliability Policy: Reliable 313 | Value: /map_updates 314 | Use Timestamp: false 315 | Value: true 316 | - Alpha: 1 317 | Class: nav2_rviz_plugins/ParticleCloud 318 | Color: 16; 127; 6 319 | Enabled: true 320 | Max Arrow Length: 0.30000001192092896 321 | Min Arrow Length: 0.10000000149011612 322 | Name: AMCL ParticleCloud 323 | Shape: Arrow (Flat) 324 | Topic: 325 | Depth: 5 326 | Durability Policy: Volatile 327 | Filter size: 10 328 | History Policy: Keep Last 329 | Reliability Policy: Best Effort 330 | Value: /particle_cloud 331 | Value: true 332 | - Alpha: 0.699999988079071 333 | Binary representation: false 334 | Binary threshold: 100 335 | Class: rviz_default_plugins/Map 336 | Color Scheme: map 337 | Draw Behind: true 338 | Enabled: true 339 | Name: Global costmap 340 | Topic: 341 | Depth: 5 342 | Durability Policy: Volatile 343 | Filter size: 10 344 | History Policy: Keep Last 345 | Reliability Policy: Reliable 346 | Value: /global_costmap/costmap 347 | Update Topic: 348 | Depth: 5 349 | Durability Policy: Volatile 350 | History Policy: Keep Last 351 | Reliability Policy: Reliable 352 | Value: /global_costmap/costmap_updates 353 | Use Timestamp: false 354 | Value: true 355 | - Alpha: 0.699999988079071 356 | Binary representation: false 357 | Binary threshold: 100 358 | Class: rviz_default_plugins/Map 359 | Color Scheme: costmap 360 | Draw Behind: false 361 | Enabled: true 362 | Name: Local costmap 363 | Topic: 364 | Depth: 5 365 | Durability Policy: Transient Local 366 | Filter size: 10 367 | History Policy: Keep Last 368 | Reliability Policy: Reliable 369 | Value: /local_costmap/costmap 370 | Update Topic: 371 | Depth: 5 372 | Durability Policy: Volatile 373 | History Policy: Keep Last 374 | Reliability Policy: Reliable 375 | Value: /local_costmap/costmap_updates 376 | Use Timestamp: false 377 | Value: true 378 | - Alpha: 1 379 | Buffer Length: 1 380 | Class: rviz_default_plugins/Path 381 | Color: 255; 25; 0 382 | Enabled: true 383 | Head Diameter: 0.30000001192092896 384 | Head Length: 0.20000000298023224 385 | Length: 0.30000001192092896 386 | Line Style: Lines 387 | Line Width: 0.029999999329447746 388 | Name: Global plan 389 | Offset: 390 | X: 0 391 | Y: 0 392 | Z: 0 393 | Pose Color: 255; 85; 255 394 | Pose Style: None 395 | Radius: 0.029999999329447746 396 | Shaft Diameter: 0.10000000149011612 397 | Shaft Length: 0.10000000149011612 398 | Topic: 399 | Depth: 5 400 | Durability Policy: Volatile 401 | Filter size: 10 402 | History Policy: Keep Last 403 | Reliability Policy: Reliable 404 | Value: /plan 405 | Value: true 406 | - Alpha: 1 407 | Axes Length: 1 408 | Axes Radius: 0.10000000149011612 409 | Class: rviz_default_plugins/Pose 410 | Color: 255; 25; 0 411 | Enabled: true 412 | Head Length: 0.10000000149011612 413 | Head Radius: 0.10000000149011612 414 | Name: Goal Pose 415 | Shaft Length: 0.20000000298023224 416 | Shaft Radius: 0.05000000074505806 417 | Shape: Arrow 418 | Topic: 419 | Depth: 5 420 | Durability Policy: Volatile 421 | Filter size: 10 422 | History Policy: Keep Last 423 | Reliability Policy: Reliable 424 | Value: /goal_pose 425 | Value: true 426 | - Alpha: 0.5 427 | Autocompute Intensity Bounds: true 428 | Autocompute Value Bounds: 429 | Max Value: 10 430 | Min Value: -10 431 | Value: true 432 | Axis: Z 433 | Channel Name: total_cost 434 | Class: rviz_default_plugins/PointCloud2 435 | Color: 255; 255; 255 436 | Color Transformer: Intensity 437 | Decay Time: 0 438 | Enabled: true 439 | Invert Rainbow: false 440 | Max Color: 255; 255; 255 441 | Max Intensity: 184.27999877929688 442 | Min Color: 0; 0; 0 443 | Min Intensity: 1.2000000476837158 444 | Name: DWB total cost cloud 445 | Position Transformer: XYZ 446 | Selectable: true 447 | Size (Pixels): 3 448 | Size (m): 0.03999999910593033 449 | Style: Tiles 450 | Topic: 451 | Depth: 5 452 | Durability Policy: Volatile 453 | History Policy: Keep Last 454 | Reliability Policy: Reliable 455 | Value: /cost_cloud 456 | Use Fixed Frame: true 457 | Use rainbow: true 458 | Value: true 459 | - Alpha: 1 460 | Class: rviz_default_plugins/Polygon 461 | Color: 25; 255; 0 462 | Enabled: true 463 | Name: Robot footprint 464 | Topic: 465 | Depth: 5 466 | Durability Policy: Volatile 467 | Filter size: 10 468 | History Policy: Keep Last 469 | Reliability Policy: Reliable 470 | Value: /local_costmap/published_footprint 471 | Value: true 472 | - Class: rviz_default_plugins/MarkerArray 473 | Enabled: true 474 | Name: Waypoints 475 | Namespaces: 476 | "": true 477 | Topic: 478 | Depth: 5 479 | Durability Policy: Volatile 480 | History Policy: Keep Last 481 | Reliability Policy: Reliable 482 | Value: /waypoints 483 | Value: true 484 | - Class: rviz_default_plugins/MarkerArray 485 | Enabled: true 486 | Name: Frontiers 487 | Namespaces: 488 | {} 489 | Topic: 490 | Depth: 5 491 | Durability Policy: Volatile 492 | History Policy: Keep Last 493 | Reliability Policy: Reliable 494 | Value: /explore/frontiers 495 | Value: true 496 | Enabled: true 497 | Global Options: 498 | Background Color: 48; 48; 48 499 | Fixed Frame: map 500 | Frame Rate: 30 501 | Name: root 502 | Tools: 503 | - Class: rviz_default_plugins/Interact 504 | Hide Inactive Objects: true 505 | - Class: rviz_default_plugins/MoveCamera 506 | - Class: rviz_default_plugins/Select 507 | - Class: rviz_default_plugins/FocusCamera 508 | - Class: rviz_default_plugins/Measure 509 | Line color: 128; 128; 0 510 | - Class: rviz_default_plugins/SetInitialPose 511 | Covariance x: 0.25 512 | Covariance y: 0.25 513 | Covariance yaw: 0.06853891909122467 514 | Topic: 515 | Depth: 5 516 | Durability Policy: Volatile 517 | History Policy: Keep Last 518 | Reliability Policy: Reliable 519 | Value: /initialpose 520 | - Class: rviz_default_plugins/SetGoal 521 | Topic: 522 | Depth: 5 523 | Durability Policy: Volatile 524 | History Policy: Keep Last 525 | Reliability Policy: Reliable 526 | Value: /goal_pose 527 | - Class: rviz_default_plugins/PublishPoint 528 | Single click: true 529 | Topic: 530 | Depth: 5 531 | Durability Policy: Volatile 532 | History Policy: Keep Last 533 | Reliability Policy: Reliable 534 | Value: /clicked_point 535 | - Class: nav2_rviz_plugins/GoalTool 536 | Transformation: 537 | Current: 538 | Class: rviz_default_plugins/TF 539 | Value: true 540 | Views: 541 | Current: 542 | Class: rviz_default_plugins/Orbit 543 | Distance: 12.234578132629395 544 | Enable Stereo Rendering: 545 | Stereo Eye Separation: 0.05999999865889549 546 | Stereo Focal Distance: 1 547 | Swap Stereo Eyes: false 548 | Value: false 549 | Focal Point: 550 | X: 1.020715594291687 551 | Y: -6.16179895401001 552 | Z: 1.6954073905944824 553 | Focal Shape Fixed Size: true 554 | Focal Shape Size: 0.05000000074505806 555 | Invert Z Axis: false 556 | Name: Current View 557 | Near Clip Distance: 0.009999999776482582 558 | Pitch: 1.139797568321228 559 | Target Frame: 560 | Value: Orbit (rviz) 561 | Yaw: 3.120405673980713 562 | Saved: ~ 563 | Window Geometry: 564 | Camera: 565 | collapsed: false 566 | Displays: 567 | collapsed: false 568 | Height: 2600 569 | Hide Left Dock: false 570 | Hide Right Dock: false 571 | Navigation 2: 572 | collapsed: false 573 | QMainWindow State: 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 574 | Selection: 575 | collapsed: false 576 | Selector: 577 | collapsed: false 578 | Time: 579 | collapsed: false 580 | Tool Properties: 581 | collapsed: false 582 | Views: 583 | collapsed: false 584 | Width: 5120 585 | X: 0 586 | Y: 64 587 | -------------------------------------------------------------------------------- /bme_ros2_navigation/rviz/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 158 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Imu1/Axes properties1 10 | - /Imu1/Acceleration properties1 11 | Splitter Ratio: 0.5 12 | Tree Height: 884 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz_default_plugins/RobotModel 54 | Collision Enabled: false 55 | Description File: "" 56 | Description Source: Topic 57 | Description Topic: 58 | Depth: 5 59 | Durability Policy: Volatile 60 | History Policy: Keep Last 61 | Reliability Policy: Reliable 62 | Value: /robot_description 63 | Enabled: true 64 | Links: 65 | All Links Enabled: true 66 | Expand Joint Details: false 67 | Expand Link Details: false 68 | Expand Tree: false 69 | Link Tree Style: Links in Alphabetic Order 70 | base_footprint: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | base_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | camera_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | camera_link_optical: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | imu_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | left_wheel: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | right_wheel: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | scan_link: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | Mass Properties: 108 | Inertia: false 109 | Mass: false 110 | Name: RobotModel 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Alpha: 1 116 | Buffer Length: 1 117 | Class: rviz_default_plugins/Path 118 | Color: 246; 211; 45 119 | Enabled: true 120 | Head Diameter: 0.30000001192092896 121 | Head Length: 0.20000000298023224 122 | Length: 0.30000001192092896 123 | Line Style: Lines 124 | Line Width: 0.029999999329447746 125 | Name: Odometry filtered 126 | Offset: 127 | X: 0 128 | Y: 0 129 | Z: 0 130 | Pose Color: 255; 85; 255 131 | Pose Style: None 132 | Radius: 0.029999999329447746 133 | Shaft Diameter: 0.10000000149011612 134 | Shaft Length: 0.10000000149011612 135 | Topic: 136 | Depth: 5 137 | Durability Policy: Volatile 138 | Filter size: 10 139 | History Policy: Keep Last 140 | Reliability Policy: Reliable 141 | Value: /trajectory 142 | Value: true 143 | - Class: rviz_default_plugins/Camera 144 | Enabled: false 145 | Far Plane Distance: 100 146 | Image Rendering: background and overlay 147 | Name: Camera 148 | Overlay Alpha: 0.5 149 | Topic: 150 | Depth: 5 151 | Durability Policy: Volatile 152 | History Policy: Keep Last 153 | Reliability Policy: Reliable 154 | Value: /camera/image/compressed 155 | Value: false 156 | Visibility: 157 | Grid: true 158 | Imu: true 159 | InteractiveMarkers: true 160 | LaserScan: true 161 | Odometry filtered: true 162 | RobotModel: true 163 | Value: true 164 | Zoom Factor: 1 165 | - Alpha: 1 166 | Autocompute Intensity Bounds: true 167 | Autocompute Value Bounds: 168 | Max Value: 10 169 | Min Value: -10 170 | Value: true 171 | Axis: Z 172 | Channel Name: intensity 173 | Class: rviz_default_plugins/LaserScan 174 | Color: 255; 255; 255 175 | Color Transformer: Intensity 176 | Decay Time: 0 177 | Enabled: true 178 | Invert Rainbow: false 179 | Max Color: 255; 255; 255 180 | Max Intensity: 1 181 | Min Color: 0; 0; 0 182 | Min Intensity: 0 183 | Name: LaserScan 184 | Position Transformer: XYZ 185 | Selectable: true 186 | Size (Pixels): 3 187 | Size (m): 0.009999999776482582 188 | Style: Flat Squares 189 | Topic: 190 | Depth: 5 191 | Durability Policy: Volatile 192 | Filter size: 10 193 | History Policy: Keep Last 194 | Reliability Policy: Reliable 195 | Value: /scan 196 | Use Fixed Frame: true 197 | Use rainbow: true 198 | Value: true 199 | - Acceleration properties: 200 | Acc. vector alpha: 0.30000001192092896 201 | Acc. vector color: 0; 0; 255 202 | Acc. vector scale: 0.05000000074505806 203 | Derotate acceleration: false 204 | Enable acceleration: true 205 | Axes properties: 206 | Axes scale: 0.30000001192092896 207 | Enable axes: true 208 | Box properties: 209 | Box alpha: 1 210 | Box color: 255; 0; 0 211 | Enable box: false 212 | x_scale: 1 213 | y_scale: 1 214 | z_scale: 1 215 | Class: rviz_imu_plugin/Imu 216 | Enabled: true 217 | Name: Imu 218 | Topic: 219 | Depth: 5 220 | Durability Policy: Volatile 221 | Filter size: 10 222 | History Policy: Keep Last 223 | Reliability Policy: Reliable 224 | Value: /imu 225 | Value: true 226 | fixed_frame_orientation: true 227 | - Class: rviz_default_plugins/InteractiveMarkers 228 | Enable Transparency: true 229 | Enabled: true 230 | Interactive Markers Namespace: /twist_server 231 | Name: InteractiveMarkers 232 | Show Axes: false 233 | Show Descriptions: false 234 | Show Visual Aids: false 235 | Value: true 236 | Enabled: true 237 | Global Options: 238 | Background Color: 48; 48; 48 239 | Fixed Frame: odom 240 | Frame Rate: 30 241 | Name: root 242 | Tools: 243 | - Class: rviz_default_plugins/Interact 244 | Hide Inactive Objects: true 245 | - Class: rviz_default_plugins/MoveCamera 246 | - Class: rviz_default_plugins/Select 247 | - Class: rviz_default_plugins/FocusCamera 248 | - Class: rviz_default_plugins/Measure 249 | Line color: 128; 128; 0 250 | - Class: rviz_default_plugins/SetInitialPose 251 | Covariance x: 0.25 252 | Covariance y: 0.25 253 | Covariance yaw: 0.06853891909122467 254 | Topic: 255 | Depth: 5 256 | Durability Policy: Volatile 257 | History Policy: Keep Last 258 | Reliability Policy: Reliable 259 | Value: /initialpose 260 | - Class: rviz_default_plugins/SetGoal 261 | Topic: 262 | Depth: 5 263 | Durability Policy: Volatile 264 | History Policy: Keep Last 265 | Reliability Policy: Reliable 266 | Value: /goal_pose 267 | - Class: rviz_default_plugins/PublishPoint 268 | Single click: true 269 | Topic: 270 | Depth: 5 271 | Durability Policy: Volatile 272 | History Policy: Keep Last 273 | Reliability Policy: Reliable 274 | Value: /clicked_point 275 | Transformation: 276 | Current: 277 | Class: rviz_default_plugins/TF 278 | Value: true 279 | Views: 280 | Current: 281 | Class: rviz_default_plugins/Orbit 282 | Distance: 8.6182861328125 283 | Enable Stereo Rendering: 284 | Stereo Eye Separation: 0.05999999865889549 285 | Stereo Focal Distance: 1 286 | Swap Stereo Eyes: false 287 | Value: false 288 | Focal Point: 289 | X: 2.59187388420105 290 | Y: -0.880598247051239 291 | Z: 1.0602256059646606 292 | Focal Shape Fixed Size: true 293 | Focal Shape Size: 0.05000000074505806 294 | Invert Z Axis: false 295 | Name: Current View 296 | Near Clip Distance: 0.009999999776482582 297 | Pitch: 0.5903983116149902 298 | Target Frame: 299 | Value: Orbit (rviz) 300 | Yaw: 2.3753981590270996 301 | Saved: ~ 302 | Window Geometry: 303 | Camera: 304 | collapsed: false 305 | Displays: 306 | collapsed: false 307 | Height: 1300 308 | Hide Left Dock: false 309 | Hide Right Dock: false 310 | QMainWindow State: 000000ff00000000fd0000000400000000000001f400000452fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000452000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000593000001730000001700fffffffb0000000c00430061006d006500720061010000027a000000c20000000000000000fb0000000c00430061006d006500720061010000041b0000023c0000000000000000fb0000000c00430061006d006500720061010000056e000000e90000000000000000fb0000000c00430061006d0065007200610100000366000001230000000000000000000000010000015d00000452fc0200000006fb0000000a00560069006500770073010000003f00000452000000a900fffffffb0000000c00430061006d00650072006101000003eb0000026c0000000000000000fb0000000a0049006d006100670065000000022c000000f90000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000c00430061006d00650072006101000001c10000017b0000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000005efc0100000002fb0000000800540069006d0065010000000000000a000000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000006a30000045200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 311 | Selection: 312 | collapsed: false 313 | Time: 314 | collapsed: false 315 | Tool Properties: 316 | collapsed: false 317 | Views: 318 | collapsed: false 319 | Width: 2560 320 | X: 0 321 | Y: 32 322 | -------------------------------------------------------------------------------- /bme_ros2_navigation/rviz/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 289 9 | - Class: rviz_common/Views 10 | Expanded: 11 | - /Current View1 12 | Name: Views 13 | Splitter Ratio: 0.5 14 | Visualization Manager: 15 | Class: "" 16 | Displays: 17 | - Alpha: 0.5 18 | Cell Size: 1 19 | Class: rviz_default_plugins/Grid 20 | Color: 160; 160; 164 21 | Enabled: true 22 | Line Style: 23 | Line Width: 0.029999999329447746 24 | Value: Lines 25 | Name: Grid 26 | Normal Cell Count: 0 27 | Offset: 28 | X: 0 29 | Y: 0 30 | Z: 0 31 | Plane: XY 32 | Plane Cell Count: 10 33 | Reference Frame: 34 | Value: true 35 | - Alpha: 0.800000011920929 36 | Class: rviz_default_plugins/RobotModel 37 | Collision Enabled: false 38 | Description File: "" 39 | Description Source: Topic 40 | Description Topic: 41 | Depth: 5 42 | Durability Policy: Volatile 43 | History Policy: Keep Last 44 | Reliability Policy: Reliable 45 | Value: /robot_description 46 | Enabled: true 47 | Links: 48 | All Links Enabled: true 49 | Expand Joint Details: false 50 | Expand Link Details: false 51 | Expand Tree: false 52 | Link Tree Style: Links in Alphabetic Order 53 | base_footprint: 54 | Alpha: 1 55 | Show Axes: false 56 | Show Trail: false 57 | base_link: 58 | Alpha: 1 59 | Show Axes: false 60 | Show Trail: false 61 | Value: true 62 | left_wheel: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | right_wheel: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | Mass Properties: 73 | Inertia: false 74 | Mass: false 75 | Name: RobotModel 76 | TF Prefix: "" 77 | Update Interval: 0 78 | Value: true 79 | Visual Enabled: true 80 | - Class: rviz_default_plugins/TF 81 | Enabled: true 82 | Filter (blacklist): "" 83 | Filter (whitelist): "" 84 | Frame Timeout: 15 85 | Frames: 86 | All Enabled: true 87 | base_footprint: 88 | Value: true 89 | base_link: 90 | Value: true 91 | left_wheel: 92 | Value: true 93 | right_wheel: 94 | Value: true 95 | Marker Scale: 1 96 | Name: TF 97 | Show Arrows: true 98 | Show Axes: true 99 | Show Names: false 100 | Tree: 101 | base_footprint: 102 | base_link: 103 | left_wheel: 104 | {} 105 | right_wheel: 106 | {} 107 | Update Interval: 0 108 | Value: true 109 | Enabled: true 110 | Global Options: 111 | Background Color: 48; 48; 48 112 | Fixed Frame: base_link 113 | Frame Rate: 30 114 | Name: root 115 | Tools: 116 | - Class: rviz_default_plugins/MoveCamera 117 | Transformation: 118 | Current: 119 | Class: rviz_default_plugins/TF 120 | Value: true 121 | Views: 122 | Current: 123 | Class: rviz_default_plugins/Orbit 124 | Distance: 1.7000000476837158 125 | Enable Stereo Rendering: 126 | Stereo Eye Separation: 0.05999999865889549 127 | Stereo Focal Distance: 1 128 | Swap Stereo Eyes: false 129 | Value: false 130 | Focal Point: 131 | X: 0 132 | Y: 0 133 | Z: 0 134 | Focal Shape Fixed Size: true 135 | Focal Shape Size: 0.05000000074505806 136 | Invert Z Axis: false 137 | Name: Current View 138 | Near Clip Distance: 0.009999999776482582 139 | Pitch: 0.33000001311302185 140 | Target Frame: 141 | Value: Orbit (rviz) 142 | Yaw: 5.5 143 | Saved: ~ 144 | Window Geometry: 145 | Displays: 146 | collapsed: false 147 | Height: 800 148 | Hide Left Dock: false 149 | Hide Right Dock: false 150 | QMainWindow State: 000000ff00000000fd0000000100000000000001c9000002c2fc0200000002fb000000100044006900730070006c006100790073010000003f000001a7000000cc00fffffffb0000000a0056006900650077007301000001ec00000115000000a900ffffff000002e1000002c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 151 | Views: 152 | collapsed: false 153 | Width: 1200 154 | X: 234 155 | Y: 260 156 | -------------------------------------------------------------------------------- /bme_ros2_navigation/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /bme_ros2_navigation/urdf/mogi_bot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | /cmd_vel 9 | 10 | 11 | left_wheel_joint 12 | right_wheel_joint 13 | 14 | 15 | 0.3 16 | 0.1 17 | 18 | 19 | 3.0 20 | 1 21 | -1 22 | 2 23 | -2 24 | 0.5 25 | -0.5 26 | 1 27 | -1 28 | 29 | 30 | odom 31 | tf 32 | odom 33 | base_footprint 34 | 30 35 | 36 | 37 | 38 | 41 | joint_states 42 | left_wheel_joint 43 | right_wheel_joint 44 | 45 | 46 | 47 | 48 | 49 | 50 | 1.3962634 51 | 52 | 640 53 | 480 54 | R8G8B8 55 | 56 | 57 | 0.1 58 | 15 59 | 60 | 61 | gaussian 62 | 65 | 0.0 66 | 0.007 67 | 68 | camera_link_optical 69 | camera/camera_info 70 | 71 | 1 72 | 20 73 | true 74 | camera/image 75 | 76 | 77 | 78 | 79 | 80 | 1 81 | 50 82 | true 83 | imu 84 | true 85 | imu_link 86 | 87 | 88 | 89 | 90 | 91 | 10 92 | scan 93 | scan_link 94 | 95 | 96 | 97 | 720 98 | 99 | 1 100 | -3.14156 101 | 3.14156 102 | 103 | 104 | 109 | 110 | 111 | 0.05 112 | 10.0 113 | 0.01 114 | 115 | 116 | gaussian 117 | 0.0 118 | 0.01 119 | 120 | scan_link 121 | 122 | 1 123 | true 124 | 125 | 126 | 127 | 128 | 129 | 130 | 1 131 | 1 132 | navsat 133 | base_link 134 | 135 | 136 | 137 | -------------------------------------------------------------------------------- /bme_ros2_navigation/urdf/mogi_bot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0 0 0.1 0 0 0 22 | 23 | 24 | 25 | 26 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 0.2 151 | 0.2 152 | 1000000.0 153 | 100.0 154 | 0.0001 155 | 1.0 156 | 157 | 158 | 159 | 0.2 160 | 0.2 161 | 1000000.0 162 | 100.0 163 | 0.0001 164 | 1.0 165 | 166 | 167 | 168 | 0.000002 169 | 0.000002 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 0 0 0 0 0 0 182 | 183 | 184 | 185 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | Gazebo/Red 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | false 268 | 269 | 270 | 271 | 272 | -------------------------------------------------------------------------------- /bme_ros2_navigation/worlds/empty.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.001 7 | 1.0 8 | 9 | 12 | 13 | 16 | 17 | 20 | 21 | 24 | 25 | 26 | 29 | ogre2 30 | 31 | 34 | 35 | 38 | 39 | 40 | 41 | EARTH_WGS84 42 | ENU 43 | 47.478950 44 | 19.057785 45 | 0 46 | 0 47 | 48 | 49 | 50 | true 51 | 0 0 10 0 0 0 52 | 0.8 0.8 0.8 1 53 | 0.2 0.2 0.2 1 54 | 55 | 1000 56 | 0.9 57 | 0.01 58 | 0.001 59 | 60 | -0.5 0.1 -0.9 61 | 62 | 63 | 0 0 -9.8000000000000007 64 | 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 65 | 66 | 67 | 0.400000006 0.400000006 0.400000006 1 68 | 0.699999988 0.699999988 0.699999988 1 69 | true 70 | 71 | 72 | 73 | true 74 | 75 | 76 | 77 | 78 | 0 0 1 79 | 100 100 80 | 81 | 82 | 83 | 84 | 85 | 50 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 0 0 1 96 | 100 100 97 | 98 | 99 | 100 | 0.8 0.8 0.8 1 101 | 0.8 0.8 0.8 1 102 | 0.8 0.8 0.8 1 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /bme_ros2_navigation_py/bme_ros2_navigation_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/bme_ros2_navigation_py/bme_ros2_navigation_py/__init__.py -------------------------------------------------------------------------------- /bme_ros2_navigation_py/bme_ros2_navigation_py/follow_waypoints.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from geometry_msgs.msg import PoseStamped 4 | from nav2_msgs.action import FollowWaypoints 5 | from rclpy.action import ActionClient 6 | from tf_transformations import quaternion_from_euler 7 | 8 | class WaypointFollower(Node): 9 | def __init__(self): 10 | super().__init__('waypoint_follower') 11 | self._action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') 12 | 13 | def define_waypoints(self): 14 | waypoints = [] 15 | 16 | # Waypoint 1 17 | wp1 = PoseStamped() 18 | wp1.header.frame_id = 'map' 19 | wp1.pose.position.x = 6.0 20 | wp1.pose.position.y = 1.5 21 | q = quaternion_from_euler(0, 0, 0) 22 | wp1.pose.orientation.x = q[0] 23 | wp1.pose.orientation.y = q[1] 24 | wp1.pose.orientation.z = q[2] 25 | wp1.pose.orientation.w = q[3] 26 | waypoints.append(wp1) 27 | 28 | # Waypoint 2 29 | wp2 = PoseStamped() 30 | wp2.header.frame_id = 'map' 31 | wp2.pose.position.x = -2.0 32 | wp2.pose.position.y = -8.0 33 | q = quaternion_from_euler(0, 0, 1.57) 34 | wp2.pose.orientation.x = q[0] 35 | wp2.pose.orientation.y = q[1] 36 | wp2.pose.orientation.z = q[2] 37 | wp2.pose.orientation.w = q[3] 38 | waypoints.append(wp2) 39 | 40 | # Waypoint 3 41 | wp3 = PoseStamped() 42 | wp3.header.frame_id = 'map' 43 | wp3.pose.position.x = 0.0 44 | wp3.pose.position.y = 0.0 45 | q = quaternion_from_euler(0, 0, 0) 46 | wp3.pose.orientation.x = q[0] 47 | wp3.pose.orientation.y = q[1] 48 | wp3.pose.orientation.z = q[2] 49 | wp3.pose.orientation.w = q[3] 50 | waypoints.append(wp3) 51 | 52 | # Add more waypoints as needed 53 | 54 | return waypoints 55 | 56 | def send_goal(self): 57 | waypoints = self.define_waypoints() 58 | goal_msg = FollowWaypoints.Goal() 59 | goal_msg.poses = waypoints 60 | 61 | self._action_client.wait_for_server() 62 | self._send_goal_future = self._action_client.send_goal_async( 63 | goal_msg, feedback_callback=self.feedback_callback 64 | ) 65 | self._send_goal_future.add_done_callback(self.goal_response_callback) 66 | 67 | def goal_response_callback(self, future): 68 | goal_handle = future.result() 69 | if not goal_handle.accepted: 70 | self.get_logger().info('Goal rejected.') 71 | return 72 | 73 | self.get_logger().info('Goal accepted.') 74 | self._get_result_future = goal_handle.get_result_async() 75 | self._get_result_future.add_done_callback(self.get_result_callback) 76 | 77 | def feedback_callback(self, feedback_msg): 78 | feedback = feedback_msg.feedback 79 | current_waypoint = feedback.current_waypoint 80 | self.get_logger().info(f'Navigating to waypoint {current_waypoint}') 81 | 82 | def get_result_callback(self, future): 83 | result = future.result().result 84 | self.get_logger().info('Waypoint following completed.') 85 | rclpy.shutdown() 86 | 87 | def main(args=None): 88 | rclpy.init(args=args) 89 | waypoint_follower = WaypointFollower() 90 | waypoint_follower.send_goal() 91 | rclpy.spin(waypoint_follower) 92 | 93 | if __name__ == '__main__': 94 | main() -------------------------------------------------------------------------------- /bme_ros2_navigation_py/bme_ros2_navigation_py/send_initialpose.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from geometry_msgs.msg import PoseWithCovarianceStamped 4 | from tf_transformations import quaternion_from_euler 5 | import time 6 | 7 | class InitialPosePublisher(Node): 8 | def __init__(self): 9 | super().__init__('initial_pose_publisher') 10 | 11 | # Declare and get parameters for the initial position and orientation 12 | self.declare_parameter('x', 0.0) 13 | self.declare_parameter('y', -4.0) 14 | self.declare_parameter('yaw', 0.0) 15 | 16 | x = self.get_parameter('x').get_parameter_value().double_value 17 | y = self.get_parameter('y').get_parameter_value().double_value 18 | yaw = self.get_parameter('yaw').get_parameter_value().double_value 19 | 20 | # Hardcoded covariance matrix 21 | covariance_matrix = [ 22 | 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 23 | 0.0, 3.0, 0.0, 0.0, 0.0, 0.0, 24 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 26 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 27 | 0.0, 0.0, 0.0, 0.0, 0.0, 3.0 28 | ] 29 | 30 | # Create a publisher for the /initialpose topic 31 | self.publisher = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10) 32 | 33 | # Create the PoseWithCovarianceStamped message 34 | initial_pose_msg = PoseWithCovarianceStamped() 35 | initial_pose_msg.header.frame_id = 'map' 36 | initial_pose_msg.header.stamp = self.get_clock().now().to_msg() 37 | initial_pose_msg.pose.pose.position.x = x 38 | initial_pose_msg.pose.pose.position.y = y 39 | initial_pose_msg.pose.pose.position.z = 0.0 40 | 41 | # Convert yaw to quaternion 42 | quaternion = quaternion_from_euler(0, 0, yaw) 43 | initial_pose_msg.pose.pose.orientation.x = quaternion[0] 44 | initial_pose_msg.pose.pose.orientation.y = quaternion[1] 45 | initial_pose_msg.pose.pose.orientation.z = quaternion[2] 46 | initial_pose_msg.pose.pose.orientation.w = quaternion[3] 47 | 48 | # Set the covariance matrix 49 | initial_pose_msg.pose.covariance = covariance_matrix 50 | 51 | # Publish the message 52 | self.get_logger().info('Publishing initial pose...') 53 | self.publisher.publish(initial_pose_msg) 54 | self.get_logger().info('Initial pose published.') 55 | 56 | def main(args=None): 57 | rclpy.init(args=args) 58 | node = InitialPosePublisher() 59 | # Give some time to publish before shutdown 60 | time.sleep(2) 61 | node.destroy_node() 62 | rclpy.shutdown() 63 | 64 | if __name__ == '__main__': 65 | main() -------------------------------------------------------------------------------- /bme_ros2_navigation_py/bme_ros2_navigation_py/slam_toolbox_load_map.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from ament_index_python.packages import get_package_share_directory 4 | from slam_toolbox.srv import DeserializePoseGraph 5 | from geometry_msgs.msg import Pose2D 6 | import os 7 | 8 | class MapLoaderNode(Node): 9 | def __init__(self): 10 | super().__init__('map_loader_node') 11 | 12 | # Declare and get the 'map_file' parameter 13 | self.declare_parameter('map_file', 'bme_ros2_navigation/maps/serialized') 14 | map_file_param = self.get_parameter('map_file').get_parameter_value().string_value 15 | 16 | # Extract package name and map file path from the parameter 17 | package_name, map_file_path = map_file_param.split('/', 1) 18 | 19 | # Get the absolute path to the map file 20 | try: 21 | package_path = get_package_share_directory(package_name) 22 | map_file = os.path.join(package_path, map_file_path) 23 | self.get_logger().info(f'Using map file: {map_file}') 24 | except Exception as e: 25 | self.get_logger().error(f'Failed to find package: {package_name}. Error: {e}') 26 | return 27 | 28 | initial_pose = Pose2D() 29 | initial_pose.x = 0.0 30 | initial_pose.y = 0.0 31 | initial_pose.theta = 0.0 32 | 33 | # Create a client for the /slam_toolbox/deserialize_map service 34 | self.client = self.create_client(DeserializePoseGraph, 'slam_toolbox/deserialize_map') 35 | 36 | # Wait for the service to become available 37 | self.get_logger().info('Waiting for /slam_toolbox/deserialize_map service...') 38 | while not self.client.wait_for_service(timeout_sec=1.0): 39 | self.get_logger().info('service not available, waiting again...') 40 | 41 | # Call the service with the map file 42 | # PROCESS_FIRST_NODE = 1, 43 | # PROCESS_NEAR_REGION = 2, 44 | # PROCESS_LOCALIZATION = 3 45 | self.request = DeserializePoseGraph.Request() 46 | self.request.filename = map_file 47 | self.request.match_type = 1 48 | self.request.initial_pose = initial_pose 49 | 50 | def send_request(self): 51 | self.get_logger().info(f'Calling /slam_toolbox/deserialize_map with {self.request.filename}...') 52 | return self.client.call_async(self.request) 53 | 54 | def main(args=None): 55 | rclpy.init(args=args) 56 | node = MapLoaderNode() 57 | future = node.send_request() 58 | rclpy.spin_until_future_complete(node, future) 59 | 60 | node.get_logger().info('Map loaded successfully.') 61 | 62 | node.destroy_node() 63 | rclpy.shutdown() 64 | 65 | if __name__ == '__main__': 66 | main() -------------------------------------------------------------------------------- /bme_ros2_navigation_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bme_ros2_navigation_py 5 | 1.0.0 6 | Python nodes for slam, localization and navigation with Gazebo Harmonic and ROS Jazzy for BME MOGI ROS2 course 7 | David Dudas 8 | Apache License 2.0 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /bme_ros2_navigation_py/resource/bme_ros2_navigation_py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MOGI-ROS/Week-7-8-ROS2-Navigation/a0313beb9795fa99e7ff42ba1f7a3fe450571f2a/bme_ros2_navigation_py/resource/bme_ros2_navigation_py -------------------------------------------------------------------------------- /bme_ros2_navigation_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/bme_ros2_navigation_py 3 | [install] 4 | install_scripts=$base/lib/bme_ros2_navigation_py 5 | -------------------------------------------------------------------------------- /bme_ros2_navigation_py/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = 'bme_ros2_navigation_py' 4 | 5 | setup( 6 | name=package_name, 7 | version='1.0.0', 8 | packages=find_packages(exclude=['test']), 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='David Dudas', 17 | maintainer_email='david.dudas@outlook.com', 18 | description='Python nodes for slam, localization and navigation with Gazebo Harmonic and ROS Jazzy for BME MOGI ROS2 course', 19 | license='Apache License 2.0', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'send_initialpose = bme_ros2_navigation_py.send_initialpose:main', 24 | 'slam_toolbox_load_map = bme_ros2_navigation_py.slam_toolbox_load_map:main', 25 | 'follow_waypoints = bme_ros2_navigation_py.follow_waypoints:main', 26 | ], 27 | }, 28 | ) 29 | --------------------------------------------------------------------------------