public static void main(String[] arg) throws URISyntaxException {
RosMainNode mainNode = new RosMainNode(new URI("http://localhost:11311"), "testImu");
mainNode.attachSubscriber(
"/multisense/imu/imu_data",
new RosImuSubscriber() {
@Override
protected void onNewMessage(
long timeStamp,
int seqId,
Quat4d orientation,
Vector3d angularVelocity,
Vector3d linearAcceleration) {
System.out.println("Gravity:" + linearAcceleration);
}
});
mainNode.execute();
}
public MultisenseHeadingSubscriber(
RosMainNode rosnode, String msgTopic, YoVariableRegistry inputRegistry) {
super();
registry = inputRegistry;
angleRadians = 0.0;
magnitudeMeters = 0.0;
DoubleYoVariable multisenseHeading = new DoubleYoVariable("multisenseHeading", registry);
DoubleYoVariable multisenseMagnitude = new DoubleYoVariable("multisenseMagnitude", registry);
multisenseHeading.setValueFromDouble(0.0);
multisenseMagnitude.setValueFromDouble(0.0);
rosnode.attachSubscriber(msgTopic, this);
}
@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();
}
