diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp index 5d7d7d01ece91cb274e2f4e6a6f0619c06f4a65e..71d4b206c18eff39e5cbdd57ad5c5181b0841e1d 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp @@ -513,7 +513,13 @@ namespace armarx::RobotUnitModule rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart(); for (const SensorDevicePtr& device : rtGetSensorDevices()) { + if (device) + { device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration); + } + else{ + ARMARX_ERROR << "Device is null!"; + } } DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes); rtPostReadSensorDeviceValues(sensorValuesTimestamp, timeSinceLastIteration); diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp index c29c7ce2d74f3f4ee7f7dc04cc96e48b46315f89..d5cb2118a5dab45ea0154693b872004afc8223e9 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp @@ -469,8 +469,8 @@ namespace armarx::RobotUnitModule // this device will be used by the PlatformUnit to make the robot's global pose // available to e.g. the NJointControllers. - addSensorDevice(std::make_shared<GlobalRobotPoseCorrectionSensorDevice>()); - addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>()); + //addSensorDevice(std::make_shared<GlobalRobotPoseCorrectionSensorDevice>()); + //addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>()); } void Devices::_postFinishDeviceInitialization() diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp index 80efce08aeaefed56bf599ceb928c7e1615e36a0..8dce4789afc04c6492ffa0d43cb2de23f5569060 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp @@ -426,7 +426,7 @@ namespace armarx::RobotUnitModule void Units::initializeLocalizationUnit() { - ARMARX_DEBUG << "initializeLocalizationUnit"; + ARMARX_DEBUG << "initializeLocalizationUnit: " << _module<RobotData>().getRobotPlatformName(); throwIfInControlThread(BOOST_CURRENT_FUNCTION); using UnitT = LocalizationSubUnit; @@ -440,6 +440,11 @@ namespace armarx::RobotUnitModule return; } + if (_module<Devices>().getSensorDevices().count(_module<RobotData>().getRobotPlatformName()) == 0) + { + return; + } + ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition"; const SensorDevicePtr& sensorDeviceRelativePosition = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName()); ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>());