SteveMacenski · john-maidbot · May 20, 2021 · Jun 1, 2021 · Jun 17, 2021 · Sep 14, 2021
diff --git a/.gitignore b/.gitignore
@@ -1 +1,2 @@
 snap_ws/*
+.vscode
diff --git a/CMake/FindCholmod.cmake → CMake/FindCHOLMOD.cmake b/CMake/FindCholmod.cmake → CMake/FindCHOLMOD.cmake
diff --git a/CMake/FindCSparse.cmake b/CMake/FindCSparse.cmake
@@ -21,5 +21,5 @@ FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
 )
 
 include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(CSPARSE DEFAULT_MSG
+find_package_handle_standard_args(CSparse DEFAULT_MSG
   CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -11,6 +11,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/CMake/")
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/karto_sdk/cmake)
 
 find_package(ament_cmake REQUIRED)
+find_package(maidbot_msg_utils REQUIRED)
+find_package(maidbot_std_srvs REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -20,18 +22,11 @@ find_package(tf2_ros REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(tf2_sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
-find_package(nav2_map_server REQUIRED)
-find_package(rviz_common REQUIRED)
-find_package(rviz_default_plugins REQUIRED)
-find_package(rviz_ogre_vendor REQUIRED)
-find_package(rviz_rendering REQUIRED)
-find_package(interactive_markers REQUIRED)
-find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
+# find_package(nav2_map_server REQUIRED)
 
 #karto_sdk lib
 set(BUILD_SHARED_LIBS ON)
@@ -41,50 +36,46 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
 
 set(dependencies
   rclcpp
+  maidbot_msg_utils
+  maidbot_std_srvs
   message_filters
   nav_msgs
   sensor_msgs
   tf2
   tf2_ros
   visualization_msgs
   pluginlib
-  nav2_map_server
   tf2_geometry_msgs
-  tf2_sensor_msgs
   std_msgs
   std_srvs
   builtin_interfaces
-  rviz_common
-  rviz_default_plugins
-  rviz_ogre_vendor
-  rviz_rendering
-  interactive_markers
-  Qt5
+  # nav2_map_server
 )
 
 set(libraries
     toolbox_common
-    SlamToolboxPlugin
     ceres_solver_plugin
     async_slam_toolbox
     sync_slam_toolbox
     localization_slam_toolbox
     lifelong_slam_toolbox
+    map_and_localization_slam_toolbox
 )
 
 find_package(PkgConfig REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(CHOLMOD REQUIRED)
 find_package(CSparse REQUIRED)
 find_package(G2O REQUIRED)
-find_package(Cholmod REQUIRED)
 find_package(LAPACK REQUIRED)
 find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 add_definitions(-DQT_NO_KEYWORDS)
 find_package(Boost REQUIRED system serialization filesystem thread)
 
-include_directories(include ${EIGEN3_INCLUDE_DIRS}
+include_directories(include lib/karto_sdk/include
+                            ${EIGEN3_INCLUDE_DIRS}
                             ${CHOLMOD_INCLUDE_DIR}
                             ${Boost_INCLUDE_DIRS}
                             ${TBB_INCLUDE_DIRS}
@@ -94,7 +85,6 @@ include_directories(include ${EIGEN3_INCLUDE_DIRS}
 add_definitions(${EIGEN3_DEFINITIONS})
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  srvs/Pause.srv
   srvs/ClearQueue.srv
   srvs/ToggleInteractive.srv
   srvs/Clear.srv
@@ -107,24 +97,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs
 )
 
-#### rviz Plugin
-qt5_wrap_cpp(MOC_FILES rviz_plugin/slam_toolbox_rviz_plugin.hpp)
-add_library(SlamToolboxPlugin SHARED
-  rviz_plugin/slam_toolbox_rviz_plugin.cpp
-  ${MOC_FILES})
-ament_target_dependencies(SlamToolboxPlugin
- ${dependencies}
-)
-target_include_directories(SlamToolboxPlugin PUBLIC
-  ${Qt5Widgets_INCLUDE_DIRS}
-  ${OGRE_INCLUDE_DIRS}
-)
-target_link_libraries(SlamToolboxPlugin ${QT_LIBRARIES} rviz_common::rviz_common)
-rosidl_target_interfaces(SlamToolboxPlugin ${PROJECT_NAME} "rosidl_typesupport_cpp")
-target_compile_definitions(SlamToolboxPlugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
-target_compile_definitions(SlamToolboxPlugin PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
-pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml) 
-
 #### Ceres Plugin
 add_library(ceres_solver_plugin solvers/ceres_solver.cpp)
 ament_target_dependencies(ceres_solver_plugin ${dependencies})
@@ -133,7 +105,8 @@ target_link_libraries(ceres_solver_plugin ${CERES_LIBRARIES}
                                           ${TBB_LIBRARIES}
                                           kartoSlamToolbox
 )
-rosidl_target_interfaces(ceres_solver_plugin ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(ceres_solver_plugin "${cpp_typesupport_target}")
 pluginlib_export_plugin_description_file(slam_toolbox solver_plugins.xml)
 
 #### Tool lib for mapping
@@ -142,7 +115,8 @@ ament_target_dependencies(toolbox_common
   ${dependencies}
 )
 target_link_libraries(toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
-rosidl_target_interfaces(toolbox_common ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(toolbox_common "${cpp_typesupport_target}")
 
 #### Mapping executibles
 add_library(async_slam_toolbox src/slam_toolbox_async.cpp)
@@ -165,9 +139,10 @@ target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${Bo
 add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp)
 target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox)
 
-#### Merging maps tool
-add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp)
-target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common)
+add_library(map_and_localization_slam_toolbox src/experimental/slam_toolbox_map_and_localization.cpp)
+target_link_libraries(map_and_localization_slam_toolbox localization_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
+add_executable(map_and_localization_slam_toolbox_node src/experimental/slam_toolbox_map_and_localization_node.cpp)
+target_link_libraries(map_and_localization_slam_toolbox_node map_and_localization_slam_toolbox)
 
 #### testing
 #if(BUILD_TESTING)
@@ -184,21 +159,13 @@ install(TARGETS async_slam_toolbox_node
                 sync_slam_toolbox_node
                 localization_slam_toolbox_node
                 lifelong_slam_toolbox_node
-                merge_maps_kinematic
+                map_and_localization_slam_toolbox_node
                 ${libraries}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(TARGETS SlamToolboxPlugin
-  EXPORT SlamToolboxPlugin
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
-)
-
 install(DIRECTORY include/
   DESTINATION include
 )
@@ -211,12 +178,11 @@ install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME}
 )
 
-install(FILES solver_plugins.xml rviz_plugins.xml
+install(FILES solver_plugins.xml
   DESTINATION share
 )
 
 ament_export_include_directories(include)
 ament_export_libraries(${libraries})
 ament_export_dependencies(${dependencies})
-ament_export_targets(SlamToolboxPlugin HAS_LIBRARY_TARGET)
 ament_package()
diff --git a/README.md b/README.md
@@ -24,7 +24,7 @@ You can find this work [here](https://joss.theoj.org/papers/10.21105/joss.02783)
 
 # Introduction
 
-Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained whil at Samsung Research, and largely in his free time. 
+Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained while at Samsung Research, and largely in his free time. 
 
 This project contains the ability to do most everything any other available SLAM library, both free and paid, and more. This includes:
 - Ordinary point-and-shoot 2D SLAM mobile robotics folks expect (start, map, save pgm file) with some nice built in utilities like saving maps
@@ -43,7 +43,7 @@ For running on live production robots, I recommend using the snap: slam-toolbox,
 
 This package has been benchmarked mapping building at 5x+ realtime up to about 30,000 sqft and 3x realtime up to about 60,000 sqft. with the largest area (I'm aware of) used was a 200,000 sq.ft. building in synchronous mode (e.i. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode. 
 
-The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work. This data is currently available upon request, but its going to be included in a larger open-source dataset down the line. 
+The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work.
 
 ![map_image](/images/circuit_launch.gif?raw=true "Map Image")
 
@@ -174,8 +174,10 @@ The following are the services/topics that are exposed for use. See the rviz plu
 
 ## Published topics
 
-| map  | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency | 
+| Topic  | Type | Description | 
 |-----|----|----|
+| map  | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency | 
+| pose | `geometry_msgs/PoseWithCovarianceStamped` | pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match |
 
 ## Exposed Services
 
@@ -236,27 +238,33 @@ The following settings and options are exposed to you. My default configuration
 
 `enable_interactive_mode` - Whether or not to allow for interactive mode to be enabled. Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode. As a result the memory for the process will increase. This is manually disabled in localization and lifelong modes since they would increase the memory utilization over time. Valid for either mapping or continued mapping modes.
 
+`position_covariance_scale` - Amount to scale position covariance when publishing pose from scan match.  This can be used to tune the influence of the pose position in a downstream localization filter.  The covariance represents the uncertainty of the measurement, so scaling up the covariance will result in the pose position having less influence on downstream filters.  Default: 1.0
+
+`yaw_covariance_scale` - Amount to scale yaw covariance when publishing pose from scan match.  See description of position_covariance_scale.  Default: 1.0
+
 `resolution` - Resolution of the 2D occupancy map to generate
 
 `max_laser_range` - Maximum laser range to use for 2D occupancy map rastering
 
 `minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode
 
+`maximum_match_interval` - Max interval in seconds between scan matches in async mode.  Note: Scan matches triggered by this condition won't update the map or scan buffer.  Default: -1 (infinity).
+
 `transform_timeout` - TF timeout for looking up transforms
 
 `tf_buffer_duration` - Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode. 
 
 `stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine.
 
-`minimum_travel_distance` - Minimum distance of travel before processing a new scan
+`minimum_travel_distance` - Minimum distance of travel before processing a new scan into the map, or adding a scan to the queue for processing in sync or localization modes.
 
 ## Matcher Params
 
 `use_scan_matching` - whether to use scan matching to refine odometric pose (uh, why would you not?)
 
 `use_scan_barycenter` - Whether to use the barycenter or scan pose
 
-`minimum_travel_heading` - Minimum changing in heading to justify an update
+`minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map
 
 `scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode
 

diff --git a/config/mapper_params_localization.yaml b/config/mapper_params_localization.yaml
@@ -36,18 +36,18 @@ slam_toolbox:
     minimum_travel_heading: 0.5
     scan_buffer_size: 3
     scan_buffer_maximum_scan_distance: 10.0
-    link_match_minimum_response_fine: 0.1  
+    link_match_minimum_response_fine: 0.1
     link_scan_maximum_distance: 1.5
-    do_loop_closing: true 
+    do_loop_closing: true
     loop_match_minimum_chain_size: 3
-    loop_match_maximum_variance_coarse: 3.0  
-    loop_match_minimum_response_coarse: 0.35    
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
     loop_match_minimum_response_fine: 0.45
 
     # Correlation Parameters - Correlation Parameters
     correlation_search_space_dimension: 0.5
     correlation_search_space_resolution: 0.01
-    correlation_search_space_smear_deviation: 0.1 
+    correlation_search_space_smear_deviation: 0.1
 
     # Correlation Parameters - Loop Closure Parameters
     loop_search_space_dimension: 8.0
@@ -56,12 +56,12 @@ slam_toolbox:
     loop_search_maximum_distance: 3.0
 
     # Scan Matcher Parameters
-    distance_variance_penalty: 0.5      
-    angle_variance_penalty: 1.0    
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
 
-    fine_search_angle_offset: 0.00349     
-    coarse_search_angle_offset: 0.349   
-    coarse_angle_resolution: 0.0349        
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
     minimum_angle_penalty: 0.9
     minimum_distance_penalty: 0.5
     use_response_expansion: true
diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml
@@ -25,7 +25,6 @@ slam_toolbox:
     transform_timeout: 0.2
     tf_buffer_duration: 14400.
     stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
-    enable_interactive_mode: true
 
     # General Parameters
     use_scan_matching: true

diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml
@@ -33,7 +33,6 @@ slam_toolbox:
     transform_timeout: 0.2
     tf_buffer_duration: 30.
     stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
-    enable_interactive_mode: true
 
     # General Parameters
     use_scan_matching: true

diff --git a/config/mapper_params_online_sync.yaml b/config/mapper_params_online_sync.yaml
@@ -33,7 +33,6 @@ slam_toolbox:
     transform_timeout: 0.2
     tf_buffer_duration: 30.
     stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
-    enable_interactive_mode: true
 
     # General Parameters
     use_scan_matching: true