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>());