- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {Charset c =
String charsetName;Charset.forName(charsetName)
- Smart code suggestions by Tabnine
}
@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); } }
public static UnpackedPointCloud unpackPointsAndIntensities(PointCloud2 pointCloud) UnpackedPointCloud packet = new UnpackedPointCloud(); int numberOfPoints = pointCloud.getWidth() * pointCloud.getHeight(); packet.points = new Point3D[numberOfPoints];
private synchronized void onNewPointcloud(RosPointCloudSubscriber.UnpackedPointCloud unpackedPointCloud) { OrganizedPointCloud organizedPointCloud = new OrganizedPointCloud(unpackedPointCloud.getWidth(), unpackedPointCloud.getHeight(), unpackedPointCloud.getXYZRGB()); ArrayList<LineModDetection> detections = new ArrayList<>(); LineModDetection bestDetection = detectObjectAndEstimatePose(organizedPointCloud, detections,false,false); System.out.println(bestDetection); if (bestDetection != null) { BufferedImage image = organizedPointCloud.getRGBImage(); Collections.sort(detections); for(int i=0;i<Math.min(detections.size(),3);i++) { drawDetectionOnImage(detections.get(i), image); } int heightOffset = frame.getHeight() - frame.getContentPane().getHeight(); int widthOffset = frame.getWidth() - frame.getContentPane().getWidth(); frame.setSize(image.getWidth() + widthOffset, image.getHeight() + heightOffset); imagePanel.setBufferedImageSafe(image); } }
public static UnpackedPointCloud unpackPointsAndIntensities(PointCloud2 pointCloud) UnpackedPointCloud packet = new UnpackedPointCloud(); int numberOfPoints = pointCloud.getWidth() * pointCloud.getHeight(); packet.points = new Point3d[numberOfPoints];
@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; } } };
/** * received a point cloud from the multisense * transform the points using the mocap data */ @Override public void onNewMessage(PointCloud2 pointCloud) { robotDataReceiver.updateRobotModel(); pointCloudReferenceFrame.update(); UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3d[] points = pointCloudData.getPoints(); RigidBodyTransform pointTransform = new RigidBodyTransform(headInMocapFrame.get()); RigidBodyTransform transformFromPointCloudOriginToHeadRoot = pointCloudReferenceFrame.getTransformToDesiredFrame(headRootReferenceFrame); pointTransform.multiply(transformFromPointCloudOriginToHeadRoot); for(int i = 0; i < points.length; i++) { pointTransform.transform(points[i]); } MultisenseMocapExperimentPacket pointCloudPacket = new MultisenseMocapExperimentPacket(); pointCloudPacket.setDestination(PacketDestination.UI); pointCloudPacket.setPointCloud(points, testInfo); packetCommunicator.send(pointCloudPacket); }
/** * received a point cloud from the multisense * transform the points using the mocap data */ @Override public void onNewMessage(PointCloud2 pointCloud) { robotDataReceiver.updateRobotModel(); pointCloudReferenceFrame.update(); UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud); Point3d[] points = pointCloudData.getPoints(); RigidBodyTransform pointTransform = new RigidBodyTransform(headInMocapFrame.get()); RigidBodyTransform transformFromPointCloudOriginToHeadRoot = pointCloudReferenceFrame.getTransformToDesiredFrame(headRootReferenceFrame); pointTransform.multiply(transformFromPointCloudOriginToHeadRoot); for(int i = 0; i < points.length; i++) { pointTransform.transform(points[i]); } MultisenseMocapExperimentPacket pointCloudPacket = new MultisenseMocapExperimentPacket(); pointCloudPacket.setDestination(PacketDestination.UI); pointCloudPacket.setPointCloud(points, testInfo); packetCommunicator.send(pointCloudPacket); }
@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 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"); */ } };
private synchronized void onNewPointcloud(RosPointCloudSubscriber.UnpackedPointCloud unpackedPointCloud) { OrganizedPointCloud organizedPointCloud = new OrganizedPointCloud(unpackedPointCloud.getWidth(), unpackedPointCloud.getHeight(), unpackedPointCloud.getXYZRGB()); ArrayList<LineModDetection> detections = new ArrayList<>(); LineModDetection bestDetection = detectObjectAndEstimatePose(organizedPointCloud, detections,false,false); System.out.println(bestDetection); if (bestDetection != null) { BufferedImage image = organizedPointCloud.getRGBImage(); Collections.sort(detections); for(int i=0;i<Math.min(detections.size(),3);i++) { drawDetectionOnImage(detections.get(i), image); } int heightOffset = frame.getHeight() - frame.getContentPane().getHeight(); int widthOffset = frame.getWidth() - frame.getContentPane().getWidth(); frame.setSize(image.getWidth() + widthOffset, image.getHeight() + heightOffset); imagePanel.setBufferedImageSafe(image); } }
@Override public void onNewMessage(PointCloud2WithSource cloudHolder) { UnpackedPointCloud pointCloudData = RosPointCloudSubscriber.unpackPointsAndIntensities(cloudHolder.getCloud()); Point3D[] points = pointCloudData.getPoints(); Point translation = cloudHolder.getTranslation(); Point3D lidarPosition = new Point3D(translation.getX(), translation.getY(), translation.getZ()); geometry_msgs.Quaternion orientation = cloudHolder.getOrientation(); Quaternion lidarQuaternion = new Quaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); LidarScanMessage lidarScanMessage = new LidarScanMessage(); lidarScanMessage.getLidarPosition().set(lidarPosition); lidarScanMessage.getLidarOrientation().set(lidarQuaternion); MessageTools.packScan(lidarScanMessage, points); lidarScanPublisher.publish(lidarScanMessage); }
@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 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)); } };
@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)); } };
@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)); } };
private void processAndSendPointCloud(UnpackedPointCloud pointCloudData) { Point3d[] points = pointCloudData.getPoints(); LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket(); localizationMapPacket.setLocalizationPointMap(points); rosModulePacketCommunicator.send(localizationMapPacket); }
private void processAndSendPointCloud(UnpackedPointCloud pointCloudData) { Point3D[] points = pointCloudData.getPoints(); LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket(); HumanoidMessageTools.packLocalizationPointMap(points, localizationMapPacket); localizationPointMapPublisher.publish(localizationMapPacket); }
@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)); } };
@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)); } };