From 55d6cd838c3f33f288344d563d69956695a9ea11 Mon Sep 17 00:00:00 2001
From: Christian Dreher <c.dreher@kit.edu>
Date: Thu, 11 May 2023 20:52:50 +0200
Subject: [PATCH] fix: Hotfixes required to run panda
---
.../RobotUnitModules/RobotUnitModuleControlThread.cpp | 6 ++++++
.../RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp | 4 ++--
.../RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp | 7 ++++++-
3 files changed, 14 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 5d7d7d01e..71d4b206c 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 c29c7ce2d..d5cb2118a 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 80efce08a..8dce4789a 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>());
--
GitLab