Option to publish only keyframes instead of full map

cfg/dlo.yaml

@@ -9,7 +9,7 @@
 
 dlo:
 
-  version: 1.4.1
+  version: 1.4.2
 
   adaptiveParams: true
 
@@ -21,5 +21,6 @@ dlo:
     child_frame: base_link
 
   mapNode:
+    publishFullMap: true
     publishFreq: 1.0
     leafSize: 0.25

include/dlo/map.h

@@ -50,6 +50,7 @@ class dlo::MapNode {
   ros::Time map_stamp;
   std::string odom_frame;
 
+  bool publish_full_map_;
   double publish_freq_;
   double leaf_size_;
 

package.xml

@@ -3,7 +3,7 @@
 <package format="2">
 
   <name>direct_lidar_odometry</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Direct LiDAR Odometry: Fast Localization with Dense Point Clouds</description>
 
   <maintainer email="[email protected]">Kenny J. Chen</maintainer>

src/dlo/map.cc

@@ -21,7 +21,10 @@ dlo::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) {
   this->getParams();
 
   this->abort_timer = this->nh.createTimer(ros::Duration(0.01), &dlo::MapNode::abortTimerCB, this);
-  this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_), &dlo::MapNode::publishTimerCB, this);
+
+  if (this->publish_full_map_){
+    this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_), &dlo::MapNode::publishTimerCB, this);
+  }
 
   this->keyframe_sub = this->nh.subscribe("keyframes", 1, &dlo::MapNode::keyframeCB, this);
   this->map_pub = this->nh.advertise<sensor_msgs::PointCloud2>("map", 1);
@@ -50,6 +53,7 @@ dlo::MapNode::~MapNode() {}
 void dlo::MapNode::getParams() {
 
   ros::param::param<std::string>("~dlo/odomNode/odom_frame", this->odom_frame, "odom");
+  ros::param::param<bool>("~dlo/mapNode/publishFullMap", this->publish_full_map_, true);
   ros::param::param<double>("~dlo/mapNode/publishFreq", this->publish_freq_, 1.0);
   ros::param::param<double>("~dlo/mapNode/leafSize", this->leaf_size_, 0.5);
 
@@ -131,6 +135,16 @@ void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe)
   this->map_stamp = keyframe->header.stamp;
   *this->dlo_map += *keyframe_pcl;
 
+  if (!this->publish_full_map_) {
+    if (keyframe_pcl->points.size() == keyframe_pcl->width * keyframe_pcl->height) {
+      sensor_msgs::PointCloud2 map_ros;
+      pcl::toROSMsg(*keyframe_pcl, map_ros);
+      map_ros.header.stamp = ros::Time::now();
+      map_ros.header.frame_id = this->odom_frame;
+      this->map_pub.publish(map_ros);
+    }
+  }
+
 }
 
 bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req,