How to use

RosPointCloudSubscriber$UnpackedPointCloud

in

us.ihmc.utilities.ros.subscriber

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