diff --git a/README.md b/README.md
index 8a061919..02aa7611 100644
--- a/README.md
+++ b/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
diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml
index db5fbec0..044db7f7 100644
--- a/config/mapper_params_offline.yaml
+++ b/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
diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml
new file mode 100644
index 00000000..ea9d93a5
--- /dev/null
+++ b/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
diff --git a/config/mapper_params_online.yaml b/config/mapper_params_online_sync.yaml
similarity index 92%
rename from config/mapper_params_online.yaml
rename to config/mapper_params_online_sync.yaml
index 8ce8e5d7..cd4de0a9 100644
--- a/config/mapper_params_online.yaml
+++ b/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
diff --git a/include/slam_toolbox/slam_toolbox.hpp b/include/slam_toolbox/slam_toolbox.hpp
index 5b54e485..499b1e7e 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,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_;
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 @@
+
+
+
+
+
+
+
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 @@
-
+
diff --git a/package.xml b/package.xml
index 4b038a0a..203cc77e 100644
--- a/package.xml
+++ b/package.xml
@@ -3,9 +3,10 @@
This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
- 0.7.3
+ 0.2.2
Steve Macenski
Brian Gerkey
+ Steve Macenski
LGPL
catkin
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 ../
diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml
index 84df5520..3aa4a59a 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]
+ #after: [desktop-qt5]
diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index ff665944..ee791277 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,19 @@ 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;
+ 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_))
@@ -293,12 +309,6 @@ SlamToolbox::~SlamToolbox()
delete mapper_;
if (dataset_)
delete dataset_;
- std::map::iterator it = \
- lasers_.begin();
- for (it; it!=lasers_.end(); ++it)
- {
- delete it->second;
- }
if (interactive_server_)
{
delete interactive_server_;
@@ -306,7 +316,7 @@ SlamToolbox::~SlamToolbox()
}
/*****************************************************************************/
-void SlamToolbox::PublishLoop(double transform_publish_period)
+void SlamToolbox::PublishTransformLoop(double transform_publish_period)
/*****************************************************************************/
{
if(transform_publish_period == 0)
@@ -317,9 +327,8 @@ 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_));
}
r.sleep();
}
@@ -349,7 +358,6 @@ void SlamToolbox::PublishVisualizations()
}
r.sleep();
}
-
}
/*****************************************************************************/
@@ -460,7 +468,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_);
@@ -620,6 +628,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_;
@@ -674,11 +704,9 @@ 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;
-
return;
}
@@ -815,12 +843,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;
}
@@ -1039,14 +1070,35 @@ bool SlamToolbox::IsPaused(const PausedApplication& app)
void SlamToolbox::Run()
/*****************************************************************************/
{
- ros::Rate r(60);
+ if (!sychronous_)
+ {
+ // 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(100);
while(ros::ok())
{
if (!q_.empty() && !IsPaused(PROCESSING))
{
posed_scan scan_w_pose = q_.front();
q_.pop();
- ROS_INFO_THROTTLE(15., "Queue size: %i", (int)q_.size());
+
+ if (q_.size() > 2)
+ {
+ 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
karto::LaserRangeFinder* laser = GetLaser(scan_w_pose.scan);
@@ -1058,6 +1110,7 @@ void SlamToolbox::Run()
break;
}
AddScan(laser, scan_w_pose.scan, scan_w_pose.pose);
+ continue; // no need to sleep if working
}
r.sleep();
}