From e1fd5c9c2b0794d6960d5e449f58300a88986593 Mon Sep 17 00:00:00 2001
From: ARMAR-User <armar@user>
Date: Wed, 5 Mar 2025 11:54:48 +0100
Subject: [PATCH] feat: Allow disabling global pose correction and localization
sensors for FR3 and Panda
---
.../RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp | 7 +++++--
.../RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h | 4 ++++
.../RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp | 8 ++++++++
3 files changed, 17 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 61d0d6c3a..834203f53 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -513,8 +513,11 @@ 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>());
+ if (getProperty<bool>("GlobalPoseCorrectionAndLocalizationSensorEnabled").getValue())
+ {
+ addSensorDevice(std::make_shared<GlobalRobotPoseCorrectionSensorDevice>());
+ addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>());
+ }
}
void
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 28f698623..6e825e6dc 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -59,6 +59,10 @@ namespace armarx::RobotUnitModule
"Syntax: semicolon separated groups , each group is CSV of joints "
"e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two "
"groups!");
+ defineOptionalProperty<bool>("GlobalPoseCorrectionAndLocalizationSensorEnabled",
+ true,
+ "Whether to enable or disable the global pose correction "
+ "and localization sensors.");
}
};
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 09f522529..b37de4a71 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -641,6 +641,14 @@ namespace armarx::RobotUnitModule
}
ARMARX_TRACE;
+ if (_module<Devices>().getSensorDevices().count(
+ _module<RobotData>().getRobotPlatformName()) == 0)
+ {
+ ARMARX_WARNING << "No sensor devices found for robot platform `"
+ << _module<RobotData>().getRobotPlatformName() << "`.";
+ return;
+ }
+
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
ARMARX_CHECK(
_module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
--
GitLab