From 35e78cf4044e8153c8c3295edda796d29943e0bc Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 3 Aug 2018 11:53:05 -0700
Subject: [PATCH 01/17] adding ros duration

---
 config/mapper_params_offline.yaml     |  1 +
 config/mapper_params_online.yaml      |  1 +
 include/slam_toolbox/slam_toolbox.hpp |  5 +++--
 src/slam_toolbox.cpp                  | 23 +++++++++++++++++++----
 4 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml
index db5fbec0..857da16a 100644
--- a/config/mapper_params_offline.yaml
+++ b/config/mapper_params_offline.yaml
@@ -26,6 +26,7 @@ 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
 
 # General Parameters
 use_scan_matching: true
diff --git a/config/mapper_params_online.yaml b/config/mapper_params_online.yaml
index 8ce8e5d7..701ae271 100644
--- a/config/mapper_params_online.yaml
+++ b/config/mapper_params_online.yaml
@@ -26,6 +26,7 @@ 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
 
 # General Parameters
 use_scan_matching: true
diff --git a/include/slam_toolbox/slam_toolbox.hpp b/include/slam_toolbox/slam_toolbox.hpp
index 5b54e485..c7601ce4 100644
--- a/include/slam_toolbox/slam_toolbox.hpp
+++ b/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,7 +141,7 @@ 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_;
 
@@ -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_;
diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index ff665944..8243f23b 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -37,6 +37,8 @@ SlamToolbox::SlamToolbox() :
                          interactive_mode_(false),
                          laser_count_(0),
                          tf_(ros::Duration(14400.)),
+                         map_to_odom_time_(0.),
+                         transform_timeout_(ros::Duration(0.2)),
                          first_measurement_(true) // 4 hours
 /*****************************************************************************/
 {
@@ -59,8 +61,9 @@ SlamToolbox::SlamToolbox() :
   double transform_publish_period;
   private_nh.param("transform_publish_period", 
                                                transform_publish_period, 0.05);
-  transform_thread_ = new boost::thread(boost::bind(&SlamToolbox::PublishLoop, 
-                                              this, transform_publish_period));
+  transform_thread_ = new boost::thread( \
+                                boost::bind(&SlamToolbox::PublishTransformLoop, 
+                                this, transform_publish_period));
   run_thread_ = new boost::thread(boost::bind(&SlamToolbox::Run, this));
   visualization_thread_ = new boost::thread(\
                        boost::bind(&SlamToolbox::PublishVisualizations, this));
@@ -97,6 +100,15 @@ void SlamToolbox::SetParams(ros::NodeHandle& private_nh_)
 {
   map_to_odom_.setIdentity();
 
+  double timeout;
+  if(!private_nh_.getParam("transform_timeout", timeout))
+  {
+    transform_timeout_ = ros::Duration(0.2);
+  }
+  else
+  {
+    transform_timeout_ = ros::Duration(timeout);
+  }
   if(!private_nh_.getParam("odom_frame", odom_frame_))
     odom_frame_ = "odom";
   if(!private_nh_.getParam("map_frame", map_frame_))
@@ -306,7 +318,7 @@ SlamToolbox::~SlamToolbox()
 }
 
 /*****************************************************************************/
-void SlamToolbox::PublishLoop(double transform_publish_period)
+void SlamToolbox::PublishTransformLoop(double transform_publish_period)
 /*****************************************************************************/
 {
   if(transform_publish_period == 0)
@@ -319,7 +331,7 @@ void SlamToolbox::PublishLoop(double transform_publish_period)
       boost::mutex::scoped_lock lock(map_to_odom_mutex_);
       ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05);
       tfB_->sendTransform(tf::StampedTransform (map_to_odom_, 
-                                   ros::Time::now(), map_frame_, odom_frame_));
+             map_to_odom_time_ + transform_timeout_, map_frame_, odom_frame_)); // TODO time should be time taken but is that up to date when behind?
     }
     r.sleep();
   }
@@ -815,12 +827,15 @@ bool SlamToolbox::AddScan(karto::LaserRangeFinder* laser,
       boost::mutex::scoped_lock lock(map_to_odom_mutex_);
       map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
                               tf::Point( odom_to_map.getOrigin() ) ).inverse();
+      map_to_odom_time_ = scan->header.stamp;
     }
     // Add the localized range scan to the dataset (for memory management)
     dataset_->Add(range_scan);
   }
   else
+  {
     delete range_scan;
+  }
 
   return processed;
 }

From 53770699eb7ea866ecea820e84f5f781f07aac3e Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 3 Aug 2018 13:46:06 -0700
Subject: [PATCH 02/17] adding async / sync options for live mode

---
 config/mapper_params_offline.yaml     |  1 +
 config/mapper_params_online.yaml      |  1 +
 include/slam_toolbox/slam_toolbox.hpp |  2 +-
 src/slam_toolbox.cpp                  | 37 +++++++++++++++++++++++----
 4 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml
index 857da16a..e70f8b68 100644
--- a/config/mapper_params_offline.yaml
+++ b/config/mapper_params_offline.yaml
@@ -27,6 +27,7 @@ resolution: 0.05
 max_laser_range: 20.0 #for rastering images
 minimum_time_interval: 0.5
 transform_timeout: 0.2
+sychronous: false
 
 # General Parameters
 use_scan_matching: true
diff --git a/config/mapper_params_online.yaml b/config/mapper_params_online.yaml
index 701ae271..8bb4cfdf 100644
--- a/config/mapper_params_online.yaml
+++ b/config/mapper_params_online.yaml
@@ -27,6 +27,7 @@ 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
 
 # General Parameters
 use_scan_matching: true
