From b31d883aede263fa2cf2b788590115c78745ac15 Mon Sep 17 00:00:00 2001
From: Markus Swarowsky <markus.swarowsky@student.kit.edu>
Date: Thu, 16 Feb 2017 20:01:15 +0100
Subject: [PATCH] first real test of level 1 pass trough controller, done a lot
of bugfixes together with Raphael
---
.../RobotAPI/components/units/KinematicUnit.h | 20 ++---
.../PassThroughController.h | 5 +-
.../DataUnits/KinematicDataUnit.h | 13 ++-
.../RobotRTControllers/RobotUnit.cpp | 80 +++++++++++++------
.../libraries/RobotRTControllers/RobotUnit.h | 4 +-
5 files changed, 82 insertions(+), 40 deletions(-)
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index 4c5cf7ddd..0faef5ace 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -90,41 +90,41 @@ namespace armarx
/**
* \return the robot xml filename as specified in the configuration
*/
- virtual std::string getRobotFilename(const Ice::Current&) const;
+ virtual std::string getRobotFilename(const Ice::Current& = GlobalIceCurrent) const;
/*!
* \brief getArmarXPackages
* \return All dependent packages, which might contain a robot file.
*/
- virtual std::vector< std::string > getArmarXPackages(const Ice::Current&) const;
+ virtual std::vector< std::string > getArmarXPackages(const Ice::Current& = GlobalIceCurrent) const;
/**
*
* \return The name of this robot instance.
*/
- virtual std::string getRobotName(const Ice::Current&) const;
+ virtual std::string getRobotName(const Ice::Current& = GlobalIceCurrent) const;
/**
*
* \return The name of this robot instance.
*/
- virtual std::string getRobotNodeSetName(const Ice::Current&) const;
+ virtual std::string getRobotNodeSetName(const Ice::Current& = GlobalIceCurrent) const;
/**
*
* \return The name of the report topic
*/
- virtual std::string getReportTopicName(const Ice::Current&) const;
+ virtual std::string getReportTopicName(const Ice::Current& = GlobalIceCurrent) const;
virtual void onInitKinematicUnit() = 0;
virtual void onStartKinematicUnit() = 0;
virtual void onExitKinematicUnit() = 0;
// proxy implementation
- virtual void requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = ::Ice::Current());
- virtual void releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = ::Ice::Current());
- virtual void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = ::Ice::Current());
- virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = ::Ice::Current());
- virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = ::Ice::Current());
+ virtual void requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = GlobalIceCurrent);
+ virtual void releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = GlobalIceCurrent);
+ virtual void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = GlobalIceCurrent);
+ virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = GlobalIceCurrent);
+ virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = GlobalIceCurrent);
/**
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
index 38e8ccac5..acb5582b8 100644
--- a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
@@ -156,6 +156,7 @@ namespace armarx
auto kinunit = prov->getRTKinematicDataUnit();
ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
//get pointers to sensor values from units
+ ARMARX_INFO << "PassThroughController for " << cfg->jointNames;
jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
//not initialized, this is done in rtPreActivateController
@@ -163,7 +164,7 @@ namespace armarx
//get pointers for the results of this controller
lvl0Targets.reserve(cfg->jointNames.size());
- for (const auto& j : cfg->jointNames)
+ for (const auto & j : cfg->jointNames)
{
auto target = dynamic_cast<TargetType*>(prov->getJointTarget(j, Traits::getControlMode()));
ARMARX_CHECK_EXPRESSION_W_HINT(target, "The joint " << j << " has no controll mode " << Traits::getControlMode());
@@ -210,7 +211,7 @@ namespace armarx
template <typename TargetType>
void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
{
- for (const auto& value : values)
+ for (const auto & value : values)
{
setJoint(value.first, value.second);
}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
index 8dad96ae9..43c57f9b2 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
@@ -49,7 +49,7 @@ namespace armarx
virtual std::vector<const int32_t*> getJointRawTorqueValues(const std::vector<std::string>& joints) const = 0;
virtual std::vector<const int32_t*> getJointRawGearTemperatures(const std::vector<std::string>& joints) const = 0;
virtual std::vector<const int32_t*> getJointRawAbsPositionValues(const std::vector<std::string>& joints) const = 0;
- virtual std::vector<const int16_t*> getJointRawMotorTemperatureValues(const std::vector<std::string> &joints) const = 0;
+ virtual std::vector<const int16_t*> getJointRawMotorTemperatureValues(const std::vector<std::string>& joints) const = 0;
};
class KinematicDataUnitPtrProvider: virtual public KinematicDataUnitInterface
@@ -63,6 +63,13 @@ namespace armarx
jointMotorTemperatures(jointNames.size(), nullptr),
jointGearTemperatures(jointNames.size(), nullptr),
jointAbsPositions(jointNames.size(), nullptr),
+ jointRawPositionValues(jointNames.size(), nullptr),
+ jointRawVelocityValues(jointNames.size(), nullptr),
+ jointRawCurrentValues(jointNames.size(), nullptr),
+ jointRawTorqueValues(jointNames.size(), nullptr),
+ jointRawGearTemperatures(jointNames.size(), nullptr),
+ jointRawAbsPositionValues(jointNames.size(), nullptr),
+ jointRawMotorTemperatureValues(jointNames.size(), nullptr),
jointNames {jointNames},
jointIndices {toIndexMap(jointNames)}
{}
@@ -130,7 +137,7 @@ namespace armarx
{
return getPointers(joints, jointRawAbsPositionValues);
}
- virtual std::vector<const int16_t*> getJointRawMotorTemperatureValues(const std::vector<std::string> &joints) const
+ virtual std::vector<const int16_t*> getJointRawMotorTemperatureValues(const std::vector<std::string>& joints) const
{
return getPointers(joints, jointRawMotorTemperatureValues);
}
@@ -158,7 +165,7 @@ namespace armarx
std::vector<const T*> getPointers(const std::vector<std::string>& joints, const std::vector<T*>& targets) const
{
std::vector<const T*> result;
- result.resize(joints.size());
+ result.reserve(joints.size());
std::transform(
joints.begin(), joints.end(), std::back_inserter(result),
[targets, this](const std::string & joint)
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
index 97ab98962..4c5115cbd 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
@@ -72,7 +72,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);
}
@@ -83,7 +83,7 @@ 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)
{
@@ -97,8 +97,12 @@ 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())
{
+ if (!lvl1)
+ {
+ continue;
+ }
result.emplace_back(lvl1->getName());
}
return result;
@@ -108,7 +112,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());
}
@@ -118,7 +122,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersReque
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : getRequestedLVL1Controllers())
+ for (const auto & lvl1 : getRequestedLVL1Controllers())
{
if (lvl1)
{
@@ -131,7 +135,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());
}
@@ -142,9 +146,17 @@ armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignmen
{
GuardType guard {dataMutex};
JointNameToLVL1Dictionary result;
- for (const auto& lvl1 : getActivatedLVL1Controllers())
+ for (const auto & joint : jointNames)
{
- for (const auto& jointMode : lvl1->jointControlModeMap)
+ result[joint] = "";
+ }
+ for (const auto & lvl1 : getActivatedLVL1Controllers())
+ {
+ if (!lvl1)
+ {
+ continue;
+ }
+ for (const auto & jointMode : lvl1->jointControlModeMap)
{
result[jointMode.first] = lvl1->getName();
}
@@ -160,7 +172,14 @@ armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModes
ARMARX_CHECK_AND_THROW(jointNames.size() == requestedModes.size(), std::logic_error);
for (std::size_t i = 0; i < jointNames.size(); ++i)
{
- result[jointNames.at(i)] = requestedModes.at(i)->getControlMode();
+ if (requestedModes.at(i))
+ {
+ result[jointNames.at(i)] = requestedModes.at(i)->getControlMode();
+ }
+ else
+ {
+ result[jointNames.at(i)] = "<NO LVL0CONTROLLER SET>";
+ }
}
return result;
}
@@ -172,7 +191,15 @@ armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModes
ARMARX_CHECK_AND_THROW(jointNames.size() == activatedModes.size(), std::logic_error);
for (std::size_t i = 0; i < jointNames.size(); ++i)
{
- result[jointNames.at(i)] = activatedModes.at(i)->getControlMode();
+ if (activatedModes.at(i))
+ {
+ result[jointNames.at(i)] = activatedModes.at(i)->getControlMode();
+ }
+ else
+ {
+ result[jointNames.at(i)] = "<NO LVL0CONTROLLER SET>";
+ }
+
}
return result;
}
@@ -180,11 +207,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);
}
@@ -198,11 +225,10 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
//check if all of these controllers exist
- for (const auto& lvl1 : controllerRequestedNames)
+ for (const auto & lvl1 : controllerRequestedNames)
{
if (!hasLVL1Controller(lvl1))
{
- ARMARX_ERROR << "No controller of the name '" << lvl1 << "' is loaded!";
throw InvalidArgumentException {"No controller of the name '" + lvl1 + "' is loaded!"};
}
}
@@ -214,7 +240,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))
{
@@ -222,10 +248,12 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
continue;
}
controllersToActivate.emplace(toActivate);
+
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);
@@ -239,7 +267,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)
{
@@ -252,7 +280,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))
{
@@ -278,7 +306,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));
}
@@ -322,7 +350,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));
}
@@ -475,6 +503,7 @@ bool armarx::RobotUnit::rtSwitchSetup()
{
return false;
}
+
//handle lvl1
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
@@ -483,6 +512,7 @@ bool armarx::RobotUnit::rtSwitchSetup()
{
rtGetActivatedLVL1Controller(i)->rtDeactivateController();
}
+
//activate new
if (rtGetRequestedLVL1Controller(i))
{
@@ -499,6 +529,7 @@ bool armarx::RobotUnit::rtSwitchSetup()
rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetRequestedLVL0Controller(i));
rtGetActivatedLVL0Controller(i) = rtGetRequestedLVL0Controller(i);
}
+
if (!rtGetRequestedLVL0Controller(i))
{
//no lvl0 controller is set!
@@ -506,12 +537,14 @@ bool armarx::RobotUnit::rtSwitchSetup()
rtGetActivatedLVL0Controller(i) = rtGetActivatedLVL0Controller(i);
}
}
+
+
return true;
}
void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- for (LVL1ControllerPtr& lvl1 : rtGetActivatedLVL1Controllers())
+ for (LVL1ControllerPtr & lvl1 : rtGetActivatedLVL1Controllers())
{
if (!lvl1)
{
@@ -545,7 +578,7 @@ void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
void armarx::RobotUnit::rtRunLVL0Controllers()
{
- for (LVL0ControllerBase* lvl0 : rtGetActivatedLVL0Controllers())
+ for (LVL0ControllerBase * lvl0 : rtGetActivatedLVL0Controllers())
{
lvl0->run();
}
@@ -555,7 +588,7 @@ bool armarx::RobotUnit::validateAddedLVL0Controllers() const
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
- for (const auto& lvl0 : lvl0ControllersEmergencyStop)
+ for (const auto & lvl0 : lvl0ControllersEmergencyStop)
{
if (!lvl0)
{
@@ -563,4 +596,5 @@ bool armarx::RobotUnit::validateAddedLVL0Controllers() const
}
}
return true;
+
}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
index 734be3dfb..a91b3d690 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
@@ -440,9 +440,9 @@ namespace armarx
//used to communicate with rt
/// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some lvl1 controllers may be null)
- TripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
+ WriteBufferedTripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
/// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some lvl1 controllers may be null)
- TripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
+ WriteBufferedTripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
//other
/// @brief Stores joints accessed via getJointTarget
--
GitLab