Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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;
- }
Add Comment
Please, Sign In to add comment