├── .circleci └── config.yml ├── .clang-format ├── .github └── workflows │ └── build-ros.yml ├── .gitignore ├── LICENSE.txt ├── README.md ├── clang-formatter.sh ├── mrpt_map_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mrpt_map_server │ │ └── map_server_node.h ├── launch │ └── mrpt_map_server.launch.py ├── package.xml ├── params │ ├── README.md │ └── metric-map-gridmap.ini └── src │ └── map_server_node.cpp ├── mrpt_msgs_bridge ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mrpt_msgs_bridge │ │ ├── beacon.hpp │ │ ├── landmark.hpp │ │ ├── marker_msgs.hpp │ │ └── network_of_poses.hpp ├── package.xml └── src │ ├── beacon.cpp │ ├── landmark.cpp │ ├── marker_msgs.cpp │ └── network_of_poses.cpp ├── mrpt_nav_interfaces ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ ├── NavigateGoal.action │ └── NavigateWaypoints.action ├── msg │ ├── GeoreferencingMetadata.msg │ ├── NavigationFeedback.msg │ └── NavigationFinalStatus.msg ├── package.xml └── srv │ ├── GetGridmapLayer.srv │ ├── GetLayers.srv │ ├── GetPointmapLayer.srv │ ├── MakePlanFromTo.srv │ └── MakePlanTo.srv ├── mrpt_navigation ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.rst └── package.xml ├── mrpt_pf_localization ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ ├── mrpt_pf_localization │ │ └── mrpt_pf_localization_core.h │ └── mrpt_pf_localization_node.h ├── launch │ └── localization.launch.py ├── package.xml ├── params │ ├── default-relocalization-pipeline.yaml │ ├── default.config.yaml │ └── map-occgrid2d.ini ├── src │ ├── main.cpp │ ├── mrpt_pf_localization │ │ └── mrpt_pf_localization_core.cpp │ └── mrpt_pf_localization_component.cpp └── test │ └── test_pf_localization.cpp ├── mrpt_pointcloud_pipeline ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mrpt_pointcloud_pipeline │ │ └── mrpt_pointcloud_pipeline_node.h ├── launch │ └── pointcloud_pipeline.launch.py ├── package.xml ├── params │ └── point-cloud-pipeline.yaml └── src │ ├── main.cpp │ └── mrpt_pointcloud_pipeline_component.cpp ├── mrpt_rawlog ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ └── rosbag2rawlog.cpp ├── mrpt_reactivenav2d ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mrpt_reactivenav2d │ │ └── mrpt_reactivenav2d_node.hpp ├── launch │ └── rnav.launch.py ├── package.xml ├── params │ └── reactive2d_default.ini └── src │ └── mrpt_reactivenav2d_node.cpp ├── mrpt_tps_astar_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── configs │ ├── ini │ │ ├── barn-pf-localization.ini │ │ └── ptgs_jackal.ini │ └── params │ │ ├── costmap-obstacles.yaml │ │ ├── costmap-prefer-waypoints.yaml │ │ ├── nav-engine-params.yaml │ │ └── planner-params.yaml ├── launch │ └── tps_astar_planner.launch.py ├── package.xml ├── path-planner-sandbox │ ├── .gitignore │ ├── costmap-obstacles.yaml │ ├── costmap-prefer-waypoints.yaml │ ├── map_pgm_0.pgm │ ├── map_pgm_299.pgm │ ├── nav-engine-params.yaml │ ├── planner-params.yaml │ ├── ptgs_jackal.ini │ ├── run-path-plan-test.sh │ ├── yaml_0.yaml │ └── yaml_299.yaml ├── python │ └── initial_pose_publisher.py └── src │ └── mrpt_tps_astar_planner_node.cpp ├── mrpt_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt ├── datasets │ ├── README.md │ ├── driving_in_office.rawlog │ └── driving_in_office_obs.rawlog ├── launch │ ├── demo_astar_planner_gridmap.launch.py │ ├── demo_composable_nodes.launch.py │ ├── demo_localization_pf_mvsim_2d_lidar.launch.py │ ├── demo_map_server_from_mm.launch.py │ ├── demo_map_server_gridmap_from_yaml.launch.py │ ├── demo_pointcloud_pipeline_mvsim.launch.py │ ├── demo_reactive_nav_mvsim.launch.py │ └── demo_ro.launch ├── maps │ ├── demo_world2.png │ ├── demo_world2.yaml │ ├── gh25_real_top_laser.simplemap │ └── gh25_simulated.simplemap ├── mvsim │ └── demo_world2.world.xml ├── package.xml ├── params │ └── mvsim_ros2_params.yaml └── rviz2 │ ├── gridmap.rviz │ └── rnav_demo.rviz └── scripts └── generate_statuses_table.py /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | jobs: 3 | ros-rolling-u22.04: 4 | docker: 5 | - image: ros:rolling-ros-base 6 | steps: 7 | - checkout 8 | - run: 9 | name: "Pull Submodules" 10 | command: | 11 | git submodule update --init --recursive 12 | - run: 13 | name: Install rosdep dependencies 14 | command: | 15 | apt update # needed for rosdep - apt to get list of packages from ROS servers 16 | rosdep install --from-paths . --ignore-src -r -y 17 | - run: 18 | name: Build with colcon 19 | command: | 20 | source /opt/ros/*/setup.bash 21 | MAKEFLAGS="-j2" colcon build --symlink-install --parallel-workers 2 --event-handlers console_direct+ 22 | workflows: 23 | version: 2 24 | build: 25 | jobs: 26 | - ros-rolling-u22.04 27 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Mobile Robot Programming Toolkit (MRPT) 3 | # http://www.mrpt.org/ 4 | # 5 | ############################################################################### 6 | 7 | # For all the available options - and taking in account the version of 8 | # clang-format see the following links: 9 | # 10 | # More accurate if we know the version in advance 11 | # http://releases.llvm.org/3.8.0/tools/clang/docs/ClangFormatStyleOptions.html 12 | # 13 | # Holds a general list of keys - may not be applicable for our version though 14 | # https://clang.llvm.org/docs/ClangFormatStyleOptions.html 15 | 16 | Language: Cpp 17 | #BasedOnStyle: LLVM 18 | BasedOnStyle: Google 19 | #BasedOnStyle: Chromium 20 | #BasedOnStyle: Mozilla 21 | #BasedOnStyle: WebKit 22 | # --- 23 | #AccessModifierOffset: -4 24 | AlignAfterOpenBracket: AlwaysBreak # Values: Align, DontAlign, AlwaysBreak 25 | #AlignConsecutiveAssignments: true 26 | #AlignConsecutiveDeclarations: true 27 | #AlignEscapedNewlinesLeft: true 28 | #AlignOperands: false 29 | AlignTrailingComments: false # Should be off, causes many dummy problems!! 30 | #AllowAllParametersOfDeclarationOnNextLine: true 31 | #AllowShortBlocksOnASingleLine: false 32 | #AllowShortCaseLabelsOnASingleLine: false 33 | #AllowShortFunctionsOnASingleLine: Empty 34 | #AllowShortIfStatementsOnASingleLine: false 35 | #AllowShortLoopsOnASingleLine: false 36 | #AlwaysBreakAfterDefinitionReturnType: None 37 | #AlwaysBreakAfterReturnType: None 38 | #AlwaysBreakBeforeMultilineStrings: true 39 | #AlwaysBreakTemplateDeclarations: true 40 | #BinPackArguments: false 41 | #BinPackParameters: false 42 | #BraceWrapping: 43 | #AfterClass: false 44 | #AfterControlStatement: false 45 | #AfterEnum: false 46 | #AfterFunction: false 47 | #AfterNamespace: false 48 | #AfterObjCDeclaration: false 49 | #AfterStruct: false 50 | #AfterUnion: false 51 | #BeforeCatch: false 52 | #BeforeElse: true 53 | #IndentBraces: false 54 | #BreakBeforeBinaryOperators: None 55 | BreakBeforeBraces: Allman 56 | #BreakBeforeTernaryOperators: true 57 | #BreakConstructorInitializersBeforeComma: false 58 | ColumnLimit: 100 59 | #CommentPragmas: '' 60 | #ConstructorInitializerAllOnOneLineOrOnePerLine: true 61 | #ConstructorInitializerIndentWidth: 4 62 | #ContinuationIndentWidth: 4 63 | #Cpp11BracedListStyle: true 64 | #DerivePointerAlignment: false 65 | #DisableFormat: false 66 | #ExperimentalAutoDetectBinPacking: false 67 | ##FixNamespaceComments: true # Not applicable in 3.8 68 | #ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 69 | #IncludeCategories: 70 | #- Regex: '.*' 71 | #Priority: 1 72 | IndentCaseLabels: true 73 | IndentWidth: 4 74 | IndentWrappedFunctionNames: true 75 | #KeepEmptyLinesAtTheStartOfBlocks: true 76 | #MacroBlockBegin: '' 77 | #MacroBlockEnd: '' 78 | MaxEmptyLinesToKeep: 1 79 | NamespaceIndentation: None 80 | #PenaltyBreakBeforeFirstCallParameter: 19 81 | #PenaltyBreakComment: 300 82 | #PenaltyBreakFirstLessLess: 120 83 | #PenaltyBreakString: 1000 84 | #PenaltyExcessCharacter: 1000000 85 | #PenaltyReturnTypeOnItsOwnLine: 200 86 | DerivePointerAlignment: false 87 | #PointerAlignment: Left 88 | ReflowComments: true # Should be true, otherwise clang-format doesn't touch comments 89 | SortIncludes: true 90 | #SpaceAfterCStyleCast: false 91 | SpaceBeforeAssignmentOperators: true 92 | #SpaceBeforeParens: ControlStatements 93 | #SpaceInEmptyParentheses: false 94 | #SpacesBeforeTrailingComments: 2 95 | #SpacesInAngles: false 96 | #SpacesInContainerLiterals: true 97 | #SpacesInCStyleCastParentheses: false 98 | #SpacesInParentheses: false 99 | #SpacesInSquareBrackets: false 100 | Standard: Cpp11 101 | TabWidth: 4 102 | UseTab: Always # Available options are Never, Always, ForIndentation 103 | # UseTab: ForIndentationAndAlignment # Not applicable in >= 3.8 104 | #--- 105 | -------------------------------------------------------------------------------- /.github/workflows/build-ros.yml: -------------------------------------------------------------------------------- 1 | # Based on GTSAM file (by @ProfFan) 2 | name: CI Build colcon 3 | 4 | on: [push, pull_request] 5 | 6 | jobs: 7 | build_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. 8 | runs-on: ubuntu-latest 9 | env: 10 | DEBIAN_FRONTEND: noninteractive 11 | strategy: 12 | matrix: 13 | # Define the Docker image(s) associated with each ROS distribution. 14 | # The include syntax allows additional variables to be defined, like 15 | # docker_image in this case. See documentation: 16 | # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build 17 | # 18 | # Platforms are defined in REP 3 and REP 2000: 19 | # https://ros.org/reps/rep-0003.html 20 | # https://ros.org/reps/rep-2000.html 21 | include: 22 | # Humble Hawksbill (May 2022 - May 2027) 23 | - docker_image: ubuntu:jammy 24 | ros_distribution: humble 25 | ros_version: 2 26 | 27 | # Jazzy (2024 - 2029) 28 | - docker_image: ubuntu:noble 29 | ros_distribution: jazzy 30 | ros_version: 2 31 | 32 | # Rolling Ridley (No End-Of-Life) 33 | - docker_image: ubuntu:noble 34 | ros_distribution: rolling 35 | ros_version: 2 36 | 37 | container: 38 | image: ${{ matrix.docker_image }} 39 | 40 | steps: 41 | - name: Checkout source code 42 | uses: actions/checkout@v3 43 | 44 | - name: setup ROS environment 45 | uses: ros-tooling/setup-ros@v0.7 46 | with: 47 | required-ros-distributions: ${{ matrix.ros_distribution }} 48 | use-ros2-testing: true 49 | 50 | - name: Install rosdep dependencies 51 | run: | 52 | . /opt/ros/*/setup.sh 53 | rosdep update 54 | rosdep install --from-paths . --ignore-src -r -y 55 | 56 | - name: Build with colcon 57 | run: | 58 | . /opt/ros/*/setup.sh 59 | env 60 | MAKEFLAGS="-j2" colcon build --symlink-install --parallel-workers 2 --event-handlers console_direct+ -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.autosave 2 | .kdev* 3 | *.kdev* 4 | *~ 5 | *.rawlog 6 | LOG_* 7 | CMakeLists.txt.user 8 | .vscode/settings.json 9 | .vscode 10 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2014-2022, José Luis Blanco-Claraco, contributors. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /clang-formatter.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | find mrpt_*/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs clang-format-14 -i 4 | -------------------------------------------------------------------------------- /mrpt_map_server/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_map_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | * FIX: remove usage of obsolete ament_target_dependencies() 8 | * Contributors: Jose Luis Blanco-Claraco 9 | 10 | 2.2.1 (2024-10-12) 11 | ------------------ 12 | 13 | 2.2.0 (2024-09-25) 14 | ------------------ 15 | * Merge pull request `#149 `_ from mrpt-ros-pkg/feature/utm-coordinates 16 | Support UTM global coordinates for geo-referenciated maps 17 | * Update package.xml: minimum required version of mp2p_icp 18 | * mrpt_map_server now publishes the map georeferenciation metadata, as topics and /tf (frames: utm, enu) 19 | * Update URL entries in package.xml to each package proper documentation 20 | * ament linters: manually enable just cmake and xml linters 21 | * reformat clang-format with 100 column width 22 | * Update README.md with geo-referenciation concepts 23 | * Contributors: Jose Luis Blanco-Claraco 24 | 25 | 2.1.1 (2024-09-02) 26 | ------------------ 27 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 28 | * update dependencies 29 | * Depend on new mrpt_lib packages (deprecate mrpt2) 30 | * Contributors: Jose Luis Blanco-Claraco 31 | 32 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 33 | * update dependencies 34 | * Depend on new mrpt_lib packages (deprecate mrpt2) 35 | * Contributors: Jose Luis Blanco-Claraco 36 | 37 | 2.1.0 (2024-08-08) 38 | ------------------ 39 | * docs: typos and clarifications 40 | * map server: simplify publication policy. Using transient local only and never republishing them 41 | * Merge branch 'ros2' into wip/port-tps-astar 42 | * Merge branch 'ros2' into wip/port-tps-astar 43 | * Contributors: Jose Luis Blanco-Claraco 44 | 45 | 2.0.1 (2024-05-28) 46 | ------------------ 47 | 48 | 2.0.0 (2024-05-28) 49 | ------------------ 50 | * Implement map getter services 51 | * Define map_server services 52 | * Comply with ROS2 REP-2003 53 | * new param force_republish_period 54 | * map_server: publish voxelmaps as points too 55 | * map_server: Implement publishing from .mm files 56 | * Rename mrpt_map: mrpt_map_server 57 | * Contributors: Jose Luis Blanco-Claraco 58 | 59 | 1.0.3 (2022-06-25) 60 | ------------------ 61 | 62 | 1.0.2 (2022-06-25) 63 | ------------------ 64 | 65 | 1.0.1 (2022-06-24) 66 | ------------------ 67 | * Fix build errors 68 | * Removed now obsolete tf_prefix 69 | * Ported to tf2 and mrpt::ros1bridge 70 | * Contributors: Jose Luis Blanco-Claraco 71 | 72 | 1.0.0 (2022-04-30) 73 | ------------------ 74 | * Update URLs to https 75 | * Update build dep to mrpt2 76 | * Contributors: Jose Luis Blanco Claraco 77 | 78 | 0.1.26 (2019-10-05) 79 | ------------------- 80 | 81 | 0.1.25 (2019-10-04) 82 | ------------------- 83 | 84 | 0.1.24 (2019-04-12) 85 | ------------------- 86 | * fix build against mrpt-1.5 87 | * Fix build against MRPT 1.9.9 88 | * Contributors: Inounx, Jose Luis Blanco-Claraco, Julian Lopez Velasquez 89 | 90 | 0.1.23 (2018-06-14) 91 | ------------------- 92 | 93 | 0.1.20 (2018-04-26) 94 | ------------------- 95 | * fix build against mrpt 2.0 96 | * partial fix build w mrpt 2.0 97 | * optimized build (-O3) 98 | * fix use c++14 99 | * Merge branch 'master' of github.com:tuw-robotics/mrpt_navigation 100 | * Merge branch 'master' into master 101 | * CMake finds MRPT >=1.5 in ROS master branch 102 | * Merge branch 'master' into compat-mrpt-1.5 103 | * CMake finds MRPT >=1.9 104 | * avoid Eigen warnings with GCC-7 105 | * Removed unnecessry MRPT_VERSION checks 106 | * Fixes for clang format 107 | * Adapted CMakeLists to new mrpt 108 | * Contributors: Borys Tymchenko, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Markus Bader 109 | 110 | 111 | 0.1.22 (2018-05-22) 112 | ------------------- 113 | * fix all catkin_lint errors 114 | * Contributors: Jose Luis Blanco-Claraco 115 | 116 | 0.1.21 (2018-04-27) 117 | ------------------- 118 | * Upgrade version 0.1.20 (`#99 `_) 119 | * fix build against mrpt 2.0 120 | * partial fix build w mrpt 2.0 121 | * optimized build (-O3) 122 | * fix use c++14 123 | * Merge branch 'master' of github.com:tuw-robotics/mrpt_navigation 124 | * Merge branch 'master' into master 125 | * CMake finds MRPT >=1.5 in ROS master branch 126 | * Merge branch 'master' into compat-mrpt-1.5 127 | * CMake finds MRPT >=1.9 128 | * avoid Eigen warnings with GCC-7 129 | * Removed unnecessry MRPT_VERSION checks 130 | * Fixes for clang format 131 | * Adapted CMakeLists to new mrpt 132 | * Contributors: Borys Tymchenko, Hunter Laux, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Markus Bader 133 | 134 | 0.1.18 (2017-01-22) 135 | ------------------- 136 | 137 | 0.1.17 (2017-01-22) 138 | ------------------- 139 | * Remove all errors generated by catkin_lint and cleanup unused templates from CMakeLists.txt files 140 | * Contributors: Jorge Santos 141 | 142 | 0.1.16 (2016-12-13) 143 | ------------------- 144 | 145 | 0.1.15 (2016-11-06) 146 | ------------------- 147 | 148 | 0.1.14 (2016-09-12) 149 | ------------------- 150 | 151 | 0.1.13 (2016-09-03) 152 | ------------------- 153 | 154 | 0.1.12 (2016-09-03) 155 | ------------------- 156 | 157 | 0.1.11 (2016-08-21) 158 | ------------------- 159 | 160 | 0.1.10 (2016-08-05) 161 | ------------------- 162 | 163 | 0.1.9 (2016-08-05) 164 | ------------------ 165 | 166 | 0.1.8 (2016-06-29) 167 | ------------------ 168 | 169 | 0.1.7 (2016-06-20) 170 | ------------------ 171 | 172 | 0.1.6 (2016-03-20) 173 | ------------------ 174 | * build fixes 175 | * Contributors: Jose Luis Blanco 176 | 177 | 0.1.5 (2015-04-29) 178 | ------------------ 179 | * Fix build against mrpt 1.3.0 180 | * Contributors: Jose Luis Blanco 181 | 182 | 0.1.4 (2014-12-27) 183 | ------------------ 184 | * Removed 'mrpt' dep from catkin_package(). 185 | I *think* this is giving problems to dependant pkgs and is not needed... 186 | * localization: New param to configure sensor sources in a flexible way 187 | * Contributors: Jose Luis Blanco 188 | 189 | 0.1.3 (2014-12-18) 190 | ------------------ 191 | * Fix many missing install files 192 | * Contributors: Jose Luis Blanco 193 | 194 | 0.1.2 (2014-12-18) 195 | ------------------ 196 | 197 | 0.1.1 (2014-12-17) 198 | ------------------ 199 | * First public binary release. 200 | 201 | 0.1.0 (2014-12-17) 202 | ------------------ 203 | * consistent version numbers 204 | * Fixes broken dependencies 205 | * Update all wiki URLs 206 | * Fix build with mrpt 1.2.x 207 | * localization uses tf odom 208 | * localization working like amcl 209 | * mrpt_rawlog_play working 210 | * ros mrpt convetion fnc renamed to convert 211 | * rawlog player 212 | * map publisher added but not yet working 213 | * Contributors: Jose Luis Blanco, Jose-Luis Blanco-Claraco, Markus Bader 214 | -------------------------------------------------------------------------------- /mrpt_map_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mrpt_map_server) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_components REQUIRED) 8 | find_package(nav_msgs REQUIRED) 9 | find_package(mrpt_msgs REQUIRED) 10 | find_package(mrpt_nav_interfaces REQUIRED) 11 | find_package(tf2 REQUIRED) 12 | find_package(tf2_ros REQUIRED) 13 | find_package(tf2_geometry_msgs REQUIRED) 14 | 15 | find_package(mrpt-maps REQUIRED) 16 | find_package(mrpt-ros2bridge REQUIRED) 17 | find_package(mrpt-topography REQUIRED) # shipped by ROS pkg mrpt_libobs 18 | find_package(mp2p_icp_map REQUIRED) 19 | 20 | message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") 21 | 22 | if(NOT CMAKE_C_STANDARD) 23 | set(CMAKE_C_STANDARD 99) 24 | endif() 25 | 26 | if(NOT CMAKE_CXX_STANDARD) 27 | set(CMAKE_CXX_STANDARD 17) 28 | endif() 29 | 30 | if (CMAKE_COMPILER_IS_GNUCXX) 31 | # High level of warnings. 32 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 33 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 34 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 35 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 36 | if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 37 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 38 | endif() 39 | endif() 40 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 41 | add_compile_options(-O3) 42 | endif() 43 | 44 | ########### 45 | ## Build ## 46 | ########### 47 | 48 | # Declare a cpp executable 49 | add_executable(map_server_node 50 | src/map_server_node.cpp 51 | include/${PROJECT_NAME}/map_server_node.h) 52 | 53 | target_include_directories(map_server_node 54 | PUBLIC 55 | $ 56 | $ 57 | ) 58 | 59 | # Specify libraries to link a library or executable target against 60 | target_link_libraries(map_server_node PRIVATE 61 | mrpt::maps 62 | mrpt::ros2bridge 63 | mrpt::topography 64 | mola::mp2p_icp_map 65 | rclcpp::rclcpp 66 | ${rclcpp_components_TARGETS} 67 | tf2::tf2 68 | ${nav_msgs_TARGETS} 69 | ${mrpt_msgs_TARGETS} 70 | ${mrpt_nav_interfaces_TARGETS} 71 | ${tf2_geometry_msgs_TARGETS} 72 | ${tf2_ros_TARGETS} 73 | ) 74 | 75 | ############# 76 | ## Install ## 77 | ############# 78 | 79 | install(TARGETS map_server_node 80 | DESTINATION lib/${PROJECT_NAME} 81 | ) 82 | 83 | install(DIRECTORY 84 | launch 85 | params 86 | DESTINATION share/${PROJECT_NAME} 87 | ) 88 | 89 | if(BUILD_TESTING) 90 | find_package(ament_lint_auto REQUIRED) 91 | # the following line skips the linter which checks for copyrights 92 | # uncomment the line when a copyright and license is not present in all source files 93 | #set(ament_cmake_copyright_FOUND TRUE) 94 | # the following line skips cpplint (only works in a git repo) 95 | # uncomment the line when this package is not in a git repo 96 | #set(ament_cmake_cpplint_FOUND TRUE) 97 | ament_lint_auto_find_test_dependencies() 98 | endif() 99 | 100 | ament_package() 101 | -------------------------------------------------------------------------------- /mrpt_map_server/README.md: -------------------------------------------------------------------------------- 1 | # mrpt_map_server 2 | 3 | # Table of Contents 4 | * [Overview](#Overview) 5 | * [Node: mrpt_map_server](#Node:-mrpt_map_server) 6 | * [Working rationale](#Working-rationale) 7 | * [ROS Parameters](#ROS-Parameters) 8 | * [Subscribed topics](#Subscribed-topics) 9 | * [Published topics](#Published-topics) 10 | * [Services](#services) 11 | * [Template ROS 2 launch files](#Template-ROS-2-launch-files) 12 | * [Demos](#Demos) 13 | 14 | ## Overview 15 | This package provides a ROS 2 node that publishes a static **map** for other nodes to use it. 16 | Unlike classic ROS 1 ``map_server``, this node can publish a range of different metric maps, not only occupancy grids. 17 | 18 | ## Node: mrpt_map_server 19 | 20 | ### Working rationale 21 | The C++ ROS 2 node loads all parameters at start up, loads the map 22 | as requested by parameters, and publishes the metric map in the corresponding topics. 23 | Messages are sent as transient local, so new subscribers can receive them even 24 | if they start afterwards. 25 | 26 | There are **three formats** in which maps can be read: 27 | 28 | 1. The **preferred format** is as an [mp2p_icp](https://github.com/MOLAorg/mp2p_icp)'s metric map file (`*.mm`), normally generated 29 | via [sm2mm](https://github.com/MOLAorg/mp2p_icp/tree/master/apps/sm2mm) from a [MRPT "simplemap"](https://docs.mrpt.org/reference/latest/class_mrpt_maps_CSimpleMap.html) (``*.simplemap``) 30 | that comes from a SLAM session, e.g. using [mola_lidar_odometry](https://docs.mola-slam.org/latest/). 31 | 32 | 3. As a [ROS standard YAML file](https://wiki.ros.org/map_server). Here, a ``*.yaml`` file specifies the metadata of a 2D occupancy gridmap, 33 | which is stored as an accompanying image file. The map will be actually encapsulated into a `metric_map_t` map with layer name `map`. 34 | 35 | 4. As a [serialized](https://docs.mrpt.org/reference/latest/group_mrpt_serialization_grp.html) MRPT metric map file. 36 | A ``*.metricmap`` file contains any of the existing 37 | [MRPT metric maps](https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html) 38 | (point clouds, grid maps, etc.), which may come from custom applications or other SLAM packages. 39 | The map will be actually encapsulated into a `metric_map_t` map with layer name `map`. 40 | 41 | So, whatever is the map source, this node will internally build a [`metric_map_t`](https://docs.mola-slam.org/latest/mp2p_icp_basics.html) 42 | with one or more map layers, so it gets published in a uniform way to subscribers. 43 | 44 | Refer to example launch files at the end of this file for examples 45 | of usage of each of these methods. 46 | 47 | 48 | ### ROS Parameters 49 | 50 | #### Related to determining where to read the map from 51 | * (Option 1 above) ``mm_file`` (Default=undefined). Determine the `metric_map_t` file to load, coming from [sm2mm](https://github.com/MOLAorg/mp2p_icp/tree/master/apps/sm2mm) or any other custom user application using `mp2p_icp`. 52 | * (Option 2 above) ``map_yaml_file`` (Default=undefined): Define this parameter to load a [ROS standard YAML file](https://wiki.ros.org/map_server) gridmap. 53 | * (Option 3 above) ``mrpt_metricmap_file`` (Default=undefined). 54 | 55 | #### Related to ROS published topics: 56 | * ``frame_id`` (Default=``map``): TF frame. 57 | * `pub_mm_topic` (Default=`mrpt_map`). Despite the map source, it will be eventually stored as a `mp2p_icp`'s `metric_map_t` (`*.mm`) structure, then each layer will be published using its **layer name** as a **topic name** and with the appropriate type 58 | (e.g. PointCloud2, OccupancyGrid,...). Also, the whole metric map is published as a generic serialized object to the topic defined by the 59 | parameter `pub_mm_topic`. 60 | 61 | ### Subscribed topics 62 | None. 63 | 64 | ### Published topics 65 | * ``${pub_mm_topic}/metric_map`` (Default: ``mrpt_map/metric_map``) (``mrpt_msgs::msg::GenericObject``) (topic name can be changed with parameter `pub_mm_topic`). 66 | * ``${pub_mm_topic}/geo_ref`` (Default: ``mrpt_map/geo_ref``) (``mrpt_msgs::msg::GenericObject``). An MRPT-serialization of ``mp2p_icp::metric_map_t::Georeferencing`` metadata (topic name can be changed with parameter `pub_mm_topic`). 67 | * ``${pub_mm_topic}/geo_ref_metadata`` (Default: ``mrpt_map/geo_ref_metadata``)(``mrpt_nav_interfaces::msgs::msg::GeoreferencingMetadata``). A ROS plain message with the contents of ``mp2p_icp::metric_map_t::Georeferencing`` metadata. 68 | * ``${pub_mm_topic}/`` (Default: ``mrpt_map/``) (``mrpt_msgs::msg::GenericObject``) 69 | * ``${pub_mm_topic}/_points`` (``sensor_msgs::msg::PointCloud2``), one per map layer. 70 | * ``${pub_mm_topic}/_gridmap`` (``nav_msgs::msg::OccupancyGrid``) 71 | * ``${pub_mm_topic}/_gridmap_metadata`` (``nav_msgs::msg::MapMetaData``) 72 | * (... one per map layer ...) 73 | 74 | If using options 2 or 3 above, there will be just one layer named `map`. 75 | 76 | ### Published tf and geo-referenced maps 77 | Refer to [the documentation](https://docs.mola-slam.org/latest/geo-referencing.html) within the MOLA project on geo-referencing for a description of the ``utm`` and ``enu`` frames, 78 | defined by this package if fed with a ``*.mm`` file with geo-referenced metadata. 79 | 80 | ![mola_mrpt_ros_geo_referenced_utm_frames](https://github.com/user-attachments/assets/28f32aac-2c24-4857-9653-9890350fd90e) 81 | 82 | 83 | ### Services 84 | * ``GetLayers``: Returns the list of map layer names: 85 | 86 | ```bash 87 | # Example usage: 88 | ros2 service call /map_server_node/get_layers mrpt_nav_interfaces/srv/GetLayers 89 | requester: making request: mrpt_nav_interfaces.srv.GetLayers_Request() 90 | 91 | response: 92 | mrpt_nav_interfaces.srv.GetLayers_Response(layers=['map']) 93 | ``` 94 | 95 | * ``GetGridmapLayer``: Can be used to request a given map layer of type gridmap. 96 | 97 | ```bash 98 | # Example usage: 99 | ros2 service call /map_server_node/get_grid_layer mrpt_nav_interfaces/srv/GetGridmapLayer "layer_name:\ 100 | 'map'" 101 | requester: making request: mrpt_nav_interfaces.srv.GetGridmapLayer_Request(layer_name='map') 102 | 103 | response: 104 | mrpt_nav_interfaces.srv.GetGridmapLayer_Response(valid=True, grid=nav_msgs.msg.OccupancyGrid(... 105 | ``` 106 | 107 | * ``GetPointmapLayer``: Can be used to request a given map layer of type point cloud. 108 | 109 | ### Template ROS 2 launch files 110 | 111 | This package provides [launch/mrpt_map_server.launch.py](launch/mrpt_map_server.launch.py): 112 | 113 | ros2 launch mrpt_map_server mrpt_map_server.launch.py 114 | 115 | which can be used in user projects to launch the MRPT map server node, by setting these [launch arguments](https://docs.ros.org/en/rolling/Tutorials/Intermediate/Launch/Using-Substitutions.html): 116 | 117 | 118 | ## Demos 119 | 120 | Launch a map server from a ROS yaml gridmap ([launch file](../mrpt_tutorials/launch/demo_map_server_gridmap_from_yaml.launch.py)): 121 | 122 | ```bash 123 | ros2 launch mrpt_tutorials demo_map_server_gridmap_from_yaml.launch.py 124 | ``` 125 | 126 | Launch a map server from a custom `.mm` map ([launch file](../mrpt_tutorials/launch/demo_map_server_from_mm.launch.py)), 127 | which in the launch file is read from the environment variable `MM_FILE`, so it can be used like: 128 | 129 | ```bash 130 | ros2 launch mrpt_tutorials demo_map_server_from_mm.launch.py mm_file:=/path/to/my/map.mm 131 | ``` 132 | -------------------------------------------------------------------------------- /mrpt_map_server/include/mrpt_map_server/map_server_node.h: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | class MapServer : public rclcpp::Node 28 | { 29 | public: 30 | MapServer(); 31 | ~MapServer(); 32 | void init(); 33 | void loop(); 34 | 35 | private: 36 | // params that come from launch file 37 | std::string pub_mm_topic_ = "map_server"; 38 | 39 | std::string service_map_str_ = "static_map"; 40 | 41 | std::string frame_id_ = "map"; 42 | 43 | std::shared_ptr tf_broadcaster_; 44 | 45 | /// metric map: will be used whatever is the incoming map format. 46 | mp2p_icp::metric_map_t theMap_; 47 | std::mutex theMapMtx_; 48 | 49 | /// (re)publish each map layer, creating the publisher the first time. 50 | /// If the number of subscriber is detected to have changed, msgs will be 51 | /// re-published. 52 | void publish_map(); 53 | 54 | // ------ publishers -------- 55 | 56 | template 57 | struct PerTopicData 58 | { 59 | typename rclcpp::Publisher::SharedPtr pub; 60 | 61 | #if 0 // disabled 62 | size_t subscribers = 0; 63 | double lastPublishTime = 0; 64 | 65 | bool new_subscribers( 66 | const rclcpp::Time& now, double forceRepublishPeriodSeconds) 67 | { 68 | const auto N = pub->get_subscription_count(); 69 | bool ret = N > subscribers; 70 | subscribers = N; 71 | 72 | if (forceRepublishPeriodSeconds > 0) 73 | { 74 | if (now.seconds() - lastPublishTime > 75 | forceRepublishPeriodSeconds) 76 | ret = true; 77 | } 78 | 79 | if (ret) lastPublishTime = now.seconds(); 80 | return ret; 81 | } 82 | #endif 83 | }; 84 | 85 | // for the top-level mm metric map: 86 | PerTopicData pubMM_; 87 | 88 | PerTopicData pubGeoRef_; 89 | PerTopicData pubGeoRefMsg_; 90 | 91 | // clang-format off 92 | // Binary form of each layer: 93 | std::map> pubLayers_; 94 | 95 | // for grid map layers: 96 | std::map> pubGridMaps_; 97 | std::map> pubGridMapsMetaData_; 98 | // for points layers: 99 | std::map> pubPointMaps_; 100 | 101 | // clang-format on 102 | 103 | sensor_msgs::msg::PointCloud2 pointmap_layer_to_msg(const mrpt::maps::CPointsMap::Ptr& pts); 104 | 105 | // Services: 106 | rclcpp::Service::SharedPtr srvMapLayers_; 107 | 108 | void srv_map_layers( 109 | const std::shared_ptr req, 110 | std::shared_ptr resp); 111 | 112 | rclcpp::Service::SharedPtr srvGetGrid_; 113 | 114 | void srv_get_gridmap( 115 | const std::shared_ptr req, 116 | std::shared_ptr resp); 117 | 118 | rclcpp::Service::SharedPtr srvGetPoints_; 119 | 120 | void srv_get_pointmap( 121 | const std::shared_ptr req, 122 | std::shared_ptr resp); 123 | }; 124 | -------------------------------------------------------------------------------- /mrpt_map_server/launch/mrpt_map_server.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for mrpt_map_server 2 | # 3 | # See the docs on the configurable launch arguments for this file in: 4 | # https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_map_server#template-ros-2-launch-files 5 | # 6 | 7 | import os 8 | from launch import LaunchDescription 9 | from launch_ros.actions import Node 10 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 11 | from launch.substitutions import LaunchConfiguration 12 | # from ament_index_python.packages import get_package_share_directory 13 | from launch.launch_description_sources import PythonLaunchDescriptionSource 14 | 15 | 16 | def generate_launch_description(): 17 | # mrpt_map_pkg_dir = get_package_share_directory('mrpt_map_server') 18 | 19 | 20 | # Format 1 in docs: 21 | mm_file_arg = DeclareLaunchArgument( 22 | 'mm_file', 23 | default_value='' 24 | ) 25 | # (legacy) Format 2 in docs: 26 | map_yaml_file_arg = DeclareLaunchArgument( 27 | 'map_yaml_file', default_value='' 28 | ) 29 | # Format 3 in docs: 30 | mrpt_metricmap_file_arg = DeclareLaunchArgument( 31 | 'mrpt_metricmap_file', default_value='' 32 | ) 33 | 34 | # Others: 35 | frame_id_arg = DeclareLaunchArgument( 36 | 'frame_id', 37 | default_value='map' 38 | ) 39 | pub_mm_topic_arg = DeclareLaunchArgument( 40 | 'pub_mm_topic', 41 | default_value='mrpt_map' 42 | ) 43 | 44 | # Node: Map server 45 | mrpt_map_server_node = Node( 46 | package='mrpt_map_server', 47 | executable='map_server_node', 48 | name='map_server_node', 49 | output='screen', 50 | parameters=[ 51 | {'map_yaml_file': LaunchConfiguration('map_yaml_file')}, 52 | {'mm_file': LaunchConfiguration('mm_file')}, 53 | {'frame_id': LaunchConfiguration('frame_id')}, 54 | {'mrpt_metricmap_file': LaunchConfiguration('mrpt_metricmap_file')}, 55 | {'pub_mm_topic': LaunchConfiguration('pub_mm_topic')}, 56 | ], 57 | ) 58 | 59 | return LaunchDescription([ 60 | map_yaml_file_arg, 61 | mm_file_arg, 62 | mrpt_metricmap_file_arg, 63 | frame_id_arg, 64 | pub_mm_topic_arg, 65 | mrpt_map_server_node 66 | ]) 67 | -------------------------------------------------------------------------------- /mrpt_map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_map_server 4 | 2.2.2 5 | This package provides a ROS 2 node that publishes a static map for other nodes to use it. Unlike classic ROS 1 ``map_server``, this node can publish a range of different metric maps, not only occupancy grids. 6 | 7 | Markus Bader 8 | Jose Luis Blanco-Claraco 9 | Shravan S Rai 10 | 11 | BSD 12 | https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_map_server 13 | 14 | ament_cmake 15 | 16 | mp2p_icp 17 | mrpt_libobs 18 | mrpt_libmaps 19 | mrpt_libros_bridge 20 | mrpt_msgs 21 | mrpt_nav_interfaces 22 | rclcpp_components 23 | tf2 24 | tf2_ros 25 | tf2_geometry_msgs 26 | 27 | ament_cmake_lint_cmake 28 | ament_cmake_xmllint 29 | ament_lint_auto 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /mrpt_map_server/params/README.md: -------------------------------------------------------------------------------- 1 | This directory holds configuration files that may be used "as is" or with very few modifications 2 | directly by end users, so it makes sense to distribute them along the package. 3 | 4 | Other more specific examples may go into the mrpt_tutorials package instead. 5 | -------------------------------------------------------------------------------- /mrpt_map_server/params/metric-map-gridmap.ini: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # 3 | # MULTIMETRIC MAP CONFIGURATION 4 | # 5 | # See docs: 6 | # https://docs.mrpt.org/reference/latest/class_mrpt_maps_CMultiMetricMap.html 7 | # 8 | # ============================================================================ 9 | [MetricMap] 10 | # Creation of maps: 11 | occupancyGrid_count=1 12 | gasGrid_count=0 13 | landmarksMap_count=0 14 | pointsMap_count=0 15 | beaconMap_count=0 16 | 17 | # Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3) 18 | likelihoodMapSelection=-1 19 | 20 | # ==================================================== 21 | # MULTIMETRIC MAP: OccGrid #00 22 | # ==================================================== 23 | # Creation Options for OccupancyGridMap 00: 24 | [MetricMap_occupancyGrid_00_creationOpts] 25 | resolution=0.06 26 | 27 | # Insertion Options for OccupancyGridMap 00: 28 | [MetricMap_occupancyGrid_00_insertOpts] 29 | mapAltitude=0 30 | useMapAltitude=0 31 | maxDistanceInsertion=15 32 | maxOccupancyUpdateCertainty=0.55 33 | considerInvalidRangesAsFreeSpace=1 34 | minLaserScanNoiseStd=0.001 35 | 36 | # Likelihood Options for OccupancyGridMap 00: 37 | [MetricMap_occupancyGrid_00_likelihoodOpts] 38 | likelihoodMethod=4 // 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Thrun, 5=LF_II 39 | 40 | LF_decimation=20 41 | LF_stdHit=0.20 42 | LF_maxCorrsDistance=0.30 43 | LF_zHit=0.95 44 | LF_zRandom=0.05 45 | LF_maxRange=80 46 | LF_alternateAverageMethod=0 47 | 48 | MI_exponent=10 49 | MI_skip_rays=10 50 | MI_ratio_max_distance=2 51 | 52 | rayTracing_useDistanceFilter=0 53 | rayTracing_decimation=10 54 | rayTracing_stdHit=0.30 55 | 56 | consensus_takeEachRange=30 57 | consensus_pow=1 58 | 59 | 60 | -------------------------------------------------------------------------------- /mrpt_msgs_bridge/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_msgs_bridge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | * FIX: remove usage of obsolete ament_target_dependencies() 8 | * Contributors: Jose Luis Blanco-Claraco 9 | 10 | 2.2.1 (2024-10-12) 11 | ------------------ 12 | 13 | 2.2.0 (2024-09-25) 14 | ------------------ 15 | * fix missing linters; tune tutorial params 16 | * Update URL entries in package.xml to each package proper documentation 17 | * ament linters: manually enable just cmake and xml linters 18 | * reformat clang-format with 100 column width 19 | * Contributors: Jose Luis Blanco-Claraco 20 | 21 | 2.1.1 (2024-09-02) 22 | ------------------ 23 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 24 | * Fix duplicated deps 25 | * update dependencies 26 | * Depend on new mrpt_lib packages (deprecate mrpt2) 27 | * Contributors: Jose Luis Blanco-Claraco 28 | 29 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 30 | * Fix duplicated deps 31 | * update dependencies 32 | * Depend on new mrpt_lib packages (deprecate mrpt2) 33 | * Contributors: Jose Luis Blanco-Claraco 34 | 35 | 2.1.0 (2024-08-08) 36 | ------------------ 37 | * Merge branch 'ros2' into wip/port-tps-astar 38 | * Contributors: Jose Luis Blanco-Claraco 39 | 40 | 2.0.1 (2024-05-28) 41 | ------------------ 42 | 43 | 2.0.0 (2024-05-28) 44 | ------------------ 45 | * Prepare demo launch files 46 | * Port mrpt localization for ros2 and whole refactor 47 | * Add ament linter for testing builds 48 | * Make marker_msgs an optional dependency. 49 | Closes `#138 `_ 50 | * Unify and clarify license headers in all files 51 | * ROS2 port: mrpt_msgs_bridge 52 | * Contributors: Jose Luis Blanco-Claraco, SRai22 53 | 54 | 1.0.3 (2022-06-25) 55 | ------------------ 56 | * Fix CMake script error due to last commit typo 57 | * Contributors: Jose Luis Blanco Claraco 58 | 59 | 1.0.2 (2022-06-25) 60 | ------------------ 61 | * Fix wrong destination for mrpt_msgs_bridge headers 62 | * Contributors: Jose Luis Blanco Claraco 63 | 64 | 1.0.1 (2022-06-24) 65 | ------------------ 66 | * New package mrpt_msgs_bridge 67 | * Contributors: Jose Luis Blanco-Claraco 68 | -------------------------------------------------------------------------------- /mrpt_msgs_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mrpt_msgs_bridge) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | find_package(geometry_msgs REQUIRED) 8 | find_package(tf2 REQUIRED) 9 | find_package(mrpt_msgs REQUIRED) 10 | 11 | # Optional: 12 | find_package(marker_msgs QUIET) 13 | if(marker_msgs_FOUND) 14 | set(MARKER_SRC src/marker_msgs.cpp) 15 | endif() 16 | 17 | find_package(mrpt-ros2bridge REQUIRED) 18 | find_package(mrpt-obs REQUIRED) 19 | 20 | message(STATUS "MRPT_VERSION: ${mrpt-obs_VERSION}") 21 | 22 | if(CMAKE_COMPILER_IS_GNUCXX) 23 | # High level of warnings. 24 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 25 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 26 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 27 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 28 | if(NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 29 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 30 | endif() 31 | endif() 32 | 33 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 34 | add_compile_options(-O3) 35 | endif() 36 | 37 | ########### 38 | ## Build ## 39 | ########### 40 | 41 | ## Specify additional locations of header files 42 | ## Your package locations should be listed before other locations 43 | include_directories(include ${catkin_INCLUDE_DIRS}) 44 | 45 | # Declare a cpp library 46 | add_library(${PROJECT_NAME} 47 | src/beacon.cpp 48 | ${MARKER_SRC} 49 | src/landmark.cpp 50 | src/network_of_poses.cpp 51 | ) 52 | 53 | 54 | ############# 55 | ## Install ## 56 | ############# 57 | 58 | # Mark executables and/or libraries for installation 59 | install(TARGETS ${PROJECT_NAME} 60 | DESTINATION lib/${PROJECT_NAME} 61 | ) 62 | 63 | # Mark cpp header files for installation 64 | install(DIRECTORY include/${PROJECT_NAME} 65 | DESTINATION include 66 | FILES_MATCHING PATTERN "*.hpp" 67 | ) 68 | 69 | # Specify libraries to link a library or executable target against 70 | target_link_libraries(${PROJECT_NAME} PRIVATE 71 | mrpt::obs 72 | mrpt::ros2bridge 73 | rclcpp::rclcpp 74 | tf2::tf2 75 | ${mrpt_msgs_TARGETS} 76 | ${marker_msgs_TARGETS} 77 | ) 78 | 79 | # Export modern CMake targets 80 | ament_export_targets(${PROJECT_NAME}) 81 | 82 | ament_export_dependencies() 83 | 84 | install( 85 | TARGETS ${PROJECT_NAME} 86 | EXPORT ${PROJECT_NAME} 87 | ARCHIVE DESTINATION lib 88 | LIBRARY DESTINATION lib 89 | RUNTIME DESTINATION bin 90 | INCLUDES DESTINATION include 91 | ) 92 | 93 | if(BUILD_TESTING) 94 | find_package(ament_lint_auto REQUIRED) 95 | find_package(ament_cmake_lint_cmake REQUIRED) 96 | find_package(ament_cmake_xmllint REQUIRED) 97 | 98 | # the following line skips the linter which checks for copyrights 99 | # uncomment the line when a copyright and license is not present in all source files 100 | #set(ament_cmake_copyright_FOUND TRUE) 101 | # the following line skips cpplint (only works in a git repo) 102 | # uncomment the line when this package is not in a git repo 103 | #set(ament_cmake_cpplint_FOUND TRUE) 104 | ament_lint_auto_find_test_dependencies() 105 | endif() 106 | 107 | ament_package() 108 | -------------------------------------------------------------------------------- /mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | #pragma once 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace mrpt_msgs_bridge 19 | { 20 | /** @name ObservationRangeBeacon: ROS2 <-> MRPT 21 | * @{ */ 22 | 23 | /** ROS->MRPT: Takes a mrpt_msgs::ObservationRangeBeacon and the relative pose 24 | * of the range sensor wrt base_link and builds a CObservationBeaconRanges 25 | * \return true on sucessful conversion, false on any error. 26 | * \sa mrpt2ros 27 | */ 28 | bool fromROS( 29 | const mrpt_msgs::msg::ObservationRangeBeacon _msg, const mrpt::poses::CPose3D& _pose, 30 | mrpt::obs::CObservationBeaconRanges& _obj); 31 | 32 | /** MRPT->ROS2: Takes a CObservationBeaconRanges and outputs range data in 33 | * mrpt_msgs::ObservationRangeBeacon \return true on sucessful conversion, false 34 | * on any error. \sa ros2mrpt 35 | */ 36 | bool toROS( 37 | const mrpt::obs::CObservationBeaconRanges& _obj, mrpt_msgs::msg::ObservationRangeBeacon& _msg); 38 | 39 | /** MRPT->ROS2: Takes a CObservationBeaconRanges and outputs range data in 40 | * mrpt_msgs::ObservationRangeBeacon + the relative pose of the range sensor wrt 41 | * base_link \return true on sucessful conversion, false on any error. \sa 42 | * ros2mrpt 43 | */ 44 | bool toROS( 45 | const mrpt::obs::CObservationBeaconRanges& _obj, mrpt_msgs::msg::ObservationRangeBeacon& _msg, 46 | geometry_msgs::msg::Pose& _pose); 47 | 48 | /** @} */ 49 | 50 | } // namespace mrpt_msgs_bridge -------------------------------------------------------------------------------- /mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | /* 10 | * File: landmark.h 11 | * Author: Vladislav Tananaev 12 | * 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace mrpt_msgs_bridge 25 | { 26 | /** @name LaserScan: ROS2 <-> MRPT 27 | * @{ */ 28 | 29 | /** ROS2->MRPT: Takes a mrpt_msgs::CObservationBearingRange and the relative 30 | * pose of the sensor wrt base_link and builds a ObservationRangeBearing \return 31 | * true on sucessful conversion, false on any error. \sa mrpt2ros 32 | */ 33 | bool fromROS( 34 | const mrpt_msgs::msg::ObservationRangeBearing& _msg, const mrpt::poses::CPose3D& _pose, 35 | mrpt::obs::CObservationBearingRange& _obj); 36 | 37 | /** MRPT->ROS2: Takes a CObservationBearingRange and outputs range data in 38 | * mrpt_msgs::ObservationRangeBearing 39 | * \return true on sucessful conversion, false on any error. 40 | * \sa ros2mrpt 41 | */ 42 | bool toROS( 43 | const mrpt::obs::CObservationBearingRange& _obj, mrpt_msgs::msg::ObservationRangeBearing& _msg); 44 | 45 | /** MRPT->ROS2: Takes a CObservationBearingRange and outputs range data in 46 | * mrpt_msgs::ObservationRangeBearing + the relative pose of the range sensor 47 | * wrt base_link 48 | * \return true on sucessful conversion, false on any error. 49 | * \sa ros2mrpt 50 | */ 51 | bool toROS( 52 | const mrpt::obs::CObservationBearingRange& _obj, mrpt_msgs::msg::ObservationRangeBearing& _msg, 53 | geometry_msgs::msg::Pose& sensorPose); 54 | 55 | /** @} */ 56 | 57 | } // namespace mrpt_msgs_bridge -------------------------------------------------------------------------------- /mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | /** 10 | * @file marker_msgs.h 11 | * @author Markus Bader 12 | * @date September, 2017 13 | * @brief Funtions to convert marker_msgs to mrpt msgs 14 | **/ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace mrpt_msgs_bridge 28 | { 29 | // NOTE: These converters are here instead of mrpt::ros2bridge since 30 | // the builds dep. MarkerDetection is not available in Debian/Ubuntu 31 | // official repos, so we need to build it here within ROS. 32 | 33 | bool fromROS( 34 | const marker_msgs::msg::MarkerDetection& _msg, const mrpt::poses::CPose3D& sensorPoseOnRobot, 35 | mrpt::obs::CObservationBearingRange& _obj); 36 | 37 | bool fromROS( 38 | const marker_msgs::msg::MarkerDetection& _msg, const mrpt::poses::CPose3D& sensorPoseOnRobot, 39 | mrpt::obs::CObservationBeaconRanges& _obj); 40 | 41 | } // namespace mrpt_msgs_bridge -------------------------------------------------------------------------------- /mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | /**\brief File includes methods for converting CNetworkOfPoses*DInf <=> 10 | * NetworkOfPoses message types 11 | */ 12 | 13 | #pragma once 14 | 15 | #include 16 | 17 | #include 18 | 19 | namespace mrpt_msgs_bridge 20 | { 21 | /**\name MRPT \rightarrow ROS2 conversions */ 22 | /**\brief Convert [MRPT] CNetworkOfPoses*D \rightarrow [ROS2] NetworkOfPoses. 23 | * \param[in] mrpt_graph MRPT graph representation 24 | * \param[out] ros_graph ROS graph representation 25 | */ 26 | /**\{*/ 27 | 28 | // TODO - convert these methods into a common polymorphic method 29 | void toROS( 30 | const mrpt::graphs::CNetworkOfPoses2D& mrpt_graph, mrpt_msgs::msg::NetworkOfPoses& ros_graph); 31 | 32 | void toROS( 33 | const mrpt::graphs::CNetworkOfPoses3D& mrpt_graph, mrpt_msgs::msg::NetworkOfPoses& ros_graph); 34 | 35 | void toROS( 36 | const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph, 37 | mrpt_msgs::msg::NetworkOfPoses& ros_graph); 38 | 39 | void toROS( 40 | const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph, 41 | mrpt_msgs::msg::NetworkOfPoses& ros_graph); 42 | 43 | void toROS( 44 | const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph, 45 | mrpt_msgs::msg::NetworkOfPoses& ros_graph); 46 | 47 | void toROS( 48 | const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph, 49 | mrpt_msgs::msg::NetworkOfPoses& ros_graph); 50 | 51 | /**\} */ 52 | 53 | ///////////////////////////////////////////////////////////////////////// 54 | 55 | /**\name ROS2 \rightarrow MRPT conversions */ 56 | /**\brief Convert [ROS2] NetworkOfPoses \rightarrow [MRPT] CNetworkOfPoses*DInf. 57 | * \param[in] mrpt_graph ROS2 graph representation 58 | * \param[out] ros_graph MRPT graph representation 59 | */ 60 | /**\{ */ 61 | void fromROS( 62 | const mrpt::graphs::CNetworkOfPoses2D& mrpt_graph, mrpt_msgs::msg::NetworkOfPoses& ros_graph); 63 | 64 | void fromROS( 65 | const mrpt::graphs::CNetworkOfPoses3D& mrpt_graph, mrpt_msgs::msg::NetworkOfPoses& ros_graph); 66 | 67 | void fromROS( 68 | const mrpt_msgs::msg::NetworkOfPoses& ros_graph, 69 | mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph); 70 | 71 | void fromROS( 72 | const mrpt_msgs::msg::NetworkOfPoses& ros_graph, 73 | mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph); 74 | 75 | void fromROS( 76 | const mrpt_msgs::msg::NetworkOfPoses& ros_graph, 77 | mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph); 78 | 79 | void fromROS( 80 | const mrpt_msgs::msg::NetworkOfPoses& ros_graph, 81 | mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph); 82 | 83 | /**\} */ 84 | 85 | } // namespace mrpt_msgs_bridge -------------------------------------------------------------------------------- /mrpt_msgs_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_msgs_bridge 4 | 2.2.2 5 | C++ library to convert between custom mrpt_msgs messages and native MRPT classes 6 | 7 | https://github.com/mrpt-ros-pkg/mrpt_navigation 8 | José Luis Blanco-Claraco 9 | Shravan S Rai 10 | 11 | José Luis Blanco-Claraco 12 | Shravan S Rai 13 | 14 | BSD 15 | https://github.com/mrpt-ros-pkg/mrpt_navigation 16 | 17 | ros_environment 18 | ament_cmake 19 | 20 | geometry_msgs 21 | mrpt_libobs 22 | mrpt_libros_bridge 23 | rclcpp 24 | tf2 25 | 26 | mrpt_msgs 27 | 28 | 29 | 30 | ament_cmake_lint_cmake 31 | ament_cmake_xmllint 32 | ament_lint_auto 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /mrpt_msgs_bridge/src/beacon.cpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "tf2/LinearMath/Matrix3x3.h" 15 | #include "tf2/transform_datatypes.h" 16 | 17 | using namespace mrpt::obs; 18 | 19 | bool mrpt_msgs_bridge::fromROS( 20 | const mrpt_msgs::msg::ObservationRangeBeacon _msg, const mrpt::poses::CPose3D& _pose, 21 | CObservationBeaconRanges& _obj) 22 | { 23 | _obj.timestamp = mrpt::ros2bridge::fromROS(_msg.header.stamp); 24 | mrpt::poses::CPose3D cpose_obj; 25 | 26 | _obj.stdError = _msg.sensor_std_range; 27 | _obj.sensorLabel = _msg.header.frame_id; 28 | _obj.maxSensorDistance = _msg.max_sensor_distance; 29 | _obj.minSensorDistance = _msg.min_sensor_distance; 30 | 31 | if (_pose.empty()) 32 | { 33 | cpose_obj = mrpt::ros2bridge::fromROS(_msg.sensor_pose_on_robot); 34 | _obj.setSensorPose(cpose_obj); 35 | } 36 | else 37 | { 38 | _obj.setSensorPose(_pose); 39 | } 40 | 41 | ASSERT_(_msg.sensed_data.size() >= 1); 42 | const size_t N = _msg.sensed_data.size(); 43 | 44 | _obj.sensedData.resize(N); 45 | 46 | for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++) 47 | { 48 | _obj.sensedData[i_mrpt].sensedDistance = _msg.sensed_data[i_mrpt].range; 49 | _obj.sensedData[i_mrpt].beaconID = _msg.sensed_data[i_mrpt].id; 50 | } 51 | return true; 52 | } 53 | 54 | bool mrpt_msgs_bridge::toROS( 55 | const CObservationBeaconRanges& _obj, mrpt_msgs::msg::ObservationRangeBeacon& _msg) 56 | { 57 | mrpt::poses::CPose3D cpose_obj; 58 | 59 | _msg.header.stamp = mrpt::ros2bridge::toROS(_obj.timestamp); 60 | _obj.getSensorPose(cpose_obj); 61 | _msg.sensor_pose_on_robot = mrpt::ros2bridge::toROS_Pose(cpose_obj); 62 | 63 | _msg.sensor_std_range = _obj.stdError; 64 | _msg.header.frame_id = _obj.sensorLabel; 65 | _msg.max_sensor_distance = _obj.maxSensorDistance; 66 | _msg.min_sensor_distance = _obj.minSensorDistance; 67 | 68 | ASSERT_(_obj.sensedData.size() >= 1); 69 | const size_t N = _obj.sensedData.size(); 70 | 71 | _msg.sensed_data.resize(N); 72 | 73 | for (std::size_t i_msg = 0; i_msg < N; i_msg++) 74 | { 75 | _msg.sensed_data[i_msg].range = _obj.sensedData[i_msg].sensedDistance; 76 | _msg.sensed_data[i_msg].id = _obj.sensedData[i_msg].beaconID; 77 | } 78 | return true; 79 | } 80 | 81 | bool mrpt_msgs_bridge::toROS( 82 | const CObservationBeaconRanges& _obj, mrpt_msgs::msg::ObservationRangeBeacon& _msg, 83 | geometry_msgs::msg::Pose& _pose) 84 | { 85 | toROS(_obj, _msg); 86 | mrpt::poses::CPose3D pose; 87 | _obj.getSensorPose(pose); 88 | _pose = mrpt::ros2bridge::toROS_Pose(pose); 89 | return true; 90 | } -------------------------------------------------------------------------------- /mrpt_msgs_bridge/src/landmark.cpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | /* 10 | * File: landmark.cpp 11 | * Author: Vladislav Tananaev 12 | * 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | bool mrpt_msgs_bridge::fromROS( 21 | const mrpt_msgs::msg::ObservationRangeBearing& _msg, const mrpt::poses::CPose3D& _pose, 22 | mrpt::obs::CObservationBearingRange& _obj) 23 | 24 | { 25 | _obj.timestamp = mrpt::ros2bridge::fromROS(_msg.header.stamp); 26 | 27 | mrpt::poses::CPose3D cpose_obj; 28 | 29 | //_obj.stdError = _msg.sensor_std_range; 30 | //_obj.sensorLabel = _msg.header.frame_id; 31 | _obj.maxSensorDistance = _msg.max_sensor_distance; 32 | _obj.minSensorDistance = _msg.min_sensor_distance; 33 | _obj.sensor_std_yaw = _msg.sensor_std_yaw; 34 | _obj.sensor_std_pitch = _msg.sensor_std_pitch; 35 | _obj.sensor_std_range = _msg.sensor_std_range; 36 | 37 | if (_pose.empty()) 38 | { 39 | _obj.setSensorPose(mrpt::ros2bridge::fromROS(_msg.sensor_pose_on_robot)); 40 | } 41 | else 42 | { 43 | _obj.setSensorPose(_pose); 44 | } 45 | 46 | ASSERT_(_msg.sensed_data.size() >= 1); 47 | const size_t N = _msg.sensed_data.size(); 48 | 49 | _obj.sensedData.resize(N); 50 | 51 | for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++) 52 | { 53 | _obj.sensedData[i_mrpt].range = _msg.sensed_data[i_mrpt].range; 54 | _obj.sensedData[i_mrpt].landmarkID = _msg.sensed_data[i_mrpt].id; 55 | _obj.sensedData[i_mrpt].yaw = _msg.sensed_data[i_mrpt].yaw; 56 | _obj.sensedData[i_mrpt].pitch = _msg.sensed_data[i_mrpt].pitch; 57 | } 58 | return true; 59 | } 60 | 61 | bool mrpt_msgs_bridge::toROS( 62 | const mrpt::obs::CObservationBearingRange& _obj, mrpt_msgs::msg::ObservationRangeBearing& _msg) 63 | { 64 | _msg.header.stamp = mrpt::ros2bridge::toROS(_obj.timestamp); 65 | 66 | _msg.sensor_pose_on_robot = mrpt::ros2bridge::toROS_Pose(_obj.sensorPose()); 67 | 68 | _msg.max_sensor_distance = _obj.maxSensorDistance; 69 | _msg.min_sensor_distance = _obj.minSensorDistance; 70 | _msg.sensor_std_yaw = _obj.sensor_std_yaw; 71 | _msg.sensor_std_pitch = _obj.sensor_std_pitch; 72 | _msg.sensor_std_range = _obj.sensor_std_range; 73 | 74 | ASSERT_(_obj.sensedData.size() >= 1); 75 | const size_t N = _obj.sensedData.size(); 76 | 77 | _msg.sensed_data.resize(N); 78 | 79 | for (std::size_t i_msg = 0; i_msg < N; i_msg++) 80 | { 81 | _msg.sensed_data[i_msg].range = _obj.sensedData[i_msg].range; 82 | _msg.sensed_data[i_msg].id = _obj.sensedData[i_msg].landmarkID; 83 | _msg.sensed_data[i_msg].yaw = _obj.sensedData[i_msg].yaw; 84 | _msg.sensed_data[i_msg].pitch = _obj.sensedData[i_msg].pitch; 85 | } 86 | return true; 87 | } 88 | 89 | bool mrpt_msgs_bridge::toROS( 90 | const mrpt::obs::CObservationBearingRange& _obj, mrpt_msgs::msg::ObservationRangeBearing& _msg, 91 | geometry_msgs::msg::Pose& sensorPose) 92 | { 93 | toROS(_obj, _msg); 94 | sensorPose = mrpt::ros2bridge::toROS_Pose(_obj.sensorPose()); 95 | return true; 96 | } -------------------------------------------------------------------------------- /mrpt_msgs_bridge/src/marker_msgs.cpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | /** 10 | * @file marker_msgs.cpp 11 | * @author Markus Bader 12 | * @date September, 2017 13 | * @brief Funtions to convert marker_msgs to mrpt msgs 14 | **/ 15 | 16 | #include 17 | 18 | #include 19 | 20 | bool mrpt_msgs_bridge::fromROS( 21 | const marker_msgs::msg::MarkerDetection& src, const mrpt::poses::CPose3D& sensorPoseOnRobot, 22 | mrpt::obs::CObservationBearingRange& des) 23 | { 24 | des.timestamp = mrpt::ros2bridge::fromROS(src.header.stamp); 25 | des.setSensorPose(sensorPoseOnRobot); 26 | des.minSensorDistance = src.distance_min; 27 | des.maxSensorDistance = src.distance_max; 28 | 29 | des.sensedData.resize(src.markers.size()); 30 | for (size_t i = 0; i < src.markers.size(); i++) 31 | { 32 | const marker_msgs::msg::Marker& marker = src.markers[i]; 33 | // mrpt::obs::CObservationBearingRange::TMeasurement 34 | auto& m = des.sensedData[i]; 35 | m.range = sqrt( 36 | marker.pose.position.x * marker.pose.position.x + 37 | marker.pose.position.y * marker.pose.position.y); 38 | m.yaw = atan2(marker.pose.position.y, marker.pose.position.x); 39 | m.pitch = 0.0; 40 | if (marker.ids.size() > 0) 41 | { 42 | m.landmarkID = marker.ids[0]; 43 | } 44 | else 45 | { 46 | m.landmarkID = -1; 47 | } 48 | } 49 | return true; 50 | } 51 | 52 | bool mrpt_msgs_bridge::fromROS( 53 | const marker_msgs::msg::MarkerDetection& src, const mrpt::poses::CPose3D& sensorPoseOnRobot, 54 | mrpt::obs::CObservationBeaconRanges& des) 55 | { 56 | des.timestamp = mrpt::ros2bridge::fromROS(src.header.stamp); 57 | 58 | des.setSensorPose(sensorPoseOnRobot); 59 | des.minSensorDistance = src.distance_min; 60 | des.maxSensorDistance = src.distance_max; 61 | 62 | des.sensedData.resize(src.markers.size()); 63 | for (size_t i = 0; i < src.markers.size(); i++) 64 | { 65 | const marker_msgs::msg::Marker& marker = src.markers[i]; 66 | // mrpt::obs::CObservationBeaconRanges::TMeasurement 67 | auto& m = des.sensedData[i]; 68 | m.sensedDistance = sqrt( 69 | marker.pose.position.x * marker.pose.position.x + 70 | marker.pose.position.y * marker.pose.position.y); 71 | m.sensorLocationOnRobot.m_coords = sensorPoseOnRobot.m_coords; 72 | if (marker.ids.size() > 0) 73 | { 74 | m.beaconID = marker.ids[0]; 75 | } 76 | else 77 | { 78 | m.beaconID = -1; 79 | } 80 | } 81 | return true; 82 | } -------------------------------------------------------------------------------- /mrpt_nav_interfaces/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_nav_interfaces 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | 8 | 2.2.1 (2024-10-12) 9 | ------------------ 10 | 11 | 2.2.0 (2024-09-25) 12 | ------------------ 13 | * Merge pull request `#149 `_ from mrpt-ros-pkg/feature/utm-coordinates 14 | Support UTM global coordinates for geo-referenciated maps 15 | * Add new msg GeoreferencingMetadata.msg 16 | * Update URL entries in package.xml to each package proper documentation 17 | * Contributors: Jose Luis Blanco-Claraco 18 | 19 | 2.1.1 (2024-09-02) 20 | ------------------ 21 | 22 | 2.1.0 (2024-08-08) 23 | ------------------ 24 | * Add two new service definitions: MakePlanTo, MakePlanFromTo 25 | * Merge branch 'ros2' into wip/port-tps-astar 26 | * Contributors: Jose Luis Blanco-Claraco 27 | 28 | 2.0.1 (2024-05-28) 29 | ------------------ 30 | * fix missing on action_msgs (should fix build on Humble) 31 | * Contributors: Jose Luis Blanco-Claraco 32 | 33 | 2.0.0 (2024-05-28) 34 | ------------------ 35 | * Define map_server services 36 | * Add new package mrpt_nav_interfaces with action and msg defintions for navigation 37 | * Contributors: Jose Luis Blanco-Claraco 38 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(mrpt_nav_interfaces) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(rosidl_default_generators REQUIRED) 16 | find_package(mrpt_msgs REQUIRED) 17 | find_package(nav_msgs REQUIRED) 18 | find_package(geometry_msgs REQUIRED) 19 | 20 | rosidl_generate_interfaces(${PROJECT_NAME} 21 | action/NavigateGoal.action 22 | action/NavigateWaypoints.action 23 | msg/GeoreferencingMetadata.msg 24 | msg/NavigationFeedback.msg 25 | msg/NavigationFinalStatus.msg 26 | srv/GetLayers.srv 27 | srv/GetGridmapLayer.srv 28 | srv/GetPointmapLayer.srv 29 | srv/MakePlanFromTo.srv 30 | srv/MakePlanTo.srv 31 | DEPENDENCIES 32 | std_msgs 33 | nav_msgs 34 | mrpt_msgs 35 | geometry_msgs 36 | ) 37 | 38 | ament_export_dependencies(rosidl_default_runtime) 39 | ament_package() 40 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/action/NavigateGoal.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/PoseStamped target 3 | --- 4 | # Result 5 | mrpt_nav_interfaces/NavigationFinalStatus state 6 | --- 7 | # Feedback 8 | mrpt_nav_interfaces/NavigationFeedback state 9 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/action/NavigateWaypoints.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | mrpt_msgs/WaypointSequence waypoints 3 | --- 4 | # Result 5 | mrpt_nav_interfaces/NavigationFinalStatus state 6 | --- 7 | # Feedback 8 | mrpt_nav_interfaces/NavigationFeedback state 9 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/msg/GeoreferencingMetadata.msg: -------------------------------------------------------------------------------- 1 | # For a diagram of the frames defined in this message, refer to online docs: 2 | # https://docs.mola-slam.org/latest/geo-referencing.html 3 | 4 | # This is true only if the map is georeferenced. If it is false, 5 | # the rest of the fields in this message should be ignored. 6 | bool valid 7 | 8 | geometry_msgs/PoseWithCovariance t_enu_to_map 9 | geometry_msgs/Pose t_enu_to_utm 10 | 11 | # The geodetic coordinates of the ENU reference frame: 12 | float64 latitude 13 | float64 longitude 14 | float64 height 15 | 16 | # UTM zone, for the ENU point. 17 | int32 utm_zone 18 | string utm_band # just one letter (or empty if valid=false) 19 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/msg/NavigationFeedback.msg: -------------------------------------------------------------------------------- 1 | int16 total_waypoints 2 | int16 reached_waypoints 3 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/msg/NavigationFinalStatus.msg: -------------------------------------------------------------------------------- 1 | int8 navigation_status 2 | 3 | int8 STATUS_SUCCESS=0 4 | int8 STATUS_ERROR=1 5 | int8 STATUS_CANCELLED=2 6 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mrpt_nav_interfaces 5 | 2.2.2 6 | Message, services, and actions, for other mrpt navigation packages. 7 | 8 | Jose Luis Blanco-Claraco 9 | Jose Luis Blanco-Claraco 10 | BSD 11 | https://github.com/mrpt-ros-pkg/mrpt_navigation 12 | 13 | ament_cmake 14 | 15 | rosidl_default_generators 16 | 17 | action_msgs 18 | geometry_msgs 19 | mrpt_msgs 20 | nav_msgs 21 | 22 | rosidl_default_runtime 23 | 24 | rosidl_interface_packages 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/srv/GetGridmapLayer.srv: -------------------------------------------------------------------------------- 1 | string layer_name 2 | --- 3 | bool valid 4 | nav_msgs/OccupancyGrid grid -------------------------------------------------------------------------------- /mrpt_nav_interfaces/srv/GetLayers.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string[] layers 3 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/srv/GetPointmapLayer.srv: -------------------------------------------------------------------------------- 1 | string layer_name 2 | --- 3 | bool valid 4 | sensor_msgs/PointCloud2 points -------------------------------------------------------------------------------- /mrpt_nav_interfaces/srv/MakePlanFromTo.srv: -------------------------------------------------------------------------------- 1 | # This service requests a given server node to: 2 | # - Use all current obstacles from all configured obstacle sources in the node configuration, 3 | # - Make a plan to go from the given robot pose to the requested pose in this service request. 4 | 5 | # Start pose 6 | geometry_msgs/Pose start 7 | # Goal pose 8 | geometry_msgs/Pose target 9 | --- 10 | # Result 11 | bool valid_path_found 12 | mrpt_msgs/WaypointSequence waypoints 13 | -------------------------------------------------------------------------------- /mrpt_nav_interfaces/srv/MakePlanTo.srv: -------------------------------------------------------------------------------- 1 | # This service requests a given server node to: 2 | # - Use all current obstacles from all configured obstacle sources in the node configuration, 3 | # - Use /tf to find out the current robot pose, 4 | # - Make a plan to go from the current robot pose, to the requested pose in this service request. 5 | 6 | # Goal 7 | geometry_msgs/PoseStamped target 8 | --- 9 | # Result 10 | bool valid_path_found 11 | mrpt_msgs/WaypointSequence waypoints 12 | -------------------------------------------------------------------------------- /mrpt_navigation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_navigation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | 8 | 2.2.1 (2024-10-12) 9 | ------------------ 10 | 11 | 2.2.0 (2024-09-25) 12 | ------------------ 13 | * Update URL entries in package.xml to each package proper documentation 14 | * ament linters: manually enable just cmake and xml linters 15 | * Contributors: Jose Luis Blanco-Claraco 16 | 17 | 2.1.1 (2024-09-02) 18 | ------------------ 19 | 20 | 2.1.0 (2024-08-08) 21 | ------------------ 22 | * Merge branch 'ros2' into wip/port-tps-astar 23 | * Contributors: Jose Luis Blanco-Claraco 24 | 25 | 2.0.1 (2024-05-28) 26 | ------------------ 27 | 28 | 2.0.0 (2024-05-28) 29 | ------------------ 30 | * Add new package mrpt_nav_interfaces with action and msg defintions for navigation 31 | * Rename mrpt_map: mrpt_map_server 32 | * Renamed pkg mrpt_local_obstacles: mrpt_pointcloud_pipeline to reflect new capabilities 33 | * Fix missing build dep 34 | * Renamed: mrpt_localization: mrpt_pf_localization 35 | * Add ament linter for testing builds 36 | * ROS2 port metapackage 37 | * Contributors: Jose Luis Blanco-Claraco, SRai22 38 | 39 | 1.0.3 (2022-06-25) 40 | ------------------ 41 | 42 | 1.0.2 (2022-06-25) 43 | ------------------ 44 | 45 | 1.0.1 (2022-06-24) 46 | ------------------ 47 | 48 | 1.0.0 (2022-04-30) 49 | ------------------ 50 | * Update URLs to https 51 | * Contributors: Jose Luis Blanco Claraco 52 | 53 | 0.1.26 (2019-10-05) 54 | ------------------- 55 | 56 | 0.1.25 (2019-10-04) 57 | ------------------- 58 | 59 | 0.1.24 (2019-04-12) 60 | ------------------- 61 | 62 | 0.1.23 (2018-06-14) 63 | ------------------- 64 | * mrpt_bridge is out of this metapackage 65 | 66 | 0.1.22 (2018-05-22) 67 | ------------------- 68 | * fix all catkin_lint errors 69 | * remove pkgs from metapkg list 70 | * Contributors: Jose Luis Blanco-Claraco 71 | 72 | 0.1.21 (2018-04-27) 73 | ------------------- 74 | * Upgrade version 0.1.20 (`#99 `_) 75 | * Contributors: Hunter Laux 76 | 77 | 0.1.18 (2017-01-22) 78 | ------------------- 79 | 80 | 0.1.17 (2017-01-22) 81 | ------------------- 82 | * Remove all errors generated by catkin_lint and cleanup unused templates from CMakeLists.txt files 83 | * Contributors: Jorge Santos 84 | 85 | 0.1.16 (2016-12-13) 86 | ------------------- 87 | 88 | 0.1.15 (2016-11-06) 89 | ------------------- 90 | 91 | 0.1.14 (2016-09-12) 92 | ------------------- 93 | 94 | 0.1.13 (2016-09-03) 95 | ------------------- 96 | 97 | 0.1.12 (2016-09-03) 98 | ------------------- 99 | 100 | 0.1.11 (2016-08-21) 101 | ------------------- 102 | 103 | 0.1.10 (2016-08-05) 104 | ------------------- 105 | 106 | 0.1.9 (2016-08-05) 107 | ------------------ 108 | 109 | 0.1.8 (2016-06-29) 110 | ------------------ 111 | 112 | 0.1.7 (2016-06-20) 113 | ------------------ 114 | 115 | 0.1.6 (2016-03-20) 116 | ------------------ 117 | * New support for range-only (RO) localization 118 | * fix build against mrpt <1.3.0 119 | 120 | 0.1.5 (2015-04-29) 121 | ------------------ 122 | 123 | 0.1.4 (2014-12-27) 124 | ------------------ 125 | * Start new pkg mrpt_local_obstacles. 126 | Fixes in package.xml's 127 | * pose_cov_ops removed from mrpt_navigation metapkg 128 | * Contributors: Jose Luis Blanco 129 | 130 | 0.1.3 (2014-12-18) 131 | ------------------ 132 | 133 | 0.1.2 (2014-12-18) 134 | ------------------ 135 | 136 | 0.1.1 (2014-12-17) 137 | ------------------ 138 | * First public binary release. 139 | 140 | 0.1.0 (2014-12-17) 141 | ------------------ 142 | * Fix metapackage xml (remove no longer existing pkg) 143 | * Contributors: Jose Luis Blanco 144 | * consistent version numbers 145 | 146 | -------------------------------------------------------------------------------- /mrpt_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(mrpt_navigation) 4 | 5 | # Find dependencies 6 | find_package(ament_cmake REQUIRED) 7 | 8 | if(BUILD_TESTING) 9 | find_package(ament_lint_auto REQUIRED) 10 | # the following line skips the linter which checks for copyrights 11 | # uncomment the line when a copyright and license is not present in all source files 12 | #set(ament_cmake_copyright_FOUND TRUE) 13 | # the following line skips cpplint (only works in a git repo) 14 | # uncomment the line when this package is not in a git repo 15 | #set(ament_cmake_cpplint_FOUND TRUE) 16 | ament_lint_auto_find_test_dependencies() 17 | endif() 18 | 19 | # The following is needed for ROS 2's ament to generate the setup hooks 20 | ament_package() -------------------------------------------------------------------------------- /mrpt_navigation/README.rst: -------------------------------------------------------------------------------- 1 | MRPT 2 | -------------------- 3 | 4 | ROS wrappers for Mobile Robot Programming Toolkit (MRPT) classes and apps. 5 | Refer to http://wiki.ros.org/mrpt_navigation for further documentation. 6 | 7 | -------------------------------------------------------------------------------- /mrpt_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mrpt_navigation 5 | 2.2.2 6 | 7 | Tools related to the Mobile Robot Programming Toolkit (MRPT). 8 | Refer to https://wiki.ros.org/mrpt_navigation for further documentation. 9 | 10 | Jose-Luis Blanco-Claraco 11 | Markus Bader 12 | Jose-Luis Blanco-Claraco 13 | Markus Bader 14 | BSD 15 | https://github.com/mrpt-ros-pkg/mrpt_navigation 16 | 17 | ament_cmake 18 | 19 | mrpt_pointcloud_pipeline 20 | mrpt_pf_localization 21 | mrpt_map_server 22 | mrpt_rawlog 23 | mrpt_reactivenav2d 24 | mrpt_tutorials 25 | mrpt_nav_interfaces 26 | 27 | ament_cmake_lint_cmake 28 | ament_cmake_xmllint 29 | ament_lint_auto 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /mrpt_pf_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mrpt_pf_localization) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | find_package(geometry_msgs REQUIRED) 8 | find_package(tf2 REQUIRED) 9 | find_package(std_msgs REQUIRED) 10 | find_package(nav_msgs REQUIRED) 11 | find_package(mrpt_msgs REQUIRED) 12 | find_package(pose_cov_ops REQUIRED) 13 | find_package(mrpt_msgs_bridge REQUIRED) 14 | find_package(sensor_msgs REQUIRED) 15 | find_package(tf2_geometry_msgs REQUIRED) 16 | find_package(mp2p_icp_map REQUIRED) 17 | find_package(mp2p_icp_filters REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(rclcpp_components REQUIRED) 20 | find_package(mola_relocalization) # OPTIONAL 21 | 22 | find_package(mrpt-ros2bridge REQUIRED) 23 | find_package(mrpt-gui REQUIRED) 24 | find_package(mrpt-slam REQUIRED) 25 | 26 | message(STATUS "MRPT_VERSION: ${mrpt-slam_VERSION}") 27 | 28 | if (CMAKE_COMPILER_IS_GNUCXX) 29 | # High level of warnings. 30 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 31 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 32 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 33 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 34 | if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 35 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 36 | endif() 37 | endif() 38 | 39 | 40 | message(STATUS "mola_relocalization_FOUND: ${mola_relocalization_FOUND}") 41 | 42 | ########### 43 | ## Build ## 44 | ########### 45 | 46 | # non-ROS C++ library: 47 | add_library(${PROJECT_NAME}_core SHARED 48 | src/${PROJECT_NAME}/${PROJECT_NAME}_core.cpp 49 | include/${PROJECT_NAME}/${PROJECT_NAME}_core.h 50 | ) 51 | 52 | target_include_directories(${PROJECT_NAME}_core 53 | PUBLIC 54 | $ 55 | $ 56 | ) 57 | 58 | target_link_libraries(${PROJECT_NAME}_core 59 | mrpt::gui 60 | mrpt::slam 61 | mrpt::ros2bridge 62 | mola::mp2p_icp_map 63 | mola::mp2p_icp_filters 64 | ) 65 | if (mola_relocalization_FOUND) 66 | target_link_libraries(${PROJECT_NAME}_core 67 | mola::mola_relocalization 68 | ) 69 | target_compile_definitions(${PROJECT_NAME}_core PRIVATE HAVE_MOLA_RELOCALIZATION) 70 | endif() 71 | 72 | 73 | # ROS node: 74 | add_executable(${PROJECT_NAME}_node 75 | src/main.cpp 76 | include/${PROJECT_NAME}_node.h 77 | ) 78 | 79 | target_link_libraries(${PROJECT_NAME}_node PRIVATE 80 | ${PROJECT_NAME}_core 81 | mrpt::gui 82 | mrpt::slam 83 | mrpt::ros2bridge 84 | rclcpp::rclcpp 85 | ${rclcpp_components_TARGETS} 86 | tf2::tf2 87 | ${pose_cov_ops_TARGETS} 88 | ${geometry_msgs_TARGETS} 89 | ${mrpt_msgs_TARGETS} 90 | ${mrpt_msgs_bridge_TARGETS} 91 | ${nav_msgs_TARGETS} 92 | ${sensor_msgs_TARGETS} 93 | ${tf2_geometry_msgs_TARGETS} 94 | ) 95 | 96 | target_include_directories(${PROJECT_NAME}_node 97 | PRIVATE 98 | include 99 | ) 100 | 101 | ####################### 102 | # ROS composable node # 103 | ####################### 104 | 105 | add_library(${PROJECT_NAME}_component SHARED 106 | src/${PROJECT_NAME}_component.cpp 107 | include/${PROJECT_NAME}_node.h 108 | ) 109 | 110 | target_link_libraries(${PROJECT_NAME}_component PRIVATE 111 | ${PROJECT_NAME}_core 112 | mrpt::gui 113 | mrpt::slam 114 | mrpt::ros2bridge 115 | rclcpp::rclcpp 116 | ${rclcpp_components_TARGETS} 117 | tf2::tf2 118 | ${pose_cov_ops_TARGETS} 119 | ${geometry_msgs_TARGETS} 120 | ${mrpt_msgs_TARGETS} 121 | ${mrpt_msgs_bridge_TARGETS} 122 | ${nav_msgs_TARGETS} 123 | ${sensor_msgs_TARGETS} 124 | ${tf2_geometry_msgs_TARGETS} 125 | ) 126 | 127 | target_include_directories(${PROJECT_NAME}_component 128 | PRIVATE 129 | include 130 | ) 131 | 132 | rclcpp_components_register_node( 133 | ${PROJECT_NAME}_component 134 | PLUGIN "PFLocalizationNode" 135 | EXECUTABLE ${PROJECT_NAME}_composable 136 | ) 137 | 138 | ament_export_targets(export_${PROJECT_NAME}) 139 | 140 | ############# 141 | ## Install ## 142 | ############# 143 | 144 | install(TARGETS ${PROJECT_NAME}_component ${PROJECT_NAME}_core ${PROJECT_NAME}_node 145 | EXPORT export_${PROJECT_NAME} 146 | ARCHIVE DESTINATION lib 147 | LIBRARY DESTINATION lib 148 | RUNTIME DESTINATION bin 149 | DESTINATION 150 | lib/${PROJECT_NAME} 151 | ) 152 | 153 | install( 154 | DIRECTORY launch params 155 | DESTINATION share/${PROJECT_NAME} 156 | ) 157 | 158 | ament_export_dependencies() 159 | 160 | if(BUILD_TESTING) 161 | find_package(ament_cmake_gtest REQUIRED) 162 | ament_add_gtest( 163 | ${PROJECT_NAME}-test test/test_pf_localization.cpp 164 | ) 165 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 166 | target_compile_definitions(${PROJECT_NAME}-test PRIVATE MRPT_LOCALIZATION_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}\") 167 | install( 168 | TARGETS ${PROJECT_NAME}-test 169 | DESTINATION bin 170 | ) 171 | 172 | find_package(ament_lint_auto REQUIRED) 173 | 174 | # the following line skips the linter which checks for copyrights 175 | # uncomment the line when a copyright and license is not present in all source files 176 | #set(ament_cmake_copyright_FOUND TRUE) 177 | # the following line skips cpplint (only works in a git repo) 178 | # uncomment the line when this package is not in a git repo 179 | #set(ament_cmake_cpplint_FOUND TRUE) 180 | ament_lint_auto_find_test_dependencies() 181 | endif() 182 | 183 | ament_package() 184 | -------------------------------------------------------------------------------- /mrpt_pf_localization/include/mrpt_pf_localization_node.h: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include // size_t 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "mrpt_msgs/msg/generic_object.hpp" 36 | 37 | #define MRPT_LOCALIZATION_NODE_DEFAULT_PARAMETER_UPDATE_SKIP 1 38 | #define MRPT_LOCALIZATION_NODE_DEFAULT_PARTICLECLOUD_UPDATE_SKIP 5 39 | #define MRPT_LOCALIZATION_NODE_DEFAULT_MAP_UPDATE_SKIP 2 40 | 41 | using mrpt::obs::CObservationOdometry; 42 | 43 | /// ROS Node 44 | class PFLocalizationNode : public rclcpp::Node 45 | { 46 | public: 47 | PFLocalizationNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 48 | ~PFLocalizationNode(); 49 | 50 | struct NodeParameters 51 | { 52 | NodeParameters() = default; 53 | ~NodeParameters() = default; 54 | 55 | void loadFrom(const mrpt::containers::yaml& cfg); 56 | 57 | double rate_hz = 2.0; //!< Execution rate in Hz 58 | 59 | /// projection into the future added to the published tf to extend its 60 | /// validity. /tf will be re-published with half this period to ensure 61 | /// that it is always valid in the /tf tree. 62 | double transform_tolerance = 0.1; 63 | 64 | /// maximum time without updating we keep using filter time instead of 65 | /// Time::now 66 | double no_update_tolerance = 1.0; 67 | 68 | /// maximum time without any observation before start complaining 69 | double no_inputs_tolerance = 2.0; 70 | 71 | std::string base_link_frame_id = "base_link"; 72 | std::string odom_frame_id = "odom"; 73 | std::string global_frame_id = "map"; 74 | 75 | std::string topic_map = "/mrpt_map/metric_map"; 76 | std::string topic_initialpose = "/initialpose"; 77 | std::string topic_odometry = "/odom"; 78 | 79 | std::string pub_topic_particles = "/particlecloud"; 80 | std::string pub_topic_pose = "/pf_pose"; 81 | 82 | /// Comma "," separated list of topics to subscribe for LaserScan msgs 83 | std::string topic_sensors_2d_scan; 84 | 85 | /// Comma "," separated list of topics to subscribe for PointCloud2 msgs 86 | std::string topic_sensors_point_clouds; 87 | 88 | /// Topic name to subscribe for GNSS msgs: 89 | std::string topic_gnss = "/gps"; 90 | }; 91 | 92 | NodeParameters nodeParams_; 93 | 94 | private: 95 | PFLocalizationCore core_; 96 | 97 | int loopCount_ = 0; //!< For decimation purposes only 98 | 99 | bool isTimeFor(int decimation) const 100 | { 101 | return decimation <= 1 || (loopCount_ % decimation == 0); 102 | } 103 | 104 | rclcpp::TimerBase::SharedPtr timer_, timerPubTF_; 105 | 106 | /// 107 | void reload_params_from_ros(); 108 | 109 | void loop(); 110 | void callbackLaser(const sensor_msgs::msg::LaserScan& msg, const std::string& topicName); 111 | void callbackPointCloud(const sensor_msgs::msg::PointCloud2& msg, const std::string& topicName); 112 | 113 | void callbackGNSS(const sensor_msgs::msg::NavSatFix& msg); 114 | 115 | void callbackBeacon(const mrpt_msgs::msg::ObservationRangeBeacon&); 116 | void callbackRobotPose(const geometry_msgs::msg::PoseWithCovarianceStamped&); 117 | void callbackInitialpose(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); 118 | void callbackOdometry(const nav_msgs::msg::Odometry&); 119 | 120 | void callbackMap(const mrpt_msgs::msg::GenericObject& obj); 121 | 122 | /// Publish the PF output mean to /tf 123 | void publishTF(); 124 | /// Publish the PF output as a PoseArray & PoseWithCovarianceStamped msg 125 | void publishParticlesAndStampedPose(); 126 | 127 | void updateEstimatedTwist(); 128 | void createOdometryFromTwist(); 129 | 130 | // These two are used in updateEstimatedTwist() 131 | std::optional prevParts_; 132 | std::optional prevStamp_; 133 | std::optional estimated_twist_; 134 | 135 | /** Sub for /initialpose */ 136 | rclcpp::Subscription::SharedPtr sub_init_pose_; 137 | 138 | rclcpp::Subscription::SharedPtr subMap_; 139 | rclcpp::Subscription::SharedPtr subOdometry_; 140 | 141 | // Sensors: 142 | std::vector::SharedPtr> subs_2dlaser_; 143 | std::vector::SharedPtr> subs_point_clouds_; 144 | rclcpp::Subscription::SharedPtr subGNSS_; 145 | 146 | rclcpp::Publisher::SharedPtr pubParticles_; 147 | 148 | rclcpp::Publisher::SharedPtr pubPose_; 149 | 150 | std::shared_ptr tf_buffer_; 151 | std::shared_ptr tf_listener_; 152 | 153 | std::shared_ptr tf_broadcaster_; 154 | 155 | std::optional last_sensor_stamp_; 156 | 157 | void useROSLogLevel(); 158 | 159 | [[nodiscard]] bool waitForTransform( 160 | mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame, 161 | const int timeoutMilliseconds = 50); 162 | 163 | void update_tf_pub_data(); 164 | std::optional tfMapOdomToPublish_; 165 | std::mutex tfMapOdomToPublishMtx_; 166 | }; 167 | -------------------------------------------------------------------------------- /mrpt_pf_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mrpt_pf_localization 5 | 2.2.2 6 | Package for robot 2D self-localization using dynamic or static (MRPT or ROS) maps. 7 | The interface is similar to amcl (https://wiki.ros.org/amcl) 8 | but supports different particle-filter algorithms, several grid maps at 9 | different heights, range-only localization, etc. 10 | 11 | Markus Bader 12 | Jose Luis Blanco-Claraco 13 | Markus Bader 14 | Raphael Zack 15 | 16 | BSD 17 | https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_pf_localization 18 | 19 | 20 | cmake 21 | ament_cmake 22 | 23 | 24 | roscpp 25 | rclcpp 26 | mola_relocalization 27 | mp2p_icp 28 | mrpt_libgui 29 | mrpt_libros_bridge 30 | mrpt_libslam 31 | mrpt_msgs 32 | mrpt_msgs_bridge 33 | nav_msgs 34 | pose_cov_ops 35 | sensor_msgs 36 | std_msgs 37 | tf2 38 | tf2_geometry_msgs 39 | rclcpp 40 | rclcpp_components 41 | 42 | ament_cmake_lint_cmake 43 | ament_cmake_xmllint 44 | ament_lint_auto 45 | 46 | mrpt_tutorials 47 | 48 | 49 | ament_cmake 50 | 51 | 52 | -------------------------------------------------------------------------------- /mrpt_pf_localization/params/default-relocalization-pipeline.yaml: -------------------------------------------------------------------------------- 1 | # mp2p_icp ICP pipeline configuration. 2 | # YAML configuration file for use with the CLI tool mp2p-icp-run or 3 | # programmatically from function mp2p_icp::icp_pipeline_from_yaml() 4 | class_name: mp2p_icp::ICP 5 | 6 | # See: mp2p_icp::Parameter 7 | params: 8 | maxIterations: 300 9 | minAbsStep_trans: 1e-4 10 | minAbsStep_rot: 5e-5 11 | 12 | #debugPrintIterationProgress: true # Print iteration progress 13 | generateDebugFiles: true # Can be override with env var "MP2P_ICP_GENERATE_DEBUG_FILES=1" 14 | saveIterationDetails: true # Store partial solutions and pairings for each ICP iteration 15 | decimationIterationDetails: 3 16 | debugFileNameFormat: "/tmp/pf-icp-run-$UNIQUE_ID-local_$LOCAL_ID$LOCAL_LABEL-to-global_$GLOBAL_ID$GLOBAL_LABEL.icplog" 17 | decimationDebugFiles: 1 18 | 19 | solvers: 20 | - class: mp2p_icp::Solver_GaussNewton 21 | params: 22 | maxIterations: 2 23 | robustKernel: 'RobustKernel::GemanMcClure' 24 | #robustKernelParam: '0.5*ADAPTIVE_THRESHOLD_SIGMA' # [m] # (adaptive) 25 | robustKernelParam: '0.5*max(0.5*ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/100)' 26 | #innerLoopVerbose: true 27 | 28 | # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher 29 | # instances to pair geometric entities between pointclouds. 30 | matchers: 31 | - class: mp2p_icp::Matcher_Points_DistanceThreshold 32 | params: 33 | #threshold: '2.0*ADAPTIVE_THRESHOLD_SIGMA' # [m] 34 | threshold: '2.0*max(0.5*ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/100)' 35 | thresholdAngularDeg: 1.0 # deg 36 | pairingsPerPoint: 1 37 | allowMatchAlreadyMatchedGlobalPoints: true # faster 38 | pointLayerMatches: 39 | - {global: "localmap", local: "decimated_for_icp", weight: 1.0} 40 | - {global: "points_map", local: "decimated_for_icp", weight: 1.0} 41 | 42 | quality: 43 | - class: mp2p_icp::QualityEvaluator_PairedRatio 44 | params: 45 | ~ # none required 46 | 47 | # This pipeline will be applied to the (probably already-filtered) input pointcloud that 48 | # arrived at the pf_localization node, which will be exposed here as a layer "raw": 49 | relocalization_observation_pipeline: 50 | # Filters: 51 | # 52 | # One filter object will be created for each entry, instancing the given class, 53 | # and with the given parameters. Filters are run in definition order on the 54 | # input metric_map_t object. 55 | # 56 | - class_name: mp2p_icp_filters::FilterDecimateVoxels 57 | params: 58 | input_pointcloud_layer: 'raw' 59 | output_pointcloud_layer: 'decimated_for_icp' 60 | voxel_filter_resolution: 1.5 # [m] 61 | minimum_input_points_to_filter: 1000 # don't decimate simpler 2D scans, etc. 62 | decimate_method: DecimateMethod::FirstPoint 63 | #decimate_method: DecimateMethod::ClosestToAverage 64 | -------------------------------------------------------------------------------- /mrpt_pf_localization/params/default.config.yaml: -------------------------------------------------------------------------------- 1 | # This file defines parameters to configure the settings of the class 2 | # PFLocalizationCore inside the ROS 2 package mrpt_pf_localization. 3 | # Refer to the package README.md and the comments below for the meaning of each 4 | # parameter. 5 | # 6 | # The two top-level entries are there so we can use this file as a ROS 2 7 | # parameter YAML file, but it can be also used directly to feed the internal 8 | # PFLocalizationCore class skipping those two top-level YAML maps, as done 9 | # in the unit tests (see "test/*.cpp") 10 | # 11 | /**: 12 | ros__parameters: 13 | # If false (default), will use 2D (SE(2)) particle filter. 14 | # Otherwise,the 3D mode (SE(3)) is enabled. 15 | use_se3_pf: false 16 | 17 | # Execution rate (in Hz) of the particle filter main loop: 18 | rate_hz: 1.0 19 | 20 | # Particle density (particles/m²) upon initialization: 21 | initial_particles_per_m2: 50 22 | 23 | # You can include the initial pose here as parameter, or send it via a topic or service. 24 | initial_pose: 25 | # Mean (center) of initial pose 26 | mean: { x: 0.0, y: 0.0, z: 0.0, yaw: 0.0, pitch: 0.0, roll: 0.0 } 27 | std_x: 1.0 # 1 sigma uncertainty [meters] 28 | std_y: 1.0 29 | std_z: 0.0 30 | std_yaw: 6.0 # 1 sigma uncertainty [radians] 31 | std_pitch: 0.0 32 | std_roll: 0.0 33 | 34 | # Shows a live 3D window with the state of the PF, the map, sensors, etc. 35 | gui_enable: true 36 | gui_camera_follow_robot: true 37 | 38 | # If set to true, the PF will not be initialized until: 39 | # - A map is provided with georeferencing information, and 40 | # - The first GNSS observation arrives (with a GGA datum) 41 | #initialize_from_gnss: true 42 | 43 | # If >0, new tentative particles will be generated from GNSS data, 44 | # to help re-localizing if using georeferenced maps: 45 | samples_drawn_from_gnss: 20 46 | 47 | # The number of standard deviations ("sigmas") to use as the area in 48 | # which to draw random samples around the input initialization pose 49 | # (when NOT using GNSS as input) 50 | relocalize_num_sigmas: 3.0 51 | 52 | # If samples_drawn_from_gnss is enabled, the number of standard 53 | # deviations ("sigmas") to use as the area in which to draw random 54 | # samples around the GNSS prediction: 55 | gnss_samples_num_sigmas: 6.0 56 | 57 | 58 | # For SE(2) mode: Uncertainty motion model for regular odometry-based motion 59 | # See docs for mrpt::obs::CActionRobotMovement2D::TMotionModelOptions or https://docs.mrpt.org/reference/latest/tutorial-motion-models.html 60 | motion_model_2d: 61 | modelSelection: mmGaussian # (mmGaussian|mmThrun) 62 | gaussianModel: 63 | a1: 0.02 # [m/m] 64 | a2: 0.001 # [m/rad] 65 | a3: 1.0 # [deg/m] 66 | a4: 0.25 # [deg/deg] 67 | minStdXY: 0.02 # [m] 68 | minStdPHI: 0.4 # [deg] 69 | thrunModel: 70 | alfa1_rot_rot: 0.1 71 | alfa2_rot_trans: 0.1 72 | alfa3_trans_trans: 0.1 73 | alfa4_trans_rot: 0.1 74 | additional_std_XY: 0.01 # [m] 75 | additional_std_phi: 0.2 # [deg] 76 | 77 | # Optional: Can be used to use only a subset of the input metric-map (.mm) 78 | # map layers for localization. Layer names must be separated by commas. 79 | #metric_map_use_only_these_layers: 'map' 80 | 81 | # For SE(2) mode: Uncertainty motion model to use when NO odometry has 82 | # been received. 83 | # See docs for mrpt::obs::CActionRobotMovement2D::TMotionModelOptions or https://docs.mrpt.org/reference/latest/tutorial-motion-models.html 84 | motion_model_no_odom_2d: 85 | modelSelection: mmGaussian # (mmGaussian|mmThrun) 86 | gaussianModel: 87 | minStdXY: 0.40 # [m] 88 | minStdPHI: 5.0 # [deg] 89 | 90 | # For SE(3) mode: Uncertainty motion model for regular odometry-based motion 91 | motion_model_3d: 92 | modelSelection: mmGaussian 93 | 94 | # For SE(3) mode: Uncertainty motion model to use when NO odometry has 95 | # been received. 96 | motion_model_no_odom_3d: 97 | modelSelection: mmGaussian 98 | 99 | # Particle filter options: 100 | # Refer to docs for: [TParticleFilterOptions](https://docs.mrpt.org/reference/latest/struct_mrpt_bayes_CParticleFilter_TParticleFilterOptions.html) 101 | # ----------------------------------------------- 102 | pf_options: 103 | # Perform adaptive sample size? (KLD algorithm) 104 | adaptiveSampleSize: true 105 | 106 | # Resampling of particles will be performed when ESS < BETA. [0.0, 1.0] 107 | BETA: 0.5 108 | 109 | # Options: pfStandardProposal | pfAuxiliaryPFStandard | pfOptimalProposal | pfAuxiliaryPFOptimal 110 | PF_algorithm: pfStandardProposal 111 | 112 | # Options: prMultinomial | prResidual | prStratified | prSystematic 113 | resamplingMethod: prMultinomial 114 | 115 | # For the next params, refer to the upstream docs above for TParticleFilterOptions 116 | # (probably no need to change them) 117 | pfAuxFilterOptimal_MaximumSearchSamples: 100 118 | powFactor: 1.0 119 | max_loglikelihood_dyn_range: 15 120 | pfAuxFilterStandard_FirstStageWeightsMonteCarlo: false 121 | pfAuxFilterOptimal_MLE: false 122 | 123 | # Dynamic sample size options. 124 | # Refer to docs for [mrpt::slam::TKLDParams](https://docs.mrpt.org/reference/latest/class_mrpt_slam_TKLDParams.html) 125 | # and to Dieter Fox's paper: 126 | # Fox, D. (2003). Adapting the sample size in particle filters through 127 | # KLD-sampling. The international Journal of robotics research, 22(12), 985-1003. 128 | # ----------------------------------------------------------------------------------- 129 | kld_options: 130 | KLD_minSampleSize: 50 131 | KLD_maxSampleSize: 10000 132 | KLD_binSize_XY: 0.25 # [meters] 133 | KLD_binSize_PHI: 0.17 # [radians] 134 | KLD_delta: 0.05 135 | KLD_epsilon: 0.05 136 | KLD_minSamplesPerBin: 0 137 | 138 | # If defined, this block will override the likelihoodOptions field of the 139 | # de-serialized metric map (.mm) used as global map: 140 | # 141 | # Docs for mrpt::maps::CPointsMap::TLikelihoodOptions: https://docs.mrpt.org/reference/latest/struct_mrpt_maps_CPointsMap_TLikelihoodOptions.html 142 | override_likelihood_point_maps: 143 | sigma_dist: 0.10 # [m] 144 | max_corr_distance: 1.5 # [m] 145 | decimation: 50 146 | 147 | # If defined, this block will override the likelihoodOptions field of the 148 | # de-serialized metric map (.mm) used as global map: 149 | # 150 | # Docs for mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions: https://docs.mrpt.org/reference/latest/class_mrpt_maps_COccupancyGridMap2D_TLikelihoodOptions.html 151 | override_likelihood_gridmaps: 152 | likelihoodMethod: 'lmLikelihoodField_Thrun' 153 | LF_stdHit: 2.0 154 | LF_zHit: 0.90 155 | LF_zRandom: 0.10 156 | LF_maxRange: 81.0 157 | LF_maxCorrsDistance: 1.0 158 | LF_decimation: 1 159 | 160 | 161 | # After relocalization, candidate poses are grouped using a SE(3) grid with this granularity: 162 | relocalization_resolution_xy: 0.25 # [m] 163 | relocalization_resolution_phi: 10.0 # [deg] 164 | 165 | relocalization_initial_divisions_xy: 4 166 | relocalization_initial_divisions_phi: 4 167 | 168 | relocalization_minimum_icp_quality: 0.50 # [0,1] 169 | relocalization_icp_sigma: 5.0 # [m] 170 | 171 | #relocalization_icp_pipeline: stored in a separate YAML files due to the limitations 172 | # of importing generic YAML nested structures as ROS params yaml files. 173 | -------------------------------------------------------------------------------- /mrpt_pf_localization/params/map-occgrid2d.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------------------- 2 | # Configuration file for the multi-metric map which will be 3 | # populated from observations in a .simplemap. 4 | # 5 | # Refer to docs: https://docs.mrpt.org/reference/latest/tutorial-mrpt-maps-model.html 6 | #------------------------------------------------------------------------------- 7 | 8 | # ==================================================== 9 | # MULTIMETRIC MAP CONFIGURATION 10 | # ==================================================== 11 | [MetricMap] 12 | # Creation of maps: 13 | occupancyGrid_count=1 14 | gasGrid_count=0 15 | landmarksMap_count=0 16 | pointsMap_count=0 17 | beaconMap_count=0 18 | 19 | # Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3) 20 | likelihoodMapSelection=-1 21 | 22 | # ==================================================== 23 | # MULTIMETRIC MAP: OccGrid #00 24 | # ==================================================== 25 | # Creation Options for OccupancyGridMap 00: 26 | [MetricMap_occupancyGrid_00_creationOpts] 27 | resolution=0.06 28 | 29 | # Insertion Options for OccupancyGridMap 00: 30 | [MetricMap_occupancyGrid_00_insertOpts] 31 | mapAltitude=0 32 | useMapAltitude=false 33 | maxDistanceInsertion=15 34 | maxOccupancyUpdateCertainty=0.55 35 | considerInvalidRangesAsFreeSpace=1 36 | minLaserScanNoiseStd=0.001 37 | 38 | # Likelihood Options for OccupancyGridMap 00: 39 | [MetricMap_occupancyGrid_00_likelihoodOpts] 40 | likelihoodMethod=4 // 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II 41 | 42 | LF_decimation=20 43 | LF_stdHit=0.20 44 | LF_maxCorrsDistance=0.30 45 | LF_zHit=0.95 46 | LF_zRandom=0.05 47 | LF_maxRange=80 48 | LF_alternateAverageMethod=0 49 | 50 | MI_exponent=10 51 | MI_skip_rays=10 52 | MI_ratio_max_distance=2 53 | 54 | rayTracing_useDistanceFilter=0 55 | rayTracing_decimation=10 56 | rayTracing_stdHit=0.30 57 | 58 | consensus_takeEachRange=30 59 | consensus_pow=1 60 | -------------------------------------------------------------------------------- /mrpt_pf_localization/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "mrpt_pf_localization_component.cpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | rclcpp::init(argc, argv); 15 | auto node = std::make_shared(); 16 | rclcpp::spin(std::dynamic_pointer_cast(node)); 17 | rclcpp::shutdown(); 18 | return 0; 19 | } -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_pointcloud_pipeline 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | * FIX: remove usage of obsolete ament_target_dependencies() 8 | * Merge pull request `#153 `_ from dppp415/ros2 9 | Composable Nodes 10 | * Some fixes and demo 11 | * Copyright 12 | * Deleted duplicated code 13 | * Nodes mrpt_pointcloud_pipeline y mrpt_pf_localization are now composables 14 | * Contributors: Jose Luis Blanco-Claraco, dppp415 15 | 16 | 2.2.1 (2024-10-12) 17 | ------------------ 18 | 19 | 2.2.0 (2024-09-25) 20 | ------------------ 21 | * Update URL entries in package.xml to each package proper documentation 22 | * ament linters: manually enable just cmake and xml linters 23 | * reformat clang-format with 100 column width 24 | * Contributors: Jose Luis Blanco-Claraco 25 | 26 | 2.1.1 (2024-09-02) 27 | ------------------ 28 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 29 | * update dependencies 30 | * Depend on new mrpt_lib packages (deprecate mrpt2) 31 | * Contributors: Jose Luis Blanco-Claraco 32 | 33 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 34 | * update dependencies 35 | * Depend on new mrpt_lib packages (deprecate mrpt2) 36 | * Contributors: Jose Luis Blanco-Claraco 37 | 38 | 2.1.0 (2024-08-08) 39 | ------------------ 40 | * Merge branch 'ros2' into wip/port-tps-astar 41 | * Merge branch 'ros2' into wip/port-tps-astar 42 | * Contributors: Jose Luis Blanco-Claraco 43 | 44 | 2.0.1 (2024-05-28) 45 | ------------------ 46 | 47 | 2.0.0 (2024-05-28) 48 | ------------------ 49 | * use namespaces for launch files 50 | * Use just one pipeline generator+filter definition file 51 | * split filtering into per-observation and final pipelines 52 | * generic launch for pipeline 53 | * Renamed pkg mrpt_local_obstacles: mrpt_pointcloud_pipeline to reflect new capabilities 54 | * Contributors: Jose Luis Blanco-Claraco 55 | 56 | 1.0.3 (2022-06-25) 57 | ------------------ 58 | 59 | 1.0.2 (2022-06-25) 60 | ------------------ 61 | 62 | 1.0.1 (2022-06-24) 63 | ------------------ 64 | * Ported to tf2 and mrpt::ros1bridge 65 | * Contributors: Jose Luis Blanco Claraco 66 | 67 | 1.0.0 (2022-04-30) 68 | ------------------ 69 | * Update URLs to https 70 | * Update build dep to mrpt2 71 | * Fix build against mrpt2 72 | * Contributors: Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Jose-Luis Blanco 73 | 74 | 0.1.26 (2019-10-05) 75 | ------------------- 76 | 77 | 0.1.25 (2019-10-04) 78 | ------------------- 79 | 80 | 0.1.24 (2019-04-12) 81 | ------------------- 82 | * Fix build against MRPT 1.9.9 83 | * Contributors: Julian Lopez Velasquez 84 | 85 | 0.1.23 (2018-06-14) 86 | ------------------- 87 | 88 | 0.1.22 (2018-05-22) 89 | ------------------- 90 | * fix all catkin_lint errors 91 | * Contributors: Jose Luis Blanco-Claraco 92 | 93 | 0.1.21 (2018-04-27) 94 | ------------------- 95 | * Upgrade version 0.1.20 (`#99 `_) 96 | * fix build against mrpt 2.0 97 | * partial fix build w mrpt 2.0 98 | * optimized build (-O3) 99 | * fix build errors against mrpt 1.5.5 100 | * Fix travis (`#94 `_) 101 | * add dep stereo_msgs 102 | * fix minor warnigngs and errors 103 | * Merge branch 'master' of github.com:tuw-robotics/mrpt_navigation 104 | * Merge branch 'master' into master 105 | * CMake finds MRPT >=1.5 in ROS master branch 106 | * Merge branch 'master' into compat-mrpt-1.5 107 | * CMake finds MRPT >=1.9 108 | * avoid Eigen warnings with GCC-7 109 | * Removed unnecessry MRPT_VERSION checks 110 | * Fixes for clang format 111 | * Adapted CMakeLists to new mrpt 112 | * Ported to a new version of MRPT 113 | * Contributors: Borys Tymchenko, Hunter Laux, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Markus Bader 114 | 115 | 0.1.20 (2018-04-26) 116 | ------------------- 117 | * fix build against mrpt 2.0 118 | * partial fix build w mrpt 2.0 119 | * optimized build (-O3) 120 | * fix build errors against mrpt 1.5.5 121 | * Fix travis (`#94 `_) 122 | * add dep stereo_msgs 123 | * add dep stereo_msgs 124 | * fix minor warnigngs and errors 125 | * Merge branch 'master' of github.com:tuw-robotics/mrpt_navigation 126 | * Merge branch 'master' into master 127 | * CMake finds MRPT >=1.5 in ROS master branch 128 | * Merge branch 'master' into compat-mrpt-1.5 129 | * CMake finds MRPT >=1.9 130 | * avoid Eigen warnings with GCC-7 131 | * Removed unnecessry MRPT_VERSION checks 132 | * Fixes for clang format 133 | * Adapted CMakeLists to new mrpt 134 | * Ported to a new version of MRPT 135 | * Contributors: Borys Tymchenko, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Markus Bader 136 | 137 | 0.1.18 (2017-01-22) 138 | ------------------- 139 | 140 | 0.1.17 (2017-01-22) 141 | ------------------- 142 | * make catkin_lint clean 143 | * Remove all errors generated by catkin_lint and cleanup unused templates from CMakeLists.txt files 144 | * Contributors: Jorge Santos, Jose Luis Blanco 145 | 146 | 0.1.16 (2016-12-13) 147 | ------------------- 148 | 149 | 0.1.15 (2016-11-06) 150 | ------------------- 151 | 152 | 0.1.14 (2016-09-12) 153 | ------------------- 154 | 155 | 0.1.13 (2016-09-03) 156 | ------------------- 157 | 158 | 0.1.12 (2016-09-03) 159 | ------------------- 160 | 161 | 0.1.11 (2016-08-21) 162 | ------------------- 163 | 164 | 0.1.10 (2016-08-05) 165 | ------------------- 166 | 167 | 0.1.9 (2016-08-05) 168 | ------------------ 169 | 170 | 0.1.8 (2016-06-29) 171 | ------------------ 172 | 173 | 0.1.7 (2016-06-20) 174 | ------------------ 175 | 176 | 0.1.6 (2016-03-20) 177 | ------------------ 178 | * fix build with latest mrpt version 179 | * Contributors: Jose Luis Blanco 180 | 181 | 0.1.5 (2015-04-29) 182 | ------------------ 183 | * mrpt_pointcloud_pipeline: Fix wrong report of number of scan sources 184 | * Fix build against mrpt 1.3.0 185 | * Contributors: Jose Luis Blanco 186 | 187 | 0.1.4 (2014-12-27) 188 | ------------------ 189 | * First working version of the package 190 | * Contributors: Jose Luis Blanco 191 | 192 | 0.1.3 (2014-12-18 23:21) 193 | ------------------------ 194 | 195 | 0.1.2 (2014-12-18 11:49) 196 | ------------------------ 197 | 198 | 0.1.1 (2014-12-17) 199 | ------------------ 200 | -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mrpt_pointcloud_pipeline) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_components REQUIRED) 8 | find_package(nav_msgs REQUIRED) 9 | find_package(sensor_msgs REQUIRED) 10 | find_package(tf2 REQUIRED) 11 | find_package(tf2_geometry_msgs REQUIRED) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | find_package(mrpt-maps REQUIRED) 15 | find_package(mrpt-obs REQUIRED) 16 | find_package(mrpt-gui REQUIRED) 17 | find_package(mrpt-ros2bridge REQUIRED) 18 | 19 | if(NOT CMAKE_C_STANDARD) 20 | set(CMAKE_C_STANDARD 99) 21 | endif() 22 | 23 | if(NOT CMAKE_CXX_STANDARD) 24 | set(CMAKE_CXX_STANDARD 17) 25 | endif() 26 | 27 | if (CMAKE_COMPILER_IS_GNUCXX) 28 | # High level of warnings. 29 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 30 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 31 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros -Wextra -Wpedantic) 32 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 33 | if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 34 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 35 | endif() 36 | endif() 37 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 38 | add_compile_options(-O3) 39 | ENDIF() 40 | 41 | 42 | find_package(mp2p_icp_filters REQUIRED) 43 | 44 | add_executable(${PROJECT_NAME}_node 45 | src/main.cpp 46 | include/${PROJECT_NAME}/mrpt_pointcloud_pipeline_node.h) 47 | 48 | target_include_directories(${PROJECT_NAME}_node 49 | PUBLIC 50 | $ 51 | $ 52 | ) 53 | 54 | target_link_libraries( 55 | ${PROJECT_NAME}_node PRIVATE 56 | mrpt::maps 57 | mrpt::obs 58 | mrpt::gui 59 | mrpt::ros2bridge 60 | mola::mp2p_icp_filters 61 | rclcpp::rclcpp 62 | ${rclcpp_components_TARGETS} 63 | ${nav_msgs_TARGETS} 64 | ${sensor_msgs_TARGETS} 65 | tf2::tf2 66 | ${tf2_geometry_msgs_TARGETS} 67 | ) 68 | 69 | ################### 70 | # Composable node # 71 | ################### 72 | 73 | add_library(${PROJECT_NAME}_component SHARED 74 | src/${PROJECT_NAME}_component.cpp 75 | include/${PROJECT_NAME}/${PROJECT_NAME}_node.h) 76 | 77 | target_include_directories(${PROJECT_NAME}_component 78 | PUBLIC 79 | $ 80 | $ 81 | ) 82 | 83 | target_link_libraries( 84 | ${PROJECT_NAME}_component PRIVATE 85 | mrpt::maps 86 | mrpt::obs 87 | mrpt::gui 88 | mrpt::ros2bridge 89 | mola::mp2p_icp_filters 90 | rclcpp::rclcpp 91 | ${rclcpp_components_TARGETS} 92 | ${nav_msgs_TARGETS} 93 | ${sensor_msgs_TARGETS} 94 | tf2::tf2 95 | ${tf2_geometry_msgs_TARGETS} 96 | ) 97 | 98 | rclcpp_components_register_node( 99 | ${PROJECT_NAME}_component 100 | PLUGIN "LocalObstaclesNode" 101 | EXECUTABLE ${PROJECT_NAME}_composable 102 | ) 103 | 104 | ########### 105 | # INSTALL # 106 | ########### 107 | 108 | install(TARGETS ${PROJECT_NAME}_component 109 | ${PROJECT_NAME}_node 110 | EXPORT export_${PROJECT_NAME} 111 | ARCHIVE DESTINATION lib 112 | LIBRARY DESTINATION lib 113 | RUNTIME DESTINATION bin 114 | DESTINATION lib/${PROJECT_NAME} 115 | ) 116 | 117 | install( 118 | DIRECTORY 119 | launch 120 | params 121 | DESTINATION share/${PROJECT_NAME} 122 | ) 123 | 124 | if(BUILD_TESTING) 125 | find_package(ament_lint_auto REQUIRED) 126 | # the following line skips the linter which checks for copyrights 127 | # uncomment the line when a copyright and license is not present in all source files 128 | #set(ament_cmake_copyright_FOUND TRUE) 129 | # the following line skips cpplint (only works in a git repo) 130 | # uncomment the line when this package is not in a git repo 131 | #set(ament_cmake_cpplint_FOUND TRUE) 132 | ament_lint_auto_find_test_dependencies() 133 | endif() 134 | 135 | ament_package() 136 | 137 | -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | /* mrpt deps*/ 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | /* ros2 deps */ 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | using namespace mrpt::system; 46 | using namespace mrpt::config; 47 | using namespace mrpt::img; 48 | using namespace mrpt::maps; 49 | using namespace mrpt::obs; 50 | 51 | class LocalObstaclesNode : public rclcpp::Node 52 | { 53 | public: 54 | /* Ctor*/ 55 | explicit LocalObstaclesNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 56 | /* Dtor*/ 57 | ~LocalObstaclesNode() {} 58 | 59 | private: 60 | /* Read parameters from the node handle*/ 61 | void read_parameters(); 62 | 63 | /* Callback: On recalc local map & publish it*/ 64 | void on_do_publish(); 65 | 66 | /* Callback: On new sensor data*/ 67 | void on_new_sensor_laser_2d( 68 | const sensor_msgs::msg::LaserScan::SharedPtr& scan, const std::string& topicName); 69 | 70 | /* Callback: On new pointcloud data*/ 71 | void on_new_sensor_pointcloud( 72 | const sensor_msgs::msg::PointCloud2::SharedPtr& pts, const std::string& topicName); 73 | 74 | /** 75 | * @brief Subscribe to a variable number of topics. 76 | * @param lstTopics String with list of topics separated with "," 77 | * @param subs[in,out] List of subscribers will be here at return. 78 | * @return The number of topics subscribed to. 79 | */ 80 | template 81 | size_t subscribe_to_multiple_topics( 82 | const std::string& lstTopics, 83 | std::vector::SharedPtr>& subscriptions, 84 | CallbackMethodType callback) 85 | { 86 | size_t num_subscriptions = 0; 87 | std::vector lstSources; 88 | mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources); 89 | 90 | // Error handling: check if lstSources is empty 91 | if (lstSources.empty()) 92 | { 93 | return 0; // Return early with 0 subscriptions 94 | } 95 | for (const auto& source : lstSources) 96 | { 97 | const auto sub = this->create_subscription( 98 | source, 1, 99 | [source, callback, this](const typename MessageT::SharedPtr msg) 100 | { callback(msg, source); }); 101 | subscriptions.push_back(sub); // 1 is the queue size 102 | num_subscriptions++; 103 | } 104 | 105 | // Return the number of subscriptions created 106 | return num_subscriptions; 107 | } 108 | 109 | // member variables 110 | CTimeLogger m_profiler; 111 | bool m_show_gui = false; 112 | bool m_one_observation_per_topic = false; 113 | std::string m_frameid_reference = "odom"; //!< type:"odom" 114 | std::string m_frameid_robot = "base_link"; //!< type: "base_link" 115 | std::string m_topics_source_2dscan = "scan, laser1"; //!< Default: "scan, laser1" 116 | std::string m_topics_source_pointclouds = ""; 117 | 118 | //!< In secs (default: 0.2). Can't be smaller than m_publish_period 119 | double m_time_window = 0.20; 120 | 121 | //!< In secs (default: 0.05). Can't be larger than m_time_window 122 | double m_publish_period = 0.05; 123 | 124 | rclcpp::TimerBase::SharedPtr m_timer_publish; 125 | 126 | // Sensor data: 127 | struct InfoPerTimeStep 128 | { 129 | std::string sourceTopic; 130 | CObservation::Ptr observation; 131 | mrpt::poses::CPose3D robot_pose; 132 | }; 133 | using obs_list_t = std::multimap; 134 | 135 | /// The history of past observations duringthe interest time window. 136 | obs_list_t m_hist_obs; 137 | std::mutex m_hist_obs_mtx; //!< mutex 138 | 139 | mrpt::gui::CDisplayWindow3D::Ptr m_gui_win; 140 | bool m_visible_raw = true, m_visible_output = true; 141 | 142 | /// Used for example to run voxel grid decimation, etc. 143 | /// Refer to mp2p_icp docs 144 | std::string m_pipeline_yaml_file; 145 | mp2p_icp_filters::FilterPipeline m_per_obs_pipeline, m_final_pipeline; 146 | mp2p_icp_filters::GeneratorSet m_generator; 147 | 148 | struct LayerTopicNames 149 | { 150 | std::string layer; 151 | std::string topic; 152 | rclcpp::Publisher::SharedPtr pub; 153 | }; 154 | std::vector layer2topic_; 155 | 156 | /** 157 | * @name ROS2 pubs/subs 158 | * @{ 159 | */ 160 | std::vector::SharedPtr> m_subs_2dlaser; 161 | std::vector::SharedPtr> m_subs_pointclouds; 162 | 163 | std::shared_ptr m_tf_buffer; 164 | std::shared_ptr m_tf_listener; 165 | }; 166 | -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for mrpt_pointcloud_pipeline, intended to be included in user's 2 | # launch files. 3 | # 4 | # See the docs on the configurable launch arguments for this file in: 5 | # https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_pointcloud_pipeline 6 | # 7 | # For ready-to-launch demos, see: https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_tutorials 8 | # 9 | 10 | from launch import LaunchDescription 11 | from launch.substitutions import TextSubstitution 12 | from launch.substitutions import LaunchConfiguration 13 | from launch_ros.actions import Node, LoadComposableNodes 14 | from launch_ros.descriptions import ComposableNode 15 | from launch.actions import DeclareLaunchArgument, Shutdown 16 | from launch.conditions import IfCondition, UnlessCondition 17 | from ament_index_python import get_package_share_directory 18 | from launch.actions import GroupAction 19 | from launch_ros.actions import PushRosNamespace 20 | import os 21 | 22 | 23 | def generate_launch_description(): 24 | myPkgDir = get_package_share_directory("mrpt_pointcloud_pipeline") 25 | # print('myPkgDir : ' + myPkgDir) 26 | 27 | use_composable = LaunchConfiguration('use_composable') 28 | 29 | lidar_topic_name_arg = DeclareLaunchArgument( 30 | 'scan_topic_name', 31 | default_value='' # /scan, /laser1, etc. 32 | ) 33 | points_topic_name_arg = DeclareLaunchArgument( 34 | 'points_topic_name', 35 | default_value='' # '/ouster/points', etc. 36 | ) 37 | show_gui_arg = DeclareLaunchArgument( 38 | 'show_gui', 39 | default_value='True' 40 | ) 41 | time_window_arg = DeclareLaunchArgument( 42 | 'time_window', 43 | default_value='0.20' 44 | ) 45 | one_observation_per_topic_arg = DeclareLaunchArgument( 46 | 'one_observation_per_topic', 47 | default_value='false' 48 | ) 49 | pipeline_yaml_file_arg = DeclareLaunchArgument( 50 | 'pipeline_yaml_file', 51 | default_value=os.path.join( 52 | myPkgDir, 'params', 'point-cloud-pipeline.yaml') 53 | ) 54 | filter_output_layer_name_arg = DeclareLaunchArgument( 55 | 'filter_output_layer_name', 56 | default_value='output', 57 | description='The mp2p_icp metric_map_t layer name(s) to be published. Comma-separated list of more than one.' 58 | ) 59 | filter_output_topic_arg = DeclareLaunchArgument( 60 | 'filter_output_topic_name', 61 | default_value='/local_map_pointcloud', 62 | description='The topic name to publish the output layer map(s). Comma-separated list of more than one, then the number must match that of filter_output_layer_name.' 63 | ) 64 | frameid_reference_arg = DeclareLaunchArgument( 65 | 'frameid_reference', 66 | default_value='odom' 67 | ) 68 | frameid_robot_arg = DeclareLaunchArgument( 69 | 'frameid_robot', 70 | default_value='base_link' 71 | ) 72 | 73 | log_level_launch_arg = DeclareLaunchArgument( 74 | "log_level", 75 | default_value=TextSubstitution(text=str("INFO")), 76 | description="Logging level" 77 | ) 78 | 79 | container_name_arg = DeclareLaunchArgument( 80 | 'container_name', 81 | default_value='', 82 | ) 83 | 84 | emit_shutdown_action = Shutdown(reason='launch is shutting down') 85 | 86 | mrpt_pointcloud_pipeline_node = Node( 87 | condition=UnlessCondition(use_composable), 88 | package='mrpt_pointcloud_pipeline', 89 | executable='mrpt_pointcloud_pipeline_node', 90 | name='mrpt_pointcloud_pipeline_node', 91 | output='screen', 92 | parameters=[ 93 | {'source_topics_2d_scans': LaunchConfiguration('scan_topic_name')}, 94 | {'source_topics_pointclouds': LaunchConfiguration( 95 | 'points_topic_name')}, 96 | {'show_gui': LaunchConfiguration('show_gui')}, 97 | {'pipeline_yaml_file': LaunchConfiguration('pipeline_yaml_file')}, 98 | {'filter_output_layer_name': LaunchConfiguration( 99 | 'filter_output_layer_name')}, 100 | {'time_window': LaunchConfiguration('time_window')}, 101 | {'topic_local_map_pointcloud': LaunchConfiguration( 102 | 'filter_output_topic_name')}, 103 | {'frameid_reference': LaunchConfiguration( 104 | 'frameid_reference')}, 105 | {'frameid_robot': LaunchConfiguration('frameid_robot')}, 106 | {'one_observation_per_topic': LaunchConfiguration( 107 | 'one_observation_per_topic')}, 108 | ], 109 | arguments=['--ros-args', '--log-level', 110 | LaunchConfiguration('log_level')], 111 | on_exit=[emit_shutdown_action] 112 | ) 113 | 114 | composable_mrpt_pointcloud_pipeline = LoadComposableNodes( 115 | condition=IfCondition(use_composable), 116 | target_container=LaunchConfiguration('container_name'), 117 | composable_node_descriptions=[ 118 | ComposableNode( 119 | package='mrpt_pointcloud_pipeline', 120 | name='mrpt_pointcloud_pipeline_component', 121 | plugin='LocalObstaclesNode', 122 | parameters=[ 123 | {'source_topics_2d_scans': LaunchConfiguration('scan_topic_name')}, 124 | {'source_topics_pointclouds': LaunchConfiguration('points_topic_name')}, 125 | {'show_gui': LaunchConfiguration('show_gui')}, 126 | {'pipeline_yaml_file': LaunchConfiguration('pipeline_yaml_file')}, 127 | {'filter_output_layer_name': LaunchConfiguration('filter_output_layer_name')}, 128 | {'time_window': LaunchConfiguration('time_window')}, 129 | {'topic_local_map_pointcloud': LaunchConfiguration('filter_output_topic_name')}, 130 | {'frameid_reference': LaunchConfiguration('frameid_reference')}, 131 | {'frameid_robot': LaunchConfiguration('frameid_robot')}, 132 | {'one_observation_per_topic': LaunchConfiguration('one_observation_per_topic')}, 133 | ], 134 | ) 135 | ] 136 | ) 137 | 138 | ld = LaunchDescription([ 139 | container_name_arg, 140 | lidar_topic_name_arg, 141 | points_topic_name_arg, 142 | show_gui_arg, 143 | time_window_arg, 144 | pipeline_yaml_file_arg, 145 | filter_output_layer_name_arg, 146 | filter_output_topic_arg, 147 | frameid_reference_arg, 148 | frameid_robot_arg, 149 | log_level_launch_arg, 150 | one_observation_per_topic_arg, 151 | mrpt_pointcloud_pipeline_node, 152 | composable_mrpt_pointcloud_pipeline, 153 | ]) 154 | 155 | # Namespace to avoid clash launch argument names with the parent scope: 156 | return LaunchDescription([GroupAction( 157 | actions=[ 158 | PushRosNamespace(namespace='pc_pipeline'), 159 | ld 160 | ])]) 161 | -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_pointcloud_pipeline 4 | 2.2.2 5 | Maintains a local obstacle map from recent sensor readings, including optional point cloud pipeline filtering or processing. 6 | 7 | Jose-Luis Blanco-Claraco 8 | Shravan S Rai 9 | 10 | Jose-Luis Blanco-Claraco 11 | Shravan S Rai 12 | 13 | BSD 14 | https://github.com/mrpt-ros-pkg/mrpt_navigation 15 | 16 | ament_cmake 17 | 18 | 19 | mp2p_icp 20 | mrpt_libgui 21 | mrpt_libmaps 22 | mrpt_libobs 23 | mrpt_libros_bridge 24 | nav_msgs 25 | rclcpp 26 | rclcpp_components 27 | sensor_msgs 28 | tf2 29 | tf2_geometry_msgs 30 | 31 | ament_cmake_lint_cmake 32 | ament_cmake_xmllint 33 | ament_lint_auto 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/params/point-cloud-pipeline.yaml: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # mp2p_icp filters definition file for mrpt_pointcloud_pipeline 3 | # 4 | # See docs for MP2P_ICP library: https://docs.mola-slam.org/mp2p_icp/ 5 | # ----------------------------------------------------------------------------- 6 | 7 | # --------------------------------------------------------------- 8 | # 1) Create temporary point map to accumulate 1+ sensor observations: 9 | # --------------------------------------------------------------- 10 | generators: 11 | - class_name: mp2p_icp_filters::Generator 12 | params: 13 | target_layer: 'accumulated_points' 14 | throw_on_unhandled_observation_class: true 15 | process_class_names_regex: '' # NONE: don't process observations in the generator, just used to create the metric map. 16 | #process_sensor_labels_regex: '.*' 17 | # metric_map_definition_ini_file: '${CURRENT_YAML_FILE_PATH}/map_definition.ini' # Use either an external INI file, or the 'metric_map_definition' YAML entry below 18 | 19 | metric_map_definition: 20 | # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html 21 | class: mrpt::maps::CSimplePointsMap 22 | #creationOpts: # none needed 23 | #insertionOpts: # none needed 24 | #renderOpts: # none needed 25 | 26 | # Then, use default generator: generate the observation raw points 27 | - class_name: mp2p_icp_filters::Generator 28 | params: 29 | target_layer: 'raw' 30 | throw_on_unhandled_observation_class: true 31 | process_class_names_regex: '.*' 32 | process_sensor_labels_regex: '.*' 33 | 34 | 35 | # --------------------------------------------------------------- 36 | # 2) Pipeline for each individual observation 37 | # --------------------------------------------------------------- 38 | per_observation: 39 | # Remove the robot body: 40 | - class_name: mp2p_icp_filters::FilterBoundingBox 41 | params: 42 | input_pointcloud_layer: 'raw' 43 | outside_pointcloud_layer: 'filtered' 44 | bounding_box_min: [ -1.0, -1.0, -2 ] 45 | bounding_box_max: [ 1.0, 1.0, 2 ] 46 | 47 | - class_name: mp2p_icp_filters::FilterMerge 48 | params: 49 | input_pointcloud_layer: 'filtered' 50 | target_layer: 'accumulated_points' 51 | 52 | # --------------------------------------------------------------- 53 | # 3) Pipeline to apply to the merged data 54 | # --------------------------------------------------------------- 55 | final: 56 | # Remove points that are too far: 57 | - class_name: mp2p_icp_filters::FilterBoundingBox 58 | params: 59 | input_pointcloud_layer: 'accumulated_points' 60 | inside_pointcloud_layer: 'layer1' 61 | bounding_box_min: [ -20.0, -20.0, 0.1 ] 62 | bounding_box_max: [ 20.0, 20.0, 1.5 ] 63 | 64 | # Split into nearby and distant 65 | - class_name: mp2p_icp_filters::FilterBoundingBox 66 | params: 67 | input_pointcloud_layer: 'layer1' 68 | inside_pointcloud_layer: 'close' 69 | outside_pointcloud_layer: 'far' 70 | bounding_box_min: [ -6.0, -6.0, -3.0 ] 71 | bounding_box_max: [ 6.0, 6.0, 3.0 ] 72 | 73 | # Downsample points: 74 | - class_name: mp2p_icp_filters::FilterDecimateVoxels 75 | params: 76 | input_pointcloud_layer: 'close' 77 | output_pointcloud_layer: 'output' 78 | voxel_filter_resolution: 0.10 # [m] 79 | decimate_method: DecimateMethod::ClosestToAverage 80 | # This option flattens the 3D point cloud into a 2D one: 81 | flatten_to: 1.0 # [m] 82 | 83 | - class_name: mp2p_icp_filters::FilterDecimateVoxels 84 | params: 85 | input_pointcloud_layer: 'far' 86 | output_pointcloud_layer: 'output' 87 | voxel_filter_resolution: 0.5 # [m] 88 | decimate_method: DecimateMethod::ClosestToAverage 89 | # This option flattens the 3D point cloud into a 2D one: 90 | flatten_to: 1.0 # [m] 91 | -------------------------------------------------------------------------------- /mrpt_pointcloud_pipeline/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | mrpt_navigation | 3 | | | 4 | | Copyright (c) 2014-2024, Individual contributors, see commit authors | 5 | | See: https://github.com/mrpt-ros-pkg/mrpt_navigation | 6 | | All rights reserved. Released under BSD 3-Clause license. See LICENSE | 7 | +------------------------------------------------------------------------+ */ 8 | 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "mrpt_pointcloud_pipeline_component.cpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | rclcpp::init(argc, argv); 15 | 16 | auto node = std::make_shared(); 17 | 18 | rclcpp::spin(node->get_node_base_interface()); 19 | 20 | rclcpp::shutdown(); 21 | 22 | return 0; 23 | } -------------------------------------------------------------------------------- /mrpt_rawlog/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mrpt_rawlog) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | find_package(tf2_ros REQUIRED) 8 | find_package(nav_msgs REQUIRED) 9 | find_package(sensor_msgs REQUIRED) 10 | find_package(rosbag2_cpp REQUIRED) 11 | find_package(tf2_geometry_msgs REQUIRED) 12 | find_package(cv_bridge REQUIRED) 13 | find_package(mrpt_msgs REQUIRED) 14 | 15 | find_package(mrpt-ros2bridge REQUIRED) 16 | message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") 17 | 18 | if (CMAKE_COMPILER_IS_GNUCXX) 19 | add_compile_options(-Wall) 20 | endif() 21 | 22 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 23 | add_compile_options(-O3) 24 | ENDIF() 25 | 26 | ###################### 27 | ## rosbag2rawlog ## 28 | ###################### 29 | find_package(mrpt-tclap REQUIRED) 30 | 31 | add_executable(rosbag2rawlog 32 | src/rosbag2rawlog.cpp) 33 | 34 | target_link_libraries( 35 | rosbag2rawlog PRIVATE 36 | mrpt::ros2bridge 37 | mrpt::tclap 38 | rclcpp::rclcpp 39 | tf2::tf2 40 | ${nav_msgs_TARGETS} 41 | ${sensor_msgs_TARGETS} 42 | ${rosbag2_cpp_TARGETS} 43 | ${tf2_msgs_TARGETS} 44 | ${cv_bridge_TARGETS} 45 | ${tf2_geometry_msgs_TARGETS} 46 | ${mrpt_msgs_TARGETS} 47 | ) 48 | 49 | 50 | # =================================================================================================== 51 | # Credits: https://gist.github.com/jtanx/96ded5e050d5ee5b19804195ee5cf5f9 52 | function(pad_string output str padchar length) 53 | string(LENGTH "${str}" _strlen) 54 | math(EXPR _strlen "${length} - ${_strlen}") 55 | 56 | if(_strlen GREATER 0) 57 | string(REPEAT ${padchar} ${_strlen} _pad) 58 | string(PREPEND str ${_pad}) 59 | endif() 60 | 61 | set(${output} "${str}" PARENT_SCOPE) 62 | endfunction() 63 | # 2 hex digits for each version part: 64 | # For example: "0.5.1" => "0x000501" 65 | macro(mrpt_version_to_hex VER TARGET_VAR_NAME) 66 | string(REGEX MATCHALL "[0-9]+" __parts "${${VER}}") 67 | 68 | if(__parts) 69 | list(GET __parts 0 __VERSION_NUMBER_MAJOR) 70 | list(GET __parts 1 __VERSION_NUMBER_MINOR) 71 | list(GET __parts 2 __VERSION_NUMBER_PATCH) 72 | pad_string(__VERSION_NUMBER_MAJOR ${__VERSION_NUMBER_MAJOR} "0" 2) 73 | pad_string(__VERSION_NUMBER_MINOR ${__VERSION_NUMBER_MINOR} "0" 2) 74 | pad_string(__VERSION_NUMBER_PATCH ${__VERSION_NUMBER_PATCH} "0" 2) 75 | set(${TARGET_VAR_NAME} "0x${__VERSION_NUMBER_MAJOR}${__VERSION_NUMBER_MINOR}${__VERSION_NUMBER_PATCH}") 76 | else() 77 | set(${TARGET_VAR_NAME} "0x000") 78 | endif() 79 | endmacro() 80 | 81 | # Convert package versions to hex so they can be used in preprocessor for wider 82 | # versions compatibility of "one-source for all": 83 | mrpt_version_to_hex(cv_bridge_VERSION cv_bridge_VERSION_HEX) 84 | target_compile_definitions(rosbag2rawlog PRIVATE CV_BRIDGE_VERSION=${cv_bridge_VERSION_HEX}) 85 | # =================================================================================================== 86 | 87 | 88 | install(TARGETS rosbag2rawlog 89 | DESTINATION bin 90 | ) 91 | 92 | #install( 93 | # DIRECTORY launch 94 | # DESTINATION share/${PROJECT_NAME} 95 | #) 96 | 97 | if(BUILD_TESTING) 98 | find_package(ament_lint_auto REQUIRED) 99 | # the following line skips the linter which checks for copyrights 100 | # uncomment the line when a copyright and license is not present in all source files 101 | #set(ament_cmake_copyright_FOUND TRUE) 102 | # the following line skips cpplint (only works in a git repo) 103 | # uncomment the line when this package is not in a git repo 104 | #set(ament_cmake_cpplint_FOUND TRUE) 105 | ament_lint_auto_find_test_dependencies() 106 | endif() 107 | 108 | ament_package() 109 | -------------------------------------------------------------------------------- /mrpt_rawlog/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mrpt_rawlog 5 | 2.2.2 6 | Nodes and programs to record and play MRPT rawlogs or to transform between rosbags and rawlogs. 7 | 8 | Markus Bader 9 | Jose Luis Blanco-Claraco 10 | 11 | BSD 12 | https://github.com/mrpt-ros-pkg/mrpt_navigation 13 | 14 | cmake 15 | ament_cmake 16 | 17 | 18 | cv_bridge 19 | mrpt_libros_bridge 20 | mrpt_libtclap 21 | mrpt_msgs 22 | nav_msgs 23 | rosbag2_cpp 24 | sensor_msgs 25 | tf2_geometry_msgs 26 | tf2_msgs 27 | tf2_ros 28 | 29 | ament_cmake_lint_cmake 30 | ament_cmake_xmllint 31 | ament_lint_auto 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /mrpt_reactivenav2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mrpt_reactivenav2d) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_components REQUIRED) 8 | find_package(nav_msgs REQUIRED) 9 | find_package(geometry_msgs) 10 | find_package(sensor_msgs REQUIRED) 11 | find_package(mrpt_msgs REQUIRED) 12 | find_package(tf2 REQUIRED) 13 | find_package(tf2_ros) 14 | find_package(tf2_geometry_msgs REQUIRED) 15 | find_package(visualization_msgs REQUIRED) 16 | find_package(mrpt_nav_interfaces REQUIRED) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | find_package(mrpt-ros2bridge REQUIRED) 20 | find_package(mrpt-nav REQUIRED) 21 | find_package(mrpt-kinematics REQUIRED) 22 | 23 | message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") 24 | if(NOT CMAKE_C_STANDARD) 25 | set(CMAKE_C_STANDARD 99) 26 | endif() 27 | 28 | if(NOT CMAKE_CXX_STANDARD) 29 | set(CMAKE_CXX_STANDARD 17) 30 | endif() 31 | 32 | if (CMAKE_COMPILER_IS_GNUCXX) 33 | # High level of warnings. 34 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 35 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 36 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 37 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 38 | if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 39 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 40 | endif() 41 | endif() 42 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 43 | add_compile_options(-O3) 44 | ENDIF() 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | 50 | ## Declare a cpp executable 51 | add_executable(${PROJECT_NAME}_node 52 | src/mrpt_reactivenav2d_node.cpp 53 | include/${PROJECT_NAME}/mrpt_reactivenav2d_node.hpp 54 | ) 55 | 56 | target_include_directories(${PROJECT_NAME}_node 57 | PUBLIC 58 | $ 59 | $ 60 | ) 61 | 62 | ## Specify libraries to link a library or executable target against 63 | target_link_libraries(${PROJECT_NAME}_node PRIVATE 64 | mrpt::nav 65 | mrpt::kinematics 66 | mrpt::ros2bridge 67 | rclcpp::rclcpp 68 | ${rclcpp_components_TARGETS} 69 | ${nav_msgs_TARGETS} 70 | ${sensor_msgs_TARGETS} 71 | ${geometry_msgs_TARGETS} 72 | ${mrpt_msgs_TARGETS} 73 | tf2::tf2 74 | ${tf2_ros_TARGETS} 75 | ${tf2_geometry_msgs_TARGETS} 76 | ${visualization_msgs_TARGETS} 77 | ${mrpt_nav_interfaces_TARGETS} 78 | ) 79 | 80 | ############# 81 | ## Install ## 82 | ############# 83 | 84 | # all install targets should use catkin DESTINATION variables 85 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 86 | 87 | 88 | ## Mark executables and/or libraries for installation 89 | install(TARGETS ${PROJECT_NAME}_node 90 | DESTINATION lib/${PROJECT_NAME} 91 | ) 92 | 93 | ## Mark other files for installation (e.g. launch and bag files, etc.) 94 | install(DIRECTORY 95 | launch 96 | params 97 | DESTINATION share/${PROJECT_NAME} 98 | ) 99 | 100 | ############# 101 | ## Testing ## 102 | ############# 103 | if(BUILD_TESTING) 104 | find_package(ament_lint_auto REQUIRED) 105 | # the following line skips the linter which checks for copyrights 106 | # uncomment the line when a copyright and license is not present in all source files 107 | #set(ament_cmake_copyright_FOUND TRUE) 108 | # the following line skips cpplint (only works in a git repo) 109 | # uncomment the line when this package is not in a git repo 110 | #set(ament_cmake_cpplint_FOUND TRUE) 111 | ament_lint_auto_find_test_dependencies() 112 | endif() 113 | 114 | ament_package() -------------------------------------------------------------------------------- /mrpt_reactivenav2d/README.md: -------------------------------------------------------------------------------- 1 | # mrpt_reactivenav2d 2 | 3 | ## Overview 4 | This package provides a ROS 2 node for reactive navigation for wheeled robots 5 | using MRPT navigation algorithms (TP-Space). 6 | 7 | ## How to cite 8 | 9 |
10 | Main papers 11 | 12 | IROS06 ([PDF](https://ingmec.ual.es/~jlblanco/papers/blanco2006tps_IROS.pdf)) 13 | ```bibtex 14 | @INPROCEEDINGS{, 15 | author = {Blanco, Jos{\'{e}}-Luis and Gonz{\'{a}}lez-Jim{\'{e}}nez, Javier and Fern{\'{a}}ndez-Madrigal, Juan-Antonio}, 16 | month = oct, 17 | title = {The Trajectory Parameter Space (TP-Space): A New Space Representation for Non-Holonomic Mobile Robot Reactive Navigation}, 18 | booktitle = {IEEE International Conference on Intelligent Robots and Systems (IROS'06)}, 19 | year = {2006}, 20 | location = {Beijing (China)} 21 | } 22 | ``` 23 | 24 |
25 | 26 |
27 | Other related papers 28 | 29 | [IEEE RAM 2023](https://ieeexplore.ieee.org/abstract/document/10355540/) 30 | ```bibtex 31 | @ARTICLE{xiao2023barn, 32 | author = {Xiao, Xuesu and Xu, Zifan and Warnell, Garrett and Stone, Peter and Gebelli Guinjoan, Ferran and T Rodrigues, Romulo and Bruyninckx, Herman and Mandala, Hanjaya and Christmann, Guilherme and Blanco, Jos{\'{e}}-Luis and Somashekara Rai, Shravan}, 33 | month = {{aug}}, 34 | title = {Autonomous Ground Navigation in Highly Constrained Spaces: Lessons learned from The 2nd BARN Challenge at ICRA 2023}, 35 | journal = {IEEE Robotics & Automation Magazine}, 36 | volume = {30}, 37 | number = {4}, 38 | year = {2023}, 39 | url = {https://ieeexplore.ieee.org/abstract/document/10355540/}, 40 | doi = {10.1109/MRA.2023.3322920}, 41 | pages = {91--97} 42 | } 43 | ``` 44 | 45 | IJARS 2015 [PDF](https://ingmec.ual.es/~jlblanco/papers/blanco2015tps_rrt.pdf) 46 | ```bibtex 47 | @ARTICLE{bellone2015tprrt, 48 | author = {Blanco, Jos{\'{e}}-Luis and Bellone, Mauro and Gim{\'{e}}nez-Fern{\'{a}}ndez, Antonio}, 49 | month = {{{may}}}, 50 | title = {TP-Space RRT: Kinematic path planning of non-holonomic any-shape vehicles}, 51 | journal = {International Journal of Advanced Robotic Systems}, 52 | volume = {12}, 53 | number = {55}, 54 | year = {2015}, 55 | url = {http://www.intechopen.com/journals/international_journal_of_advanced_robotic_systems/tp-space-rrt-ndash-kinematic-path-planning-of-non-holonomic-any-shape-vehicles}, 56 | doi = {10.5772/60463} 57 | } 58 | ``` 59 | 60 |
61 | 62 | 63 | ## Configuration 64 | 65 | The main **parameters** of our approach are: 66 | 67 | - **Robot shape**: The "2D foot-print" of the robot. 68 | - **PTGs**: One or more families of trajectories, used to look ahead and plan what is the most interesting next motor command. 69 | - **Motion decision**: These parameters can be tuned to modify the heuristics that control what motor actions are selected. 70 | 71 | ## Demos 72 | 73 | ### Navigation in simulated warehouse 74 | 75 | ros2 launch mrpt_tutorials reactive_nav_demo_with_mvsim.launch.py 76 | 77 | to start: 78 | 79 | * ``mrpt_reactivenav2d`` for the autonomous reactive navigation (this package), 80 | * ``mrpt_pointcloud_pipeline`` for generating input obstacles for the navigator from lidar data, 81 | * ``mvsim`` to simulate a live robot that can be controlled by the navigator. 82 | 83 | ## Node: mrpt_reactivenav2d_node 84 | 85 | ### Working rationale 86 | 87 | The C++ ROS 2 node comprises XXX 88 | 89 | 90 | ### ROS 2 parameters 91 | 92 | XXX 93 | 94 | ### Subscribed topics 95 | * xxx 96 | 97 | ### Published topics 98 | * xxx 99 | 100 | * `reactivenav_events` (`std_msgs/String`): One message with a string keyword will be published for each important navigation event. 101 | The list possible message strings are: 102 | * `START`: Start of navigation 103 | * `END`: Successful end of navigation command (reach of single goal, or final waypoint of waypoint list). 104 | * `WP_REACHED (REACHED|SKIPPED)`: Reached an intermediary waypoint in waypoint list navigation. The waypoint may have been physically reached or just skipped. 105 | * `WP_NEW `: Heading towards a new intermediary/final waypoint in waypoint list navigation. 106 | * `ERROR`: Error asking sensory data from robot or sending motor commands. 107 | * `WAY_SEEMS_BLOCKED`: No progression made towards target for a predefined period of time. 108 | * `APPARENT_COLLISION`: Apparent collision event (i.e. there is at least one obstacle point inside the robot shape). 109 | * `CANNOT_GET_CLOSER`: Target seems to be blocked by an obstacle. 110 | 111 | ### Actions 112 | 113 | #### NavigateGoal 114 | 115 | Can be used to navigate to a single target SE(2) pose. 116 | 117 |
118 | Example output 119 | 120 | Example CLI call: 121 | ```bash 122 | ros2 action send_goal /rnav/navigate_goal mrpt_nav_interfaces/action/NavigateGoal --feedback \ 123 | "target: 124 | header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'} 125 | pose: {position: {x: 3.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}} 126 | " 127 | ``` 128 | 129 | Example output: 130 | ``` 131 | Goal accepted with ID: 38d28cd7ad604e41bcfa7d50162fbd93 132 | 133 | Feedback: 134 | state: 135 | total_waypoints: 1 136 | reached_waypoints: 0 137 | 138 | Feedback: 139 | state: 140 | total_waypoints: 1 141 | reached_waypoints: 0 142 | 143 | [...] 144 | 145 | Feedback: 146 | state: 147 | total_waypoints: 1 148 | reached_waypoints: 0 149 | 150 | Result: 151 | state: 152 | navigation_status: 0 153 | 154 | Goal finished with status: SUCCEEDED 155 | ``` 156 |
157 | 158 | #### NavigateWaypoints 159 | 160 | Can be used to navigate to a sequence of waypoints. 161 | 162 |
163 | Example output 164 | 165 | Example CLI call: 166 | ```bash 167 | ros2 action send_goal /rnav/navigate_waypoints mrpt_nav_interfaces/action/NavigateWaypoints --feedback \ 168 | "waypoints: 169 | header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'} 170 | waypoints: 171 | - target: {position: {x: 2.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}} 172 | ignore_heading: true 173 | allow_skip: false 174 | allowed_distance: 0.30 175 | - target: {position: {x: 4.0, y: 0.2, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}} 176 | ignore_heading: true 177 | allow_skip: false 178 | allowed_distance: 0.30 179 | " 180 | ``` 181 | 182 | Example output: 183 | ``` 184 | Goal accepted with ID: f8180e191f0d4f5ca8466c77acbab333 185 | 186 | Feedback: 187 | state: 188 | total_waypoints: 2 189 | reached_waypoints: -1 190 | 191 | Feedback: 192 | state: 193 | total_waypoints: 2 194 | reached_waypoints: 0 195 | 196 | Feedback: 197 | state: 198 | total_waypoints: 2 199 | reached_waypoints: 1 200 | 201 | [...] 202 | 203 | Feedback: 204 | state: 205 | total_waypoints: 2 206 | reached_waypoints: 1 207 | 208 | Result: 209 | state: 210 | navigation_status: 0 211 | 212 | Goal finished with status: SUCCEEDED 213 | ``` 214 |
215 | 216 | 217 | ### Template ROS 2 launch files 218 | 219 | This package provides [launch/reactivenav.launch.py](launch/reactivenav.launch.py): 220 | 221 | ros2 launch mrpt_reactivenav2d reactivenav.launch.py 222 | 223 | which can be used in user projects to launch the MRPT reactive navigation node, by setting these [launch arguments](https://docs.ros.org/en/rolling/Tutorials/Intermediate/Launch/Using-Substitutions.html): 224 | 225 | * ``XXX_config_file``: Path to an INI file with... 226 | 227 | 228 | -------------------------------------------------------------------------------- /mrpt_reactivenav2d/launch/rnav.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for mrpt_reactivenav2d, intended to be included in user's 2 | # launch files. 3 | # 4 | # See the docs on the configurable launch arguments for this file in: 5 | # https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_reactivenav2d 6 | # 7 | # For ready-to-launch demos, see: https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_tutorials 8 | # 9 | 10 | from launch import LaunchDescription 11 | from launch.substitutions import TextSubstitution 12 | from launch.substitutions import LaunchConfiguration 13 | from launch_ros.actions import Node 14 | from launch.actions import DeclareLaunchArgument, Shutdown 15 | from ament_index_python import get_package_share_directory 16 | from launch.actions import GroupAction 17 | from launch_ros.actions import PushRosNamespace 18 | import os 19 | 20 | 21 | def generate_launch_description(): 22 | # myPkgDir = get_package_share_directory("mrpt_reactivenav2d") 23 | # print('myPkgDir: ' + myPkgDir) 24 | 25 | default_rnav_cfg_file = os.path.join( 26 | get_package_share_directory('mrpt_reactivenav2d'), 27 | 'params', 'reactive2d_default.ini') 28 | 29 | rnav_cfg_file_arg = DeclareLaunchArgument( 30 | 'rnav_cfg_file', 31 | default_value=default_rnav_cfg_file 32 | ) 33 | topic_robot_shape_arg = DeclareLaunchArgument( 34 | 'topic_robot_shape', 35 | default_value='' 36 | ) 37 | topic_obstacles_arg = DeclareLaunchArgument( 38 | 'topic_obstacles', 39 | default_value='/local_map_pointcloud' 40 | ) 41 | topic_reactive_nav_goal_arg = DeclareLaunchArgument( 42 | 'topic_reactive_nav_goal', 43 | default_value='/goal_pose' 44 | ) 45 | topic_reactive_nav_waypoint_sequence_arg = DeclareLaunchArgument( 46 | 'topic_wp_seq', 47 | default_value='/rnav_waypoint_sequence' 48 | ) 49 | nav_period_arg = DeclareLaunchArgument( 50 | 'nav_period', 51 | default_value='0.20' 52 | ) 53 | frameid_reference_arg = DeclareLaunchArgument( 54 | 'frameid_reference', 55 | default_value='map' 56 | ) 57 | frameid_robot_arg = DeclareLaunchArgument( 58 | 'frameid_robot', 59 | default_value='base_link' 60 | ) 61 | save_nav_log_arg = DeclareLaunchArgument( 62 | 'save_nav_log', 63 | default_value='False' 64 | ) 65 | topic_cmd_vel_arg = DeclareLaunchArgument( 66 | 'topic_cmd_vel', 67 | default_value='/cmd_vel' 68 | ) 69 | pure_pursuit_mode_launch_arg = DeclareLaunchArgument( 70 | "pure_pursuit_mode", 71 | default_value=TextSubstitution(text=str("False")), 72 | description="If enabled, no obstacle avoidance will be attempted" 73 | ) 74 | 75 | log_level_launch_arg = DeclareLaunchArgument( 76 | "log_level", 77 | default_value=TextSubstitution(text=str("INFO")), 78 | description="Logging level" 79 | ) 80 | 81 | emit_shutdown_action = Shutdown(reason='launch is shutting down') 82 | 83 | # remappings={('/cmd_vel', LaunchConfiguration('cmd_vel_out'))}, 84 | 85 | node_rnav2d_launch = Node( 86 | package='mrpt_reactivenav2d', 87 | executable='mrpt_reactivenav2d_node', 88 | name='mrpt_reactivenav2d_node', 89 | output='screen', 90 | parameters=[ 91 | { 92 | 'cfg_file_reactive': LaunchConfiguration('rnav_cfg_file'), 93 | 'topic_robot_shape': LaunchConfiguration('topic_robot_shape'), 94 | 'topic_obstacles': LaunchConfiguration('topic_obstacles'), 95 | 'topic_reactive_nav_goal': LaunchConfiguration('topic_reactive_nav_goal'), 96 | 'topic_wp_seq': LaunchConfiguration('topic_wp_seq'), 97 | 'nav_period': LaunchConfiguration('nav_period'), 98 | 'frameid_reference': LaunchConfiguration('frameid_reference'), 99 | 'frameid_robot': LaunchConfiguration('frameid_robot'), 100 | 'save_nav_log': LaunchConfiguration('save_nav_log'), 101 | 'topic_cmd_vel': LaunchConfiguration('topic_cmd_vel'), 102 | 'pure_pursuit_mode': LaunchConfiguration('pure_pursuit_mode'), 103 | } 104 | ], 105 | arguments=['--ros-args', '--log-level', 106 | LaunchConfiguration('log_level')], 107 | on_exit=[emit_shutdown_action] 108 | ) 109 | 110 | ld = LaunchDescription([ 111 | rnav_cfg_file_arg, 112 | log_level_launch_arg, 113 | topic_robot_shape_arg, 114 | topic_obstacles_arg, 115 | topic_reactive_nav_goal_arg, 116 | topic_reactive_nav_waypoint_sequence_arg, 117 | nav_period_arg, 118 | frameid_reference_arg, 119 | frameid_robot_arg, 120 | save_nav_log_arg, 121 | topic_cmd_vel_arg, 122 | pure_pursuit_mode_launch_arg, 123 | node_rnav2d_launch, 124 | ]) 125 | 126 | # Namespace to avoid clash launch argument names with the parent scope: 127 | return LaunchDescription([GroupAction( 128 | actions=[ 129 | PushRosNamespace(namespace='rnav'), 130 | ld 131 | ])]) 132 | -------------------------------------------------------------------------------- /mrpt_reactivenav2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_reactivenav2d 4 | 2.2.2 5 | Reactive navigation for wheeled robots using MRPT navigation algorithms (TP-Space) 6 | 7 | Jose-Luis Blanco-Claraco 8 | Shravan S Rai 9 | 10 | Jose-Luis Blanco-Claraco 11 | 12 | BSD 13 | https://github.com/mrpt-ros-pkg/mrpt_navigation 14 | 15 | ament_cmake 16 | 17 | 18 | geometry_msgs 19 | mrpt_libnav 20 | mrpt_libros_bridge 21 | mrpt_nav_interfaces 22 | nav_msgs 23 | rclcpp 24 | rclcpp_components 25 | sensor_msgs 26 | std_msgs 27 | stereo_msgs 28 | tf2 29 | tf2_geometry_msgs 30 | tf2_ros 31 | visualization_msgs 32 | 33 | ament_cmake_lint_cmake 34 | ament_cmake_xmllint 35 | ament_lint_auto 36 | 37 | 38 | ament_cmake 39 | 40 | 41 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_tps_astar_planner_node 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | * FIX: remove usage of obsolete ament_target_dependencies() 8 | * Merge pull request `#156 `_ from r-aguilera/ros2 9 | mrpt_tps_astar_planner_node: improves on obstacle points update 10 | * astar_planner: limit excessive obstacle points ... 11 | logging 12 | * astar_planner: prevent node from shutting down ... 13 | on missing tf info 14 | * astar planner node: it will always return the best found path, even if success==false 15 | * Merge pull request `#155 `_ from r-aguilera/ros2 16 | Fix MakePlanFromTo srv using robot pose as start 17 | * Fix MakePlanFromTo srv using robot pose as start 18 | * Contributors: Jose Luis Blanco-Claraco, Raúl Aguilera 19 | 20 | 2.2.1 (2024-10-12) 21 | ------------------ 22 | * Update demo for astar 23 | * Publish costmaps to ROS for visualization too apart of the custom MRPT GUI 24 | * Improve astar navigation demo 25 | * astar planner: add refine() step and parameter to optionally disable it 26 | * astar node: add two more launch args: problem_world_bbox_margin and problem_world_bbox_ignore_obstacles 27 | * astar params: add more comments and tune for speed 28 | * PTGs .ini: Add docs on how to enable backward motions 29 | * Contributors: Jose Luis Blanco-Claraco 30 | 31 | 2.2.0 (2024-09-25) 32 | ------------------ 33 | * fix missing linters; tune tutorial params 34 | * Update URL entries in package.xml to each package proper documentation 35 | * ament linters: manually enable just cmake and xml linters 36 | * Add roslog INFO traces to measure time spent initializing PTGs 37 | * reformat clang-format with 100 column width 38 | * Contributors: Jose Luis Blanco-Claraco 39 | 40 | 2.1.1 (2024-09-02) 41 | ------------------ 42 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 43 | * Fix duplicated deps 44 | * update dependencies 45 | * Depend on new mrpt_lib packages (deprecate mrpt2) 46 | * Contributors: Jose Luis Blanco-Claraco 47 | 48 | * Remove temporary workaround in for buggy mrpt_libros_bridge package.xml 49 | * Fix duplicated deps 50 | * update dependencies 51 | * Depend on new mrpt_lib packages (deprecate mrpt2) 52 | * Contributors: Jose Luis Blanco-Claraco 53 | 54 | 2.1.0 (2024-08-08) 55 | ------------------ 56 | * Merge pull request `#147 `_ from r-aguilera/ros2 57 | mrpt_tps_astar_planner_node tf data availability 58 | * added max duration to wait for tf data 59 | * Merge pull request `#146 `_ from r-aguilera/ros2 60 | parameterized astar plan waypoints fields 61 | * Merge pull request `#145 `_ from mrpt-ros-pkg/ros2_astar 62 | add WaypointSequence missing header 63 | * Implement two services for making navigation plans; code clean up; delete "replan" topic 64 | * FIX: transform point cloud observations 65 | * add frame_id params 66 | * reset planner obstacles between path plans 67 | * Add param to create subscribers with map-like QoS 68 | * Enable multiple obstacle sources (grids and points) 69 | * delete old ros1 files 70 | * Merge pull request `#144 `_ from mrpt-ros-pkg/wip/port-tps-astar 71 | Port TPS A* planner to ROS2 72 | * Contributors: Jose Luis Blanco-Claraco, Raúl Aguilera, SRai22 73 | 74 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mrpt_tps_astar_planner) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_components REQUIRED) 8 | find_package(nav_msgs REQUIRED) 9 | find_package(geometry_msgs) 10 | find_package(sensor_msgs REQUIRED) 11 | find_package(mrpt_msgs REQUIRED) 12 | find_package(mrpt_nav_interfaces REQUIRED) 13 | find_package(tf2 REQUIRED) 14 | find_package(tf2_ros) 15 | find_package(tf2_geometry_msgs REQUIRED) 16 | find_package(visualization_msgs REQUIRED) 17 | 18 | 19 | ## System dependencies are found with CMake's conventions 20 | find_package(mrpt-ros2bridge REQUIRED) 21 | find_package(mrpt-nav REQUIRED) 22 | find_package(mrpt-kinematics REQUIRED) 23 | find_package(mrpt-math REQUIRED) 24 | find_package(mrpt-system REQUIRED) 25 | find_package(mrpt-containers REQUIRED) 26 | find_package(mrpt-maps REQUIRED) 27 | find_package(mrpt-gui REQUIRED) 28 | find_package(mrpt-opengl REQUIRED) 29 | 30 | find_package(mrpt_path_planning REQUIRED) 31 | if (TARGET mrpt_path_planning AND NOT TARGET mrpt_path_planning::mrpt_path_planning) 32 | add_library(mrpt_path_planning::mrpt_path_planning ALIAS mrpt_path_planning) 33 | endif() 34 | 35 | message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") 36 | 37 | if(NOT CMAKE_C_STANDARD) 38 | set(CMAKE_C_STANDARD 99) 39 | endif() 40 | 41 | if(NOT CMAKE_CXX_STANDARD) 42 | set(CMAKE_CXX_STANDARD 17) 43 | endif() 44 | 45 | if (CMAKE_COMPILER_IS_GNUCXX) 46 | # High level of warnings. 47 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 48 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 49 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 50 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 51 | if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 52 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 53 | endif() 54 | endif() 55 | 56 | ########### 57 | ## Build ## 58 | ########### 59 | 60 | ## Declare a cpp executable 61 | add_executable(${PROJECT_NAME}_node 62 | src/mrpt_tps_astar_planner_node.cpp 63 | ) 64 | 65 | #target_include_directories(${PROJECT_NAME}_node 66 | # PUBLIC 67 | # $ 68 | # $ 69 | #) 70 | 71 | # Specify libraries to link a library or executable target against 72 | target_link_libraries(${PROJECT_NAME}_node PRIVATE 73 | mrpt::nav 74 | mrpt::kinematics 75 | mrpt::ros2bridge 76 | mrpt_path_planning::mrpt_path_planning 77 | rclcpp::rclcpp 78 | ${rclcpp_components_TARGETS} 79 | ${nav_msgs_TARGETS} 80 | ${sensor_msgs_TARGETS} 81 | ${geometry_msgs_TARGETS} 82 | ${mrpt_msgs_TARGETS} 83 | ${mrpt_nav_interfaces_TARGETS} 84 | tf2::tf2 85 | ${tf2_ros_TARGETS} 86 | ${tf2_geometry_msgs_TARGETS} 87 | ${visualization_msgs_TARGETS} 88 | ) 89 | 90 | ############# 91 | ## Install ## 92 | ############# 93 | 94 | # all install targets should use catkin DESTINATION variables 95 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 96 | 97 | 98 | ## Mark executables and/or libraries for installation 99 | install(TARGETS ${PROJECT_NAME}_node 100 | DESTINATION lib/${PROJECT_NAME} 101 | ) 102 | 103 | ## Mark other files for installation (e.g. launch and bag files, etc.) 104 | install(DIRECTORY 105 | launch 106 | configs 107 | DESTINATION share/${PROJECT_NAME} 108 | ) 109 | 110 | ############# 111 | ## Testing ## 112 | ############# 113 | if(BUILD_TESTING) 114 | find_package(ament_lint_auto REQUIRED) 115 | # the following line skips the linter which checks for copyrights 116 | # uncomment the line when a copyright and license is not present in all source files 117 | #set(ament_cmake_copyright_FOUND TRUE) 118 | # the following line skips cpplint (only works in a git repo) 119 | # uncomment the line when this package is not in a git repo 120 | #set(ament_cmake_cpplint_FOUND TRUE) 121 | ament_lint_auto_find_test_dependencies() 122 | endif() 123 | 124 | ament_package() 125 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/README.md: -------------------------------------------------------------------------------- 1 | # mrpt_tps_astar_planner 2 | 3 | ## Overview 4 | This package provides a ROS 2 node that uses the PTG-based A* planner in mrpt_path_planning 5 | to publish waypoint sequences moving a non-holonomic robot from A to B, taking into account 6 | its real shape, orientation, and kinematic constraints. 7 | 8 | ## How to cite 9 | 10 |
11 | TBD! 12 |
13 | 14 | 15 | ## Configuration 16 | 17 | Write me! 18 | 19 | ## Demos 20 | 21 | Write me! 22 | 23 | ## Node: mrpt_tps_astar_planner_node 24 | 25 | ### Working rationale 26 | 27 | Uses A* over a SE(2) lattice and PTGs to sample collision-free paths. The implementation is an anytime algorithm. 28 | 29 | 30 | ### ROS 2 parameters 31 | 32 | * `topic_wp_seq_pub` (Default: `/waypoints`) Desired name of the topic in which to publish the calculated paths. 33 | 34 | * `topic_goal_sub`: The name of the topic to subscribe for goal poses (`geometry_msgs/PoseStamped`). 35 | 36 | * `show_gui`: Shows its own MRPT GUI with the planned paths. 37 | 38 | * `topic_gridmap_sub`: One or more (comma separated) topic names to subscribe for occupancy grid maps. 39 | 40 | * `topic_obstacle_points_sub`: One or more (comma separated) topic names to subscribe for obstacle points. 41 | 42 | 43 | ### Subscribed topics 44 | * xxx 45 | 46 | ### Published topics 47 | * `` (Default: `/waypoints`) (`mrpt_msgs::msg::WaypointSequence`): Calculated trajectory, in mrpt_msgs format with complete details for each waypoints. 48 | * `_path` (Default: `/waypoints_path`) (`nav_msgs::msg::Path`): Calculated trajectory, as `nav_msgs::Path`. Mostly useful for visualization only. 49 | 50 | ### Services 51 | 52 | Write me! 53 | 54 | ### Template ROS 2 launch files 55 | 56 | This package provides [launch/tps_astar_planner.launch.py](launch/tps_astar_planner.launch.py): 57 | 58 | ros2 launch tps_astar_planner tps_astar_planner.launch.py 59 | 60 | which can be used in user projects to launch the planner, by setting these [launch arguments](https://docs.ros.org/en/rolling/Tutorials/Intermediate/Launch/Using-Substitutions.html): 61 | 62 | * ``XXX``: XXX 63 | 64 | 65 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/configs/ini/barn-pf-localization.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------ 2 | # Config file for the application PF Localization 3 | # See: http://www.mrpt.org/list-of-mrpt-apps/application-pf-localization/ 4 | #------------------------------------------------------ 5 | 6 | #--------------------------------------------------------------------------- 7 | # Section: [KLD_options] 8 | # Use: Options for the adaptive sample size KLD-algorithm 9 | # Refer to paper: 10 | # D. Fox, W. Burgard, F. Dellaert, and S. Thrun, "Monte Carlo localization: 11 | # Efficient position estimation for mobile robots," Proc. of the 12 | # National Conference on Artificial Intelligence (AAAI),v.113, p.114,1999. 13 | #--------------------------------------------------------------------------- 14 | [KLD_options] 15 | KLD_binSize_PHI_deg=10 16 | KLD_binSize_XY=0.10 17 | KLD_delta=0.01 18 | KLD_epsilon=0.01 19 | KLD_maxSampleSize=2000 20 | KLD_minSampleSize=250 21 | KLD_minSamplesPerBin=0 22 | 23 | #--------------------------------------------------------------------------- 24 | # Section: [PF_options] 25 | # Use: The parameters for the PF algorithms 26 | #--------------------------------------------------------------------------- 27 | [PF_options] 28 | # The Particle Filter algorithm: 29 | # 0: pfStandardProposal *** 30 | # 1: pfAuxiliaryPFStandard 31 | # 2: pfOptimalProposal 32 | # 3: pfAuxiliaryPFOptimal *** 33 | # 34 | PF_algorithm=0 35 | 36 | # The Particle Filter Resampling method: 37 | # 0: prMultinomial 38 | # 1: prResidual 39 | # 2: prStratified 40 | # 3: prSystematic 41 | resamplingMethod=0 42 | 43 | # Set to 1 to enable KLD adaptive sample size: 44 | adaptiveSampleSize=1 45 | 46 | # Only for algorithm=3 (pfAuxiliaryPFOptimal) 47 | pfAuxFilterOptimal_MaximumSearchSamples=10 48 | 49 | # Resampling threshold 50 | BETA=0.5 51 | 52 | # Number of particles (IGNORED IN THIS APPLICATION, SUPERSEDED BY "particles_count" below) 53 | sampleSize=1 54 | 55 | 56 | #--------------------------------------------------------------------------- 57 | # Default "noise" parameters for odometry in observations-only rawlog formats 58 | #--------------------------------------------------------------------------- 59 | [DummyOdometryParams] 60 | minStdXY = 0.10 // (meters) 61 | minStdPHI = 2.0 // (degrees) 62 | 63 | 64 | #--------------------------------------------------------------------------- 65 | # Section: [LocalizationExperiment] 66 | # Use: Here come global parameters for the app. 67 | #--------------------------------------------------------------------------- 68 | [LocalizationExperiment] 69 | 70 | # The map in the ".simplemap" format or just a ".gridmap" (the program detects the file extension) 71 | # This map is used to localize the robot within it: 72 | map_file= 73 | 74 | # The source file (RAW-LOG) with action/observation pairs 75 | rawlog_file= 76 | 77 | # The directory where the log files will be saved (left in blank if no log is desired) 78 | logOutput_dir=LOG_LOCALIZATION 79 | 80 | # Freq. of 3D scene log 81 | 3DSceneFrequency=1 82 | 83 | # Initial number of particles (if dynamic sample size is enabled, the population may change afterwards). 84 | particles_count=2000 85 | 86 | # 1: Uniform distribution over the range, 0: Uniform distribution over the free cells of the gridmap in the range: 87 | init_PDF_mode=0 88 | init_PDF_min_x=-1 89 | init_PDF_max_x=1 90 | init_PDF_min_y=-1 91 | init_PDF_max_y=1 92 | init_PDF_min_phi_deg = -45 93 | init_PDF_max_phi_deg = 45 94 | 95 | 96 | SHOW_PROGRESS_3D_REAL_TIME = true 97 | 98 | # ==================================================== 99 | # 100 | # MULTIMETRIC MAP CONFIGURATION 101 | # 102 | # ==================================================== 103 | [MetricMap] 104 | # Creation of maps: 105 | occupancyGrid_count=1 106 | gasGrid_count=0 107 | landmarksMap_count=0 108 | pointsMap_count=0 109 | beaconMap_count=0 110 | 111 | # Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3) 112 | likelihoodMapSelection=-1 113 | 114 | # Enables (1) / Disables (0) insertion into specific maps: 115 | enableInsertion_pointsMap=1 116 | enableInsertion_landmarksMap=1 117 | enableInsertion_gridMaps=1 118 | enableInsertion_gasGridMaps=1 119 | enableInsertion_beaconMap=1 120 | 121 | # ==================================================== 122 | # MULTIMETRIC MAP: OccGrid #00 123 | # ==================================================== 124 | # Creation Options for OccupancyGridMap 00: 125 | [MetricMap_occupancyGrid_00_creationOpts] 126 | resolution=0.06 127 | 128 | # Insertion Options for OccupancyGridMap 00: 129 | [MetricMap_occupancyGrid_00_insertOpts] 130 | mapAltitude=0 131 | useMapAltitude=0 132 | maxDistanceInsertion=15 133 | maxOccupancyUpdateCertainty=0.55 134 | considerInvalidRangesAsFreeSpace=1 135 | minLaserScanNoiseStd=0.001 136 | 137 | # Likelihood Options for OccupancyGridMap 00: 138 | [MetricMap_occupancyGrid_00_likelihoodOpts] 139 | likelihoodMethod=4 // 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II 140 | 141 | LF_decimation=5 142 | LF_stdHit=0.40 143 | LF_maxCorrsDistance=0.30 144 | LF_zHit=0.90 145 | LF_zRandom=0.10 146 | LF_maxRange=80 147 | LF_alternateAverageMethod=0 148 | 149 | MI_exponent=10 150 | MI_skip_rays=10 151 | MI_ratio_max_distance=2 152 | 153 | rayTracing_useDistanceFilter=0 154 | rayTracing_decimation=10 155 | rayTracing_stdHit=0.30 156 | 157 | consensus_takeEachRange=30 158 | consensus_pow=1 159 | 160 | 161 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/configs/ini/ptgs_jackal.ini: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------ 2 | # Example PTG configuration file for "selfdriving" library 3 | # ------------------------------------------------------------------------ 4 | 5 | # Max linear vel (m/s): 6 | @define ROBOT_MAX_V 1.0 7 | # Max angular vel (deg/s): 8 | @define ROBOT_MAX_W 60.0 9 | # Max distance to "foresee" obstacles (m): 10 | @define NAV_MAX_REF_DIST 3.0 # no need to be larger than (A* timestep) * (max_vel) 11 | 12 | 13 | [SelfDriving] 14 | min_obstacles_height = 0.0 // Minimum `z` coordinate of obstacles to be considered fo collision checking 15 | max_obstacles_height = 2.0 // Maximum `z` coordinate of obstacles to be considered fo collision checking 16 | 17 | # PTGs: See classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( https://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html) 18 | # refer to papers for details. 19 | #------------------------------------------------------------------------------ 20 | PTG_COUNT = 1 # Set to 2 to enable moving backwards 21 | 22 | PTG0_Type = ptg::DiffDrive_C 23 | PTG0_resolution = 0.05 # 0.05 # Look-up-table cell size or resolution (in meters) 24 | PTG0_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths 25 | PTG0_num_paths= 151 26 | PTG0_v_max_mps = ${ROBOT_MAX_V}*(1-abs(dir)/3.2) 27 | PTG0_w_max_dps = ${ROBOT_MAX_W} 28 | PTG0_K = +1.0 # +1: forwards, -1: backward 29 | PTG0_score_priority = 1.0 30 | 31 | PTG1_Type = ptg::DiffDrive_C 32 | PTG1_resolution = 0.075 # 0.05 # Look-up-table cell size or resolution (in meters) 33 | PTG1_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths 34 | PTG1_num_paths= 161 35 | PTG1_v_max_mps = ${ROBOT_MAX_V} 36 | PTG1_w_max_dps = ${ROBOT_MAX_W} 37 | PTG1_K = -1.0 # +1: forwards, -1: backward 38 | PTG1_score_priority = 0.5 39 | 40 | # Default 2D robot shape for collision checks: 41 | # Each PTG will use only one of either (a) polygonal 2D shape or, (b) radius of a circular shape 42 | 43 | # 44 | # IMPORTANT: Barn map obstacles are points, we must ADD the cylinders radius: 45 | # 46 | #RobotModel_shape2D_xs = 0.265 0.265 -0.265 -0.265 47 | #RobotModel_shape2D_ys = -0.225 0.225 0.225 -0.225 48 | RobotModel_shape2D_xs = 0.30 0.30 -0.30 -0.30 49 | RobotModel_shape2D_ys = -0.27 0.27 0.27 -0.27 50 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/configs/params/costmap-obstacles.yaml: -------------------------------------------------------------------------------- 1 | # For CostEvaluatorCostMap 2 | resolution: 0.10 3 | preferredClearanceDistance: 1.50 4 | maxCost: 15.0 5 | 6 | useAverageOfPath: false 7 | 8 | maxRadiusFromRobot: 0.0 # [meters] 9 | 10 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/configs/params/costmap-prefer-waypoints.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | waypointInfluenceRadius: 1.5 4 | costScale: 0.5 5 | useAverageOfPath: true 6 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/configs/params/nav-engine-params.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | planner_bbox_margin: 4.0 # [m] 4 | 5 | enqueuedActionsToleranceXY: 0.05 # [m] 6 | enqueuedActionsTolerancePhi: 15.0 # [deg] 7 | 8 | enqueuedActionsTimeoutMultiplier: 4 # (ratio) 9 | 10 | minEdgeTimeToRefinePath: 0.75 # [seconds] 11 | 12 | lookAheadImmediateCollisionChecking: 2.5 # [seconds] 13 | 14 | maxDistanceForTargetApproach: 1.0 # [m] 15 | maxRelativeHeadingForTargetApproach: 180 # [deg] 16 | 17 | # For debugging later with the MRPT navlog-viewer app: 18 | #generateNavLogFiles: true 19 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/configs/params/planner-params.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | # It is safe to use coarse lattice values, since a post-processing path 4 | # refining stage will normally improve the result, with a very reduced overall 5 | # time cost: 6 | grid_resolution_xy: 0.20 # [meters] 7 | grid_resolution_yaw: 10.0 # [deg] 8 | 9 | # find_feasible_paths_to_neighbors() params: 10 | # Note: path refining will always use *all* PTG trajectories, despite this value 11 | max_ptg_trajectories_to_explore: 51 12 | max_ptg_speeds_to_explore: 1 13 | ptg_sample_timestamps: [1.0, 2.0] # seconds 14 | 15 | # Weight of heading towards the target: 16 | # the larger, the most favorable to move "looking at" the target direction. 17 | heuristic_heading_weight: 0.25 18 | 19 | SE2_metricAngleWeight: 2.0 20 | pathInterpolatedSegments: 3 21 | 22 | maximumComputationTime: 10.0 # [seconds] 23 | 24 | #saveDebugVisualizationDecimation: 1 25 | debugVisualizationShowEdgeCosts: true 26 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_tps_astar_planner 4 | 2.2.2 5 | ROS Path Planner with A* in TP-Space Engine 6 | 7 | Jose-Luis Blanco-Claraco 8 | Shravan S Rai 9 | 10 | Shravan_S_Rai 11 | Jose-Luis Blanco-Claraco 12 | 13 | BSD 14 | https://github.com/mrpt-ros-pkg/mrpt_navigation 15 | 16 | ament_cmake 17 | 18 | 19 | mrpt_libgui 20 | mrpt_libmaps 21 | mrpt_libnav 22 | mrpt_libros_bridge 23 | mrpt_msgs 24 | mrpt_nav_interfaces 25 | mrpt_path_planning 26 | nav_msgs 27 | rclcpp 28 | rclcpp_components 29 | sensor_msgs 30 | tf2 31 | tf2_geometry_msgs 32 | tf2_ros 33 | visualization_msgs 34 | 35 | ament_cmake_lint_cmake 36 | ament_cmake_xmllint 37 | ament_lint_auto 38 | 39 | 40 | ament_cmake 41 | 42 | 43 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/.gitignore: -------------------------------------------------------------------------------- 1 | ReacNavGrid_*.gz 2 | *.dat.gz 3 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/costmap-obstacles.yaml: -------------------------------------------------------------------------------- 1 | # For CostEvaluatorCostMap 2 | resolution: 0.04 3 | preferredClearanceDistance: 0.60 4 | maxCost: 4.0 5 | 6 | useAverageOfPath: false 7 | 8 | maxRadiusFromRobot: 15.0 # [meters] 9 | 10 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/costmap-prefer-waypoints.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | waypointInfluenceRadius: 1.5 4 | costScale: 0.5 5 | useAverageOfPath: true 6 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/map_pgm_0.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tps_astar_planner/path-planner-sandbox/map_pgm_0.pgm -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/map_pgm_299.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tps_astar_planner/path-planner-sandbox/map_pgm_299.pgm -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/nav-engine-params.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | planner_bbox_margin: 4.0 # [m] 4 | 5 | enqueuedActionsToleranceXY: 0.05 # [m] 6 | enqueuedActionsTolerancePhi: 15.0 # [deg] 7 | 8 | enqueuedActionsTimeoutMultiplier: 1.5 # (ratio) 9 | 10 | minEdgeTimeToRefinePath: 0.75 # [seconds] 11 | 12 | lookAheadImmediateCollisionChecking: 1.0 # [seconds] 13 | 14 | maxDistanceForTargetApproach: 1.0 # [m] 15 | maxRelativeHeadingForTargetApproach: 180 # [deg] 16 | 17 | # For debugging later with the MRPT navlog-viewer app: 18 | #generateNavLogFiles: true 19 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/planner-params.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | # It is safe to use coarse lattice values, since a post-processing path 4 | # refining stage will normally improve the result, with a very reduced overall 5 | # time cost: 6 | grid_resolution_xy: 0.30 # [meters] 7 | grid_resolution_yaw: 25.0 # [deg] 8 | 9 | # find_feasible_paths_to_neighbors() params: 10 | # Note: path refining will always use *all* PTG trajectories, despite this value 11 | max_ptg_trajectories_to_explore: 51 12 | max_ptg_speeds_to_explore: 1 13 | ptg_sample_timestamps: [0.5, 1.0] # seconds 14 | 15 | heuristic_heading_weight: 0.25 16 | SE2_metricAngleWeight: 1.0 17 | pathInterpolatedSegments: 5 18 | 19 | maximumComputationTime: 10.0 # [seconds] 20 | 21 | #saveDebugVisualizationDecimation: 1 22 | debugVisualizationShowEdgeCosts: true 23 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/ptgs_jackal.ini: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------ 2 | # Example PTG configuration file for "selfdriving" library 3 | # ------------------------------------------------------------------------ 4 | 5 | # Max linear vel (m/s): 6 | @define ROBOT_MAX_V 1.0 7 | # Max angular vel (deg/s): 8 | @define ROBOT_MAX_W 60.0 9 | # Max distance to "foresee" obstacles (m): 10 | @define NAV_MAX_REF_DIST 5.0 11 | 12 | 13 | [SelfDriving] 14 | min_obstacles_height = 0.0 // Minimum `z` coordinate of obstacles to be considered fo collision checking 15 | max_obstacles_height = 2.0 // Maximum `z` coordinate of obstacles to be considered fo collision checking 16 | 17 | # PTGs: See classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( https://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html) 18 | # refer to papers for details. 19 | #------------------------------------------------------------------------------ 20 | PTG_COUNT = 2 21 | 22 | PTG0_Type = ptg::DiffDrive_C 23 | PTG0_resolution = 0.05 # Look-up-table cell size or resolution (in meters) 24 | PTG0_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths 25 | PTG0_num_paths= 201 26 | PTG0_v_max_mps = ${ROBOT_MAX_V} 27 | PTG0_w_max_dps = ${ROBOT_MAX_W} 28 | PTG0_K = +1.0 # +1: forwards, -1: backward 29 | PTG0_score_priority = 1.0 30 | 31 | PTG1_Type = ptg::DiffDrive_C 32 | PTG1_resolution = 0.05 # Look-up-table cell size or resolution (in meters) 33 | PTG1_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths 34 | PTG1_num_paths= 201 35 | PTG1_v_max_mps = ${ROBOT_MAX_V} 36 | PTG1_w_max_dps = ${ROBOT_MAX_W} 37 | PTG1_K = -1.0 # +1: forwards, -1: backward 38 | PTG1_score_priority = 0.5 39 | 40 | # Default 2D robot shape for collision checks: 41 | # Each PTG will use only one of either (a) polygonal 2D shape or, (b) radius of a circular shape 42 | RobotModel_shape2D_xs = 0.254 0.254 -0.254 -0.254 43 | RobotModel_shape2D_ys = -0.215 0.215 0.215 -0.215 44 | -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/run-path-plan-test.sh: -------------------------------------------------------------------------------- 1 | path-planner-cli -g "[-2 13 90]" -s "[-2 3 90]" \ 2 | --planner "selfdriving::TPS_Astar" \ 3 | -c ptgs_jackal.ini \ 4 | --obstacles yaml_0.yaml \ 5 | --planner-parameters planner-params.yaml \ 6 | --costmap-obstacles costmap-obstacles.yaml -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/yaml_0.yaml: -------------------------------------------------------------------------------- 1 | image: map_pgm_0.pgm 2 | resolution: 0.15 3 | origin: [-4.5, 0.0, 0] 4 | occupied_thresh: 0.50 5 | free_thresh: 0.50 6 | negate: 0 -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/path-planner-sandbox/yaml_299.yaml: -------------------------------------------------------------------------------- 1 | image: map_pgm_299.pgm 2 | resolution: 0.15 3 | origin: [-4.5, 0.0, 0] 4 | occupied_thresh: 0.50 5 | free_thresh: 0.50 6 | negate: 0 -------------------------------------------------------------------------------- /mrpt_tps_astar_planner/python/initial_pose_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion 5 | from tf.transformations import quaternion_from_euler 6 | 7 | def pose_callback(pose): 8 | print(pose) 9 | 10 | if __name__ == '__main__': 11 | # Initialize the ROS node 12 | rospy.init_node('initial_pose_node') 13 | 14 | # Get the initial position parameters 15 | initial_x = rospy.get_param('initial_x', -2.0) 16 | initial_y = rospy.get_param('initial_y', 3.0) 17 | initial_yaw = rospy.get_param('initial_phi', 1.57) 18 | 19 | # Create a PoseWithCovarianceStamped message 20 | initial_pose = PoseWithCovarianceStamped() 21 | initial_pose.header.frame_id = 'map' 22 | initial_pose.pose.pose.position.x = initial_x 23 | initial_pose.pose.pose.position.y = initial_y 24 | q = quaternion_from_euler(0,0,initial_yaw) 25 | initial_pose.pose.pose.orientation = Quaternion(q[0],q[1],q[2],q[3]) 26 | 27 | # Publish the initial pose to the /initialpose topic 28 | pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=10) 29 | sub = rospy.Subscriber('mrpt_pose', PoseWithCovarianceStamped, pose_callback) 30 | rate = rospy.Rate(1) # 10 Hz 31 | while not rospy.is_shutdown(): 32 | pub.publish(initial_pose) 33 | rate.sleep() 34 | break 35 | -------------------------------------------------------------------------------- /mrpt_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.2 (2025-05-28) 6 | ------------------ 7 | * Fix tutorial demo launchs missing 'use_composable' argument 8 | * Merge pull request `#153 `_ from dppp415/ros2 9 | Composable Nodes 10 | * Fix 11 | * Some fixes and demo 12 | * Contributors: Jose Luis Blanco-Claraco, dppp415 13 | 14 | 2.2.1 (2024-10-12) 15 | ------------------ 16 | * Update demo for astar 17 | * Publish costmaps to ROS for visualization too apart of the custom MRPT GUI 18 | * rviz layout: show rnav selected path 19 | * Improve astar navigation demo 20 | * astar planner: add refine() step and parameter to optionally disable it 21 | * astar node: add two more launch args: problem_world_bbox_margin and problem_world_bbox_ignore_obstacles 22 | * Contributors: Jose Luis Blanco-Claraco 23 | 24 | 2.2.0 (2024-09-25) 25 | ------------------ 26 | * Merge pull request `#149 `_ from mrpt-ros-pkg/feature/utm-coordinates 27 | Support UTM global coordinates for geo-referenciated maps 28 | * demo launch file: add "mm_file" as a proper launch required argument instead of an optional env variable 29 | * fix missing linters; tune tutorial params 30 | * Update URL entries in package.xml to each package proper documentation 31 | * ament linters: manually enable just cmake and xml linters 32 | * Contributors: Jose Luis Blanco-Claraco 33 | 34 | 2.1.1 (2024-09-02) 35 | ------------------ 36 | 37 | 2.1.0 (2024-08-08) 38 | ------------------ 39 | * add basic astar launch demo 40 | * Merge branch 'ros2' into wip/port-tps-astar 41 | * Merge branch 'ros2' into wip/port-tps-astar 42 | * Contributors: Jose Luis Blanco-Claraco 43 | 44 | 2.0.1 (2024-05-28) 45 | ------------------ 46 | 47 | 2.0.0 (2024-05-28) 48 | ------------------ 49 | * Port to ROS2 50 | * Contributors: Jose Luis Blanco-Claraco 51 | 52 | 1.0.3 (2022-06-25) 53 | ------------------ 54 | 55 | 1.0.2 (2022-06-25) 56 | ------------------ 57 | 58 | 1.0.1 (2022-06-24) 59 | ------------------ 60 | * Removed now obsolete tf_prefix 61 | * Contributors: Jose Luis Blanco-Claraco 62 | 63 | 1.0.0 (2022-04-30) 64 | ------------------ 65 | 66 | 0.1.26 (2019-10-05) 67 | ------------------- 68 | 69 | 0.1.25 (2019-10-04) 70 | ------------------- 71 | 72 | 0.1.24 (2019-04-12) 73 | ------------------- 74 | 75 | 0.1.23 (2018-06-14) 76 | ------------------- 77 | 78 | 0.1.22 (2018-05-22) 79 | ------------------- 80 | * fix all catkin_lint errors 81 | * Contributors: Jose Luis Blanco-Claraco 82 | 83 | 0.1.21 (2018-04-27) 84 | ------------------- 85 | * Upgrade version 0.1.20 (`#99 `_) 86 | * Merge branch 'master' into master 87 | * Merge branch 'master' into compat-mrpt-1.5 88 | * Adapted CMakeLists to new mrpt 89 | * Contributors: Borys Tymchenko, Hunter Laux, Jose Luis Blanco-Claraco 90 | 91 | 0.1.20 (2018-04-26) 92 | ------------------- 93 | * Adapted CMakeLists to new mrpt 94 | * Contributors: Borys Tymchenko, Jose Luis Blanco-Claraco 95 | 96 | 0.1.18 (2017-01-22) 97 | ------------------- 98 | 99 | 0.1.17 (2017-01-22) 100 | ------------------- 101 | * make catkin_lint clean 102 | * Remove all errors generated by catkin_lint and cleanup unused templates from CMakeLists.txt files 103 | * Contributors: Jorge Santos, Jose Luis Blanco 104 | 105 | 0.1.16 (2016-12-13) 106 | ------------------- 107 | 108 | 0.1.15 (2016-11-06) 109 | ------------------- 110 | 111 | 0.1.14 (2016-09-12) 112 | ------------------- 113 | 114 | 0.1.13 (2016-09-03) 115 | ------------------- 116 | * fix problematic (for bloom) German characters 117 | * Contributors: Jose Luis Blanco 118 | 119 | 0.1.12 (2016-09-03) 120 | ------------------- 121 | 122 | 0.1.11 (2016-08-21) 123 | ------------------- 124 | 125 | 0.1.10 (2016-08-05) 126 | ------------------- 127 | 128 | 0.1.9 (2016-08-05) 129 | ------------------ 130 | 131 | 0.1.8 (2016-06-29) 132 | ------------------ 133 | 134 | 0.1.7 (2016-06-20) 135 | ------------------ 136 | 137 | 0.1.6 (2016-03-20) 138 | ------------------ 139 | 140 | 0.1.5 (2015-04-29) 141 | ------------------ 142 | * update on gazebo model for tutorial 143 | * Contributors: Markus Bader 144 | 145 | 0.1.4 (2014-12-27) 146 | ------------------ 147 | 148 | 0.1.3 (2014-12-18) 149 | ------------------ 150 | * Fix many missing install files 151 | * Contributors: Jose Luis Blanco 152 | 153 | 0.1.2 (2014-12-18) 154 | ------------------ 155 | 156 | 0.1.1 (2014-12-17) 157 | ------------------ 158 | * First public binary release. 159 | 160 | 0.1.0 (2014-12-17) 161 | ------------------ 162 | * re-added run-time deps 163 | * consistent version numbers 164 | * config and demos tested 165 | 166 | -------------------------------------------------------------------------------- /mrpt_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mrpt_tutorials) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | # Mark other files for installation (e.g. launch and bag files, etc.) 8 | install(DIRECTORY 9 | datasets 10 | launch 11 | maps 12 | mvsim 13 | params 14 | rviz2 15 | DESTINATION share/${PROJECT_NAME} 16 | ) 17 | 18 | ament_export_dependencies() 19 | 20 | if(BUILD_TESTING) 21 | find_package(ament_lint_auto REQUIRED) 22 | # the following line skips the linter which checks for copyrights 23 | # uncomment the line when a copyright and license is not present in all source files 24 | #set(ament_cmake_copyright_FOUND TRUE) 25 | # the following line skips cpplint (only works in a git repo) 26 | # uncomment the line when this package is not in a git repo 27 | #set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /mrpt_tutorials/datasets/README.md: -------------------------------------------------------------------------------- 1 | These sample datasets are used in unit tests of the mrpt_pf_localization package. 2 | 3 | Credits of the original dataset: 4 | 5 | @date 15. July 2014 6 | @author Markus Bader 7 | -------------------------------------------------------------------------------- /mrpt_tutorials/datasets/driving_in_office.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tutorials/datasets/driving_in_office.rawlog -------------------------------------------------------------------------------- /mrpt_tutorials/datasets/driving_in_office_obs.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tutorials/datasets/driving_in_office_obs.rawlog -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_astar_planner_gridmap.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | from launch import LaunchDescription 7 | from launch.substitutions import TextSubstitution 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 11 | from ament_index_python import get_package_share_directory 12 | from launch.launch_description_sources import PythonLaunchDescriptionSource 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | tutsDir = get_package_share_directory("mrpt_tutorials") 18 | # print('tutsDir : ' + tutsDir) 19 | 20 | mrpt_astar_planner_launch = IncludeLaunchDescription( 21 | PythonLaunchDescriptionSource([os.path.join( 22 | get_package_share_directory('mrpt_tps_astar_planner'), 'launch', 23 | 'tps_astar_planner.launch.py')]), 24 | launch_arguments={ 25 | 'topic_goal_sub': '/goal_pose', 26 | 'show_gui': 'False', 27 | 'topic_obstacles_gridmap_sub': '/mrpt_map/map_gridmap', 28 | 'topic_static_maps': '/mrpt_map/map_gridmap', 29 | 'topic_wp_seq_pub': '/waypoints', 30 | # 'problem_world_bbox_ignore_obstacles': 'True', 31 | }.items() 32 | ) 33 | 34 | mrpt_map_launch = IncludeLaunchDescription( 35 | PythonLaunchDescriptionSource([os.path.join( 36 | get_package_share_directory('mrpt_map_server'), 'launch', 37 | 'mrpt_map_server.launch.py')]), 38 | launch_arguments={ 39 | 'map_yaml_file': os.path.join(tutsDir, 'maps', 'demo_world2.yaml'), 40 | }.items() 41 | ) 42 | 43 | mvsim_node = Node( 44 | package='mvsim', 45 | executable='mvsim_node', 46 | name='mvsim', 47 | output='screen', 48 | parameters=[ 49 | # os.path.join(tutsDir, 'params', 'mvsim_ros2_params.yaml'), 50 | { 51 | "world_file": os.path.join(tutsDir, 'mvsim', 'demo_world2.world.xml'), 52 | "do_fake_localization": True, 53 | "headless": True, 54 | }] 55 | ) 56 | 57 | # Launch for mrpt_pointcloud_pipeline: 58 | pointcloud_pipeline_launch = IncludeLaunchDescription( 59 | PythonLaunchDescriptionSource([os.path.join( 60 | get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch', 61 | 'pointcloud_pipeline.launch.py')]), 62 | launch_arguments={ 63 | 'use_composable': 'False', 64 | 'log_level': 'INFO', 65 | 'scan_topic_name': '/laser1, /laser2', 66 | 'points_topic_name': '/lidar1_points', 67 | 'filter_output_topic_name': '/local_map_pointcloud', 68 | 'time_window': '0.20', 69 | 'show_gui': 'False', 70 | 'frameid_robot': 'base_link', 71 | 'frameid_reference': 'odom', 72 | }.items() 73 | ) 74 | 75 | # Launch for mrpt_reactivenav2d: 76 | node_rnav2d_launch = IncludeLaunchDescription( 77 | PythonLaunchDescriptionSource([os.path.join( 78 | get_package_share_directory('mrpt_reactivenav2d'), 'launch', 79 | 'rnav.launch.py')]), 80 | launch_arguments={ 81 | 'log_level': 'INFO', 82 | # 'save_nav_log': 'True', 83 | 'frameid_robot': 'base_link', 84 | 'frameid_reference': 'map', 85 | 'topic_reactive_nav_goal': '/rnav_goal', # we don't publish this topic 86 | 'topic_wp_seq': '/waypoints', # only this one instead. 87 | }.items() 88 | ) 89 | 90 | rviz2_node = Node( 91 | package='rviz2', 92 | executable='rviz2', 93 | name='rviz2', 94 | arguments=[ 95 | '-d', [os.path.join(tutsDir, 'rviz2', 'gridmap.rviz')]] 96 | ) 97 | return LaunchDescription([ 98 | mrpt_map_launch, 99 | mrpt_astar_planner_launch, 100 | mvsim_node, 101 | pointcloud_pipeline_launch, 102 | node_rnav2d_launch, 103 | rviz2_node 104 | ]) 105 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_composable_nodes.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | from launch import LaunchDescription 7 | from launch.substitutions import TextSubstitution 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node, ComposableNodeContainer 10 | from launch.conditions import IfCondition 11 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 12 | from ament_index_python import get_package_share_directory 13 | from launch.launch_description_sources import PythonLaunchDescriptionSource 14 | import os 15 | 16 | 17 | def generate_launch_description(): 18 | tutsDir = get_package_share_directory("mrpt_tutorials") 19 | # print('tutsDir : ' + tutsDir) 20 | 21 | use_composable = LaunchConfiguration('use_composable') 22 | 23 | use_composable_arg = DeclareLaunchArgument( 24 | 'use_composable', 25 | default_value='false', 26 | ) 27 | 28 | container = ComposableNodeContainer( 29 | condition = IfCondition(use_composable), 30 | name='demo_composable_container', 31 | package = 'rclcpp_components', 32 | executable = 'component_container', 33 | namespace = '', 34 | output = 'screen', 35 | ) 36 | 37 | mrpt_map_launch = IncludeLaunchDescription( 38 | PythonLaunchDescriptionSource([os.path.join( 39 | get_package_share_directory('mrpt_map_server'), 'launch', 40 | 'mrpt_map_server.launch.py')]), 41 | launch_arguments={ 42 | 'map_yaml_file': os.path.join(tutsDir, 'maps', 'demo_world2.yaml'), 43 | }.items() 44 | ) 45 | 46 | mvsim_node = Node( 47 | package='mvsim', 48 | executable='mvsim_node', 49 | name='mvsim', 50 | output='screen', 51 | parameters=[ 52 | # os.path.join(tutsDir, 'params', 'mvsim_ros2_params.yaml'), 53 | { 54 | "world_file": os.path.join(tutsDir, 'mvsim', 'demo_world2.world.xml'), 55 | "do_fake_localization": True, 56 | "headless": True, 57 | }] 58 | ) 59 | 60 | # Launch for mrpt_pointcloud_pipeline: 61 | pointcloud_pipeline_launch = IncludeLaunchDescription( 62 | PythonLaunchDescriptionSource([os.path.join( 63 | get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch', 64 | 'pointcloud_pipeline.launch.py')]), 65 | launch_arguments={ 66 | # 67 | 'use_composable': LaunchConfiguration('use_composable'), 68 | 'container_name': 'demo_composable_container', 69 | # 70 | 'log_level': 'INFO', 71 | 'scan_topic_name': '/laser1, /laser2', 72 | 'points_topic_name': '/lidar1_points', 73 | 'filter_output_topic_name': '/local_map_pointcloud', 74 | 'time_window': '0.20', 75 | 'show_gui': 'False', 76 | 'frameid_robot': 'base_link', 77 | 'frameid_reference': 'odom', 78 | }.items() 79 | ) 80 | 81 | # Launch for pf_localization: 82 | pf_localization_launch = IncludeLaunchDescription( 83 | PythonLaunchDescriptionSource([os.path.join( 84 | get_package_share_directory('mrpt_pf_localization'), 'launch', 85 | 'localization.launch.py')]), 86 | launch_arguments={ 87 | # 88 | 'use_composable': LaunchConfiguration('use_composable'), 89 | 'container_name': 'demo_composable_container', 90 | # 91 | 'log_level': 'INFO', 92 | 'log_level_core': 'INFO', 93 | 'topic_sensors_2d_scan': '/laser1', 94 | 'topic_sensors_point_clouds': '', 95 | 96 | # For robots with wheels odometry, use: 'base_link'-> 'odom' -> 'map' 97 | # For systems without wheels odometry, use: 'base_link'-> 'base_link' -> 'map' 98 | 'base_link_frame_id': 'base_link', 99 | 'odom_frame_id': 'odom', 100 | 'global_frame_id': 'map', 101 | }.items() 102 | ) 103 | 104 | rviz2_node = Node( 105 | package='rviz2', 106 | executable='rviz2', 107 | name='rviz2', 108 | arguments=[ 109 | '-d', [os.path.join(tutsDir, 'rviz2', 'gridmap.rviz')]] 110 | ) 111 | 112 | return LaunchDescription([ 113 | use_composable_arg, 114 | container, 115 | mrpt_map_launch, 116 | pf_localization_launch, 117 | mvsim_node, 118 | pointcloud_pipeline_launch, 119 | rviz2_node, 120 | ]) 121 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_localization_pf_mvsim_2d_lidar.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | from launch import LaunchDescription 7 | from launch.substitutions import TextSubstitution 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 11 | from ament_index_python import get_package_share_directory 12 | from launch.launch_description_sources import PythonLaunchDescriptionSource 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | tutsDir = get_package_share_directory("mrpt_tutorials") 18 | # print('tutsDir : ' + tutsDir) 19 | 20 | # Launch for pf_localization: 21 | pf_localization_launch = IncludeLaunchDescription( 22 | PythonLaunchDescriptionSource([os.path.join( 23 | get_package_share_directory('mrpt_pf_localization'), 'launch', 24 | 'localization.launch.py')]), 25 | launch_arguments={ 26 | 'log_level': 'INFO', 27 | 'log_level_core': 'INFO', 28 | 'topic_sensors_2d_scan': '/laser1', 29 | 'topic_sensors_point_clouds': '', 30 | 31 | # For robots with wheels odometry, use: 'base_link'-> 'odom' -> 'map' 32 | # For systems without wheels odometry, use: 'base_link'-> 'base_link' -> 'map' 33 | 'base_link_frame_id': 'base_link', 34 | 'odom_frame_id': 'odom', 35 | 'global_frame_id': 'map', 36 | }.items() 37 | ) 38 | 39 | mrpt_map_launch = IncludeLaunchDescription( 40 | PythonLaunchDescriptionSource([os.path.join( 41 | get_package_share_directory('mrpt_map_server'), 'launch', 42 | 'mrpt_map_server.launch.py')]), 43 | launch_arguments={ 44 | 'map_yaml_file': os.path.join(tutsDir, 'maps', 'demo_world2.yaml'), 45 | }.items() 46 | ) 47 | 48 | mvsim_node = Node( 49 | package='mvsim', 50 | executable='mvsim_node', 51 | name='mvsim', 52 | output='screen', 53 | parameters=[ 54 | os.path.join(tutsDir, 'params', 'mvsim_ros2_params.yaml'), 55 | { 56 | "world_file": os.path.join(tutsDir, 'mvsim', 'demo_world2.world.xml'), 57 | }] 58 | ) 59 | 60 | rviz2_node = Node( 61 | package='rviz2', 62 | executable='rviz2', 63 | name='rviz2', 64 | arguments=[ 65 | '-d', [os.path.join(tutsDir, 'rviz2', 'gridmap.rviz')]] 66 | ) 67 | 68 | return LaunchDescription([ 69 | pf_localization_launch, 70 | mvsim_node, 71 | rviz2_node, 72 | mrpt_map_launch, 73 | ]) 74 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_map_server_from_mm.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | from launch import LaunchDescription 7 | from launch.substitutions import TextSubstitution 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 11 | from ament_index_python import get_package_share_directory 12 | from launch.launch_description_sources import PythonLaunchDescriptionSource 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | tutsDir = get_package_share_directory("mrpt_tutorials") 18 | 19 | arg_mm_file = DeclareLaunchArgument( 20 | name='mm_file' 21 | ) 22 | 23 | mrpt_map_launch = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource([os.path.join( 25 | get_package_share_directory('mrpt_map_server'), 'launch', 26 | 'mrpt_map_server.launch.py')]), 27 | launch_arguments={ 28 | 'mm_file': LaunchConfiguration('mm_file'), 29 | }.items() 30 | ) 31 | 32 | rviz2_node = Node( 33 | package='rviz2', 34 | executable='rviz2', 35 | name='rviz2', 36 | arguments=[ 37 | '-d', [os.path.join(tutsDir, 'rviz2', 'gridmap.rviz')]] 38 | ) 39 | return LaunchDescription([ 40 | mrpt_map_launch, 41 | arg_mm_file, 42 | rviz2_node 43 | ]) 44 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_map_server_gridmap_from_yaml.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | from launch import LaunchDescription 7 | from launch.substitutions import TextSubstitution 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 11 | from ament_index_python import get_package_share_directory 12 | from launch.launch_description_sources import PythonLaunchDescriptionSource 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | tutsDir = get_package_share_directory("mrpt_tutorials") 18 | # print('tutsDir : ' + tutsDir) 19 | 20 | mrpt_map_launch = IncludeLaunchDescription( 21 | PythonLaunchDescriptionSource([os.path.join( 22 | get_package_share_directory('mrpt_map_server'), 'launch', 23 | 'mrpt_map_server.launch.py')]), 24 | launch_arguments={ 25 | 'map_yaml_file': os.path.join(tutsDir, 'maps', 'demo_world2.yaml'), 26 | }.items() 27 | ) 28 | 29 | rviz2_node = Node( 30 | package='rviz2', 31 | executable='rviz2', 32 | name='rviz2', 33 | arguments=[ 34 | '-d', [os.path.join(tutsDir, 'rviz2', 'gridmap.rviz')]] 35 | ) 36 | return LaunchDescription([ 37 | mrpt_map_launch, 38 | rviz2_node 39 | ]) 40 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_pointcloud_pipeline_mvsim.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | import os 7 | from launch import LaunchDescription 8 | from launch_ros.actions import Node 9 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 10 | from launch.substitutions import LaunchConfiguration 11 | from ament_index_python.packages import get_package_share_directory 12 | from launch.launch_description_sources import PythonLaunchDescriptionSource 13 | 14 | 15 | def generate_launch_description(): 16 | 17 | # Launch for mrpt_pointcloud_pipeline: 18 | pointcloud_pipeline_launch = IncludeLaunchDescription( 19 | PythonLaunchDescriptionSource([os.path.join( 20 | get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch', 21 | 'pointcloud_pipeline.launch.py')]), 22 | launch_arguments={ 23 | 'use_composable': 'False', 24 | 'log_level': 'INFO', 25 | 'scan_topic_name': '/laser1, /laser2', 26 | 'points_topic_name': '/camera1_points', 27 | 'time_window': '0.20', 28 | 'show_gui': 'True', 29 | }.items() 30 | ) 31 | 32 | mvsim_pkg_share_dir = get_package_share_directory('mvsim') 33 | # Finding the launch file 34 | launch_file_name = 'demo_jackal.launch.py' 35 | mvsim_launch_file_path = os.path.join( 36 | mvsim_pkg_share_dir, 'mvsim_tutorial', launch_file_name) 37 | 38 | # Check if the launch file exists 39 | if not os.path.isfile(mvsim_launch_file_path): 40 | raise Exception( 41 | f"Launch file '{mvsim_launch_file_path}' does not exist!") 42 | 43 | return LaunchDescription([ 44 | IncludeLaunchDescription( 45 | PythonLaunchDescriptionSource(mvsim_launch_file_path) 46 | ), 47 | pointcloud_pipeline_launch 48 | ]) 49 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_reactive_nav_mvsim.launch.py: -------------------------------------------------------------------------------- 1 | # ROS 2 launch file for example in mrpt_tutorials 2 | # 3 | # See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation 4 | # 5 | 6 | import os 7 | from launch import LaunchDescription 8 | from ament_index_python.packages import get_package_share_directory 9 | from launch_ros.actions import Node 10 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 11 | from launch.substitutions import LaunchConfiguration 12 | from launch.launch_description_sources import PythonLaunchDescriptionSource 13 | 14 | 15 | def generate_launch_description(): 16 | tutsDir = get_package_share_directory("mrpt_tutorials") 17 | 18 | default_world_file = os.path.join( 19 | get_package_share_directory('mvsim'), 20 | 'mvsim_tutorial', 'demo_warehouse.world.xml') 21 | 22 | arg_world_file_launch = DeclareLaunchArgument( 23 | name='world_file', 24 | default_value=default_world_file 25 | ) 26 | 27 | # Launch for mrpt_pointcloud_pipeline: 28 | pointcloud_pipeline_launch = IncludeLaunchDescription( 29 | PythonLaunchDescriptionSource([os.path.join( 30 | get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch', 31 | 'pointcloud_pipeline.launch.py')]), 32 | launch_arguments={ 33 | 'use_composable': 'False', 34 | 'log_level': 'INFO', 35 | 'scan_topic_name': '/scanner1', 36 | 'points_topic_name': '/lidar1_points', 37 | 'filter_output_topic_name': '/local_map_pointcloud', 38 | 'time_window': '0.20', 39 | 'show_gui': 'True', 40 | 'frameid_robot': 'base_link', 41 | 'frameid_reference': 'odom', 42 | }.items() 43 | ) 44 | 45 | node_mvsim_launch = Node( 46 | package='mvsim', 47 | executable='mvsim_node', 48 | name='mvsim_simulator', 49 | output='screen', 50 | parameters=[ 51 | { 52 | 'world_file': LaunchConfiguration('world_file') 53 | }, 54 | { 55 | # This allows running without SLAM/Particle filter (Disable when using them!) 56 | 'do_fake_localization': True 57 | } 58 | ] 59 | ) 60 | 61 | node_rviz2_launch = Node( 62 | package='rviz2', 63 | executable='rviz2', 64 | name='rviz2', 65 | arguments=[ 66 | '-d', [os.path.join(tutsDir, 'rviz2', 'rnav_demo.rviz')]] 67 | ) 68 | 69 | # Launch for mrpt_reactivenav2d: 70 | node_rnav2d_launch = IncludeLaunchDescription( 71 | PythonLaunchDescriptionSource([os.path.join( 72 | get_package_share_directory('mrpt_reactivenav2d'), 'launch', 73 | 'rnav.launch.py')]), 74 | launch_arguments={ 75 | 'log_level': 'INFO', 76 | # 'save_nav_log': 'True', 77 | 'frameid_robot': 'base_link', 78 | 'frameid_reference': 'map', 79 | }.items() 80 | ) 81 | 82 | return LaunchDescription([ 83 | arg_world_file_launch, 84 | pointcloud_pipeline_launch, 85 | node_mvsim_launch, 86 | node_rviz2_launch, 87 | node_rnav2d_launch 88 | ]) 89 | -------------------------------------------------------------------------------- /mrpt_tutorials/launch/demo_ro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mrpt_tutorials/maps/demo_world2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tutorials/maps/demo_world2.png -------------------------------------------------------------------------------- /mrpt_tutorials/maps/demo_world2.yaml: -------------------------------------------------------------------------------- 1 | image: demo_world2.png 2 | resolution: 0.25 3 | origin: [-5.0, -5.0, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /mrpt_tutorials/maps/gh25_real_top_laser.simplemap: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tutorials/maps/gh25_real_top_laser.simplemap -------------------------------------------------------------------------------- /mrpt_tutorials/maps/gh25_simulated.simplemap: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_navigation/9dec61c4d9bbe45607b1349bc0361d59184adb5b/mrpt_tutorials/maps/gh25_simulated.simplemap -------------------------------------------------------------------------------- /mrpt_tutorials/mvsim/demo_world2.world.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 0 4 | 5 | 6 | 7 | false 8 | true 0.01 9 | 35 10 | 60 11 | 20 12 | 30 30 0 13 | 14 | 15 | 16 | 19 | 20 | 21 | ../maps/demo_world2.yaml 22 | 23 | 24 | 25 | 26 | 29 | 30 | 31 | 34 | 35 | 30 30 0 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /mrpt_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_tutorials 4 | 2.2.2 5 | Example files used as tutorials for MRPT ROS packages 6 | 7 | Jose Luis Blanco-Claraco 8 | Markus Bader 9 | Jose Luis Blanco-Claraco 10 | 11 | BSD 12 | https://github.com/mrpt-ros-pkg/mrpt_navigation 13 | 14 | cmake 15 | ament_cmake 16 | 17 | teleop_twist_keyboard 18 | mvsim 19 | 20 | ament_cmake_lint_cmake 21 | ament_cmake_xmllint 22 | ament_lint_auto 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /mrpt_tutorials/params/mvsim_ros2_params.yaml: -------------------------------------------------------------------------------- 1 | # ROS2 parameter files for same-name launch "*.launch.py" 2 | /**: 3 | ros__parameters: 4 | #world_file: overrdian via launch parameter 5 | do_fake_localization: false # publish tf odom -> base_link 6 | publisher_history_len: 100 # history window length of publishers 7 | show_gui: true 8 | use_sim_time: false 9 | #realtime_factor: 1.0 10 | #base_watchdog_timeout: 0.5 # [s] 11 | #gui_refresh_period: 100 # [ms] 12 | #period_ms_publish_tf: 20 # [ms] 13 | -------------------------------------------------------------------------------- /scripts/generate_statuses_table.py: -------------------------------------------------------------------------------- 1 | #!/bin/env python3 2 | 3 | import os 4 | 5 | 6 | def list_directories(path): 7 | # List all directories under the given path 8 | directories = [d for d in os.listdir( 9 | path) if os.path.isdir(os.path.join(path, d))] 10 | return directories 11 | 12 | 13 | def extract_plain_name(directory): 14 | # Extract the plain name without the full path 15 | return os.path.basename(directory) 16 | 17 | 18 | def process_directories(path, items_to_remove, pattern_text): 19 | directories = list_directories(path) 20 | 21 | sorted_directories = sorted(directories, key=extract_plain_name) 22 | 23 | for plain_name in sorted_directories: 24 | 25 | # Check if the plain name passes the filter 26 | if plain_name not in items_to_remove: 27 | # Print the pattern text with the name 28 | output_text = pattern_text.format(name=plain_name) 29 | print(output_text) 30 | 31 | 32 | path_to_search = '..' 33 | items_to_remove = ['scripts', 'docs', 34 | '.vscode', '.github', '.circleci', '.git'] 35 | pattern_text = \ 36 | "| {name} | "\ 37 | "[![Build Status](https://build.ros2.org/job/Hbin_uJ64__{name}__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__{name}__ubuntu_jammy_amd64__binary/) | " \ 38 | "[![Build Status](https://build.ros2.org/job/Jbin_uN64__{name}__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__{name}__ubuntu_noble_amd64__binary/) | " \ 39 | "[![Build Status](https://build.ros2.org/job/Kbin_uN64__{name}__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__{name}__ubuntu_noble_amd64__binary/) | " \ 40 | "[![Build Status](https://build.ros2.org/job/Rbin_uN64__{name}__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__{name}__ubuntu_noble_amd64__binary/) |" 41 | 42 | process_directories(path_to_search, items_to_remove, pattern_text) 43 | --------------------------------------------------------------------------------