Transform time stamp + sync/async options + snap generation script

README.md

@@ -2,6 +2,10 @@
 
 Fork on slam_karto. I've renamed since really there's about 5% of the code that's the same, everything else has been restructured or removed entirely. 
 
+For live running on robots, I recommend using the snap: slam-toolbox, it has optimizations in it that make it 10x faster. You need the deb/source install for the other developer level tools that don't need to be on the robot (rviz plugins, etc).
+
+I've seen this way maps building at 5x+ realtime up to about 20,000 sqft and 3x realtime up to about 60,000 sqft.
+
 ## Introduction 
 
 ### Solver Plugins

config/mapper_params_offline.yaml

@@ -21,11 +21,14 @@ base_frame: base_link
 debug_logging: false
 throttle_scans: 1
 publish_occupancy_map: true
-transform_publish_period: 0 #if 0 never publishes odometry
+transform_publish_period: 0.02 #if 0 never publishes odometry
 map_update_interval: 10.0
 resolution: 0.05
 max_laser_range: 20.0 #for rastering images
 minimum_time_interval: 0.5
+transform_timeout: 0.2
+sychronous: true
+online: false
 
 # General Parameters
 use_scan_matching: true

config/mapper_params_online_async.yaml

@@ -0,0 +1,68 @@
+
+# link_match_minimum_response_fine: 0.4 
+# link_scan_maximum_distance: 20.0
+# loop_search_maximum_distance: 50.0
+# distance_variance_penalty: 1.8             
+# angle_variance_penalty: 0.20    
+# minimum_angle_penalty: 1.6 #0.9
+
+# Plugin params
+solver_plugin: solver_plugins::CeresSolver
+ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+ceres_preconditioner: SCHUR_JACOBI
+ceres_trust_strategy: LEVENBERG_MARQUARDT
+ceres_dogleg_type: TRADITIONAL_DOGLEG
+ceres_loss_function: None
+
+# ROS Parameters
+odom_frame: odom
+map_frame: map
+base_frame: base_link
+debug_logging: false
+throttle_scans: 1
+publish_occupancy_map: true
+transform_publish_period: 0.02 #if 0 never publishes odometry
+map_update_interval: 5.0
+resolution: 0.05
+max_laser_range: 20.0 #for rastering images
+minimum_time_interval: 0.5
+transform_timeout: 0.2
+sychronous: false # whether we should use all readings and queue up - might be delayed. if off will skip readings but not lag
+online: true
+
+# General Parameters
+use_scan_matching: true
+use_scan_barycenter: true
+minimum_travel_distance: 0.5
+minimum_travel_heading: 0.5
+scan_buffer_size: 10
+scan_buffer_maximum_scan_distance: 10
+link_match_minimum_response_fine: 0.1  
+link_scan_maximum_distance: 1.5
+loop_search_maximum_distance: 3.0
+do_loop_closing: true 
+loop_match_minimum_chain_size: 10           
+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 Parameters - Loop Closure Parameters
+loop_search_space_dimension: 8.0
+loop_search_space_resolution: 0.05
+loop_search_space_smear_deviation: 0.03
+
+# Scan Matcher Parameters
+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        
+minimum_angle_penalty: 0.9
+minimum_distance_penalty: 0.5
+use_response_expansion: true

config/mapper_params_online.yaml → config/mapper_params_online_sync.yaml

@@ -26,6 +26,9 @@ map_update_interval: 5.0
 resolution: 0.05
 max_laser_range: 20.0 #for rastering images
 minimum_time_interval: 0.5
+transform_timeout: 0.2
+sychronous: true # whether we should use all readings and queue up - might be delayed. if off will skip readings but not lag
+online: true
 
 # General Parameters
 use_scan_matching: true

include/slam_toolbox/slam_toolbox.hpp

@@ -86,7 +86,7 @@ class SlamToolbox
   // threads
   void Run();
   void PublishVisualizations();
-  void PublishLoop(double transform_publish_period);
+  void PublishTransformLoop(double transform_publish_period);
 
   // setup
   void SetParams(ros::NodeHandle& nh);
@@ -141,9 +141,9 @@ class SlamToolbox
   // Storage for ROS parameters
   std::string odom_frame_, map_frame_, base_frame_;
   int throttle_scans_;
-  ros::Duration map_update_interval_;
+  ros::Duration map_update_interval_, transform_timeout_;
   double resolution_, minimum_time_interval_, minimum_travel_distance_;
-  bool publish_occupancy_map_, first_measurement_;
+  bool publish_occupancy_map_, first_measurement_, sychronous_, online_;
 
   // Karto bookkeeping
   karto::Mapper* mapper_;
