public void publish(Point3D[] points, float[] intensities, String frameId) { PointCloud2 message = getMessage(); message.getHeader().setFrameId(frameId); message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis())); message.setHeight(1); message.setWidth(points.length); message.setPointStep(pointType.getPointStep()); int dataLength = pointType.getPointStep() * points.length; message.setRowStep(dataLength); message.setIsBigendian(false); message.setIsDense(true); message.setFields(pointType.getPointField()); ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength); for(int i=0;i<points.length;i++) { buffer.writeFloat((float)points[i].getX()); buffer.writeFloat((float)points[i].getY()); buffer.writeFloat((float)points[i].getZ()); buffer.writeFloat(intensities[i]); } message.setData(buffer); publish(message); }
int numberOfPoints = pointCloud.getWidth() * pointCloud.getHeight(); packet.points = new Point3d[numberOfPoints]; packet.pointType = PointType.fromFromFieldNames(pointCloud.getFields()); packet.width = pointCloud.getWidth(); packet.height = pointCloud.getHeight(); int offset = pointCloud.getData().arrayOffset(); int pointStep = pointCloud.getPointStep(); byte[] array = pointCloud.getData().array(); ByteBuffer byteBuffer = ByteBuffer.wrap(array, offset, numberOfPoints * pointStep); if (pointCloud.getIsBigendian()) byteBuffer.order(ByteOrder.BIG_ENDIAN); else
@Override public void onNewMessage(PointCloud2 pointCloud) { if(DEBUG) { System.out.println(getClass().getSimpleName() + ": Received point cloud from " + rosTopic + " at rate " + 1.0 / timer.averageLap() + " FPS"); timer.lap(); } UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3D[] points = pointCloudData.getPoints(); ArrayList<Point3D> pointsAsArrayList = new ArrayList<Point3D>(Arrays.asList(points)); long[] timestamps = new long[points.length]; long time = pointCloud.getHeader().getStamp().totalNsecs(); Arrays.fill(timestamps, time); pointCloudDataReceiver.receivedPointCloudData(cloudFrame, sensorframe, timestamps, pointsAsArrayList, pointCloudSource); } }
@Override public synchronized void onNewMessage(PointCloud2 pointCloud) { growablePointCloud.clear(); UnpackedPointCloud unpackedCloud=unpackPointsAndIntensities(pointCloud); for (int i = 0; i < unpackedCloud.getPoints().length; i++) { switch (PointType.fromFromFieldNames(pointCloud.getFields())) { case XYZI : if (includePoint(unpackedCloud.getPoints()[i], unpackedCloud.getIntensities()[i])) growablePointCloud.addPoint(unpackedCloud.getPoints()[i], unpackedCloud.getIntensities()[i]); break; case XYZRGB : if (includePoint(unpackedCloud.getPoints()[i], unpackedCloud.getPointColors()[i])) growablePointCloud.addPoint(unpackedCloud.getPoints()[i], unpackedCloud.getPointColors()[i]); break; } } System.out.println("Publishing " + pointCloud.getHeader().getSeq() + " " + growablePointCloud.size() + " points"); publisher.publish(growablePointCloud.getPoints(), growablePointCloud.getColors(), pointCloud.getHeader().getFrameId()); /* * Transform3d pinkBlobTransform = new Transform3d(); * pinkBlobTransform.setTranslation(new Vector3d(growablePointCloud.getMeanPoint())); * tfPublisher.publish(pinkBlobTransform, pointCloud.getHeader().getStamp().totalNsecs(), "/multisense/left_camera_optical_frame", "pinkBlob"); */ } };
@Override public void onNewMessage(PointCloud2 pointCloud) { if(DEBUG) { System.out.println(getClass().getSimpleName() + ": Received point cloud from " + rosTopic + " at rate " + 1.0 / timer.averageLap() + " FPS"); timer.lap(); } UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3d[] points = pointCloudData.getPoints(); ArrayList<Point3d> pointsAsArrayList = new ArrayList<Point3d>(Arrays.asList(points)); long[] timestamps = new long[points.length]; long time = pointCloud.getHeader().getStamp().totalNsecs(); Arrays.fill(timestamps, time); pointCloudDataReceiver.receivedPointCloudData(cloudFrame, sensorframe, timestamps, pointsAsArrayList, pointCloudSource); } }
@Override public synchronized void onNewMessage(PointCloud2 pointCloud) { growablePointCloud.clear(); UnpackedPointCloud unpackedCloud=unpackPointsAndIntensities(pointCloud); for (int i = 0; i < unpackedCloud.getPoints().length; i++) { switch (PointType.fromFromFieldNames(pointCloud.getFields())) { case XYZI : if (includePoint(unpackedCloud.getPoints()[i], unpackedCloud.getIntensities()[i])) growablePointCloud.addPoint(unpackedCloud.getPoints()[i], unpackedCloud.getIntensities()[i]); break; case XYZRGB : if (includePoint(unpackedCloud.getPoints()[i], unpackedCloud.getPointColors()[i])) growablePointCloud.addPoint(unpackedCloud.getPoints()[i], unpackedCloud.getPointColors()[i]); break; } } System.out.println("Publishing " + pointCloud.getHeader().getSeq() + " " + growablePointCloud.size() + " points"); publisher.publish(growablePointCloud.getPoints(), growablePointCloud.getColors(), pointCloud.getHeader().getFrameId()); /* * Transform3d pinkBlobTransform = new Transform3d(); * pinkBlobTransform.setTranslation(new Vector3d(growablePointCloud.getMeanPoint())); * tfPublisher.publish(pinkBlobTransform, pointCloud.getHeader().getStamp().totalNsecs(), "/multisense/left_camera_optical_frame", "pinkBlob"); */ } };
public void publish(Point3d[] points, float[] intensities, String frameId) { PointCloud2 message = getMessage(); message.getHeader().setFrameId(frameId); message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis())); message.setHeight(1); message.setWidth(points.length); message.setPointStep(pointType.getPointStep()); int dataLength = pointType.getPointStep() * points.length; message.setRowStep(dataLength); message.setIsBigendian(false); message.setIsDense(true); message.setFields(pointType.getPointField()); ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength); for(int i=0;i<points.length;i++) { buffer.writeFloat((float)points[i].getX()); buffer.writeFloat((float)points[i].getY()); buffer.writeFloat((float)points[i].getZ()); buffer.writeFloat(intensities[i]); } message.setData(buffer); publish(message); }
int numberOfPoints = pointCloud.getWidth() * pointCloud.getHeight(); packet.points = new Point3D[numberOfPoints]; packet.pointType = PointType.fromFromFieldNames(pointCloud.getFields()); packet.width = pointCloud.getWidth(); packet.height = pointCloud.getHeight(); int offset = pointCloud.getData().arrayOffset(); int pointStep = pointCloud.getPointStep(); byte[] array = pointCloud.getData().array(); ByteBuffer byteBuffer = ByteBuffer.wrap(array, offset, numberOfPoints * pointStep); if (pointCloud.getIsBigendian()) byteBuffer.order(ByteOrder.BIG_ENDIAN); else
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3D[] scanPoints = pointCloudData.getPoints(); long timestamp = pointCloud.getHeader().getStamp().totalNsecs(); scanDataToPublish.set(new ScanData(timestamp, scanPoints)); } };
public void publish(Point3D[] points, MutableColor[] colors, String frameId) { PointCloud2 message = getMessage(); message.getHeader().setFrameId(frameId); message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis())); message.setHeight(1); message.setWidth(points.length); message.setPointStep(pointType.getPointStep()); int dataLength = pointType.getPointStep() * points.length; message.setRowStep(dataLength); message.setIsBigendian(false); message.setIsDense(true); message.setFields(pointType.getPointField()); ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength); for(int i=0;i<points.length;i++) { buffer.writeFloat((float)points[i].getX()); buffer.writeFloat((float)points[i].getY()); buffer.writeFloat((float)points[i].getZ()); buffer.writeByte((int)colors[i].getZ()); buffer.writeByte((int)colors[i].getY()); buffer.writeByte((int)colors[i].getX()); buffer.writeByte(0); //dummy; } message.setData(buffer); publish(message); }
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3d[] scanPoints = pointCloudData.getPoints(); long timestamp = pointCloud.getHeader().getStamp().totalNsecs(); scanDataToPublish.set(new ScanData(timestamp, scanPoints)); } };
public void publish(Point3d[] points, Color3f[] colors, String frameId) { PointCloud2 message = getMessage(); message.getHeader().setFrameId(frameId); message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis())); message.setHeight(1); message.setWidth(points.length); message.setPointStep(pointType.getPointStep()); int dataLength = pointType.getPointStep() * points.length; message.setRowStep(dataLength); message.setIsBigendian(false); message.setIsDense(true); message.setFields(pointType.getPointField()); ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength); for(int i=0;i<points.length;i++) { buffer.writeFloat((float)points[i].getX()); buffer.writeFloat((float)points[i].getY()); buffer.writeFloat((float)points[i].getZ()); buffer.writeByte((int)colors[i].getZ()); buffer.writeByte((int)colors[i].getY()); buffer.writeByte((int)colors[i].getX()); buffer.writeByte(0); //dummy; } message.setData(buffer); publish(message); }
@Override public void onNewMessage(PointCloud2WithSource pointCloud) { PointCloud2 cloud = pointCloud.getCloud(); UnpackedPointCloud pointCloudData = RosPointCloudReceiver.unpackPointsAndIntensities(cloud); Point3D[] scanPoints = pointCloudData.getPoints(); long timestamp = cloud.getHeader().getStamp().totalNsecs(); scanDataToPublish.set(new ScanData(timestamp, scanPoints)); } };
public void publish(Point3D[] points, MutableColor color, String frameId) { PointCloud2 message = getMessage(); message.getHeader().setFrameId(frameId); message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis())); message.setHeight(1); message.setWidth(points.length); message.setPointStep(pointType.getPointStep()); int dataLength = pointType.getPointStep() * points.length; message.setRowStep(dataLength); message.setIsBigendian(false); message.setIsDense(true); message.setFields(pointType.getPointField()); ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength); for(int i=0;i<points.length;i++) { buffer.writeFloat((float)points[i].getX()); buffer.writeFloat((float)points[i].getY()); buffer.writeFloat((float)points[i].getZ()); buffer.writeByte((int)color.getZ()); buffer.writeByte((int)color.getY()); buffer.writeByte((int)color.getX()); buffer.writeByte(0); //dummy; } message.setData(buffer); publish(message); }
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3D[] scanPoints = pointCloudData.getPoints(); Color[] colors = pointCloudData.getPointColors(); long timestamp = pointCloud.getHeader().getStamp().totalNsecs(); pointCloudDataToPublish.set(new ColorPointCloudData(timestamp, scanPoints, colors)); } };
public void publish(Point3d[] points, Color3f color, String frameId) { PointCloud2 message = getMessage(); message.getHeader().setFrameId(frameId); message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis())); message.setHeight(1); message.setWidth(points.length); message.setPointStep(pointType.getPointStep()); int dataLength = pointType.getPointStep() * points.length; message.setRowStep(dataLength); message.setIsBigendian(false); message.setIsDense(true); message.setFields(pointType.getPointField()); ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength); for(int i=0;i<points.length;i++) { buffer.writeFloat((float)points[i].getX()); buffer.writeFloat((float)points[i].getY()); buffer.writeFloat((float)points[i].getZ()); buffer.writeByte((int)color.getZ()); buffer.writeByte((int)color.getY()); buffer.writeByte((int)color.getX()); buffer.writeByte(0); //dummy; } message.setData(buffer); publish(message); }
@Override public void onNewMessage(PointCloud2WithSource pointCloud) { PointCloud2 cloud = pointCloud.getCloud(); UnpackedPointCloud pointCloudData = RosPointCloudReceiver.unpackPointsAndIntensities(cloud); Point3d[] scanPoints = pointCloudData.getPoints(); long timestamp = cloud.getHeader().getStamp().totalNsecs(); scanDataToPublish.set(new ScanData(timestamp, scanPoints)); } };