daily pastebin goal
18%
SHARE
TWEET

Untitled

a guest Jun 13th, 2018 66 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. sensor_msgs::PointCloud2 prepareTimedPointCloud2Message(const ros::Time timestamp, const std::string &frameId,
  2.                                                         const int numPoints) {
  3.     sensor_msgs::PointCloud2 msg;
  4.     msg.header.stamp = timestamp;
  5.     msg.header.frame_id = frameId;
  6.     msg.height = 1;
  7.     msg.width = numPoints;
  8.     msg.fields.resize(4);
  9.     msg.fields[0].name = "x";
  10.     msg.fields[0].offset = 0;
  11.     msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
  12.     msg.fields[0].count = 1;
  13.     msg.fields[1].name = "y";
  14.     msg.fields[1].offset = 4;
  15.     msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
  16.     msg.fields[1].count = 1;
  17.     msg.fields[2].name = "z";
  18.     msg.fields[2].offset = 8;
  19.     msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
  20.     msg.fields[2].count = 1;
  21.     msg.fields[3].name = "time";
  22.     msg.fields[3].offset = 12;
  23.     msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
  24.     msg.fields[3].count = 1;
  25.     msg.is_bigendian = false;
  26.     msg.point_step = 16;
  27.     msg.row_step = 16 * msg.width;
  28.     msg.is_dense = true;
  29.     msg.data.resize(16 * numPoints);
  30.     return msg;
  31. }
  32.  
  33. sensor_msgs::PointCloud2 toTimedPointCloud2Message(const ros::Time timestamp, const std::string &frameId,
  34.                                                    const ::cartographer::sensor::TimedPointCloud &pointCloud) {
  35.     auto msg = prepareTimedPointCloud2Message(timestamp, frameId, pointCloud.size());
  36.     ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
  37.     for (const Eigen::Vector4f &point : pointCloud) {
  38.         stream.next(point.x());
  39.         stream.next(point.y());
  40.         stream.next(point.z());
  41.         stream.next(point.w());
  42.     }
  43.     return msg;
  44. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top