diff --git a/include/slam_toolbox/slam_toolbox.hpp b/include/slam_toolbox/slam_toolbox.hpp
index c7601ce4..f1618df9 100644
--- a/include/slam_toolbox/slam_toolbox.hpp
+++ b/include/slam_toolbox/slam_toolbox.hpp
@@ -143,7 +143,7 @@ class SlamToolbox
   int throttle_scans_;
   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_;
 
   // Karto bookkeeping
   karto::Mapper* mapper_;
diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index 8243f23b..2651545a 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -100,6 +100,8 @@ void SlamToolbox::SetParams(ros::NodeHandle& private_nh_)
 {
   map_to_odom_.setIdentity();
 
+  if(!private_nh_.getParam("sychronous", sychronous_))
+    sychronous_ = true;
   double timeout;
   if(!private_nh_.getParam("transform_timeout", timeout))
   {
@@ -272,7 +274,7 @@ void SlamToolbox::SetROSInterfaces(ros::NodeHandle& node)
   ssClear_manual_ = node.advertiseService("clear_changes", &SlamToolbox::ClearChangesCallback, this);
   ssSave_map_ = node.advertiseService("save_map", &SlamToolbox::SaveMapCallback, this);
   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node, "/scan", 5);
-  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
+  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 2);
   scan_filter_->registerCallback(boost::bind(&SlamToolbox::LaserCallback, this, _1));
   marker_publisher_ = node.advertise<visualization_msgs::MarkerArray>("karto_graph_visualization",1);
   scan_publisher_ = node.advertise<sensor_msgs::LaserScan>("karto_scan_visualization",10);
@@ -686,10 +688,26 @@ void SlamToolbox::LaserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     return;
   }
 
-  // ok... maybe valid we can try
-  q_.push(posed_scan(scan, pose));
-  last_scan_time = scan->header.stamp.toSec(); 
-  last_pose = pose;
+  if (sychronous_) // todo async
+  {
+    // synchronous
+    q_.push(posed_scan(scan, pose));
+    last_scan_time = scan->header.stamp.toSec(); 
+    last_pose = pose;   
+  }
+  else
+  {
+    // asynchonous
+    karto::LaserRangeFinder* laser = GetLaser(scan);
+
+    if(!laser)
+    {
+      ROS_WARN("Failed to create laser device for %s; discarding scan",
+         scan->header.frame_id.c_str());
+      return;
+    }
+    AddScan(laser, scan, pose);
+  }
 
   return;
 }
@@ -1054,6 +1072,15 @@ bool SlamToolbox::IsPaused(const PausedApplication& app)
 void SlamToolbox::Run()
 /*****************************************************************************/
 {
+  if (!sychronous_) // todo async
+  {
+    // asychronous - don't need to run to dequeue
+    ROS_INFO("Exiting Run thread - asynchronous mode selected.");
+    return;
+  }
+
+  ROS_INFO_ONCE("Run thread enabled - synchronous mode selected.");
+
   ros::Rate r(60);
   while(ros::ok())
   {

From 98a83d8784a0b5abe0d1caab8d678d334adf3d3e Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 3 Aug 2018 13:46:17 -0700
Subject: [PATCH 03/17] generation snap script

---
 snap/generate_snap | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)
 create mode 100755 snap/generate_snap

diff --git a/snap/generate_snap b/snap/generate_snap
new file mode 100755
index 00000000..fc65d4fe
--- /dev/null
+++ b/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 ../

From 8f1b087ec22d27ff6d64d7da6598fd907bd58238 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 3 Aug 2018 13:51:55 -0700
Subject: [PATCH 04/17] logging updates for reasonable prints

---
 src/slam_toolbox.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index 2651545a..28f58967 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -474,7 +474,7 @@ void SlamToolbox::PublishGraph()
     return;
   }
 
-  ROS_INFO_THROTTLE(15.,"Graph size: %i",(int)graph.size());
+  ROS_INFO("Graph size: %i",(int)graph.size());
   bool interactive_mode = false;
   {
     boost::mutex::scoped_lock lock(interactive_mutex_);
@@ -1088,7 +1088,11 @@ void SlamToolbox::Run()
     {
       posed_scan scan_w_pose = q_.front();
       q_.pop();
-      ROS_INFO_THROTTLE(15., "Queue size: %i", (int)q_.size());
+
+      if (q_.size() > 2)
+      {
+        ROS_WARN("Queue size: %i", (int)q_.size());
+      }
 
       // Check whether we know about this laser yet
       karto::LaserRangeFinder* laser = GetLaser(scan_w_pose.scan);

From 9036b45530c590a6e53992ee33ada86a31cb677b Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 3 Aug 2018 14:00:51 -0700
Subject: [PATCH 05/17] readme

---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index 8a061919..0b640733 100644
--- a/README.md
+++ b/README.md
@@ -2,6 +2,8 @@
 
 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).
+
 ## Introduction 
 
 ### Solver Plugins

From 2d882b2b0e4f08f058b235a7ec1311045fb8c122 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 16:21:38 -0700
Subject: [PATCH 06/17] optional sync and async modes for live

---
 config/mapper_params_offline.yaml             |  5 +-
 ...e.yaml => mapper_params_online_async.yaml} |  1 +
 config/mapper_params_online_sync.yaml         | 68 +++++++++++++++++++
 include/slam_toolbox/slam_toolbox.hpp         |  2 +-
 launch/build_map_w_params_online_async.launch |  7 ++
 ... => build_map_w_params_online_sync.launch} |  2 +-
 snap/snapcraft.yaml                           | 30 +++++---
 src/slam_toolbox.cpp                          | 18 +++--
 8 files changed, 116 insertions(+), 17 deletions(-)
 rename config/{mapper_params_online.yaml => mapper_params_online_async.yaml} (99%)
 create mode 100644 config/mapper_params_online_sync.yaml
 create mode 100644 launch/build_map_w_params_online_async.launch
 rename launch/{build_map_w_params_online.launch => build_map_w_params_online_sync.launch} (84%)

diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml
index e70f8b68..044db7f7 100644
--- a/config/mapper_params_offline.yaml
+++ b/config/mapper_params_offline.yaml
@@ -21,13 +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: false
+sychronous: true
+online: false
 
 # General Parameters
 use_scan_matching: true
diff --git a/config/mapper_params_online.yaml b/config/mapper_params_online_async.yaml
similarity index 99%
rename from config/mapper_params_online.yaml
rename to config/mapper_params_online_async.yaml
index 8bb4cfdf..ea9d93a5 100644
--- a/config/mapper_params_online.yaml
+++ b/config/mapper_params_online_async.yaml
@@ -28,6 +28,7 @@ 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
diff --git a/config/mapper_params_online_sync.yaml b/config/mapper_params_online_sync.yaml
new file mode 100644
index 00000000..cd4de0a9
--- /dev/null
+++ b/config/mapper_params_online_sync.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: 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
+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
diff --git a/include/slam_toolbox/slam_toolbox.hpp b/include/slam_toolbox/slam_toolbox.hpp
index f1618df9..499b1e7e 100644
--- a/include/slam_toolbox/slam_toolbox.hpp
+++ b/include/slam_toolbox/slam_toolbox.hpp
@@ -143,7 +143,7 @@ class SlamToolbox
   int throttle_scans_;
   ros::Duration map_update_interval_, transform_timeout_;
   double resolution_, minimum_time_interval_, minimum_travel_distance_;
-  bool publish_occupancy_map_, first_measurement_, sychronous_;
+  bool publish_occupancy_map_, first_measurement_, sychronous_, online_;
 
   // Karto bookkeeping
   karto::Mapper* mapper_;
diff --git a/launch/build_map_w_params_online_async.launch b/launch/build_map_w_params_online_async.launch
new file mode 100644
index 00000000..b586f146
--- /dev/null
+++ b/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>
diff --git a/launch/build_map_w_params_online.launch b/launch/build_map_w_params_online_sync.launch
similarity index 84%
rename from launch/build_map_w_params_online.launch
rename to launch/build_map_w_params_online_sync.launch
index 922e654c..02e74453 100644
--- a/launch/build_map_w_params_online.launch
+++ b/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>
diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml
index 84df5520..1b96c04e 100644
--- a/snap/snapcraft.yaml
+++ b/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]
diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index 28f58967..09424e59 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -100,6 +100,8 @@ void SlamToolbox::SetParams(ros::NodeHandle& private_nh_)
 {
   map_to_odom_.setIdentity();
 
+  if(!private_nh_.getParam("online", online_))
+    online_ = true;
   if(!private_nh_.getParam("sychronous", sychronous_))
     sychronous_ = true;
   double timeout;
@@ -333,7 +335,7 @@ void SlamToolbox::PublishTransformLoop(double transform_publish_period)
       boost::mutex::scoped_lock lock(map_to_odom_mutex_);
       ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05);
       tfB_->sendTransform(tf::StampedTransform (map_to_odom_, 
-             map_to_odom_time_ + transform_timeout_, map_frame_, odom_frame_)); // TODO time should be time taken but is that up to date when behind?
+             map_to_odom_time_ + transform_timeout_, map_frame_, odom_frame_));
     }
     r.sleep();
   }
@@ -688,7 +690,7 @@ void SlamToolbox::LaserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     return;
   }
 
