コード例 #1
0
  @Override
  public void initializeSimulatedSensors(ObjectCommunicator scsSensorsCommunicator) {
    sensorSuitePacketCommunicator.attachListener(
        RobotConfigurationData.class, robotConfigurationDataBuffer);

    SCSCameraDataReceiver cameraDataReceiver =
        new SCSCameraDataReceiver(
            sensorInformation.getCameraParameters(0).getRobotSide(),
            modelFactory,
            sensorInformation.getCameraParameters(0).getSensorNameInSdf(),
            robotConfigurationDataBuffer,
            scsSensorsCommunicator,
            sensorSuitePacketCommunicator,
            ppsTimestampOffsetProvider);
    cameraDataReceiver.start();

    //      if (sensorInformation.getPointCloudParameters().length > 0)
    //      {
    //         new SCSPointCloudDataReceiver(depthDataProcessor, robotPoseBuffer, scsCommunicator);
    //      }

    if (sensorInformation.getLidarParameters().length > 0) {
      new SCSPointCloudLidarReceiver(
          sensorInformation.getLidarParameters(0).getSensorNameInSdf(),
          scsSensorsCommunicator,
          pointCloudDataReceiver);
    }

    //      if (DRCConfigParameters.CALIBRATE_ARM_MODE)
    //      {
    //         ArmCalibrationHelper armCalibrationHelper = new
    // ArmCalibrationHelper(sensorSuitePacketCommunicator, jointMap);
    //         cameraReceiver.registerCameraListener(armCalibrationHelper);
    //      }

    //      IMUBasedHeadPoseCalculatorFactory.create(sensorSuitePacketCommunicator,
    // sensorInformation);

    VisionPoseEstimator visionPoseEstimator =
        new VisionPoseEstimator(
            sensorSuitePacketCommunicator,
            pointCloudDataReceiver,
            modelFactory,
            robotConfigurationDataBuffer,
            false);
    cameraDataReceiver.registerCameraListener(visionPoseEstimator);
  }
コード例 #2
0
  @Override
  public void initializePhysicalSensors(URI rosCoreURI) {
    if (rosCoreURI == null)
      throw new RuntimeException(
          getClass().getSimpleName()
              + " Physical sensor requires rosURI to be set in "
              + NetworkParameters.defaultParameterFile);

    sensorSuitePacketCommunicator.attachListener(
        RobotConfigurationData.class, robotConfigurationDataBuffer);

    ForceSensorNoiseEstimator forceSensorNoiseEstimator =
        new ForceSensorNoiseEstimator(sensorSuitePacketCommunicator);
    sensorSuitePacketCommunicator.attachListener(
        RobotConfigurationData.class, forceSensorNoiseEstimator);

    RosMainNode rosMainNode = new RosMainNode(rosCoreURI, "atlas/sensorSuiteManager", true);

    DRCRobotCameraParameters multisenseLeftEyeCameraParameters =
        sensorInformation.getCameraParameters(AtlasSensorInformation.MULTISENSE_SL_LEFT_CAMERA_ID);
    DRCRobotLidarParameters multisenseLidarParameters =
        sensorInformation.getLidarParameters(AtlasSensorInformation.MULTISENSE_LIDAR_ID);
    DRCRobotPointCloudParameters multisenseStereoParameters =
        sensorInformation.getPointCloudParameters(AtlasSensorInformation.MULTISENSE_STEREO_ID);

    MultiSenseSensorManager multiSenseSensorManager =
        new MultiSenseSensorManager(
            modelFactory,
            pointCloudDataReceiver,
            robotConfigurationDataBuffer,
            rosMainNode,
            sensorSuitePacketCommunicator,
            ppsTimestampOffsetProvider,
            rosCoreURI,
            multisenseLeftEyeCameraParameters,
            multisenseLidarParameters,
            multisenseStereoParameters,
            sensorInformation.setupROSParameterSetters());

    DRCRobotCameraParameters leftFishEyeCameraParameters =
        sensorInformation.getCameraParameters(AtlasSensorInformation.BLACKFLY_LEFT_CAMERA_ID);
    DRCRobotCameraParameters rightFishEyeCameraParameters =
        sensorInformation.getCameraParameters(AtlasSensorInformation.BLACKFLY_RIGHT_CAMERA_ID);

    FisheyeCameraReceiver leftFishEyeCameraReceiver =
        new FisheyeCameraReceiver(
            modelFactory,
            leftFishEyeCameraParameters,
            robotConfigurationDataBuffer,
            sensorSuitePacketCommunicator,
            ppsTimestampOffsetProvider,
            rosMainNode);
    FisheyeCameraReceiver rightFishEyeCameraReceiver =
        new FisheyeCameraReceiver(
            modelFactory,
            rightFishEyeCameraParameters,
            robotConfigurationDataBuffer,
            sensorSuitePacketCommunicator,
            ppsTimestampOffsetProvider,
            rosMainNode);

    leftFishEyeCameraReceiver.start();
    rightFishEyeCameraReceiver.start();

    VisionPoseEstimator visionPoseEstimator =
        new VisionPoseEstimator(
            sensorSuitePacketCommunicator,
            pointCloudDataReceiver,
            modelFactory,
            robotConfigurationDataBuffer,
            true);
    multiSenseSensorManager.registerCameraListener(visionPoseEstimator);

    blackFlyParameterSetters = new SideDependentList<BlackFlyParameterSetter>();
    for (RobotSide side : RobotSide.values)
      blackFlyParameterSetters.put(
          side,
          new BlackFlyParameterSetter(
              rosMainNode,
              side,
              "/" + side.getLowerCaseName() + "/camera/camera_nodelet",
              sensorSuitePacketCommunicator));

    ppsTimestampOffsetProvider.attachToRosMainNode(rosMainNode);

    //      if (DRCConfigParameters.CALIBRATE_ARM_MODE)
    //      {
    //         ArmCalibrationHelper armCalibrationHelper = new
    // ArmCalibrationHelper(sensorSuitePacketCommunicator, jointMap);
    //         multiSenseSensorManager.registerCameraListener(armCalibrationHelper);
    //      }

    multiSenseSensorManager.initializeParameterListeners();

    //      IMUBasedHeadPoseCalculatorFactory.create(sensorSuitePacketCommunicator,
    // sensorInformation, rosMainNode);
    rosMainNode.execute();
  }