diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
index 0a11b09754f6a47f7b52e3fe0958f66aeeb034bd..2bb041a7b577bf9a9443dcb4f405949e840a636e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -121,10 +121,14 @@ namespace armarx
const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
-
- if (sensorGlobalPositionCorrection == nullptr or sensorRelativePosition == nullptr)
+ if (sensorGlobalPositionCorrection == nullptr)
+ {
+ ARMARX_ERROR << "The global position correction sensor is not available.";
+ return;
+ }
+ if (sensorRelativePosition == nullptr)
{
- ARMARX_ERROR << "one of the sensors is not available";
+ ARMARX_ERROR << "The relative position sensor is not available.";
return;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index a7083b662e54b0a0c02ad847c3e0f197bce74d58..cc1e89e592642c8adbd137c071023a472284e4ec 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -33,6 +33,44 @@
namespace armarx::RobotUnitModule
{
+
+ RobotDataPropertyDefinitions::RobotDataPropertyDefinitions(std::string prefix) :
+ ModuleBasePropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("RobotFileName",
+ "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>("RobotFileNameProject",
+ "",
+ "Project in which the robot filename is located "
+ "(if robot is loaded from an external project)");
+
+ defineOptionalProperty<std::string>(
+ "RobotName",
+ "",
+ "Override robot name if you want to load multiple robots of the same type");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetName",
+ "Robot",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineOptionalProperty<std::string>(
+ "PlatformName",
+ "Platform",
+ "Name of the platform needs to correspond to a node in the virtual robot.");
+ defineOptionalProperty<bool>("PlatformAndLocalizationUnitsEnabled",
+ true,
+ "Enable or disable the platform and localization units.");
+ defineOptionalProperty<std::string>("PlatformInstanceName",
+ "Platform",
+ "Name of the platform instance (will publish "
+ "values on PlatformInstanceName + 'State')");
+ }
+
+ bool
+ RobotData::arePlatformAndLocalizationUnitsEnabled() const
+ {
+ return _arePlatformAndLocalizationUnitsEnabled;
+ }
+
const std::string&
RobotData::getRobotPlatformName() const
{
@@ -112,6 +150,8 @@ namespace armarx::RobotUnitModule
robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
robotFileName = getProperty<std::string>("RobotFileName").getValue();
robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ _arePlatformAndLocalizationUnitsEnabled =
+ getProperty<bool>("PlatformAndLocalizationUnitsEnabled").getValue();
robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
@@ -160,4 +200,5 @@ namespace armarx::RobotUnitModule
robotPool.reset(new RobotPool(robot, 10));
}
}
+
} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index 7b5defcbfbec169a83e7185f688a5016851c24ab..c6e94bd515bea5a6f88597c935eb277c234a33d2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -40,32 +40,7 @@ namespace armarx::RobotUnitModule
class RobotDataPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- RobotDataPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
- {
- defineRequiredProperty<std::string>("RobotFileName",
- "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>("RobotFileNameProject",
- "",
- "Project in which the robot filename is located "
- "(if robot is loaded from an external project)");
-
- defineOptionalProperty<std::string>(
- "RobotName",
- "",
- "Override robot name if you want to load multiple robots of the same type");
- defineOptionalProperty<std::string>(
- "RobotNodeSetName",
- "Robot",
- "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineOptionalProperty<std::string>(
- "PlatformName",
- "Platform",
- "Name of the platform needs to correspond to a node in the virtual robot.");
- defineOptionalProperty<std::string>("PlatformInstanceName",
- "Platform",
- "Name of the platform instance (will publish "
- "values on PlatformInstanceName + 'State')");
- }
+ RobotDataPropertyDefinitions(std::string prefix);
};
/**
@@ -99,21 +74,26 @@ namespace armarx::RobotUnitModule
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
+ bool arePlatformAndLocalizationUnitsEnabled() const;
+
/**
* @brief Returns the name of the robot's platform
* @return The name of the robot's platform
*/
const std::string& getRobotPlatformName() const;
+
/**
* @brief Returns the name of the robot's RobotNodeSet
* @return The name of the robot's RobotNodeSet
*/
const std::string& getRobotNodetSeName() const;
+
/**
* @brief Returns the name of the project containing the robot's model
* @return The name of the project containing the robot's model
*/
const std::string& getRobotProjectName() const;
+
/**
* @brief Returns the file name of the robot's model
* @return The file name of the robot's model
@@ -150,6 +130,8 @@ namespace armarx::RobotUnitModule
std::string robotFileName;
/// @brief The name of the robot's platform
std::string robotPlatformName;
+ /// @brief Indicates whether the robot platform unit is enabled.
+ bool _arePlatformAndLocalizationUnitsEnabled = false;
/// @brief The name of the robot's platform instance
std::string robotPlatformInstanceName;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index ad3cc0f97680eabc2519b58598823294c770733f..bd92c93f31a9007c338b9ae4bcd52d9ebe1c4d50 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -163,29 +163,54 @@ namespace armarx::RobotUnitModule
void
Units::initializeDefaultUnits()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto beg = TimeUtil::GetTime(true);
{
+ ARMARX_TRACE;
auto guard = getGuard();
+
+ ARMARX_TRACE;
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
ARMARX_INFO << "initializing default units";
+
+ ARMARX_TRACE;
initializeKinematicUnit();
ARMARX_DEBUG << "KinematicUnit initialized";
- ARMARX_DEBUG << "initializing LocalizationUnit";
- initializeLocalizationUnit();
- ARMARX_DEBUG << "LocalizationUnit initialized";
+ ARMARX_TRACE;
+ if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+ {
+ ARMARX_DEBUG << "initializing LocalizationUnit";
+ initializeLocalizationUnit();
+ ARMARX_DEBUG << "LocalizationUnit initialized";
+ }
+ ARMARX_TRACE;
+
+ if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+ {
+ ARMARX_DEBUG << "initializing PlatformUnit";
+ initializePlatformUnit();
+ ARMARX_DEBUG << "PlatformUnit initialized";
+ }
- initializePlatformUnit();
- ARMARX_DEBUG << "PlatformUnit initialized";
+ ARMARX_TRACE;
initializeForceTorqueUnit();
ARMARX_DEBUG << "ForceTorqueUnit initialized";
+
+ ARMARX_TRACE;
initializeInertialMeasurementUnit();
ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+
+ ARMARX_TRACE;
initializeTrajectoryControllerUnit();
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+
+ ARMARX_TRACE;
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
+
+ ARMARX_TRACE;
}
ARMARX_INFO << "initializing default units...done! "
<< (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
@@ -371,6 +396,8 @@ namespace armarx::RobotUnitModule
using UnitT = PlatformSubUnit;
using IfaceT = PlatformUnitInterface;
+ ARMARX_TRACE;
+
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -378,12 +405,14 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
//is there a platform dev?
if (_module<RobotData>().getRobotPlatformName().empty())
{
ARMARX_INFO << "no platform unit created since no platform name was given";
return;
}
+ ARMARX_TRACE;
if (!_module<Devices>().getControlDevices().has(
_module<RobotData>().getRobotPlatformName()))
{
@@ -392,16 +421,19 @@ namespace armarx::RobotUnitModule
<< _module<RobotData>().getRobotPlatformName() << "' was not found";
return;
}
+ ARMARX_TRACE;
const ControlDevicePtr& controlDevice =
_module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
const SensorDevicePtr& sensorDevice =
_module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
JointController* jointVel =
controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(jointVel);
ARMARX_CHECK_EXPRESSION(
sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
//add it
+ ARMARX_TRACE;
const std::string configName = getProperty<std::string>("PlatformUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
@@ -411,9 +443,11 @@ namespace armarx::RobotUnitModule
_module<RobotData>().getRobotPlatformInstanceName());
ARMARX_DEBUG << "creating unit " << configName
<< " using these properties: " << properties->getPropertiesForPrefix("");
+ ARMARX_TRACE;
IceInternal::Handle<UnitT> unit =
Component::create<UnitT>(properties, configName, getConfigDomain());
//config
+ ARMARX_TRACE;
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
config->initialVelocityX = 0;
@@ -427,8 +461,10 @@ namespace armarx::RobotUnitModule
config,
false,
true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
+ ARMARX_TRACE;
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
new NJointHolonomicPlatformRelativePositionControllerConfig;
@@ -457,9 +493,11 @@ namespace armarx::RobotUnitModule
configGlobalPositionCtrlCfg,
false,
true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
+ ARMARX_TRACE;
unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
_module<RobotData>().getRobotPlatformName());
@@ -470,12 +508,14 @@ namespace armarx::RobotUnitModule
void
Units::initializeLocalizationUnit()
{
+ ARMARX_TRACE;
ARMARX_DEBUG << "initializeLocalizationUnit";
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = LocalizationSubUnit;
using IfaceT = LocalizationUnitInterface;
+ ARMARX_TRACE;
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -483,13 +523,18 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
+ ARMARX_CHECK(
+ _module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
+ << _module<RobotData>().getRobotPlatformName();
const SensorDevicePtr& sensorDeviceRelativePosition =
_module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
->asA<SensorValueHolonomicPlatformRelativePosition>());
+ ARMARX_TRACE;
// const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
// ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index e1c717c58e00be94df3035916b5d3d4f0a50a589..bedfc430f0dee9f2030ac495d2ba6dd74bfa1350 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -82,8 +82,8 @@ namespace armarx
ControlThreadOutputBuffer::forEachNewLoggingEntry(ConsumerFunctor consumer)
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(isInitialized);
- ARMARX_VERBOSE << VAROUT(entries.size());
+ ARMARX_CHECK(isInitialized);
+ ARMARX_DEBUG << VAROUT(entries.size());
const size_t writePosition_local = writePosition.load(); // copy to prevent external changes
if (writePosition_local - onePastLoggingReadPosition >= numEntries)
{
@@ -107,7 +107,7 @@ namespace armarx
}
//consume all
const std::size_t num = writePosition_local - onePastLoggingReadPosition;
- ARMARX_VERBOSE << num << " new entries to be treated";
+ ARMARX_DEBUG << num << " new entries to be treated";
for (std::size_t offset = 0; onePastLoggingReadPosition < writePosition_local;
++onePastLoggingReadPosition, ++offset)
{
@@ -276,10 +276,8 @@ namespace armarx
}
}
- const auto PotentiallyMinimizeMember = [](auto & member, bool minimize) {
- return minimize ? 0 : member;
- };
-
+ const auto PotentiallyMinimizeMember = [](auto& member, bool minimize)
+ { return minimize ? 0 : member; };
detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other,
bool minimize) :
@@ -301,13 +299,13 @@ namespace armarx
maxAlign{1}
{
ARMARX_DEBUG << "copying RtMessageLogBuffer with minimize = " << minimize << " "
- << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
- << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
- << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
- << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
- << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace) << " "
- << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost) << " "
- << VAROUT(maxAlign);
+ << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
+ << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
+ << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
+ << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
+ << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace)
+ << " " << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost)
+ << " " << VAROUT(maxAlign);
for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
{
const RtMessageLogEntryBase* entry = other.entries.at(idx);
@@ -361,10 +359,12 @@ namespace armarx
deleteAll();
if (requiredAdditionalEntries || entries.size() < numEntries)
{
- const auto numExcessEntries = std::max(requiredAdditionalEntries, numEntries - entries.size());
+ const auto numExcessEntries =
+ std::max(requiredAdditionalEntries, numEntries - entries.size());
const auto requiredSize = entries.size() + numExcessEntries;
ARMARX_WARNING << "Iteration " << iterationCount << " required "
- << requiredAdditionalEntries << " | " << numExcessEntries << " additional message entries. \n"
+ << requiredAdditionalEntries << " | " << numExcessEntries
+ << " additional message entries. \n"
<< "The requested total number of entries is " << requiredSize << ". \n"
<< "The current number of entries is " << entries.size() << ". \n"
<< "The maximal number of entries is "
@@ -514,10 +514,10 @@ namespace armarx
{
ARMARX_TRACE;
ARMARX_DEBUG << "Copy ControlThreadOutputBufferEntry with parameters: " << VAROUT(minimize)
- << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
- << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
- << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
- << VAROUT(messages.entries.size());
+ << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
+ << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
+ << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
+ << VAROUT(messages.entries.size());
void* place = buffer.data();
std::size_t space = buffer.size();
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index b247f12167424f45a2e87c690461d5aaab5516d0..129771f55fec581f806de5e09d8c7a7d928f2ac5 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -114,7 +114,7 @@ namespace armarx::introspection
const std::string& name)
{
ARMARX_TRACE;
- ARMARX_CHECK_EQUAL(0, entries.count(name));
+ ARMARX_CHECK_EQUAL(entries.count(name), 0) << name;
entries.add(name, Entry(name, ptr));
return entries.at(name);
}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
index be011068dffcabda87b34bc3da51d6e8b432a2e1..27ab8bff523e82ccd52f02bede86a33b51e79225 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
@@ -67,6 +67,7 @@ namespace armarx::aron::framed
{
dto.header.frame = bo.frame;
dto.header.agent = bo.agent;
+ dto.pose = Eigen::Matrix4f::Identity();
Eigen::Vector3f vec;
vec.x() = bo.position->x;
vec.y() = bo.position->y;