-  if (sychronous_) // todo async
+  if (sychronous_)
   {
     // synchronous
     q_.push(posed_scan(scan, pose));
@@ -1072,7 +1074,7 @@ bool SlamToolbox::IsPaused(const PausedApplication& app)
 void SlamToolbox::Run()
 /*****************************************************************************/
 {
-  if (!sychronous_) // todo async
+  if (!sychronous_)
   {
     // asychronous - don't need to run to dequeue
     ROS_INFO("Exiting Run thread - asynchronous mode selected.");
@@ -1091,7 +1093,15 @@ void SlamToolbox::Run()
 
       if (q_.size() > 2)
       {
-        ROS_WARN("Queue size: %i", (int)q_.size());
+        if (online_ && sychronous_) // always sync. but makes intent clearer
+        {
+          ROS_WARN("Queue size has grown to: %i. "
+                   "Recommend stopping until message is gone.", (int)q_.size());
+        }
+        else
+        {
+          ROS_WARN_THROTTLE(10., "Queue size: %i", (int)q_.size());
+        }
       }
 
       // Check whether we know about this laser yet

From 2ce0559689ed6259118f4cfd2e610bd85316e342 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 16:30:04 -0700
Subject: [PATCH 07/17] adding CI

---
 .snapcraft/travis_snapcraft.cfg | Bin 0 -> 2464 bytes
 .travis.yml                     |  16 ++++++++++++++++
 snap/generate_snap              |   2 +-
 3 files changed, 17 insertions(+), 1 deletion(-)
 create mode 100644 .snapcraft/travis_snapcraft.cfg
 create mode 100644 .travis.yml

diff --git a/.snapcraft/travis_snapcraft.cfg b/.snapcraft/travis_snapcraft.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..03aa13e3becfc300a88aec7020d77e60e38d8d27
GIT binary patch
literal 2464
zcmV;R319XD5Lon-@U(`~p<=vhcR{SsQ80vBDDO@$Ja}U@H@XWMAA09YvV_JCqXY*0
zQ7?JTm>byOnt@f+x}g(QX;^1e!Z`ji2vGr?p<W|yHuCta{!+8fDOeppf(r0kvuc&?
z$aST=c)#SZo&|J|6730pv|ijPvWvYn0tAsiyBoy-&Me47fb+yP@yeGp+kThIVO7gA
zdw=zr`7mW%9Nz6a#p@J(Q53lVVVhR1<=8Mtp3-=sDlGTRLKN6p1@iiCK#f9nv8(h8
z^$QTB6LTEW=45!7qlbuR&TH6fcf~_(Fff}jO-@Gl;4+<Qb$Ln6w32*mG2nhJaO+eZ
zZk`5jsRjl<=7doH(s&UrQH-ENSOo}$RtrQ&>sw6U;H^`?5&jzDmPtwh6jvEUWsr1G
zUf;L*Y|%ZD1sZ6{T9URJsbhcNNFP-^3!Xiq{WdIDztQ%BM;@8)0}jHSM+|i-xY^}t
zel)xnljip(0RIY3Ct9XZYAiomS&?YvwW8sBxdqXo0V9j<sH$EEL4~L)yx?tNjtOag
zx`_1KvTqabDA2#DqbR16>h$ntjZ#)sP8X6e{1`^`TmeMYLiHn~cLx@M`PGM@u3n(n
zHB?bk>&;S0?JFk`Jo@GJor5%d6Zu7_OO5qZzBQUZC~?X`(Ly+;b{HX|Q3C%e?L5$;
z=FELvvCDOde0O<6QQY6tbRRwPB1th7-<kbpaSfyHMKmW%@Wl7YPKnh8ZK?=&w|BRC
zJo3dC!NdsC<-qq&`~X?DP1$I!GC))2+lu@oeENvpYthZ3<|9ER-o;x;qmuLyay8Wn
zfvS3sTLAo<+k#049`$0L+tau#48C<C&aT=iq@4A7Uyk$d4oRZ7@@(jsgq#HXtzD69
z3E{&|crwZIJW0PUOEIiT9%r!+DZ9!8rz?A8?A)|^x9tB9IAkfW*RcR``pq&j{$sy=
zK$Y&MjCW=USmO`|^_n73s!(OR8_UW`DOe!Lf~&!srDtR~NUX$LmIx+)fm+g-Bbs~o
zGkD`l25(&<;jT-(<WscwVr8=$j-I|6$OA3Jo`N%Z8oqI~F%%K{H>j_nPUgzA{MB%f
zYLpsJBgHwW^qPb)0a}nhrj8Hxvd{K{DJX-Ym*0uEptB3zB>22i!Cyxd4fw+4U+JOB
zh_pT@^Y$y!u@SGa!oD#wgBh{CLn-*1!shRFW6+R3(r&__*119f)3jb<kNi28T`^JX
zk+#zJ`Ovogwb1Dl83e*J^_61+-k4HngTCW>qnoQqY;0uWM!SXgm2NwD83zfI5;iB-
z&JCF5hUvQHUYmIQr~JE%Q=)K+rRmHl;}@P?zX<2bOB%5iIG#dz#XqYlwQJDD%?d%V
zwdOi*$7KcF8)ThxiM11<*I)y%;Ej8if0NJ-?<8x6GE8Cd)veJkj)FWmy6EYlh<5lO
zd7aef)P?Ld5fB$5zX!@BX)F?CEv0F<kJaPyv}&cz=3D8wv&?=!W&)CW)J;6RD72a&
z#BXhh`%M++x`rZ$&?kv^Dl(^_y(k;CR#K|G&mksvc~o^YQQxK!If-FKW*RQv@_3lM
z8F9gd(-u^ZSQ6!9Q@;a_7$hn^!+{&5yRIn@GEx}XF*VH3<8YRz+&1-CU^p*Jo*ce}
z0H~feO#)hU)n3zfLwY=#dHNVqKGp*dF1=JkpyG9wRnp)Iynz}=j05m8`~~BS*Y$i0
z5Dr!Of)VW~VyTPOhyz8{x^sJ9ogfQ>&YPq1YWjl=e}_O{23|{505rHjDnbgk`x{cO
zUl@fhMaB0i%md>|8`mUZ2yU~|lbu;s*g=ZIGt6>yY(%iK6>9Ux8xV?OkhRuzK$v2D
zJyMK#73HeO^2h=OqV`ldEx284H<=MF-)Dv^TB`+YHUnCKE8XKK?1~~<{(VZu%i*G<
zS;BJ3K?iW{(p4P1(?FhN!ExDH)Z>4r<bFJAn|PQkPkMe?$$#l&=BTb#(i1M!oz5%(
z0!w&}m+pqUTA^3sKqBtHf2_<ob<w9fn+|$q{sON?>j;}LQ=Pijv1pCtwaXaHIi?3&
z$0Jhki7){FrpbxavI0QpH}o?CyxbC6&7YL-Ge^~8HZR(@o?elu2P)#BF|&AG@(H3W
zi~*$g5`9U}ycH!xAhOkg&^^;fsX>s1W<;|FCvG^Ac~~y&=qPl;cf$ld@@^Vhv9W&0
zxSb#-Y7P5d2zB1SIG)zEyemqq<>Q^-x?7!`9+t~`L&%hNy6(l-_V%AH#A2K3-rjt*
zeJSK`tFtu<c0;ZE;<9!^$}TIw_2t~Io6!uQJ&=W>m*<(IpUl1`r)>IFcejf3+JzJx
zN6NLZ2XLBSQKK+_VQOjtdV2ofo<&R@d#S{S(xlT^P$sm6+DJnu4PTIgQ5b*>GUHm?
zdb-Ej0ozhP4rkw5JFrBa;tz}v!kY^y_G%sdBOIn38FB@y1VExZ9t2VO4n^yx04mPA
zrmvgqxmRagKJZe(PYKM;vrmsiizU!Zy7@0{3z2b24TdZ&;yf9%l_=>#yt6}|r3p)K
zr3#z}MIEVta3kOG5kDmaXekygPz<J7%SN_=%ame>@*gA5@lB)F8a;AC8T;&4aE6*?
zoan&HeKBeRN0)e|P_p=oL?AwZ#xJ8D=uWH>7;`)`JnfGyjm!I5(X_L%S3|n*6IC<#
zidCBG2r2>q%Dh?aZG=1WJM#*6Ix5Ruaxo>+0_u39g^PSo8-L<7rrYV6rJ=QOAp>pn
zwjD9FrjgqbbmMx!4<v<g_*F~G5F;uzgo?1waI<BMHz8BEoEjpc2V9LNV+M;Ej4;}?
z6d1yThszVZ`*n7mYtFhY>>B3L3rFXDG=WH}<LV{6=S*yA>h<9Z&%K9f47hwMEtNfJ
zv)Q~F0U?+XitwVak1^>`<Tzst{_mE2dy9Xf$ctWNr<q=!L!sY{SERu7h}!G*%EgWM
zLlXGqSEQ@#rlEjFkdehTw29)dQ9i1RaaGIi9g3-~y928Oqq8<(Dnhu~*IA0IM|OP0
ztM3gXRh<p7y#Au}rwiNlU^{&v&k@62Qe_LtO$wyR!XCSC5)4AgIne83vz?~mVm1F9
z@#)+|j?0s;vfYLKBcae_N>T(anY}hO?`noYKqba=>dCUkuQO~TLGpMW>Fbac^o^}3
ztStgt4>#%Lb~&odyrXug+uxv41|fXmLrf^MSW!n5em^YS24Pfagsurv<)1}nFolJk
ej+Yi^mTnY|^cg}~k11?#_&U~SAl}#u{xn~Qfy3MY

