@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)); } };
@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"); */ } };
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud cloud = unpackPointsAndIntensities(pointCloud); if (cloud.getWidth() > 1200) { int ptr = 0; Point3D[] points = new Point3D[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; Color[] pointColors = new Color[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; for (int j = 0; j < cloud.getHeight(); j += 2) for (int i = 0; i < cloud.getWidth(); i += 2) { points[ptr] = cloud.getPoints()[j * cloud.getWidth() + i]; pointColors[ptr] = cloud.getPointColors()[j * cloud.getWidth() + i]; ptr++; } UnpackedPointCloud scaledCloud = new UnpackedPointCloud(cloud.getWidth() / 2, cloud.getHeight() / 2, cloud.getPointType(), points, pointColors); onNewPointcloud(scaledCloud); return; } else { onNewPointcloud(cloud); return; } } };
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud cloud = unpackPointsAndIntensities(pointCloud); if (cloud.getWidth() > 1200) { int ptr = 0; Point3d[] points = new Point3d[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; Color[] pointColors = new Color[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; for (int j = 0; j < cloud.getHeight(); j += 2) for (int i = 0; i < cloud.getWidth(); i += 2) { points[ptr] = cloud.getPoints()[j * cloud.getWidth() + i]; pointColors[ptr] = cloud.getPointColors()[j * cloud.getWidth() + i]; ptr++; } UnpackedPointCloud scaledCloud = new UnpackedPointCloud(cloud.getWidth() / 2, cloud.getHeight() / 2, cloud.getPointType(), points, pointColors); onNewPointcloud(scaledCloud); return; } else { onNewPointcloud(cloud); return; } } };