@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 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"); */ } };