Skip to content
Snippets Groups Projects
Commit 996cb971 authored by Raphael's avatar Raphael
Browse files

apply changes due to changes in ArmarXCore

parent 639591fc
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
+ 1
− 1
View file @ 996cb971
......@@ -219,7 +219,7 @@ namespace armarx
}
bool rtUpdateControlStruct()
{
return controlDataTripleBuffer.getUpToDateData();
return controlDataTripleBuffer.updateReadBuffer();
}
virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
{
......
source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
+ 55
− 55
View file @ 996cb971
......@@ -41,7 +41,7 @@ void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::
throw std::invalid_argument {"A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!"};
}
lvl0Controllers[jointName][controlMode] = &lvl0Controller;
if(controlMode == ControlModes::EmergencyStopMode)
if (controlMode == ControlModes::EmergencyStopMode)
{
lvl0ControllersEmergencyStop.at(jointNameIndices.at(jointName)) = &lvl0Controller;
}
......@@ -59,12 +59,12 @@ armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::stri
return *lvl0Controllers.at(jointName).at(controlMode);
}
armarx::RobotUnit::RobotUnit(const std::vector<std::string> &jointNames):
jointNames{jointNames},
jointNameIndices{toIndexMap(jointNames)},
controllersRequested{jointNames.size()},
controllersActivated{jointNames.size()},
lvl0ControllersEmergencyStop{jointNames.size()}
armarx::RobotUnit::RobotUnit(const std::vector<std::string>& jointNames):
jointNames {jointNames},
jointNameIndices {toIndexMap(jointNames)},
controllersRequested {jointNames.size()},
controllersActivated {jointNames.size()},
lvl0ControllersEmergencyStop {jointNames.size()}
{}
Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
......@@ -77,7 +77,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesLoaded(const Ice::Current&)
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
for (const auto & lvl1 : lvl1Controllers)
for (const auto& lvl1 : lvl1Controllers)
{
result.emplace_back(lvl1.first);
}
......@@ -88,9 +88,9 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
for (const auto & lvl1 : getRequestedLVL1Controllers())
for (const auto& lvl1 : getRequestedLVL1Controllers())
{
if(lvl1)
if (lvl1)
{
result.emplace_back(lvl1->getName());
}
......@@ -102,7 +102,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
for (const auto & lvl1 : getActivatedLVL1Controllers())
for (const auto& lvl1 : getActivatedLVL1Controllers())
{
result.emplace_back(lvl1->getName());
}
......@@ -113,7 +113,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersLoade
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
for (const auto & lvl1 : lvl1Controllers)
for (const auto& lvl1 : lvl1Controllers)
{
result[lvl1.first] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1.second->getProxy());
}
......@@ -123,9 +123,9 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersReque
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
for (const auto & lvl1 : getRequestedLVL1Controllers())
for (const auto& lvl1 : getRequestedLVL1Controllers())
{
if(lvl1)
if (lvl1)
{
result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
}
......@@ -136,7 +136,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActiv
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
for (const auto & lvl1 : getActivatedLVL1Controllers())
for (const auto& lvl1 : getActivatedLVL1Controllers())
{
result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
}
......@@ -147,9 +147,9 @@ armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignmen
{
GuardType guard {dataMutex};
JointNameToLVL1Dictionary result;
for (const auto & lvl1 : getActivatedLVL1Controllers())
for (const auto& lvl1 : getActivatedLVL1Controllers())
{
for (const auto & jointMode : lvl1->jointControlModeMap)
for (const auto& jointMode : lvl1->jointControlModeMap)
{
result[jointMode.first] = lvl1->getName();
}
......@@ -185,11 +185,11 @@ armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlMode
{
GuardType guard {dataMutex};
JointNameToControlModesDictionary result;
for (const auto & joint : lvl0Controllers)
for (const auto& joint : lvl0Controllers)
{
std::vector<std::string> modes;
modes.reserve(joint.second.size());
for (const auto & mode : joint.second)
for (const auto& mode : joint.second)
{
modes.emplace_back(mode.first);
}
......@@ -202,7 +202,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
{
GuardType guard {dataMutex};
//check if all of these controllers exist
for (const auto & lvl1 : controllerRequestedNames)
for (const auto& lvl1 : controllerRequestedNames)
{
if (!hasLVL1Controller(lvl1))
{
......@@ -218,7 +218,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
std::set<std::string> controllersToActivate {currentActiveLVL1Controllers.begin(), currentActiveLVL1Controllers.end()}; //these controllers will be set as active controllers
for (const auto & toActivate : controllerRequestedNames)
for (const auto& toActivate : controllerRequestedNames)
{
if (controllersToActivate.count(toActivate))
{
......@@ -229,7 +229,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
ARMARX_INFO << "activate '" << toActivate << "'";
const auto& lvl1 = lvl1Controllers.at(toActivate);
//check and update the assignement map
for (const auto & jointControlMode : lvl1->jointControlModeMap)
for (const auto& jointControlMode : lvl1->jointControlModeMap)
{
const auto& joint = jointControlMode.first;
const auto& currentAssignedLVL1 = lvl1ControllerAssignement.at(joint);
......@@ -243,7 +243,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
continue;
}
//deactivate other controller
for (auto & assignement : lvl1ControllerAssignement)
for (auto& assignement : lvl1ControllerAssignement)
{
if (assignement.second == currentAssignedLVL1)
{
......@@ -256,7 +256,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
}
}
//verify (exception for collision of requested lvl1)
for (const auto & toActivate : controllerRequestedNames)
for (const auto& toActivate : controllerRequestedNames)
{
if (!controllersToActivate.count(toActivate))
{
......@@ -282,7 +282,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
//populate controllersRequested.lvl1Controllers
getRequestedLVL1Controllers().clear();
getRequestedLVL1Controllers().reserve(controllersToActivate.size());
for (const auto & lvl1 : controllersToActivate)
for (const auto& lvl1 : controllersToActivate)
{
getRequestedLVL1Controllers().emplace_back(lvl1Controllers.at(lvl1));
}
......@@ -324,7 +324,7 @@ armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::
lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
lvl1->jointIndices.clear();
lvl1->jointIndices.reserve(jointsUsedByLVL1Controler.size());
for(const auto& j: jointsUsedByLVL1Controler)
for (const auto& j : jointsUsedByLVL1Controler)
{
lvl1->jointIndices.emplace_back(jointNameIndices.at(j.first));
}
......@@ -454,65 +454,65 @@ bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std
bool armarx::RobotUnit::rtControllersWereSwitched() const
{
return controllersRequested.getUpToDateData();
return controllersRequested.updateReadBuffer();
}
std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::getRequestedLVL0Controllers()
std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getRequestedLVL0Controllers()
{
return controllersRequested.getWriteBuffer().lvl0Controllers;
}
std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::getRequestedLVL1Controllers()
std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getRequestedLVL1Controllers()
{
return controllersRequested.getWriteBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::getRequestedLVL0Controllers() const
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getRequestedLVL0Controllers() const
{
return controllersRequested.getWriteBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::getRequestedLVL1Controllers() const
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getRequestedLVL1Controllers() const
{
return controllersRequested.getWriteBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::getActivatedLVL0Controllers() const
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getActivatedLVL0Controllers() const
{
return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::getActivatedLVL1Controllers() const
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getActivatedLVL1Controllers() const
{
return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::rtGetRequestedLVL0Controllers() const
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetRequestedLVL0Controllers() const
{
return controllersRequested.getReadBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::rtGetRequestedLVL1Controllers() const
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetRequestedLVL1Controllers() const
{
return controllersRequested.getReadBuffer().lvl1Controllers;
}
std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::rtGetActivatedLVL0Controllers()
std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetActivatedLVL0Controllers()
{
return controllersActivated.getWriteBuffer().lvl0Controllers;
}
std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::rtGetActivatedLVL1Controllers()
std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetActivatedLVL1Controllers()
{
return controllersActivated.getWriteBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::rtGetActivatedLVL0Controllers() const
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetActivatedLVL0Controllers() const
{
return controllersActivated.getWriteBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::rtGetActivatedLVL1Controllers() const
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetActivatedLVL1Controllers() const
{
return controllersActivated.getWriteBuffer().lvl1Controllers;
}
......@@ -524,20 +524,20 @@ void armarx::RobotUnit::rtCommitActivatedLVL1Controllers()
bool armarx::RobotUnit::rtSwitchSetup()
{
if(!rtControllersWereSwitched())
if (!rtControllersWereSwitched())
{
return false;
}
//handle lvl1
for(std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
//deactivate old
if(rtGetActivatedLVL1Controllers().at(i))
if (rtGetActivatedLVL1Controllers().at(i))
{
rtGetActivatedLVL1Controllers().at(i)->rtDeactivateController();
}
//activate new
if(rtGetRequestedLVL1Controllers().at(i))
if (rtGetRequestedLVL1Controllers().at(i))
{
rtGetRequestedLVL1Controllers().at(i)->rtActivateController();
}
......@@ -545,9 +545,9 @@ bool armarx::RobotUnit::rtSwitchSetup()
rtGetActivatedLVL1Controllers().at(i) = rtGetRequestedLVL1Controllers().at(i);
}
//handle lvl0
for(std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
for (std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
{
if(rtGetRequestedLVL0Controllers().at(i) != rtGetActivatedLVL0Controllers().at(i))
if (rtGetRequestedLVL0Controllers().at(i) != rtGetActivatedLVL0Controllers().at(i))
{
rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controllers().at(i), rtGetRequestedLVL0Controllers().at(i));
rtGetActivatedLVL0Controllers().at(i) = rtGetRequestedLVL0Controllers().at(i);
......@@ -556,11 +556,11 @@ bool armarx::RobotUnit::rtSwitchSetup()
return true;
}
void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
for(LVL1ControllerPtr& lvl1: rtGetActivatedLVL1Controllers())
for (LVL1ControllerPtr& lvl1 : rtGetActivatedLVL1Controllers())
{
if(!lvl1)
if (!lvl1)
{
continue;
}
......@@ -570,16 +570,16 @@ void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time &sensorValuesTi
void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
{
for(std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
if(!rtGetRequestedLVL1Controllers().at(i))
if (!rtGetRequestedLVL1Controllers().at(i))
{
continue;
}
if(rtGetRequestedLVL1Controllers().at(i)->rtUsesJoint(index))
if (rtGetRequestedLVL1Controllers().at(i)->rtUsesJoint(index))
{
rtGetRequestedLVL1Controllers().at(i)->rtDeactivateController();
for(auto used:rtGetRequestedLVL1Controllers().at(i)->rtGetJointIndices())
for (auto used : rtGetRequestedLVL1Controllers().at(i)->rtGetJointIndices())
{
rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controllers().at(i), lvl0ControllersEmergencyStop.at(i));
rtGetActivatedLVL0Controllers().at(i) = lvl0ControllersEmergencyStop.at(i);
......@@ -592,7 +592,7 @@ void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
void armarx::RobotUnit::rtRunLVL0Controllers()
{
for(LVL0ControllerBase* lvl0: rtGetActivatedLVL0Controllers())
for (LVL0ControllerBase* lvl0 : rtGetActivatedLVL0Controllers())
{
lvl0->run();
}
......@@ -600,9 +600,9 @@ void armarx::RobotUnit::rtRunLVL0Controllers()
bool armarx::RobotUnit::rtValidateLVL0ControllerSetup() const
{
for(const auto& lvl0: lvl0ControllersEmergencyStop)
for (const auto& lvl0 : lvl0ControllersEmergencyStop)
{
if(!lvl0)
if (!lvl0)
{
return false;
}
......
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