Function cloudCallback

swarm_exploration/traj_utils/src/process_msg.cpp:27–65

}

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) {
  auto tn = ros::Time::now();
  if ((tn - last_cloud_time_).toSec() < 5) {
    return;
  }
  last_cloud_time_ = tn;

  pcl::PointCloud<pcl::PointXYZ> pts;
  pcl::PointCloud<pcl::PointXYZ> pts2;
  pcl::fromROSMsg(*msg, pts);
  vector<Eigen::Vector3d> inf_pts(27);

  for (int i = 0; i < pts.points.size(); ++i) {
    Eigen::Vector3d pt;
    pt(0) = pts[i].x;
    pt(1) = pts[i].y;
    pt(2) = pts[i].z;
    for (int i = 0; i < 3; ++i)
      pt(i) = floor((pt(i) - map_origin_(i)) * 10);
    for (int i = 0; i < 3; ++i)
      pt(i) = (pt(i) + 0.5) * 0.1 + map_origin_(i);
    inflatePoint(pt, 1, inf_pts);
    for (auto pi : inf_pts) {
      pcl::PointXYZ pj;
      pj.x = pi(0);
      pj.y = pi(1);
      pj.z = pi(2);
      pts2.points.push_back(pj);
    }
  }
  pts2.width = pts2.points.size();
  pts2.height = 1;
  pts2.is_dense = true;
  pts2.header.frame_id = "world";

  sensor_msgs::PointCloud2 cloud;
  pcl::toROSMsg(pts2, cloud);
  cloud_pub.publish(cloud);
}

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