Tabnine Logo
RosPointCloudSubscriber$UnpackedPointCloud.getPoints
Code IndexAdd Tabnine to your IDE (free)

How to use
getPoints
method
in
us.ihmc.utilities.ros.subscriber.RosPointCloudSubscriber$UnpackedPointCloud

Best Java code snippets using us.ihmc.utilities.ros.subscriber.RosPointCloudSubscriber$UnpackedPointCloud.getPoints (Showing top 16 results out of 315)

origin: us.ihmc/IHMCPerception

  @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);
  }
}
origin: us.ihmc/ihmc-perception

  @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);
  }
}
origin: us.ihmc/IHMCAvatarInterfaces

  @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));
  }
};
origin: us.ihmc/IHMCROSTools

  @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");
    */
  }
};
origin: us.ihmc/ihmc-perception

  @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;
   }
  }
};
origin: us.ihmc/IHMCPerception

  @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;
   }
  }
};
origin: us.ihmc/IHMCAvatarInterfaces

/**
* 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);
}
origin: us.ihmc/DarpaRoboticsChallenge

/**
* 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);
}
origin: us.ihmc/ihmc-ros-tools

  @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");
    */
  }
};
origin: us.ihmc/robot-environment-awareness-application

@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);
}
origin: us.ihmc/IHMCAvatarInterfaces

  @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));
  }
};
origin: us.ihmc/ihmc-avatar-interfaces

  @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));
  }
};
origin: us.ihmc/ihmc-avatar-interfaces

  @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));
  }
};
origin: us.ihmc/IHMCPerception

private void processAndSendPointCloud(UnpackedPointCloud pointCloudData)
{
 Point3d[] points = pointCloudData.getPoints();
 LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket();
 localizationMapPacket.setLocalizationPointMap(points);
 rosModulePacketCommunicator.send(localizationMapPacket);
}
origin: us.ihmc/ihmc-perception

private void processAndSendPointCloud(UnpackedPointCloud pointCloudData)
{
 Point3D[] points = pointCloudData.getPoints();
 LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket();
 HumanoidMessageTools.packLocalizationPointMap(points, localizationMapPacket);
 localizationPointMapPublisher.publish(localizationMapPacket);
}
origin: us.ihmc/ihmc-avatar-interfaces

  @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));
  }
};
us.ihmc.utilities.ros.subscriberRosPointCloudSubscriber$UnpackedPointCloudgetPoints

Popular methods of RosPointCloudSubscriber$UnpackedPointCloud

  • getPointColors
  • <init>
  • getHeight
  • getIntensities
  • getPointType
  • getWidth
  • getXYZRGB

Popular in Java

  • Reactive rest calls using spring rest template
  • scheduleAtFixedRate (Timer)
  • putExtra (Intent)
  • setRequestProperty (URLConnection)
  • Component (java.awt)
    A component is an object having a graphical representation that can be displayed on the screen and t
  • GridBagLayout (java.awt)
    The GridBagLayout class is a flexible layout manager that aligns components vertically and horizonta
  • Vector (java.util)
    Vector is an implementation of List, backed by an array and synchronized. All optional operations in
  • Semaphore (java.util.concurrent)
    A counting semaphore. Conceptually, a semaphore maintains a set of permits. Each #acquire blocks if
  • Manifest (java.util.jar)
    The Manifest class is used to obtain attribute information for a JarFile and its entries.
  • Loader (org.hibernate.loader)
    Abstract superclass of object loading (and querying) strategies. This class implements useful common
  • Top 12 Jupyter Notebook extensions
Tabnine Logo
  • Products

    Search for Java codeSearch for JavaScript code
  • IDE Plugins

    IntelliJ IDEAWebStormVisual StudioAndroid StudioEclipseVisual Studio CodePyCharmSublime TextPhpStormVimGoLandRubyMineEmacsJupyter NotebookJupyter LabRiderDataGripAppCode
  • Company

    About UsContact UsCareers
  • Resources

    FAQBlogTabnine AcademyTerms of usePrivacy policyJava Code IndexJavascript Code Index
Get Tabnine for your IDE now