change detection_lidar_eucluster.

src/detection/detection_lidar_EuclideanClustering/main.cpp

@@ -26,6 +26,8 @@ std::vector<Box> prev_boxes_, curr_boxes_;
 
 std::shared_ptr<ObstacleDetector<pcl::PointXYZ>> obstacle_detector;
 
+void * gpa_cluster,*gpa_ground;
+
 
 void publishDetectedObjects(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>&& cloud_clusters)
 {
@@ -50,7 +52,7 @@ void publishDetectedObjects(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>&& c
   // Transform boxes from lidar frame to base_link frame, and convert to jsk and autoware msg formats
   for (auto& box : curr_boxes_)
   {
-      std::cout<<"     box x y z"<<box.position(0)<<" "<<box.position(1)<<" "<<box.position(2)<<std::endl;
+      std::cout<<"     box x y z "<<box.position(0)<<" "<<box.position(1)<<" "<<box.position(2)<<std::endl;
 //    geometry_msgs::Pose pose, pose_transformed;
 //    pose.position.x = box.position(0);
 //    pose.position.y = box.position(1);
@@ -71,6 +73,51 @@ void publishDetectedObjects(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>&& c
   curr_boxes_.clear();
 }
 
+void sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize =  reinterpret_cast<int * >(strOut);
+    *pHeadSize = static_cast<int >(4 + point_cloud->header.frame_id.size()+4+8) ;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    uint32_t * pu32 =  reinterpret_cast<uint32_t *>(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = reinterpret_cast<pcl::PointXYZI *>(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+    std::cout<<"width: "<<point_cloud->width<<std::endl;
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+
+    delete [] strOut;
+}
+
+void sharepointxyz( pcl::PointCloud<pcl::PointXYZ>::Ptr &raw_cloud,void * pa)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->header.seq = 1;
+    point_cloud->header.stamp = 0;
+
+    int nPCount = raw_cloud->points.size();
+    std::cout<<" size: "<<nPCount<<std::endl;
+    int i;
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = raw_cloud->points.at(i).x;
+         xp.y = raw_cloud->points.at(i).y;
+          xp.z = raw_cloud->points.at(i).z;
+           xp.intensity = 100;
+           point_cloud->push_back(xp);
+    }
+
+    sharepointcloud(pa,point_cloud);
+}
+
 void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZ>::Ptr &raw_cloud)
 {
     // Downsampleing, ROI, and removing the car roof
@@ -83,6 +130,9 @@ void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZ>::Ptr &raw_cloud)
     auto cloud_clusters = obstacle_detector->clustering(segmented_clouds.first, CLUSTER_THRESH, CLUSTER_MIN_SIZE, CLUSTER_MAX_SIZE);
 
     publishDetectedObjects(std::move(cloud_clusters));
+
+    sharepointxyz(segmented_clouds.first,gpa_cluster);
+    sharepointxyz(segmented_clouds.second,gpa_ground);
 }
 
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -163,6 +213,8 @@ int main(int argc, char *argv[])
 
     obstacle_detector = std::make_shared<ObstacleDetector<pcl::PointXYZ>>();
 
+    gpa_cluster = iv::modulecomm::RegisterSend("lidarpc_cluster",10000000,1);
+    gpa_ground  = iv::modulecomm::RegisterSend("lidarpc_ground",10000000,1);
     void * pa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
 
     return a.exec();