├── .clang-format ├── .gitignore ├── .travis.sh ├── .travis.yml ├── LICENSE.txt ├── README.md ├── mrpt_ekf_slam_2d ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ └── rosconsole.config ├── include │ └── mrpt_ekf_slam_2d │ │ ├── mrpt_ekf_slam_2d.h │ │ └── mrpt_ekf_slam_2d_wrapper.h ├── launch │ ├── ekf_slam_2d.launch │ └── ekf_slam_2d_rawlog.launch ├── mainpage.dox ├── package.xml ├── rviz │ └── rviz_conf_ekf_2d.rviz ├── src │ ├── mrpt_ekf_slam_2d.cpp │ ├── mrpt_ekf_slam_2d_app.cpp │ └── mrpt_ekf_slam_2d_wrapper.cpp └── tutorial │ ├── kf-slam_demo_2d.ini │ └── kf-slam_demo_2d.rawlog ├── mrpt_ekf_slam_3d ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ └── rosconsole.config ├── include │ └── mrpt_ekf_slam_3d │ │ ├── mrpt_ekf_slam_3d.h │ │ └── mrpt_ekf_slam_3d_wrapper.h ├── launch │ ├── ekf_slam_3d.launch │ ├── ekf_slam_3d_rawlog.launch │ └── ekf_slam_3d_wheeled_robot.launch ├── mainpage.dox ├── package.xml ├── rviz │ └── rviz_conf_ekf_3d.rviz ├── src │ ├── mrpt_ekf_slam_3d.cpp │ ├── mrpt_ekf_slam_3d_app.cpp │ └── mrpt_ekf_slam_3d_wrapper.cpp └── tutorial │ ├── kf-slam_6D_demo.ini │ └── kf-slam_6D_demo.rawlog ├── mrpt_graphslam_2d ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ ├── ros_laser_odometry.ini │ ├── ros_laser_odometry_LC_MR_real.ini │ ├── ros_odometry_2DRangeScans.ini │ ├── ros_odometry_2DRangeScans_LC.ini │ ├── ros_odometry_2DRangeScans_LC_MR_real.ini │ └── ros_odometry_2DRangeScans_LC_MR_simul.ini ├── include │ └── mrpt_graphslam_2d │ │ ├── CConnectionManager.h │ │ ├── CGraphSlamEngine_MR.h │ │ ├── CGraphSlamEngine_MR_impl.h │ │ ├── CGraphSlamEngine_ROS.h │ │ ├── CGraphSlamEngine_ROS_impl.h │ │ ├── CGraphSlamHandler_ROS.h │ │ ├── CGraphSlamHandler_ROS_impl.h │ │ ├── CMapMerger.h │ │ ├── ERD │ │ ├── CLoopCloserERD_MR.h │ │ └── CLoopCloserERD_MR_impl.h │ │ ├── NRD │ │ ├── CFixedIntervalsNRD_MR.h │ │ ├── CFixedIntervalsNRD_MR_impl.h │ │ ├── CICPCriteriaNRD_MR.h │ │ └── CICPCriteriaNRD_MR_impl.h │ │ ├── TNeighborAgentMapProps.h │ │ ├── TUserOptionsChecker_ROS.h │ │ ├── TUserOptionsChecker_ROS_impl.h │ │ ├── interfaces │ │ ├── CEdgeRegistrationDecider_MR.h │ │ ├── CEdgeRegistrationDecider_MR_impl.h │ │ ├── CGraphSlamOptimizer_MR.h │ │ ├── CGraphSlamOptimizer_MR_impl.h │ │ ├── CNodeRegistrationDecider_MR.h │ │ ├── CNodeRegistrationDecider_MR_impl.h │ │ ├── CRegistrationDeciderOrOptimizer_MR.h │ │ ├── CRegistrationDeciderOrOptimizer_MR_impl.h │ │ ├── CRegistrationDeciderOrOptimizer_ROS.h │ │ └── CRegistrationDeciderOrOptimizer_ROS_impl.h │ │ └── misc │ │ └── common.h ├── launch │ ├── graphslam.launch │ ├── setup_robot_for_mr_demo.launch │ ├── sr_graphslam_demo.launch │ └── sr_graphslam_demo_gt.launch ├── nodes │ └── rename_rviz_topics.py ├── package.xml ├── rosbags │ └── demo_short_loop │ │ ├── demo.bag │ │ ├── pioneer_agent.jpg │ │ └── workspace.jpg ├── rviz │ ├── README.md │ ├── graphslam_real_1.rviz │ ├── graphslam_real_2.rviz │ ├── graphslam_real_3.rviz │ ├── sr_graphslam.rviz │ ├── sr_graphslam_demo_gt.rviz │ └── templates │ │ ├── graphslam_bag_1.rviz │ │ ├── graphslam_bag_2.rviz │ │ ├── graphslam_bag_3.rviz │ │ ├── graphslam_gazebo_1.rviz │ │ ├── graphslam_gazebo_2.rviz │ │ ├── graphslam_gazebo_3.rviz │ │ └── graphslam_gazebo_4.rviz └── src │ ├── CConnectionManager.cpp │ ├── CMapMerger.cpp │ ├── TNeighborAgentMapProps.cpp │ ├── common.cpp │ ├── map_merger_node.cpp │ ├── mrpt_graphslam_2d_mr_node.cpp │ └── mrpt_graphslam_2d_node.cpp ├── mrpt_icp_slam_2d ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ └── rosconsole.config ├── include │ └── mrpt_icp_slam_2d │ │ └── mrpt_icp_slam_2d_wrapper.h ├── launch │ ├── icp_slam.launch │ ├── icp_slam_gui.launch │ ├── icp_slam_rawlog.launch │ └── mvsim_icp_slam.launch ├── mainpage.dox ├── package.xml ├── rviz │ └── rviz_conf.rviz ├── src │ ├── mrpt_icp_slam_2d_app.cpp │ └── mrpt_icp_slam_2d_wrapper.cpp └── tutorial │ ├── icp_slam_demo.ini │ └── icp_slam_demo.rawlog ├── mrpt_rbpf_slam ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── default.yaml │ └── rosconsole.config ├── include │ └── mrpt_rbpf_slam │ │ ├── mrpt_rbpf_slam.h │ │ ├── mrpt_rbpf_slam_wrapper.h │ │ └── options.h ├── launch │ ├── mvsim_slam.launch │ ├── rbpf_slam.launch │ ├── rbpf_slam_rawlog.launch │ ├── rbpf_slam_turtlebot3.launch │ ├── ro_slam.launch │ ├── ro_slam_rawlog.launch │ └── rviz.launch ├── package.xml ├── rviz │ └── rviz_conf.rviz ├── src │ ├── mrpt_rbpf_slam.cpp │ ├── mrpt_rbpf_slam_node.cpp │ ├── mrpt_rbpf_slam_wrapper.cpp │ └── options.cpp └── tutorial │ ├── RO-SLAM_demo.ini │ ├── RO-SLAM_demo.rawlog │ ├── grid_slam_demo.ini │ ├── grid_slam_demo.rawlog │ └── grid_slam_demo2.rawlog └── mrpt_slam ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml /.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: 80 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: false # TODO - Compilation errors on this! 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 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | build* 3 | install* 4 | 5 | *.DS_Store 6 | .Spotlight-V100 7 | Session.vim 8 | 9 | # Ignore qtcreator files: 10 | CMakeLists.txt.user* 11 | .vscode 12 | -------------------------------------------------------------------------------- /.travis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x 4 | set -e 5 | 6 | apt-get update 7 | apt-get install -y sudo software-properties-common wget # minimum deps for the commands below 8 | 9 | echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" 10 | sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' 11 | wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 12 | sudo add-apt-repository $MRPT_PPA -y 13 | sudo apt-get update -qq 14 | sudo apt-get install -qq -y python-rosdep 15 | sudo rosdep init 16 | rosdep update; while [ $? != 0 ]; do sleep 1; rosdep update; done 17 | 18 | # Use rosdep to install all dependencies (including ROS itself) 19 | rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO 20 | 21 | # ROS env vars: 22 | source /opt/ros/$CI_ROS_DISTRO/setup.bash 23 | 24 | # Setup catkin Workspace: 25 | mkdir -p ~/catkin_ws/src 26 | ln -s $CI_SOURCE_PATH ~/catkin_ws/src # Link the repo we are testing to the new workspace 27 | cd ~/catkin_ws/ 28 | 29 | # Build [and Install] packages, build tests, and run tests 30 | catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release 31 | catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests 32 | catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests 33 | # Check results 34 | catkin_test_results ./build_isolated 35 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # We cannot use latest ROS distributions in travis since we are limited to trusty 2 | # So, let's use docker. 3 | sudo: required 4 | dist: trusty 5 | language: c++ 6 | notifications: 7 | env: 8 | matrix: 9 | - CI_ROS_DISTRO=kinetic MRPT_BRANCH="2.0" MRPT_PPA="ppa:joseluisblancoc/mrpt" 10 | - CI_ROS_DISTRO=kinetic MRPT_BRANCH="1.5" MRPT_PPA="ppa:joseluisblancoc/mrpt-1.5" 11 | # - ROS_DISTRO=kinetic BUILDER='catkin_make_isolated' BUILDER='catkin_make' 12 | before_script: 13 | - case $CI_ROS_DISTRO in "kinetic") export DISTRO=xenial;; "lunar") export DISTRO=xenial;; esac; 14 | - export DOCKER_IMAGE=ubuntu:$DISTRO 15 | - export CI_SOURCE_PATH=$(pwd) 16 | - export REPOSITORY_NAME=${PWD##*/} 17 | - if [ ! "$ROS_PARALLEL_JOBS" ]; then export ROS_PARALLEL_JOBS="-j8 -l8"; fi 18 | - docker images 19 | script: 20 | - docker run -v $HOME:$HOME -e MRPT_PPA -e CI_SOURCE_PATH -e REPOSITORY_NAME -e HOME -e DISTRO -e CI_ROS_DISTRO -e ROS_PARALLEL_JOBS $DOCKER_IMAGE bash -c 'cd $CI_SOURCE_PATH; source .travis.sh' 21 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | | Distro | Build dev | Build release | Stable sync | 2 | | --- | --- | --- | --- | 3 | | ROS1 Noetic @ u20.04 | [![Build Status](https://build.ros.org/job/Ndev__mrpt_slam__ubuntu_focal_amd64/badge/icon)](https://build.ros.org/job/Ndev__mrpt_slam__ubuntu_focal_amd64/) | [![Build Status](https://build.ros.org/job/Nbin_uF64__mrpt_slam__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros.org/job/Nbin_uF64__mrpt_slam__ubuntu_focal_amd64__binary/) | [![Version](https://img.shields.io/ros/v/noetic/mrpt_slam)](https://index.ros.org/search/?term=mrpt_slam) | 4 | 5 | | EOL Distros | Last release | 6 | | --- | --- | 7 | | ROS1 Melodic @ u18.04 | [![Version](https://img.shields.io/ros/v/melodic/mrpt_slam)](https://index.ros.org/search/?term=mrpt_slam) | 8 | 9 | 10 | 11 | 12 | MRPT-based SLAM packages 13 | ------------------------- 14 | 15 | ROS wrappers for SLAM algorithms in Mobile Robot Programming Toolkit (MRPT). 16 | Refer to https://wiki.ros.org/mrpt_slam for further documentation. 17 | 18 | Active branches: 19 | * `ros2`: Main development branch, for ROS 2 distributions (WIP). 20 | * `ros1`: Development branch for ROS 1 distributions. 21 | 22 | Obsolete branches: 23 | * `master` and `compat-mrpt-1.3`: Deprecated, for backwards compatibility only, do not use. 24 | * `melodic-devel`: Branch for melodic only (required due to lack of dependencies, one pkg disabled) 25 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_ekf_slam_2d 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.19 (2024-09-18) 6 | ------------------- 7 | * Fix cmake target creation 8 | * Workaround for missing TBB::tbb target in Noetic 9 | * cmake: fix case and whitespaces 10 | * Contributors: Jose Luis Blanco-Claraco 11 | 12 | 0.1.18 (2024-09-05) 13 | ------------------- 14 | * Port to new 'mrpt_lib*' packages 15 | * Contributors: Jose Luis Blanco-Claraco 16 | 17 | 0.1.17 (2024-06-05) 18 | ------------------- 19 | 20 | 0.1.16 (2024-03-25) 21 | ------------------- 22 | 23 | 0.1.15 (2023-08-02) 24 | ------------------- 25 | 26 | 0.1.14 (2023-04-12) 27 | ------------------- 28 | * Fix build against last mrpt 2.7.x 29 | * Contributors: Jose Luis Blanco-Claraco 30 | 31 | 0.1.11 (2022-06-24) 32 | ------------------- 33 | * Ported to tf2 and mrpt::ros1bridge 34 | * Fixed all 2D and 3D EKF wrappers to work with mrpt 2.x API 35 | * Update build dep to mrpt2 36 | * Merge pull request `#63 `_ from reinzor/patch-1 37 | fix(CMakeLists.txt): Install rviz and config dir 38 | * fix(CMakeLists.txt): Install rviz and config dir 39 | Required for running the example launch files 40 | * Contributors: Jose Luis Blanco-Claraco, Rein Appeldoorn 41 | 42 | 0.1.10 (2019-10-05) 43 | ------------------- 44 | * fix build against mrpt2 45 | * Contributors: Jose Luis Blanco-Claraco 46 | 47 | 0.1.9 (2019-04-14) 48 | ------------------ 49 | * Fix build against MRPT 1.9.9 50 | * Switch header guards to pragma once 51 | * Contributors: Jose Luis Blanco-Claraco, Julian Lopez Velasquez, Vladislav Tananaev 52 | 53 | 0.1.8 (2018-09-21) 54 | ------------------ 55 | * Make catkin_lint clean 56 | * Contributors: Jose Luis Blanco Claraco 57 | 58 | 0.1.7 (2018-09-20) 59 | ------------------ 60 | 61 | 0.1.6 (2018-06-14) 62 | ------------------ 63 | * Fixed build with mrpt 2.0 64 | * fix build; add optimized builds (-O3) 65 | * fix build against mrpt 1.5 series 66 | * use consistent cmake conventions for c++14 67 | * all but mrpt_graphslam_2d compiling against mrpt2.0 68 | * porting to mrpt2 69 | Fixes and cleanups for config files 70 | * [mrpt_ekf_slam_2d] Update description in package.xml. 71 | * [mrpt_ekf_slam_2d] Fix comment in launch file. 72 | * [mrpt_ekf_slam_2d] Add missing dependencies to package.xml 73 | Add test of *.launch files for the node. 74 | * Move transforms to callbacks. 75 | * Move publishTF to callback. 76 | * catkin_lint error fixes 77 | * Contributors: Jose Luis Blanco, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Logrus, Magnus Gärtner, Max Kuzmin, Vladislav Tananaev 78 | 79 | 0.1.5 (2016-11-18) 80 | ------------------ 81 | * Fix cmakelists typo 82 | * Contributors: Nikos Koukis 83 | 84 | 0.1.3 (2016-09-27) 85 | ------------------ 86 | * Avoid gcc warnings with MRPT < 2.0.0 87 | * fix build against mrpt < 1.5.0 88 | * Contributors: Jose-Luis Blanco-Claraco 89 | 90 | 0.1.2 (2016-09-24) 91 | ------------------ 92 | * Make formatting conform to ROS C++ Style Guide. 93 | * Output logs only in ROS. 94 | * Add a guard for new mrpt::ros1bridge::rosLoggerLvlToMRPTLoggerLvl and mrpt::ros1bridge::mrptToROSLoggerCallback functions for MRPT version less than 1.5.0. 95 | * Add streaming of MRPT logs to ROS logs. 96 | * Contributors: Logrus, Vladislav Tananaev 97 | 98 | 0.1.1 (2016-08-22) 99 | ------------------ 100 | * First public version, as a result of Vladislav Tananaev's GSoC2016 work. 101 | * Contributors: Jose Luis Blanco, Logrus 102 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 3.1) 4 | project(mrpt_ekf_slam_2d) 5 | 6 | #find catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roslib 9 | roscpp 10 | tf2 11 | tf2_ros 12 | tf2_geometry_msgs 13 | std_msgs 14 | nav_msgs 15 | sensor_msgs 16 | visualization_msgs 17 | dynamic_reconfigure 18 | mrpt_msgs_bridge 19 | roslaunch 20 | ) 21 | 22 | #find mrpt packages 23 | find_package(mrpt-gui REQUIRED) 24 | find_package(mrpt-slam REQUIRED) 25 | find_package(mrpt-ros1bridge REQUIRED) 26 | 27 | message(STATUS "MRPT_VERSION: ${mrpt-slam_VERSION}") 28 | 29 | # fix for Noetic: 30 | if(NOT EXISTS TBB::tbb AND TBB_LIBRARY AND TBB_INCLUDE_DIRS) 31 | add_library(MY_TBB INTERFACE) 32 | target_include_directories(MY_TBB INTERFACE ${TBB_INCLUDE_DIRS}) 33 | target_link_libraries(MY_TBB INTERFACE ${TBB_LIBRARY}) 34 | add_library(TBB::tbb ALIAS MY_TBB) 35 | endif() 36 | 37 | 38 | if(CMAKE_COMPILER_IS_GNUCXX) 39 | # High level of warnings. 40 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 41 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 42 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 43 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 44 | if(NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 45 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 46 | endif() 47 | endif() 48 | 49 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 50 | add_compile_options(-O3) 51 | endif() 52 | 53 | catkin_package( 54 | CATKIN_DEPENDS 55 | nav_msgs 56 | sensor_msgs 57 | std_msgs 58 | visualization_msgs 59 | tf2 60 | tf2_ros 61 | tf2_geometry_msgs 62 | mrpt_msgs_bridge 63 | ) 64 | 65 | ########### 66 | ## Build ## 67 | ########### 68 | 69 | include_directories( 70 | ${catkin_INCLUDE_DIRS} 71 | include 72 | ) 73 | 74 | add_executable(mrpt_ekf_slam_2d 75 | src/mrpt_ekf_slam_2d.cpp 76 | src/mrpt_ekf_slam_2d_wrapper.cpp 77 | src/mrpt_ekf_slam_2d_app.cpp 78 | ) 79 | 80 | TARGET_LINK_LIBRARIES(mrpt_ekf_slam_2d 81 | mrpt::slam 82 | mrpt::gui 83 | mrpt::ros1bridge 84 | ${catkin_LIBRARIES} 85 | ) 86 | 87 | ############# 88 | ## Install ## 89 | ############# 90 | 91 | # Mark executables and/or libraries for installation 92 | install(TARGETS mrpt_ekf_slam_2d 93 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 94 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 96 | ) 97 | 98 | # Mark other files for installation (e.g. launch and bag files, etc.) 99 | install(DIRECTORY 100 | config 101 | launch 102 | rviz 103 | tutorial 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | ) 106 | 107 | ############# 108 | ## Testing ## 109 | ############# 110 | # Add testing for *.launch files 111 | roslaunch_add_file_check(launch) 112 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/config/rosconsole.config: -------------------------------------------------------------------------------- 1 | # Set the default ros output 2 | log4j.logger.ros=WARN 3 | log4j.logger.ros.mrpt_ekf_slam_2d=DEBUG -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/include/mrpt_ekf_slam_2d/mrpt_ekf_slam_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File:mrpt_ekf_slam_3d.h 3 | * Author: Vladislav Tananaev 4 | * 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 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 | 30 | using mrpt::slam::CRangeBearingKFSLAM2D; 31 | 32 | /** 33 | * @brief The EKFslam class provides EKF SLAM 2d from MRPT libraries. 34 | * 35 | */ 36 | class EKFslam 37 | { 38 | public: 39 | /** 40 | * @brief constructor 41 | */ 42 | EKFslam(); 43 | /** 44 | * @brief destructor 45 | */ 46 | virtual ~EKFslam(); 47 | /** 48 | * @brief init 3D window from mrpt lib 49 | */ 50 | void init3Dwindow(); 51 | /** 52 | * @brief run 3D window update from mrpt lib 53 | */ 54 | void run3Dwindow(); 55 | /** 56 | * @brief convert landmark to 3d point 57 | */ 58 | void landmark_to_3d( 59 | const CRangeBearingKFSLAM2D::KFArray_FEAT& lm, mrpt::math::TPoint3D& p); 60 | /** 61 | * @brief read ini file 62 | * 63 | * @param ini_filename the name of the ini file to read 64 | */ 65 | void read_iniFile(std::string ini_filename); 66 | /** 67 | * @brief calculate the actions from odometry model for current observation 68 | * 69 | * @param _sf current observation 70 | * @param _odometry raw odometry 71 | */ 72 | void observation( 73 | mrpt::obs::CSensoryFrame::Ptr _sf, 74 | mrpt::obs::CObservationOdometry::Ptr _odometry); 75 | 76 | protected: 77 | CRangeBearingKFSLAM2D mapping; ///< EKF slam 2d class 78 | 79 | mrpt::system::TTimeStamp 80 | timeLastUpdate_; ///< last update of the pose and map 81 | 82 | mrpt::obs::CActionCollection::Ptr action; ///< actions 83 | mrpt::obs::CSensoryFrame::Ptr sf; ///< observations 84 | 85 | mrpt::poses::CPose2D 86 | odomLastObservation_; ///< last observation of odometry 87 | bool use_motion_model_default_options_; ///< used default odom_params 88 | mrpt::obs::CActionRobotMovement2D::TMotionModelOptions 89 | motion_model_default_options_; ///< used if there are is not odom 90 | mrpt::obs::CActionRobotMovement2D::TMotionModelOptions 91 | motion_model_options_; ///< used with odom value motion noise 92 | 93 | mrpt::poses::CPosePDFGaussian robotPose_; ///< current robot pose 94 | std::vector LMs_; ///< vector of the landmarks 95 | /// vector of the landmarks ID 96 | std::map LM_IDs_; 97 | mrpt::math::CMatrixDouble fullCov_; ///< full covariance matrix 98 | mrpt::math::CVectorDouble fullState_; ///< full state vector 99 | 100 | mrpt::gui::CDisplayWindow3D::Ptr win3d; ///< MRPT window 101 | bool SHOW_3D_LIVE; 102 | bool CAMERA_3DSCENE_FOLLOWS_ROBOT; 103 | std::vector meanPath; 104 | }; 105 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/launch/ekf_slam_2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/launch/ekf_slam_2d_rawlog.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b mrpt_ekf_slam_2d is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_ekf_slam_2d 4 | 0.1.19 5 | This package is a wrapper for the implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks. 6 | Jose Luis Blanco-Claraco 7 | 8 | BSD 9 | 10 | http://mrpt.org/ 11 | http://ros.org/wiki/mrpt_ekf_slam_2d 12 | https://github.com/mrpt-ros-pkg/mrpt_slam/issues 13 | 14 | Jose Luis 15 | Vladislav Tananaev 16 | 17 | catkin 18 | 19 | roscpp 20 | roslaunch 21 | roslib 22 | tf2 23 | tf2_ros 24 | tf2_geometry_msgs 25 | std_msgs 26 | sensor_msgs 27 | visualization_msgs 28 | nav_msgs 29 | dynamic_reconfigure 30 | mrpt_libros_bridge 31 | mrpt_libgui 32 | mrpt_libslam 33 | mrpt_msgs_bridge 34 | 35 | rviz 36 | mrpt_rawlog 37 | 38 | 39 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/rviz/rviz_conf_ekf_2d.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /MarkerArray1 12 | - /MarkerArray2 13 | Splitter Ratio: 0.5 14 | Tree Height: 359 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679016 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: true 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | Lidar1: 61 | Value: true 62 | Lidar2: 63 | Value: true 64 | base_link: 65 | Value: true 66 | car_rear_axis_center: 67 | Value: true 68 | gps: 69 | Value: true 70 | map: 71 | Value: true 72 | Marker Scale: 1 73 | Name: TF 74 | Show Arrows: true 75 | Show Axes: true 76 | Show Names: true 77 | Tree: 78 | map: 79 | base_link: 80 | car_rear_axis_center: 81 | Lidar1: 82 | {} 83 | Lidar2: 84 | {} 85 | gps: 86 | {} 87 | Update Interval: 0 88 | Value: true 89 | - Class: rviz/MarkerArray 90 | Enabled: true 91 | Marker Topic: /state_viz 92 | Name: MarkerArray 93 | Namespaces: 94 | "": true 95 | Queue Size: 100 96 | Value: true 97 | - Class: rviz/MarkerArray 98 | Enabled: true 99 | Marker Topic: /data_association_viz 100 | Name: MarkerArray 101 | Namespaces: 102 | "": true 103 | Queue Size: 100 104 | Value: true 105 | Enabled: true 106 | Global Options: 107 | Background Color: 48; 48; 48 108 | Fixed Frame: map 109 | Frame Rate: 30 110 | Name: root 111 | Tools: 112 | - Class: rviz/Interact 113 | Hide Inactive Objects: true 114 | - Class: rviz/MoveCamera 115 | - Class: rviz/Select 116 | - Class: rviz/FocusCamera 117 | - Class: rviz/Measure 118 | - Class: rviz/SetInitialPose 119 | Topic: /initialpose 120 | - Class: rviz/SetGoal 121 | Topic: /move_base_simple/goal 122 | - Class: rviz/PublishPoint 123 | Single click: true 124 | Topic: /clicked_point 125 | Value: true 126 | Views: 127 | Current: 128 | Class: rviz/ThirdPersonFollower 129 | Distance: 12.1110897 130 | Enable Stereo Rendering: 131 | Stereo Eye Separation: 0.0599999987 132 | Stereo Focal Distance: 1 133 | Swap Stereo Eyes: false 134 | Value: false 135 | Focal Point: 136 | X: 1.18020105 137 | Y: 0.321366072 138 | Z: 0 139 | Focal Shape Fixed Size: true 140 | Focal Shape Size: 0.0500000007 141 | Invert Z Axis: false 142 | Name: Current View 143 | Near Clip Distance: 0.00999999978 144 | Pitch: 0.145398125 145 | Target Frame: 146 | Value: ThirdPersonFollower (rviz) 147 | Yaw: 2.90540552 148 | Saved: ~ 149 | Window Geometry: 150 | Displays: 151 | collapsed: false 152 | Height: 1056 153 | Hide Left Dock: true 154 | Hide Right Dock: false 155 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000001f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000001f5000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000032700fffffffb0000000800540069006d006501000000000000045000000000000000000000062a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 156 | Selection: 157 | collapsed: false 158 | Time: 159 | collapsed: false 160 | Tool Properties: 161 | collapsed: false 162 | Views: 163 | collapsed: false 164 | Width: 1855 165 | X: 65 166 | Y: 24 167 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/src/mrpt_ekf_slam_2d_app.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "mrpt_ekf_slam_2d/mrpt_ekf_slam_2d_wrapper.h" 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "mrpt_ekf_slam_2d"); 7 | ros::NodeHandle n; 8 | ros::Rate r(100); 9 | EKFslamWrapper slam; 10 | slam.get_param(); 11 | slam.init(); 12 | ros::Duration(3).sleep(); 13 | 14 | if (!slam.rawlogPlay()) 15 | { // if not play from rawlog file 16 | 17 | while (ros::ok()) 18 | { 19 | ros::spinOnce(); 20 | r.sleep(); 21 | } 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/tutorial/kf-slam_demo_2d.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------ 2 | # Config file for the KF-SLAM application 3 | # See: http://www.mrpt.org/list-of-mrpt-apps/application-kf-slam/ 4 | #------------------------------------------------------ 5 | 6 | 7 | #------------------------------------------------- 8 | # Section: [MappingApplication] 9 | # Use: Here comes global parameters for the app. 10 | #------------------------------------------------- 11 | [MappingApplication] 12 | 13 | # Implementation to use: 14 | # - CRangeBearingKFSLAM 15 | # - CRangeBearingKFSLAM2D 16 | # 17 | kf_implementation = CRangeBearingKFSLAM2D //only this option possible for 2d EKF 18 | 19 | # Left blank if not available 20 | ground_truth_file= //not used 21 | 22 | # Left blank if not available 23 | ground_truth_file_robot= //not used 24 | 25 | # The directory where the log files will be saved (left in blank if no log is required) 26 | logOutput_dir=LOG_EKF-SLAM 27 | 28 | SAVE_LOG_FREQUENCY=10 //not used 29 | 30 | SHOW_3D_LIVE= true 31 | CAMERA_3DSCENE_FOLLOWS_ROBOT= true 32 | 33 | 34 | 35 | # ---------------------------------------------------------- 36 | # Kalman Filter generic options 37 | # ---------------------------------------------------------- 38 | [RangeBearingKFSLAM_KalmanFilter] 39 | # 0: Full EKF 40 | # 1: EKF 'a la' Davison 41 | method=0 42 | 43 | verbose=1 44 | 45 | 46 | #------------------------------------------------- 47 | # Options defined by CRangeBearingKFSLAM class 48 | #------------------------------------------------- 49 | [RangeBearingKFSLAM] 50 | stdXY_no_odo=0.1 51 | stdPhi_no_odo_deg=2 // degs 52 | 53 | std_odo_z_additional=0 // Additional uncertainty in z 54 | 55 | force_ignore_odometry = false 56 | 57 | # Used for the sensor model 58 | std_sensor_range = 0.03 // meters 59 | std_sensor_yaw_deg = 0.1 // degrees 60 | std_sensor_pitch_deg = 0.1 // degrees 61 | 62 | 63 | # Exagerate the uncertainties for ease of visualization: 64 | quantiles_3D_representation=3 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_2d/tutorial/kf-slam_demo_2d.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_ekf_slam_2d/tutorial/kf-slam_demo_2d.rawlog -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_ekf_slam_3d 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.19 (2024-09-18) 6 | ------------------- 7 | * Fix cmake target creation 8 | * Workaround for missing TBB::tbb target in Noetic 9 | * cmake: fix case and whitespaces 10 | * Contributors: Jose Luis Blanco-Claraco 11 | 12 | 0.1.18 (2024-09-05) 13 | ------------------- 14 | * Port to new 'mrpt_lib*' packages 15 | * Contributors: Jose Luis Blanco-Claraco 16 | 17 | 0.1.17 (2024-06-05) 18 | ------------------- 19 | 20 | 0.1.16 (2024-03-25) 21 | ------------------- 22 | 23 | 0.1.15 (2023-08-02) 24 | ------------------- 25 | 26 | 0.1.14 (2023-04-12) 27 | ------------------- 28 | * Fix build against last mrpt 2.7.x 29 | * Contributors: Jose Luis Blanco-Claraco 30 | 31 | 0.1.11 (2022-06-24) 32 | ------------------- 33 | * Ported to tf2 and mrpt::ros1bridge 34 | * Fixed all 2D and 3D EKF wrappers to work with mrpt 2.x API 35 | * Update build dep to mrpt2 36 | * Contributors: Jose Luis Blanco Claraco 37 | 38 | 0.1.10 (2019-10-05) 39 | ------------------- 40 | * fix build against mrpt2 41 | * Contributors: Jose Luis Blanco-Claraco 42 | 43 | 0.1.9 (2019-04-14) 44 | ------------------ 45 | * Fix build against MRPT 1.9.9 46 | * Switch header guards to pragma once 47 | * Contributors: Jose Luis Blanco-Claraco, Julian Lopez Velasquez, Vladislav Tananaev 48 | 49 | 0.1.8 (2018-09-21) 50 | ------------------ 51 | * Make catkin_lint clean 52 | * Contributors: Jose Luis Blanco Claraco 53 | 54 | 0.1.7 (2018-09-20) 55 | ------------------ 56 | 57 | 0.1.6 (2018-06-14) 58 | ------------------ 59 | * Fixed compilation with MRPT 1.5 60 | * fix build in mrpt 2.0 61 | * fix build; add optimized builds (-O3) 62 | * use consistent cmake conventions for c++14 63 | * Fixes and cleanups for config files 64 | * [mrpt_ekf_slam_3d] Update description and dependencies of package.xml 65 | Add test for *.launch files. 66 | * Move transforms to callbacks. 67 | * Define C++11 avoiding direct manipulation of CXX_FLAGS 68 | * catkin_lint error fixes 69 | * Contributors: Jose Luis Blanco, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Logrus, Magnus Gärtner, Max Kuzmin, Vladislav Tananaev 70 | 71 | 0.1.5 (2016-11-18) 72 | ------------------ 73 | * Don't instruct cmake to search eigen3, mrpt has it 74 | * Fix cmakelists typo 75 | * avoid cmake errors if eigen is not found 76 | * Contributors: Jose Luis Blanco-Claraco, Nikos Koukis 77 | 78 | 0.1.3 (2016-09-27) 79 | ------------------ 80 | * Avoid gcc warnings with MRPT < 2.0.0 81 | * fix build against mrpt < 1.5.0 82 | * Contributors: Jose-Luis Blanco-Claraco 83 | 84 | 0.1.2 (2016-09-24) 85 | ------------------ 86 | * Make formatting conform to ROS C++ Style Guide. 87 | * fix eigen3 dependency problem 88 | * Output logs only in ROS. 89 | * Add a guard for new mrpt::ros1bridge::rosLoggerLvlToMRPTLoggerLvl and mrpt::ros1bridge::mrptToROSLoggerCallback functions for MRPT version less than 1.5.0. 90 | * Add streaming of MRPT logs to ROS logs. 91 | * Contributors: Jose Luis Blanco, Logrus, Vladislav Tananaev 92 | 93 | 0.1.1 (2016-08-22) 94 | ------------------ 95 | * First public version, as a result of Vladislav Tananaev's GSoC2016 work. 96 | * Skip mrpt_ekf_slam_3d if version of MRPT is lower than 1.5.0. 97 | * Contributors: Jose Luis Blanco, Logrus 98 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 3.1) 4 | project(mrpt_ekf_slam_3d) 5 | #find catkin packages 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | roslib 9 | roscpp 10 | tf2 11 | tf2_ros 12 | tf2_geometry_msgs 13 | std_msgs 14 | nav_msgs 15 | sensor_msgs 16 | visualization_msgs 17 | dynamic_reconfigure 18 | roslaunch 19 | mrpt_msgs_bridge 20 | ) 21 | 22 | #find mrpt packages 23 | find_package(mrpt-gui REQUIRED) 24 | find_package(mrpt-slam REQUIRED) 25 | find_package(mrpt-ros1bridge REQUIRED) 26 | message(STATUS "MRPT_VERSION: ${mrpt-slam_VERSION}") 27 | 28 | # fix for Noetic: 29 | if(NOT EXISTS TBB::tbb AND TBB_LIBRARY AND TBB_INCLUDE_DIRS) 30 | add_library(MY_TBB INTERFACE) 31 | target_include_directories(MY_TBB INTERFACE ${TBB_INCLUDE_DIRS}) 32 | target_link_libraries(MY_TBB INTERFACE ${TBB_LIBRARY}) 33 | add_library(TBB::tbb ALIAS MY_TBB) 34 | endif() 35 | 36 | 37 | 38 | if(CMAKE_COMPILER_IS_GNUCXX) 39 | # High level of warnings. 40 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 41 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 42 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 43 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 44 | if(NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 45 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 46 | endif() 47 | endif() 48 | 49 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 50 | add_compile_options(-O3) 51 | endif() 52 | 53 | catkin_package( 54 | CATKIN_DEPENDS 55 | nav_msgs 56 | sensor_msgs 57 | std_msgs 58 | visualization_msgs 59 | tf2 60 | tf2_ros 61 | tf2_geometry_msgs 62 | mrpt_msgs_bridge 63 | ) 64 | 65 | ########### 66 | ## Build ## 67 | ########### 68 | 69 | include_directories( 70 | ${catkin_INCLUDE_DIRS} 71 | # ${Eigen_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} 72 | include 73 | ) 74 | 75 | 76 | add_executable(mrpt_ekf_slam_3d 77 | src/mrpt_ekf_slam_3d.cpp 78 | src/mrpt_ekf_slam_3d_wrapper.cpp 79 | src/mrpt_ekf_slam_3d_app.cpp 80 | ) 81 | 82 | TARGET_LINK_LIBRARIES(mrpt_ekf_slam_3d 83 | mrpt::gui 84 | mrpt::slam 85 | mrpt::ros1bridge 86 | ${catkin_LIBRARIES} 87 | ) 88 | 89 | ############# 90 | ## Install ## 91 | ############# 92 | 93 | # Mark executables and/or libraries for installation 94 | install(TARGETS mrpt_ekf_slam_3d 95 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | ) 99 | 100 | # Mark other files for installation (e.g. launch and bag files, etc.) 101 | install(DIRECTORY 102 | launch 103 | tutorial 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | ) 106 | 107 | ############# 108 | ## Testing ## 109 | ############# 110 | # Add testing for *.launch files 111 | roslaunch_add_file_check(launch) 112 | 113 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/config/rosconsole.config: -------------------------------------------------------------------------------- 1 | # Set the default ros output 2 | log4j.logger.ros=WARN 3 | log4j.logger.ros.mrpt_ekf_slam_3d=DEBUG -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/include/mrpt_ekf_slam_3d/mrpt_ekf_slam_3d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File:mrpt_ekf_slam_3d.h 3 | * Author: Vladislav Tananaev 4 | * 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 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 | 31 | /** 32 | * @brief The EKFslam class provides EKF SLAM 3d from MRPT libraries. 33 | * 34 | */ 35 | class EKFslam 36 | { 37 | public: 38 | /** 39 | * @brief constructor 40 | */ 41 | EKFslam(); 42 | /** 43 | * @brief destructor 44 | */ 45 | virtual ~EKFslam(); 46 | /** 47 | * @brief init 3D window from mrpt lib 48 | */ 49 | void init3Dwindow(); 50 | /** 51 | * @brief run 3D window update from mrpt lib 52 | */ 53 | void run3Dwindow(); 54 | /** 55 | * @brief convert landmark to 3d point 56 | */ 57 | void landmark_to_3d( 58 | const mrpt::slam::CRangeBearingKFSLAM::KFArray_FEAT& lm, 59 | mrpt::math::TPoint3D& p); 60 | /** 61 | * @brief read ini file 62 | * 63 | * @param ini_filename the name of the ini file to read 64 | */ 65 | void read_iniFile(std::string ini_filename); 66 | /** 67 | * @brief calculate the actions from odometry model for current observation 68 | * 69 | * @param _sf current observation 70 | * @param _odometry raw odometry 71 | */ 72 | void observation( 73 | mrpt::obs::CSensoryFrame::Ptr _sf, 74 | mrpt::obs::CObservationOdometry::Ptr _odometry); 75 | 76 | protected: 77 | mrpt::slam::CRangeBearingKFSLAM mapping; ///< EKF slam 3d class 78 | 79 | mrpt::system::TTimeStamp 80 | timeLastUpdate_; ///< last update of the pose and map 81 | 82 | mrpt::obs::CActionCollection::Ptr action; ///< actions 83 | mrpt::obs::CSensoryFrame::Ptr sf; ///< observations 84 | 85 | mrpt::poses::CPose3D 86 | odomLastObservation_; ///< last observation of odometry 87 | mrpt::obs::CActionRobotMovement3D::TMotionModelOptions 88 | motion_model_options_; ///< used with odom value motion noise 89 | 90 | mrpt::gui::CDisplayWindow3D::Ptr win3d; ///< MRPT window 91 | bool SHOW_3D_LIVE; 92 | bool CAMERA_3DSCENE_FOLLOWS_ROBOT; 93 | std::vector meanPath; 94 | mrpt::poses::CPose3DQuatPDFGaussian robotPose_; ///< current robot pose 95 | std::vector LMs_; ///< vector of the landmarks 96 | /// vector of the landmarks ID 97 | std::map LM_IDs_; 98 | mrpt::math::CMatrixDouble fullCov_; ///< full covariance matrix 99 | mrpt::math::CVectorDouble fullState_; ///< full state vector 100 | }; 101 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/launch/ekf_slam_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/launch/ekf_slam_3d_rawlog.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/launch/ekf_slam_3d_wheeled_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b mrpt_ekf_slam_3d is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_ekf_slam_3d 4 | 0.1.19 5 | This package is a wrapper for the implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks. 6 | Jose Luis Blanco-Claraco 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/mrpt_ekf_slam_3d 11 | https://github.com/mrpt-ros-pkg/mrpt_slam/issues 12 | 13 | Jose Luis 14 | Vladislav Tananaev 15 | 16 | http://mrpt.org/ 17 | 18 | catkin 19 | 20 | roscpp 21 | roslaunch 22 | roslib 23 | tf2 24 | tf2_ros 25 | tf2_geometry_msgs 26 | std_msgs 27 | sensor_msgs 28 | visualization_msgs 29 | nav_msgs 30 | dynamic_reconfigure 31 | cmake_modules 32 | mrpt_libros_bridge 33 | mrpt_libgui 34 | mrpt_libslam 35 | mrpt_msgs_bridge 36 | 37 | rviz 38 | mrpt_rawlog 39 | 40 | 41 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/rviz/rviz_conf_ekf_3d.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /MarkerArray1 12 | Splitter Ratio: 0.5 13 | Tree Height: 358 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/TF 55 | Enabled: true 56 | Frame Timeout: 15 57 | Frames: 58 | All Enabled: true 59 | base_link: 60 | Value: true 61 | landmark_link: 62 | Value: true 63 | map: 64 | Value: true 65 | odom: 66 | Value: true 67 | Marker Scale: 1 68 | Name: TF 69 | Show Arrows: true 70 | Show Axes: true 71 | Show Names: true 72 | Tree: 73 | map: 74 | odom: 75 | base_link: 76 | landmark_link: 77 | {} 78 | Update Interval: 0 79 | Value: true 80 | - Class: rviz/MarkerArray 81 | Enabled: true 82 | Marker Topic: /state_viz 83 | Name: MarkerArray 84 | Namespaces: 85 | "": true 86 | Queue Size: 100 87 | Value: true 88 | - Class: rviz/MarkerArray 89 | Enabled: true 90 | Marker Topic: /data_association_viz 91 | Name: MarkerArray 92 | Namespaces: 93 | "": true 94 | Queue Size: 100 95 | Value: true 96 | Enabled: true 97 | Global Options: 98 | Background Color: 48; 48; 48 99 | Fixed Frame: map 100 | Frame Rate: 30 101 | Name: root 102 | Tools: 103 | - Class: rviz/Interact 104 | Hide Inactive Objects: true 105 | - Class: rviz/MoveCamera 106 | - Class: rviz/Select 107 | - Class: rviz/FocusCamera 108 | - Class: rviz/Measure 109 | - Class: rviz/SetInitialPose 110 | Topic: /initialpose 111 | - Class: rviz/SetGoal 112 | Topic: /move_base_simple/goal 113 | - Class: rviz/PublishPoint 114 | Single click: true 115 | Topic: /clicked_point 116 | Value: true 117 | Views: 118 | Current: 119 | Class: rviz/Orbit 120 | Distance: 18.7924 121 | Enable Stereo Rendering: 122 | Stereo Eye Separation: 0.06 123 | Stereo Focal Distance: 1 124 | Swap Stereo Eyes: false 125 | Value: false 126 | Focal Point: 127 | X: -1.30632 128 | Y: 2.29616 129 | Z: 2.56103 130 | Name: Current View 131 | Near Clip Distance: 0.01 132 | Pitch: 0.425398 133 | Target Frame: 134 | Value: Orbit (rviz) 135 | Yaw: 5.71541 136 | Saved: ~ 137 | Window Geometry: 138 | Displays: 139 | collapsed: false 140 | Height: 639 141 | Hide Left Dock: false 142 | Hide Right Dock: true 143 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000001f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001f5000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001d9000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005150000003efc0100000002fb0000000800540069006d0065010000000000000515000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003d3000001f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 144 | Selection: 145 | collapsed: false 146 | Time: 147 | collapsed: false 148 | Tool Properties: 149 | collapsed: false 150 | Views: 151 | collapsed: true 152 | Width: 1301 153 | X: 65 154 | Y: 24 155 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/src/mrpt_ekf_slam_3d_app.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "mrpt_ekf_slam_3d/mrpt_ekf_slam_3d_wrapper.h" 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "mrpt_ekf_slam_3d"); 7 | ros::NodeHandle n; 8 | ros::Rate r(100); 9 | EKFslamWrapper slam; 10 | slam.get_param(); 11 | slam.init(); 12 | ros::Duration(3).sleep(); 13 | 14 | if (!slam.rawlogPlay()) 15 | { 16 | while (ros::ok()) 17 | { 18 | ros::spinOnce(); 19 | r.sleep(); 20 | } 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/tutorial/kf-slam_6D_demo.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------ 2 | # Config file for the KF-SLAM application 3 | # See: http://www.mrpt.org/list-of-mrpt-apps/application-kf-slam/ 4 | #------------------------------------------------------ 5 | 6 | 7 | #------------------------------------------------- 8 | # Section: [MappingApplication] 9 | # Use: Here comes global parameters for the app. 10 | #------------------------------------------------- 11 | [MappingApplication] 12 | 13 | # The source file (RAW-LOG) with action/observation pairs 14 | rawlog_file=../../datasets/kf-slam_6D_demo.rawlog //not used 15 | 16 | # Left blank if not available 17 | ground_truth_file=../../datasets/kf-slam_6D_demo_ground_truth.txt //not used 18 | 19 | # Left blank if not available 20 | ground_truth_file_robot=../../datasets/kf-slam_6D_demo_ground_truth_robot_path.txt //not used 21 | 22 | # The directory where the log files will be saved (left in blank if no log is required) 23 | logOutput_dir=LOG_EKF-SLAM //not used 24 | 25 | SAVE_LOG_FREQUENCY=1 //not used 26 | SAVE_MAP_REPRESENTATIONS = false //not used 27 | SAVE_3D_SCENES = false //not used 28 | 29 | SHOW_3D_LIVE = true 30 | CAMERA_3DSCENE_FOLLOWS_ROBOT = true 31 | 32 | # ---------------------------------------------------------- 33 | # Kalman Filter generic options 34 | # ---------------------------------------------------------- 35 | [RangeBearingKFSLAM_KalmanFilter] 36 | # 0: Full EKF 37 | # 1: EKF 'a la' Davison (More efficient!) 38 | method=1 39 | 40 | verbose=1 41 | 42 | 43 | #------------------------------------------------- 44 | # Options defined by CRangeBearingKFSLAM class 45 | #------------------------------------------------- 46 | [RangeBearingKFSLAM] 47 | stdXY_no_odo=0.1 48 | stdPhi_no_odo_deg=2 // degs 49 | 50 | std_odo_z_additional=0.05 // Additional uncertainty in z 51 | 52 | # Used for the sensor model 53 | std_sensor_range= 0.02 // meters 54 | std_sensor_yaw_deg=0.3 // degrees 55 | std_sensor_pitch_deg=0.3 // degrees 56 | 57 | 58 | # Exagerate the uncertainties for ease of visualization: 59 | quantiles_3D_representation=20 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /mrpt_ekf_slam_3d/tutorial/kf-slam_6D_demo.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_ekf_slam_3d/tutorial/kf-slam_6D_demo.rawlog -------------------------------------------------------------------------------- /mrpt_graphslam_2d/.gitignore: -------------------------------------------------------------------------------- 1 | rosbags/* 2 | rviz/*bag* 3 | rviz/*gazebo* 4 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 3.1) 4 | project(mrpt_graphslam_2d) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | std_msgs 13 | sensor_msgs 14 | geometry_msgs 15 | nav_msgs 16 | mrpt_msgs 17 | mrpt_msgs_bridge 18 | tf2 19 | tf2_ros 20 | tf2_geometry_msgs 21 | fkie_multimaster_msgs 22 | ) 23 | ## System dependencies are found with CMake's conventions 24 | find_package(mrpt-gui REQUIRED) 25 | find_package(mrpt-graphslam REQUIRED) 26 | find_package(mrpt-ros1bridge REQUIRED) 27 | 28 | message(STATUS "MRPT_VERSION: ${mrpt-slam_VERSION}") 29 | 30 | # fix for Noetic: 31 | if(NOT EXISTS TBB::tbb AND TBB_LIBRARY AND TBB_INCLUDE_DIRS) 32 | add_library(MY_TBB INTERFACE) 33 | target_include_directories(MY_TBB INTERFACE ${TBB_INCLUDE_DIRS}) 34 | target_link_libraries(MY_TBB INTERFACE ${TBB_LIBRARY}) 35 | add_library(TBB::tbb ALIAS MY_TBB) 36 | endif() 37 | 38 | 39 | if(CMAKE_COMPILER_IS_GNUCXX) 40 | # High level of warnings. 41 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 42 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 43 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 44 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 45 | if(NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 46 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 47 | endif() 48 | endif() 49 | 50 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 51 | add_compile_options(-O3) 52 | endif() 53 | 54 | ################################################ 55 | ## Declare ROS messages, services and actions ## 56 | ################################################ 57 | 58 | ################################################ 59 | ## Declare ROS dynamic reconfigure parameters ## 60 | ################################################ 61 | 62 | ################################### 63 | ## catkin specific configuration ## 64 | ################################### 65 | 66 | catkin_package( 67 | # INCLUDE_DIRS include 68 | #LIBRARIES mrpt_graphslam_2d 69 | CATKIN_DEPENDS 70 | roscpp 71 | rospy 72 | std_msgs 73 | sensor_msgs 74 | geometry_msgs 75 | nav_msgs 76 | mrpt_msgs 77 | mrpt_msgs_bridge 78 | tf2 79 | tf2_ros 80 | tf2_geometry_msgs 81 | fkie_multimaster_msgs 82 | # DEPENDS system_lib 83 | ) 84 | 85 | ########### 86 | ## Build ## 87 | ########### 88 | 89 | ## Specify additional locations of header files 90 | ## Your package locations should be listed before other locations 91 | include_directories( 92 | include 93 | ${catkin_INCLUDE_DIRS} 94 | ) 95 | 96 | set(SR_GRAPHSLAM_SRC 97 | src/CConnectionManager.cpp 98 | src/common.cpp 99 | ) 100 | 101 | set(MR_GRAPHSLAM_MERGER_SRC 102 | src/CMapMerger.cpp 103 | src/TNeighborAgentMapProps.cpp 104 | ) 105 | 106 | add_library(${PROJECT_NAME} 107 | ${SR_GRAPHSLAM_SRC} 108 | ) 109 | add_executable(${PROJECT_NAME}_node 110 | ${SR_GRAPHSLAM_SRC} 111 | src/${PROJECT_NAME}_node.cpp 112 | ) 113 | add_executable(${PROJECT_NAME}_mr_node 114 | ${SR_GRAPHSLAM_SRC} 115 | src/${PROJECT_NAME}_mr_node.cpp 116 | ) 117 | add_executable(map_merger_node 118 | ${SR_GRAPHSLAM_SRC} 119 | ${MR_GRAPHSLAM_MERGER_SRC} 120 | src/map_merger_node.cpp 121 | ) 122 | 123 | ## Specify libraries to link a library or executable target against 124 | target_link_libraries(${PROJECT_NAME} 125 | mrpt::gui 126 | mrpt::graphslam 127 | mrpt::ros1bridge 128 | ${catkin_LIBRARIES} 129 | ) 130 | target_link_libraries(${PROJECT_NAME}_node 131 | mrpt::gui 132 | mrpt::graphslam 133 | mrpt::ros1bridge 134 | ${catkin_LIBRARIES} 135 | ) 136 | target_link_libraries(${PROJECT_NAME}_mr_node 137 | mrpt::gui 138 | mrpt::graphslam 139 | mrpt::ros1bridge 140 | ${catkin_LIBRARIES} 141 | ) 142 | target_link_libraries(map_merger_node 143 | mrpt::gui 144 | mrpt::graphslam 145 | mrpt::ros1bridge 146 | ${catkin_LIBRARIES} 147 | ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # Mark executables and/or libraries for installation 154 | install( 155 | TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_mr_node map_merger_node 156 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 157 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 158 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 159 | ) 160 | 161 | # Mark other files for installation (e.g. launch and bag files, etc.) 162 | install(DIRECTORY 163 | launch 164 | config 165 | rviz 166 | rosbags 167 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini: -------------------------------------------------------------------------------- 1 | //---------------------------------------------------------------------------- 2 | // Config file for the using the graphslam-engine application 3 | // 4 | // ~ The MRPT Library ~ 5 | //---------------------------------------------------------------------------- 6 | 7 | ############################################################### 8 | # Sat Jun 25 16:43:35 EEST 2016, Nikos Koukis, nickkouk@gmail.com 9 | # Used, tested for the following specifications 10 | # observations handled : CObservationOdometry, CObservation2DRangeScan, CActionRobotMovement2D 11 | # Produced map : 2D 12 | # Graph type : mrpt::graphs::CNetworkOfPoses2DInf 13 | # NodeRegistrationDecider tested : CFixedIntervalsNRD 14 | # EdgeRegistrationDecider : CICPGoodnessERD 15 | # GraphSlamSolver : CLevMarqGSO 16 | # Datasets used : 17 | # MRPT/share/mrpt/datasets/graphslam-engine/action_observations_map/range_030_bearing_015.rawlog 18 | # MRPT/share/mrpt/datasets/graphslam-engine-demos/observation_only_map2/range_030_bearing_015.rawlog 19 | ############################################################### 20 | 21 | 22 | [GeneralConfiguration] 23 | 24 | output_dir_fname = graphslam_results 25 | user_decides_about_output_dir = false 26 | save_3DScene = true 27 | save_3DScene_fname = output_scene.3DScene 28 | save_graph = true 29 | save_graph_fname = output_graph.graph 30 | 31 | ; Set the verbosity of the output messages. Only messages over the specified 32 | ; will be printed to the console. 33 | ; Available options in ascending order are as follows: 34 | ; LVL_DEBUG => 0, 35 | ; LVL_INFO => 1, 36 | ; LVL_WARN => 2, 37 | ; LVL_ERROR => 3 38 | class_verbosity = 1 39 | 40 | 41 | ; min node difference for an edge to be considered as a loop closure 42 | ; Used in both the visualization procedure for updating the Loop closures 43 | ; counter as well as the optimization procedure since it affects how often we 44 | ; otpimize the full graph 45 | LC_min_nodeid_diff= 100 46 | 47 | ####################################################### 48 | [NodeRegistrationDeciderParameters] 49 | 50 | registration_max_distance = 0.1 // meters 51 | registration_max_angle = 5 // degrees 52 | class_verbosity = 1 53 | 54 | 55 | ######################################################## 56 | [EdgeRegistrationDeciderParameters] 57 | 58 | ;ICP_goodness_thresh = 0.10 // threshold for accepting the ICP constraint in the graph 59 | ICP_goodness_thresh = 0.70 // threshold for accepting the ICP constraint in the graph 60 | ;ICP_goodness_thresh = 1 // no ICP constraints added 61 | 62 | // Either check ICP using distance OR node count from current node 63 | ICP_max_distance = 0.5 // maximum distance for checking other nodes for ICP constraints 64 | ;ICP_max_distance = -1 // check against all other nodes 65 | 66 | class_verbosity = 1 67 | 68 | [OptimizerParameters] 69 | 70 | optimization_on_second_thread = false 71 | optimization_distance = 0.4; 72 | 73 | // Levenberg-Marquardt parameters 74 | verbose = false 75 | profiler = false 76 | max_iterations = 100 77 | scale_hessian = 0.2 78 | tau = 1e-3 79 | 80 | class_verbosity = 1 81 | 82 | [MappingParameters] 83 | 84 | resolution=0.03 85 | mapAltitude = 0 86 | useMapAltitude = 0 87 | maxDistanceInsertion = 5 88 | maxOccupancyUpdateCertainty = 0.80 89 | considerInvalidRangesAsFreeSpace = 1 90 | decimation = 1 91 | horizontalTolerance = 0.00088 92 | wideningBeamsWithDistance = 1 93 | 94 | 95 | ######################################################## 96 | # seems that it doesn't read hex, using cfg_file.read_int 97 | # hardcode the integer values instead 98 | # http://www.binaryhexconverter.com/hex-to-decimal-converter 99 | [VisualizationParameters] 100 | 101 | visualize_optimized_graph = 1 102 | 103 | optimized_show_ID_labels = 0 104 | optimized_show_ground_grid = 1 105 | optimized_show_edges = 1 106 | optimized_edge_color = 4278257152 107 | optimized_edge_width = 0.5 108 | optimized_show_node_corners = 1 109 | optimized_show_edge_rel_poses = 0 110 | optimized_edge_rel_poses_color = 1090486271 111 | optimized_nodes_edges_corner_scale = 0.0 112 | optimized_nodes_corner_scale = 0.1 113 | optimized_point_size = 10 114 | optimized_point_color = 1090486271 115 | 116 | visualize_map = true 117 | visualize_laser_scans = true 118 | visualize_odometry_poses = true 119 | visualize_ground_truth = true 120 | visualize_estimated_trajectory = true 121 | visualize_SLAM_metric = false // extra displayPlots showing the evolution of the SLAM metric 122 | enable_curr_pos_viewport = true 123 | 124 | [ICP] 125 | maxIterations = 100 // The maximum number of iterations to execute if convergence is not achieved before 126 | minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: 127 | minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: 128 | 129 | thresholdDist = 0.3 // Initial maximum distance for matching a pair of points 130 | thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. 131 | 132 | ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) 133 | 134 | smallestThresholdDist = 0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. 135 | onlyClosestCorrespondences = false // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) 136 | doRANSAC = true 137 | 138 | # 0: icpClassic 139 | # 1: icpLevenbergMarquardt 140 | ICP_algorithm = icpLevenbergMarquardt 141 | 142 | # decimation to apply to the point cloud being registered against the map 143 | # Reduce to "1" to obtain the best accuracy 144 | corresponding_points_decimation = 5 145 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h" 13 | 14 | #include 15 | #include 16 | 17 | namespace mrpt { namespace graphslam { 18 | 19 | /**\brief Class template that provides a wrapper around the MRPT 20 | * CGraphSlamEngine class template and implements methods for interacting with 21 | * ROS. 22 | * 23 | */ 24 | template 25 | class CGraphSlamEngine_ROS : public CGraphSlamEngine 26 | { 27 | public: 28 | typedef CGraphSlamEngine parent; 29 | 30 | CGraphSlamEngine_ROS( 31 | ros::NodeHandle* nh, 32 | const std::string& config_file, 33 | const std::string& rawlog_fname="", 34 | const std::string& fname_GT="", 35 | mrpt::graphslam::CWindowManager* win_manager=NULL, 36 | mrpt::graphslam::deciders::CNodeRegistrationDecider* node_reg=NULL, 37 | mrpt::graphslam::deciders::CEdgeRegistrationDecider* edge_reg=NULL, 38 | mrpt::graphslam::optimizers::CGraphSlamOptimizer* optimizer=NULL 39 | ); 40 | virtual ~CGraphSlamEngine_ROS(); 41 | /**\brief Wrapper method around the protected setup* class methods. 42 | * 43 | * Handy for setting up publishers, subscribers, services 44 | * all at once. 45 | * 46 | */ 47 | void setupComm(); 48 | 49 | /**\brief Initialize object instance. */ 50 | void initClass(); 51 | ros::NodeHandle* m_nh; 52 | protected: 53 | /**\brief Read the problem configuration parameters 54 | * 55 | * \sa readROSParameters, CGraphSlamEngine::loadParams 56 | */ 57 | void readParams(); 58 | /**\brief Provide feedback about the SLAM operation 59 | * 60 | * Method makes the necessary calls to all the publishers of the class and 61 | * broadcaster instances 62 | */ 63 | virtual void usePublishersBroadcasters(); 64 | /**\brief Read configuration parameters from the ROS parameter server. 65 | * 66 | * \note Method is automatically called on object construction. 67 | * \sa readParams, initClass 68 | */ 69 | void readROSParameters(); 70 | virtual bool _execGraphSlamStep( 71 | mrpt::obs::CActionCollection::Ptr& action, 72 | mrpt::obs::CSensoryFrame::Ptr& observations, 73 | mrpt::obs::CObservation::Ptr& observation, 74 | size_t& rawlog_entry); 75 | 76 | /**\name setup* ROS-related methods 77 | *\brief Methods for setting up topic subscribers, publishers, and 78 | * corresponding services 79 | * 80 | * \sa setupComm 81 | */ 82 | /**\{*/ 83 | virtual void setupSubs(); 84 | virtual void setupPubs(); 85 | virtual void setupSrvs(); 86 | /**\}*/ 87 | /**\brief Custom Callback queue for processing requests for the 88 | * services outside the standard CallbackQueue. 89 | * 90 | * \note Logical thing would be to define it in CGraphSlamEngine_MR, but that 91 | * results in an segfault with the following error message: 92 | * ``` 93 | * pure virtual method called 94 | * terminate called without an active exception 95 | * ``` 96 | */ 97 | ros::CallbackQueue custom_service_queue; 98 | 99 | 100 | int m_queue_size; 101 | }; 102 | 103 | } } // end of namespaces 104 | 105 | #include "mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h" 106 | 107 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | namespace mrpt { namespace graphslam { 12 | 13 | template 14 | CGraphSlamEngine_ROS::CGraphSlamEngine_ROS( 15 | ros::NodeHandle* nh, 16 | const std::string& config_file, 17 | const std::string& rawlog_fname/* ="" */, 18 | const std::string& fname_GT /* ="" */, 19 | mrpt::graphslam::CWindowManager* win_manager /* = NULL */, 20 | mrpt::graphslam::deciders::CNodeRegistrationDecider* node_reg /* = NULL */, 21 | mrpt::graphslam::deciders::CEdgeRegistrationDecider* edge_reg /* = NULL */, 22 | mrpt::graphslam::optimizers::CGraphSlamOptimizer* optimizer /* = NULL */): 23 | parent::CGraphSlamEngine( 24 | config_file, 25 | rawlog_fname, 26 | fname_GT, 27 | win_manager, 28 | node_reg, 29 | edge_reg, 30 | optimizer), 31 | m_nh(nh) 32 | { 33 | this->initClass(); 34 | } 35 | 36 | template 37 | CGraphSlamEngine_ROS::~CGraphSlamEngine_ROS() { 38 | MRPT_LOG_DEBUG_STREAM("In Destructor: Deleting CGraphSlamEngine_ROS instance..."); 39 | ros::shutdown(); 40 | } 41 | 42 | template 43 | void CGraphSlamEngine_ROS::initClass() { 44 | using namespace mrpt::graphslam; 45 | this->m_class_name = "CGraphSlamEngine_ROS"; 46 | this->setLoggerName(this->m_class_name); 47 | 48 | // http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#queue_size:_publish.28.29_behavior_and_queuing 49 | m_queue_size = 10; 50 | 51 | this->setupComm(); 52 | 53 | // in case of ROS specific deciders/optimizers (they inherit from the CDeciderOrOptimizer_ROS interface) 54 | // set the NodeHandle 55 | { 56 | CRegistrationDeciderOrOptimizer_ROS* dec_opt_ros = 57 | dynamic_cast*>(this->m_node_reg); 58 | 59 | if (dec_opt_ros) { 60 | dec_opt_ros->setNodeHandle(m_nh); 61 | } 62 | } 63 | { 64 | CRegistrationDeciderOrOptimizer_ROS* dec_opt_ros = 65 | dynamic_cast*>(this->m_edge_reg); 66 | if (dec_opt_ros) { 67 | dec_opt_ros->setNodeHandle(m_nh); 68 | } 69 | } 70 | { 71 | CRegistrationDeciderOrOptimizer_ROS* dec_opt_ros = 72 | dynamic_cast*>(this->m_optimizer); 73 | if (dec_opt_ros) { 74 | dec_opt_ros->setNodeHandle(m_nh); 75 | } 76 | } 77 | 78 | this->readParams(); 79 | } 80 | 81 | template 82 | void CGraphSlamEngine_ROS::readParams() { 83 | 84 | this->readROSParameters(); 85 | } 86 | 87 | template 88 | void CGraphSlamEngine_ROS::readROSParameters() { } 89 | 90 | template 91 | bool CGraphSlamEngine_ROS::_execGraphSlamStep( 92 | mrpt::obs::CActionCollection::Ptr& action, 93 | mrpt::obs::CSensoryFrame::Ptr& observations, 94 | mrpt::obs::CObservation::Ptr& observation, 95 | size_t& rawlog_entry) { 96 | 97 | bool continue_exec = parent::_execGraphSlamStep( 98 | action, observations, observation, rawlog_entry); 99 | this->usePublishersBroadcasters(); 100 | 101 | return continue_exec; 102 | } 103 | 104 | template 105 | void CGraphSlamEngine_ROS::usePublishersBroadcasters() { } 106 | 107 | template 108 | void CGraphSlamEngine_ROS::setupComm() { 109 | MRPT_LOG_INFO_STREAM( 110 | "Setting up subscribers, publishers, services..."); 111 | 112 | // setup subscribers, publishers, services... 113 | this->setupSubs(); 114 | this->setupPubs(); 115 | this->setupSrvs(); 116 | 117 | } 118 | 119 | template 120 | void CGraphSlamEngine_ROS::setupSubs() { 121 | MRPT_LOG_DEBUG_STREAM("Setting up subscribers..."); 122 | 123 | } 124 | 125 | template 126 | void CGraphSlamEngine_ROS::setupPubs() { 127 | MRPT_LOG_DEBUG_STREAM("Setting up publishers..."); 128 | 129 | } 130 | 131 | template 132 | void CGraphSlamEngine_ROS::setupSrvs() { 133 | MRPT_LOG_DEBUG_STREAM("Setting up services..."); 134 | 135 | } 136 | 137 | 138 | } } // end of namespaces 139 | 140 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/CMapMerger.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | #pragma once 12 | 13 | #include "mrpt_graphslam_2d/CConnectionManager.h" 14 | #include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" 15 | #include "mrpt_graphslam_2d/misc/common.h" 16 | 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 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | const mrpt::poses::CPose3D EMPTY_POSE; 39 | 40 | using namespace mrpt::system; 41 | using namespace mrpt::img; 42 | using namespace mrpt::maps; 43 | using namespace mrpt::obs; 44 | 45 | namespace mrpt 46 | { 47 | namespace graphslam 48 | { 49 | /**\brief Class responsible of the execution of the map_merger_node. 50 | * 51 | * Instance of the graph asks of the generated maps of all the running 52 | * GraphSlamAgents computes a feasible map-merging, if any, and returns the 53 | * resulting global map to the user. 54 | */ 55 | class CMapMerger 56 | { 57 | public: 58 | typedef std::map maps_t; 59 | typedef std::map neighbor_to_is_used_t; 60 | typedef std::map 61 | neighbor_to_rel_pose_t; 62 | 63 | /**\brief Robot trajectory visual object type */ 64 | typedef std::map 65 | trajectories_t; 66 | typedef std::vector neighbors_t; 67 | CMapMerger(mrpt::system::COutputLogger* logger_in, ros::NodeHandle* nh_in); 68 | ~CMapMerger(); 69 | void mergeMaps(); 70 | /**\brief Query and fetch the list of new graphSLAM agents. 71 | * 72 | * \return True if execution is to continue normally. 73 | */ 74 | bool updateState(); 75 | 76 | private: 77 | /**\brief Compact method for monitoring the given keystrokes for the given 78 | * observer. 79 | */ 80 | void monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer); 81 | void initWindowVisuals(mrpt::graphslam::CWindowManager* win_manager); 82 | mrpt::graphslam::CWindowManager* initWindowVisuals(); 83 | 84 | /**\brief CConnectionManager instance for fetching the running graphSLAM 85 | * agents 86 | */ 87 | neighbors_t m_neighbors; 88 | std::map m_neighbors_to_windows; 89 | mrpt::system::COutputLogger* m_logger; 90 | ros::NodeHandle* m_nh; 91 | mrpt::graphslam::detail::CConnectionManager m_conn_manager; 92 | 93 | /**\brief Topic namespace under which current node is going to be 94 | * publishing. 95 | */ 96 | std::string m_global_ns; 97 | /**\brief Topic namespace under which, options that are used during the map 98 | * alignment procedure are fetched from 99 | */ 100 | std::string m_options_ns; 101 | std::string m_feedback_ns; 102 | size_t m_queue_size; 103 | mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options; 104 | 105 | std::string quit_keypress1; 106 | std::string quit_keypress2; 107 | std::string map_merge_keypress; 108 | 109 | bool save_map_merging_results; 110 | 111 | mrpt::graphslam::CWindowManager* m_fused_map_win_manager; 112 | 113 | }; // end of CMapMerger 114 | 115 | } // namespace graphslam 116 | } // namespace mrpt 117 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | #pragma once 12 | 13 | #include "mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace mrpt 19 | { 20 | namespace graphslam 21 | { 22 | namespace deciders 23 | { 24 | /** Loop Closer Edge Registration Decider class that can also be used in 25 | * multi-robot SLAM operations since it can utilize information from other 26 | * robot agents and correct own graph according to the strategy described in 27 | * [1]. 28 | * 29 | * \note Multi-robot-related classes are suffixed with _MR. 30 | */ 31 | template 32 | class CLoopCloserERD_MR : public virtual CLoopCloserERD, 33 | public virtual CEdgeRegistrationDecider_MR 34 | { 35 | public: 36 | /**\brief Handy typedefs */ 37 | /**\{*/ 38 | /**\brief type of graph constraints */ 39 | typedef CLoopCloserERD lc_parent_t; /**< parent class */ 40 | typedef CEdgeRegistrationDecider_MR 41 | mr_parent_t; /**< parent class */ 42 | typedef CLoopCloserERD_MR decider_t; /**< handy self type */ 43 | typedef typename lc_parent_t::constraint_t constraint_t; 44 | typedef typename lc_parent_t::pose_t pose_t; 45 | typedef typename lc_parent_t::range_ops_t range_ops_t; 46 | typedef typename lc_parent_t::partitions_t partitions_t; 47 | typedef typename lc_parent_t::nodes_to_scans2D_t nodes_to_scans2D_t; 48 | typedef mrpt::graphslam::CGraphSlamEngine_MR engine_t; 49 | typedef typename GRAPH_T::global_pose_t global_pose_t; 50 | /**\}*/ 51 | 52 | CLoopCloserERD_MR(); 53 | 54 | // member implementations 55 | bool updateState( 56 | mrpt::obs::CActionCollection::Ptr action, 57 | mrpt::obs::CSensoryFrame::Ptr observations, 58 | mrpt::obs::CObservation::Ptr observation); 59 | void addBatchOfNodeIDsAndScans( 60 | const std::map& 61 | nodeIDs_to_scans2D); 62 | void addScanMatchingEdges(TNodeID curr_nodeID); 63 | void fetchNodeIDsForScanMatching( 64 | const TNodeID& curr_nodeID, std::set* nodes_set); 65 | 66 | protected: 67 | }; 68 | 69 | } // namespace deciders 70 | } // namespace graphslam 71 | } // namespace mrpt 72 | 73 | #include "CLoopCloserERD_MR_impl.h" 74 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | namespace mrpt { namespace graphslam { namespace deciders { 13 | 14 | // Ctors, Dtors 15 | template 16 | CLoopCloserERD_MR::CLoopCloserERD_MR() { 17 | // CLoopCloser Ctor is automatically called. 18 | 19 | // since this is for MR-SLAM, we do expect more than one node registered 20 | // between successive calls 21 | this->m_override_registered_nodes_check = true; 22 | 23 | this->initializeLoggers("CLoopCloserERD_MR"); 24 | } 25 | 26 | template 27 | void CLoopCloserERD_MR::addBatchOfNodeIDsAndScans( 28 | const std::map< 29 | TNodeID, 30 | mrpt::obs::CObservation2DRangeScan::Ptr>& nodeIDs_to_scans2D) { 31 | mr_parent_t::addBatchOfNodeIDsAndScans(nodeIDs_to_scans2D); 32 | 33 | this->updateMapPartitions(/*full update=*/ true, 34 | /* is_first_time_node_reg = */ false); 35 | 36 | } // end of addBatchOfNodeIDsAndScans 37 | 38 | template 39 | void CLoopCloserERD_MR::addScanMatchingEdges( 40 | TNodeID curr_nodeID) { 41 | MRPT_START; 42 | 43 | // Do scan-matching only if I have initially registered curr_nodeID in the 44 | // graph. 45 | bool is_own = this->m_engine->isOwnNodeID(curr_nodeID); 46 | if (is_own) { 47 | lc_parent_t::addScanMatchingEdges(curr_nodeID); 48 | } 49 | 50 | MRPT_END; 51 | } 52 | 53 | template 54 | void CLoopCloserERD_MR::fetchNodeIDsForScanMatching( 55 | const TNodeID& curr_nodeID, 56 | std::set* nodes_set) { 57 | ASSERT_(nodes_set); 58 | 59 | size_t fetched_nodeIDs = 0; 60 | for (int nodeID_i = static_cast(curr_nodeID)-1; 61 | (fetched_nodeIDs <= static_cast(this->m_laser_params.prev_nodes_for_ICP) && 62 | nodeID_i >= 0); 63 | --nodeID_i) { 64 | if (this->m_engine->isOwnNodeID(nodeID_i)) { 65 | nodes_set->insert(nodeID_i); 66 | fetched_nodeIDs++; 67 | } 68 | } 69 | } 70 | 71 | // Member methods implementations 72 | template 73 | bool CLoopCloserERD_MR::updateState( 74 | mrpt::obs::CActionCollection::Ptr action, 75 | mrpt::obs::CSensoryFrame::Ptr observations, 76 | mrpt::obs::CObservation::Ptr observation ) { 77 | 78 | bool success = lc_parent_t::updateState(action, observations, observation); 79 | 80 | // search for possible edges with the other agent's graph. 81 | // TODO 82 | 83 | return success; 84 | } 85 | 86 | 87 | } } } // end of namespaces 88 | 89 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | #include "mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h" 13 | #include 14 | 15 | namespace mrpt { namespace graphslam { namespace deciders { 16 | 17 | template 18 | class CFixedIntervalsNRD_MR : 19 | public virtual CFixedIntervalsNRD, 20 | public virtual CNodeRegistrationDecider_MR 21 | { 22 | public: 23 | typedef CNodeRegistrationDecider_MR parent_cm; 24 | typedef CFixedIntervalsNRD parent_mrpt; 25 | typedef typename GRAPH_T::global_pose_t global_pose_t; 26 | 27 | CFixedIntervalsNRD_MR(); 28 | 29 | private: 30 | 31 | 32 | }; 33 | 34 | } } } // end of namespaces 35 | 36 | #include "CFixedIntervalsNRD_MR_impl.h" 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | namespace mrpt { namespace graphslam { namespace deciders { 13 | 14 | template 15 | CFixedIntervalsNRD_MR::CFixedIntervalsNRD_MR() { 16 | this->initializeLoggers("CFixedIntervalsNRD_MR"); 17 | } 18 | 19 | } } } // end of namespaces 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | #include "mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h" 13 | #include 14 | 15 | namespace mrpt { namespace graphslam { namespace deciders { 16 | 17 | template 18 | class CICPCriteriaNRD_MR : 19 | public virtual CICPCriteriaNRD, 20 | public virtual CNodeRegistrationDecider_MR 21 | { 22 | public: 23 | typedef CNodeRegistrationDecider_MR parent_cm; 24 | typedef CICPCriteriaNRD parent_mrpt; 25 | typedef typename GRAPH_T::global_pose_t global_pose_t; 26 | 27 | CICPCriteriaNRD_MR(); 28 | 29 | private: 30 | 31 | 32 | }; 33 | 34 | } } } // end of namespaces 35 | 36 | #include "CICPCriteriaNRD_MR_impl.h" 37 | 38 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | namespace mrpt { namespace graphslam { namespace deciders { 13 | 14 | template 15 | CICPCriteriaNRD_MR::CICPCriteriaNRD_MR() { 16 | this->initializeLoggers("CICPCriteriaNRD_MR"); 17 | } 18 | 19 | } } } // end of namespaces 20 | 21 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/TNeighborAgentMapProps.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace mrpt 22 | { 23 | namespace graphslam 24 | { 25 | /**\brief Struct holding a COccupancyGridMap2D (as well as subscribers for 26 | * updating it) for a specific running GraphSlamAgent instance. 27 | * 28 | * \note used in the global map-merging procedure 29 | */ 30 | struct TNeighborAgentMapProps 31 | { 32 | TNeighborAgentMapProps( 33 | mrpt::system::COutputLogger* logger_in, 34 | const mrpt_msgs::GraphSlamAgent& agent_in, ros::NodeHandle* nh_in); 35 | 36 | void readROSParameters(); 37 | void setupComm(); 38 | void setupSubs(); 39 | void setupPubs(); 40 | /**\brief Callback method to be called when new data in the map topic is 41 | * available 42 | */ 43 | void updateGridMap(const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap); 44 | /**\brief Callback method to be called when new data in the Estimated 45 | * Trajectory topic is available 46 | */ 47 | void updateRobotTrajectory(const nav_msgs::Path::ConstPtr& nav_robot_traj); 48 | 49 | /**\brief Return how many nodes have been registered in the fetched robot 50 | * trajectory 51 | */ 52 | size_t getNodeCount() { return nav_robot_trajectory->poses.size(); } 53 | 54 | mrpt::system::COutputLogger* m_logger; 55 | ros::NodeHandle* nh; 56 | /**\brief Pointer to the GraphSlamAgent instance of the neighbor */ 57 | const mrpt_msgs::GraphSlamAgent& agent; 58 | /**\brief Map generated by the corresponding agent - in ROS form */ 59 | nav_msgs::OccupancyGrid::ConstPtr nav_map; 60 | /**\brief Trajectory of the correspondign agent - in ROS form */ 61 | nav_msgs::Path::ConstPtr nav_robot_trajectory; 62 | /**\name Subscriber instances */ 63 | /**\{ */ 64 | /**\brief Map subscriber instance */ 65 | ros::Subscriber map_sub; 66 | ros::Subscriber robot_trajectory_sub; 67 | /**\} */ 68 | /**\name Topic names */ 69 | /**\{ */ 70 | /**\brief Map topic to subscribe and fetch the map from */ 71 | std::string map_topic; 72 | std::string robot_trajectory_topic; 73 | /**\} */ 74 | size_t queue_size; 75 | bool has_init_class; 76 | bool has_setup_comm; 77 | 78 | }; // end of TNeighborAgentMapProps 79 | 80 | } // namespace graphslam 81 | } // namespace mrpt 82 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include "mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR.h" 17 | #include "mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR.h" 18 | #include "mrpt_graphslam_2d/ERD/CLoopCloserERD_MR.h" 19 | 20 | namespace mrpt { namespace graphslam { namespace apps { 21 | 22 | template 23 | struct TUserOptionsChecker_ROS: 24 | public mrpt::graphslam::apps::TUserOptionsChecker { 25 | 26 | /**\name handy typedefs for the creation of deciders/optimzer instances from 27 | * the corresponding strings 28 | * 29 | * \note ROS-related classes are suffixed with _ROS 30 | */ 31 | /**\{*/ 32 | typedef std::map< 33 | std::string, 34 | mrpt::graphslam::deciders::CNodeRegistrationDecider*(*)()> 35 | node_regs_t; 36 | typedef std::map< 37 | std::string, 38 | mrpt::graphslam::deciders::CEdgeRegistrationDecider*(*)()> 39 | edge_regs_t; 40 | typedef std::map< 41 | std::string, 42 | mrpt::graphslam::optimizers::CGraphSlamOptimizer*(*)()> 43 | optimizers_t; 44 | /**\brief Parent class */ 45 | typedef mrpt::graphslam::apps::TUserOptionsChecker parent; 46 | /**\}*/ 47 | 48 | TUserOptionsChecker_ROS(); 49 | ~TUserOptionsChecker_ROS(); 50 | /**\brief Create deciders, optimizers specific to the ROS case */ 51 | void createDeciderOptimizerMappings(); 52 | void populateDeciderOptimizerProperties(); 53 | 54 | }; 55 | 56 | 57 | } } } // end of namespaces 58 | #include "mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h" 59 | 60 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | #pragma once 10 | 11 | namespace mrpt { namespace graphslam { namespace apps { 12 | 13 | template 14 | TUserOptionsChecker_ROS::TUserOptionsChecker_ROS() { 15 | } 16 | 17 | template 18 | TUserOptionsChecker_ROS::~TUserOptionsChecker_ROS() { 19 | } 20 | 21 | template 22 | void TUserOptionsChecker_ROS:: 23 | createDeciderOptimizerMappings() { 24 | using namespace std; 25 | using namespace mrpt::graphs; 26 | using namespace mrpt::graphslam::apps; 27 | using namespace mrpt::graphslam::deciders; 28 | parent::createDeciderOptimizerMappings(); 29 | 30 | // node registration deciders 31 | this->node_regs_map["CICPCriteriaNRD_MR"] = 32 | parent::template createNodeRegistrationDecider>; 33 | this->node_regs_map["CFixedIntervalsNRD_MR"] = 34 | parent::template createNodeRegistrationDecider>; 35 | 36 | // edge registration deciders 37 | this->edge_regs_map["CLoopCloserERD_MR"] = 38 | parent::template createEdgeRegistrationDecider>; 39 | 40 | // optimizers 41 | 42 | } // end of createDeciderOptimizerMappings 43 | 44 | template 45 | void TUserOptionsChecker_ROS::populateDeciderOptimizerProperties() { 46 | using namespace mrpt::graphslam::apps; 47 | using namespace std; 48 | 49 | parent::populateDeciderOptimizerProperties(); 50 | { // CICPCriteriaNRD_MR 51 | TRegistrationDeciderProps* dec = new TRegistrationDeciderProps; 52 | dec->name = "CICPCriteriaNRD_MR"; 53 | dec->description = 54 | "Multi-robot SLAM implementation of the CICPCriteriaNRD class based on \"Condensed Measurements\""; 55 | dec->type = "Node"; 56 | dec->rawlog_format = "Both"; 57 | dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2"); 58 | dec->is_mr_slam_class = "true"; 59 | 60 | this->regs_descriptions.push_back(dec); 61 | } 62 | { // CFixedIntervalsNRD_MR 63 | TRegistrationDeciderProps* dec = new TRegistrationDeciderProps; 64 | dec->name = "CFixedIntervalsNRD_MR"; 65 | dec->description = 66 | "Multi-robot SLAM implementation of the CFixedIntervalsNRD class based on \"Condensed Measurements\""; 67 | dec->type = "Node"; 68 | dec->rawlog_format = "Both"; 69 | dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2"); 70 | dec->is_mr_slam_class = "true"; 71 | 72 | this->regs_descriptions.push_back(dec); 73 | } 74 | 75 | { // CLoopCloserERD_MR 76 | TRegistrationDeciderProps* dec = new TRegistrationDeciderProps; 77 | dec->name = "CLoopCloserERD_MR"; 78 | dec->description = 79 | "Multi-robot SLAM implementation of the CLoopCloserERD class based on \"Condensed Measurements\""; 80 | dec->type = "Edge"; 81 | dec->rawlog_format = "Both"; 82 | dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2"); 83 | dec->is_mr_slam_class = "true"; 84 | 85 | this->regs_descriptions.push_back(dec); 86 | } 87 | 88 | } 89 | 90 | } } } //end namespaces 91 | 92 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | #pragma once 11 | 12 | #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h" 13 | #include "mrpt_graphslam_2d/CConnectionManager.h" 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace mrpt::graphs; 19 | 20 | namespace mrpt 21 | { 22 | namespace graphslam 23 | { 24 | namespace deciders 25 | { 26 | /**\brief Edge Registration Decider virtual method. 27 | * 28 | * \b Edge Registration Decider classes that are to be used in a multi-robot 29 | * SLAM scheme according to the Condensed Measurements multi-robot strategy by 30 | * M.T. Lazaro et al. [1] are to inherit from this method. 31 | * 32 | * \note Condensed Measurements-related classes are suffixed with _MR. 33 | * 34 | * \note For an example of inheriting from this class, see the 35 | * mrpt::graphslam::deciders::CLoopCloserERD_MR. 36 | * 37 | * [1] Multi-robot 39 | * SLAM using Condensed Measurements - M.T. Lazaro, L.M. Paz, P. Pinies, 40 | * J.A. Castellanos, G. Grisetti 41 | */ 42 | template 43 | class CEdgeRegistrationDecider_MR 44 | : public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer_MR< 45 | GRAPH_T>, 46 | public virtual mrpt::graphslam::deciders:: 47 | CRangeScanEdgeRegistrationDecider 48 | { 49 | public: 50 | CEdgeRegistrationDecider_MR(); 51 | ~CEdgeRegistrationDecider_MR(); 52 | virtual void addBatchOfNodeIDsAndScans( 53 | const std::map& 54 | nodeIDs_to_scans2D); 55 | 56 | protected: 57 | }; 58 | 59 | } // namespace deciders 60 | } // namespace graphslam 61 | } // namespace mrpt 62 | 63 | #include "CEdgeRegistrationDecider_MR_impl.h" 64 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | namespace mrpt { namespace graphslam { namespace deciders { 13 | 14 | template 15 | CEdgeRegistrationDecider_MR::CEdgeRegistrationDecider_MR() {} 16 | 17 | template 18 | CEdgeRegistrationDecider_MR::~CEdgeRegistrationDecider_MR() {} 19 | 20 | template 21 | void CEdgeRegistrationDecider_MR::addBatchOfNodeIDsAndScans( 22 | const std::map< 23 | TNodeID, 24 | mrpt::obs::CObservation2DRangeScan::Ptr>& nodeIDs_to_scans2D) { 25 | 26 | this->m_nodes_to_laser_scans2D.insert( 27 | nodeIDs_to_scans2D.begin(), 28 | nodeIDs_to_scans2D.end()); 29 | 30 | // update the last known number of nodeIDs 31 | this->m_last_total_num_nodes = this->m_graph->nodeCount(); 32 | 33 | } // end of addBatchOfNodeIDsAndScans 34 | 35 | 36 | } } } // end of namespaces 37 | 38 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | namespace mrpt { namespace graphslam { namespace optimizers { 13 | 14 | /**\brief Interface for implementing graphSLAM optimizer classes specific to 15 | * the Condensed Measurements MR-SLAM case 16 | */ 17 | class CGraphSlamOptimizer_MR : 18 | public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer_MR 19 | public virtual CGraphSlamOptimizer 20 | { 21 | public: 22 | CGraphSlamOptimizer_MR (); 23 | ~CGraphSlamOptimizer_MR (); 24 | 25 | private: 26 | 27 | 28 | } } } // end of namespaces 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | namespace mrpt { namespace graphslam { namespace optimizers { 13 | 14 | template 15 | CGraphSlamOptimizer_MR::CGraphSlamOptimizer_MR() {} 16 | 17 | template 18 | CGraphSlamOptimizer_MR::~CGraphSlamOptimizer_MR() {} 19 | 20 | } } } // end of namespaces 21 | 22 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h" 13 | #include 14 | #include 15 | 16 | namespace mrpt { namespace graphslam { namespace deciders { 17 | 18 | /**\brief Node Registration Decider Interface Class. 19 | * 20 | * \b Node Registration Decider classes that are to be used in a multi-robot 21 | * SLAM scheme according to the Condensed Measurements multi-robot strategy by 22 | * M.T. Lazaro et al. [1] are to inherit from this method. 23 | * 24 | * \note Condensed Measurements-related classes are suffixed with _MR. 25 | * 26 | * \note For an example of inheriting from this class, see the 27 | * mrpt::graphslam::deciders::CFixedIntervalsNRD_MR. 28 | * 29 | * [1] Multi-robot 31 | * SLAM using Condensed Measurements - M.T. Lazaro, L.M. Paz, P. Pinies, 32 | * J.A. Castellanos, G. Grisetti 33 | */ 34 | template 35 | class CNodeRegistrationDecider_MR : 36 | public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer_MR, 37 | public virtual CNodeRegistrationDecider 38 | { 39 | public: 40 | typedef typename GRAPH_T::global_pose_t global_pose_t; 41 | 42 | CNodeRegistrationDecider_MR (); 43 | ~CNodeRegistrationDecider_MR (); 44 | protected: 45 | /**\brief Decorate a pose according to the TMRSlamNodeAnnotation fields 46 | * 47 | * \note Do this only for the nodes that are initially registered in the graph by 48 | * the current CGraphSlamEngine_t class. Nodes of other graphSLAM-agents that 49 | * are to be integrated must have already filled these fields. 50 | */ 51 | void addNodeAnnotsToPose(global_pose_t* pose) const; 52 | }; 53 | 54 | } } } // end of namespaces 55 | 56 | #include "CNodeRegistrationDecider_MR_impl.h" 57 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR_impl.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | #pragma once 12 | 13 | namespace mrpt 14 | { 15 | namespace graphslam 16 | { 17 | namespace deciders 18 | { 19 | template 20 | CNodeRegistrationDecider_MR::CNodeRegistrationDecider_MR() 21 | { 22 | } 23 | 24 | template 25 | CNodeRegistrationDecider_MR::~CNodeRegistrationDecider_MR() 26 | { 27 | } 28 | 29 | template 30 | void CNodeRegistrationDecider_MR::addNodeAnnotsToPose( 31 | global_pose_t* pose) const 32 | { 33 | } 34 | 35 | template <> 36 | void CNodeRegistrationDecider_MR:: 37 | addNodeAnnotsToPose(global_pose_t* pose) const 38 | { 39 | ASSERT_(pose); 40 | 41 | pose->agent_ID_str = this->own_ns; 42 | // ASSUMPTION: addNodeAnnotsToPose is going to be called right before the 43 | // actual registration. 44 | // Mark it with the nodeID that is up-next 45 | pose->nodeID_loc = this->m_graph->nodeCount(); 46 | } 47 | 48 | } // namespace deciders 49 | } // namespace graphslam 50 | } // namespace mrpt 51 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mrpt_graphslam_2d/CGraphSlamEngine_MR.h" 4 | #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h" 5 | #include "mrpt_graphslam_2d/CConnectionManager.h" 6 | 7 | // forward declaration 8 | namespace mrpt { namespace graphslam { 9 | template class CGraphSlamEngine_MR; 10 | } }// end of namespaces 11 | 12 | namespace mrpt { namespace graphslam { 13 | 14 | /**\brief Interface for implementing deciders/optimizers related to the Condensed 15 | * Measurements multi-robot graphSLAM algorithm. 16 | * 17 | * \warning Beware that this class does not inherit from the 18 | * mrpt::graphslam::CNodeRegistrationDeciderOrOptimizer. 19 | * 20 | * \note Condensed Measurements-related classes are suffixed with _MR. 21 | * 22 | */ 23 | template 24 | class CRegistrationDeciderOrOptimizer_MR : 25 | public mrpt::graphslam::CRegistrationDeciderOrOptimizer_ROS 26 | { 27 | public: 28 | typedef CGraphSlamEngine_MR engine_t; 29 | 30 | CRegistrationDeciderOrOptimizer_MR(); 31 | ~CRegistrationDeciderOrOptimizer_MR(); 32 | 33 | void setCGraphSlamEnginePtr(const engine_t* engine); 34 | virtual void setCConnectionManagerPtr( 35 | mrpt::graphslam::detail::CConnectionManager* conn_manager); 36 | 37 | protected: 38 | /**\brief Pointer to the CConnectionManager instance 39 | */ 40 | mrpt::graphslam::detail::CConnectionManager* m_conn_manager; 41 | /**\brief Constant pointer to the CGraphSlamEngine_MR instance. 42 | */ 43 | const engine_t* m_engine; 44 | std::string own_ns; 45 | 46 | 47 | }; 48 | 49 | } } // end of namespaces 50 | 51 | // template methods implementations 52 | #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR_impl.h" 53 | 54 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR_impl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace mrpt { namespace graphslam { 4 | 5 | template 6 | CRegistrationDeciderOrOptimizer_MR::CRegistrationDeciderOrOptimizer_MR() { 7 | this->is_mr_slam_class = true; 8 | } 9 | 10 | template 11 | CRegistrationDeciderOrOptimizer_MR::~CRegistrationDeciderOrOptimizer_MR() { } 12 | 13 | template 14 | void CRegistrationDeciderOrOptimizer_MR::setCConnectionManagerPtr( 15 | mrpt::graphslam::detail::CConnectionManager* conn_manager) { 16 | ASSERTMSG_(conn_manager, "\nInvalid CConnectionManager* pointer.\n"); 17 | 18 | m_conn_manager = conn_manager; 19 | own_ns = m_conn_manager->getTrimmedNs(); 20 | } 21 | 22 | template 23 | void CRegistrationDeciderOrOptimizer_MR::setCGraphSlamEnginePtr( 24 | const engine_t* engine) { 25 | ASSERTMSG_(engine, "CGraphSlamEngine pointer is NULL"); 26 | m_engine = engine; 27 | } 28 | 29 | 30 | 31 | } } // end of namespaces 32 | 33 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace mrpt { namespace graphslam { 9 | 10 | /**\brief Interface class that all ROS-specific deciders/optimizers can inherit 11 | * from. 12 | * 13 | * \note ROS-related classes are suffixed with _ROS 14 | */ 15 | template 16 | class CRegistrationDeciderOrOptimizer_ROS : 17 | public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer 18 | { 19 | public: 20 | CRegistrationDeciderOrOptimizer_ROS(); 21 | virtual ~CRegistrationDeciderOrOptimizer_ROS(); 22 | 23 | virtual void setNodeHandle(ros::NodeHandle* nh); 24 | 25 | protected: 26 | /**\brief NodeHandle instance 27 | */ 28 | ros::NodeHandle* m_nh; 29 | 30 | 31 | }; 32 | 33 | 34 | } } // end of namespaces 35 | 36 | #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS_impl.h" 37 | 38 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS_impl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace mrpt { namespace graphslam { 4 | 5 | 6 | template 7 | CRegistrationDeciderOrOptimizer_ROS::CRegistrationDeciderOrOptimizer_ROS() { } 8 | template 9 | CRegistrationDeciderOrOptimizer_ROS::~CRegistrationDeciderOrOptimizer_ROS() { } 10 | 11 | template 12 | void CRegistrationDeciderOrOptimizer_ROS::setNodeHandle(ros::NodeHandle* nh) { 13 | ASSERTMSG_(nh, "\nInvalid NodeHandle instance was provided.\n"); 14 | 15 | m_nh = nh; 16 | } 17 | 18 | 19 | } } // end of namespaces 20 | 21 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/include/mrpt_graphslam_2d/misc/common.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | | 3 | http://www.mrpt.org/ | | | | Copyright (c) 4 | 2005-2016, Individual contributors, see AUTHORS file | | See: 5 | http://www.mrpt.org/Authors - All rights reserved. | | 6 | Released under BSD License. See details in http://www.mrpt.org/License | 7 | +---------------------------------------------------------------------------+ 8 | */ 9 | 10 | /**\file common.h 11 | * 12 | * Defined methods used in different parts of the mrpt_graphslam_2d application. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace mrpt 23 | { 24 | namespace graphslam 25 | { 26 | namespace detail 27 | { 28 | std::string getGridMapAlignmentResultsAsString( 29 | const mrpt::poses::CPosePDF& pdf, 30 | const mrpt::slam::CGridMapAligner::TReturnInfo& ret_info); 31 | 32 | bool isEssentiallyZero(const mrpt::poses::CPose2D& p); 33 | 34 | } // namespace detail 35 | } // namespace graphslam 36 | } // namespace mrpt 37 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/launch/graphslam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/launch/setup_robot_for_mr_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/launch/sr_graphslam_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 11 | 12 | 13 | 14 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/launch/sr_graphslam_demo_gt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/nodes/rename_rviz_topics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Tue Jun 13 17:21:40 EEST 2017, Nikos Koukis 3 | 4 | """ 5 | Current script generates rviz files based on the templates files of 6 | rviz/templates and usign the current computer's hostname for determining the 7 | namespaces of the topics. This is a required step for visualizing multi-robot 8 | graphSLAM when running based on rosbags or based on measurements from Gazebo 9 | since each multi-robot agent runs on a separate ROS core to simulate as much as 10 | possible a real-time setup. 11 | 12 | """ 13 | 14 | import os 15 | import sys 16 | import logging 17 | import socket 18 | 19 | from colorlog import ColoredFormatter 20 | 21 | LOG_LEVEL = logging.DEBUG 22 | LOGFORMAT = ("%(log_color)s%(levelname)-5s%(reset)s " 23 | "| %(log_color)s%(message)s%(reset)s") 24 | 25 | logging.root.setLevel(LOG_LEVEL) 26 | formatter = ColoredFormatter(LOGFORMAT) 27 | stream = logging.StreamHandler() 28 | stream.setLevel(LOG_LEVEL) 29 | stream.setFormatter(formatter) 30 | 31 | logger = logging.getLogger("RvizRenamer") 32 | logger.setLevel(LOG_LEVEL) 33 | logger.addHandler(stream) 34 | 35 | 36 | 37 | def rename_topics_in_rviz_file(templ_file, replace_dict): 38 | """Read from a single template file and write the modified contents to an 39 | actual rviz file one directory prior to the templates directory. 40 | """ 41 | 42 | with open(templ_file, 'r') as templ: 43 | templ_lines = templ.readlines() 44 | head, tail = os.path.split(templ_file) 45 | 46 | # create the actual rviz files in the parent dir of templates 47 | actual_file = os.path.join(os.path.dirname(head), tail) 48 | logger.info("Writing file: %s" % os.path.abspath(actual_file)) 49 | with open(actual_file, 'w') as f: 50 | # rewrite its line of the templ file by making the appropriate 51 | # substitution 52 | f.writelines([l.format(**replace_dict) for l in templ_lines]) 53 | 54 | 55 | def validate_args(): 56 | """Make sure that the user understands what the script does.""" 57 | 58 | if len(sys.argv) > 1: 59 | logger.warning("Current script modifies the template rviz files " 60 | "found in rviz/templates so that their topics match the " 61 | "running computer's hostname.\n" 62 | "Run this without any additional arguments.") 63 | logger.warning("Exiting...") 64 | sys.exit(-1) 65 | 66 | 67 | def main(): 68 | """Main.""" 69 | 70 | logger.info("Initializing...") 71 | validate_args() 72 | 73 | # fetch the rviz files that topics renaming is to happen 74 | script_dir = os.path.dirname(os.path.realpath(__file__)) 75 | rviz_dir_path = os.path.join(script_dir, "..", "rviz", "templates") 76 | 77 | rviz_templ_files = [os.path.join(rviz_dir_path, i) 78 | for i in os.listdir(rviz_dir_path)] 79 | 80 | # do the renaming only for files used for "gazebo" (suffix "gazebo") or 81 | # recorded with rosbag (suffix "gazebo") 82 | 83 | rviz_templ_files = filter(lambda name: "bag" in name or "gazebo" in name, 84 | rviz_templ_files) 85 | 86 | curr_hostname = socket.gethostname() 87 | replace_dict = {"HOSTNAME_PLACEHOLDER": curr_hostname} 88 | 89 | logger.info("Rviz files to operate on:\n%s\n\n" % 90 | os.linesep.join([os.path.abspath(f) 91 | for f in rviz_templ_files])) 92 | 93 | # print the strings to be replaced 94 | logger.info("Replacing: %s ==> %s" % replace_dict.items()[0]) 95 | 96 | for templ_file in rviz_templ_files: 97 | rename_topics_in_rviz_file(templ_file, replace_dict) 98 | logger.info("All done.") 99 | 100 | 101 | if __name__ == "__main__": 102 | main() 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_graphslam_2d 4 | 0.1.19 5 | 6 | Implement graphSLAM using the mrpt-graphslam library, in an online fashion 7 | by directly reading measurements off ROS Topics. 8 | 9 | 10 | http://www.mrpt.org/ 11 | https://wiki.ros.org/mrpt_slam 12 | Nikos Koukis 13 | Jose Luis Blanco-Claraco 14 | Nikos Koukis 15 | BSD 16 | 17 | catkin 18 | 19 | roscpp 20 | rospy 21 | geometry_msgs 22 | nav_msgs 23 | std_msgs 24 | sensor_msgs 25 | mrpt_libros_bridge 26 | mrpt_libgui 27 | mrpt_libapps 28 | mrpt_msgs 29 | mrpt_msgs_bridge 30 | tf2 31 | tf2_ros 32 | tf2_geometry_msgs 33 | fkie_multimaster_msgs 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/rosbags/demo_short_loop/demo.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_graphslam_2d/rosbags/demo_short_loop/demo.bag -------------------------------------------------------------------------------- /mrpt_graphslam_2d/rosbags/demo_short_loop/pioneer_agent.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_graphslam_2d/rosbags/demo_short_loop/pioneer_agent.jpg -------------------------------------------------------------------------------- /mrpt_graphslam_2d/rosbags/demo_short_loop/workspace.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_graphslam_2d/rosbags/demo_short_loop/workspace.jpg -------------------------------------------------------------------------------- /mrpt_graphslam_2d/rviz/README.md: -------------------------------------------------------------------------------- 1 | Directory contains rviz-related files 2 | 3 | Currently the \*gazebo\* rviz files refer to static topics and the user has to 4 | manually change csldesktop to his own hostname for them to appropriately 5 | visualize the procedure. Same goes for the \*real\* files. 6 | 7 | There are plans to dynamically create the rviz files based on the user's hostname / current configuration using the [rviz_generator package](https://github.com/bergercookie/rviz_generator) 8 | 9 | 10 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/src/TNeighborAgentMapProps.cpp: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | #include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" 12 | 13 | using namespace mrpt::system; 14 | using namespace mrpt_msgs; 15 | using namespace mrpt::maps; 16 | using namespace mrpt::graphslam; 17 | using namespace ros; 18 | using namespace nav_msgs; 19 | using namespace std; 20 | 21 | TNeighborAgentMapProps::TNeighborAgentMapProps( 22 | mrpt::system::COutputLogger* logger_in, 23 | const mrpt_msgs::GraphSlamAgent& agent_in, ros::NodeHandle* nh_in) 24 | : m_logger(logger_in), 25 | nh(nh_in), 26 | agent(agent_in), 27 | queue_size(1), 28 | has_init_class(false), 29 | has_setup_comm(false) 30 | { 31 | ASSERT_(nh); 32 | ASSERT_(m_logger); 33 | m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps constructor"); 34 | 35 | this->map_topic = 36 | "/" + agent.topic_namespace.data + "/" + "feedback" + "/" + "gridmap"; 37 | this->robot_trajectory_topic = "/" + agent.topic_namespace.data + "/" + 38 | "feedback" + "/" + "robot_trajectory"; 39 | 40 | cout << "Map topic: " << this->map_topic << endl; 41 | cout << "Trajectory topic: " << this->robot_trajectory_topic << endl; 42 | 43 | has_init_class = true; 44 | readROSParameters(); 45 | } 46 | 47 | void TNeighborAgentMapProps::readROSParameters() 48 | { 49 | // std::map agent_pose; 50 | // bool res = nh->getParam("/" + agent.topic_namespace.data + "/" + 51 | // "global_pos", agent_pose); 52 | 53 | // if (res) { 54 | // m_logger->logFmt(LVL_INFO, "%s", getMapAsString(agent_pose).c_str()); 55 | 56 | // global_init_pos = pose_t( 57 | // atof(pos_x), 58 | // atof(pos_y), 59 | // atof(rot_z)); 60 | // m_logger->logFmt(LVL_INFO, "global_init_pos: %s\n", global_init_pos); 61 | //} 62 | // else { 63 | // cout << "parameter fetch was not successful" << endl; 64 | //} 65 | } 66 | 67 | void TNeighborAgentMapProps::setupComm() 68 | { 69 | ASSERT_(has_init_class); 70 | m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupComm"); 71 | this->setupSubs(); 72 | has_setup_comm = true; 73 | } 74 | 75 | void TNeighborAgentMapProps::setupSubs() 76 | { 77 | m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupSubs"); 78 | this->map_sub = nh->subscribe( 79 | this->map_topic, this->queue_size, 80 | &TNeighborAgentMapProps::updateGridMap, this); 81 | 82 | this->robot_trajectory_sub = nh->subscribe( 83 | this->robot_trajectory_topic, this->queue_size, 84 | &TNeighborAgentMapProps::updateRobotTrajectory, this); 85 | } // end of setupSubs 86 | 87 | void TNeighborAgentMapProps::updateGridMap( 88 | const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap) 89 | { 90 | nav_map = nav_gridmap; 91 | } // end of updateGridMap 92 | 93 | void TNeighborAgentMapProps::updateRobotTrajectory( 94 | const nav_msgs::Path::ConstPtr& nav_robot_traj) 95 | { 96 | nav_robot_trajectory = nav_robot_traj; 97 | } 98 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/src/common.cpp: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | | 3 | http://www.mrpt.org/ | | | | Copyright (c) 4 | 2005-2016, Individual contributors, see AUTHORS file | | See: 5 | http://www.mrpt.org/Authors - All rights reserved. | | 6 | Released under BSD License. See details in http://www.mrpt.org/License | 7 | +---------------------------------------------------------------------------+ 8 | */ 9 | 10 | #include 11 | #include "mrpt_graphslam_2d/misc/common.h" 12 | #include 13 | 14 | using namespace mrpt::containers; 15 | using namespace mrpt::poses; 16 | using namespace mrpt::math; 17 | using namespace std; 18 | using namespace mrpt::math; 19 | 20 | std::string mrpt::graphslam::detail::getGridMapAlignmentResultsAsString( 21 | const mrpt::poses::CPosePDF& pdf, 22 | const mrpt::slam::CGridMapAligner::TReturnInfo& ret_info) 23 | { 24 | CPosePDFSOG::Ptr pdf_out = CPosePDFSOG::Create(); 25 | pdf_out->copyFrom(pdf); 26 | CPose2D pose_out; 27 | CMatrixDouble33 cov_out; 28 | pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out); 29 | 30 | stringstream ss; 31 | ss << "--------------------" << endl; 32 | ss << "Results: " << endl 33 | << "\tPDFPtr pose: " << pdf.getMeanVal() << endl 34 | << "\t# Correspondences: " << ret_info.correspondences.size() << endl 35 | << "\tAlignment goodness: " << ret_info.goodness << endl 36 | << "\tModes size: " << pdf_out->size() << endl 37 | << "\tModes: " << endl 38 | << getSTLContainerAsString(pdf_out->getSOGModes()) 39 | << "\tMost likely pose: " << pose_out << endl 40 | << "\tCorresponding covariance matrix: " << endl 41 | << cov_out << endl; 42 | ss << "--------------------" << endl; 43 | return ss.str(); 44 | } 45 | 46 | bool mrpt::graphslam::detail::isEssentiallyZero(const mrpt::poses::CPose2D& p) 47 | { 48 | double epsilon = 0.001; 49 | return ( 50 | approximatelyEqual(p.x(), 0.0, epsilon) && // all 0s 51 | approximatelyEqual(p.y(), 0.0, epsilon) && 52 | approximatelyEqual(p.phi(), 0.0, epsilon)); 53 | } 54 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/src/map_merger_node.cpp: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "mrpt_graphslam_2d/CMapMerger.h" 18 | #include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" 19 | 20 | using namespace mrpt::graphslam; 21 | using namespace mrpt::graphslam::detail; 22 | using namespace mrpt_msgs; 23 | using namespace mrpt::maps; 24 | using namespace ros; 25 | using namespace nav_msgs; 26 | using namespace std; 27 | 28 | /**\brief Node that fetches the local maps produced by the graphSLAM agents and 29 | * joins them together using a RANSAC-based map-merging technique 30 | * 31 | * Node is to be used for inspecting the overall graphSLAM procedure and 32 | * present the user with a final version of all the independent maps after 33 | * merging. 34 | */ 35 | int main(int argc, char** argv) 36 | { 37 | // init ROS Node 38 | std::string node_name = "map_merger"; 39 | ros::init(argc, argv, node_name); 40 | ros::NodeHandle nh; 41 | ros::Rate loop_rate(10); 42 | 43 | // initialize logger. 44 | COutputLogger logger; 45 | logger.setLoggerName(node_name); 46 | logger.setMinLoggingLevel(LVL_DEBUG); 47 | logger.logFmt(LVL_WARN, "Initialized %s node...\n", node_name.c_str()); 48 | 49 | CMapMerger map_merger(&logger, &nh); 50 | 51 | bool continue_exec = true; 52 | while (ros::ok() && continue_exec) 53 | { 54 | continue_exec = map_merger.updateState(); 55 | ros::spinOnce(); 56 | loop_rate.sleep(); 57 | } 58 | 59 | return 0; 60 | } 61 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/src/mrpt_graphslam_2d_mr_node.cpp: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | // MRPT headers 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | // ROS headers 20 | #include "mrpt_graphslam_2d/CGraphSlamHandler_ROS.h" 21 | 22 | using namespace mrpt; 23 | using namespace mrpt::poses; 24 | using namespace mrpt::obs; 25 | using namespace mrpt::system; 26 | using namespace mrpt::graphs; 27 | using namespace mrpt::math; 28 | using namespace mrpt::opengl; 29 | using namespace mrpt::graphslam; 30 | using namespace mrpt::graphslam::deciders; 31 | using namespace mrpt::graphslam::optimizers; 32 | using namespace mrpt::graphslam::apps; 33 | 34 | using namespace std; 35 | 36 | /** Main function of the mrpt_graphslam condensed-measurements _application */ 37 | int main(int argc, char** argv) 38 | { 39 | COutputLogger logger; 40 | 41 | try 42 | { 43 | std::string node_name = "mrpt_graphslam_2d_mr"; 44 | 45 | ros::init(argc, argv, node_name); 46 | ros::NodeHandle nh; 47 | 48 | node_name = node_name + nh.getNamespace(); 49 | logger.setLoggerName(node_name); 50 | logger.logFmt(LVL_WARN, "Initialized %s node...\n", node_name.c_str()); 51 | 52 | ros::Rate loop_rate(10); 53 | 54 | // Initialization 55 | TUserOptionsChecker_ROS options_checker; 56 | CGraphSlamHandler_ROS graphslam_handler( 57 | &logger, &options_checker, &nh); 58 | graphslam_handler.readParams(); 59 | graphslam_handler.initEngine_MR(); 60 | graphslam_handler.setupComm(); 61 | 62 | std::string ns = nh.getNamespace(); 63 | // overwite default results directory due to the multi-robot nature 64 | graphslam_handler.setResultsDirName( 65 | std::string(ns.begin() + 2, ns.end())); 66 | 67 | // print the parameters just for verification 68 | graphslam_handler.printParams(); 69 | 70 | bool cont_exec = true; 71 | while (ros::ok() && cont_exec) 72 | { 73 | cont_exec = graphslam_handler.usePublishersBroadcasters(); 74 | 75 | ros::spinOnce(); 76 | loop_rate.sleep(); 77 | } 78 | } 79 | catch (exception& e) 80 | { 81 | cout << "Known error!" << endl; 82 | logger.logFmt(LVL_ERROR, "Caught exception: %s", e.what()); 83 | mrpt::system::pause(); 84 | return -1; 85 | } 86 | catch (...) 87 | { 88 | cout << "Unknown error!" << endl; 89 | logger.logFmt(LVL_ERROR, "Finished with unknown exception. Exiting\n."); 90 | mrpt::system::pause(); 91 | return -1; 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /mrpt_graphslam_2d/src/mrpt_graphslam_2d_node.cpp: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ 9 | */ 10 | 11 | // MRPT headers 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | // ROS headers 20 | #include "mrpt_graphslam_2d/CGraphSlamHandler_ROS.h" 21 | 22 | using namespace mrpt; 23 | using namespace mrpt::poses; 24 | using namespace mrpt::obs; 25 | using namespace mrpt::system; 26 | using namespace mrpt::graphs; 27 | using namespace mrpt::math; 28 | using namespace mrpt::opengl; 29 | using namespace mrpt::graphslam; 30 | using namespace mrpt::graphslam::deciders; 31 | using namespace mrpt::graphslam::optimizers; 32 | using namespace mrpt::graphslam::apps; 33 | 34 | using namespace std; 35 | 36 | /** Main function of the mrpt_graphslam application */ 37 | int main(int argc, char** argv) 38 | { 39 | std::string node_name = "mrpt_graphslam_2d"; 40 | 41 | COutputLogger logger; 42 | logger.setLoggerName(node_name); 43 | logger.logFmt(LVL_WARN, "Initializing %s node...\n", node_name.c_str()); 44 | 45 | ros::init(argc, argv, node_name); 46 | ros::NodeHandle nh; 47 | 48 | ros::Rate loop_rate(10); 49 | 50 | try 51 | { 52 | // Initialization 53 | TUserOptionsChecker_ROS options_checker; 54 | CGraphSlamHandler_ROS graphslam_handler( 55 | &logger, &options_checker, &nh); 56 | graphslam_handler.readParams(); 57 | graphslam_handler.initEngine_ROS(); 58 | graphslam_handler.setupComm(); 59 | 60 | // print the parameters just for verification 61 | graphslam_handler.printParams(); 62 | 63 | bool cont_exec = true; 64 | while (ros::ok() && cont_exec) 65 | { 66 | cont_exec = graphslam_handler.usePublishersBroadcasters(); 67 | 68 | ros::spinOnce(); 69 | loop_rate.sleep(); 70 | } 71 | } 72 | catch (exception& e) 73 | { 74 | ROS_ERROR_STREAM( 75 | "Finished with a (known) exception!" << std::endl 76 | << e.what() << std::endl); 77 | mrpt::system::pause(); 78 | return -1; 79 | } 80 | catch (...) 81 | { 82 | ROS_ERROR_STREAM("Finished with a (unknown) exception!" << std::endl); 83 | mrpt::system::pause(); 84 | return -1; 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_icp_slam_2d 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.19 (2024-09-18) 6 | ------------------- 7 | * Fix cmake target creation 8 | * Workaround for missing TBB::tbb target in Noetic 9 | * cmake: fix case and whitespaces 10 | * Contributors: Jose Luis Blanco-Claraco 11 | 12 | 0.1.18 (2024-09-05) 13 | ------------------- 14 | * Port to new 'mrpt_lib*' packages 15 | * Contributors: Jose Luis Blanco-Claraco 16 | 17 | 0.1.17 (2024-06-05) 18 | ------------------- 19 | 20 | 0.1.16 (2024-03-25) 21 | ------------------- 22 | * Fix build against mrpt 2.12.0 23 | * Contributors: Jose Luis Blanco-Claraco 24 | 25 | 0.1.15 (2023-08-02) 26 | ------------------- 27 | * Fix build errors and warnings with latest mrpt2 versions 28 | (Closes: `#71 `_) 29 | * Contributors: Jose Luis Blanco-Claraco 30 | 31 | 0.1.14 (2023-04-12) 32 | ------------------- 33 | * fix mvsim param: should not publish fake loc 34 | * fixes for the new mvsim-based demo 35 | * Contributors: Jose Luis Blanco-Claraco 36 | 37 | 0.1.11 (2022-06-24) 38 | ------------------- 39 | * Ported to tf2 and mrpt::ros1bridge 40 | * Fix build with mrpt2 41 | * Fixed all 2D and 3D EKF wrappers to work with mrpt 2.x API 42 | * Update build dep to mrpt2 43 | * Contributors: Jose Luis Blanco Claraco 44 | 45 | 0.1.10 (2019-10-05) 46 | ------------------- 47 | * fix build against current mrpt2 48 | * Contributors: Jose Luis Blanco Claraco 49 | 50 | 0.1.9 (2019-04-14) 51 | ------------------ 52 | * Fix build againt MRPT 1.9.9 53 | * Switch header guards to pragma once 54 | * Contributors: Jose Luis Blanco-Claraco, Julian Lopez Velasquez, Vladislav Tananaev 55 | 56 | 0.1.8 (2018-09-21) 57 | ------------------ 58 | * Make catkin_lint clean 59 | * Contributors: Jose Luis Blanco Claraco 60 | 61 | 0.1.7 (2018-09-20) 62 | ------------------ 63 | 64 | 0.1.6 (2018-06-14) 65 | ------------------ 66 | * Fixed compilation with MRPT 1.5 67 | * Fixed indents 68 | * Fixed build with mrpt 2.0 69 | * partial fix build w mrpt 2.0 70 | * fix build in mrpt 2.0 71 | * fix build; add optimized builds (-O3) 72 | * fix build against mrpt 1.5 series 73 | * use consistent cmake conventions for c++14 74 | * all but mrpt_graphslam_2d compiling against mrpt2.0 75 | * porting to mrpt2 76 | * fix `#28 `_, compiling all nodes using -std=c++14 77 | * CMake finds MRPT >=1.5 in branch master 78 | * mrpt_icp_slam_2d: comment to adding-variables 79 | * mrpt_icp_slam_2d: show in rviz and adding parameter to adjust rate in launch-files 80 | trajectory_update_rate: default 10 Hz 81 | trajectory_publish_rate: default 5 Hz 82 | [Reference] http://wiki.ros.org/hector_trajectory_server?distro=kinetic 83 | * mrpt_icp_slam_2d: adding trajectory. 84 | [Reference] http://wiki.ros.org/hector_trajectory_server?distro=kinetic 85 | * delete unused variable: curPDF. 86 | * Merge pull request `#20 `_ from Logrus/master 87 | Fixes and cleanups for config files 88 | * [mrpt_icp_slam_2d] Update description and dependencies of package.xml 89 | Add test for *.launch files. 90 | * Move transforms to callbacks. 91 | * Define C++11 avoiding direct manipulation of CXX_FLAGS 92 | * catkin_lint error fixes 93 | * Fix build against latest dev mrpt 1.5.0 94 | * Contributors: Jose Luis Blanco, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Logrus, Magnus Gärtner, Max Kuzmin, Vladislav Tananaev, tyuownu 95 | 96 | 0.1.5 (2016-11-18) 97 | ------------------ 98 | * Fix cmakelists typo 99 | * Contributors: Nikos Koukis 100 | 101 | 0.1.3 (2016-09-27) 102 | ------------------ 103 | 104 | 0.1.2 (2016-09-24) 105 | ------------------ 106 | * Make formatting conform to ROS C++ Style Guide. 107 | * Output logs only in ROS. 108 | * Add a guard for new mrpt::ros1bridge::rosLoggerLvlToMRPTLoggerLvl and mrpt::ros1bridge::mrptToROSLoggerCallback functions for MRPT version less than 1.5.0. 109 | * Add streaming of MRPT logs to ROS logs. 110 | * Contributors: Logrus, Vladislav Tananaev 111 | 112 | 0.1.1 (2016-08-22) 113 | ------------------ 114 | * First public version, as a result of Vladislav Tananaev's GSoC2016 work. 115 | * Contributors: Jose Luis Blanco, Logrus, Raphael Zack 116 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 3.1) 4 | project(mrpt_icp_slam_2d) 5 | # Load catkin and all dependencies required for this package 6 | # TODO: remove all from COMPONENTS that are not catkin packages. 7 | 8 | #find catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roslib 11 | roscpp 12 | tf2 13 | tf2_ros 14 | tf2_geometry_msgs 15 | std_msgs 16 | nav_msgs 17 | sensor_msgs 18 | visualization_msgs 19 | dynamic_reconfigure 20 | roslaunch) 21 | 22 | #find mrpt packages 23 | find_package(mrpt-gui REQUIRED) 24 | find_package(mrpt-slam REQUIRED) 25 | find_package(mrpt-ros1bridge REQUIRED) 26 | 27 | # fix for Noetic: 28 | if(NOT EXISTS TBB::tbb AND TBB_LIBRARY AND TBB_INCLUDE_DIRS) 29 | add_library(MY_TBB INTERFACE) 30 | target_include_directories(MY_TBB INTERFACE ${TBB_INCLUDE_DIRS}) 31 | target_link_libraries(MY_TBB INTERFACE ${TBB_LIBRARY}) 32 | add_library(TBB::tbb ALIAS MY_TBB) 33 | endif() 34 | 35 | if(CMAKE_COMPILER_IS_GNUCXX) 36 | # High level of warnings. 37 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 38 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 39 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 40 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 41 | if(NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 42 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 43 | endif() 44 | endif() 45 | 46 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 47 | add_compile_options(-O3) 48 | endif() 49 | 50 | catkin_package( 51 | CATKIN_DEPENDS 52 | nav_msgs 53 | sensor_msgs 54 | std_msgs 55 | visualization_msgs 56 | tf2 57 | tf2_ros 58 | tf2_geometry_msgs 59 | ) 60 | 61 | ########### 62 | ## Build ## 63 | ########### 64 | 65 | include_directories( 66 | ${catkin_INCLUDE_DIRS} 67 | include 68 | ) 69 | 70 | 71 | 72 | add_executable(mrpt_icp_slam_2d 73 | src/mrpt_icp_slam_2d_wrapper.cpp 74 | src/mrpt_icp_slam_2d_app.cpp 75 | ) 76 | 77 | TARGET_LINK_LIBRARIES(mrpt_icp_slam_2d 78 | mrpt::slam 79 | mrpt::gui 80 | mrpt::ros1bridge 81 | ${catkin_LIBRARIES} 82 | ) 83 | 84 | ############# 85 | ## Install ## 86 | ############# 87 | 88 | # Mark executables and/or libraries for installation 89 | install(TARGETS mrpt_icp_slam_2d 90 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 91 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 93 | ) 94 | 95 | # Mark other files for installation (e.g. launch and bag files, etc.) 96 | install(DIRECTORY 97 | launch 98 | tutorial 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 100 | ) 101 | 102 | ############# 103 | ## Testing ## 104 | ############# 105 | # Add testing for *.launch files 106 | roslaunch_add_file_check(launch) 107 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/config/rosconsole.config: -------------------------------------------------------------------------------- 1 | # Set the default ros output 2 | log4j.logger.ros=WARN 3 | log4j.logger.ros.mrpt_icp_slam_2d=DEBUG -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/launch/icp_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/launch/icp_slam_gui.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/launch/icp_slam_rawlog.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/launch/mvsim_icp_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b mrpt_icp_slam_2d is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_icp_slam_2d 4 | 0.1.19 5 | mrpt_icp_slam_2d contains a wrapper on MRPT's 2D ICP-SLAM algorithms. 6 | Jose Luis Blanco-Claraco 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/mrpt_icp_slam_2d 11 | https://github.com/mrpt-ros-pkg/mrpt_slam/issues 12 | 13 | Jose Luis Blanco Claraco 14 | Vladislav Tananaev 15 | 16 | catkin 17 | 18 | roslaunch 19 | roscpp 20 | roslib 21 | tf2 22 | tf2_ros 23 | tf2_geometry_msgs 24 | std_msgs 25 | sensor_msgs 26 | visualization_msgs 27 | nav_msgs 28 | dynamic_reconfigure 29 | mrpt_libros_bridge 30 | mrpt_libgui 31 | mrpt_libslam 32 | 33 | rviz 34 | mrpt_rawlog 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/src/mrpt_icp_slam_2d_app.cpp: -------------------------------------------------------------------------------- 1 | #include "mrpt_icp_slam_2d/mrpt_icp_slam_2d_wrapper.h" 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "mrpt_icp_slam_2d"); 6 | ros::NodeHandle n; 7 | ros::Rate r(100); 8 | ICPslamWrapper slam; 9 | slam.get_param(); 10 | slam.init(); 11 | 12 | ROS_INFO_STREAM("About to enter the main spin loop."); 13 | 14 | // if (!slam.rawlogPlay()) 15 | { // if not play from rawlog file 16 | ros::spin(); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/tutorial/icp_slam_demo.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------ 2 | # Config file for the "ICP-SLAM" application 3 | # See: http://www.mrpt.org/list-of-mrpt-apps/application-icp-slam/ 4 | #------------------------------------------------------------ 5 | 6 | 7 | 8 | #======================================================= 9 | # Section: [ICP] 10 | # Parameters of ICP inside the ICP-based SLAM class 11 | #======================================================= 12 | [ICP] 13 | maxIterations = 80 // The maximum number of iterations to execute if convergence is not achieved before 14 | minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: 15 | minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: 16 | 17 | thresholdDist = 0.3 // Initial maximum distance for matching a pair of points 18 | thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. 19 | 20 | ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) 21 | 22 | smallestThresholdDist=0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. 23 | onlyClosestCorrespondences=true // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) 24 | onlyUniqueRobust = false // Force unique correspondences in both directions when pairing two point clouds 25 | 26 | # 0: icpClassic 27 | # 1: icpLevenbergMarquardt 28 | ICP_algorithm = icpClassic 29 | 30 | # decimation to apply to the point cloud being registered against the map 31 | # Reduce to "1" to obtain the best accuracy 32 | corresponding_points_decimation = 5 33 | 34 | 35 | #======================================================= 36 | # Section: [MappingApplication] 37 | # Use: Here comes global parameters for the app. 38 | #======================================================= 39 | [MappingApplication] 40 | # The source file (RAW-LOG) with action/observation pairs 41 | rawlog_file= //not used 42 | rawlog_offset=0 //not used 43 | 44 | # The directory where the log files will be saved (left in blank if no log is required) 45 | logOutput_dir=LOG_ICP-SLAM 46 | LOG_FREQUENCY=50 //not used // The frequency of log files generation: 47 | SAVE_3D_SCENE=1 //not used 48 | SAVE_POSE_LOG=0 //not used 49 | CAMERA_3DSCENE_FOLLOWS_ROBOT=1 50 | SHOW_PROGRESS_3D_REAL_TIME=1 //activate mrpt window 51 | 52 | SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS=5 //not used 53 | SHOW_LASER_SCANS_3D = true 54 | 55 | localizationLinDistance = 0.2 // The distance threshold for correcting odometry with ICP (meters) 56 | localizationAngDistance = 5 // The distance threshold for correcting odometry with ICP (degrees) 57 | 58 | insertionLinDistance = 1.2 // The distance threshold for inserting observations in the map (meters) 59 | insertionAngDistance = 45.0 // The distance threshold for inserting observations in the map (degrees) 60 | 61 | minICPgoodnessToAccept = 0.40 // Minimum ICP quality to accept correction [0,1]. 62 | 63 | # Neeeded for LM method, which only supports point-map to point-map matching. 64 | matchAgainstTheGrid = 0 65 | 66 | # ======================================================== 67 | # MULTIMETRIC MAP CONFIGURATION 68 | # See docs for (Google for) mrpt::maps::CMultiMetricMap 69 | # ======================================================== 70 | # Creation of maps: 71 | occupancyGrid_count=1 72 | gasGrid_count=0 //not used 73 | landmarksMap_count=0 //not used 74 | beaconMap_count=0 //not used 75 | pointsMap_count=1 76 | 77 | # Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3) 78 | likelihoodMapSelection=-1 79 | 80 | # ==================================================== 81 | # MULTIMETRIC MAP: PointsMap #00 82 | # ==================================================== 83 | # Creation Options for PointsMap 00: 84 | [MappingApplication_pointsMap_00_insertOpts] 85 | minDistBetweenLaserPoints = 0.05 86 | fuseWithExisting = false 87 | isPlanarMap = false 88 | 89 | 90 | 91 | 92 | # ==================================================== 93 | # MULTIMETRIC MAP: OccGrid #00 94 | # ==================================================== 95 | # Creation Options for OccupancyGridMap 00: 96 | [MappingApplication_occupancyGrid_00_creationOpts] 97 | resolution=0.10 98 | disableSaveAs3DObject=0 99 | 100 | 101 | # Insertion Options for OccupancyGridMap 00: 102 | [MappingApplication_occupancyGrid_00_insertOpts] 103 | mapAltitude=0 104 | useMapAltitude=0 105 | maxDistanceInsertion=30 106 | maxOccupancyUpdateCertainty=0.54 107 | considerInvalidRangesAsFreeSpace=1 108 | minLaserScanNoiseStd=0.001 109 | 110 | wideningBeamsWithDistance = 1 111 | 112 | 113 | # Likelihood Options for OccupancyGridMap 00: 114 | [MappingApplication_occupancyGrid_00_likelihoodOpts] 115 | likelihoodMethod=4 // 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II 116 | LF_decimation=4 117 | LF_stdHit=0.05 118 | LF_maxCorrsDistance=0.50 119 | LF_zHit=0.999 120 | LF_zRandom=0.001 121 | LF_maxRange=30 122 | LF_alternateAverageMethod=0 123 | 124 | enableLikelihoodCache=1 125 | 126 | -------------------------------------------------------------------------------- /mrpt_icp_slam_2d/tutorial/icp_slam_demo.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_icp_slam_2d/tutorial/icp_slam_demo.rawlog -------------------------------------------------------------------------------- /mrpt_rbpf_slam/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_rbpf_slam 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.19 (2024-09-18) 6 | ------------------- 7 | * Fix cmake target creation 8 | * Workaround for missing TBB::tbb target in Noetic 9 | * cmake: fix case and whitespaces 10 | * Contributors: Jose Luis Blanco-Claraco 11 | 12 | 0.1.18 (2024-09-05) 13 | ------------------- 14 | * Port to new 'mrpt_lib*' packages 15 | * Contributors: Jose Luis Blanco-Claraco 16 | 17 | 0.1.17 (2024-06-05) 18 | ------------------- 19 | 20 | 0.1.16 (2024-03-25) 21 | ------------------- 22 | * Fix build against mrpt 2.12.0 23 | * Contributors: Jose Luis Blanco-Claraco 24 | 25 | 0.1.15 (2023-08-02) 26 | ------------------- 27 | 28 | 0.1.14 (2023-04-12) 29 | ------------------- 30 | * Fix build against last mrpt 2.7.x 31 | * fix mvsim param: should not publish fake loc 32 | * fixes for the new mvsim-based demo 33 | * Contributors: Jose Luis Blanco-Claraco 34 | 35 | 0.1.11 (2022-06-24) 36 | ------------------- 37 | * Ported to tf2 and mrpt::ros1bridge 38 | * Coarser grid resolution 39 | * fix build with mrpt2 40 | * Fixed all 2D and 3D EKF wrappers to work with mrpt 2.x API 41 | * Update URLs to https 42 | * Update build dep to mrpt2 43 | * Merge pull request `#62 `_ from mx-robotics/master 44 | Flag added to switch between a static and moving sensor 45 | * Flag added to switch between a static and moving sensor 46 | * Contributors: Jose Luis Blanco-Claraco, Markus Bader 47 | 48 | 0.1.10 (2019-10-05) 49 | ------------------- 50 | * fix build against mrpt2 51 | * Contributors: Jose Luis Blanco-Claraco 52 | 53 | 0.1.9 (2019-04-14) 54 | ------------------ 55 | * fix build against mrpt-1.5 & 1.9.9 56 | * Feature/refactor mrpt rbpf slam 57 | * Add option to specify simplemap save path 58 | * Add some non zero default noise values for thrun's motion model 59 | * Small documentation fixes 60 | * Rename example file 61 | * Switch header guards to pragma once 62 | * Add launch file for turtlebot3 63 | * Switch default log level to INFO 64 | * Contributors: Jose Luis Blanco-Claraco, Julian Lopez Velasquez, Vladislav Tananaev 65 | 66 | 0.1.8 (2018-09-21) 67 | ------------------ 68 | * Make catkin_lint clean 69 | * Contributors: Jose Luis Blanco Claraco 70 | 71 | 0.1.7 (2018-09-20) 72 | ------------------ 73 | 74 | 0.1.6 (2018-06-14) 75 | ------------------ 76 | * Merge pull request `#42 `_ from MaxGsomGsom/master 77 | Fixed build with MRPT 1.5 78 | * Fixed compilation with MRPT 1.5 79 | * Merge pull request `#41 `_ from MaxGsomGsom/master 80 | Fixed build with mrpt 2.0 81 | * Fixed build with mrpt 2.0 82 | * partial fix build w mrpt 2.0 83 | * fix build in mrpt 2.0 84 | * fix build; add optimized builds (-O3) 85 | * fix build against mrpt 1.5 series 86 | * Fixed wrong timeout for waitForTransform. 87 | * Fixed wrong timeout for waitForTransform. 88 | * use consistent cmake conventions for c++14 89 | * all but mrpt_graphslam_2d compiling against mrpt2.0 90 | * porting to mrpt2 91 | * fix `#28 `_, compiling all nodes using -std=c++14 92 | * CMake finds MRPT >=1.5 in branch master 93 | * Merge pull request `#22 `_ from bergercookie/graphslam-devel 94 | Add support for 2D multi-robot SLAM 95 | * mrpt_rbpf:Correct include guard 96 | * fix build with MRPT 1.5.0 97 | * Merge pull request `#20 `_ from Logrus/master 98 | Fixes and cleanups for config files 99 | * [mrpt_rbpf_slam] Update description and dependencies of package.xml 100 | Add test for *.launch files. 101 | * Move transforms to callbacks. 102 | * Define C++11 avoiding direct manipulation of CXX_FLAGS 103 | * catkin_lint error fixes 104 | * Fix build against latest dev mrpt 1.5.0 105 | * Contributors: Albert Kasdorf, Jose Luis Blanco, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Logrus, Magnus Gärtner, Max Kuzmin, Nikos Koukis, Vladislav Tananaev 106 | 107 | 0.1.5 (2016-11-18) 108 | ------------------ 109 | * Include ros/console hdr 110 | * Fix cmakelists typo 111 | * Contributors: Nikos Koukis 112 | 113 | 0.1.3 (2016-09-27) 114 | ------------------ 115 | 116 | 0.1.2 (2016-09-24) 117 | ------------------ 118 | * Make formatting conform to ROS C++ Style Guide. 119 | * Fix missing CObservationBeaconRanges.h include in mrpt_rbpf_slam. 120 | * Fix description of the rbpf package. 121 | * Output logs only in ROS. 122 | * Add a guard for new mrpt::ros1bridge::rosLoggerLvlToMRPTLoggerLvl and mrpt::ros1bridge::mrptToROSLoggerCallback functions for MRPT version less than 1.5.0. 123 | * Add streaming of MRPT logs to ROS logs. 124 | * Update example config files with new localizeLinDistance and localizeAngDistance_deg. 125 | * Fix build against MRPT<1.3.0 126 | * Contributors: Jose Luis Blanco, Logrus, Vladislav Tananaev 127 | 128 | 0.1.1 (2016-08-22) 129 | ------------------ 130 | * First public version, as a result of Vladislav Tananaev's GSoC2016 work. 131 | * Contributors: Jose Luis Blanco, Logrus 132 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(mrpt_rbpf_slam) 3 | 4 | #find catkin packages 5 | find_package(catkin REQUIRED COMPONENTS 6 | roslib 7 | roscpp 8 | tf2 9 | tf2_ros 10 | tf2_geometry_msgs 11 | std_msgs 12 | nav_msgs 13 | sensor_msgs 14 | visualization_msgs 15 | dynamic_reconfigure 16 | roslaunch 17 | mrpt_msgs_bridge 18 | ) 19 | 20 | #find mrpt packages 21 | find_package(mrpt-gui REQUIRED) 22 | find_package(mrpt-slam REQUIRED) 23 | find_package(mrpt-ros1bridge REQUIRED) 24 | 25 | # fix for Noetic: 26 | if(NOT EXISTS TBB::tbb AND TBB_LIBRARY AND TBB_INCLUDE_DIRS) 27 | add_library(MY_TBB INTERFACE) 28 | target_include_directories(MY_TBB INTERFACE ${TBB_INCLUDE_DIRS}) 29 | target_link_libraries(MY_TBB INTERFACE ${TBB_LIBRARY}) 30 | add_library(TBB::tbb ALIAS MY_TBB) 31 | endif() 32 | 33 | if(CMAKE_COMPILER_IS_GNUCXX) 34 | # High level of warnings. 35 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 36 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 37 | add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) 38 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 39 | if(NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 40 | add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) 41 | endif() 42 | endif() 43 | 44 | if(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 45 | add_compile_options(-O3) 46 | endif() 47 | 48 | catkin_package( 49 | CATKIN_DEPENDS 50 | nav_msgs 51 | sensor_msgs 52 | std_msgs 53 | visualization_msgs 54 | tf2 55 | tf2_ros 56 | tf2_geometry_msgs 57 | mrpt_msgs_bridge 58 | ) 59 | 60 | ########### 61 | ## Build ## 62 | ########### 63 | 64 | include_directories( 65 | ${catkin_INCLUDE_DIRS} 66 | include 67 | ) 68 | 69 | 70 | 71 | add_executable(mrpt_rbpf_slam 72 | src/mrpt_rbpf_slam.cpp 73 | src/mrpt_rbpf_slam_wrapper.cpp 74 | src/mrpt_rbpf_slam_node.cpp 75 | src/options.cpp 76 | ) 77 | 78 | TARGET_LINK_LIBRARIES(mrpt_rbpf_slam 79 | mrpt::slam 80 | mrpt::gui 81 | mrpt::ros1bridge 82 | ${catkin_LIBRARIES} 83 | ) 84 | 85 | ############# 86 | ## Install ## 87 | ############# 88 | 89 | # Mark executables and/or libraries for installation 90 | install(TARGETS mrpt_rbpf_slam 91 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 94 | ) 95 | 96 | # Mark other files for installation (e.g. launch and bag files, etc.) 97 | install(DIRECTORY 98 | config 99 | launch 100 | tutorial 101 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 102 | ) 103 | 104 | ############# 105 | ## Testing ## 106 | ############# 107 | # Add testing for *.launch files 108 | roslaunch_add_file_check(launch) 109 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/config/default.yaml: -------------------------------------------------------------------------------- 1 | # Ros loop update frequency 2 | update_loop_frequency: 100 # Hz (or 100 updates per sec) 3 | 4 | # Absolute path to the folder where to save *.simplemap 5 | # Note: everything in '/tmp' folder is removed if computer is restarted 6 | simplemap_save_folder: /tmp/ 7 | 8 | # More documentation: 9 | # http://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/ 10 | motion_model: 11 | type: "thrun" # choose thrun or gaussian 12 | gaussian_motion_model_options: 13 | a1: 0.034 14 | a2: 0.057 15 | a3: 0.014 16 | a4: 0.097 17 | minStdXY: 0.005 18 | minStdPHI: 0.05 19 | thrun_motion_model_options: 20 | particle_count: 100 21 | alfa1_rot_rot: 0.1 22 | alfa2_rot_trans: 0.1 23 | alfa3_trans_trans: 0.1 24 | alfa4_trans_rot: 0.1 25 | additional_std_XY: 0.1 # m 26 | additional_std_phi: 0.1 # rad 27 | 28 | mrpt_visualization_options: 29 | show_window: true 30 | camera_follow_robot: false 31 | window_update_delay: 1 # [ms] 32 | width: 600 # [pix] 33 | height: 500 # [pix] 34 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/config/rosconsole.config: -------------------------------------------------------------------------------- 1 | # Set the default ros output 2 | log4j.logger.ros=WARN 3 | log4j.logger.ros.mrpt_rbpf_slam=DEBUG 4 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: mrpt_rbpf_slam.h 3 | * Author: Vladislav Tananaev 4 | */ 5 | 6 | #pragma once 7 | #include 8 | 9 | #include 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 | #include 32 | #include 33 | 34 | namespace mrpt_rbpf_slam 35 | { 36 | /** 37 | * @brief The PFslam class provides Rao-Blackwellized Particle filter SLAM from 38 | * MRPT libraries. 39 | */ 40 | class PFslam 41 | { 42 | public: 43 | struct Options 44 | { 45 | mrpt::obs::CActionRobotMovement2D::TMotionModelOptions 46 | motion_model_options_; ///< used with odom value motion 47 | ///< noise 48 | mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions 49 | rbpfMappingOptions_; ///< options for SLAM from ini file 50 | bool CAMERA_3DSCENE_FOLLOWS_ROBOT_; 51 | bool SHOW_PROGRESS_IN_WINDOW_; 52 | int SHOW_PROGRESS_IN_WINDOW_DELAY_MS_; 53 | int PROGRESS_WINDOW_WIDTH_, PROGRESS_WINDOW_HEIGHT_; 54 | std::string simplemap_path_prefix; 55 | } options_; 56 | 57 | PFslam() = default; 58 | virtual ~PFslam(); 59 | 60 | void init3Dwindow(); 61 | 62 | void run3Dwindow(); 63 | 64 | /** 65 | * @brief Read ini file 66 | * 67 | * @param[in] ini_filename the name of the ini file to read 68 | */ 69 | void readIniFile(const std::string& ini_filename); 70 | 71 | /** 72 | * @brief initialize the SLAM 73 | */ 74 | void initSlam(Options options); 75 | 76 | /** 77 | * @brief Read pairs of actions and observations from rawlog file 78 | * 79 | * @param[in] rawlog_filename the name of rawlog file to read 80 | * @param[out] data vector of pairs of actions and observations 81 | */ 82 | void readRawlog( 83 | const std::string& rawlog_filename, 84 | std::vector< 85 | std::pair>& 86 | data); 87 | 88 | /** 89 | * @brief Calculate the actions from odometry model for current observation 90 | * 91 | * @param[in] sensory_frame current observation 92 | * @param[in] odometry raw odometry 93 | */ 94 | void observation( 95 | const mrpt::obs::CSensoryFrame::ConstPtr sensory_frame, 96 | const mrpt::obs::CObservationOdometry::ConstPtr odometry); 97 | 98 | protected: 99 | mrpt::slam::CMetricMapBuilderRBPF mapBuilder_; ///< map builder 100 | mrpt::obs::CActionCollection::Ptr action_; ///< actions 101 | mrpt::obs::CSensoryFrame::Ptr sensory_frame_; ///< observations 102 | 103 | mrpt::poses::CPose2D 104 | odomLastObservation_; ///< last observation of odometry 105 | bool use_motion_model_default_options_; ///< used default odom_params 106 | mrpt::system::TTimeStamp 107 | timeLastUpdate_; ///< last update of the pose and map 108 | 109 | const mrpt::maps::CMultiMetricMap* 110 | metric_map_; ///< receive map after iteration of SLAM to metric map 111 | mrpt::poses::CPose3DPDFParticles curPDF; ///< current robot pose 112 | 113 | mrpt::gui::CDisplayWindow3D::Ptr win3D_; ///< MRPT window 114 | }; 115 | } // namespace mrpt_rbpf_slam 116 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/include/mrpt_rbpf_slam/options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: options.h 3 | * Author: Vladislav Tananaev 4 | */ 5 | #pragma once 6 | #include 7 | #include 8 | 9 | namespace mrpt_rbpf_slam{ 10 | bool loadOptions(const ros::NodeHandle& nh, PFslam::Options& options); 11 | } 12 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/mvsim_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/rbpf_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/rbpf_slam_rawlog.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/rbpf_slam_turtlebot3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/ro_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/ro_slam_rawlog.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_rbpf_slam 4 | 0.1.19 5 | This package is used for gridmap SLAM. The interface is similar to gmapping (https://wiki.ros.org/gmapping) but the package supports different particle-filter algorithms, range-only SLAM, can work with several grid maps simultaneously and more. 6 | 7 | http://www.mrpt.org/ 8 | Vladislav Tananaev 9 | Jose Luis Blanco-Claraco 10 | 11 | Vladislav Tananaev 12 | BSD 13 | 14 | http://mrpt.org/ 15 | 16 | catkin 17 | 18 | dynamic_reconfigure 19 | mrpt_libros_bridge 20 | mrpt_libgui 21 | mrpt_libslam 22 | nav_msgs 23 | roscpp 24 | roslaunch 25 | roslib 26 | sensor_msgs 27 | std_msgs 28 | tf2 29 | tf2_ros 30 | tf2_geometry_msgs 31 | visualization_msgs 32 | mrpt_msgs_bridge 33 | 34 | mrpt_rawlog 35 | mvsim 36 | rviz 37 | 38 | 39 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/rviz/rviz_conf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PoseArray1 10 | - /PoseArray2 11 | - /TF1 12 | - /TF1/Frames1 13 | Splitter Ratio: 0.5 14 | Tree Height: 330 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.03 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.7 56 | Class: rviz/Map 57 | Color Scheme: map 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /map 62 | Value: true 63 | - Arrow Length: 0.3 64 | Class: rviz/PoseArray 65 | Color: 255; 25; 0 66 | Enabled: true 67 | Name: PoseArray 68 | Topic: /particlecloud 69 | Value: true 70 | - Arrow Length: 0.3 71 | Class: rviz/PoseArray 72 | Color: 0; 255; 0 73 | Enabled: true 74 | Name: PoseArray 75 | Topic: /particlecloud_beacons 76 | Value: true 77 | - Class: rviz/TF 78 | Enabled: true 79 | Frame Timeout: 15 80 | Frames: 81 | All Enabled: true 82 | base_link: 83 | Value: true 84 | beacon_link: 85 | Value: true 86 | map: 87 | Value: true 88 | odom: 89 | Value: true 90 | Marker Scale: 1 91 | Name: TF 92 | Show Arrows: true 93 | Show Axes: true 94 | Show Names: true 95 | Tree: 96 | map: 97 | odom: 98 | base_link: 99 | beacon_link: 100 | {} 101 | Update Interval: 0 102 | Value: true 103 | - Class: rviz/MarkerArray 104 | Enabled: true 105 | Marker Topic: /beacons_viz 106 | Name: MarkerArray 107 | Namespaces: 108 | {} 109 | Queue Size: 100 110 | Value: true 111 | Enabled: true 112 | Global Options: 113 | Background Color: 48; 48; 48 114 | Fixed Frame: map 115 | Frame Rate: 30 116 | Name: root 117 | Tools: 118 | - Class: rviz/Interact 119 | Hide Inactive Objects: true 120 | - Class: rviz/MoveCamera 121 | - Class: rviz/Select 122 | - Class: rviz/FocusCamera 123 | - Class: rviz/Measure 124 | - Class: rviz/SetInitialPose 125 | Topic: /initialpose 126 | - Class: rviz/SetGoal 127 | Topic: /move_base_simple/goal 128 | - Class: rviz/PublishPoint 129 | Single click: true 130 | Topic: /clicked_point 131 | Value: true 132 | Views: 133 | Current: 134 | Class: rviz/Orbit 135 | Distance: 35.884 136 | Enable Stereo Rendering: 137 | Stereo Eye Separation: 0.06 138 | Stereo Focal Distance: 1 139 | Swap Stereo Eyes: false 140 | Value: false 141 | Focal Point: 142 | X: 0 143 | Y: 0 144 | Z: 0 145 | Name: Current View 146 | Near Clip Distance: 0.01 147 | Pitch: 0.700398 148 | Target Frame: 149 | Value: Orbit (rviz) 150 | Yaw: 4.6154 151 | Saved: ~ 152 | Window Geometry: 153 | Displays: 154 | collapsed: false 155 | Height: 611 156 | Hide Left Dock: false 157 | Hide Right Dock: false 158 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000001d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d9000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001d9000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000001d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 159 | Selection: 160 | collapsed: false 161 | Time: 162 | collapsed: false 163 | Tool Properties: 164 | collapsed: false 165 | Views: 166 | collapsed: false 167 | Width: 1200 168 | X: 55 169 | Y: 14 170 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/src/mrpt_rbpf_slam_node.cpp: -------------------------------------------------------------------------------- 1 | #include "mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h" 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "mrpt_rpbf_slam"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_p("~"); 8 | 9 | // Setup ros loop frequency from params 10 | double frequency; 11 | nh_p.param("update_loop_frequency", frequency, 100.); 12 | ros::Rate rate(frequency); 13 | 14 | mrpt_rbpf_slam::PFslamWrapper slam; 15 | // Read parameters and configure node 16 | // and setup callbacks 17 | if (!slam.getParams(nh_p) || !slam.init(nh)) 18 | { 19 | return EXIT_FAILURE; 20 | } 21 | 22 | ros::Duration(1).sleep(); 23 | 24 | // If play from rawlog file options is specified 25 | // play and then terminate application 26 | if (slam.rawlogPlay()) 27 | { 28 | return EXIT_SUCCESS; 29 | } 30 | 31 | // Otherwise work as a usual rosnode 32 | while (ros::ok()) 33 | { 34 | ros::spinOnce(); 35 | rate.sleep(); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/src/options.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: options.cpp 3 | * Author: Vladislav Tananaev 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | namespace mrpt_rbpf_slam 10 | { 11 | namespace 12 | { 13 | using namespace mrpt::obs; 14 | 15 | bool loadThrunModelParameters(const ros::NodeHandle& nh, 16 | CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel& thrunModel) 17 | { 18 | bool success = true; 19 | ros::NodeHandle sub_nh(nh, "thrun_motion_model_options"); 20 | auto getParam = [&success, &sub_nh](const std::string param, auto& val) { 21 | success = success && sub_nh.getParam(param, val); 22 | }; 23 | 24 | thrunModel.nParticlesCount = sub_nh.param("particle_count", 100); 25 | getParam("alfa1_rot_rot", thrunModel.alfa1_rot_rot); 26 | getParam("alfa2_rot_trans", thrunModel.alfa2_rot_trans); 27 | getParam("alfa3_trans_trans", thrunModel.alfa3_trans_trans); 28 | getParam("alfa4_trans_rot", thrunModel.alfa4_trans_rot); 29 | getParam("additional_std_XY", thrunModel.additional_std_XY); 30 | getParam("additional_std_phi", thrunModel.additional_std_phi); 31 | return success; 32 | } 33 | 34 | bool loadGaussianModelParameters(const ros::NodeHandle& nh, 35 | CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel& gaussian_model) 36 | { 37 | bool success = true; 38 | ros::NodeHandle sub_nh(nh, "gaussian_motion_model_options"); 39 | auto getParam = [&success, &sub_nh](const std::string param, auto& val) { 40 | success = success && sub_nh.getParam(param, val); 41 | }; 42 | 43 | getParam("a1", gaussian_model.a1); 44 | getParam("a2", gaussian_model.a2); 45 | getParam("a3", gaussian_model.a3); 46 | getParam("a4", gaussian_model.a4); 47 | getParam("minStdXY", gaussian_model.minStdXY); 48 | getParam("minStdPHI", gaussian_model.minStdPHI); 49 | return success; 50 | } 51 | 52 | bool loadMotionModelParameters(const ros::NodeHandle& nh, 53 | CActionRobotMovement2D::TMotionModelOptions& motion_model_options) 54 | { 55 | static const std::map motion_models = { 56 | { "thrun", CActionRobotMovement2D::mmThrun }, { "gaussian", CActionRobotMovement2D::mmGaussian } 57 | }; 58 | 59 | bool success = true; 60 | ros::NodeHandle sub_nh(nh, "motion_model"); 61 | auto getParam = [&success, &sub_nh](const std::string param, auto& val) { 62 | success = success && sub_nh.getParam(param, val); 63 | }; 64 | 65 | std::string model_type; 66 | getParam("type", model_type); 67 | if (!motion_models.count(model_type)) 68 | { 69 | ROS_ERROR_STREAM("Specified motion model " << model_type << " is not supported."); 70 | return false; 71 | } 72 | motion_model_options.modelSelection = motion_models.at(model_type); 73 | success = success && loadThrunModelParameters(sub_nh, motion_model_options.thrunModel); 74 | success = success && loadGaussianModelParameters(sub_nh, motion_model_options.gaussianModel); 75 | return success; 76 | } 77 | 78 | bool loadVisualizationOptions(const ros::NodeHandle& nh, PFslam::Options& options) 79 | { 80 | bool success = true; 81 | ros::NodeHandle sub_nh(nh, "mrpt_visualization_options"); 82 | success = success && sub_nh.getParam("width", options.PROGRESS_WINDOW_WIDTH_); 83 | success = success && sub_nh.getParam("height", options.PROGRESS_WINDOW_HEIGHT_); 84 | success = success && sub_nh.getParam("window_update_delay", options.SHOW_PROGRESS_IN_WINDOW_DELAY_MS_); 85 | success = success && sub_nh.getParam("show_window", options.SHOW_PROGRESS_IN_WINDOW_); 86 | success = success && sub_nh.getParam("camera_follow_robot", options.CAMERA_3DSCENE_FOLLOWS_ROBOT_); 87 | return success; 88 | } 89 | } // namespace 90 | 91 | bool loadOptions(const ros::NodeHandle& nh, PFslam::Options& options) 92 | { 93 | bool success = true; 94 | success = success && loadMotionModelParameters(nh, options.motion_model_options_); 95 | success = success && loadVisualizationOptions(nh, options); 96 | success = success && nh.getParam("simplemap_save_folder", options.simplemap_path_prefix); 97 | return success; 98 | } 99 | 100 | } // namespace mrpt_rbpf_slam 101 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/tutorial/RO-SLAM_demo.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------ 2 | # Config file for the "RBPF-SLAM" application 3 | # See: http://www.mrpt.org/list-of-mrpt-apps/application-rbpf-slam/ 4 | #------------------------------------------------------------ 5 | 6 | 7 | #======================================================= 8 | # Section: [MappingApplication] 9 | # Use: Here comes global parameters for the app. 10 | #======================================================= 11 | [MappingApplication] 12 | CAMERA_3DSCENE_FOLLOWS_ROBOT=0 13 | 14 | SHOW_PROGRESS_IN_WINDOW=1 15 | PROGRESS_WINDOW_WIDTH=1000 16 | PROGRESS_WINDOW_HEIGHT=700 17 | 18 | insertionLinDistance=0.0 // The distance threshold for inserting observations in the map (meters) 19 | insertionAngDistance_deg=0.0 // The distance threshold for inserting observations in the map (degrees) 20 | 21 | localizeLinDistance = 0.1 // The distance threshold for actually running the PF, otherwise will rely on odometry (meters) 22 | localizeAngDistance_deg = 10.0 // The rotational threshold for actually running the PF, otherwise will rely on odometry (degrees) 23 | 24 | #---------------------------------------------------------------------------------- 25 | # The Particle Filter algorithm: 26 | # 0: pfStandardProposal 27 | # 1: pfAuxiliaryPFStandard 28 | # 2: pfOptimalProposal *** (ICP,...) 29 | # 3: pfAuxiliaryPFOptimal *** (Optimal Aux) 30 | # 31 | # See: /docs/html/topic_pf.html or http://mrpt.sourceforge.net/topic_pf.html 32 | #---------------------------------------------------------------------------------- 33 | PF_algorithm=0 34 | adaptiveSampleSize=0 // 0: Fixed # of particles, 1: KLD adaptive 35 | 36 | #---------------------------------------------------------------------------------- 37 | # The Particle Filter Resampling method: 38 | # 0: prMultinomial 39 | # 1: prResidual 40 | # 2: prStratified 41 | # 3: prSystematic 42 | # 43 | # See: /docs/html/topic_resampling.html or http://mrpt.sourceforge.net/topic_resampling.html 44 | #---------------------------------------------------------------------------------- 45 | resamplingMethod=3 46 | 47 | sampleSize=10 // Sample size (for fixed number) 48 | BETA=0.50 // Resampling ESS threshold 49 | 50 | 51 | # ======================================================== 52 | # MULTIMETRIC MAP CONFIGURATION 53 | # See docs for (Google for) mrpt::maps::CMultiMetricMap 54 | # ======================================================== 55 | # Creation of maps: 56 | occupancyGrid_count=0 57 | gasGrid_count=0 58 | landmarksMap_count=0 59 | pointsMap_count=0 60 | beaconMap_count=1 61 | 62 | # Selection of map for likelihood: (FuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3,beaconMap=4) 63 | likelihoodMapSelection=-1 64 | 65 | # Enables (1) / Disables (0) insertion into specific maps: 66 | enableInsertion_pointsMap=1 67 | enableInsertion_landmarksMap=1 68 | enableInsertion_gridMaps=1 69 | enableInsertion_gasGridMaps=1 70 | enableInsertion_beaconMap=1 71 | 72 | # ==================================================== 73 | # CHybridMetricMapPDF::TPredictionParams 74 | # ==================================================== 75 | powFactor=1 // A "power factor" for updating weights 76 | pfAuxFilterOptimal_MaximumSearchSamples=100 // For PF algorithm=3 77 | 78 | # ----------------------------------------------------------------- 79 | # pfOptimalProposal_mapSelection 80 | # Only for PF algorithm=2 (Exact "pfOptimalProposal") 81 | # Select the map on which to calculate the optimal 82 | # proposal distribution. Values: 83 | # 0: Gridmap -> Uses Scan matching-based approximation (based on Stachniss' work) 84 | # 1: Landmarks -> Uses matching to approximate optimal 85 | # 2: Beacons -> Used for exact optimal proposal in RO-SLAM 86 | # ----------------------------------------------------------------- 87 | pfOptimalProposal_mapSelection=2 88 | 89 | 90 | # Adaptive sample size parameters ------------------ 91 | KLD_maxSampleSize=6 92 | KLD_minSampleSize=5 93 | KLD_binSize_XY=10.00 94 | KLD_binSize_PHI_deg=30 95 | KLD_delta=0.01 96 | KLD_epsilon=0.40 97 | 98 | # ==================================================== 99 | # MULTIMETRIC MAP: BeaconMap #00 100 | # ==================================================== 101 | # Creation Options for BeaconMap 00: 102 | [MappingApplication_beaconMap_00_insertOpts] 103 | insertAsMonteCarlo=0 // 0:Insert as SOG, 1: Insert as MC, after convergence pass to Gaussian 104 | maxElevation_deg=0 // [-90,90] 105 | minElevation_deg=0 // [-90,90] 106 | MC_numSamplesPerMeter=1000 // Number of particles per meter of range, i.e. per meter of the "radius of the ring". 107 | MC_maxStdToGauss=0.07 // Threshold for colapsing samples into a Gaussian 108 | MC_thresholdNegligible=30 // For deleting samples 109 | SOG_thresholdNegligible=40 // For deleting SOG modes 110 | SOG_maxDistBetweenGaussians=0.60 // For initializing the SOG modes 111 | SOG_separationConstant=2 // A constant to compute the std. dev. in the tangent direction 112 | 113 | # Likelihood Options for BeaconMap 00: 114 | [MappingApplication_beaconMap_00_likelihoodOpts] 115 | rangeStd=0.02 116 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/tutorial/RO-SLAM_demo.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_rbpf_slam/tutorial/RO-SLAM_demo.rawlog -------------------------------------------------------------------------------- /mrpt_rbpf_slam/tutorial/grid_slam_demo.ini: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------ 2 | # Config file for the "RBPF-SLAM" application 3 | # See: http://www.mrpt.org/list-of-mrpt-apps/application-rbpf-slam/ 4 | #------------------------------------------------------------ 5 | 6 | 7 | #======================================================= 8 | # Section: [MappingApplication] 9 | # Use: Here comes global parameters for the app. 10 | #======================================================= 11 | [MappingApplication] 12 | CAMERA_3DSCENE_FOLLOWS_ROBOT=0 13 | SHOW_PROGRESS_IN_WINDOW=1 14 | 15 | insertionLinDistance=0.5 // The distance threshold for inserting observations in the map (meters) 16 | insertionAngDistance_deg=25.0 // The distance threshold for inserting observations in the map (degrees) 17 | 18 | localizeLinDistance = 1.20 // The distance threshold for updating the RBPF 19 | localizeAngDistance_deg = 35.0 // The distance threshold for inserting observations in the map (degrees) 20 | 21 | #---------------------------------------------------------------------------------- 22 | # The Particle Filter algorithm: 23 | # 0: pfStandardProposal 24 | # 1: pfAuxiliaryPFStandard 25 | # 2: pfOptimalProposal *** (ICP,...) 26 | # 3: pfAuxiliaryPFOptimal *** (Optimal SAMPLING) 27 | # 28 | # See: /docs/html/topic_pf.html or http://mrpt.sourceforge.net/topic_pf.html 29 | #---------------------------------------------------------------------------------- 30 | PF_algorithm=2 31 | adaptiveSampleSize=0 // 0: Fixed # of particles, 1: KLD adaptive 32 | 33 | #---------------------------------------------------------------------------------- 34 | # The Particle Filter Resampling method: 35 | # 0: prMultinomial 36 | # 1: prResidual 37 | # 2: prStratified 38 | # 3: prSystematic 39 | # 40 | # See: /docs/html/topic_resampling.html or http://mrpt.sourceforge.net/topic_resampling.html 41 | #---------------------------------------------------------------------------------- 42 | resamplingMethod=0 43 | 44 | sampleSize=20 // Sample size (for fixed number) 45 | BETA=0.50 // Resampling ESS threshold 46 | 47 | max_loglikelihood_dyn_range = 20 48 | 49 | # ======================================================== 50 | # MULTIMETRIC MAP CONFIGURATION 51 | # See docs for (Google for) mrpt::maps::CMultiMetricMap 52 | # ======================================================== 53 | # Creation of maps: 54 | occupancyGrid_count=1 55 | gasGrid_count=0 56 | landmarksMap_count=0 57 | beaconMap_count=0 58 | pointsMap_count=0 59 | 60 | # Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3) 61 | likelihoodMapSelection=-1 62 | 63 | # ==================================================== 64 | # CHybridMetricMapPDF::TPredictionParams 65 | # ==================================================== 66 | powFactor=0.001 // A "power factor" for updating weights 67 | pfAuxFilterOptimal_MaximumSearchSamples=600 // For PF algorithm=3 68 | 69 | 70 | 71 | # ----------------------------------------------------------------- 72 | # pfOptimalProposal_mapSelection 73 | # Only for PF algorithm=2 (Exact "pfOptimalProposal") 74 | # Select the map on which to calculate the optimal 75 | # proposal distribution. Values: 76 | # 0: Gridmap -> Uses Scan matching-based approximation (based on Stachniss' work) 77 | # 1: Landmarks -> Uses matching to approximate optimal 78 | # 2: Beacons -> Used for exact optimal proposal in RO-SLAM 79 | # ----------------------------------------------------------------- 80 | pfOptimalProposal_mapSelection=0 81 | 82 | # Adaptive sample size parameters ------------------ 83 | KLD_maxSampleSize=150 84 | KLD_minSampleSize=15 85 | KLD_binSize_XY=0.03 86 | KLD_binSize_PHI_deg=1 87 | KLD_delta=0.001 88 | KLD_epsilon=0.5 89 | 90 | 91 | # ==================================================== 92 | # MULTIMETRIC MAP: OccGrid #00 93 | # ==================================================== 94 | # Creation Options for OccupancyGridMap 00: 95 | [MappingApplication_occupancyGrid_00_creationOpts] 96 | resolution=0.10 97 | disableSaveAs3DObject=0 98 | 99 | 100 | # Insertion Options for OccupancyGridMap 00: 101 | [MappingApplication_occupancyGrid_00_insertOpts] 102 | mapAltitude=0 103 | useMapAltitude=0 104 | maxDistanceInsertion=30 105 | maxOccupancyUpdateCertainty=0.54 106 | considerInvalidRangesAsFreeSpace=1 107 | minLaserScanNoiseStd=0.001 108 | 109 | wideningBeamsWithDistance = 1 110 | 111 | 112 | # Likelihood Options for OccupancyGridMap 00: 113 | [MappingApplication_occupancyGrid_00_likelihoodOpts] 114 | likelihoodMethod=4 // 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II 115 | LF_decimation=4 116 | LF_stdHit=0.05 117 | LF_maxCorrsDistance=0.50 118 | LF_zHit=0.999 119 | LF_zRandom=0.001 120 | LF_maxRange=30 121 | LF_alternateAverageMethod=0 122 | 123 | enableLikelihoodCache=1 124 | 125 | -------------------------------------------------------------------------------- /mrpt_rbpf_slam/tutorial/grid_slam_demo.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_rbpf_slam/tutorial/grid_slam_demo.rawlog -------------------------------------------------------------------------------- /mrpt_rbpf_slam/tutorial/grid_slam_demo2.rawlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrpt-ros-pkg/mrpt_slam/8aad0909dc5999ac74f1d9342d9d2dddeb771e2f/mrpt_rbpf_slam/tutorial/grid_slam_demo2.rawlog -------------------------------------------------------------------------------- /mrpt_slam/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrpt_slam 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.19 (2024-09-18) 6 | ------------------- 7 | 8 | 0.1.18 (2024-09-05) 9 | ------------------- 10 | 11 | 0.1.17 (2024-06-05) 12 | ------------------- 13 | 14 | 0.1.16 (2024-03-25) 15 | ------------------- 16 | 17 | 0.1.15 (2023-08-02) 18 | ------------------- 19 | 20 | 0.1.14 (2023-04-12) 21 | ------------------- 22 | * changelogs 23 | * Contributors: Jose Luis Blanco-Claraco 24 | 25 | 0.1.11 (2022-06-24) 26 | ------------------- 27 | 28 | 0.1.10 (2019-10-05) 29 | ------------------- 30 | 31 | 0.1.9 (2019-04-14) 32 | ------------------ 33 | 34 | 0.1.8 (2018-09-21) 35 | ------------------ 36 | * Make catkin_lint clean 37 | * Contributors: Jose Luis Blanco Claraco 38 | 39 | 0.1.7 (2018-09-20) 40 | ------------------ 41 | * metapkg: Add missing graphslam 42 | * Contributors: Jose Luis Blanco Claraco 43 | 44 | 0.1.6 (2018-06-14) 45 | ------------------ 46 | 47 | 0.1.5 (2016-11-18) 48 | ------------------ 49 | 50 | 0.1.3 (2016-09-27) 51 | ------------------ 52 | 53 | 0.1.2 (2016-09-24) 54 | ------------------ 55 | 56 | 0.1.1 (2016-08-22) 57 | ------------------ 58 | * First public version, as a result of Vladislav Tananaev's GSoC2016 work. 59 | * Contributors: Jose Luis Blanco, Jose Luis Blanco-Claraco, Logrus 60 | -------------------------------------------------------------------------------- /mrpt_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrpt_slam) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /mrpt_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrpt_slam 4 | 0.1.19 5 | mrpt_slam 6 | Vladislav Tananaev 7 | Jose Luis Blanco Claraco 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/mrpt_slam 12 | https://github.com/mrpt-ros-pkg/mrpt_slam/issues 13 | 14 | Vladislav Tananaev 15 | Jose Luis Blanco Claraco 16 | 17 | 18 | catkin 19 | 20 | mrpt_ekf_slam_3d 21 | mrpt_ekf_slam_2d 22 | mrpt_icp_slam_2d 23 | mrpt_graphslam_2d 24 | mrpt_rbpf_slam 25 | 26 | 27 | 28 | 29 | 30 | 31 | --------------------------------------------------------------------------------