timed_pointcloud2.cpp

sensor_msgs::PointCloud2 prepareTimedPointCloud2Message(const ros::Time timestamp, const std::string &frameId,
                                                        const int numPoints) {
    sensor_msgs::PointCloud2 msg;
    msg.header.stamp = timestamp;
    msg.header.frame_id = frameId;
    msg.height = 1;
    msg.width = numPoints;
    msg.fields.resize(4);
    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.fields[3].name = "time";
    msg.fields[3].offset = 12;
    msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
    msg.fields[3].count = 1;
    msg.is_bigendian = false;
    msg.point_step = 16;
    msg.row_step = 16 * msg.width;
    msg.is_dense = true;
    msg.data.resize(16 * numPoints);
    return msg;
}

sensor_msgs::PointCloud2 toTimedPointCloud2Message(const ros::Time timestamp, const std::string &frameId,
                                                   const ::cartographer::sensor::TimedPointCloud &pointCloud) {
    auto msg = prepareTimedPointCloud2Message(timestamp, frameId, pointCloud.size());
    ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
    for (const Eigen::Vector4f &point : pointCloud) {
        stream.next(point.x());
        stream.next(point.y());
        stream.next(point.z());
        stream.next(point.w());
    }
    return msg;
}