Skip to content
Snippets Groups Projects
Commit 0727808d authored by Christian Dreher's avatar Christian Dreher Committed by ARMAR-7a
Browse files

feat: Allow disabling platform unit and localization unit for robot testing on a benchmark

parent 1c51b4f8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+ 41
− 0
View file @ 0727808d
......@@ -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
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+ 8
− 26
View file @ 0727808d
......@@ -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;
......
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+ 50
− 5
View file @ 0727808d
......@@ -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>());
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment