Map merging

CMakeLists.txt

@@ -3,7 +3,7 @@ project(slam_toolbox)
 
 add_compile_options(-std=c++11)
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/CMake/")
-set(CMAKE_CXX_FLAGS "-fpermissive -std=c++11")                                                                            
+set(CMAKE_CXX_FLAGS "-fpermissive -std=c++11")
 
 find_package(Boost REQUIRED system serialization filesystem)
 
@@ -80,6 +80,7 @@ add_service_files(DIRECTORY srvs
   LoopClosure.srv
   MergeMaps.srv
   AddSubmap.srv
+  AddMap.srv
   SerializePoseGraph.srv
 )
 generate_messages(
@@ -145,11 +146,11 @@ add_dependencies(slam_toolbox ${PROJECT_NAME}_generate_messages_cpp)
 target_link_libraries(slam_toolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 #### Merging maps tool
-add_executable(merge_maps_tool src/merge_maps_tool.cpp)
-add_dependencies(merge_maps_tool ${PROJECT_NAME}_generate_messages_cpp)
-target_link_libraries(merge_maps_tool ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp)
+add_dependencies(merge_maps_kinematic ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(merge_maps_kinematic ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
-install(TARGETS slam_toolbox merge_maps_tool spa_solver_plugin g2O_solver_plugin ceres_solver_plugin SlamToolboxPlugin
+install(TARGETS slam_toolbox merge_maps_kinematic spa_solver_plugin g2O_solver_plugin ceres_solver_plugin SlamToolboxPlugin
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

config/mapper_params.yaml

@@ -17,7 +17,8 @@ ceres_loss_function: None
 # ROS Parameters
 odom_frame: odom
 map_frame: map
-base_frame: base_link
+base_frame: base_footprint
+laser_frame: base_laser_link
 debug_logging: false
 throttle_scans: 1
 publish_occupancy_map: true
@@ -63,3 +64,6 @@ coarse_angle_resolution: 0.0349
 minimum_angle_penalty: 0.9
 minimum_distance_penalty: 0.5
 use_response_expansion: true
+
+# Load Map Parameters
+inverted_laser: true

include/slam_toolbox/merge_maps_tool.hpp → include/slam_toolbox/merge_maps_kinematic.hpp

@@ -48,36 +48,27 @@
 #include <unistd.h>
 #include <fstream>
 
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/serialization/vector.hpp>
-#include <boost/serialization/list.hpp>
-#include <boost/serialization/string.hpp>
-
 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
 
-class MergeMapTool
+class MergeMapsKinematic
 {
 
 public:
-  MergeMapTool();
-  ~MergeMapTool();
+  MergeMapsKinematic();
+  ~MergeMapsKinematic();
 
 private:
+
   // setup
   void SetConfigs();
 
-  // callbacks
+  // callback
   bool MergeMapCallback(slam_toolbox::MergeMaps::Request  &req, slam_toolbox::MergeMaps::Response &resp);
   bool AddSubmapCallback(slam_toolbox::AddSubmap::Request  &req, slam_toolbox::AddSubmap::Response &resp);
   void ProcessInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
-  void KartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans);
-
-  inline bool FileExists(const std::string& name)
-  {
-    struct stat buffer;
-    return (stat (name.c_str(), &buffer) == 0); 
-  }
+  void KartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans, nav_msgs::GetMap::Response& map);
+  typedef std::vector<karto::LocalizedRangeScanVector>::iterator localized_range_scans_vec_it;
+  typedef karto::LocalizedRangeScanVector::iterator localized_range_scans_it;
 
   // ROS-y-ness
   ros::NodeHandle nh_;
@@ -89,12 +80,21 @@ class MergeMapTool
   double resolution_;
   double max_laser_range_;
   int num_submaps_;
-  nav_msgs::GetMap::Response map_;
+
+  //karto bookkeeping
+  std::map<std::string, karto::LaserRangeFinder*> lasers_;
+  std::vector<karto::Dataset*> dataset_vec_;
 
   // TF
   tf::TransformBroadcaster* tfB_;
 
   // visualization
   interactive_markers::InteractiveMarkerServer* interactive_server_;
   std::map<int, Eigen::Vector3d> submap_locations_;
+  std::vector<karto::LocalizedRangeScanVector> scans_vec_;
+  std::map<int, tf::Transform> submap_marker_transform_;
+
+  //apply transformation to correct pose
+  karto::Pose2 ApplyCorrection(const karto::Pose2& pose, const tf::Transform& submap_correction);
+  karto::Vector2<kt_double> ApplyCorrection(const karto::Vector2<kt_double>&  pose, const tf::Transform& submap_correction);
 };

include/slam_toolbox/slam_toolbox.hpp

@@ -50,6 +50,7 @@
 #include "slam_toolbox/SaveMap.h"
 #include "slam_toolbox/LoopClosure.h"
 #include "slam_toolbox/SerializePoseGraph.h"
+#include "slam_toolbox/AddMap.h"
 
 #include <string>
 #include <map>
@@ -135,18 +136,20 @@ class SlamToolbox
   // state
   bool IsPaused(const PausedApplication& app);
 
+  bool LoadMapperCallback(slam_toolbox::AddMap::Request  &req, slam_toolbox::AddMap::Response &resp);
+
   // ROS-y-ness
   ros::NodeHandle nh_;
   tf::TransformListener tf_;
   tf::TransformBroadcaster* tfB_;
   message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
   tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
   ros::Publisher sst_, sstm_, marker_publisher_, scan_publisher_;
-  ros::ServiceServer ssMap_, ssClear_, ssInteractive_, ssLoopClosure_, ssPause_processing_, ssPause_measurements_, ssClear_manual_, ssSave_map_, ssSerialize_;
+  ros::ServiceServer ssMap_, ssClear_, ssInteractive_, ssLoopClosure_, ssPause_processing_, ssPause_measurements_, ssClear_manual_, ssSave_map_, ssSerialize_, ssLoadMap_;
   nav_msgs::GetMap::Response map_;
 
   // Storage for ROS parameters
-  std::string odom_frame_, map_frame_, base_frame_;
+  std::string odom_frame_, map_frame_, base_frame_, laser_frame_;
   int throttle_scans_;
   ros::Duration map_update_interval_;
   double resolution_, minimum_time_interval_, minimum_travel_distance_;

launch/build_map_w_params.launch

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

launch/merge_maps_kinematic.launch

@@ -0,0 +1,6 @@
+<launch>
+
+  <node pkg="slam_toolbox" type="merge_maps_kinematic" name="merge_maps_kinematic" output="screen">
+  </node>
+
+</launch>

mapping_plugin/slam_toolbox_rviz_plugin.cpp

@@ -43,17 +43,17 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent):
   nh.getParam("/slam_toolbox/paused_new_measurements", paused_measure);
   nh.getParam("/slam_toolbox/paused_processing", paused_process);
   nh.getParam("/slam_toolbox/interactive_mode", interactive);
-
+  _serialize = nh.serviceClient<slam_toolbox::SerializePoseGraph>("/slam_toolbox/serialize_map");
+  _load_map = nh.serviceClient<slam_toolbox::AddMap>("/slam_toolbox/load_map");
   _clearChanges = nh.serviceClient<slam_toolbox::Clear>("/slam_toolbox/clear_changes");
   _saveChanges = nh.serviceClient<slam_toolbox::LoopClosure>("/slam_toolbox/manual_loop_closure");
   _saveMap = nh.serviceClient<slam_toolbox::SaveMap>("/slam_toolbox/save_map");
   _clearQueue = nh.serviceClient<slam_toolbox::ClearQueue>("/slam_toolbox/clear_queue");
   _interactive = nh.serviceClient<slam_toolbox::ToggleInteractive>("/slam_toolbox/toggle_interactive_mode");
   _pause_processing = nh.serviceClient<slam_toolbox::Pause>("/slam_toolbox/pause_processing");
   _pause_measurements = nh.serviceClient<slam_toolbox::Pause>("/slam_toolbox/pause_new_measurements");
-  _load_submap = nh.serviceClient<slam_toolbox::AddSubmap>("/merge_maps_tool/add_submap");
-  _merge = nh.serviceClient<slam_toolbox::MergeMaps>("/merge_maps_tool/merge_maps");
-  _serialize = nh.serviceClient<slam_toolbox::SerializePoseGraph>("/slam_toolbox/serialize_map");
+  _load_submap_for_merging = nh.serviceClient<slam_toolbox::AddSubmap>("/map_merging/add_submap");
+  _merge = nh.serviceClient<slam_toolbox::MergeMaps>("/map_merging/merge_submaps");
 
   _vbox = new QVBoxLayout();
   _hbox1 = new QHBoxLayout();
@@ -63,6 +63,7 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent):
   _hbox5 = new QHBoxLayout();
   _hbox6 = new QHBoxLayout();
   _hbox7 = new QHBoxLayout();
+  _hbox8 = new QHBoxLayout();
 
   QFrame* _line = new QFrame();
   _line->setFrameShape(QFrame::HLine);
@@ -81,14 +82,17 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent):
   _button4->setText("Clear Measurement Queue");
   connect(_button4, SIGNAL(clicked()), this, SLOT(ClearQueue()));
   _button5 = new QPushButton(this);
-  _button5->setText("Load Pose Graph");
-  connect(_button5, SIGNAL(clicked()), this, SLOT(LoadPoseGraph()));
+  _button5->setText("Add Submap");
+  connect(_button5, SIGNAL(clicked()), this, SLOT(LoadSubmap()));
   _button6 = new QPushButton(this);
   _button6->setText("Generate Map");
   connect(_button6, SIGNAL(clicked()), this, SLOT(GenerateMap()));
   _button7 = new QPushButton(this);
   _button7->setText("Serialize Map");
   connect(_button7, SIGNAL(clicked()), this, SLOT(SerializeMap()));
+  _button8 = new QPushButton(this);
+  _button8->setText("Load Map");
+  connect(_button8, SIGNAL(clicked()), this, SLOT(LoadMap()));
 
   _label1 = new QLabel(this);
   _label1->setText("Interactive");
@@ -116,6 +120,7 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent):
   _line1 = new QLineEdit();
   _line2 = new QLineEdit();
   _line3 = new QLineEdit();
