protected void setupSubscribers(ConnectedNode connectedNode) {
DRCRobotSensorInformation sensorInforamtion = robotModel.getSensorInformation();
DRCRobotLidarParameters lidarParameters = sensorInforamtion.getLidarParameters(0);
if (lidarParameters != null && lidarParameters.getLidarSpindleJointTopic() != null) {
jointSubscriber =
connectedNode.newSubscriber(
lidarParameters.getLidarSpindleJointTopic(), sensor_msgs.JointState._TYPE);
}
}
@Override
public void connect() throws IOException {
sensorSuitePacketCommunicator.connect();
if (sensorInformation.getLidarParameters().length > 0) {
pointCloudDataReceiver.start();
}
}
@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);
}
@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();
}