literal 0
HcmV?d00001

diff --git a/.travis.yml b/.travis.yml
new file mode 100644
index 00000000..033aa78e
--- /dev/null
+++ b/.travis.yml
@@ -0,0 +1,16 @@
+language: bash
+sudo: required
+services: [docker]
+after_success:
+- openssl aes-256-cbc -K $encrypted_04c78d356583_key -iv $encrypted_04c78d356583_iv
+  -in .snapcraft/travis_snapcraft.cfg -out .snapcraft/snapcraft.cfg -d
+deploy:
+  skip_cleanup: true
+  provider: script
+  script: docker run -v $(pwd):$(pwd) -t snapcore/snapcraft sh -c
+          "apt update -qq &&
+           cd $(pwd) &&
+           ./snap/generate_snap.sh &&
+           if [ "${TRAVIS_PULL_REQUEST}" = "false" ] && [ "${TRAVIS_BRANCH}" = "master" ] ; then 
+             snapcraft push snap_ws/*.snap --release edge;
+           fi"
diff --git a/snap/generate_snap b/snap/generate_snap
index fc65d4fe..2e761da7 100755
--- a/snap/generate_snap
+++ b/snap/generate_snap
@@ -8,7 +8,7 @@ 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
+#git clone -b kinetic-devel https://github.com/ros-visualization/rviz.git
 cd ../
 
 # move snap and hooks to right place

From 6899dc5e847fa1cadce30578866d519cf7c6b42f Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 16:41:15 -0700
Subject: [PATCH 08/17] run all branches

---
 .travis.yml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.travis.yml b/.travis.yml
index 033aa78e..2d4fdee3 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -14,3 +14,5 @@ deploy:
            if [ "${TRAVIS_PULL_REQUEST}" = "false" ] && [ "${TRAVIS_BRANCH}" = "master" ] ; then 
              snapcraft push snap_ws/*.snap --release edge;
            fi"
+  on:
+    branch: /.*/

From a25fae68ba19a17756dfef1bc3f60013964d8d4e Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 16:44:44 -0700
Subject: [PATCH 09/17] run all branches

---
 .travis.yml | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 2d4fdee3..2e0e6863 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -14,5 +14,4 @@ deploy:
            if [ "${TRAVIS_PULL_REQUEST}" = "false" ] && [ "${TRAVIS_BRANCH}" = "master" ] ; then 
              snapcraft push snap_ws/*.snap --release edge;
            fi"
-  on:
-    branch: /.*/
+  all_branches: true

From 2832e6180f82a16709cc815053ac2f1b5bcd1431 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 16:48:32 -0700
Subject: [PATCH 10/17] run all branches

