private void verifyDataFromGazeboRemainsTheSame(LidarScanParameters polarLidarScanParameters) { verifyLidarScanDefinitionDoesNotChange(polarLidarScanParameters); verifyTimeIncrementRemainsZero(polarLidarScanParameters); }
@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; } } };
public static void main(String[] arg) { RosLineModDetector detector = new RosLineModDetector("examples/drill/drill.obj"); detector.rosPointCloudSubscriber.wailTillRegistered(); System.out.println("connected to rosmaster"); }
@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); }
private void processAndSendPointCloud(UnpackedPointCloud pointCloudData) { Point3d[] points = pointCloudData.getPoints(); LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket(); localizationMapPacket.setLocalizationPointMap(points); rosModulePacketCommunicator.send(localizationMapPacket); }
public void onNewMessage(LaserScan message) { LidarScanParameters polarLidarScanParameters = new LidarScanParameters(message.getRanges().length, 1, message.getAngleMin(), message.getAngleMax(), message.getTimeIncrement(), message.getRangeMin(), message.getRangeMax(), message.getScanTime(), message.getHeader().getStamp().totalNsecs()); if (DEBUG) { verifyDataFromGazeboRemainsTheSame(polarLidarScanParameters); } LidarScan polarLidarScan = new LidarScan(polarLidarScanParameters, message.getRanges(),sensorId); newScan(polarLidarScan); }
public void removeSubscriber(RosTopicSubscriberInterface<? extends Message> subscriber) { if (subscribers.containsValue(subscriber) && rosSubscribers.containsKey(subscriber)) { Subscriber<? extends Message> rosSubscriber = rosSubscribers.get(subscriber); rosSubscriber.shutdown(); rosSubscribers.remove(subscriber); subscribers.remove(subscriber.getMessageType()); } }
public RosFiducialDetector(RosMainNode rosMainNode, String imageTopic, String cameraInfoTopic, FiducialDetector<GrayF32> detector) { cameraInfoSubscriber = new RosCameraInfoSubscriber(rosMainNode, cameraInfoTopic); rosMainNode.attachPublisher("/tf", tfPublisher); rosMainNode.attachSubscriber(imageTopic, this); this.detector = detector; if(VISUALIZE) { imagePanel= new ImagePanel(100, 100); frame=ShowImages.showWindow(imagePanel, "Fiducials"); } }
public void onNewMessage(sensor_msgs.CompressedImage message) { long timeStamp = message.getHeader().getStamp().totalNsecs(); imageReceived(timeStamp, RosTools.bufferedImageFromRosMessageJpeg(colorModel, message)); }
@Override public synchronized void onNewMessage(sensor_msgs.Imu message) { this.timeStamp = message.getHeader().getStamp().totalNsecs(); this.seq_id = message.getHeader().getSeq(); this.frameId = message.getHeader().getFrameId(); RosTools.packRosQuaternionToEuclidQuaternion(message.getOrientation(), this.orientationEstimate); RosTools.packRosVector3ToEuclidTuple3D(message.getAngularVelocity(), this.angularVelocity); RosTools.packRosVector3ToEuclidTuple3D(message.getLinearAcceleration(), this.linearAcceleration); onNewMessage(timeStamp, seq_id, orientationEstimate, angularVelocity, linearAcceleration); }
public void onNewMessage(sensor_msgs.Image message) { long timeStamp = message.getHeader().getStamp().totalNsecs(); imageReceived(timeStamp, RosTools.bufferedImageFromRosMessageRaw(colorModel, message)); }
@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; } } };
private void processAndSendPointCloud(UnpackedPointCloud pointCloudData) { Point3D[] points = pointCloudData.getPoints(); LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket(); HumanoidMessageTools.packLocalizationPointMap(points, localizationMapPacket); localizationPointMapPublisher.publish(localizationMapPacket); }
private void verifyDataFromGazeboRemainsTheSame(LidarScanParameters polarLidarScanParameters) { verifyLidarScanDefinitionDoesNotChange(polarLidarScanParameters); verifyTimeIncrementRemainsZero(polarLidarScanParameters); }
public static void main(String[] arg) { RosLineModDetector detector = new RosLineModDetector("examples/drill/drill.obj"); detector.rosPointCloudSubscriber.wailTillRegistered(); System.out.println("connected to rosmaster"); }
public void removeSubscriber(RosTopicSubscriberInterface<? extends Message> subscriber) { if (subscribers.containsValue(subscriber) && rosSubscribers.containsKey(subscriber)) { Subscriber<? extends Message> rosSubscriber = rosSubscribers.get(subscriber); rosSubscriber.shutdown(); rosSubscribers.remove(subscriber); subscribers.remove(subscriber.getMessageType()); } }
public RosFiducialDetector(RosMainNode rosMainNode, String imageTopic, String cameraInfoTopic, FiducialDetector<GrayF32> detector) { cameraInfoSubscriber = new RosCameraInfoSubscriber(rosMainNode, cameraInfoTopic); rosMainNode.attachPublisher("/tf", tfPublisher); rosMainNode.attachSubscriber(imageTopic, this); this.detector = detector; if(VISUALIZE) { imagePanel= new ImagePanel(100, 100); frame=ShowImages.showWindow(imagePanel, "Fiducials"); } }
public void onNewMessage(sensor_msgs.CompressedImage message) { long timeStamp = message.getHeader().getStamp().totalNsecs(); imageReceived(timeStamp, RosTools.bufferedImageFromRosMessageJpeg(colorModel, message)); }
@Override public synchronized void onNewMessage(sensor_msgs.Imu message) { this.timeStamp = message.getHeader().getStamp().totalNsecs(); this.seq_id = message.getHeader().getSeq(); this.frameId = message.getHeader().getFrameId(); RosTools.packRosQuaternionToQuat4d(message.getOrientation(), this.orientationEstimate); RosTools.packRosVector3ToVector3d(message.getAngularVelocity(), this.angularVelocity); RosTools.packRosVector3ToVector3d(message.getLinearAcceleration(), this.linearAcceleration); onNewMessage(timeStamp, seq_id, orientationEstimate, angularVelocity, linearAcceleration); }
public void onNewMessage(sensor_msgs.Image message) { long timeStamp = message.getHeader().getStamp().totalNsecs(); imageReceived(timeStamp, RosTools.bufferedImageFromRosMessageRaw(colorModel, message)); }