publish slam info for 3rd-party usage

include/utility.h

 #pragma once
 #ifndef _UTILITY_LIDAR_ODOMETRY_H_
 #define _UTILITY_LIDAR_ODOMETRY_H_
+#define PCL_NO_PRECOMPILE 
 
 #include <ros/ros.h>
 
@@ -277,8 +278,8 @@ public:
     }
 };
 
-
-sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
+template<typename T>
+sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, const T& thisCloud, ros::Time thisStamp, std::string thisFrame)
 {
     sensor_msgs::PointCloud2 tempCloud;
     pcl::toROSMsg(*thisCloud, tempCloud);

msg/cloud_info.msg

@@ -26,4 +26,8 @@ float32 initialGuessYaw
 # Point cloud messages
 sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed
 sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature
-sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature
\ No newline at end of file
+sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature
+
+# 3rd party messages
+sensor_msgs/PointCloud2 keyframe_cloud
+sensor_msgs/PointCloud2 keyframe_poses

src/mapOptmization.cpp

@@ -70,10 +70,12 @@ public:
     ros::Publisher pubIcpKeyFrames;
     ros::Publisher pubRecentKeyFrames;
     ros::Publisher pubRecentKeyFrame;
-    ros::Publisher pubRecentKeyFrameRaw;
     ros::Publisher pubCloudRegisteredRaw;
     ros::Publisher pubLoopConstraintEdge;
 
+    ros::Publisher pubSLAMInfo;
+    ros::Publisher pubSLAMDummy;
+
     ros::Subscriber subCloud;
     ros::Subscriber subGPS;
     ros::Subscriber subLoop;
@@ -178,9 +180,11 @@ public:
 
         pubRecentKeyFrames    = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
         pubRecentKeyFrame     = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
-        pubRecentKeyFrameRaw  = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_unregistered", 1);
         pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);
 
+        pubSLAMInfo           = nh.advertise<lio_sam::cloud_info>("lio_sam/mapping/slam_info", 1);
+        pubSLAMDummy          = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/slam_dummy", 1);
+
         downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
         downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
         downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
@@ -1710,14 +1714,6 @@ public:
             *cloudOut += *transformPointCloud(laserCloudSurfLastDS,    &thisPose6D);
             publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
         }
-        // publish unregistered key frame
-        if (pubRecentKeyFrameRaw.getNumSubscribers() != 0)
-        {
-            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
-            *cloudOut += *laserCloudCornerLastDS;
-            *cloudOut += *laserCloudSurfLastDS;
-            publishCloud(&pubRecentKeyFrameRaw, cloudOut, timeLaserInfoStamp, lidarFrame);
-        }
         // publish registered high-res raw cloud
         if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
         {
@@ -1734,6 +1730,17 @@ public:
             globalPath.header.frame_id = odometryFrame;
             pubPath.publish(globalPath);
         }
+        // publish SLAM infomation for 3rd-party usage
+        if (pubSLAMInfo.getNumSubscribers() != 0)
+        {
+            lio_sam::cloud_info slamInfo;
+            slamInfo.header.stamp = timeLaserInfoStamp;
+            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
+            *cloudOut += *laserCloudCornerLastDS;
+            *cloudOut += *laserCloudSurfLastDS;
+            slamInfo.keyframe_cloud = publishCloud(&pubSLAMDummy, cloudOut, timeLaserInfoStamp, lidarFrame);
+            slamInfo.keyframe_poses = publishCloud(&pubSLAMDummy, cloudKeyPoses6D, timeLaserInfoStamp, odometryFrame);
+        }
     }
 };