---
 .travis.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 2e0e6863..92f4bfae 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -14,4 +14,5 @@ deploy:
            if [ "${TRAVIS_PULL_REQUEST}" = "false" ] && [ "${TRAVIS_BRANCH}" = "master" ] ; then 
              snapcraft push snap_ws/*.snap --release edge;
            fi"
-  all_branches: true
+  on:
+    all_branches: true

From f2a2ed0a2c3938149a572dec5f9b21f97eebd502 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 16:51:35 -0700
Subject: [PATCH 11/17] rename script

---
 .travis.yml          | 2 +-
 src/slam_toolbox.cpp | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 92f4bfae..aa42cac0 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -10,7 +10,7 @@ deploy:
   script: docker run -v $(pwd):$(pwd) -t snapcore/snapcraft sh -c
           "apt update -qq &&
            cd $(pwd) &&
-           ./snap/generate_snap.sh &&
+           ./snap/generate_snap &&
            if [ "${TRAVIS_PULL_REQUEST}" = "false" ] && [ "${TRAVIS_BRANCH}" = "master" ] ; then 
              snapcraft push snap_ws/*.snap --release edge;
            fi"
diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index 09424e59..03e4c1e9 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -333,7 +333,6 @@ void SlamToolbox::PublishTransformLoop(double transform_publish_period)
   {
     {
       boost::mutex::scoped_lock lock(map_to_odom_mutex_);
-      ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05);
       tfB_->sendTransform(tf::StampedTransform (map_to_odom_, 
              map_to_odom_time_ + transform_timeout_, map_frame_, odom_frame_));
     }

From 72eebe0c29d2e052eddde87d3e7cb93198cda43a Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 17:06:21 -0700
Subject: [PATCH 12/17] rosdep - bring back rviz

---
 snap/generate_snap  | 2 +-
 snap/snapcraft.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/snap/generate_snap b/snap/generate_snap
index 2e761da7..fc65d4fe 100755
--- a/snap/generate_snap
+++ b/snap/generate_snap
@@ -8,7 +8,7 @@ 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
+git clone -b kinetic-devel https://github.com/ros-visualization/rviz.git
 cd ../
 
 # move snap and hooks to right place
diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml
index 1b96c04e..8d0063c4 100644
--- a/snap/snapcraft.yaml
+++ b/snap/snapcraft.yaml
@@ -45,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]

From e85e12b46691e66f8f74330ce488c6ba5709283b Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Mon, 6 Aug 2018 17:21:58 -0700
Subject: [PATCH 13/17] remove travis

---
 .snapcraft/travis_snapcraft.cfg | Bin 2464 -> 0 bytes
 .travis.yml                     |  18 ------------------
 snap/snapcraft.yaml             |   4 ++--
 3 files changed, 2 insertions(+), 20 deletions(-)
 delete mode 100644 .snapcraft/travis_snapcraft.cfg
 delete mode 100644 .travis.yml

diff --git a/.snapcraft/travis_snapcraft.cfg b/.snapcraft/travis_snapcraft.cfg
deleted file mode 100644
index 03aa13e3becfc300a88aec7020d77e60e38d8d27..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2464
zcmV;R319XD5Lon-@U(`~p<=vhcR{SsQ80vBDDO@$Ja}U@H@XWMAA09YvV_JCqXY*0
zQ7?JTm>byOnt@f+x}g(QX;^1e!Z`ji2vGr?p<W|yHuCta{!+8fDOeppf(r0kvuc&?
z$aST=c)#SZo&|J|6730pv|ijPvWvYn0tAsiyBoy-&Me47fb+yP@yeGp+kThIVO7gA
zdw=zr`7mW%9Nz6a#p@J(Q53lVVVhR1<=8Mtp3-=sDlGTRLKN6p1@iiCK#f9nv8(h8
z^$QTB6LTEW=45!7qlbuR&TH6fcf~_(Fff}jO-@Gl;4+<Qb$Ln6w32*mG2nhJaO+eZ
zZk`5jsRjl<=7doH(s&UrQH-ENSOo}$RtrQ&>sw6U;H^`?5&jzDmPtwh6jvEUWsr1G
zUf;L*Y|%ZD1sZ6{T9URJsbhcNNFP-^3!Xiq{WdIDztQ%BM;@8)0}jHSM+|i-xY^}t
zel)xnljip(0RIY3Ct9XZYAiomS&?YvwW8sBxdqXo0V9j<sH$EEL4~L)yx?tNjtOag
zx`_1KvTqabDA2#DqbR16>h$ntjZ#)sP8X6e{1`^`TmeMYLiHn~cLx@M`PGM@u3n(n
zHB?bk>&;S0?JFk`Jo@GJor5%d6Zu7_OO5qZzBQUZC~?X`(Ly+;b{HX|Q3C%e?L5$;
z=FELvvCDOde0O<6QQY6tbRRwPB1th7-<kbpaSfyHMKmW%@Wl7YPKnh8ZK?=&w|BRC
zJo3dC!NdsC<-qq&`~X?DP1$I!GC))2+lu@oeENvpYthZ3<|9ER-o;x;qmuLyay8Wn
zfvS3sTLAo<+k#049`$0L+tau#48C<C&aT=iq@4A7Uyk$d4oRZ7@@(jsgq#HXtzD69
z3E{&|crwZIJW0PUOEIiT9%r!+DZ9!8rz?A8?A)|^x9tB9IAkfW*RcR``pq&j{$sy=
zK$Y&MjCW=USmO`|^_n73s!(OR8_UW`DOe!Lf~&!srDtR~NUX$LmIx+)fm+g-Bbs~o
zGkD`l25(&<;jT-(<WscwVr8=$j-I|6$OA3Jo`N%Z8oqI~F%%K{H>j_nPUgzA{MB%f
zYLpsJBgHwW^qPb)0a}nhrj8Hxvd{K{DJX-Ym*0uEptB3zB>22i!Cyxd4fw+4U+JOB
zh_pT@^Y$y!u@SGa!oD#wgBh{CLn-*1!shRFW6+R3(r&__*119f)3jb<kNi28T`^JX
zk+#zJ`Ovogwb1Dl83e*J^_61+-k4HngTCW>qnoQqY;0uWM!SXgm2NwD83zfI5;iB-
z&JCF5hUvQHUYmIQr~JE%Q=)K+rRmHl;}@P?zX<2bOB%5iIG#dz#XqYlwQJDD%?d%V
zwdOi*$7KcF8)ThxiM11<*I)y%;Ej8if0NJ-?<8x6GE8Cd)veJkj)FWmy6EYlh<5lO
zd7aef)P?Ld5fB$5zX!@BX)F?CEv0F<kJaPyv}&cz=3D8wv&?=!W&)CW)J;6RD72a&
z#BXhh`%M++x`rZ$&?kv^Dl(^_y(k;CR#K|G&mksvc~o^YQQxK!If-FKW*RQv@_3lM
z8F9gd(-u^ZSQ6!9Q@;a_7$hn^!+{&5yRIn@GEx}XF*VH3<8YRz+&1-CU^p*Jo*ce}
z0H~feO#)hU)n3zfLwY=#dHNVqKGp*dF1=JkpyG9wRnp)Iynz}=j05m8`~~BS*Y$i0
z5Dr!Of)VW~VyTPOhyz8{x^sJ9ogfQ>&YPq1YWjl=e}_O{23|{505rHjDnbgk`x{cO
zUl@fhMaB0i%md>|8`mUZ2yU~|lbu;s*g=ZIGt6>yY(%iK6>9Ux8xV?OkhRuzK$v2D
zJyMK#73HeO^2h=OqV`ldEx284H<=MF-)Dv^TB`+YHUnCKE8XKK?1~~<{(VZu%i*G<
zS;BJ3K?iW{(p4P1(?FhN!ExDH)Z>4r<bFJAn|PQkPkMe?$$#l&=BTb#(i1M!oz5%(
z0!w&}m+pqUTA^3sKqBtHf2_<ob<w9fn+|$q{sON?>j;}LQ=Pijv1pCtwaXaHIi?3&
z$0Jhki7){FrpbxavI0QpH}o?CyxbC6&7YL-Ge^~8HZR(@o?elu2P)#BF|&AG@(H3W
zi~*$g5`9U}ycH!xAhOkg&^^;fsX>s1W<;|FCvG^Ac~~y&=qPl;cf$ld@@^Vhv9W&0
zxSb#-Y7P5d2zB1SIG)zEyemqq<>Q^-x?7!`9+t~`L&%hNy6(l-_V%AH#A2K3-rjt*
zeJSK`tFtu<c0;ZE;<9!^$}TIw_2t~Io6!uQJ&=W>m*<(IpUl1`r)>IFcejf3+JzJx
zN6NLZ2XLBSQKK+_VQOjtdV2ofo<&R@d#S{S(xlT^P$sm6+DJnu4PTIgQ5b*>GUHm?
zdb-Ej0ozhP4rkw5JFrBa;tz}v!kY^y_G%sdBOIn38FB@y1VExZ9t2VO4n^yx04mPA
zrmvgqxmRagKJZe(PYKM;vrmsiizU!Zy7@0{3z2b24TdZ&;yf9%l_=>#yt6}|r3p)K
zr3#z}MIEVta3kOG5kDmaXekygPz<J7%SN_=%ame>@*gA5@lB)F8a;AC8T;&4aE6*?
zoan&HeKBeRN0)e|P_p=oL?AwZ#xJ8D=uWH>7;`)`JnfGyjm!I5(X_L%S3|n*6IC<#
zidCBG2r2>q%Dh?aZG=1WJM#*6Ix5Ruaxo>+0_u39g^PSo8-L<7rrYV6rJ=QOAp>pn
zwjD9FrjgqbbmMx!4<v<g_*F~G5F;uzgo?1waI<BMHz8BEoEjpc2V9LNV+M;Ej4;}?
z6d1yThszVZ`*n7mYtFhY>>B3L3rFXDG=WH}<LV{6=S*yA>h<9Z&%K9f47hwMEtNfJ
zv)Q~F0U?+XitwVak1^>`<Tzst{_mE2dy9Xf$ctWNr<q=!L!sY{SERu7h}!G*%EgWM
zLlXGqSEQ@#rlEjFkdehTw29)dQ9i1RaaGIi9g3-~y928Oqq8<(Dnhu~*IA0IM|OP0
ztM3gXRh<p7y#Au}rwiNlU^{&v&k@62Qe_LtO$wyR!XCSC5)4AgIne83vz?~mVm1F9
z@#)+|j?0s;vfYLKBcae_N>T(anY}hO?`noYKqba=>dCUkuQO~TLGpMW>Fbac^o^}3
ztStgt4>#%Lb~&odyrXug+uxv41|fXmLrf^MSW!n5em^YS24Pfagsurv<)1}nFolJk
ej+Yi^mTnY|^cg}~k11?#_&U~SAl}#u{xn~Qfy3MY

diff --git a/.travis.yml b/.travis.yml
deleted file mode 100644
index aa42cac0..00000000
--- a/.travis.yml
+++ /dev/null
@@ -1,18 +0,0 @@
-language: bash
-sudo: required
-services: [docker]
-after_success:
-- openssl aes-256-cbc -K $encrypted_04c78d356583_key -iv $encrypted_04c78d356583_iv
-  -in .snapcraft/travis_snapcraft.cfg -out .snapcraft/snapcraft.cfg -d
-deploy:
-  skip_cleanup: true
-  provider: script
-  script: docker run -v $(pwd):$(pwd) -t snapcore/snapcraft sh -c
-          "apt update -qq &&
-           cd $(pwd) &&
-           ./snap/generate_snap &&
-           if [ "${TRAVIS_PULL_REQUEST}" = "false" ] && [ "${TRAVIS_BRANCH}" = "master" ] ; then 
-             snapcraft push snap_ws/*.snap --release edge;
-           fi"
-  on:
-    all_branches: true
diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml
index 8d0063c4..3aa4a59a 100644
--- a/snap/snapcraft.yaml
+++ b/snap/snapcraft.yaml
@@ -45,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]

From 0ee79d68d628c2747106d8f3705245022f8dbfd1 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Tue, 7 Aug 2018 16:20:19 -0700
Subject: [PATCH 14/17] async less hoops to jump through

---
 src/slam_toolbox.cpp | 52 ++++++++++++++++++++++++--------------------
 1 file changed, 28 insertions(+), 24 deletions(-)

diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index 03e4c1e9..2be716c7 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -276,7 +276,7 @@ void SlamToolbox::SetROSInterfaces(ros::NodeHandle& node)
   ssClear_manual_ = node.advertiseService("clear_changes", &SlamToolbox::ClearChangesCallback, this);
   ssSave_map_ = node.advertiseService("save_map", &SlamToolbox::SaveMapCallback, this);
   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node, "/scan", 5);
-  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 2);
+  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
   scan_filter_->registerCallback(boost::bind(&SlamToolbox::LaserCallback, this, _1));
   marker_publisher_ = node.advertise<visualization_msgs::MarkerArray>("karto_graph_visualization",1);
   scan_publisher_ = node.advertise<sensor_msgs::LaserScan>("karto_scan_visualization",10);
@@ -364,7 +364,6 @@ void SlamToolbox::PublishVisualizations()
     }
     r.sleep();
   }
-
 }
 
 /*****************************************************************************/
