Merge pull request #17 from juliangaal/master

Better integration into ROS ecosystem

Author: seungjae24

include/patchworkpp/utils.hpp

@@ -65,8 +65,8 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID,
                                   (float, y, y)
                                   (float, z, z)
                                   (float, intensity, intensity)
-                                  (uint16_t, label, label)
-                                  (uint16_t, id, id))
+                                  (std::uint16_t, label, label)
+                                  (std::uint16_t, id, id))
 
 
 void PointXYZILID2XYZI(pcl::PointCloud<PointXYZILID>& src,
@@ -240,7 +240,7 @@ void calculate_precision_recall_without_vegetation(const pcl::PointCloud<PointXY
 }
 
 
-int save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, string seq, int count){
+void save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, string seq, int count){
 
   std::string count_str = std::to_string(count);
   std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str;

include/tools/kitti_loader.hpp

@@ -92,6 +92,8 @@ class KittiLoader {
             }
 
         }
+
+        return 0;
     }
 
 private:

rviz/demo.rviz

@@ -110,7 +110,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.10000000149011612
       Style: Flat Squares
-      Topic: /demo/ground
+      Topic: /ground_segmentation/ground
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -138,7 +138,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.10000000149011612
       Style: Flat Squares
-      Topic: /demo/nonground
+      Topic: /ground_segmentation/nonground
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true

src/demo.cpp

@@ -19,9 +19,10 @@ ros::Publisher pub_ground;
 ros::Publisher pub_non_ground;
 
 template<typename T>
-sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, std::string frame_id = "map") {
+sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, const ros::Time& stamp, std::string frame_id = "map") {
     sensor_msgs::PointCloud2 cloud_ROS;
     pcl::toROSMsg(cloud, cloud_ROS);
+    cloud_ROS.header.stamp = stamp;
     cloud_ROS.header.frame_id = frame_id;
     return cloud_ROS;
 }
@@ -38,12 +39,12 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg)
 
     PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken);
 
-    cout << "\033[1;32m" << "Result: Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() <<  "/ NonGround: " << pc_non_ground.size()
-         << " (running_time: " << time_taken << " sec)" << "\033[0m" << endl;
+    ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() <<  "/ NonGround: " << pc_non_ground.size()
+         << " (running_time: " << time_taken << " sec)" << "\033[0m");
 
-    pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.frame_id));
-    pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.frame_id));
-    pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.frame_id));
+    pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id));
+    pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id));
+    pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id));
 }
 
 int main(int argc, char**argv) {