From 73d2a91745df9d9b817d98e8f8586d2b5d956ccc Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Thu, 15 Oct 2020 08:25:37 +0200
Subject: [PATCH] Add output
---
.../RobotUnitModules/RobotUnitModuleDevices.cpp | 5 ++++-
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 11 ++++++++++-
2 files changed, 14 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 6d5e0bdb0..057ca4b17 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -660,7 +660,10 @@ namespace armarx::RobotUnitModule
}
else
{
- ARMARX_WARNING << "SensorValue for SensorDevice " << name << " has no position data";
+ ARMARX_WARNING << "SensorValue for SensorDevice "
+ << name << " is of type "
+ << dev->getSensorValueType()
+ << " which does not derive SensorValue1DoFActuatorPosition";
}
}
else
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 6ade7b907..d6a15aa88 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -199,15 +199,19 @@ namespace armarx::RobotUnitModule
using UnitT = KinematicSubUnit;
//add ctrl et al
+ ARMARX_DEBUG << "createKinematicSubUnit...";
std::map<std::string, UnitT::ActuatorData> devs;
for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
{
- ARMARX_CHECK_EXPRESSION(controlDevice);
+ ARMARX_CHECK_NOT_NULL(controlDevice);
const auto& controlDeviceName = controlDevice->getDeviceName();
+ ARMARX_DEBUG << "looking at device " << controlDeviceName
+ << controlDevice->getJointControllerToTargetTypeNameMap();
if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
{
UnitT::ActuatorData ad;
ad.name = controlDeviceName;
+ ARMARX_DEBUG << "has sensor device: " << _module<Devices>().getSensorDevices().has(controlDeviceName);
ad.sensorDeviceIndex =
_module<Devices>().getSensorDevices().has(controlDeviceName) ?
_module<Devices>().getSensorDevices().index(controlDeviceName) :
@@ -240,6 +244,10 @@ namespace armarx::RobotUnitModule
if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
{
+ ARMARX_DEBUG << "created controllers for "
+ << controlDeviceName
+ << " pos/tor/vel " << ad.ctrlPos.get() << " / "
+ << ad.ctrlTor.get() << " / " << ad.ctrlVel.get();
devs[controlDeviceName] = std::move(ad);
}
}
@@ -249,6 +257,7 @@ namespace armarx::RobotUnitModule
ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
return nullptr;
}
+ ARMARX_IMPORTANT << "Found " << devs.size() << " joint devices - adding KinematicUnit";
ARMARX_CHECK_EXPRESSION(!devs.empty()) << "no 1DoF ControlDevice found with matching SensorDevice";
//add it
const std::string configName = getProperty<std::string>("KinematicUnitName");
--
GitLab