@@ -635,6 +634,28 @@ void SlamToolbox::ProcessInteractiveFeedback(const \
 void SlamToolbox::LaserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
+  if (!sychronous_)
+  {
+    // asynchonous
+    karto::LaserRangeFinder* laser = GetLaser(scan);
+
+    if(!laser)
+    {
+      ROS_WARN("Failed to create laser device for %s; discarding scan",
+         scan->header.frame_id.c_str());
+      return;
+    }
+
+    karto::Pose2 pose;
+    if(!GetOdomPose(pose, scan->header.stamp))
+    {
+      return;
+    }
+
+    AddScan(laser, scan, pose);
+    return;
+  }
+
   static karto::Pose2 last_pose;
   static double last_scan_time = 0.;
   static double min_dist2 = minimum_travel_distance_*minimum_travel_distance_;
@@ -689,27 +710,9 @@ void SlamToolbox::LaserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     return;
   }
 
-  if (sychronous_)
-  {
-    // synchronous
-    q_.push(posed_scan(scan, pose));
-    last_scan_time = scan->header.stamp.toSec(); 
-    last_pose = pose;   
-  }
-  else
-  {
-    // asynchonous
-    karto::LaserRangeFinder* laser = GetLaser(scan);
-
-    if(!laser)
-    {
-      ROS_WARN("Failed to create laser device for %s; discarding scan",
-         scan->header.frame_id.c_str());
-      return;
-    }
-    AddScan(laser, scan, pose);
-  }
-
+  q_.push(posed_scan(scan, pose));
+  last_scan_time = scan->header.stamp.toSec(); 
+  last_pose = pose;
   return;
 }
 
