现在网上的代码都是使用pcl转成ros消息再发布,特别麻烦
sensor_msgs::PointCloud2 PreparePointCloud2Message(const std::string &frame_id, const int num_points)
{
sensor_msgs::PointCloud2 msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = frame_id;
msg.height = 1;
msg.width = num_points;
msg.fields.resize(3);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.is_bigendian = false;
msg.point_step = 12;
msg.row_step = 12 * msg.width;
msg.is_dense = true;
msg.data.resize(12 * num_points);
return msg;
}
auto msg = PreparePointCloud2Message("base_link", cluster_points_size);
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
for (const auto &temp_point : points)
{
stream.next(temp_point.position.x());
stream.next(temp_point.position.y());
stream.next(0.0);
}
}