add SaveMap.srv and add an outdoor mapping example

koide3 · Jan 22, 2018 · e9d8e64 · e9d8e64
1 parent e4dd764
commit e9d8e64
Show file tree

Hide file tree

Showing 9 changed files with 91 additions and 7 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -46,6 +46,10 @@ add_message_files(FILES
   FloorCoeffs.msg
 )
 
+add_service_files(FILES
+  SaveMap.srv
+)
+
 generate_messages(DEPENDENCIES std_msgs)
 
 ###################################

diff --git a/README.md b/README.md
@@ -25,6 +25,8 @@ All the parameters are listed in *launch/hdl_graph_slam.launch* as ros params.
 ### Services
 - */hdl_graph_slam/dump*  (std_srvs/Empty)
   - save all data (point clouds, floor coeffs, odoms, and pose graph) to the current directory.
+- */hdl_graph_slam/save_map*  (hdl_graph_slam/SaveMap)
+  - save generated map as a PCD file.
 
 ## Requirements
 ***hdl_graph_slam*** requires the following libraries:
@@ -43,14 +45,20 @@ make -j8
 sudo make install
 ```
 
-The following ros packages are required:
+The following ROS packages are required:
 - geodesy
+- nmea_msgs
 - pcl_ros
 ```bash
-sudo apt-get install ros-indigo-geodesy ros-indigo-pcl_ros
+sudo apt-get install ros-indigo-geodesy ros-indigo-pcl_ros ros-indigo-nmea-msgs
 ```
 
-## Example
+**[optional]** *bag_player.py* script requires ProgressBar2.
+```bash
+sudo pip install ProgressBar2
+```
+
+## Example1 (Indoor)
 
 Example bag files (recorded in a small room): 
 - [hdl_501.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501.bag.tar.gz) (raw data, 344MB)
@@ -80,6 +88,35 @@ You'll see a generated point cloud like:
 
 <img src="imgs/top.png" height="256pix" /> <img src="imgs/birds.png" height="256pix" /> 
 
+You can save the generated map by:
+```bash
+rosservice call /hdl_graph_slam/save_map "resolution: 0.05
+destination: '/full_path_directory/map.pcd'"
+```
+
+## Example2 (Outdoor)
+
+Bag file (recorded in an outdoor environment): 
+- [hdl_400.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz) (raw data, about 900MB)
+
+
+```bash
+rosparam set use_sim_time true
+roslaunch hdl_graph_slam hdl_graph_slam_400.launch
+```
+
+```bash
+roscd hdl_graph_slam/rviz
+rviz -d hdl_graph_slam.rviz
+```
+
+```bash
+rosbag play --clock hdl_400.bag
+```
+
+
+<img src="imgs/hdl_400_points.png" height="256pix" /> <img src="imgs/hdl_400_graph.png" height="256pix" /> 
+
 
 ## Example with GPS feature
 Ford Campus Vision and Lidar Data Set <a href="http://robots.engin.umich.edu/SoftwareData/Ford">[URL]</a>.

diff --git a/apps/hdl_graph_slam_nodelet.cpp b/apps/hdl_graph_slam_nodelet.cpp
@@ -9,6 +9,7 @@
 #include <boost/filesystem.hpp>
 #include <boost/algorithm/string.hpp>
 #include <Eigen/Dense>
+#include <pcl/io/pcd_io.h>
 
 #include <ros/ros.h>
 #include <geodesy/utm.h>
@@ -28,6 +29,7 @@
 #include <hdl_graph_slam/FloorCoeffs.h>
 
 #include <std_srvs/Empty.h>
+#include <hdl_graph_slam/SaveMap.h>
 
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -92,8 +94,11 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
     sync.reset(new message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>(*odom_sub, *cloud_sub, 32));
     sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));
     floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 32, &HdlGraphSlamNodelet::floor_coeffs_callback, this);
-    gps_sub = nh.subscribe("/gps/geopoint", 32, &HdlGraphSlamNodelet::gps_callback, this);
-    nmea_sub = nh.subscribe("/gpsimu_driver/nmea_sentence", 32, &HdlGraphSlamNodelet::nmea_callback, this);
+
+    if(private_nh.param<bool>("enable_gps", true)) {
+      gps_sub = nh.subscribe("/gps/geopoint", 32, &HdlGraphSlamNodelet::gps_callback, this);
+      nmea_sub = nh.subscribe("/gpsimu_driver/nmea_sentence", 32, &HdlGraphSlamNodelet::nmea_callback, this);
+    }
 
     // publishers
     markers_pub = nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);
@@ -102,6 +107,7 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
     read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);
 
     dump_service_server = nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this);
+    save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &HdlGraphSlamNodelet::save_map_service, this);
 
     double graph_update_interval = private_nh.param<double>("graph_update_interval", 3.0);
     double map_cloud_update_interval = private_nh.param<double>("map_cloud_update_interval", 10.0);
@@ -621,6 +627,33 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
     return true;
   }
 
+  /**
+   * @brief save map data as pcd
+   * @param req
+   * @param res
+   * @return
+   */
+  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
+    std::vector<KeyFrameSnapshot::Ptr> snapshot;
+
+    keyframes_snapshot_mutex.lock();
+    snapshot = keyframes_snapshot;
+    keyframes_snapshot_mutex.unlock();
+
+    auto cloud = map_cloud_generator->generate(snapshot, req.resolution);
+    if(!cloud) {
+      res.success = false;
+      return true;
+    }
+
+    cloud->header.frame_id = map_frame_id;
+    cloud->header.stamp = snapshot.back()->cloud->header.stamp;
+
+    int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
+    res.success = ret == 0;
+
+    return true;
+  }
 private:
   // ROS
   ros::NodeHandle nh;
@@ -649,8 +682,8 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
   ros::Publisher read_until_pub;
   ros::Publisher map_points_pub;
 
-  std::atomic_bool dump_request;
   ros::ServiceServer dump_service_server;
+  ros::ServiceServer save_map_service_server;
 
   // keyframe queue
   std::mutex keyframe_queue_mutex;

diff --git a/imgs/hdl_400_graph.png b/imgs/hdl_400_graph.png
diff --git a/imgs/hdl_400_points.png b/imgs/hdl_400_points.png
diff --git a/launch/hdl_graph_slam.launch b/launch/hdl_graph_slam.launch
@@ -3,6 +3,7 @@
   <!-- arguments -->
   <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
   <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="false" />
 
   <!-- in case you use velodyne_driver, comment out the following line -->
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
@@ -50,6 +51,7 @@
   <!-- hdl_graph_slam_nodelet -->
   <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
     <!-- keyframe registration params -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
     <param name="max_keyframes_per_update" value="10" />
     <param name="keyframe_delta_trans" value="2.0" />
     <param name="keyframe_delta_angle" value="2.0" />

diff --git a/launch/hdl_graph_slam_400.launch b/launch/hdl_graph_slam_400.launch
@@ -2,7 +2,8 @@
 <launch>
   <!-- arguments -->
   <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
-  <arg name="enable_floor_detection" default="false" />
+  <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="false" />
 
   <!-- in case you use velodyne_driver, comment out the following line -->
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
@@ -50,6 +51,7 @@
   <!-- hdl_graph_slam_nodelet -->
   <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
     <!-- keyframe registration params -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
     <param name="max_keyframes_per_update" value="10" />
     <param name="keyframe_delta_trans" value="2.0" />
     <param name="keyframe_delta_angle" value="2.0" />

diff --git a/launch/hdl_graph_slam_ford.launch b/launch/hdl_graph_slam_ford.launch
@@ -3,6 +3,7 @@
   <!-- arguments -->
   <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
   <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="true" />
 
   <!-- in case you use velodyne_driver, comment out the following line -->
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
@@ -53,6 +54,7 @@
   <!-- hdl_graph_slam_nodelet -->
   <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
     <!-- keyframe registration params -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
     <param name="max_keyframes_per_update" value="10" />
     <param name="keyframe_delta_trans" value="10.0" />
     <param name="keyframe_delta_angle" value="3.0" />

diff --git a/srv/SaveMap.srv b/srv/SaveMap.srv
@@ -0,0 +1,4 @@
+float32 resolution
+string destination
+---
+bool success