ROS service to save generated map into a .pcd

CMakeLists.txt

@@ -40,8 +40,17 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   geometry_msgs
   pcl_ros
+  message_generation
 )
 
+add_service_files(
+  DIRECTORY srv
+  FILES
+  save_pcd.srv
+)
+
+generate_messages()
+
 catkin_package(
   CATKIN_DEPENDS
     roscpp

README.md

@@ -60,6 +60,13 @@ If successful, RViz will open and you will see similar terminal outputs to the f
     <img src="./doc/img/terminal_output.png" alt="drawing" width="400"/>
 </p>
 
+### Services
+To save DLO's generated map into `.pcd` format, call the following service:
+
+```
+rosservice call /robot/dlo_map/save_pcd LEAF_SIZE SAVE_PATH
+```
+
 ### Test Data
 For your convenience, we provide example test data [here](https://ucla.box.com/shared/static/ziojd3auzp0zzcgwb1ucau9anh69xwv9.bag) (9 minutes, ~4.2GB). To run, first launch DLO (with default point cloud and IMU topics) via:
 

cfg/dlo.yaml

@@ -9,7 +9,7 @@
 
 dlo:
 
-  version: 1.3.1
+  version: 1.4.0
 
   adaptiveParams: true
 

include/dlo/dlo.h

@@ -45,6 +45,7 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>
 
+#include <direct_lidar_odometry/save_pcd.h>
 #include <nano_gicp/nano_gicp.hpp>
 
 typedef pcl::PointXYZI PointType;

include/dlo/map.h

@@ -30,6 +30,9 @@ class dlo::MapNode {
 
   void keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe);
 
+  bool savePcd(direct_lidar_odometry::save_pcd::Request& req,
+               direct_lidar_odometry::save_pcd::Response& res);
+
   void getParams();
 
   ros::NodeHandle nh;
@@ -39,6 +42,8 @@ class dlo::MapNode {
   ros::Subscriber keyframe_sub;
   ros::Publisher map_pub;
 
+  ros::ServiceServer save_pcd_srv;
+
   pcl::PointCloud<PointType>::Ptr dlo_map;
   pcl::VoxelGrid<PointType> voxelgrid;
 

package.xml

@@ -3,7 +3,7 @@
 <package format="2">
 
   <name>direct_lidar_odometry</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>Direct LiDAR Odometry: Fast Localization with Dense Point Clouds</description>
 
   <maintainer email="kennyjchen@ucla.edu">Kenny J. Chen</maintainer>
@@ -16,6 +16,9 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>

src/dlo/map.cc

@@ -26,6 +26,8 @@ dlo::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) {
   this->keyframe_sub = this->nh.subscribe("keyframes", 1, &dlo::MapNode::keyframeCB, this);
   this->map_pub = this->nh.advertise<sensor_msgs::PointCloud2>("map", 1);
 
+  this->save_pcd_srv = this->nh.advertiseService("save_pcd", &dlo::MapNode::savePcd, this);
+
   // initialize map
   this->dlo_map = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);
 
@@ -99,10 +101,6 @@ void dlo::MapNode::abortTimerCB(const ros::TimerEvent& e) {
 
 void dlo::MapNode::publishTimerCB(const ros::TimerEvent& e) {
 
-  this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_);
-  this->voxelgrid.setInputCloud(this->dlo_map);
-  this->voxelgrid.filter(*this->dlo_map);
-
   if (this->dlo_map->points.size() == this->dlo_map->width * this->dlo_map->height) {
     sensor_msgs::PointCloud2 map_ros;
     pcl::toROSMsg(*this->dlo_map, map_ros);
@@ -124,8 +122,44 @@ void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe)
   pcl::PointCloud<PointType>::Ptr keyframe_pcl = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);
   pcl::fromROSMsg(*keyframe, *keyframe_pcl);
 
+  // voxel filter
+  this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_);
+  this->voxelgrid.setInputCloud(keyframe_pcl);
+  this->voxelgrid.filter(*keyframe_pcl);
+
   // save keyframe to map
   this->map_stamp = keyframe->header.stamp;
   *this->dlo_map += *keyframe_pcl;
 
 }
+
+bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req,
+                           direct_lidar_odometry::save_pcd::Response& res) {
+
+  pcl::PointCloud<PointType>::Ptr m =
+    pcl::PointCloud<PointType>::Ptr (boost::make_shared<pcl::PointCloud<PointType>>(*this->dlo_map));
+
+  float leaf_size = req.leaf_size;
+  std::string p = req.save_path;
+
+  std::cout << std::setprecision(2) << "Saving map to " << p + "/dlo_map.pcd" << "... "; std::cout.flush();
+
+  // voxelize map
+  pcl::VoxelGrid<PointType> vg;
+  vg.setLeafSize(leaf_size, leaf_size, leaf_size);
+  vg.setInputCloud(m);
+  vg.filter(*m);
+
+  // save map
+  int ret = pcl::io::savePCDFileBinary(p + "/dlo_map.pcd", *m);
+  res.success = ret == 0;
+
+  if (res.success) {
+    std::cout << "done" << std::endl;
+  } else {
+    std::cout << "failed" << std::endl;
+  }
+
+  return res.success;
+
+}

srv/save_pcd.srv

@@ -0,0 +1,4 @@
+float32 leaf_size
+string save_path
+---
+bool success