@@ -1082,7 +1085,7 @@ void SlamToolbox::Run()
 
   ROS_INFO_ONCE("Run thread enabled - synchronous mode selected.");
 
-  ros::Rate r(60);
+  ros::Rate r(100);
   while(ros::ok())
   {
     if (!q_.empty() && !IsPaused(PROCESSING))
@@ -1113,6 +1116,7 @@ void SlamToolbox::Run()
         break;
       }
       AddScan(laser, scan_w_pose.scan, scan_w_pose.pose);
+      continue; // no need to sleep if working
     }
     r.sleep();
   }

From 4271802bf8908e84136ddcf4fd8bfc161f4ace2d Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Tue, 7 Aug 2018 18:19:33 -0700
Subject: [PATCH 15/17] stop double deletion

---
 src/slam_toolbox.cpp | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index 2be716c7..ee791277 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -309,12 +309,6 @@ SlamToolbox::~SlamToolbox()
     delete mapper_;
   if (dataset_)
     delete dataset_;
-  std::map<std::string, karto::LaserRangeFinder*>::iterator it = \
-                                                               lasers_.begin();
-  for (it; it!=lasers_.end(); ++it)
-  {
-    delete it->second;
-  }
   if (interactive_server_)
   {
     delete interactive_server_;

From 0b96ffaf819ca0c903b8fbe2e8a5f9e8b78eca63 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Tue, 7 Aug 2018 18:22:39 -0700
Subject: [PATCH 16/17] speed comment

---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index 0b640733..02aa7611 100644
--- a/README.md
+++ b/README.md
@@ -4,6 +4,8 @@ Fork on slam_karto. I've renamed since really there's about 5% of the code that'
 
 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

From 720ecbfd9f29df61eb98a7e72fb2fbf89c048762 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Tue, 7 Aug 2018 19:22:06 -0700
Subject: [PATCH 17/17] versioning for slam toolbox and author

---
 package.xml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/package.xml b/package.xml
index 4b038a0a..203cc77e 100644
--- a/package.xml
+++ b/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>