Method SegCb_

sloam/src/segmentation/inferenceNode.cpp:104–146

}

void SegNode::SegCb_(const sensor_msgs::PointCloud2ConstPtr &cloudMsg)
{

  if ((prevStamp - cloudMsg->header.stamp).toSec() < 1.0)
    return;

  CloudT::Ptr cloud(new CloudT);
  pcl::fromROSMsg(*cloudMsg, *cloud);
  pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);

  // RUN SEGMENTATION
  cv::Mat rMask = cv::Mat::zeros(cloudMsg->height, cloudMsg->width, CV_8U);
  segmentator_->run(cloud, rMask);
  // cv::imwrite("mask.jpg", rMask);

  CloudT::Ptr groundCloud(new CloudT());
  segmentator_->maskCloud(cloud, rMask, groundCloud, 1);

  CloudT::Ptr treeCloud(new CloudT);
  segmentator_->maskCloud(cloud, rMask, treeCloud, 255, true);

  std::vector<std::vector<TreeVertex>> landmarks;
  graphDetector_.computeGraph(cloud, treeCloud, landmarks);

  // std::cout << "SAVING DATA" << std::endl;
  // // write class instance to archive
  // std::ofstream ofs("landmarks");
  // boost::archive::text_oarchive oa(ofs);
  // oa << landmarks;
  // // write ground cloud to archive
  // pcl::io::savePCDFileASCII("ground.pcd", *groundCloud);
  // pcl::io::savePCDFileASCII("tree.pcd", *treeCloud);

  auto trellis = trellisCloud(landmarks);

  trellis->header = cloud->header;
  treeCloud->header = cloud->header;
  groundCloud->header = cloud->header;

  trellisPub_.publish(trellis);
  treePub_.publish(treeCloud);
  groundPub_.publish(groundCloud);
}

int main(int argc, char **argv)
{