Function callbackNode

nodes/pub_for_legoloam.cpp:43–70

}

void callbackNode(const sensor_msgs::PointCloud2::ConstPtr &msg) {
  cout << msg->header.seq << "th node come" << endl;
  pcl::PointCloud<PointType> pc_curr = cloudmsg2cloud<PointType>(msg);
  pcl::PointCloud<PointType> pc_ground;
  pcl::PointCloud<PointType> pc_non_ground;
  pc_ground.header = pc_curr.header;
  pc_non_ground.header = pc_curr.header;

  static double time_taken;

  cout << "Operating patchwork..." << endl;
  PatchworkGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken);

  auto msg_curr = cloud2msg(pc_curr);
  auto msg_ground = cloud2msg(pc_ground);

  patchwork::ground_estimate cloud_estimate;
  cloud_estimate.header = msg->header;
  cloud_estimate.curr = msg_curr;
  cloud_estimate.ground = msg_ground;
  EstimatePublisher.publish(cloud_estimate);

  /*
  CloudPublisher.publish(cloud2msg(pc_curr));
  PositivePublisher.publish(cloud2msg(pc_ground));
  NegativePublisher.publish(cloud2msg(pc_non_ground));
  */
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "Benchmark");