@@ -155,6 +155,7 @@ class SlamToolbox
   int laser_count_;
   boost::thread *transform_thread_, *run_thread_, *visualization_thread_;
   tf::Transform map_to_odom_;
+  ros::Time map_to_odom_time_;
   bool inverted_laser_, pause_graph_, pause_processing_, pause_new_measurements_, interactive_mode_;
   double max_laser_range_;
   karto::Pose2 current_pose_;

launch/build_map_w_params_online_async.launch

@@ -0,0 +1,7 @@
+<launch>
+
+  <node pkg="slam_toolbox" type="slam_toolbox" name="slam_toolbox" output="screen">
+    <rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_online_async.yaml" />
+  </node>
+
+</launch>

launch/build_map_w_params_online.launch → launch/build_map_w_params_online_sync.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <node pkg="slam_toolbox" type="slam_toolbox" name="slam_toolbox" output="screen">
-    <rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_online.yaml" />
+    <rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_online_sync.yaml" />
   </node>
 
 </launch>

package.xml

@@ -3,9 +3,10 @@
   <description>
      This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
   </description>
-  <version>0.7.3</version>
+  <version>0.2.2</version>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <author>Brian Gerkey</author>
+  <author>Steve Macenski</author>
   <license>LGPL</license>
 
   <buildtool_depend>catkin</buildtool_depend>

snap/generate_snap

@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# make workspace
+mkdir -p snap_ws/src
+cd snap_ws && catkin_init_workspace
+cd src
+
+# add all the necessary things to it
+git clone -b kinetic-devel https://github.com/SteveMacenski/slam_toolbox.git
+git clone -b kinetic-devel https://github.com/SteveMacenski/KartoSDK-slam_toolbox.git
+git clone -b kinetic-devel https://github.com/ros-visualization/rviz.git
+cd ../
+
+# move snap and hooks to right place
+mkdir snap
+cp -r src/slam_toolbox/snap/snapcraft.yaml snap
+
+# build the snap
+snapcraft
+
+# go back to root
+cd ../

snap/snapcraft.yaml

@@ -1,5 +1,5 @@
 name: slam-toolbox
-version: '0.1.0'
+version: '0.2.1'
 summary: Slam Toolbox based on Karto's SDK.
 description: |
   A toolbox for building high quality maps of large spaces with a LIDAR.
@@ -10,17 +10,29 @@ confinement: devmode
 type: app
 
 apps:
-  slam-toolbox:
-    command: roslaunch slam_toolbox build_map_w_params.launch
+  slam-toolbox-online-sync:
+    command: roslaunch slam_toolbox build_map_w_params_online_sync.launch
     environment: 
       LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/lapack:$SNAP/usr/lib/libblas  # pluginlib (lapack, blas)
     plugs: [network, network-bind]
 
-  rviz:
-    command: desktop-launch $SNAP/opt/ros/kinetic/lib/rviz/rviz -d $ROS_PACKAGE_PATH/slam_toolbox/config/slam_toolbox_default.rviz
-    environment:
-      LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu/OGRE-1.9.0 # rviz plugin (ogre)
-    plugs: [network, network-bind, home, x11, opengl]
+  slam-toolbox-online-async:
+    command: roslaunch slam_toolbox build_map_w_params_online_async.launch
+    environment: 
+      LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/lapack:$SNAP/usr/lib/libblas
+    plugs: [network, network-bind]
+
+  slam-toolbox-offline:
+    command: roslaunch slam_toolbox build_map_w_params_offline.launch
+    environment: 
+      LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/lapack:$SNAP/usr/lib/libblas
+    plugs: [network, network-bind]
+
+  # rviz:
+  #   command: desktop-launch $SNAP/opt/ros/kinetic/lib/rviz/rviz -d $ROS_PACKAGE_PATH/slam_toolbox/config/slam_toolbox_default.rviz
+  #   environment:
+  #     LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu/OGRE-1.9.0 # rviz plugin (ogre)
+  #   plugs: [network, network-bind, home, x11, opengl]
 
   roscore:
     command: roscore
@@ -33,6 +45,6 @@ parts:
     include-roscore: true
     rosdistro: kinetic
     catkin-ros-master-uri: http://localhost:11311
-    catkin-packages: [open_karto, slam_toolbox, rviz]
+    catkin-packages: [open_karto, slam_toolbox] #rviz
     recursive-rosinstall: false
-    after: [desktop-qt5]
+    #after: [desktop-qt5]