├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md └── PULL_REQUEST_TEMPLATE.md ├── .gitignore ├── CMake ├── FindCHOLMOD.cmake ├── FindCSparse.cmake ├── FindG2O.cmake ├── FindGTSAM.cmake ├── FindMKL.cmake └── FindSuiteSparse.cmake ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── README.md ├── config ├── mapper_params_lifelong.yaml ├── mapper_params_localization.yaml ├── mapper_params_offline.yaml ├── mapper_params_online_async.yaml ├── mapper_params_online_sync.yaml └── slam_toolbox_default.rviz ├── images ├── ceres_solver_comparison.png ├── circuit_launch.gif ├── mapping_steves_apartment.gif ├── rviz_plugin.png └── slam_toolbox_sync.png ├── include └── slam_toolbox │ ├── experimental │ ├── slam_toolbox_lifelong.hpp │ └── slam_toolbox_map_and_localization.hpp │ ├── get_pose_helper.hpp │ ├── laser_utils.hpp │ ├── loop_closure_assistant.hpp │ ├── map_saver.hpp │ ├── merge_maps_kinematic.hpp │ ├── serialization.hpp │ ├── slam_mapper.hpp │ ├── slam_toolbox_async.hpp │ ├── slam_toolbox_common.hpp │ ├── slam_toolbox_localization.hpp │ ├── slam_toolbox_sync.hpp │ ├── snap_utils.hpp │ ├── toolbox_msgs.hpp │ ├── toolbox_types.hpp │ └── visualization_utils.hpp ├── launch ├── lifelong_launch.py ├── localization_launch.py ├── merge_maps_kinematic_launch.py ├── offline_launch.py ├── online_async_launch.py └── online_sync_launch.py ├── lib └── karto_sdk │ ├── Authors │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include │ └── karto_sdk │ │ ├── Karto.h │ │ ├── Macros.h │ │ ├── Mapper.h │ │ ├── Math.h │ │ ├── Types.h │ │ ├── nanoflann.hpp │ │ └── nanoflann_adaptors.h │ ├── package.xml │ └── src │ ├── Karto.cpp │ └── Mapper.cpp ├── package.xml ├── rviz_plugin ├── slam_toolbox_rviz_plugin.cpp └── slam_toolbox_rviz_plugin.hpp ├── rviz_plugins.xml ├── solver_plugins.xml ├── solvers ├── ceres_solver.cpp ├── ceres_solver.hpp └── ceres_utils.h ├── src ├── experimental │ ├── slam_toolbox_lifelong.cpp │ ├── slam_toolbox_lifelong_node.cpp │ ├── slam_toolbox_map_and_localization.cpp │ └── slam_toolbox_map_and_localization_node.cpp ├── laser_utils.cpp ├── loop_closure_assistant.cpp ├── map_saver.cpp ├── merge_maps_kinematic.cpp ├── slam_mapper.cpp ├── slam_toolbox_async.cpp ├── slam_toolbox_async_node.cpp ├── slam_toolbox_common.cpp ├── slam_toolbox_localization.cpp ├── slam_toolbox_localization_node.cpp ├── slam_toolbox_sync.cpp └── slam_toolbox_sync_node.cpp ├── srv ├── AddSubmap.srv ├── Clear.srv ├── ClearQueue.srv ├── DeserializePoseGraph.srv ├── LoopClosure.srv ├── MergeMaps.srv ├── Pause.srv ├── Reset.srv ├── SaveMap.srv ├── SerializePoseGraph.srv └── ToggleInteractive.srv └── test ├── constraints_on_graph.dat ├── lifelong_metrics_test.cpp └── process_constraints.py /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Required Info:** 11 | 12 | - Operating System: 13 | - 14 | - Installation type: 15 | - 16 | - ROS Version 17 | - 18 | - Version or commit hash: 19 | - 20 | - Laser unit: 21 | - 22 | 23 | #### Steps to reproduce issue 24 | 25 | 26 | #### Expected behavior 27 | 28 | #### Actual behavior 29 | 30 | #### Additional information 31 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | #### Feature description 11 | 12 | 13 | #### Implementation considerations 14 | 15 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | --- 4 | 5 | ## Basic Info 6 | 7 | | Info | Please fill out this column | 8 | | ------ | ----------- | 9 | | Ticket(s) this addresses | (add tickets here #1) | 10 | | Primary OS tested on | (Ubuntu, MacOS, Windows) | 11 | | Robotic platform tested on | (Steve's Robot, gazebo simulation of Tally, hardware turtlebot) | 12 | 13 | --- 14 | 15 | ## Description of contribution in a few bullet points 16 | 17 | 21 | 22 | ## Description of documentation updates required from your changes 23 | 24 | 28 | 29 | --- 30 | 31 | ## Future work that may be required in bullet points 32 | 33 | 38 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | snap_ws/* 2 | .vscode/* -------------------------------------------------------------------------------- /CMake/FindCHOLMOD.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) -------------------------------------------------------------------------------- /CMake/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | PATH_SUFFIXES 14 | suitesparse 15 | ufsparse 16 | ) 17 | 18 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 19 | PATHS 20 | /usr/lib 21 | /usr/local/lib 22 | /opt/local/lib 23 | /sw/lib 24 | ) 25 | 26 | include(FindPackageHandleStandardArgs) 27 | find_package_handle_standard_args(CSparse DEFAULT_MSG 28 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 29 | -------------------------------------------------------------------------------- /CMake/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | /opt/ros/indigo/include 13 | /opt/ros/jade/include 14 | /opt/ros/kinetic/include 15 | NO_DEFAULT_PATH 16 | ) 17 | 18 | # Macro to unify finding both the debug and release versions of the 19 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 20 | # macro. 21 | 22 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 23 | 24 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 25 | NAMES "g2o_${MYLIBRARYNAME}_d" 26 | PATHS 27 | ${G2O_ROOT}/lib/Debug 28 | ${G2O_ROOT}/lib 29 | $ENV{G2O_ROOT}/lib/Debug 30 | $ENV{G2O_ROOT}/lib 31 | NO_DEFAULT_PATH 32 | ) 33 | 34 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 35 | NAMES "g2o_${MYLIBRARYNAME}_d" 36 | PATHS 37 | ~/Library/Frameworks 38 | /Library/Frameworks 39 | /usr/local/lib 40 | /usr/local/lib64 41 | /usr/lib 42 | /usr/lib64 43 | /opt/local/lib 44 | /sw/local/lib 45 | /sw/lib 46 | /opt/ros/indigo/lib 47 | /opt/ros/jade/lib 48 | /opt/ros/kinetic/lib 49 | ) 50 | 51 | FIND_LIBRARY(${MYLIBRARY} 52 | NAMES "g2o_${MYLIBRARYNAME}" 53 | PATHS 54 | ${G2O_ROOT}/lib/Release 55 | ${G2O_ROOT}/lib 56 | $ENV{G2O_ROOT}/lib/Release 57 | $ENV{G2O_ROOT}/lib 58 | NO_DEFAULT_PATH 59 | ) 60 | 61 | FIND_LIBRARY(${MYLIBRARY} 62 | NAMES "g2o_${MYLIBRARYNAME}" 63 | PATHS 64 | ~/Library/Frameworks 65 | /Library/Frameworks 66 | /usr/local/lib 67 | /usr/local/lib64 68 | /usr/lib 69 | /usr/lib64 70 | /opt/local/lib 71 | /sw/local/lib 72 | /sw/lib 73 | /opt/ros/indigo/lib 74 | /opt/ros/jade/lib 75 | /opt/ros/kinetic/lib 76 | ) 77 | 78 | IF(NOT ${MYLIBRARY}_DEBUG) 79 | IF(MYLIBRARY) 80 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 81 | ENDIF(MYLIBRARY) 82 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 83 | 84 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 85 | 86 | # Find the core elements 87 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 88 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 89 | 90 | # Find the CLI library 91 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 92 | 93 | # Find the pluggable solvers 94 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 95 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 96 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 97 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 98 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 99 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 100 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 101 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 102 | 103 | # Find the predefined types 104 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 105 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 106 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 107 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 108 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 109 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 110 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 111 | 112 | # G2O solvers declared found if we found at least one solver 113 | SET(G2O_SOLVERS_FOUND "NO") 114 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 115 | SET(G2O_SOLVERS_FOUND "YES") 116 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 117 | 118 | # G2O itself declared found if we found the core libraries and at least one solver 119 | SET(G2O_FOUND "NO") 120 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 121 | SET(G2O_FOUND "YES") 122 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) -------------------------------------------------------------------------------- /CMake/FindGTSAM.cmake: -------------------------------------------------------------------------------- 1 | # This is FindGTSAM.cmake 2 | # CMake module to locate the GTSAM package 3 | # 4 | # The following cache variables may be set before calling this script: 5 | # 6 | # GTSAM_DIR (or GTSAM_ROOT): (Optional) The install prefix OR source tree of gtsam (e.g. /usr/local or src/gtsam) 7 | # GTSAM_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory 8 | # within it (e.g build-debug). Without this defined, this script tries to 9 | # intelligently find the build directory based on the project's build directory name 10 | # or based on the build type (Debug/Release/etc). 11 | # 12 | # The following variables will be defined: 13 | # 14 | # GTSAM_FOUND : TRUE if the package has been successfully found 15 | # GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories 16 | # GTSAM_LIBS : paths to GTSAM's libraries 17 | # 18 | # NOTES on compiling against an uninstalled GTSAM build tree: 19 | # - A GTSAM source tree will be automatically searched for in the directory 20 | # 'gtsam' next to your project directory, after searching 21 | # CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. 22 | # - The build directory will be searched first with the same name as your 23 | # project's build directory, e.g. if you build from 'MyProject/build-optimized', 24 | # 'gtsam/build-optimized' will be searched first. Next, a build directory for 25 | # your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is 26 | # 'Release', then 'gtsam/build-release' will be searched next. Finally, plain 27 | # 'gtsam/build' will be searched. 28 | # - You can control the gtsam build directory name directly by defining the CMake 29 | # cache variable 'GTSAM_BUILD_NAME', then only 'gtsam/${GTSAM_BUILD_NAME} will 30 | # be searched. 31 | # - Use the standard CMAKE_PREFIX_PATH, or GTSAM_DIR, to find a specific gtsam 32 | # directory. 33 | 34 | # Get path suffixes to help look for gtsam 35 | if(GTSAM_BUILD_NAME) 36 | set(gtsam_build_names "${GTSAM_BUILD_NAME}/gtsam") 37 | else() 38 | # lowercase build type 39 | string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) 40 | # build suffix of this project 41 | get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) 42 | 43 | set(gtsam_build_names "${my_build_name}/gtsam" "build-${build_type_suffix}/gtsam" "build/gtsam") 44 | endif() 45 | 46 | # Use GTSAM_ROOT or GTSAM_DIR equivalently 47 | if(GTSAM_ROOT AND NOT GTSAM_DIR) 48 | set(GTSAM_DIR "${GTSAM_ROOT}") 49 | endif() 50 | 51 | if(GTSAM_DIR) 52 | # Find include dirs 53 | find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h 54 | PATHS "${GTSAM_DIR}/include" "${GTSAM_DIR}" NO_DEFAULT_PATH 55 | DOC "GTSAM include directories") 56 | 57 | # Find libraries 58 | find_library(GTSAM_LIBS NAMES gtsam 59 | HINTS "${GTSAM_DIR}/lib" "${GTSAM_DIR}" NO_DEFAULT_PATH 60 | PATH_SUFFIXES ${gtsam_build_names} 61 | DOC "GTSAM libraries") 62 | else() 63 | # Find include dirs 64 | set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include) 65 | find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h 66 | PATHS ${extra_include_paths} 67 | DOC "GTSAM include directories") 68 | if(NOT GTSAM_INCLUDE_DIR) 69 | message(STATUS "Searched for gtsam headers in default paths plus ${extra_include_paths}") 70 | endif() 71 | 72 | # Find libraries 73 | find_library(GTSAM_LIBS NAMES gtsam 74 | HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib 75 | PATH_SUFFIXES ${gtsam_build_names} 76 | DOC "GTSAM libraries") 77 | endif() 78 | 79 | # handle the QUIETLY and REQUIRED arguments and set GTSAM_FOUND to TRUE 80 | # if all listed variables are TRUE 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(GTSAM DEFAULT_MSG 83 | GTSAM_LIBS GTSAM_INCLUDE_DIR) 84 | 85 | 86 | -------------------------------------------------------------------------------- /CMake/FindMKL.cmake: -------------------------------------------------------------------------------- 1 | # This file is adapted from the one in OpenMEEG: http://www-sop.inria.fr/athena/software/OpenMEEG/ 2 | # - Try to find the Intel Math Kernel Library 3 | # Once done this will define 4 | # 5 | # MKL_FOUND - system has MKL 6 | # MKL_ROOT_DIR - path to the MKL base directory 7 | # MKL_INCLUDE_DIR - the MKL include directory 8 | # MKL_LIBRARIES - MKL libraries 9 | # 10 | # There are few sets of libraries: 11 | # Array indexes modes: 12 | # LP - 32 bit indexes of arrays 13 | # ILP - 64 bit indexes of arrays 14 | # Threading: 15 | # SEQUENTIAL - no threading 16 | # INTEL - Intel threading library 17 | # GNU - GNU threading library 18 | # MPI support 19 | # NOMPI - no MPI support 20 | # INTEL - Intel MPI library 21 | # OPEN - Open MPI library 22 | # SGI - SGI MPT Library 23 | 24 | # linux 25 | IF(UNIX AND NOT APPLE) 26 | IF(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL "x86_64") 27 | SET(MKL_ARCH_DIR "intel64") 28 | ELSE() 29 | SET(MKL_ARCH_DIR "32") 30 | ENDIF() 31 | ENDIF() 32 | # macos 33 | IF(APPLE) 34 | SET(MKL_ARCH_DIR "intel64") 35 | ENDIF() 36 | 37 | IF(FORCE_BUILD_32BITS) 38 | set(MKL_ARCH_DIR "32") 39 | ENDIF() 40 | # windows 41 | IF(WIN32) 42 | IF(${CMAKE_SIZEOF_VOID_P} EQUAL 8) 43 | SET(MKL_ARCH_DIR "intel64") 44 | ELSE() 45 | SET(MKL_ARCH_DIR "ia32") 46 | ENDIF() 47 | ENDIF() 48 | 49 | SET(MKL_THREAD_VARIANTS SEQUENTIAL GNUTHREAD INTELTHREAD) 50 | SET(MKL_MODE_VARIANTS ILP LP) 51 | SET(MKL_MPI_VARIANTS NOMPI INTELMPI OPENMPI SGIMPT) 52 | 53 | FIND_PATH(MKL_ROOT_DIR 54 | include/mkl_cblas.h 55 | PATHS 56 | $ENV{MKLDIR} 57 | /opt/intel/mkl/ 58 | /opt/intel/mkl/*/ 59 | /opt/intel/cmkl/ 60 | /opt/intel/cmkl/*/ 61 | /opt/intel/*/mkl/ 62 | /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal 63 | "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl" 64 | "C:/Program Files (x86)/Intel/Composer XE 2013/mkl" 65 | "C:/Program Files/Intel/MKL/*/" 66 | "C:/Program Files/Intel/ComposerXE-2011/mkl" 67 | "C:/Program Files/Intel/Composer XE 2013/mkl" 68 | ) 69 | 70 | FIND_PATH(MKL_INCLUDE_DIR 71 | mkl_cblas.h 72 | PATHS 73 | ${MKL_ROOT_DIR}/include 74 | ${INCLUDE_INSTALL_DIR} 75 | ) 76 | 77 | FIND_PATH(MKL_FFTW_INCLUDE_DIR 78 | fftw3.h 79 | PATH_SUFFIXES fftw 80 | PATHS 81 | ${MKL_ROOT_DIR}/include 82 | ${INCLUDE_INSTALL_DIR} 83 | NO_DEFAULT_PATH 84 | ) 85 | 86 | IF(WIN32) 87 | SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}") 88 | 89 | IF (MKL_INCLUDE_DIR MATCHES "10.") 90 | IF(CMAKE_CL_64) 91 | SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64) 92 | ELSE() 93 | SET(MKL_LIBS mkl_solver mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libguide mkl_lapack95 mkl_blas95) 94 | ENDIF() 95 | ELSEIF(MKL_INCLUDE_DIR MATCHES "2013") # version 11 ... 96 | IF(CMAKE_CL_64) 97 | SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_intel_thread libiomp5md mkl_lapack95_lp64 mkl_blas95_lp64) 98 | ELSE() 99 | SET(MKL_LIBS mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libiomp5md mkl_lapack95 mkl_blas95) 100 | ENDIF() 101 | ELSE() # old MKL 9 102 | SET(MKL_LIBS mkl_solver mkl_c libguide mkl_lapack mkl_ia32) 103 | ENDIF() 104 | 105 | IF (MKL_INCLUDE_DIR MATCHES "10.3") 106 | SET(MKL_LIBS ${MKL_LIBS} libiomp5md) 107 | ENDIF() 108 | 109 | FOREACH (LIB ${MKL_LIBS}) 110 | FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH) 111 | IF(${LIB}_PATH) 112 | SET(MKL_LIBRARIES ${MKL_LIBRARIES} ${${LIB}_PATH}) 113 | ELSE() 114 | MESSAGE(STATUS "Could not find ${LIB}: disabling MKL") 115 | ENDIF() 116 | ENDFOREACH() 117 | SET(MKL_FOUND ON) 118 | ELSE() # UNIX and macOS 119 | FIND_LIBRARY(MKL_CORE_LIBRARY 120 | mkl_core 121 | PATHS 122 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 123 | ${MKL_ROOT_DIR}/lib/ 124 | ) 125 | 126 | # Threading libraries 127 | FIND_LIBRARY(MKL_SEQUENTIAL_LIBRARY 128 | mkl_sequential 129 | PATHS 130 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 131 | ${MKL_ROOT_DIR}/lib/ 132 | ) 133 | 134 | FIND_LIBRARY(MKL_INTELTHREAD_LIBRARY 135 | mkl_intel_thread 136 | PATHS 137 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 138 | ${MKL_ROOT_DIR}/lib/ 139 | ) 140 | 141 | # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above) 142 | IF(NOT APPLE) 143 | FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY 144 | mkl_gnu_thread 145 | PATHS 146 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 147 | ${MKL_ROOT_DIR}/lib/ 148 | ) 149 | ENDIF() 150 | 151 | # Intel Libraries 152 | IF("${MKL_ARCH_DIR}" STREQUAL "32") 153 | FIND_LIBRARY(MKL_LP_LIBRARY 154 | mkl_intel 155 | PATHS 156 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 157 | ${MKL_ROOT_DIR}/lib/ 158 | ) 159 | 160 | FIND_LIBRARY(MKL_ILP_LIBRARY 161 | mkl_intel 162 | PATHS 163 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 164 | ${MKL_ROOT_DIR}/lib/ 165 | ) 166 | else() 167 | FIND_LIBRARY(MKL_LP_LIBRARY 168 | mkl_intel_lp64 169 | PATHS 170 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 171 | ${MKL_ROOT_DIR}/lib/ 172 | ) 173 | 174 | FIND_LIBRARY(MKL_ILP_LIBRARY 175 | mkl_intel_ilp64 176 | PATHS 177 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 178 | ${MKL_ROOT_DIR}/lib/ 179 | ) 180 | ENDIF() 181 | 182 | # Lapack 183 | FIND_LIBRARY(MKL_LAPACK_LIBRARY 184 | mkl_lapack 185 | PATHS 186 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 187 | ${MKL_ROOT_DIR}/lib/ 188 | ) 189 | 190 | IF(NOT MKL_LAPACK_LIBRARY) 191 | FIND_LIBRARY(MKL_LAPACK_LIBRARY 192 | mkl_lapack95_lp64 193 | PATHS 194 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 195 | ${MKL_ROOT_DIR}/lib/ 196 | ) 197 | ENDIF() 198 | 199 | # iomp5 200 | IF("${MKL_ARCH_DIR}" STREQUAL "32") 201 | IF(UNIX AND NOT APPLE) 202 | FIND_LIBRARY(MKL_IOMP5_LIBRARY 203 | iomp5 204 | PATHS 205 | ${MKL_ROOT_DIR}/../lib/ia32 206 | ) 207 | ELSE() 208 | SET(MKL_IOMP5_LIBRARY "") # no need for mac 209 | ENDIF() 210 | else() 211 | IF(UNIX AND NOT APPLE) 212 | FIND_LIBRARY(MKL_IOMP5_LIBRARY 213 | iomp5 214 | PATHS 215 | ${MKL_ROOT_DIR}/../lib/intel64 216 | ) 217 | ELSE() 218 | SET(MKL_IOMP5_LIBRARY "") # no need for mac 219 | ENDIF() 220 | ENDIF() 221 | 222 | foreach (MODEVAR ${MKL_MODE_VARIANTS}) 223 | foreach (THREADVAR ${MKL_THREAD_VARIANTS}) 224 | if (MKL_CORE_LIBRARY AND MKL_${MODEVAR}_LIBRARY AND MKL_${THREADVAR}_LIBRARY) 225 | set(MKL_${MODEVAR}_${THREADVAR}_LIBRARIES 226 | ${MKL_${MODEVAR}_LIBRARY} ${MKL_${THREADVAR}_LIBRARY} ${MKL_CORE_LIBRARY} 227 | ${MKL_LAPACK_LIBRARY} ${MKL_IOMP5_LIBRARY}) 228 | # message("${MODEVAR} ${THREADVAR} ${MKL_${MODEVAR}_${THREADVAR}_LIBRARIES}") # for debug 229 | endif() 230 | endforeach() 231 | endforeach() 232 | 233 | IF(APPLE) 234 | SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES}) 235 | ELSE() 236 | SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) 237 | ENDIF() 238 | 239 | MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY 240 | MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) 241 | ENDIF() 242 | 243 | INCLUDE(FindPackageHandleStandardArgs) 244 | find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES) 245 | 246 | #if(MKL_FOUND) 247 | # LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack 248 | #endif() 249 | 250 | MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES) -------------------------------------------------------------------------------- /CMake/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 39 | PATHS 40 | ${SUITE_SPARSE_ROOT}/lib 41 | /usr/lib 42 | /usr/local/lib 43 | /opt/local/lib 44 | /sw/lib 45 | ) 46 | 47 | 48 | # Different platforms seemingly require linking against different sets of libraries 49 | IF(CYGWIN) 50 | FIND_PACKAGE(PkgConfig) 51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 52 | PATHS 53 | /usr/lib 54 | /usr/local/lib 55 | /opt/local/lib 56 | /sw/lib 57 | ) 58 | PKG_CHECK_MODULES(LAPACK lapack) 59 | 60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 61 | 62 | # MacPorts build of the SparseSuite requires linking against extra libraries 63 | 64 | ELSEIF(APPLE) 65 | 66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 67 | PATHS 68 | /usr/lib 69 | /usr/local/lib 70 | /opt/local/lib 71 | /sw/lib 72 | ) 73 | 74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 75 | PATHS 76 | /usr/lib 77 | /usr/local/lib 78 | /opt/local/lib 79 | /sw/lib 80 | ) 81 | 82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 83 | PATHS 84 | /usr/lib 85 | /usr/local/lib 86 | /opt/local/lib 87 | /sw/lib 88 | ) 89 | 90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 91 | ELSE(APPLE) 92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 93 | ENDIF(CYGWIN) 94 | 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 96 | SET(CHOLMOD_FOUND TRUE) 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 98 | SET(CHOLMOD_FOUND FALSE) 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 100 | 101 | # Look for csparse; note the difference in the directory specifications! 102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 103 | PATHS 104 | /usr/include/suitesparse 105 | /usr/include 106 | /opt/local/include 107 | /usr/local/include 108 | /sw/include 109 | /usr/include/ufsparse 110 | /opt/local/include/ufsparse 111 | /usr/local/include/ufsparse 112 | /sw/include/ufsparse 113 | ) 114 | 115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 116 | PATHS 117 | /usr/lib 118 | /usr/local/lib 119 | /opt/local/lib 120 | /sw/lib 121 | ) 122 | 123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 124 | SET(CSPARSE_FOUND TRUE) 125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 126 | SET(CSPARSE_FOUND FALSE) 127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 128 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:eloquent-ros-base-bionic 2 | 3 | # USE BASH 4 | SHELL ["/bin/bash", "-c"] 5 | 6 | # RUN LINE BELOW TO REMOVE debconf ERRORS (MUST RUN BEFORE ANY apt-get CALLS) 7 | RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections 8 | 9 | RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends apt-utils 10 | 11 | # slam_toolbox 12 | RUN mkdir -p colcon_ws/src 13 | RUN cd colcon_ws/src && git clone -b eloquent-devel https://github.com/SteveMacenski/slam_toolbox.git 14 | RUN source /opt/ros/eloquent/setup.bash \ 15 | && cd colcon_ws \ 16 | && rosdep update \ 17 | && rosdep install -y -r --from-paths src --ignore-src --rosdistro=eloquent -y 18 | 19 | RUN source /opt/ros/eloquent/setup.bash \ 20 | && cd colcon_ws/ \ 21 | && colcon build --cmake-args=-DCMAKE_BUILD_TYPE=Release \ 22 | && colcon test 23 | -------------------------------------------------------------------------------- /config/mapper_params_lifelong.yaml: -------------------------------------------------------------------------------- 1 | 2 | slam_toolbox: 3 | ros__parameters: 4 | 5 | # Plugin params 6 | solver_plugin: solver_plugins::CeresSolver 7 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 8 | ceres_preconditioner: SCHUR_JACOBI 9 | ceres_trust_strategy: LEVENBERG_MARQUARDT 10 | ceres_dogleg_type: TRADITIONAL_DOGLEG 11 | ceres_loss_function: None 12 | 13 | # ROS Parameters 14 | odom_frame: odom 15 | map_frame: map 16 | base_frame: base_footprint 17 | scan_topic: /scan 18 | use_map_saver: true 19 | mode: mapping 20 | 21 | # lifelong params 22 | lifelong_search_use_tree: false 23 | lifelong_minimum_score: 0.1 24 | lifelong_iou_match: 0.85 25 | lifelong_node_removal_score: 0.04 26 | lifelong_overlap_score_scale: 0.06 27 | lifelong_constraint_multiplier: 0.08 28 | lifelong_nearby_penalty: 0.001 29 | lifelong_candidates_scale: 0.03 30 | 31 | # if you'd like to immediately start continuing a map at a given pose 32 | # or at the dock, but they are mutually exclusive, if pose is given 33 | # will use pose 34 | #map_file_name: test_steve 35 | #map_start_pose: [0.0, 0.0, 0.0] 36 | #map_start_at_dock: true 37 | 38 | debug_logging: false 39 | throttle_scans: 1 40 | transform_publish_period: 0.02 #if 0 never publishes odometry 41 | map_update_interval: 5.0 42 | resolution: 0.05 43 | min_laser_range: 0.0 #for rastering images 44 | max_laser_range: 20.0 #for rastering images 45 | minimum_time_interval: 0.5 46 | transform_timeout: 0.2 47 | tf_buffer_duration: 10. 48 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 49 | 50 | # General Parameters 51 | use_scan_matching: true 52 | use_scan_barycenter: true 53 | minimum_travel_distance: 0.5 54 | minimum_travel_heading: 0.5 55 | scan_buffer_size: 10 56 | scan_buffer_maximum_scan_distance: 10.0 57 | link_match_minimum_response_fine: 0.1 58 | link_scan_maximum_distance: 1.5 59 | loop_search_maximum_distance: 3.0 60 | do_loop_closing: true 61 | loop_match_minimum_chain_size: 10 62 | loop_match_maximum_variance_coarse: 3.0 63 | loop_match_minimum_response_coarse: 0.35 64 | loop_match_minimum_response_fine: 0.45 65 | 66 | # Correlation Parameters - Correlation Parameters 67 | correlation_search_space_dimension: 0.5 68 | correlation_search_space_resolution: 0.01 69 | correlation_search_space_smear_deviation: 0.1 70 | 71 | # Correlation Parameters - Loop Closure Parameters 72 | loop_search_space_dimension: 8.0 73 | loop_search_space_resolution: 0.05 74 | loop_search_space_smear_deviation: 0.03 75 | 76 | # Scan Matcher Parameters 77 | distance_variance_penalty: 0.5 78 | angle_variance_penalty: 1.0 79 | 80 | fine_search_angle_offset: 0.00349 81 | coarse_search_angle_offset: 0.349 82 | coarse_angle_resolution: 0.0349 83 | minimum_angle_penalty: 0.9 84 | minimum_distance_penalty: 0.5 85 | use_response_expansion: true 86 | min_pass_through: 2 87 | occupancy_threshold: 0.1 88 | -------------------------------------------------------------------------------- /config/mapper_params_localization.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | solver_plugin: solver_plugins::CeresSolver 4 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 5 | ceres_preconditioner: SCHUR_JACOBI 6 | ceres_trust_strategy: LEVENBERG_MARQUARDT 7 | ceres_dogleg_type: TRADITIONAL_DOGLEG 8 | ceres_loss_function: None 9 | 10 | # ROS Parameters 11 | odom_frame: odom 12 | map_frame: map 13 | base_frame: base_footprint 14 | scan_topic: /scan 15 | mode: mapping #localization 16 | 17 | # if you'd like to start localizing on bringup in a map and pose 18 | #map_file_name: test_steve 19 | #map_start_pose: [5.0, 1.0, 0.0] 20 | 21 | debug_logging: false 22 | throttle_scans: 1 23 | transform_publish_period: 0.02 #if 0 never publishes odometry 24 | map_update_interval: 5.0 25 | resolution: 0.05 26 | min_laser_range: 0.0 #for rastering images 27 | max_laser_range: 20.0 #for rastering images 28 | minimum_time_interval: 0.5 29 | transform_timeout: 0.2 30 | tf_buffer_duration: 30. 31 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 32 | 33 | # General Parameters 34 | use_scan_matching: true 35 | use_scan_barycenter: true 36 | minimum_travel_distance: 0.5 37 | minimum_travel_heading: 0.5 38 | scan_buffer_size: 3 39 | scan_buffer_maximum_scan_distance: 10.0 40 | link_match_minimum_response_fine: 0.1 41 | link_scan_maximum_distance: 1.5 42 | do_loop_closing: true 43 | loop_match_minimum_chain_size: 3 44 | loop_match_maximum_variance_coarse: 3.0 45 | loop_match_minimum_response_coarse: 0.35 46 | loop_match_minimum_response_fine: 0.45 47 | 48 | # Correlation Parameters - Correlation Parameters 49 | correlation_search_space_dimension: 0.5 50 | correlation_search_space_resolution: 0.01 51 | correlation_search_space_smear_deviation: 0.1 52 | 53 | # Correlation Parameters - Loop Closure Parameters 54 | loop_search_space_dimension: 8.0 55 | loop_search_space_resolution: 0.05 56 | loop_search_space_smear_deviation: 0.03 57 | loop_search_maximum_distance: 3.0 58 | 59 | # Scan Matcher Parameters 60 | distance_variance_penalty: 0.5 61 | angle_variance_penalty: 1.0 62 | 63 | fine_search_angle_offset: 0.00349 64 | coarse_search_angle_offset: 0.349 65 | coarse_angle_resolution: 0.0349 66 | minimum_angle_penalty: 0.9 67 | minimum_distance_penalty: 0.5 68 | use_response_expansion: true 69 | min_pass_through: 2 70 | occupancy_threshold: 0.1 71 | -------------------------------------------------------------------------------- /config/mapper_params_offline.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # Plugin params 5 | solver_plugin: solver_plugins::CeresSolver 6 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 7 | ceres_preconditioner: SCHUR_JACOBI 8 | ceres_trust_strategy: LEVENBERG_MARQUARDT 9 | ceres_dogleg_type: TRADITIONAL_DOGLEG 10 | ceres_loss_function: None 11 | 12 | # ROS Parameters 13 | odom_frame: odom 14 | map_frame: map 15 | base_frame: base_footprint 16 | scan_topic: /scan 17 | use_map_saver: true 18 | mode: mapping #localization 19 | debug_logging: false 20 | throttle_scans: 1 21 | transform_publish_period: 0.02 #if 0 never publishes odometry 22 | map_update_interval: 10.0 23 | resolution: 0.05 24 | min_laser_range: 0.0 #for rastering images 25 | max_laser_range: 20.0 #for rastering images 26 | minimum_time_interval: 0.5 27 | transform_timeout: 0.2 28 | tf_buffer_duration: 14400. 29 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 30 | enable_interactive_mode: true 31 | 32 | # General Parameters 33 | use_scan_matching: true 34 | use_scan_barycenter: true 35 | minimum_travel_distance: 0.5 36 | minimum_travel_heading: 0.5 37 | scan_buffer_size: 10 38 | scan_buffer_maximum_scan_distance: 10.0 39 | link_match_minimum_response_fine: 0.1 40 | link_scan_maximum_distance: 1.5 41 | loop_search_maximum_distance: 3.0 42 | do_loop_closing: true 43 | loop_match_minimum_chain_size: 10 44 | loop_match_maximum_variance_coarse: 3.0 45 | loop_match_minimum_response_coarse: 0.35 46 | loop_match_minimum_response_fine: 0.45 47 | 48 | # Correlation Parameters - Correlation Parameters 49 | correlation_search_space_dimension: 0.5 50 | correlation_search_space_resolution: 0.01 51 | correlation_search_space_smear_deviation: 0.1 52 | 53 | # Correlation Parameters - Loop Closure Parameters 54 | loop_search_space_dimension: 8.0 55 | loop_search_space_resolution: 0.05 56 | loop_search_space_smear_deviation: 0.03 57 | 58 | # Scan Matcher Parameters 59 | distance_variance_penalty: 0.5 60 | angle_variance_penalty: 1.0 61 | 62 | fine_search_angle_offset: 0.00349 63 | coarse_search_angle_offset: 0.349 64 | coarse_angle_resolution: 0.0349 65 | minimum_angle_penalty: 0.9 66 | minimum_distance_penalty: 0.5 67 | use_response_expansion: true 68 | min_pass_through: 2 69 | occupancy_threshold: 0.1 70 | -------------------------------------------------------------------------------- /config/mapper_params_online_async.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # Plugin params 5 | solver_plugin: solver_plugins::CeresSolver 6 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 7 | ceres_preconditioner: SCHUR_JACOBI 8 | ceres_trust_strategy: LEVENBERG_MARQUARDT 9 | ceres_dogleg_type: TRADITIONAL_DOGLEG 10 | ceres_loss_function: None 11 | 12 | # ROS Parameters 13 | odom_frame: odom 14 | map_frame: map 15 | base_frame: base_footprint 16 | scan_topic: /scan 17 | use_map_saver: true 18 | mode: mapping #localization 19 | 20 | # if you'd like to immediately start continuing a map at a given pose 21 | # or at the dock, but they are mutually exclusive, if pose is given 22 | # will use pose 23 | #map_file_name: test_steve 24 | # map_start_pose: [0.0, 0.0, 0.0] 25 | #map_start_at_dock: true 26 | 27 | debug_logging: false 28 | throttle_scans: 1 29 | transform_publish_period: 0.02 #if 0 never publishes odometry 30 | map_update_interval: 5.0 31 | resolution: 0.05 32 | min_laser_range: 0.0 #for rastering images 33 | max_laser_range: 20.0 #for rastering images 34 | minimum_time_interval: 0.5 35 | transform_timeout: 0.2 36 | tf_buffer_duration: 30.0 37 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 38 | enable_interactive_mode: true 39 | 40 | # General Parameters 41 | use_scan_matching: true 42 | use_scan_barycenter: true 43 | minimum_travel_distance: 0.5 44 | minimum_travel_heading: 0.5 45 | scan_buffer_size: 10 46 | scan_buffer_maximum_scan_distance: 10.0 47 | link_match_minimum_response_fine: 0.1 48 | link_scan_maximum_distance: 1.5 49 | loop_search_maximum_distance: 3.0 50 | do_loop_closing: true 51 | loop_match_minimum_chain_size: 10 52 | loop_match_maximum_variance_coarse: 3.0 53 | loop_match_minimum_response_coarse: 0.35 54 | loop_match_minimum_response_fine: 0.45 55 | 56 | # Correlation Parameters - Correlation Parameters 57 | correlation_search_space_dimension: 0.5 58 | correlation_search_space_resolution: 0.01 59 | correlation_search_space_smear_deviation: 0.1 60 | 61 | # Correlation Parameters - Loop Closure Parameters 62 | loop_search_space_dimension: 8.0 63 | loop_search_space_resolution: 0.05 64 | loop_search_space_smear_deviation: 0.03 65 | 66 | # Scan Matcher Parameters 67 | distance_variance_penalty: 0.5 68 | angle_variance_penalty: 1.0 69 | 70 | fine_search_angle_offset: 0.00349 71 | coarse_search_angle_offset: 0.349 72 | coarse_angle_resolution: 0.0349 73 | minimum_angle_penalty: 0.9 74 | minimum_distance_penalty: 0.5 75 | use_response_expansion: true 76 | min_pass_through: 2 77 | occupancy_threshold: 0.1 78 | -------------------------------------------------------------------------------- /config/mapper_params_online_sync.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # Plugin params 5 | solver_plugin: solver_plugins::CeresSolver 6 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 7 | ceres_preconditioner: SCHUR_JACOBI 8 | ceres_trust_strategy: LEVENBERG_MARQUARDT 9 | ceres_dogleg_type: TRADITIONAL_DOGLEG 10 | ceres_loss_function: None 11 | 12 | # ROS Parameters 13 | odom_frame: odom 14 | map_frame: map 15 | base_frame: base_footprint 16 | scan_topic: /scan 17 | use_map_saver: true 18 | mode: mapping #localization 19 | 20 | # if you'd like to immediately start continuing a map at a given pose 21 | # or at the dock, but they are mutually exclusive, if pose is given 22 | # will use pose 23 | #map_file_name: test_steve 24 | #map_start_pose: [0.0, 0.0, 0.0] 25 | #map_start_at_dock: true 26 | 27 | debug_logging: false 28 | throttle_scans: 1 29 | transform_publish_period: 0.02 #if 0 never publishes odometry 30 | map_update_interval: 5.0 31 | resolution: 0.05 32 | min_laser_range: 0.0 #for rastering images 33 | max_laser_range: 20.0 #for rastering images 34 | minimum_time_interval: 0.5 35 | transform_timeout: 0.2 36 | tf_buffer_duration: 30. 37 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 38 | enable_interactive_mode: true 39 | 40 | # General Parameters 41 | use_scan_matching: true 42 | use_scan_barycenter: true 43 | minimum_travel_distance: 0.5 44 | minimum_travel_heading: 0.5 45 | scan_buffer_size: 10 46 | scan_buffer_maximum_scan_distance: 10.0 47 | link_match_minimum_response_fine: 0.1 48 | link_scan_maximum_distance: 1.5 49 | loop_search_maximum_distance: 3.0 50 | do_loop_closing: true 51 | loop_match_minimum_chain_size: 10 52 | loop_match_maximum_variance_coarse: 3.0 53 | loop_match_minimum_response_coarse: 0.35 54 | loop_match_minimum_response_fine: 0.45 55 | 56 | # Correlation Parameters - Correlation Parameters 57 | correlation_search_space_dimension: 0.5 58 | correlation_search_space_resolution: 0.01 59 | correlation_search_space_smear_deviation: 0.1 60 | 61 | # Correlation Parameters - Loop Closure Parameters 62 | loop_search_space_dimension: 8.0 63 | loop_search_space_resolution: 0.05 64 | loop_search_space_smear_deviation: 0.03 65 | 66 | # Scan Matcher Parameters 67 | distance_variance_penalty: 0.5 68 | angle_variance_penalty: 1.0 69 | 70 | fine_search_angle_offset: 0.00349 71 | coarse_search_angle_offset: 0.349 72 | coarse_angle_resolution: 0.0349 73 | minimum_angle_penalty: 0.9 74 | minimum_distance_penalty: 0.5 75 | use_response_expansion: true 76 | min_pass_through: 2 77 | occupancy_threshold: 0.1 78 | -------------------------------------------------------------------------------- /config/slam_toolbox_default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 154 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | - Class: slam_toolbox::SlamToolboxPlugin 30 | Name: SlamToolboxPlugin 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | Enabled: true 53 | Global Options: 54 | Background Color: 48; 48; 48 55 | Fixed Frame: map 56 | Frame Rate: 30 57 | Name: root 58 | Tools: 59 | - Class: rviz_default_plugins/Interact 60 | Hide Inactive Objects: true 61 | - Class: rviz_default_plugins/MoveCamera 62 | - Class: rviz_default_plugins/Select 63 | - Class: rviz_default_plugins/FocusCamera 64 | - Class: rviz_default_plugins/Measure 65 | Line color: 128; 128; 0 66 | - Class: rviz_default_plugins/SetInitialPose 67 | Covariance x: 0.25 68 | Covariance y: 0.25 69 | Covariance yaw: 0.06853891909122467 70 | Topic: 71 | Depth: 5 72 | Durability Policy: Volatile 73 | History Policy: Keep Last 74 | Reliability Policy: Reliable 75 | Value: /initialpose 76 | - Class: rviz_default_plugins/SetGoal 77 | Topic: 78 | Depth: 5 79 | Durability Policy: Volatile 80 | History Policy: Keep Last 81 | Reliability Policy: Reliable 82 | Value: /goal_pose 83 | - Class: rviz_default_plugins/PublishPoint 84 | Single click: true 85 | Topic: 86 | Depth: 5 87 | Durability Policy: Volatile 88 | History Policy: Keep Last 89 | Reliability Policy: Reliable 90 | Value: /clicked_point 91 | Transformation: 92 | Current: 93 | Class: rviz_default_plugins/TF 94 | Value: true 95 | Views: 96 | Current: 97 | Class: rviz_default_plugins/Orbit 98 | Distance: 10 99 | Enable Stereo Rendering: 100 | Stereo Eye Separation: 0.05999999865889549 101 | Stereo Focal Distance: 1 102 | Swap Stereo Eyes: false 103 | Value: false 104 | Focal Point: 105 | X: 0 106 | Y: 0 107 | Z: 0 108 | Focal Shape Fixed Size: true 109 | Focal Shape Size: 0.05000000074505806 110 | Invert Z Axis: false 111 | Name: Current View 112 | Near Clip Distance: 0.009999999776482582 113 | Pitch: 0.785398006439209 114 | Target Frame: 115 | Value: Orbit (rviz) 116 | Yaw: 0.785398006439209 117 | Saved: ~ 118 | Window Geometry: 119 | Displays: 120 | collapsed: false 121 | Height: 846 122 | Hide Left Dock: false 123 | Hide Right Dock: false 124 | QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 125 | Selection: 126 | collapsed: false 127 | SlamToolboxPlugin: 128 | collapsed: false 129 | Time: 130 | collapsed: false 131 | Tool Properties: 132 | collapsed: false 133 | Views: 134 | collapsed: false 135 | Width: 1200 136 | X: 72 137 | Y: 60 138 | -------------------------------------------------------------------------------- /images/ceres_solver_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SteveMacenski/slam_toolbox/e59f778874653085d43082502d0463f8ade0221d/images/ceres_solver_comparison.png -------------------------------------------------------------------------------- /images/circuit_launch.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SteveMacenski/slam_toolbox/e59f778874653085d43082502d0463f8ade0221d/images/circuit_launch.gif -------------------------------------------------------------------------------- /images/mapping_steves_apartment.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SteveMacenski/slam_toolbox/e59f778874653085d43082502d0463f8ade0221d/images/mapping_steves_apartment.gif -------------------------------------------------------------------------------- /images/rviz_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SteveMacenski/slam_toolbox/e59f778874653085d43082502d0463f8ade0221d/images/rviz_plugin.png -------------------------------------------------------------------------------- /images/slam_toolbox_sync.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SteveMacenski/slam_toolbox/e59f778874653085d43082502d0463f8ade0221d/images/slam_toolbox_sync.png -------------------------------------------------------------------------------- /include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__EXPERIMENTAL__SLAM_TOOLBOX_LIFELONG_HPP_ 20 | #define SLAM_TOOLBOX__EXPERIMENTAL__SLAM_TOOLBOX_LIFELONG_HPP_ 21 | 22 | #include 23 | #include "slam_toolbox/slam_toolbox_common.hpp" 24 | 25 | namespace slam_toolbox 26 | { 27 | 28 | class LifelongSlamToolbox : public SlamToolbox 29 | { 30 | public: 31 | explicit LifelongSlamToolbox(rclcpp::NodeOptions options); 32 | ~LifelongSlamToolbox() {} 33 | CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 34 | 35 | // computation metrics 36 | double computeObjectiveScore( 37 | const double & intersect_over_union, const double & area_overlap, 38 | const double & reading_overlap, const int & num_constraints, 39 | const double & initial_score, const int & num_candidates) const; 40 | static double computeIntersect(LocalizedRangeScan * s1, LocalizedRangeScan * s2); 41 | static double computeIntersectOverUnion(LocalizedRangeScan * s1, LocalizedRangeScan * s2); 42 | static double computeAreaOverlapRatio( 43 | LocalizedRangeScan * ref_scan, 44 | LocalizedRangeScan * candidate_scan); 45 | static double computeReadingOverlapRatio( 46 | LocalizedRangeScan * ref_scan, 47 | LocalizedRangeScan * candidate_scan); 48 | static void computeIntersectBounds( 49 | LocalizedRangeScan * s1, LocalizedRangeScan * s2, double & x_l, 50 | double & x_u, double & y_l, double & y_u); 51 | 52 | protected: 53 | void laserCallback( 54 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; 55 | bool deserializePoseGraphCallback( 56 | const std::shared_ptr request_header, 57 | const std::shared_ptr req, 58 | std::shared_ptr resp) override; 59 | 60 | void evaluateNodeDepreciation(LocalizedRangeScan * range_scan); 61 | void removeFromSlamGraph(Vertex * vertex); 62 | double computeScore( 63 | LocalizedRangeScan * reference_scan, Vertex * candidate, 64 | const double & initial_score, const int & num_candidates); 65 | ScoredVertices computeScores(Vertices & near_scans, LocalizedRangeScan * range_scan); 66 | Vertices FindScansWithinRadius(LocalizedRangeScan * scan, const double & radius); 67 | void updateScoresSlamGraph(const double & score, Vertex * vertex); 68 | void checkIsNotNormalized(const double & value); 69 | 70 | bool use_tree_; 71 | double iou_thresh_; 72 | double removal_score_; 73 | double overlap_scale_; 74 | double constraint_scale_; 75 | double candidates_scale_; 76 | double iou_match_; 77 | double nearby_penalty_; 78 | }; 79 | 80 | } // namespace slam_toolbox 81 | 82 | #endif // SLAM_TOOLBOX__EXPERIMENTAL__SLAM_TOOLBOX_LIFELONG_HPP_ 83 | -------------------------------------------------------------------------------- /include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2022, Steve Macenski 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | #ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_ 18 | #define SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_ 19 | 20 | #include 21 | #include "slam_toolbox/slam_toolbox_localization.hpp" 22 | #include "std_srvs/srv/set_bool.hpp" 23 | 24 | namespace slam_toolbox 25 | { 26 | 27 | class MapAndLocalizationSlamToolbox : public LocalizationSlamToolbox 28 | { 29 | public: 30 | explicit MapAndLocalizationSlamToolbox(rclcpp::NodeOptions options); 31 | virtual ~MapAndLocalizationSlamToolbox() {} 32 | void loadPoseGraphByParams() override; 33 | CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 34 | CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 35 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 36 | 37 | protected: 38 | void laserCallback( 39 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; 40 | bool serializePoseGraphCallback( 41 | const std::shared_ptr request_header, 42 | const std::shared_ptr req, 43 | std::shared_ptr resp) override; 44 | bool deserializePoseGraphCallback( 45 | const std::shared_ptr request_header, 46 | const std::shared_ptr req, 47 | std::shared_ptr resp) override; 48 | bool setLocalizationModeCallback( 49 | const std::shared_ptr request_header, 50 | const std::shared_ptr req, 51 | std::shared_ptr resp); 52 | LocalizedRangeScan * addScan( 53 | LaserRangeFinder * laser, 54 | const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, 55 | Pose2 & pose) override; 56 | void toggleMode(bool enable_localization); 57 | 58 | std::shared_ptr> ssSetLocalizationMode_; 59 | }; 60 | 61 | } // namespace slam_toolbox 62 | 63 | #endif // SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_ 64 | -------------------------------------------------------------------------------- /include/slam_toolbox/get_pose_helper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * snap_utils 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ 20 | #define SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ 21 | 22 | #include 23 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 24 | #include "slam_toolbox/toolbox_types.hpp" 25 | #include "karto_sdk/Mapper.h" 26 | 27 | namespace pose_utils 28 | { 29 | 30 | // helper to get the robots position 31 | class GetPoseHelper 32 | { 33 | public: 34 | GetPoseHelper( 35 | tf2_ros::Buffer * tf, 36 | const std::string & base_frame, 37 | const std::string & odom_frame) 38 | : tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame) 39 | { 40 | } 41 | 42 | bool getOdomPose(karto::Pose2 & karto_pose, const rclcpp::Time & t) 43 | { 44 | geometry_msgs::msg::TransformStamped base_ident, odom_pose; 45 | base_ident.header.stamp = t; 46 | base_ident.header.frame_id = base_frame_; 47 | base_ident.transform.rotation.w = 1.0; 48 | 49 | try { 50 | odom_pose = tf_->transform(base_ident, odom_frame_); 51 | } catch (tf2::TransformException & e) { 52 | return false; 53 | } 54 | 55 | const double yaw = tf2::getYaw(odom_pose.transform.rotation); 56 | karto_pose = karto::Pose2(odom_pose.transform.translation.x, 57 | odom_pose.transform.translation.y, yaw); 58 | 59 | return true; 60 | } 61 | 62 | private: 63 | tf2_ros::Buffer * tf_; 64 | std::string base_frame_, odom_frame_; 65 | }; 66 | 67 | } // namespace pose_utils 68 | 69 | #endif // SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ 70 | -------------------------------------------------------------------------------- /include/slam_toolbox/laser_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * toolbox_types 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__LASER_UTILS_HPP_ 20 | #define SLAM_TOOLBOX__LASER_UTILS_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "rclcpp/rclcpp.hpp" 28 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 29 | #include "slam_toolbox/toolbox_types.hpp" 30 | #include "tf2/utils.h" 31 | 32 | namespace laser_utils 33 | { 34 | 35 | // Convert a laser scan to a vector of readings 36 | inline std::vector scanToReadings( 37 | const sensor_msgs::msg::LaserScan & scan, 38 | const bool & inverted) 39 | { 40 | std::vector readings; 41 | 42 | if (inverted) { 43 | for (std::vector::const_reverse_iterator it = scan.ranges.rbegin(); 44 | it != scan.ranges.rend(); ++it) 45 | { 46 | readings.push_back(*it); 47 | } 48 | } else { 49 | for (std::vector::const_iterator it = scan.ranges.begin(); it != scan.ranges.end(); 50 | ++it) 51 | { 52 | readings.push_back(*it); 53 | } 54 | } 55 | 56 | return readings; 57 | } 58 | 59 | // Store laser scanner information 60 | class LaserMetadata 61 | { 62 | public: 63 | LaserMetadata(); 64 | ~LaserMetadata(); 65 | LaserMetadata(karto::LaserRangeFinder * lsr, bool invert); 66 | bool isInverted() const; 67 | karto::LaserRangeFinder * getLaser(); 68 | void invertScan(sensor_msgs::msg::LaserScan & scan) const; 69 | 70 | private: 71 | karto::LaserRangeFinder * laser; 72 | bool inverted; 73 | }; 74 | 75 | // Help take a scan from a laser and create a laser object 76 | class LaserAssistant 77 | { 78 | public: 79 | template 80 | LaserAssistant( 81 | NodeT node, tf2_ros::Buffer * tf, 82 | const std::string & base_frame); 83 | ~LaserAssistant(); 84 | LaserMetadata toLaserMetadata(sensor_msgs::msg::LaserScan scan); 85 | 86 | private: 87 | karto::LaserRangeFinder * makeLaser(const double & mountingYaw); 88 | bool isInverted(double & mountingYaw); 89 | 90 | rclcpp::Logger logger_; 91 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_; 92 | tf2_ros::Buffer * tf_; 93 | sensor_msgs::msg::LaserScan scan_; 94 | std::string frame_, base_frame_; 95 | geometry_msgs::msg::TransformStamped laser_pose_; 96 | }; 97 | 98 | // Hold some scans and utilities around them 99 | class ScanHolder 100 | { 101 | public: 102 | explicit ScanHolder(std::map & lasers); 103 | ~ScanHolder(); 104 | sensor_msgs::msg::LaserScan getCorrectedScan(const int & id); 105 | void addScan(const sensor_msgs::msg::LaserScan scan); 106 | 107 | private: 108 | std::unique_ptr> current_scans_; 109 | std::map & lasers_; 110 | }; 111 | 112 | } // namespace laser_utils 113 | 114 | #endif // SLAM_TOOLBOX__LASER_UTILS_HPP_ 115 | -------------------------------------------------------------------------------- /include/slam_toolbox/loop_closure_assistant.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * loop_closure_assistant 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__LOOP_CLOSURE_ASSISTANT_HPP_ 20 | #define SLAM_TOOLBOX__LOOP_CLOSURE_ASSISTANT_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "tf2_ros/transform_broadcaster.h" 29 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 30 | #include "tf2/utils.h" 31 | #include "rclcpp/rclcpp.hpp" 32 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 33 | #include "interactive_markers/interactive_marker_server.hpp" 34 | #include "interactive_markers/menu_handler.hpp" 35 | 36 | #include "slam_toolbox/toolbox_types.hpp" 37 | #include "slam_toolbox/laser_utils.hpp" 38 | #include "slam_toolbox/visualization_utils.hpp" 39 | 40 | namespace loop_closure_assistant 41 | { 42 | 43 | using namespace ::toolbox_types; // NOLINT 44 | 45 | class LoopClosureAssistant 46 | { 47 | public: 48 | template 49 | LoopClosureAssistant( 50 | NodeT node, karto::Mapper * mapper, 51 | laser_utils::ScanHolder * scan_holder, PausedState & state, 52 | ProcessType & processor_type); 53 | 54 | void clearMovedNodes(); 55 | void processInteractiveFeedback( 56 | const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback); 57 | void publishGraph(); 58 | void setMapper(karto::Mapper * mapper); 59 | 60 | private: 61 | bool manualLoopClosureCallback( 62 | const std::shared_ptr request_header, 63 | const std::shared_ptr req, 64 | std::shared_ptr resp); 65 | bool clearChangesCallback( 66 | const std::shared_ptr request_header, 67 | const std::shared_ptr req, 68 | std::shared_ptr resp); 69 | bool interactiveModeCallback( 70 | const std::shared_ptr request_header, 71 | const std::shared_ptr req, 72 | std::shared_ptr resp); 73 | 74 | void moveNode(const int& id, const Eigen::Vector3d& pose); 75 | void addMovedNodes(const int& id, Eigen::Vector3d vec); 76 | 77 | std::unique_ptr tfB_; 78 | laser_utils::ScanHolder * scan_holder_; 79 | rclcpp::Publisher::SharedPtr marker_publisher_; 80 | rclcpp::Publisher::SharedPtr scan_publisher_; 81 | rclcpp::Service::SharedPtr ssClear_manual_; 82 | rclcpp::Service::SharedPtr ssLoopClosure_; 83 | rclcpp::Service::SharedPtr ssInteractive_; 84 | boost::mutex moved_nodes_mutex_; 85 | std::map moved_nodes_; 86 | karto::Mapper * mapper_; 87 | karto::ScanSolver * solver_; 88 | std::unique_ptr interactive_server_; 89 | boost::mutex interactive_mutex_; 90 | bool interactive_mode_, enable_interactive_mode_; 91 | std::string map_frame_; 92 | PausedState & state_; 93 | ProcessType & processor_type_; 94 | 95 | rclcpp::Clock::SharedPtr clock_; 96 | rclcpp::Logger logger_; 97 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_; 98 | }; 99 | 100 | } // namespace loop_closure_assistant 101 | 102 | #endif // SLAM_TOOLBOX__LOOP_CLOSURE_ASSISTANT_HPP_ 103 | -------------------------------------------------------------------------------- /include/slam_toolbox/map_saver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * map_saver 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__MAP_SAVER_HPP_ 20 | #define SLAM_TOOLBOX__MAP_SAVER_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include "rclcpp/rclcpp.hpp" 26 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 27 | #include "slam_toolbox/toolbox_msgs.hpp" 28 | 29 | namespace map_saver 30 | { 31 | 32 | // a service to save a map with a given name as requested 33 | class MapSaver 34 | { 35 | public: 36 | template 37 | MapSaver(NodeT node, const std::string & map_name); 38 | 39 | protected: 40 | bool saveMapCallback( 41 | const std::shared_ptr request_header, 42 | const std::shared_ptr request, 43 | std::shared_ptr response); 44 | 45 | private: 46 | rclcpp::Logger logger_; 47 | std::string namespace_str_; 48 | rclcpp::Service::SharedPtr server_; 49 | rclcpp::Subscription::SharedPtr sub_; 50 | std::string service_name_, map_name_; 51 | bool received_map_; 52 | }; 53 | 54 | } // namespace map_saver 55 | 56 | #endif // SLAM_TOOLBOX__MAP_SAVER_HPP_ 57 | -------------------------------------------------------------------------------- /include/slam_toolbox/merge_maps_kinematic.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Author 3 | * Copyright (c) 2018, Simbe Robotics, Inc. 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__MERGE_MAPS_KINEMATIC_HPP_ 20 | #define SLAM_TOOLBOX__MERGE_MAPS_KINEMATIC_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "rclcpp/rclcpp.hpp" 32 | #include "interactive_markers/interactive_marker_server.hpp" 33 | #include "interactive_markers/menu_handler.hpp" 34 | #include "tf2_ros/transform_broadcaster.h" 35 | #include "tf2_ros/transform_listener.h" 36 | #include "tf2_ros/message_filter.h" 37 | #include "tf2/LinearMath/Matrix3x3.h" 38 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 39 | #include "tf2/utils.h" 40 | 41 | #include "../lib/karto_sdk/include/karto_sdk/Mapper.h" 42 | #include "slam_toolbox/toolbox_types.hpp" 43 | #include "slam_toolbox/toolbox_msgs.hpp" 44 | #include "slam_toolbox/laser_utils.hpp" 45 | #include "slam_toolbox/visualization_utils.hpp" 46 | 47 | using namespace toolbox_types; // NOLINT 48 | using namespace karto; // NOLINT 49 | 50 | class MergeMapsKinematic : public rclcpp::Node 51 | { 52 | typedef std::vector::iterator LocalizedRangeScansVecIt; 53 | typedef karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt; 54 | 55 | public: 56 | MergeMapsKinematic(); 57 | ~MergeMapsKinematic(); 58 | // setup 59 | void configure(); 60 | 61 | private: 62 | // callback 63 | bool mergeMapCallback( 64 | const std::shared_ptr request_header, 65 | const std::shared_ptr req, 66 | std::shared_ptr resp); 67 | bool addSubmapCallback( 68 | const std::shared_ptr request_header, 69 | const std::shared_ptr req, 70 | std::shared_ptr resp); 71 | void processInteractiveFeedback( 72 | visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback); 73 | void kartoToROSOccupancyGrid( 74 | const karto::LocalizedRangeScanVector & scans, 75 | nav_msgs::srv::GetMap::Response & map); 76 | void transformScan(LocalizedRangeScansIt iter, tf2::Transform & submap_correction); 77 | 78 | // apply transformation to correct pose 79 | karto::Pose2 applyCorrection(const karto::Pose2 & pose, const tf2::Transform & submap_correction); 80 | karto::Vector2 applyCorrection( 81 | const karto::Vector2 & pose, 82 | const tf2::Transform & submap_correction); 83 | 84 | // ROS-y-ness 85 | std::vector>> sstmS_; 86 | std::vector>> sstS_; 87 | std::shared_ptr> ssMap_; 88 | std::shared_ptr> ssSubmap_; 89 | 90 | // karto bookkeeping 91 | std::map lasers_; 92 | std::vector> dataset_vec_; 93 | 94 | // TF 95 | std::unique_ptr tfB_; 96 | 97 | // visualization 98 | std::unique_ptr interactive_server_; 99 | 100 | // state 101 | std::map submap_locations_; 102 | std::vector scans_vec_; 103 | std::map submap_marker_transform_; 104 | std::map prev_submap_marker_transform_; 105 | double resolution_; 106 | int min_pass_through_; 107 | double occupancy_threshold_; 108 | int num_submaps_; 109 | }; 110 | 111 | #endif // SLAM_TOOLBOX__MERGE_MAPS_KINEMATIC_HPP_ 112 | -------------------------------------------------------------------------------- /include/slam_toolbox/serialization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Author 3 | * Copyright (c) 2018, Simbe Robotics, Inc. 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__SERIALIZATION_HPP_ 20 | #define SLAM_TOOLBOX__SERIALIZATION_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "rclcpp/rclcpp.hpp" 27 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 28 | #include "karto_sdk/Mapper.h" 29 | 30 | namespace serialization 31 | { 32 | 33 | inline bool fileExists(const std::string & name) 34 | { 35 | struct stat buffer; 36 | return stat(name.c_str(), &buffer) == 0; 37 | } 38 | 39 | template 40 | inline bool write( 41 | const std::string & filename, 42 | karto::Mapper & mapper, 43 | karto::Dataset & dataset, 44 | NodeT node) 45 | { 46 | try { 47 | mapper.SaveToFile(filename + std::string(".posegraph")); 48 | dataset.SaveToFile(filename + std::string(".data")); 49 | return true; 50 | } catch (boost::archive::archive_exception e) { 51 | RCLCPP_ERROR(node->get_logger(), 52 | "Failed to write file: Exception %s", e.what()); 53 | return false; 54 | } 55 | } 56 | 57 | template 58 | inline bool read( 59 | const std::string & filename, 60 | karto::Mapper & mapper, 61 | karto::Dataset & dataset, 62 | NodeT node) 63 | { 64 | if (!fileExists(filename + std::string(".posegraph"))) { 65 | RCLCPP_ERROR(node->get_logger(), 66 | "serialization::Read: Failed to open " 67 | "requested file: %s.", filename.c_str()); 68 | return false; 69 | } 70 | 71 | try { 72 | mapper.LoadFromFile(filename + std::string(".posegraph")); 73 | dataset.LoadFromFile(filename + std::string(".data")); 74 | return true; 75 | } catch (boost::archive::archive_exception e) { 76 | RCLCPP_ERROR(node->get_logger(), 77 | "serialization::Read: Failed to read file: " 78 | "Exception: %s", e.what()); 79 | } 80 | 81 | return false; 82 | } 83 | 84 | } // namespace serialization 85 | 86 | #endif // SLAM_TOOLBOX__SERIALIZATION_HPP_ 87 | -------------------------------------------------------------------------------- /include/slam_toolbox/slam_mapper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Author 3 | * Copyright (c) 2019 Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__SLAM_MAPPER_HPP_ 20 | #define SLAM_TOOLBOX__SLAM_MAPPER_HPP_ 21 | 22 | #include 23 | #include "geometry_msgs/msg/quaternion.hpp" 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 26 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 27 | #include "tf2/utils.h" 28 | #include "slam_toolbox/toolbox_types.hpp" 29 | 30 | namespace mapper_utils 31 | { 32 | 33 | using namespace ::karto; // NOLINT 34 | 35 | class SMapper 36 | { 37 | public: 38 | SMapper(); 39 | ~SMapper(); 40 | 41 | // get occupancy grid from scans 42 | karto::OccupancyGrid * getOccupancyGrid(const double & resolution); 43 | 44 | // convert Karto pose to TF pose 45 | tf2::Transform toTfPose(const karto::Pose2 & pose) const; 46 | 47 | // convert TF pose to karto pose 48 | karto::Pose2 toKartoPose(const tf2::Transform & pose) const; 49 | 50 | template 51 | void configure(const NodeT & node); 52 | 53 | void Reset(); 54 | 55 | // // processors 56 | // kt_bool ProcessAtDock(LocalizedRangeScan* pScan); 57 | // kt_bool ProcessAgainstNode(LocalizedRangeScan* pScan, const int& nodeId); 58 | // kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan); 59 | // kt_bool ProcessLocalization(LocalizedRangeScan* pScan); 60 | 61 | void setMapper(karto::Mapper * mapper); 62 | karto::Mapper * getMapper(); 63 | 64 | void clearLocalizationBuffer(); 65 | 66 | protected: 67 | std::unique_ptr mapper_; 68 | }; 69 | 70 | } // namespace mapper_utils 71 | 72 | #endif // SLAM_TOOLBOX__SLAM_MAPPER_HPP_ 73 | -------------------------------------------------------------------------------- /include/slam_toolbox/slam_toolbox_async.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_ASYNC_HPP_ 21 | #define SLAM_TOOLBOX__SLAM_TOOLBOX_ASYNC_HPP_ 22 | 23 | #include 24 | #include "slam_toolbox/slam_toolbox_common.hpp" 25 | 26 | namespace slam_toolbox 27 | { 28 | 29 | class AsynchronousSlamToolbox : public SlamToolbox 30 | { 31 | public: 32 | explicit AsynchronousSlamToolbox(rclcpp::NodeOptions options); 33 | ~AsynchronousSlamToolbox() {} 34 | 35 | protected: 36 | void laserCallback( 37 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; 38 | bool deserializePoseGraphCallback( 39 | const std::shared_ptr request_header, 40 | const std::shared_ptr req, 41 | std::shared_ptr resp) override; 42 | }; 43 | 44 | } // namespace slam_toolbox 45 | 46 | #endif // SLAM_TOOLBOX__SLAM_TOOLBOX_ASYNC_HPP_ 47 | -------------------------------------------------------------------------------- /include/slam_toolbox/slam_toolbox_common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 5 | * Copyright Work Modifications (c) 2019, Steve Macenski 6 | * 7 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 8 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 9 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 10 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 11 | * 12 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 13 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 14 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 15 | * CONDITIONS. 16 | * 17 | */ 18 | 19 | #ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ 20 | #define SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "lifecycle_msgs/msg/state.hpp" 33 | #include "rclcpp/rclcpp.hpp" 34 | #include "bondcpp/bond.hpp" 35 | #include "bond/msg/constants.hpp" 36 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 37 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp" 38 | #include "message_filters/subscriber.h" 39 | #include "tf2_ros/transform_broadcaster.h" 40 | #include "tf2_ros/transform_listener.h" 41 | #include "tf2_ros/create_timer_ros.h" 42 | #include "tf2_ros/message_filter.h" 43 | #include "tf2/LinearMath/Matrix3x3.h" 44 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 45 | #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" 46 | 47 | #include "pluginlib/class_loader.hpp" 48 | 49 | #include "slam_toolbox/toolbox_types.hpp" 50 | #include "slam_toolbox/slam_mapper.hpp" 51 | #include "slam_toolbox/snap_utils.hpp" 52 | #include "slam_toolbox/laser_utils.hpp" 53 | #include "slam_toolbox/get_pose_helper.hpp" 54 | #include "slam_toolbox/map_saver.hpp" 55 | #include "slam_toolbox/loop_closure_assistant.hpp" 56 | 57 | namespace slam_toolbox 58 | { 59 | 60 | // dirty, dirty cheat I love 61 | using namespace ::toolbox_types; // NOLINT 62 | using namespace ::karto; // NOLINT 63 | 64 | class SlamToolbox : public rclcpp_lifecycle::LifecycleNode 65 | { 66 | public: 67 | explicit SlamToolbox(rclcpp::NodeOptions); 68 | SlamToolbox(); 69 | virtual ~SlamToolbox(); 70 | virtual void loadPoseGraphByParams(); 71 | 72 | // Create bond connection for nav2 lifecycle manager 73 | void createBond(); 74 | // Destroy bond connection for nav2 lifecycle manager 75 | void destroyBond(); 76 | 77 | CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; 78 | CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; 79 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; 80 | CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; 81 | CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 82 | 83 | protected: 84 | // threads 85 | void publishVisualizations(); 86 | void publishTransformLoop(const double & transform_publish_period); 87 | 88 | // setup 89 | void setParams(); 90 | void setSolver(); 91 | void setROSInterfaces(); 92 | 93 | // callbacks 94 | virtual void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) = 0; 95 | bool mapCallback( 96 | const std::shared_ptr request_header, 97 | const std::shared_ptr req, 98 | std::shared_ptr res); 99 | virtual bool serializePoseGraphCallback( 100 | const std::shared_ptr request_header, 101 | const std::shared_ptr req, 102 | std::shared_ptr resp); 103 | virtual bool deserializePoseGraphCallback( 104 | const std::shared_ptr request_header, 105 | const std::shared_ptr req, 106 | std::shared_ptr resp); 107 | virtual bool resetCallback( 108 | const std::shared_ptr request_header, 109 | const std::shared_ptr req, 110 | std::shared_ptr resp); 111 | 112 | // Loaders 113 | void loadSerializedPoseGraph(std::unique_ptr &, std::unique_ptr &); 114 | 115 | // functional bits 116 | karto::LaserRangeFinder * getLaser(const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan); 117 | virtual karto::LocalizedRangeScan * addScan( 118 | karto::LaserRangeFinder * laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, 119 | karto::Pose2 & karto_pose); 120 | karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder * laser, PosedScan & scanWPose); 121 | bool updateMap(); 122 | tf2::Stamped setTransformFromPoses( 123 | const karto::Pose2 & pose, 124 | const karto::Pose2 & karto_pose, const rclcpp::Time & t, 125 | const bool & update_reprocessing_transform); 126 | karto::LocalizedRangeScan * getLocalizedRangeScan( 127 | karto::LaserRangeFinder * laser, 128 | const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, 129 | karto::Pose2 & karto_pose); 130 | bool shouldStartWithPoseGraph( 131 | std::string & filename, geometry_msgs::msg::Pose2D & pose, 132 | bool & start_at_dock); 133 | bool shouldProcessScan( 134 | const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, 135 | const karto::Pose2 & pose); 136 | void publishPose( 137 | const Pose2 & pose, 138 | const Matrix3 & cov, 139 | const rclcpp::Time & t); 140 | 141 | // pausing bits 142 | bool isPaused(const PausedApplication & app); 143 | bool pauseNewMeasurementsCallback( 144 | const std::shared_ptr request_header, 145 | const std::shared_ptr req, 146 | std::shared_ptr resp); 147 | 148 | // ROS-y-ness 149 | std::unique_ptr tf_; 150 | std::unique_ptr tfL_; 151 | std::unique_ptr tfB_; 152 | std::unique_ptr> scan_filter_sub_; 154 | std::unique_ptr> scan_filter_; 155 | std::shared_ptr> sst_; 156 | std::shared_ptr> sstm_; 157 | std::shared_ptr> pose_pub_; 159 | std::shared_ptr> ssMap_; 160 | std::shared_ptr> ssPauseMeasurements_; 161 | std::shared_ptr> ssSerialize_; 162 | std::shared_ptr> ssDesserialize_; 163 | std::shared_ptr> ssReset_; 164 | 165 | // Storage for ROS parameters 166 | std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; 167 | bool use_map_saver_; 168 | bool use_lifecycle_manager_; 169 | rclcpp::Duration transform_timeout_, minimum_time_interval_; 170 | std_msgs::msg::Header scan_header; 171 | int throttle_scans_, scan_queue_size_; 172 | 173 | double resolution_; 174 | double position_covariance_scale_; 175 | double yaw_covariance_scale_; 176 | bool first_measurement_, enable_interactive_mode_; 177 | 178 | // Book keeping 179 | std::unique_ptr smapper_; 180 | std::unique_ptr dataset_; 181 | std::map lasers_; 182 | 183 | // helpers 184 | std::unique_ptr laser_assistant_; 185 | std::unique_ptr pose_helper_; 186 | std::unique_ptr map_saver_; 187 | std::unique_ptr closure_assistant_; 188 | std::unique_ptr scan_holder_; 189 | 190 | // Internal state 191 | std::vector> threads_; 192 | tf2::Transform map_to_odom_; 193 | boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_; 194 | PausedState state_; 195 | nav_msgs::srv::GetMap::Response map_; 196 | ProcessType processor_type_; 197 | std::unique_ptr process_near_pose_; 198 | tf2::Transform reprocessing_transform_; 199 | 200 | // pluginlib 201 | pluginlib::ClassLoader solver_loader_; 202 | std::shared_ptr solver_; 203 | 204 | // Connection to tell that server is still up 205 | std::unique_ptr bond_{nullptr}; 206 | }; 207 | 208 | } // namespace slam_toolbox 209 | 210 | #endif // SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ 211 | -------------------------------------------------------------------------------- /include/slam_toolbox/slam_toolbox_localization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2019, Steve Macenski 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_LOCALIZATION_HPP_ 20 | #define SLAM_TOOLBOX__SLAM_TOOLBOX_LOCALIZATION_HPP_ 21 | 22 | #include 23 | #include "slam_toolbox/slam_toolbox_common.hpp" 24 | #include "std_srvs/srv/empty.hpp" 25 | 26 | namespace slam_toolbox 27 | { 28 | 29 | class LocalizationSlamToolbox : public SlamToolbox 30 | { 31 | public: 32 | explicit LocalizationSlamToolbox(rclcpp::NodeOptions options); 33 | virtual ~LocalizationSlamToolbox() {} 34 | virtual void loadPoseGraphByParams(); 35 | CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 36 | CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 37 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; 38 | 39 | protected: 40 | virtual void laserCallback( 41 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; 42 | void localizePoseCallback( 43 | const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); 44 | bool clearLocalizationBuffer( 45 | const std::shared_ptr request_header, 46 | const std::shared_ptr req, 47 | std::shared_ptr resp); 48 | 49 | virtual bool serializePoseGraphCallback( 50 | const std::shared_ptr request_header, 51 | const std::shared_ptr req, 52 | std::shared_ptr resp) override; 53 | virtual bool deserializePoseGraphCallback( 54 | const std::shared_ptr request_header, 55 | const std::shared_ptr req, 56 | std::shared_ptr resp) override; 57 | 58 | virtual LocalizedRangeScan * addScan( 59 | LaserRangeFinder * laser, 60 | const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, 61 | Pose2 & pose) override; 62 | 63 | std::shared_ptr> 64 | localization_pose_sub_; 65 | std::shared_ptr > clear_localization_; 66 | }; 67 | 68 | } // namespace slam_toolbox 69 | 70 | #endif // SLAM_TOOLBOX__SLAM_TOOLBOX_LOCALIZATION_HPP_ 71 | -------------------------------------------------------------------------------- /include/slam_toolbox/slam_toolbox_sync.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_SYNC_HPP_ 21 | #define SLAM_TOOLBOX__SLAM_TOOLBOX_SYNC_HPP_ 22 | 23 | #include 24 | #include 25 | #include "slam_toolbox/slam_toolbox_common.hpp" 26 | 27 | namespace slam_toolbox 28 | { 29 | 30 | class SynchronousSlamToolbox : public SlamToolbox 31 | { 32 | public: 33 | explicit SynchronousSlamToolbox(rclcpp::NodeOptions options); 34 | ~SynchronousSlamToolbox() {} 35 | void run(); 36 | CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 37 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 38 | 39 | protected: 40 | void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; 41 | bool clearQueueCallback( 42 | const std::shared_ptr request_header, 43 | const std::shared_ptr req, 44 | std::shared_ptr resp); 45 | bool deserializePoseGraphCallback( 46 | const std::shared_ptr request_header, 47 | const std::shared_ptr req, 48 | std::shared_ptr resp) override; 49 | bool resetCallback( 50 | const std::shared_ptr request_header, 51 | const std::shared_ptr req, 52 | std::shared_ptr resp) override; 53 | 54 | std::queue q_; 55 | std::shared_ptr> ssClear_; 56 | boost::mutex q_mutex_; 57 | }; 58 | 59 | } // namespace slam_toolbox 60 | 61 | #endif // SLAM_TOOLBOX__SLAM_TOOLBOX_SYNC_HPP_ 62 | -------------------------------------------------------------------------------- /include/slam_toolbox/snap_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * snap_utils 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__SNAP_UTILS_HPP_ 20 | #define SLAM_TOOLBOX__SNAP_UTILS_HPP_ 21 | 22 | #include 23 | 24 | namespace snap_utils 25 | { 26 | 27 | // whether this is running in a snap container 28 | inline bool isInSnap() 29 | { 30 | char * snap_common = getenv("SNAP_COMMON"); 31 | if (snap_common != NULL) { 32 | return true; 33 | } 34 | return false; 35 | } 36 | 37 | // get path of shared space 38 | inline std::string getSnapPath() 39 | { 40 | char * snap_common = getenv("SNAP_COMMON"); 41 | return std::string(snap_common); 42 | } 43 | 44 | } // namespace snap_utils 45 | 46 | #endif // SLAM_TOOLBOX__SNAP_UTILS_HPP_ 47 | -------------------------------------------------------------------------------- /include/slam_toolbox/toolbox_msgs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2019, Steve Macenski 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ 20 | #define SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ 21 | 22 | #include "nav_msgs/msg/map_meta_data.hpp" 23 | #include "sensor_msgs/msg/laser_scan.hpp" 24 | #include "nav_msgs/srv/get_map.hpp" 25 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 26 | 27 | #include "visualization_msgs/msg/marker_array.hpp" 28 | #include "visualization_msgs/msg/interactive_marker.hpp" 29 | #include "visualization_msgs/msg/interactive_marker_control.hpp" 30 | #include "visualization_msgs/msg/interactive_marker_feedback.hpp" 31 | 32 | #include "slam_toolbox/srv/pause.hpp" 33 | #include "slam_toolbox/srv/reset.hpp" 34 | #include "slam_toolbox/srv/clear_queue.hpp" 35 | #include "slam_toolbox/srv/toggle_interactive.hpp" 36 | #include "slam_toolbox/srv/clear.hpp" 37 | #include "slam_toolbox/srv/save_map.hpp" 38 | #include "slam_toolbox/srv/loop_closure.hpp" 39 | #include "slam_toolbox/srv/serialize_pose_graph.hpp" 40 | #include "slam_toolbox/srv/deserialize_pose_graph.hpp" 41 | #include "slam_toolbox/srv/merge_maps.hpp" 42 | #include "slam_toolbox/srv/add_submap.hpp" 43 | 44 | #endif // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ 45 | -------------------------------------------------------------------------------- /include/slam_toolbox/toolbox_types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * toolbox_types 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__TOOLBOX_TYPES_HPP_ 20 | #define SLAM_TOOLBOX__TOOLBOX_TYPES_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 27 | #include "tf2_ros/buffer.h" 28 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 29 | #include "tf2/transform_datatypes.h" 30 | 31 | #include "karto_sdk/Mapper.h" 32 | #include "slam_toolbox/toolbox_msgs.hpp" 33 | 34 | // compute linear index for given map coords 35 | #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 36 | 37 | namespace toolbox_types 38 | { 39 | 40 | // object containing a scan pointer and a position 41 | struct PosedScan 42 | { 43 | PosedScan(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_in, karto::Pose2 pose_in) 44 | : scan(scan_in), pose(pose_in) 45 | { 46 | } 47 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan; 48 | karto::Pose2 pose; 49 | }; 50 | 51 | // object containing a vertex pointer and an updated score 52 | struct ScoredVertex 53 | { 54 | ScoredVertex(karto::Vertex * vertex, double score) 55 | : vertex_(vertex), score_(score) 56 | { 57 | } 58 | 59 | double GetScore() 60 | { 61 | return score_; 62 | } 63 | 64 | karto::Vertex * GetVertex() 65 | { 66 | return vertex_; 67 | } 68 | 69 | karto::Vertex * vertex_; 70 | double score_; 71 | }; 72 | 73 | typedef std::vector ScoredVertices; 74 | typedef std::vector *> Vertices; 75 | 76 | // types of pause functionality available 77 | enum PausedApplication 78 | { 79 | PROCESSING = 0, 80 | VISUALIZING_GRAPH = 1, 81 | NEW_MEASUREMENTS = 2 82 | }; 83 | 84 | // types of sensor processing 85 | enum ProcessType 86 | { 87 | PROCESS = 0, 88 | PROCESS_FIRST_NODE = 1, 89 | PROCESS_NEAR_REGION = 2, 90 | PROCESS_LOCALIZATION = 3 91 | }; 92 | 93 | // Pause state 94 | struct PausedState 95 | { 96 | PausedState() 97 | { 98 | state_map_[NEW_MEASUREMENTS] = false; 99 | state_map_[VISUALIZING_GRAPH] = false; 100 | state_map_[PROCESSING] = false; 101 | } 102 | 103 | void set(const PausedApplication & app, const bool & state) 104 | { 105 | boost::mutex::scoped_lock lock(pause_mutex_); 106 | state_map_[app] = state; 107 | } 108 | 109 | bool get(const PausedApplication & app) 110 | { 111 | boost::mutex::scoped_lock lock(pause_mutex_); 112 | return state_map_[app]; 113 | } 114 | 115 | std::map state_map_; 116 | boost::mutex pause_mutex_; 117 | }; 118 | 119 | typedef std::map *>> VerticeMap; 120 | typedef std::vector *> EdgeVector; 121 | typedef std::map *> ScanMap; 122 | typedef std::vector *> ScanVector; 123 | typedef slam_toolbox::srv::DeserializePoseGraph::Request procType; 124 | 125 | typedef std::unordered_map::iterator GraphIterator; 126 | typedef std::unordered_map::const_iterator ConstGraphIterator; 127 | 128 | typedef rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CallbackReturn; 129 | 130 | } // namespace toolbox_types 131 | 132 | #endif // SLAM_TOOLBOX__TOOLBOX_TYPES_HPP_ 133 | -------------------------------------------------------------------------------- /include/slam_toolbox/visualization_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * visualization_utils 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX__VISUALIZATION_UTILS_HPP_ 20 | #define SLAM_TOOLBOX__VISUALIZATION_UTILS_HPP_ 21 | 22 | #include 23 | #include 24 | 25 | #include "rclcpp/rclcpp.hpp" 26 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 27 | #include "visualization_msgs/msg/marker.hpp" 28 | #include "visualization_msgs/msg/interactive_marker.hpp" 29 | #include "visualization_msgs/msg/interactive_marker_feedback.hpp" 30 | 31 | namespace vis_utils 32 | { 33 | 34 | template 35 | inline visualization_msgs::msg::Marker toMarker( 36 | const std::string & frame, 37 | const std::string & ns, 38 | const double & scale, 39 | NodeT node) 40 | { 41 | visualization_msgs::msg::Marker marker; 42 | 43 | marker.header.frame_id = frame; 44 | marker.header.stamp = node->now(); 45 | marker.ns = ns; 46 | marker.type = visualization_msgs::msg::Marker::SPHERE; 47 | marker.pose.position.z = 0.0; 48 | marker.pose.orientation.w = 1.; 49 | marker.scale.x = scale; 50 | marker.scale.y = scale; 51 | marker.scale.z = scale; 52 | marker.color.r = 1.0; 53 | marker.color.g = 0; 54 | marker.color.b = 0.0; 55 | marker.color.a = 1.; 56 | marker.action = visualization_msgs::msg::Marker::ADD; 57 | marker.lifetime = rclcpp::Duration::from_seconds(0); 58 | 59 | return marker; 60 | } 61 | 62 | template 63 | inline visualization_msgs::msg::InteractiveMarker toInteractiveMarker( 64 | visualization_msgs::msg::Marker & marker, 65 | const double & scale, 66 | NodeT node) 67 | { 68 | // marker basics 69 | visualization_msgs::msg::InteractiveMarker int_marker; 70 | int_marker.header.frame_id = marker.header.frame_id; 71 | int_marker.header.stamp = node->now(); 72 | int_marker.name = std::to_string(marker.id); 73 | int_marker.pose.orientation.w = 1.; 74 | int_marker.pose.position.x = marker.pose.position.x; 75 | int_marker.pose.position.y = marker.pose.position.y; 76 | int_marker.scale = scale; 77 | 78 | // translate control 79 | visualization_msgs::msg::InteractiveMarkerControl control; 80 | control.orientation_mode = 81 | visualization_msgs::msg::InteractiveMarkerControl::FIXED; 82 | control.always_visible = true; 83 | control.orientation.w = 0; 84 | control.orientation.x = 0.7071; 85 | control.orientation.y = 0; 86 | control.orientation.z = 0.7071; 87 | control.interaction_mode = 88 | visualization_msgs::msg::InteractiveMarkerControl::MOVE_PLANE; 89 | control.markers.push_back( marker ); 90 | int_marker.controls.push_back( control ); 91 | 92 | // rotate control 93 | visualization_msgs::msg::InteractiveMarkerControl control_rot; 94 | control_rot.orientation_mode = 95 | visualization_msgs::msg::InteractiveMarkerControl::FIXED; 96 | control_rot.always_visible = true; 97 | control_rot.orientation.w = 0; 98 | control_rot.orientation.x = 0.7071; 99 | control_rot.orientation.y = 0; 100 | control_rot.orientation.z = 0.7071; 101 | control_rot.interaction_mode = 102 | visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; 103 | int_marker.controls.push_back(control_rot); 104 | 105 | return int_marker; 106 | } 107 | 108 | inline void toNavMap( 109 | const karto::OccupancyGrid * occ_grid, 110 | nav_msgs::msg::OccupancyGrid & map) 111 | { 112 | // Translate to ROS format 113 | kt_int32s width = occ_grid->GetWidth(); 114 | kt_int32s height = occ_grid->GetHeight(); 115 | karto::Vector2 offset = 116 | occ_grid->GetCoordinateConverter()->GetOffset(); 117 | 118 | if (map.info.width != (unsigned int) width || 119 | map.info.height != (unsigned int) height || 120 | map.info.origin.position.x != offset.GetX() || 121 | map.info.origin.position.y != offset.GetY()) 122 | { 123 | map.info.origin.position.x = offset.GetX(); 124 | map.info.origin.position.y = offset.GetY(); 125 | map.info.width = width; 126 | map.info.height = height; 127 | map.data.resize(map.info.width * map.info.height); 128 | } 129 | 130 | for (kt_int32s y = 0; y < height; y++) { 131 | for (kt_int32s x = 0; x < width; x++) { 132 | kt_int8u value = occ_grid->GetValue(karto::Vector2(x, y)); 133 | switch (value) { 134 | case karto::GridStates_Unknown: 135 | map.data[MAP_IDX(map.info.width, x, y)] = -1; 136 | break; 137 | case karto::GridStates_Occupied: 138 | map.data[MAP_IDX(map.info.width, x, y)] = 100; 139 | break; 140 | case karto::GridStates_Free: 141 | map.data[MAP_IDX(map.info.width, x, y)] = 0; 142 | break; 143 | } 144 | } 145 | } 146 | } 147 | 148 | } // namespace vis_utils 149 | 150 | #endif // SLAM_TOOLBOX__VISUALIZATION_UTILS_HPP_ 151 | -------------------------------------------------------------------------------- /launch/lifelong_launch.py: -------------------------------------------------------------------------------- 1 | from ament_index_python.packages import get_package_share_directory 2 | from launch import LaunchDescription 3 | from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, 4 | RegisterEventHandler) 5 | from launch.conditions import IfCondition 6 | from launch.events import matches_action 7 | from launch.substitutions import (AndSubstitution, LaunchConfiguration, 8 | NotSubstitution) 9 | from launch_ros.actions import LifecycleNode 10 | from launch_ros.event_handlers import OnStateTransition 11 | from launch_ros.events.lifecycle import ChangeState 12 | from lifecycle_msgs.msg import Transition 13 | 14 | 15 | def generate_launch_description(): 16 | autostart = LaunchConfiguration('autostart') 17 | use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager") 18 | 19 | declare_autostart_cmd = DeclareLaunchArgument( 20 | 'autostart', default_value='true', 21 | description='Automatically startup the slamtoolbox. ' 22 | 'Ignored when use_lifecycle_manager is true.') 23 | declare_use_lifecycle_manager = DeclareLaunchArgument( 24 | 'use_lifecycle_manager', default_value='false', 25 | description='Enable bond connection during node activation') 26 | 27 | start_lifelong_slam_toolbox_node = LifecycleNode( 28 | parameters=[ 29 | get_package_share_directory("slam_toolbox") + '/config/mapper_params_lifelong.yaml', 30 | {'use_lifecycle_manager': use_lifecycle_manager} 31 | ], 32 | package='slam_toolbox', 33 | executable='lifelong_slam_toolbox_node', 34 | name='slam_toolbox', 35 | output='screen', 36 | namespace='' 37 | ) 38 | 39 | configure_event = EmitEvent( 40 | event=ChangeState( 41 | lifecycle_node_matcher=matches_action(start_lifelong_slam_toolbox_node), 42 | transition_id=Transition.TRANSITION_CONFIGURE 43 | ), 44 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 45 | ) 46 | 47 | activate_event = RegisterEventHandler( 48 | OnStateTransition( 49 | target_lifecycle_node=start_lifelong_slam_toolbox_node, 50 | start_state="configuring", 51 | goal_state="inactive", 52 | entities=[ 53 | LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), 54 | EmitEvent(event=ChangeState( 55 | lifecycle_node_matcher=matches_action(start_lifelong_slam_toolbox_node), 56 | transition_id=Transition.TRANSITION_ACTIVATE 57 | )) 58 | ] 59 | ), 60 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 61 | ) 62 | 63 | ld = LaunchDescription() 64 | 65 | ld.add_action(declare_autostart_cmd) 66 | ld.add_action(declare_use_lifecycle_manager) 67 | ld.add_action(start_lifelong_slam_toolbox_node) 68 | ld.add_action(configure_event) 69 | ld.add_action(activate_event) 70 | 71 | return ld 72 | -------------------------------------------------------------------------------- /launch/localization_launch.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | from launch import LaunchDescription 6 | from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, 7 | RegisterEventHandler) 8 | from launch.conditions import IfCondition 9 | from launch.events import matches_action 10 | from launch.substitutions import (AndSubstitution, LaunchConfiguration, 11 | NotSubstitution) 12 | from launch_ros.actions import LifecycleNode 13 | from launch_ros.event_handlers import OnStateTransition 14 | from launch_ros.events.lifecycle import ChangeState 15 | from lifecycle_msgs.msg import Transition 16 | 17 | 18 | def generate_launch_description(): 19 | use_sim_time = LaunchConfiguration('use_sim_time') 20 | use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager") 21 | slam_params_file = LaunchConfiguration('slam_params_file') 22 | autostart = LaunchConfiguration('autostart') 23 | 24 | declare_use_sim_time_argument = DeclareLaunchArgument( 25 | 'use_sim_time', 26 | default_value='true', 27 | description='Use simulation/Gazebo clock') 28 | declare_slam_params_file_cmd = DeclareLaunchArgument( 29 | 'slam_params_file', 30 | default_value=os.path.join(get_package_share_directory("slam_toolbox"), 31 | 'config', 'mapper_params_localization.yaml'), 32 | description='Full path to the ROS2 parameters file to use for the slam_toolbox node') 33 | declare_autostart_cmd = DeclareLaunchArgument( 34 | 'autostart', default_value='true', 35 | description='Automatically startup the slamtoolbox. ' 36 | 'Ignored when use_lifecycle_manager is true.') 37 | declare_use_lifecycle_manager = DeclareLaunchArgument( 38 | 'use_lifecycle_manager', default_value='false', 39 | description='Enable bond connection during node activation') 40 | 41 | start_localization_slam_toolbox_node = LifecycleNode( 42 | parameters=[ 43 | slam_params_file, 44 | { 45 | 'use_lifecycle_manager': use_lifecycle_manager, 46 | 'use_sim_time': use_sim_time 47 | } 48 | ], 49 | package='slam_toolbox', 50 | executable='localization_slam_toolbox_node', 51 | name='slam_toolbox', 52 | output='screen', 53 | namespace='' 54 | ) 55 | 56 | configure_event = EmitEvent( 57 | event=ChangeState( 58 | lifecycle_node_matcher=matches_action(start_localization_slam_toolbox_node), 59 | transition_id=Transition.TRANSITION_CONFIGURE 60 | ), 61 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 62 | ) 63 | 64 | activate_event = RegisterEventHandler( 65 | OnStateTransition( 66 | target_lifecycle_node=start_localization_slam_toolbox_node, 67 | start_state="configuring", 68 | goal_state="inactive", 69 | entities=[ 70 | LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), 71 | EmitEvent(event=ChangeState( 72 | lifecycle_node_matcher=matches_action(start_localization_slam_toolbox_node), 73 | transition_id=Transition.TRANSITION_ACTIVATE 74 | )) 75 | ] 76 | ), 77 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 78 | ) 79 | 80 | ld = LaunchDescription() 81 | 82 | ld.add_action(declare_use_sim_time_argument) 83 | ld.add_action(declare_slam_params_file_cmd) 84 | ld.add_action(declare_autostart_cmd) 85 | ld.add_action(declare_use_lifecycle_manager) 86 | ld.add_action(start_localization_slam_toolbox_node) 87 | ld.add_action(configure_event) 88 | ld.add_action(activate_event) 89 | 90 | return ld 91 | -------------------------------------------------------------------------------- /launch/merge_maps_kinematic_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | import launch_ros.actions 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription([ 7 | launch_ros.actions.Node( 8 | package='slam_toolbox', 9 | executable='merge_maps_kinematic', 10 | name='slam_toolbox', 11 | output='screen' 12 | ) 13 | ]) 14 | -------------------------------------------------------------------------------- /launch/offline_launch.py: -------------------------------------------------------------------------------- 1 | from ament_index_python.packages import get_package_share_directory 2 | from launch import LaunchDescription 3 | from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, 4 | RegisterEventHandler) 5 | from launch.conditions import IfCondition 6 | from launch.events import matches_action 7 | from launch.substitutions import (AndSubstitution, LaunchConfiguration, 8 | NotSubstitution) 9 | from launch_ros.actions import LifecycleNode 10 | from launch_ros.event_handlers import OnStateTransition 11 | from launch_ros.events.lifecycle import ChangeState 12 | from lifecycle_msgs.msg import Transition 13 | 14 | 15 | def generate_launch_description(): 16 | autostart = LaunchConfiguration('autostart') 17 | use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager") 18 | 19 | declare_autostart_cmd = DeclareLaunchArgument( 20 | 'autostart', default_value='true', 21 | description='Automatically startup the slamtoolbox. ' 22 | 'Ignored when use_lifecycle_manager is true.') 23 | declare_use_lifecycle_manager = DeclareLaunchArgument( 24 | 'use_lifecycle_manager', default_value='false', 25 | description='Enable bond connection during node activation') 26 | 27 | start_sync_slam_toolbox_node = LifecycleNode( 28 | parameters=[ 29 | get_package_share_directory("slam_toolbox") + '/config/mapper_params_offline.yaml', 30 | {'use_lifecycle_manager': use_lifecycle_manager} 31 | ], 32 | package='slam_toolbox', 33 | executable='sync_slam_toolbox_node', 34 | name='slam_toolbox', 35 | output='screen', 36 | namespace='' 37 | ) 38 | 39 | configure_event = EmitEvent( 40 | event=ChangeState( 41 | lifecycle_node_matcher=matches_action(start_sync_slam_toolbox_node), 42 | transition_id=Transition.TRANSITION_CONFIGURE 43 | ), 44 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 45 | ) 46 | 47 | activate_event = RegisterEventHandler( 48 | OnStateTransition( 49 | target_lifecycle_node=start_sync_slam_toolbox_node, 50 | start_state="configuring", 51 | goal_state="inactive", 52 | entities=[ 53 | LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), 54 | EmitEvent(event=ChangeState( 55 | lifecycle_node_matcher=matches_action(start_sync_slam_toolbox_node), 56 | transition_id=Transition.TRANSITION_ACTIVATE 57 | )) 58 | ] 59 | ), 60 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 61 | ) 62 | 63 | ld = LaunchDescription() 64 | 65 | ld.add_action(declare_autostart_cmd) 66 | ld.add_action(declare_use_lifecycle_manager) 67 | ld.add_action(start_sync_slam_toolbox_node) 68 | ld.add_action(configure_event) 69 | ld.add_action(activate_event) 70 | 71 | return ld 72 | -------------------------------------------------------------------------------- /launch/online_async_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, 6 | RegisterEventHandler) 7 | from launch.conditions import IfCondition 8 | from launch.events import matches_action 9 | from launch.substitutions import (AndSubstitution, LaunchConfiguration, 10 | NotSubstitution) 11 | from launch_ros.actions import LifecycleNode 12 | from launch_ros.event_handlers import OnStateTransition 13 | from launch_ros.events.lifecycle import ChangeState 14 | from lifecycle_msgs.msg import Transition 15 | 16 | 17 | def generate_launch_description(): 18 | autostart = LaunchConfiguration('autostart') 19 | use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager") 20 | use_sim_time = LaunchConfiguration('use_sim_time') 21 | slam_params_file = LaunchConfiguration('slam_params_file') 22 | 23 | declare_autostart_cmd = DeclareLaunchArgument( 24 | 'autostart', default_value='true', 25 | description='Automatically startup the slamtoolbox. ' 26 | 'Ignored when use_lifecycle_manager is true.') 27 | declare_use_lifecycle_manager = DeclareLaunchArgument( 28 | 'use_lifecycle_manager', default_value='false', 29 | description='Enable bond connection during node activation') 30 | declare_use_sim_time_argument = DeclareLaunchArgument( 31 | 'use_sim_time', 32 | default_value='true', 33 | description='Use simulation/Gazebo clock') 34 | declare_slam_params_file_cmd = DeclareLaunchArgument( 35 | 'slam_params_file', 36 | default_value=os.path.join(get_package_share_directory("slam_toolbox"), 37 | 'config', 'mapper_params_online_async.yaml'), 38 | description='Full path to the ROS2 parameters file to use for the slam_toolbox node') 39 | 40 | start_async_slam_toolbox_node = LifecycleNode( 41 | parameters=[ 42 | slam_params_file, 43 | { 44 | 'use_lifecycle_manager': use_lifecycle_manager, 45 | 'use_sim_time': use_sim_time 46 | } 47 | ], 48 | package='slam_toolbox', 49 | executable='async_slam_toolbox_node', 50 | name='slam_toolbox', 51 | output='screen', 52 | namespace='' 53 | ) 54 | 55 | configure_event = EmitEvent( 56 | event=ChangeState( 57 | lifecycle_node_matcher=matches_action(start_async_slam_toolbox_node), 58 | transition_id=Transition.TRANSITION_CONFIGURE 59 | ), 60 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 61 | ) 62 | 63 | activate_event = RegisterEventHandler( 64 | OnStateTransition( 65 | target_lifecycle_node=start_async_slam_toolbox_node, 66 | start_state="configuring", 67 | goal_state="inactive", 68 | entities=[ 69 | LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), 70 | EmitEvent(event=ChangeState( 71 | lifecycle_node_matcher=matches_action(start_async_slam_toolbox_node), 72 | transition_id=Transition.TRANSITION_ACTIVATE 73 | )) 74 | ] 75 | ), 76 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 77 | ) 78 | 79 | ld = LaunchDescription() 80 | 81 | ld.add_action(declare_autostart_cmd) 82 | ld.add_action(declare_use_lifecycle_manager) 83 | ld.add_action(declare_use_sim_time_argument) 84 | ld.add_action(declare_slam_params_file_cmd) 85 | ld.add_action(start_async_slam_toolbox_node) 86 | ld.add_action(configure_event) 87 | ld.add_action(activate_event) 88 | 89 | return ld 90 | -------------------------------------------------------------------------------- /launch/online_sync_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, 6 | RegisterEventHandler) 7 | from launch.conditions import IfCondition 8 | from launch.events import matches_action 9 | from launch.substitutions import (AndSubstitution, LaunchConfiguration, 10 | NotSubstitution) 11 | from launch_ros.actions import LifecycleNode 12 | from launch_ros.event_handlers import OnStateTransition 13 | from launch_ros.events.lifecycle import ChangeState 14 | from lifecycle_msgs.msg import Transition 15 | 16 | 17 | def generate_launch_description(): 18 | autostart = LaunchConfiguration('autostart') 19 | use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager") 20 | use_sim_time = LaunchConfiguration('use_sim_time') 21 | slam_params_file = LaunchConfiguration('slam_params_file') 22 | 23 | declare_autostart_cmd = DeclareLaunchArgument( 24 | 'autostart', default_value='true', 25 | description='Automatically startup the slamtoolbox. ' 26 | 'Ignored when use_lifecycle_manager is true.') 27 | declare_use_lifecycle_manager = DeclareLaunchArgument( 28 | 'use_lifecycle_manager', default_value='false', 29 | description='Enable bond connection during node activation') 30 | declare_use_sim_time_argument = DeclareLaunchArgument( 31 | 'use_sim_time', 32 | default_value='true', 33 | description='Use simulation/Gazebo clock') 34 | declare_slam_params_file_cmd = DeclareLaunchArgument( 35 | 'slam_params_file', 36 | default_value=os.path.join(get_package_share_directory("slam_toolbox"), 37 | 'config', 'mapper_params_online_sync.yaml'), 38 | description='Full path to the ROS2 parameters file to use for the slam_toolbox node') 39 | 40 | start_sync_slam_toolbox_node = LifecycleNode( 41 | parameters=[ 42 | slam_params_file, 43 | { 44 | 'use_lifecycle_manager': use_lifecycle_manager, 45 | 'use_sim_time': use_sim_time 46 | } 47 | ], 48 | package='slam_toolbox', 49 | executable='sync_slam_toolbox_node', 50 | name='slam_toolbox', 51 | output='screen', 52 | namespace='' 53 | ) 54 | 55 | configure_event = EmitEvent( 56 | event=ChangeState( 57 | lifecycle_node_matcher=matches_action(start_sync_slam_toolbox_node), 58 | transition_id=Transition.TRANSITION_CONFIGURE 59 | ), 60 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 61 | ) 62 | 63 | activate_event = RegisterEventHandler( 64 | OnStateTransition( 65 | target_lifecycle_node=start_sync_slam_toolbox_node, 66 | start_state="configuring", 67 | goal_state="inactive", 68 | entities=[ 69 | LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), 70 | EmitEvent(event=ChangeState( 71 | lifecycle_node_matcher=matches_action(start_sync_slam_toolbox_node), 72 | transition_id=Transition.TRANSITION_ACTIVATE 73 | )) 74 | ] 75 | ), 76 | condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) 77 | ) 78 | 79 | ld = LaunchDescription() 80 | 81 | ld.add_action(declare_autostart_cmd) 82 | ld.add_action(declare_use_lifecycle_manager) 83 | ld.add_action(declare_use_sim_time_argument) 84 | ld.add_action(declare_slam_params_file_cmd) 85 | ld.add_action(start_sync_slam_toolbox_node) 86 | ld.add_action(configure_event) 87 | ld.add_action(activate_event) 88 | 89 | return ld 90 | -------------------------------------------------------------------------------- /lib/karto_sdk/Authors: -------------------------------------------------------------------------------- 1 | Karto Open Source Library 1.0 2 | 3 | Contributors: 4 | ------------------------------- 5 | Michael A. Eriksen (eriksen@ai.sri.com) 6 | Benson Limketkai (bensonl@ai.sri.com) 7 | Steven Macenski (steven.macenski@simberobotics.com) -------------------------------------------------------------------------------- /lib/karto_sdk/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package karto_sdk 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.4 (2016-03-02) 6 | ------------------ 7 | * update build status badges 8 | * Adds LocalizedRangeScanWithPoints range scan 9 | * Contributors: Michael Ferguson, Russell Toris 10 | 11 | 1.1.3 (2016-02-16) 12 | ------------------ 13 | * Link against, and export depend on, boost 14 | * Contributors: Hai Nguyen, Michael Ferguson 15 | 16 | 1.1.2 (2015-07-13) 17 | ------------------ 18 | * Added getters and setters for parameters inside Mapper so they can be changed via the ROS param server. 19 | * Contributors: Luc Bettaieb, Michael Ferguson 20 | 21 | 1.1.1 (2015-05-07) 22 | ------------------ 23 | * Makes FindValidPoints robust to the first point in the scan being a NaN 24 | * Bump minimum cmake version requirement 25 | * Fix cppcheck warnings 26 | * Fix newlines (dos2unix) & superfluous whitespace 27 | * Protect functions that throw away const-ness (check dirty flag) with mutex 28 | * Don't modify scan during loop closure check - work on a copy of it 29 | * removed useless return to avoid cppcheck error 30 | * Add Mapper::SetUseScanMatching 31 | * Remove html entities from log output 32 | * Fix NANs cause raytracing to take forever 33 | * Contributors: Daniel Pinyol, Michael Ferguson, Paul Mathieu, Siegfried-A. Gevatter Pujals, liz-murphy 34 | 35 | 1.1.0 (2014-06-15) 36 | ------------------ 37 | * Release as a pure catkin ROS package 38 | -------------------------------------------------------------------------------- /lib/karto_sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(karto_sdk) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0") 9 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/") 10 | 11 | find_package(ament_cmake REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(Eigen3 REQUIRED) 14 | find_package(Boost REQUIRED system serialization filesystem thread) 15 | find_package(TBB REQUIRED NO_CMAKE_PACKAGE_REGISTRY) 16 | 17 | set(dependencies 18 | rclcpp 19 | rclcpp_lifecycle 20 | ) 21 | 22 | include_directories(include ${EIGEN3_INCLUDE_DIRS} 23 | ${Boost_INCLUDE_DIR} 24 | ${BOOST_INCLUDE_DIRS} 25 | ${TBB_INCLUDE_DIRS} 26 | ) 27 | 28 | add_definitions(${EIGEN3_DEFINITIONS}) 29 | 30 | include_directories(include ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 31 | add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp) 32 | ament_target_dependencies(kartoSlamToolbox ${dependencies}) 33 | target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} TBB::tbb) 34 | 35 | install(DIRECTORY include/ 36 | DESTINATION include/ 37 | ) 38 | -------------------------------------------------------------------------------- /lib/karto_sdk/LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /lib/karto_sdk/include/karto_sdk/Macros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef KARTO_SDK__MACROS_H_ 19 | #define KARTO_SDK__MACROS_H_ 20 | 21 | //////////////////////////////////////////////////////////////////////////////////////// 22 | //////////////////////////////////////////////////////////////////////////////////////// 23 | //////////////////////////////////////////////////////////////////////////////////////// 24 | 25 | /** 26 | * Karto defines for handling deprecated code 27 | */ 28 | #ifndef KARTO_DEPRECATED 29 | # if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) 30 | # define KARTO_DEPRECATED __attribute__((deprecated)) 31 | # elif defined(__INTEL) || defined(_MSC_VER) 32 | # define KARTO_DEPRECATED __declspec(deprecated) 33 | # else 34 | # define KARTO_DEPRECATED 35 | # endif 36 | #endif 37 | 38 | //////////////////////////////////////////////////////////////////////////////////////// 39 | //////////////////////////////////////////////////////////////////////////////////////// 40 | //////////////////////////////////////////////////////////////////////////////////////// 41 | 42 | /** 43 | * Karto defines for windows dynamic build 44 | */ 45 | #if defined(_MSC_VER) || defined(__CYGWIN__) || \ 46 | defined(__MINGW32__) || defined( __BCPLUSPLUS__) || \ 47 | defined( __MWERKS__) 48 | # if defined( _LIB ) || defined( KARTO_STATIC ) || defined( STATIC_BUILD ) 49 | # define KARTO_EXPORT 50 | # else 51 | # ifdef KARTO_DYNAMIC 52 | # define KARTO_EXPORT __declspec(dllexport) 53 | # else 54 | # define KARTO_EXPORT __declspec(dllimport) 55 | # endif // KARTO_DYNAMIC 56 | # endif 57 | #else 58 | # define KARTO_EXPORT 59 | #endif 60 | 61 | //////////////////////////////////////////////////////////////////////////////////////// 62 | //////////////////////////////////////////////////////////////////////////////////////// 63 | //////////////////////////////////////////////////////////////////////////////////////// 64 | 65 | /** 66 | * Helper defines for std iterator loops 67 | */ 68 | #define forEach(listtype, list) \ 69 | for (listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter) 70 | 71 | #define forEachAs(listtype, list, iter) \ 72 | for (listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter) 73 | 74 | #define const_forEach(listtype, list) \ 75 | for (listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter) 76 | 77 | #define const_forEachAs(listtype, list, iter) \ 78 | for (listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter) 79 | 80 | #define forEachR(listtype, list) \ 81 | for (listtype::reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter) 82 | 83 | #define const_forEachR(listtype, list) \ 84 | for (listtype::const_reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter) 85 | 86 | 87 | //////////////////////////////////////////////////////////////////////////////////////// 88 | //////////////////////////////////////////////////////////////////////////////////////// 89 | //////////////////////////////////////////////////////////////////////////////////////// 90 | 91 | /** 92 | * Disable annoying compiler warnings 93 | */ 94 | 95 | #if defined(__INTEL) || defined(_MSC_VER) 96 | 97 | // Disable the warning: 'identifier' : class 'type' 98 | // needs to have dll-interface to be used by clients of class 'type2' 99 | #pragma warning(disable:4251) 100 | 101 | #endif 102 | 103 | #ifdef __INTEL_COMPILER 104 | 105 | // Disable the warning: conditional expression is constant 106 | #pragma warning(disable:4127) 107 | 108 | // Disable the warning: 'identifier' : unreferenced formal parameter 109 | #pragma warning(disable:4100) 110 | 111 | // remark #383: value copied to temporary, reference to temporary used 112 | #pragma warning(disable:383) 113 | 114 | // remark #981: operands are evaluated in unspecified order 115 | // disabled -> completely pointless if the functions do not have side effects 116 | #pragma warning(disable:981) 117 | 118 | // remark #1418: external function definition with no prior declaration 119 | #pragma warning(disable:1418) 120 | 121 | // remark #1572: floating-point equality and inequality comparisons are unreliable 122 | // disabled -> everyone knows it, the parser passes this problem deliberately to the user 123 | #pragma warning(disable:1572) 124 | 125 | // remark #10121: 126 | #pragma warning(disable:10121) 127 | 128 | #endif // __INTEL_COMPILER 129 | 130 | #endif // KARTO_SDK__MACROS_H_ 131 | -------------------------------------------------------------------------------- /lib/karto_sdk/include/karto_sdk/Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef KARTO_SDK__MATH_H_ 19 | #define KARTO_SDK__MATH_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "Types.h" 26 | 27 | namespace karto 28 | { 29 | /** 30 | * Platform independent pi definitions 31 | */ 32 | const kt_double KT_PI = 3.14159265358979323846; // The value of PI 33 | const kt_double KT_2PI = 6.28318530717958647692; // 2 * PI 34 | const kt_double KT_PI_2 = 1.57079632679489661923; // PI / 2 35 | const kt_double KT_PI_180 = 0.01745329251994329577; // PI / 180 36 | const kt_double KT_180_PI = 57.29577951308232087685; // 180 / PI 37 | 38 | /** 39 | * Lets define a small number! 40 | */ 41 | const kt_double KT_TOLERANCE = 1e-06; 42 | 43 | /** 44 | * Lets define max value of kt_int32s (int32_t) to use it to mark invalid scans 45 | */ 46 | 47 | const kt_int32s INVALID_SCAN = std::numeric_limits::max(); 48 | 49 | namespace math 50 | { 51 | /** 52 | * Converts degrees into radians 53 | * @param degrees 54 | * @return radian equivalent of degrees 55 | */ 56 | inline kt_double DegreesToRadians(kt_double degrees) 57 | { 58 | return degrees * KT_PI_180; 59 | } 60 | 61 | /** 62 | * Converts radians into degrees 63 | * @param radians 64 | * @return degree equivalent of radians 65 | */ 66 | inline kt_double RadiansToDegrees(kt_double radians) 67 | { 68 | return radians * KT_180_PI; 69 | } 70 | 71 | /** 72 | * Square function 73 | * @param value 74 | * @return square of value 75 | */ 76 | template 77 | inline T Square(T value) 78 | { 79 | return value * value; 80 | } 81 | 82 | /** 83 | * Round function 84 | * @param value 85 | * @return rounds value to the nearest whole number (as double) 86 | */ 87 | inline kt_double Round(kt_double value) 88 | { 89 | return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5); 90 | } 91 | 92 | /** 93 | * Binary minimum function 94 | * @param value1 95 | * @param value2 96 | * @return the lesser of value1 and value2 97 | */ 98 | template 99 | inline const T & Minimum(const T & value1, const T & value2) 100 | { 101 | return value1 < value2 ? value1 : value2; 102 | } 103 | 104 | /** 105 | * Binary maximum function 106 | * @param value1 107 | * @param value2 108 | * @return the greater of value1 and value2 109 | */ 110 | template 111 | inline const T & Maximum(const T & value1, const T & value2) 112 | { 113 | return value1 > value2 ? value1 : value2; 114 | } 115 | 116 | /** 117 | * Clips a number to the specified minimum and maximum values. 118 | * @param n number to be clipped 119 | * @param minValue minimum value 120 | * @param maxValue maximum value 121 | * @return the clipped value 122 | */ 123 | template 124 | inline const T & Clip(const T & n, const T & minValue, const T & maxValue) 125 | { 126 | return Minimum(Maximum(n, minValue), maxValue); 127 | } 128 | 129 | /** 130 | * Checks whether two numbers are equal within a certain tolerance. 131 | * @param a 132 | * @param b 133 | * @return true if a and b differ by at most a certain tolerance. 134 | */ 135 | inline kt_bool DoubleEqual(kt_double a, kt_double b) 136 | { 137 | double delta = a - b; 138 | return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE; 139 | } 140 | 141 | /** 142 | * Checks whether value is in the range [0;maximum) 143 | * @param value 144 | * @param maximum 145 | */ 146 | template 147 | inline kt_bool IsUpTo(const T & value, const T & maximum) 148 | { 149 | return value >= 0 && value < maximum; 150 | } 151 | 152 | /** 153 | * Checks whether value is in the range [0;maximum) 154 | * Specialized version for unsigned int (kt_int32u) 155 | * @param value 156 | * @param maximum 157 | */ 158 | template<> 159 | inline kt_bool IsUpTo(const kt_int32u & value, const kt_int32u & maximum) 160 | { 161 | return value < maximum; 162 | } 163 | 164 | 165 | /** 166 | * Checks whether value is in the range [a;b] 167 | * @param value 168 | * @param a 169 | * @param b 170 | */ 171 | template 172 | inline kt_bool InRange(const T & value, const T & a, const T & b) 173 | { 174 | return value >= a && value <= b; 175 | } 176 | 177 | /** 178 | * Normalizes angle to be in the range of [-pi, pi] 179 | * @param angle to be normalized 180 | * @return normalized angle 181 | */ 182 | inline kt_double NormalizeAngle(kt_double angle) 183 | { 184 | while (angle < -KT_PI) { 185 | if (angle < -KT_2PI) { 186 | angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI; 187 | } else { 188 | angle += KT_2PI; 189 | } 190 | } 191 | 192 | while (angle > KT_PI) { 193 | if (angle > KT_2PI) { 194 | angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI; 195 | } else { 196 | angle -= KT_2PI; 197 | } 198 | } 199 | 200 | assert(math::InRange(angle, -KT_PI, KT_PI)); 201 | 202 | return angle; 203 | } 204 | 205 | /** 206 | * Returns an equivalent angle to the first parameter such that the difference 207 | * when the second parameter is subtracted from this new value is an angle 208 | * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi. 209 | * @param minuend 210 | * @param subtrahend 211 | * @return normalized angle 212 | */ 213 | inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend) 214 | { 215 | while (minuend - subtrahend < -KT_PI) { 216 | minuend += KT_2PI; 217 | } 218 | 219 | while (minuend - subtrahend > KT_PI) { 220 | minuend -= KT_2PI; 221 | } 222 | 223 | return minuend; 224 | } 225 | 226 | /** 227 | * Align a value to the alignValue. 228 | * The alignValue should be the power of two (2, 4, 8, 16, 32 and so on) 229 | * @param value 230 | * @param alignValue 231 | * @return aligned value 232 | */ 233 | template 234 | inline T AlignValue(size_t value, size_t alignValue = 8) 235 | { 236 | return static_cast((value + (alignValue - 1)) & ~(alignValue - 1)); 237 | } 238 | } // namespace math 239 | 240 | } // namespace karto 241 | 242 | #endif // KARTO_SDK__MATH_H_ 243 | -------------------------------------------------------------------------------- /lib/karto_sdk/include/karto_sdk/Types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef KARTO_SDK__TYPES_H_ 19 | #define KARTO_SDK__TYPES_H_ 20 | 21 | #include 22 | 23 | /** 24 | * Karto type definition 25 | * Ensures platform independent types for windows, linux and mac 26 | */ 27 | #if defined(_MSC_VER) 28 | 29 | typedef signed __int8 kt_int8s; 30 | typedef unsigned __int8 kt_int8u; 31 | 32 | typedef signed __int16 kt_int16s; 33 | typedef unsigned __int16 kt_int16u; 34 | 35 | typedef signed __int32 kt_int32s; 36 | typedef unsigned __int32 kt_int32u; 37 | 38 | typedef signed __int64 kt_int64s; 39 | typedef unsigned __int64 kt_int64u; 40 | 41 | #else 42 | 43 | #include 44 | 45 | typedef int8_t kt_int8s; 46 | typedef uint8_t kt_int8u; 47 | 48 | typedef int16_t kt_int16s; 49 | typedef uint16_t kt_int16u; 50 | 51 | typedef int32_t kt_int32s; 52 | typedef uint32_t kt_int32u; 53 | 54 | #if defined(__LP64__) 55 | typedef signed long kt_int64s; // NOLINT 56 | typedef unsigned long kt_int64u; // NOLINT 57 | #else 58 | typedef signed long long kt_int64s; // NOLINT 59 | typedef unsigned long long kt_int64u; // NOLINT 60 | #endif 61 | 62 | #endif 63 | 64 | typedef bool kt_bool; 65 | typedef char kt_char; 66 | typedef float kt_float; 67 | typedef double kt_double; 68 | 69 | #endif // KARTO_SDK__TYPES_H_ 70 | -------------------------------------------------------------------------------- /lib/karto_sdk/include/karto_sdk/nanoflann_adaptors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * nanoflann_adaptors.h 3 | * Copyright (c) 2018, Simbe Robotics 4 | * Copyright (c) 2019, Samsung Research America 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #ifndef KARTO_SDK__NANOFLANN_ADAPTORS_H_ 21 | #define KARTO_SDK__NANOFLANN_ADAPTORS_H_ 22 | 23 | #include "nanoflann.hpp" 24 | 25 | // And this is the "dataset to kd-tree" adaptor class: 26 | template 27 | struct VertexVectorPoseNanoFlannAdaptor 28 | { 29 | const Derived & obj; //!< A const ref to the data set origin 30 | 31 | /// The constructor that sets the data set source 32 | explicit VertexVectorPoseNanoFlannAdaptor(const Derived & obj_) 33 | : obj(obj_) {} 34 | 35 | /// CRTP helper method 36 | inline const Derived & derived() const {return obj;} 37 | 38 | // Must return the number of data points 39 | inline size_t kdtree_get_point_count() const {return derived().size();} 40 | 41 | // Returns the dim'th component of the idx'th point in the class: 42 | // Since this is inlined and the "dim" argument is typically an immediate value, the 43 | // "if/else's" are actually solved at compile time. 44 | inline double kdtree_get_pt(const size_t idx, const size_t dim) const 45 | { 46 | if (dim == 0) {return derived()[idx]->GetObject()->GetCorrectedPose().GetX();} else { 47 | return derived()[idx]->GetObject()->GetCorrectedPose().GetY(); 48 | } 49 | } 50 | 51 | // Optional bounding-box computation: return false 52 | // to default to a standard bbox computation loop. 53 | // Return true if the BBOX was already computed by the 54 | // class and returned in "bb" so it can be avoided to redo it again. 55 | // Look at bb.size() to find out the expected 56 | // dimensionality (e.g. 2 or 3 for point clouds) 57 | template 58 | bool kdtree_get_bbox(BBOX & /*bb*/) const {return false;} 59 | }; 60 | 61 | // And this is the "dataset to kd-tree" adaptor class: 62 | template 63 | struct VertexVectorScanCenterNanoFlannAdaptor 64 | { 65 | const Derived & obj; 66 | 67 | explicit VertexVectorScanCenterNanoFlannAdaptor(const Derived & obj_) 68 | : obj(obj_) {} 69 | 70 | inline const Derived & derived() const {return obj;} 71 | 72 | inline size_t kdtree_get_point_count() const {return derived().size();} 73 | 74 | inline double kdtree_get_pt(const size_t idx, const size_t dim) const 75 | { 76 | if (dim == 0) {return derived()[idx]->GetObject()->GetBarycenterPose().GetX();} else { 77 | return derived()[idx]->GetObject()->GetBarycenterPose().GetY(); 78 | } 79 | } 80 | 81 | template 82 | bool kdtree_get_bbox(BBOX & /*bb*/) const {return false;} 83 | }; // namespace VertexVectorScanCenterNanoFlannAdaptor 84 | 85 | #endif // KARTO_SDK__NANOFLANN_ADAPTORS_H_ 86 | -------------------------------------------------------------------------------- /lib/karto_sdk/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | karto_sdk 4 | 1.1.4 5 | Catkinized ROS packaging of the OpenKarto library 6 | 7 | Michael Ferguson 8 | Luc Bettaieb 9 | LGPLv3 10 | 11 | ament_cmake 12 | 13 | boost 14 | tbb 15 | libblas-dev 16 | liblapack-dev 17 | 18 | boost 19 | tbb 20 | libblas-dev 21 | liblapack-dev 22 | 23 | 24 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | slam_toolbox 5 | 2.9.0 6 | 7 | This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets 8 | 9 | Steve Macenski 10 | Michel Hidalgo 11 | LGPL 12 | Steve Macenski 13 | 14 | ament_cmake 15 | 16 | pluginlib 17 | eigen 18 | message_filters 19 | nav_msgs 20 | rclcpp 21 | sensor_msgs 22 | tf2_ros 23 | tf2 24 | tf2_sensor_msgs 25 | visualization_msgs 26 | std_srvs 27 | boost 28 | interactive_markers 29 | std_msgs 30 | suitesparse 31 | liblapack-dev 32 | libceres-dev 33 | tf2_geometry_msgs 34 | tbb 35 | libqt5-core 36 | libqt5-widgets 37 | qtbase5-dev 38 | builtin_interfaces 39 | rosidl_default_generators 40 | lifecycle_msgs 41 | bondcpp 42 | bond 43 | rclcpp_lifecycle 44 | rviz_common 45 | rviz_default_plugins 46 | rviz_ogre_vendor 47 | rviz_rendering 48 | eigen 49 | pluginlib 50 | message_filters 51 | nav_msgs 52 | rclcpp 53 | sensor_msgs 54 | tf2 55 | tf2_ros 56 | tf2_sensor_msgs 57 | visualization_msgs 58 | std_srvs 59 | boost 60 | interactive_markers 61 | std_msgs 62 | suitesparse 63 | liblapack-dev 64 | libceres-dev 65 | tf2_geometry_msgs 66 | tbb 67 | libqt5-core 68 | libqt5-widgets 69 | nav2_map_server 70 | builtin_interfaces 71 | rosidl_default_generators 72 | lifecycle_msgs 73 | bondcpp 74 | bond 75 | rclcpp_lifecycle 76 | 77 | libqt5-core 78 | libqt5-gui 79 | libqt5-opengl 80 | libqt5-widgets 81 | 82 | ament_cmake_gtest 83 | launch 84 | launch_testing 85 | ament_cmake_flake8 86 | ament_cmake_cpplint 87 | ament_cmake_uncrustify 88 | ament_lint_auto 89 | 90 | rosidl_interface_packages 91 | 92 | 93 | ament_cmake 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /rviz_plugin/slam_toolbox_rviz_plugin.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2018, Simbe Robotics, Inc. 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef RVIZ_PLUGIN__SLAM_TOOLBOX_RVIZ_PLUGIN_H_ 20 | #define RVIZ_PLUGIN__SLAM_TOOLBOX_RVIZ_PLUGIN_H_ 21 | 22 | #include 23 | #include 24 | // QT 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | // STL 36 | #include 37 | #include 38 | #include 39 | // ROS 40 | #include "rclcpp/rclcpp.hpp" 41 | #include "rviz_common/panel.hpp" 42 | #include "slam_toolbox/toolbox_msgs.hpp" 43 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 44 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 45 | 46 | class QLineEdit; 47 | class QSpinBox; 48 | class QComboBox; 49 | 50 | namespace rviz_common 51 | { 52 | class VisualizationManager; 53 | } // namespace rviz_common 54 | 55 | namespace slam_toolbox 56 | { 57 | 58 | enum ContinueMappingType 59 | { 60 | PROCESS_CMT = 0, 61 | PROCESS_FIRST_NODE_CMT = 1, 62 | PROCESS_NEAR_REGION_CMT = 2, 63 | LOCALIZE_CMT = 3 64 | }; 65 | 66 | class SlamToolboxPlugin : public rviz_common::Panel 67 | { 68 | Q_OBJECT 69 | 70 | public: 71 | explicit SlamToolboxPlugin(QWidget * parent = 0); 72 | 73 | virtual ~SlamToolboxPlugin(); 74 | 75 | private Q_SLOTS: 76 | void ClearChanges(); 77 | void SaveChanges(); 78 | void SaveMap(); 79 | void ClearQueue(); 80 | void InteractiveCb(int state); 81 | void PauseMeasurementsCb(int state); 82 | void FirstNodeMatchCb(); 83 | void PoseEstMatchCb(); 84 | void CurEstMatchCb(); 85 | void LocalizeCb(); 86 | void LoadSubmap(); 87 | void GenerateMap(); 88 | void SerializeMap(); 89 | void DeserializeMap(); 90 | 91 | void updateCheckStateIfExternalChange(); 92 | 93 | protected: 94 | QVBoxLayout * _vbox; 95 | QHBoxLayout * _hbox1; 96 | QHBoxLayout * _hbox2; 97 | QHBoxLayout * _hbox3; 98 | QHBoxLayout * _hbox4; 99 | QHBoxLayout * _hbox5; 100 | QHBoxLayout * _hbox6; 101 | QHBoxLayout * _hbox7; 102 | QHBoxLayout * _hbox8; 103 | QHBoxLayout * _hbox9; 104 | QHBoxLayout * _hbox10; 105 | 106 | QPushButton * _button1; 107 | QPushButton * _button2; 108 | QPushButton * _button3; 109 | QPushButton * _button4; 110 | QPushButton * _button5; 111 | QPushButton * _button6; 112 | QPushButton * _button7; 113 | QPushButton * _button8; 114 | 115 | QLineEdit * _line1; 116 | QLineEdit * _line2; 117 | QLineEdit * _line3; 118 | QLineEdit * _line4; 119 | QLineEdit * _line5; 120 | QLineEdit * _line6; 121 | QLineEdit * _line7; 122 | 123 | QCheckBox * _check1; 124 | QCheckBox * _check2; 125 | 126 | QRadioButton * _radio1; 127 | QRadioButton * _radio2; 128 | QRadioButton * _radio3; 129 | QRadioButton * _radio4; 130 | 131 | QLabel * _label1; 132 | QLabel * _label2; 133 | QLabel * _label4; 134 | QLabel * _label5; 135 | QLabel * _label6; 136 | QLabel * _label7; 137 | QLabel * _label8; 138 | 139 | QFrame * _line; 140 | 141 | rclcpp::Node::SharedPtr ros_node_; 142 | rclcpp::Client::SharedPtr _clearChanges; 143 | rclcpp::Client::SharedPtr _saveChanges; 144 | rclcpp::Client::SharedPtr _saveMap; 145 | rclcpp::Client::SharedPtr _clearQueue; 146 | rclcpp::Client::SharedPtr _interactive; 147 | rclcpp::Client::SharedPtr _pause_measurements; 148 | rclcpp::Client::SharedPtr _load_submap_for_merging; 149 | rclcpp::Client::SharedPtr _merge; 150 | rclcpp::Client::SharedPtr _serialize; 151 | rclcpp::Client::SharedPtr _load_map; 152 | 153 | std::unique_ptr _thread; 154 | 155 | ContinueMappingType _match_type; 156 | 157 | rclcpp::Subscription::SharedPtr _initialposeSub; 158 | 159 | void InitialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose); 160 | }; 161 | 162 | } // namespace slam_toolbox 163 | 164 | #endif // RVIZ_PLUGIN__SLAM_TOOLBOX_RVIZ_PLUGIN_H_ 165 | -------------------------------------------------------------------------------- /rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | Panel to assist in Mapping and SLAM with the slam_toolbox 7 | 8 | 9 | -------------------------------------------------------------------------------- /solver_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | Ceres Optimizer for Slam Toolbox 7 | 8 | 9 | 10 | 15 | 16 | 21 | 22 | 27 | -------------------------------------------------------------------------------- /solvers/ceres_solver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Simbe Robotics, Inc. 3 | * Author: Steve Macenski (stevenmacenski@gmail.com) 4 | */ 5 | 6 | #ifndef SOLVERS__CERES_SOLVER_HPP_ 7 | #define SOLVERS__CERES_SOLVER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "karto_sdk/Mapper.h" 17 | #include "solvers/ceres_utils.h" 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 21 | #include "std_srvs/srv/empty.hpp" 22 | #include "slam_toolbox/toolbox_types.hpp" 23 | 24 | namespace solver_plugins 25 | { 26 | 27 | using namespace ::toolbox_types; // NOLINT 28 | 29 | class CeresSolver : public karto::ScanSolver 30 | { 31 | public: 32 | CeresSolver(); 33 | virtual ~CeresSolver(); 34 | 35 | public: 36 | // Get corrected poses after optimization 37 | virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const; 38 | 39 | virtual void Compute(); // Solve 40 | virtual void Clear(); // Resets the corrections 41 | virtual void Reset(); // Resets the solver plugin clean 42 | 43 | virtual void Configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node); 44 | 45 | // Adds a node to the solver 46 | virtual void AddNode(karto::Vertex * pVertex); 47 | // Adds a constraint to the solver 48 | virtual void AddConstraint(karto::Edge * pEdge); 49 | // Get graph stored 50 | virtual std::unordered_map * getGraph(); 51 | // Removes a node from the solver correction table 52 | virtual void RemoveNode(kt_int32s id); 53 | // Removes constraints from the optimization problem 54 | virtual void RemoveConstraint(kt_int32s sourceId, kt_int32s targetId); 55 | 56 | // change a node's pose 57 | virtual void ModifyNode(const int & unique_id, Eigen::Vector3d pose); 58 | // get a node's current pose yaw 59 | virtual void GetNodeOrientation(const int & unique_id, double & pose); 60 | 61 | private: 62 | // karto 63 | karto::ScanSolver::IdPoseVector corrections_; 64 | 65 | // ceres 66 | ceres::Solver::Options options_; 67 | ceres::Problem::Options options_problem_; 68 | ceres::LossFunction * loss_function_; 69 | ceres::Problem * problem_; 70 | ceres::Manifold * angle_manifold_; 71 | bool was_constant_set_, debug_logging_; 72 | 73 | // graph 74 | std::unordered_map * nodes_; 75 | std::unordered_map * blocks_; 76 | std::unordered_map::iterator first_node_; 77 | boost::mutex nodes_mutex_; 78 | 79 | // ros 80 | rclcpp::Logger logger_{rclcpp::get_logger("CeresSolver")}; 81 | }; 82 | 83 | } // namespace solver_plugins 84 | 85 | #endif // SOLVERS__CERES_SOLVER_HPP_ 86 | -------------------------------------------------------------------------------- /solvers/ceres_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Simbe Robotics 3 | * Author: Steve Macenski 4 | */ 5 | 6 | #ifndef SOLVERS__CERES_UTILS_H_ 7 | #define SOLVERS__CERES_UTILS_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | /*****************************************************************************/ 15 | /*****************************************************************************/ 16 | /*****************************************************************************/ 17 | inline std::size_t GetHash(const int & x, const int & y) 18 | { 19 | return (std::hash()(x) ^ (std::hash()(y) << 1)) >> 1; 20 | } 21 | 22 | /*****************************************************************************/ 23 | /*****************************************************************************/ 24 | /*****************************************************************************/ 25 | 26 | // Normalizes the angle in radians between [-pi and pi). 27 | template 28 | inline T NormalizeAngle(const T & angle_radians) 29 | { 30 | T two_pi(2.0 * M_PI); 31 | return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi); 32 | } 33 | 34 | /*****************************************************************************/ 35 | /*****************************************************************************/ 36 | /*****************************************************************************/ 37 | 38 | // Defines a manifold for updating the angle to be constrained in [-pi to pi). 39 | class AngleManifold { 40 | public: 41 | template 42 | bool Plus(const T* x_radians, 43 | const T* delta_radians, 44 | T* x_plus_delta_radians) const { 45 | *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians); 46 | return true; 47 | } 48 | 49 | template 50 | bool Minus(const T* y_radians, 51 | const T* x_radians, 52 | T* y_minus_x_radians) const { 53 | *y_minus_x_radians = NormalizeAngle(*y_radians - *x_radians); 54 | return true; 55 | } 56 | 57 | static ceres::Manifold* Create() { 58 | return new ceres::AutoDiffManifold; 59 | } 60 | }; 61 | 62 | /*****************************************************************************/ 63 | /*****************************************************************************/ 64 | /*****************************************************************************/ 65 | 66 | template 67 | Eigen::Matrix RotationMatrix2D(T yaw_radians) 68 | { 69 | const T cos_yaw = ceres::cos(yaw_radians); 70 | const T sin_yaw = ceres::sin(yaw_radians); 71 | Eigen::Matrix rotation; 72 | rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; 73 | return rotation; 74 | } 75 | 76 | /*****************************************************************************/ 77 | /*****************************************************************************/ 78 | /*****************************************************************************/ 79 | 80 | class PoseGraph2dErrorTerm 81 | { 82 | public: 83 | PoseGraph2dErrorTerm( 84 | double x_ab, double y_ab, double yaw_ab_radians, 85 | const Eigen::Matrix3d & sqrt_information) 86 | : p_ab_(x_ab, y_ab), yaw_ab_radians_(yaw_ab_radians), sqrt_information_(sqrt_information) 87 | { 88 | } 89 | 90 | template 91 | bool operator()( 92 | const T * const x_a, const T * const y_a, const T * const yaw_a, 93 | const T * const x_b, const T * const y_b, const T * const yaw_b, 94 | T * residuals_ptr) const 95 | { 96 | const Eigen::Matrix p_a(*x_a, *y_a); 97 | const Eigen::Matrix p_b(*x_b, *y_b); 98 | Eigen::Map> residuals_map(residuals_ptr); 99 | residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - 100 | p_ab_.cast(); 101 | residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast(yaw_ab_radians_)); 102 | // Scale the residuals by the square root information 103 | // matrix to account for the measurement uncertainty. 104 | residuals_map = sqrt_information_.template cast() * residuals_map; 105 | return true; 106 | } 107 | 108 | static ceres::CostFunction * Create( 109 | double x_ab, double y_ab, double yaw_ab_radians, 110 | const Eigen::Matrix3d & sqrt_information) 111 | { 112 | return new ceres::AutoDiffCostFunction( 113 | new PoseGraph2dErrorTerm( 114 | x_ab, y_ab, yaw_ab_radians, 115 | sqrt_information)); 116 | } 117 | 118 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 119 | 120 | private: 121 | // The position of B relative to A in the A frame. 122 | const Eigen::Vector2d p_ab_; 123 | // The orientation of frame B relative to frame A. 124 | const double yaw_ab_radians_; 125 | // The inverse square root of the measurement covariance matrix. 126 | const Eigen::Matrix3d sqrt_information_; 127 | }; 128 | 129 | #endif // SOLVERS__CERES_UTILS_H_ 130 | -------------------------------------------------------------------------------- /src/experimental/slam_toolbox_lifelong_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #include 21 | #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" 22 | 23 | int main(int argc, char ** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::NodeOptions options; 27 | auto lifelong_node = std::make_shared(options); 28 | rclcpp::spin(lifelong_node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /src/experimental/slam_toolbox_map_and_localization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2022, Steve Macenski 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | #include 18 | #include 19 | #include "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp" 20 | 21 | namespace slam_toolbox 22 | { 23 | 24 | /*****************************************************************************/ 25 | MapAndLocalizationSlamToolbox::MapAndLocalizationSlamToolbox(rclcpp::NodeOptions options) 26 | : LocalizationSlamToolbox(options) 27 | /*****************************************************************************/ 28 | { 29 | this->declare_parameter("localization_on_configure", false); 30 | } 31 | 32 | /*****************************************************************************/ 33 | CallbackReturn 34 | MapAndLocalizationSlamToolbox::on_configure(const rclcpp_lifecycle::State & state) 35 | /*****************************************************************************/ 36 | { 37 | SlamToolbox::on_configure(state); 38 | toggleMode(this->get_parameter("localization_on_configure").as_bool()); 39 | 40 | // disable interactive mode 41 | enable_interactive_mode_ = false; 42 | 43 | return CallbackReturn::SUCCESS; 44 | } 45 | 46 | /*****************************************************************************/ 47 | CallbackReturn 48 | MapAndLocalizationSlamToolbox::on_activate(const rclcpp_lifecycle::State & state) 49 | /*****************************************************************************/ 50 | { 51 | SlamToolbox::on_activate(state); 52 | ssSetLocalizationMode_ = create_service( 53 | "slam_toolbox/set_localization_mode", 54 | std::bind(&MapAndLocalizationSlamToolbox::setLocalizationModeCallback, this, 55 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 56 | return CallbackReturn::SUCCESS; 57 | } 58 | 59 | /*****************************************************************************/ 60 | CallbackReturn 61 | MapAndLocalizationSlamToolbox::on_deactivate(const rclcpp_lifecycle::State & state) 62 | /*****************************************************************************/ 63 | { 64 | SlamToolbox::on_deactivate(state); 65 | ssSetLocalizationMode_.reset(); 66 | return CallbackReturn::SUCCESS; 67 | } 68 | 69 | /*****************************************************************************/ 70 | bool MapAndLocalizationSlamToolbox::setLocalizationModeCallback( 71 | const std::shared_ptr request_header, 72 | const std::shared_ptr req, 73 | std::shared_ptr resp) 74 | /*****************************************************************************/ 75 | { 76 | toggleMode(req->data); 77 | 78 | resp->success = true; 79 | return true; 80 | } 81 | 82 | void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { 83 | bool in_localization_mode = processor_type_ == PROCESS_LOCALIZATION; 84 | if (in_localization_mode == enable_localization) { 85 | return; 86 | } 87 | 88 | if (enable_localization) { 89 | RCLCPP_INFO(get_logger(), "Enabling localization ..."); 90 | processor_type_ = PROCESS_LOCALIZATION; 91 | 92 | localization_pose_sub_ = 93 | create_subscription( 94 | "initialpose", 1, std::bind(&MapAndLocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1)); 95 | clear_localization_ = create_service( 96 | "slam_toolbox/clear_localization_buffer", 97 | std::bind(&MapAndLocalizationSlamToolbox::clearLocalizationBuffer, this, 98 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 99 | 100 | // in localization mode, disable map saver 101 | map_saver_.reset(); 102 | } 103 | else { 104 | RCLCPP_INFO(get_logger(), "Enabling mapping ..."); 105 | processor_type_ = PROCESS; 106 | localization_pose_sub_.reset(); 107 | clear_localization_.reset(); 108 | map_saver_ = std::make_unique(shared_from_this(), map_name_); 109 | 110 | boost::mutex::scoped_lock lock(smapper_mutex_); 111 | if (smapper_ && !smapper_->getMapper()->GetLocalizationVertices().empty()) { 112 | smapper_->clearLocalizationBuffer(); 113 | } 114 | } 115 | } 116 | 117 | /*****************************************************************************/ 118 | void MapAndLocalizationSlamToolbox::loadPoseGraphByParams() 119 | /*****************************************************************************/ 120 | { 121 | if (processor_type_ == PROCESS_LOCALIZATION) { 122 | LocalizationSlamToolbox::loadPoseGraphByParams(); 123 | } 124 | else { 125 | SlamToolbox::loadPoseGraphByParams(); 126 | } 127 | } 128 | 129 | /*****************************************************************************/ 130 | bool MapAndLocalizationSlamToolbox::serializePoseGraphCallback( 131 | const std::shared_ptr request_header, 132 | const std::shared_ptr req, 133 | std::shared_ptr resp) 134 | /*****************************************************************************/ 135 | { 136 | if (processor_type_ == PROCESS_LOCALIZATION) { 137 | return LocalizationSlamToolbox::serializePoseGraphCallback(request_header, req, resp); 138 | } 139 | else { 140 | return SlamToolbox::serializePoseGraphCallback(request_header, req, resp); 141 | } 142 | } 143 | 144 | /*****************************************************************************/ 145 | bool MapAndLocalizationSlamToolbox::deserializePoseGraphCallback( 146 | const std::shared_ptr request_header, 147 | const std::shared_ptr req, 148 | std::shared_ptr resp) 149 | /*****************************************************************************/ 150 | { 151 | if (processor_type_ == PROCESS_LOCALIZATION) { 152 | return LocalizationSlamToolbox::deserializePoseGraphCallback(request_header, req, resp); 153 | } 154 | else { 155 | return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); 156 | } 157 | } 158 | 159 | /*****************************************************************************/ 160 | void MapAndLocalizationSlamToolbox::laserCallback( 161 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) 162 | /*****************************************************************************/ 163 | { 164 | // store scan header 165 | scan_header = scan->header; 166 | // no odom info 167 | Pose2 pose; 168 | if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { 169 | RCLCPP_WARN(get_logger(), "Failed to compute odom pose"); 170 | return; 171 | } 172 | 173 | // ensure the laser can be used 174 | LaserRangeFinder * laser = getLaser(scan); 175 | 176 | if (!laser) { 177 | RCLCPP_WARN(get_logger(), "Failed to create laser device for" 178 | " %s; discarding scan", scan->header.frame_id.c_str()); 179 | return; 180 | } 181 | 182 | addScan(laser, scan, pose); 183 | } 184 | 185 | /*****************************************************************************/ 186 | LocalizedRangeScan * MapAndLocalizationSlamToolbox::addScan( 187 | LaserRangeFinder * laser, 188 | const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, 189 | Pose2 & odom_pose) 190 | /*****************************************************************************/ 191 | { 192 | if (processor_type_ == PROCESS_LOCALIZATION) { 193 | return LocalizationSlamToolbox::addScan(laser, scan, odom_pose); 194 | } 195 | else { 196 | return SlamToolbox::addScan(laser, scan, odom_pose); 197 | } 198 | } 199 | 200 | } // namespace slam_toolbox 201 | 202 | #include "rclcpp_components/register_node_macro.hpp" 203 | 204 | // Register the component with class_loader. 205 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 206 | // is being loaded into a running process. 207 | RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::MapAndLocalizationSlamToolbox) 208 | -------------------------------------------------------------------------------- /src/experimental/slam_toolbox_map_and_localization_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2022, Steve Macenski 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | #include 18 | #include "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | rclcpp::NodeOptions options; 24 | auto node = std::make_shared(options); 25 | rclcpp::spin(node->get_node_base_interface()); 26 | rclcpp::shutdown(); 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /src/laser_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * laser_utils 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "slam_toolbox/laser_utils.hpp" 25 | 26 | namespace laser_utils 27 | { 28 | 29 | LaserMetadata::LaserMetadata() 30 | { 31 | } 32 | 33 | LaserMetadata::~LaserMetadata() 34 | { 35 | } 36 | 37 | LaserMetadata::LaserMetadata(karto::LaserRangeFinder * lsr, bool invert) 38 | { 39 | laser = lsr; 40 | inverted = invert; 41 | } 42 | 43 | bool LaserMetadata::isInverted() const 44 | { 45 | return inverted; 46 | } 47 | 48 | karto::LaserRangeFinder * LaserMetadata::getLaser() 49 | { 50 | return laser; 51 | } 52 | 53 | void LaserMetadata::invertScan(sensor_msgs::msg::LaserScan & scan) const 54 | { 55 | sensor_msgs::msg::LaserScan temp; 56 | temp.intensities.reserve(scan.intensities.size()); 57 | temp.ranges.reserve(scan.ranges.size()); 58 | const bool has_intensities = scan.intensities.size() > 0 ? true : false; 59 | 60 | for (int i = scan.ranges.size(); i != 0; i--) { 61 | temp.ranges.push_back(scan.ranges[i]); 62 | if (has_intensities) { 63 | temp.intensities.push_back(scan.intensities[i]); 64 | } 65 | } 66 | 67 | scan.ranges = temp.ranges; 68 | scan.intensities = temp.intensities; 69 | } 70 | 71 | template 72 | LaserAssistant::LaserAssistant( 73 | NodeT node, 74 | tf2_ros::Buffer * tf, const std::string & base_frame) 75 | : logger_(node->get_logger()), parameters_interface_(node->get_node_parameters_interface()), 76 | tf_(tf), base_frame_(base_frame) 77 | { 78 | } 79 | 80 | LaserAssistant::~LaserAssistant() 81 | { 82 | } 83 | 84 | LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan) 85 | { 86 | scan_ = scan; 87 | frame_ = scan_.header.frame_id; 88 | 89 | double mountingYaw; 90 | bool inverted = isInverted(mountingYaw); 91 | karto::LaserRangeFinder * laser = makeLaser(mountingYaw); 92 | LaserMetadata laserMeta(laser, inverted); 93 | return laserMeta; 94 | } 95 | 96 | karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw) 97 | { 98 | karto::LaserRangeFinder * laser = 99 | karto::LaserRangeFinder::CreateLaserRangeFinder( 100 | karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar")); 101 | laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x, 102 | laser_pose_.transform.translation.y, mountingYaw)); 103 | 104 | double min_laser_range = 0.0; 105 | if (!parameters_interface_->has_parameter("min_laser_range")) { 106 | parameters_interface_->declare_parameter( 107 | "min_laser_range", 108 | rclcpp::ParameterValue(min_laser_range)); 109 | } 110 | min_laser_range = parameters_interface_->get_parameter("min_laser_range").as_double(); 111 | 112 | if (min_laser_range < 0) { 113 | RCLCPP_WARN( 114 | logger_, 115 | "You've set minimum laser range to be negative," 116 | "this isn't allowed so it will be set to (%.1f).", scan_.range_min); 117 | min_laser_range = scan_.range_min; 118 | } 119 | 120 | if (min_laser_range < scan_.range_min) { 121 | RCLCPP_WARN( 122 | logger_, 123 | "minimum laser range setting (%.1f m) exceeds the capabilities " 124 | "of the used Lidar (%.1f m)", min_laser_range, scan_.range_min); 125 | min_laser_range = scan_.range_min; 126 | } 127 | 128 | laser->SetMinimumRange(min_laser_range); 129 | laser->SetMaximumRange(scan_.range_max); 130 | laser->SetMinimumAngle(scan_.angle_min); 131 | laser->SetMaximumAngle(scan_.angle_max); 132 | laser->SetAngularResolution(scan_.angle_increment); 133 | 134 | bool is_360_lidar = false; 135 | const float angular_range = std::fabs(scan_.angle_max - scan_.angle_min); 136 | if (std::fabs(angular_range - 2.0 * M_PI) < (scan_.angle_increment - (std::numeric_limits::epsilon() * 2.0f*M_PI))) { 137 | is_360_lidar = true; 138 | } 139 | 140 | // Check if we have a 360 laser, but incorrectly setup as to produce 141 | // measurements in range [0, 360] rather than appropriately as [0, 360) 142 | if (angular_range > 6.10865 /*350 deg*/ && std::round(angular_range / scan_.angle_increment) + 1 == scan_.ranges.size()) { 143 | is_360_lidar = false; 144 | } 145 | 146 | laser->SetIs360Laser(is_360_lidar); 147 | 148 | double max_laser_range = 25; 149 | if (!parameters_interface_->has_parameter("max_laser_range")) { 150 | parameters_interface_->declare_parameter( 151 | "max_laser_range", 152 | rclcpp::ParameterValue(max_laser_range)); 153 | } 154 | max_laser_range = parameters_interface_->get_parameter("max_laser_range").as_double(); 155 | 156 | if (max_laser_range <= 0) { 157 | RCLCPP_WARN( 158 | logger_, 159 | "You've set maximum_laser_range to be negative," 160 | "this isn't allowed so it will be set to (%.1f).", scan_.range_max); 161 | max_laser_range = scan_.range_max; 162 | } 163 | 164 | if (max_laser_range > scan_.range_max) { 165 | RCLCPP_WARN( 166 | logger_, 167 | "maximum laser range setting (%.1f m) exceeds the capabilities " 168 | "of the used Lidar (%.1f m)", max_laser_range, scan_.range_max); 169 | max_laser_range = scan_.range_max; 170 | } 171 | laser->SetRangeThreshold(max_laser_range); 172 | return laser; 173 | } 174 | 175 | bool LaserAssistant::isInverted(double & mountingYaw) 176 | { 177 | geometry_msgs::msg::TransformStamped laser_ident; 178 | laser_ident.header.stamp = scan_.header.stamp; 179 | laser_ident.header.frame_id = frame_; 180 | laser_ident.transform.rotation.w = 1.0; 181 | 182 | laser_pose_ = tf_->transform(laser_ident, base_frame_); 183 | mountingYaw = tf2::getYaw(laser_pose_.transform.rotation); 184 | 185 | RCLCPP_DEBUG( 186 | logger_, "laser %s's pose wrt base: %.3f %.3f %.3f %.3f", 187 | frame_.c_str(), laser_pose_.transform.translation.x, 188 | laser_pose_.transform.translation.y, 189 | laser_pose_.transform.translation.z, mountingYaw); 190 | 191 | geometry_msgs::msg::Vector3Stamped laser_orient; 192 | laser_orient.vector.z = laser_orient.vector.y = 0.; 193 | laser_orient.vector.z = 1 + laser_pose_.transform.translation.z; 194 | laser_orient.header.stamp = scan_.header.stamp; 195 | laser_orient.header.frame_id = base_frame_; 196 | laser_orient = tf_->transform(laser_orient, frame_); 197 | 198 | if (laser_orient.vector.z <= 0) { 199 | RCLCPP_DEBUG( 200 | logger_, "laser is mounted upside-down"); 201 | return true; 202 | } 203 | 204 | return false; 205 | } 206 | 207 | ScanHolder::ScanHolder(std::map & lasers) 208 | : lasers_(lasers) 209 | { 210 | current_scans_ = std::make_unique>(); 211 | } 212 | 213 | ScanHolder::~ScanHolder() 214 | { 215 | current_scans_.reset(); 216 | } 217 | 218 | sensor_msgs::msg::LaserScan ScanHolder::getCorrectedScan(const int & id) 219 | { 220 | sensor_msgs::msg::LaserScan scan = current_scans_->at(id); 221 | const laser_utils::LaserMetadata & laser = lasers_[scan.header.frame_id]; 222 | if (laser.isInverted()) { 223 | laser.invertScan(scan); 224 | } 225 | return scan; 226 | } 227 | 228 | void ScanHolder::addScan(const sensor_msgs::msg::LaserScan scan) 229 | { 230 | current_scans_->push_back(scan); 231 | } 232 | 233 | // explicit instantiation for the supported template types 234 | template LaserAssistant::LaserAssistant( 235 | rclcpp::Node::SharedPtr, tf2_ros::Buffer *, const std::string &); 236 | template LaserAssistant::LaserAssistant( 237 | rclcpp_lifecycle::LifecycleNode::SharedPtr, tf2_ros::Buffer *, const std::string &); 238 | 239 | } // namespace laser_utils 240 | -------------------------------------------------------------------------------- /src/map_saver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * map_saver 3 | * Copyright (c) 2019, Samsung Research America 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #include 20 | #include 21 | #include "slam_toolbox/map_saver.hpp" 22 | 23 | namespace map_saver 24 | { 25 | 26 | /*****************************************************************************/ 27 | template 28 | MapSaver::MapSaver(NodeT node, const std::string & map_name) 29 | : logger_(node->get_logger()), namespace_str_(node->get_namespace()), 30 | map_name_(map_name), received_map_(false) 31 | /*****************************************************************************/ 32 | { 33 | server_ = node->template create_service( 34 | "slam_toolbox/save_map", 35 | std::bind( 36 | &MapSaver::saveMapCallback, this, std::placeholders::_1, 37 | std::placeholders::_2, std::placeholders::_3)); 38 | 39 | auto mapCallback = 40 | [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void 41 | { 42 | received_map_ = true; 43 | }; 44 | 45 | sub_ = node->template create_subscription( 46 | map_name_, rclcpp::QoS(1), mapCallback); 47 | } 48 | 49 | /*****************************************************************************/ 50 | bool MapSaver::saveMapCallback( 51 | const std::shared_ptr request_header, 52 | const std::shared_ptr req, 53 | std::shared_ptr response) 54 | /*****************************************************************************/ 55 | { 56 | if (!received_map_) { 57 | RCLCPP_WARN( 58 | logger_, 59 | "Map Saver: Cannot save map, no map yet received on topic %s.", 60 | map_name_.c_str()); 61 | response->result = response->RESULT_NO_MAP_RECEIEVD; 62 | return false; 63 | } 64 | 65 | const std::string name = req->name.data; 66 | std::string set_namespace; 67 | if (!namespace_str_.empty()) { 68 | set_namespace = " -r __ns:=" + namespace_str_; 69 | } 70 | 71 | if (name != "") { 72 | RCLCPP_INFO( 73 | logger_, 74 | "SlamToolbox: Saving map as %s.", name.c_str()); 75 | int rc = system(("ros2 run nav2_map_server map_saver_cli -f " + name + " --ros-args -p map_subscribe_transient_local:=true" + set_namespace).c_str()); 76 | if (rc == 0) { 77 | response->result = response->RESULT_SUCCESS; 78 | } else { 79 | response->result = response->RESULT_UNDEFINED_FAILURE; 80 | } 81 | } else { 82 | RCLCPP_INFO( 83 | logger_, 84 | "SlamToolbox: Saving map in current directory."); 85 | int rc = system(("ros2 run nav2_map_server map_saver_cli --ros-args -p map_subscribe_transient_local:=true" + set_namespace).c_str()); 86 | if (rc == 0) { 87 | response->result = response->RESULT_SUCCESS; 88 | } else { 89 | response->result = response->RESULT_UNDEFINED_FAILURE; 90 | } 91 | } 92 | 93 | rclcpp::sleep_for(std::chrono::seconds(1)); 94 | return true; 95 | } 96 | 97 | // explicit instantiation for the supported template types 98 | template MapSaver::MapSaver(rclcpp::Node::SharedPtr, const std::string &); 99 | template MapSaver::MapSaver(rclcpp_lifecycle::LifecycleNode::SharedPtr, const std::string &); 100 | 101 | } // namespace map_saver 102 | -------------------------------------------------------------------------------- /src/slam_toolbox_async.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #include 21 | #include "slam_toolbox/slam_toolbox_async.hpp" 22 | 23 | namespace slam_toolbox 24 | { 25 | 26 | /*****************************************************************************/ 27 | AsynchronousSlamToolbox::AsynchronousSlamToolbox(rclcpp::NodeOptions options) 28 | : SlamToolbox(options) 29 | /*****************************************************************************/ 30 | { 31 | } 32 | 33 | /*****************************************************************************/ 34 | void AsynchronousSlamToolbox::laserCallback( 35 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) 36 | /*****************************************************************************/ 37 | { 38 | // store scan header 39 | scan_header = scan->header; 40 | // no odom info 41 | Pose2 pose; 42 | if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { 43 | RCLCPP_WARN(get_logger(), "Failed to compute odom pose"); 44 | return; 45 | } 46 | 47 | // ensure the laser can be used 48 | LaserRangeFinder * laser = getLaser(scan); 49 | 50 | if (!laser) { 51 | RCLCPP_WARN(get_logger(), "Failed to create laser device for" 52 | " %s; discarding scan", scan->header.frame_id.c_str()); 53 | return; 54 | } 55 | 56 | // if not paused, process scan 57 | if (shouldProcessScan(scan, pose)) { 58 | addScan(laser, scan, pose); 59 | } 60 | } 61 | 62 | /*****************************************************************************/ 63 | bool AsynchronousSlamToolbox::deserializePoseGraphCallback( 64 | const std::shared_ptr request_header, 65 | const std::shared_ptr req, 66 | std::shared_ptr resp) 67 | /*****************************************************************************/ 68 | { 69 | if (req->match_type == procType::LOCALIZE_AT_POSE) { 70 | RCLCPP_WARN(get_logger(), "Requested a localization deserialization " 71 | "in non-localization mode."); 72 | return false; 73 | } 74 | 75 | return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); 76 | } 77 | 78 | } // namespace slam_toolbox 79 | 80 | #include "rclcpp_components/register_node_macro.hpp" 81 | 82 | // Register the component with class_loader. 83 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 84 | // is being loaded into a running process. 85 | RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::AsynchronousSlamToolbox) 86 | -------------------------------------------------------------------------------- /src/slam_toolbox_async_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #include 21 | #include "slam_toolbox/slam_toolbox_async.hpp" 22 | 23 | int main(int argc, char ** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::NodeOptions options; 27 | auto async_node = std::make_shared(options); 28 | rclcpp::spin(async_node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /src/slam_toolbox_localization_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #include 21 | #include "slam_toolbox/slam_toolbox_localization.hpp" 22 | 23 | int main(int argc, char ** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::NodeOptions options; 27 | auto localize_node = std::make_shared(options); 28 | rclcpp::spin(localize_node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /src/slam_toolbox_sync.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #include "slam_toolbox/slam_toolbox_sync.hpp" 21 | 22 | #include 23 | namespace slam_toolbox 24 | { 25 | 26 | /*****************************************************************************/ 27 | SynchronousSlamToolbox::SynchronousSlamToolbox(rclcpp::NodeOptions options) 28 | : SlamToolbox(options) 29 | /*****************************************************************************/ 30 | { 31 | } 32 | 33 | /*****************************************************************************/ 34 | void SynchronousSlamToolbox::run() 35 | /*****************************************************************************/ 36 | { 37 | rclcpp::Rate r(100); 38 | while (rclcpp::ok()) { 39 | boost::this_thread::interruption_point(); 40 | if (!isPaused(PROCESSING)) { 41 | PosedScan scan_w_pose(nullptr, karto::Pose2()); // dummy, updated in critical section 42 | bool queue_empty = true; 43 | { 44 | boost::mutex::scoped_lock lock(q_mutex_); 45 | queue_empty = q_.empty(); 46 | if (!queue_empty) { 47 | scan_w_pose = q_.front(); 48 | q_.pop(); 49 | 50 | if (q_.size() > 10) { 51 | RCLCPP_WARN(get_logger(), "Queue size has grown to: %i. " 52 | "Recommend stopping until message is gone if online mapping.", 53 | (int)q_.size()); 54 | } 55 | } 56 | } 57 | if (!queue_empty) { 58 | addScan(getLaser(scan_w_pose.scan), scan_w_pose); 59 | continue; 60 | } 61 | } 62 | 63 | r.sleep(); 64 | } 65 | } 66 | 67 | /*****************************************************************************/ 68 | CallbackReturn 69 | SynchronousSlamToolbox::on_activate(const rclcpp_lifecycle::State & state) 70 | /*****************************************************************************/ 71 | { 72 | SlamToolbox::on_activate(state); 73 | ssClear_ = this->create_service( 74 | "slam_toolbox/clear_queue", 75 | std::bind( 76 | &SynchronousSlamToolbox::clearQueueCallback, this, 77 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 78 | threads_.push_back( 79 | std::make_unique( 80 | boost::bind(&SynchronousSlamToolbox::run, this))); 81 | return CallbackReturn::SUCCESS; 82 | } 83 | 84 | /*****************************************************************************/ 85 | CallbackReturn 86 | SynchronousSlamToolbox::on_deactivate(const rclcpp_lifecycle::State & state) 87 | /*****************************************************************************/ 88 | { 89 | SlamToolbox::on_deactivate(state); 90 | ssClear_.reset(); 91 | return CallbackReturn::SUCCESS; 92 | } 93 | 94 | /*****************************************************************************/ 95 | void SynchronousSlamToolbox::laserCallback( 96 | sensor_msgs::msg::LaserScan::ConstSharedPtr scan) 97 | /*****************************************************************************/ 98 | { 99 | // store scan header 100 | scan_header = scan->header; 101 | // no odom info 102 | Pose2 pose; 103 | if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { 104 | RCLCPP_WARN(get_logger(), "Failed to compute odom pose"); 105 | return; 106 | } 107 | 108 | // ensure the laser can be used 109 | LaserRangeFinder * laser = getLaser(scan); 110 | 111 | if (!laser) { 112 | RCLCPP_WARN(get_logger(), "SynchronousSlamToolbox: Failed to create laser" 113 | " device for %s; discarding scan", scan->header.frame_id.c_str()); 114 | return; 115 | } 116 | 117 | // if sync and valid, add to queue 118 | if (shouldProcessScan(scan, pose)) { 119 | boost::mutex::scoped_lock lock(q_mutex_); 120 | q_.push(PosedScan(scan, pose)); 121 | } 122 | } 123 | 124 | /*****************************************************************************/ 125 | bool SynchronousSlamToolbox::clearQueueCallback( 126 | const std::shared_ptr request_header, 127 | const std::shared_ptr req, 128 | std::shared_ptr resp) 129 | /*****************************************************************************/ 130 | { 131 | RCLCPP_INFO(get_logger(), "SynchronousSlamToolbox: " 132 | "Clearing all queued scans to add to map."); 133 | while (!q_.empty()) { 134 | q_.pop(); 135 | } 136 | resp->status = true; 137 | return resp->status; 138 | } 139 | 140 | /*****************************************************************************/ 141 | bool SynchronousSlamToolbox::deserializePoseGraphCallback( 142 | const std::shared_ptr request_header, 143 | const std::shared_ptr req, 144 | std::shared_ptr resp) 145 | /*****************************************************************************/ 146 | { 147 | if (req->match_type == procType::LOCALIZE_AT_POSE) { 148 | RCLCPP_ERROR(get_logger(), "Requested a localization deserialization " 149 | "in non-localization mode."); 150 | return false; 151 | } 152 | return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); 153 | } 154 | 155 | /*****************************************************************************/ 156 | bool SynchronousSlamToolbox::resetCallback( 157 | const std::shared_ptr request_header, 158 | const std::shared_ptr req, 159 | std::shared_ptr resp) 160 | /*****************************************************************************/ 161 | { 162 | { 163 | boost::mutex::scoped_lock lock(q_mutex_); 164 | // Clear the scan queue. 165 | while (!q_.empty()) { 166 | q_.pop(); 167 | } 168 | } 169 | return SlamToolbox::resetCallback(request_header, req, resp); 170 | } 171 | 172 | } // namespace slam_toolbox 173 | 174 | #include "rclcpp_components/register_node_macro.hpp" 175 | 176 | // Register the component with class_loader. 177 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 178 | // is being loaded into a running process. 179 | RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::SynchronousSlamToolbox) 180 | -------------------------------------------------------------------------------- /src/slam_toolbox_sync_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. 4 | * Copyright Work Modifications (c) 2019, Steve Macenski 5 | * 6 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 7 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 8 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 9 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 10 | * 11 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 12 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 13 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 14 | * CONDITIONS. 15 | * 16 | */ 17 | 18 | /* Author: Steven Macenski */ 19 | 20 | #include 21 | #include "slam_toolbox/slam_toolbox_sync.hpp" 22 | 23 | int main(int argc, char ** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::NodeOptions options; 27 | auto sync_node = std::make_shared(options); 28 | rclcpp::spin(sync_node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /srv/AddSubmap.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- -------------------------------------------------------------------------------- /srv/Clear.srv: -------------------------------------------------------------------------------- 1 | 2 | 3 | --- -------------------------------------------------------------------------------- /srv/ClearQueue.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool status -------------------------------------------------------------------------------- /srv/DeserializePoseGraph.srv: -------------------------------------------------------------------------------- 1 | int8 UNSET = 0 2 | int8 START_AT_FIRST_NODE = 1 3 | int8 START_AT_GIVEN_POSE = 2 4 | int8 LOCALIZE_AT_POSE = 3 5 | 6 | # inital_pose should be Map -> base_frame (parameter, generally base_link) 7 | # 8 | 9 | string filename 10 | int8 match_type 11 | geometry_msgs/Pose2D initial_pose 12 | --- -------------------------------------------------------------------------------- /srv/LoopClosure.srv: -------------------------------------------------------------------------------- 1 | 2 | --- -------------------------------------------------------------------------------- /srv/MergeMaps.srv: -------------------------------------------------------------------------------- 1 | 2 | --- -------------------------------------------------------------------------------- /srv/Pause.srv: -------------------------------------------------------------------------------- 1 | # trigger pause toggle 2 | 3 | --- 4 | bool status -------------------------------------------------------------------------------- /srv/Reset.srv: -------------------------------------------------------------------------------- 1 | # Set this to 'true' to pause new measurements immediately after reset. 2 | # Note: This is a set behaviour and not a toggle behaviour, contrary to Pause.srv service. 3 | bool pause_new_measurements false 4 | --- 5 | # Result code defintions 6 | uint8 RESULT_SUCCESS=0 7 | 8 | uint8 result 9 | -------------------------------------------------------------------------------- /srv/SaveMap.srv: -------------------------------------------------------------------------------- 1 | std_msgs/String name 2 | --- 3 | # Result code defintions 4 | uint8 RESULT_SUCCESS=0 5 | uint8 RESULT_NO_MAP_RECEIEVD=1 6 | uint8 RESULT_UNDEFINED_FAILURE=255 7 | 8 | uint8 result 9 | -------------------------------------------------------------------------------- /srv/SerializePoseGraph.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- 3 | # Result code defintions 4 | uint8 RESULT_SUCCESS=0 5 | uint8 RESULT_FAILED_TO_WRITE_FILE=255 6 | 7 | uint8 result -------------------------------------------------------------------------------- /srv/ToggleInteractive.srv: -------------------------------------------------------------------------------- 1 | 2 | --- -------------------------------------------------------------------------------- /test/lifelong_metrics_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_toolbox 3 | * Copyright (c) 2019, Steve Macenski 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Steven Macenski */ 18 | 19 | #ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_ 20 | #define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_ 21 | 22 | #include 23 | #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" 24 | 25 | // 3 potential test cases, t1 is used. 26 | // t1 = IOU([3.5, 4.0, 3.0, 4.0], [3.5, 5.5, 3.0, 3.0]) == 6.0 27 | // t2 = IOU([4.5, 3.0, 5.0, 2.0], [4.5, 4.5, 3, 3]) == 3.0 28 | // t3 = IOU([4.5, 3.5, 3.0, 3.0], [2.5, 5.5, 3, 3]) == 1.0 29 | 30 | namespace karto 31 | { 32 | 33 | TEST(LifelingMetricsTests, TestBounds) 34 | { 35 | LocalizedRangeScan * s1 = new LocalizedRangeScan(); 36 | LocalizedRangeScan * s2 = new LocalizedRangeScan(); 37 | Pose2 p1 = Pose2(3.5, 4.0, 0.0); 38 | Pose2 p2 = Pose2(3.5, 5.5, 0.0); 39 | s1->SetBarycenterPose(p1); 40 | s2->SetBarycenterPose(p2); 41 | BoundingBox2 bb1, bb2; 42 | bb1.SetMinimum(Vector2(2.0, 2.0)); 43 | bb1.SetMaximum(Vector2(5.0, 6.0)); 44 | bb2.SetMinimum(Vector2(2.0, 4.0)); 45 | bb2.SetMaximum(Vector2(5.0, 7.0)); 46 | s1->SetBoundingBox(bb1); 47 | s2->SetBoundingBox(bb2); 48 | PointVectorDouble pts; 49 | pts.push_back(Vector2(3.0, 5.0)); // inside 50 | pts.push_back(Vector2(3.0, 3.0)); // outside 51 | s2->SetPointReadings(pts, true); 52 | double x_l, x_u, y_l, y_u; 53 | bool dirty = false; 54 | s1->SetIsDirty(dirty); 55 | s2->SetIsDirty(dirty); 56 | slam_toolbox::LifelongSlamToolbox::computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u); 57 | EXPECT_EQ(x_l, 2.0); 58 | EXPECT_EQ(x_u, 5.0); 59 | EXPECT_EQ(y_l, 4.0); 60 | EXPECT_EQ(y_u, 6.0); 61 | delete s1; 62 | delete s2; 63 | } 64 | 65 | TEST(LifelingMetricsTests, TestIntersect) 66 | { 67 | LocalizedRangeScan * s1 = new LocalizedRangeScan(); 68 | LocalizedRangeScan * s2 = new LocalizedRangeScan(); 69 | Pose2 p1 = Pose2(3.5, 4.0, 0.0); 70 | Pose2 p2 = Pose2(3.5, 5.5, 0.0); 71 | s1->SetBarycenterPose(p1); 72 | s2->SetBarycenterPose(p2); 73 | BoundingBox2 bb1, bb2; 74 | bb1.SetMinimum(Vector2(2.0, 2.0)); 75 | bb1.SetMaximum(Vector2(5.0, 6.0)); 76 | bb2.SetMinimum(Vector2(2.0, 4.0)); 77 | bb2.SetMaximum(Vector2(5.0, 7.0)); 78 | s1->SetBoundingBox(bb1); 79 | s2->SetBoundingBox(bb2); 80 | PointVectorDouble pts; 81 | pts.push_back(Vector2(3.0, 5.0)); // inside 82 | pts.push_back(Vector2(3.0, 3.0)); // outside 83 | s2->SetPointReadings(pts, true); 84 | bool dirty = false; 85 | s1->SetIsDirty(dirty); 86 | s2->SetIsDirty(dirty); 87 | double intersect = slam_toolbox::LifelongSlamToolbox::computeIntersect(s1, s2); 88 | EXPECT_EQ(intersect, 6.0); 89 | delete s1; 90 | delete s2; 91 | } 92 | 93 | TEST(LifelingMetricsTests, TestIntersectOverUnion) 94 | { 95 | LocalizedRangeScan * s1 = new LocalizedRangeScan(); 96 | LocalizedRangeScan * s2 = new LocalizedRangeScan(); 97 | Pose2 p1 = Pose2(3.5, 4.0, 0.0); 98 | Pose2 p2 = Pose2(3.5, 5.5, 0.0); 99 | s1->SetBarycenterPose(p1); 100 | s2->SetBarycenterPose(p2); 101 | BoundingBox2 bb1, bb2; 102 | bb1.SetMinimum(Vector2(2.0, 2.0)); 103 | bb1.SetMaximum(Vector2(5.0, 6.0)); 104 | bb2.SetMinimum(Vector2(2.0, 4.0)); 105 | bb2.SetMaximum(Vector2(5.0, 7.0)); 106 | s1->SetBoundingBox(bb1); 107 | s2->SetBoundingBox(bb2); 108 | PointVectorDouble pts; 109 | pts.push_back(Vector2(3.0, 5.0)); // inside 110 | pts.push_back(Vector2(3.0, 3.0)); // outside 111 | s2->SetPointReadings(pts, true); 112 | bool dirty = false; 113 | s1->SetIsDirty(dirty); 114 | s2->SetIsDirty(dirty); 115 | double intersect_over_union = 116 | slam_toolbox::LifelongSlamToolbox::computeIntersectOverUnion(s1, s2); 117 | EXPECT_EQ(intersect_over_union, 0.4); 118 | delete s1; 119 | delete s2; 120 | } 121 | 122 | TEST(LifelingMetricsTests, TestAreaOverlap) 123 | { 124 | LocalizedRangeScan * s1 = new LocalizedRangeScan(); 125 | LocalizedRangeScan * s2 = new LocalizedRangeScan(); 126 | Pose2 p1 = Pose2(3.5, 4.0, 0.0); 127 | Pose2 p2 = Pose2(3.5, 5.5, 0.0); 128 | s1->SetBarycenterPose(p1); 129 | s2->SetBarycenterPose(p2); 130 | BoundingBox2 bb1, bb2; 131 | bb1.SetMinimum(Vector2(2.0, 2.0)); 132 | bb1.SetMaximum(Vector2(5.0, 6.0)); 133 | bb2.SetMinimum(Vector2(2.0, 4.0)); 134 | bb2.SetMaximum(Vector2(5.0, 7.0)); 135 | s1->SetBoundingBox(bb1); 136 | s2->SetBoundingBox(bb2); 137 | PointVectorDouble pts; 138 | pts.push_back(Vector2(3.0, 5.0)); // inside 139 | pts.push_back(Vector2(3.0, 3.0)); // outside 140 | s2->SetPointReadings(pts, true); 141 | bool dirty = false; 142 | s1->SetIsDirty(dirty); 143 | s2->SetIsDirty(dirty); 144 | double area = slam_toolbox::LifelongSlamToolbox::computeAreaOverlapRatio(s1, s2); 145 | EXPECT_NEAR(area, 0.6666, 0.01); 146 | delete s1; 147 | delete s2; 148 | } 149 | 150 | TEST(LifelingMetricsTests, TestPtOverlap) 151 | { 152 | LocalizedRangeScan * s1 = new LocalizedRangeScan(); 153 | LocalizedRangeScan * s2 = new LocalizedRangeScan(); 154 | Pose2 p1 = Pose2(3.5, 4.0, 0.0); 155 | Pose2 p2 = Pose2(3.5, 5.5, 0.0); 156 | s1->SetBarycenterPose(p1); 157 | s2->SetBarycenterPose(p2); 158 | BoundingBox2 bb1, bb2; 159 | bb1.SetMinimum(Vector2(2.0, 2.0)); 160 | bb1.SetMaximum(Vector2(5.0, 6.0)); 161 | bb2.SetMinimum(Vector2(2.0, 4.0)); 162 | bb2.SetMaximum(Vector2(5.0, 7.0)); 163 | s1->SetBoundingBox(bb1); 164 | s2->SetBoundingBox(bb2); 165 | PointVectorDouble pts; 166 | pts.push_back(Vector2(3.0, 5.0)); // inside 167 | pts.push_back(Vector2(3.0, 3.0)); // outside 168 | s2->SetPointReadings(pts, true); 169 | bool dirty = false; 170 | s1->SetIsDirty(dirty); 171 | s2->SetIsDirty(dirty); 172 | double area = slam_toolbox::LifelongSlamToolbox::computeReadingOverlapRatio(s1, s2); 173 | EXPECT_EQ(area, 0.5); 174 | delete s1; 175 | delete s2; 176 | } 177 | 178 | } // namespace karto 179 | 180 | int main(int argc, char ** argv) 181 | { 182 | testing::InitGoogleTest(&argc, argv); 183 | return RUN_ALL_TESTS(); 184 | } 185 | 186 | #endif // SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_ 187 | -------------------------------------------------------------------------------- /test/process_constraints.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import matplotlib.pyplot as plt 3 | import random 4 | 5 | ''' 6 | Purpose: This is a simple graphing tool to find the relationship between number of nodes 7 | and number of constraints over time. 8 | 9 | To use: add the following code to publishGraph and disable the optimizer printouts 10 | 11 | std::map*> > vertices 12 | = mapper_->GetGraph()->GetVertices(); 13 | std::vector*>::const_iterator it; 14 | std::map vertex_ctr; 15 | for (it = vertices[mapper_->GetMapperSensorManager()->GetSensorNames()[0]].begin(); 16 | it != vertices[mapper_->GetMapperSensorManager()->GetSensorNames()[0]].end(); ++it) 17 | { 18 | int num = (*it)->GetEdges().size(); 19 | if (vertex_ctr.find(num) == vertex_ctr.end()) 20 | { 21 | vertex_ctr[num] = 1; 22 | } 23 | vertex_ctr[num]++; 24 | } 25 | 26 | std::cout << "UpdateMap: Vertex count: " << std::endl; 27 | std::map::const_iterator it2; 28 | for (it2 = vertex_ctr.begin(); it2 != vertex_ctr.end(); ++it2) 29 | { 30 | std::cout << it2->first << " constraints are in " 31 | << it2->second << " vertexes" << std::endl; 32 | } 33 | std::cout << std::endl; 34 | ''' 35 | 36 | 37 | def readFileToList(filename): 38 | with open(filename) as fp: 39 | line = fp.readline() 40 | lines = [] 41 | lines.append(line) 42 | while line: 43 | line = fp.readline() 44 | lines.append(line) 45 | return lines 46 | 47 | 48 | def getSingleSets(lines): 49 | measurements = [] 50 | measurement = [] 51 | for line in lines: 52 | if line == "\n": 53 | continue 54 | if "UpdateMap: Vertex count:" in line: 55 | measurements.append(measurement) 56 | measurement = [] 57 | measurement.append(line) 58 | return measurements[1:] 59 | 60 | 61 | def processForData(measurements): 62 | measurements_out = [] 63 | measurement_out = [] 64 | for measurement in measurements: 65 | for m in measurement: 66 | txt = m.split(" ") 67 | nums = [r for r in txt if r.isdigit()] 68 | if not nums: 69 | continue 70 | measurement_out.append(nums) 71 | measurements_out.append(measurement_out) 72 | measurement_out = [] 73 | return measurements_out 74 | 75 | 76 | def plotData(data): 77 | # plot lines 78 | 79 | # give us the number of lines to create and unque items 80 | keys = [] 81 | for d in data: 82 | for m in d: 83 | if m[0] not in keys: 84 | keys.append(m[0]) 85 | 86 | dat = {} 87 | for k in keys: 88 | dat[k] = [] 89 | 90 | for d in data: 91 | local_keys = [] 92 | for pt in d: 93 | local_keys.append(pt[0]) 94 | dat[pt[0]].append(int(pt[1])) 95 | for k in keys: 96 | if k not in local_keys: 97 | dat[k].append(0) 98 | 99 | total_nodes = [] 100 | for d in data: 101 | summ = 0 102 | for pt in d: 103 | summ = summ + int(pt[1]) 104 | total_nodes.append(summ) 105 | 106 | for k in keys: 107 | plt.plot([10*i for i in range(0, len(dat[k]))], dat[k], 108 | marker='o', color=[random.random(), random.random(), random.random()], 109 | linewidth=2, label=k+' Constraints') 110 | 111 | plt.plot([10*i for i in range(0, len(dat[k]))], 112 | total_nodes, marker='o', color=[random.random(), 113 | random.random(), random.random()], linewidth=2, label='Total Num. Nodes') 114 | plt.legend() 115 | plt.xlabel("time (s)") 116 | plt.ylabel("Node count") 117 | # plt.yscale("log") 118 | plt.show() 119 | 120 | 121 | if __name__ == "__main__": 122 | filename = sys.argv[1] 123 | print("reading file: " + filename) 124 | listOfContents = readFileToList(filename) 125 | listOfListOfMeasurements = getSingleSets(listOfContents) 126 | data = processForData(listOfListOfMeasurements) 127 | plotData(data) 128 | --------------------------------------------------------------------------------