private static LidarMount getSensor(FloatingRootJointRobot robot, String sensorName) { ArrayList<LidarMount> lidarSensors = robot.getSensors(LidarMount.class); if(lidarSensors.size() == 0) { System.err.println("DRCLidar: No LIDAR units found on SDF Robot."); return null; } for(LidarMount lidarMount : lidarSensors) { if(lidarMount.getName().equals(sensorName)) { return lidarMount; } } System.err.println("DRCLidar: Could Not find " + sensorName + "Using " + lidarSensors.get(0).getName() + "Instead! FIX THIS!"); return lidarSensors.get(0); }
public static void setupDRCRobotLidar(FloatingRootJointRobot robot, Graphics3DAdapter graphics3dAdapter, LocalObjectCommunicator objectCommunicator, DRCRobotJointMap jointMap, DRCRobotLidarParameters lidarParams, TimestampProvider timestampProvider, boolean startLidar) { if (graphics3dAdapter != null) { LidarMount lidarMount = getSensor(robot, lidarParams.getSensorNameInSdf()); LidarScanParameters lidarScanParameters = lidarMount.getLidarScanParameters(); int horizontalRays = lidarScanParameters.pointsPerSweep; int scanHeight = lidarScanParameters.scanHeight; float fov = lidarScanParameters.sweepYawMax - lidarScanParameters.sweepYawMin; float near = lidarScanParameters.minRange; float far = lidarScanParameters.maxRange; DRCLidarCallback callback = new DRCLidarCallback(objectCommunicator, lidarScanParameters, lidarParams.getSensorId()); GPULidar lidar = graphics3dAdapter.createGPULidar(callback, horizontalRays, scanHeight, fov, near, far); lidarMount.setLidar(lidar); } }
public static void setupDRCRobotLidar(FloatingRootJointRobot robot, Graphics3DAdapter graphics3dAdapter, LocalObjectCommunicator objectCommunicator, DRCRobotJointMap jointMap, DRCRobotLidarParameters lidarParams, TimestampProvider timestampProvider, boolean startLidar) { if (graphics3dAdapter != null) { LidarMount lidarMount = getSensor(robot, lidarParams.getSensorNameInSdf()); LidarSensorDescription desciption = lidarMount.getDescription(); int horizontalRays = desciption.getPointsPerSweep(); int scanHeight = desciption.getScanHeight(); float fov = (float) (desciption.getSweepYawMax() - desciption.getSweepYawMin()); float near = (float) desciption.getMinRange(); float far = (float) desciption.getMaxRange(); LidarScanParameters lidarScanParameters = new LidarScanParameters(desciption.getPointsPerSweep(), desciption.getScanHeight(), (float) desciption.getSweepYawMin(), (float) desciption.getSweepYawMax(), (float) desciption.getHeightPitchMin(), (float) desciption.getHeightPitchMax(), 0, (float) desciption.getMinRange(), (float) desciption.getMaxRange(), 0, 0); DRCLidarCallback callback = new DRCLidarCallback(objectCommunicator, lidarScanParameters, lidarParams.getSensorId()); GPULidar lidar = graphics3dAdapter.createGPULidar(callback, horizontalRays, scanHeight, fov, near, far); lidarMount.setLidar(lidar); } }
lidarSensorDescription.setSweepYawLimits(lidarScanParameters.getMinRange(), lidarScanParameters.getMaxRange()); lidarSensorDescription.setHeightPitchLimits(lidarScanParameters.heightPitchMin, lidarScanParameters.heightPitchMax); LidarMount lidarMount = new LidarMount(lidarSensorDescription); lidarJoint.addLidarMount(lidarMount);
public static void setupDRCRobotLidar(FloatingRootJointRobot robot, Graphics3DAdapter graphics3dAdapter, LocalObjectCommunicator objectCommunicator, DRCRobotJointMap jointMap, DRCRobotLidarParameters lidarParams, TimestampProvider timestampProvider, boolean startLidar) { if (graphics3dAdapter != null) { LidarMount lidarMount = getSensor(robot, lidarParams.getSensorNameInSdf()); LidarScanParameters lidarScanParameters = lidarMount.getLidarScanParameters(); int horizontalRays = lidarScanParameters.pointsPerSweep; int scanHeight = lidarScanParameters.scanHeight; float fov = lidarScanParameters.sweepYawMax - lidarScanParameters.sweepYawMin; float near = lidarScanParameters.minRange; float far = lidarScanParameters.maxRange; DRCLidarCallback callback = new DRCLidarCallback(objectCommunicator, lidarScanParameters, lidarParams.getSensorId()); GPULidar lidar = graphics3dAdapter.createGPULidar(callback, horizontalRays, scanHeight, fov, near, far); lidarMount.setLidar(lidar); } }
public static LidarMount getSensor(FloatingRootJointRobot robot, String sensorName) { ArrayList<LidarMount> lidarSensors = robot.getSensors(LidarMount.class); if (lidarSensors.size() == 0) { System.err.println("DRCLidar: No LIDAR units found on SDF Robot."); return null; } for (LidarMount lidarMount : lidarSensors) { if (lidarMount.getName().equals(sensorName)) { return lidarMount; } } System.err.println("DRCLidar: Could Not find " + sensorName + "Using " + lidarSensors.get(0).getName() + "Instead! FIX THIS!"); return lidarSensors.get(0); }
private static LidarMount getSensor(FloatingRootJointRobot robot, String sensorName) { ArrayList<LidarMount> lidarSensors = robot.getSensors(LidarMount.class); if(lidarSensors.size() == 0) { System.err.println("DRCLidar: No LIDAR units found on SDF Robot."); return null; } for(LidarMount lidarMount : lidarSensors) { if(lidarMount.getName().equals(sensorName)) { return lidarMount; } } System.err.println("DRCLidar: Could Not find " + sensorName + "Using " + lidarSensors.get(0).getName() + "Instead! FIX THIS!"); return lidarSensors.get(0); }