From 996cb9714897a7f64de2e94e1c8799ecef5ee27d Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Wed, 15 Feb 2017 13:24:04 +0100
Subject: [PATCH] apply changes due to changes in ArmarXCore
---
.../RobotRTControllers/LVL1Controller.h | 2 +-
.../RobotRTControllers/RobotUnit.cpp | 110 +++++++++---------
2 files changed, 56 insertions(+), 56 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
index c765dbf48..ba2bdcfc5 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -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
{
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
index 99718eaf2..095e2ef7e 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
@@ -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;
}
--
GitLab