+  _line4 = new QLineEdit();
 
   _button1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
   _button2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
@@ -133,6 +138,7 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent):
   _hbox1->addWidget(_check3);
   _hbox1->addWidget(_label3);
 
+
   _hbox2->addWidget(_button1);
   _hbox2->addWidget(_button2);
 
@@ -149,11 +155,15 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent):
   _hbox7->addWidget(_button7);
   _hbox7->addWidget(_line3);
 
+  _hbox8->addWidget(_button8);
+  _hbox8->addWidget(_line4);
+
   _vbox->addWidget(_label5);
   _vbox->addLayout(_hbox1);
   _vbox->addLayout(_hbox2);
   _vbox->addLayout(_hbox3);
   _vbox->addLayout(_hbox7);
+  _vbox->addLayout(_hbox8);
   _vbox->addLayout(_hbox4);
   _vbox->addWidget(_line);
   _vbox->addWidget(_label4);
@@ -183,30 +193,41 @@ void SlamToolboxPlugin::SerializeMap()
   msg.request.filename = _line3->text().toStdString();
   if (!_serialize.call(msg))
   {
-    ROS_WARN("Failed to serialize pose graph to file, is service running?");
+    ROS_WARN("SlamToolbox: Failed to serialize pose graph to file, is service running?");
   }
 }
 
 /*****************************************************************************/
-void SlamToolboxPlugin::LoadPoseGraph()
+void SlamToolboxPlugin::LoadMap()
 /*****************************************************************************/
 {
   slam_toolbox::AddSubmap msg;
-  msg.request.filename = _line2->text().toStdString();
-  if (!_load_submap.call(msg))
+  msg.request.filename = _line4->text().toStdString();
+  if (!_load_map.call(msg))
   {
-    ROS_WARN("Failed to load pose graph from file, is service running?");
+     ROS_WARN("SlamToolbox: Failed to load mapper object from file, is service running?");
   }
 }
 
+/*****************************************************************************/
+void SlamToolboxPlugin::LoadSubmap()
+/*****************************************************************************/
+{
+  slam_toolbox::AddSubmap msg;
+  msg.request.filename = _line2->text().toStdString();
+  if (!_load_submap_for_merging.call(msg))
+  {
+    ROS_WARN("MergeMaps: Failed to load pose graph from file, is service running?");
+  }
+}
 /*****************************************************************************/
 void SlamToolboxPlugin::GenerateMap()
 /*****************************************************************************/
 {
   slam_toolbox::MergeMaps msg;
   if (!_merge.call(msg))
   {
-    ROS_WARN("Failed to merge maps, is service running?");
+    ROS_WARN("MergeMaps: Failed to merge maps, is service running?");
   }
 }
 
@@ -217,7 +238,7 @@ void SlamToolboxPlugin::ClearChanges()
   slam_toolbox::Clear msg;
   if (!_clearChanges.call(msg))
   {
-    ROS_WARN("Failed to clear changes, is service running?");
+    ROS_WARN("SlamToolbox: Failed to clear changes, is service running?");
   }
 }
 
@@ -229,7 +250,7 @@ void SlamToolboxPlugin::SaveChanges()
 
   if (!_saveChanges.call(msg))
   {
-    ROS_WARN("Failed to save changes, is service running?");
+    ROS_WARN("SlamToolbox: Failed to save changes, is service running?");
   }
 }
 
@@ -241,7 +262,7 @@ void SlamToolboxPlugin::SaveMap()
   msg.request.name.data = _line1->text().toStdString();
   if (!_saveMap.call(msg))
   {
-    ROS_WARN("Failed to save map as %s, is service running?", 
+    ROS_WARN("SlamToolbox: Failed to save map as %s, is service running?",
               msg.request.name.data.c_str());
   }
 }
@@ -264,7 +285,7 @@ void SlamToolboxPlugin::InteractiveCb(int state)
   slam_toolbox::ToggleInteractive msg;
   if (!_interactive.call(msg))
   {
-    ROS_WARN("Failed to toggle interactive mode, is service running?");
+    ROS_WARN("SlamToolbox: Failed to toggle interactive mode, is service running?");
   }
 }
 
@@ -275,7 +296,7 @@ void SlamToolboxPlugin::PauseProcessingCb(int state)
   slam_toolbox::Pause msg;
   if (!_pause_processing.call(msg))
   {
-    ROS_WARN("Failed to toggle pause processing, is service running?");
+    ROS_WARN("SlamToolbox: Failed to toggle pause processing, is service running?");
   }
 }
 
@@ -286,7 +307,7 @@ void SlamToolboxPlugin::PauseMeasurementsCb(int state)
   slam_toolbox::Pause msg;
   if (!_pause_measurements.call(msg))
   {
-    ROS_WARN("Failed to toggle pause measurements, is service running?");
+    ROS_WARN("SlamToolbox: Failed to toggle pause measurements, is service running?");
   }
 }