From 1d5e621d35e1689d222cc0caf1f7223adef0c884 Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Tue, 13 Jun 2017 17:07:40 +0200
Subject: [PATCH] reduce the number of smartptr copies
---
.../components/units/RobotUnit/RobotUnit.cpp | 89 +++++++++++--------
.../components/units/RobotUnit/RobotUnit.h | 25 +++---
.../RobotUnit/Units/KinematicSubUnit.cpp | 6 +-
.../units/RobotUnit/Units/KinematicSubUnit.h | 2 +-
4 files changed, 72 insertions(+), 50 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 7a64d003a..3079003f0 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -416,11 +416,11 @@ namespace armarx
LVL1KinematicUnitPassThroughControllerConfigPtr config = new LVL1KinematicUnitPassThroughControllerConfig; \
config->controlMode=controlMode; \
config->deviceName=controlDeviceName; \
- LVL1ControllerPtr lvl1 = createLVL1Controller( \
- "LVL1KinematicUnitPassThroughController", \
- "LVL1KU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
- config, \
- false ); \
+ const LVL1ControllerPtr& lvl1 = createLVL1Controller( \
+ "LVL1KinematicUnitPassThroughController", \
+ "LVL1KU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
+ config, \
+ false ); \
pt = LVL1KinematicUnitPassThroughControllerPtr::dynamicCast(lvl1); \
ARMARX_CHECK_EXPRESSION(pt); \
} \
@@ -452,7 +452,7 @@ namespace armarx
//fill devices (sensor + controller)
unit->setupData(getProperty<std::string>("RobotFileName").getValue(), robot->clone(robot->getName()), std::move(devs), this);
//add
- addUnit(unit);
+ addUnit(std::move(unit));
}
void RobotUnit::initializePlatformUnit()
@@ -505,7 +505,7 @@ namespace armarx
unit->pt = ctrl;
unit->platformSensorIndex = sensorDevices.index(robotPlatformName);
//add
- addUnit(unit);
+ addUnit(std::move(unit));
}
void RobotUnit::initializeForceTorqueUnit()
@@ -566,7 +566,7 @@ namespace armarx
IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, configDomain);
unit->devs = ftdevs;
//add
- addUnit(unit);
+ addUnit(std::move(unit));
}
void RobotUnit::initializeInertialMeasurementUnit()
@@ -607,7 +607,7 @@ namespace armarx
IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, configDomain);
unit->devs = imudevs;
//add
- addUnit(unit);
+ addUnit(std::move(unit));
}
void RobotUnit::addUnit(ManagedIceObjectPtr unit)
@@ -615,6 +615,12 @@ namespace armarx
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
getArmarXManager()->addObjectAsync(unit, "", true, false);
+ //maybe add it to the sub units
+ RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(unit);
+ if (rsu)
+ {
+ subUnits.emplace_back(std::move(rsu));
+ }
units.emplace_back(std::move(unit));
}
@@ -906,7 +912,11 @@ namespace armarx
return createLVL1Controller(className, instanceName, LVL1ControllerRegistry::get(className)->GenerateConfigFromVariants(variants), GlobalIceCurrent/*to select ice overload*/);
}
- LVL1ControllerPtr RobotUnit::createLVL1Controller(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, bool deletable)
+ const LVL1ControllerPtr& RobotUnit::createLVL1Controller(
+ const std::string& className,
+ const std::string& instanceName,
+ const LVL1ControllerConfigPtr& config,
+ bool deletable)
{
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
@@ -936,7 +946,7 @@ namespace armarx
initLVL1UsedControlTargets.clear();
initLVL1ThreadId = std::thread::id{};
};
- LVL1ControllerPtr lvl1 = factory->create(LVL1ControllerDescriptionProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config, robot);
+ LVL1ControllerPtr lvl1 {factory->create(LVL1ControllerDescriptionProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config, robot)};
std::vector<char> ctrlDeviceUsedBitmap(getNumberOfControlDevices(), false);
std::vector<std::size_t> ctrlDeviceUsedIndices;
@@ -956,9 +966,9 @@ namespace armarx
std::move(ctrlDeviceUsedIndices),
deletable);
getArmarXManager()->addObjectAsync(lvl1, instanceName, false, false);
- lvl1Controllers[instanceName] = lvl1;
+ lvl1Controllers[instanceName] = std::move(lvl1);
listenerPrx->lvl1ControllerCreated(instanceName);
- return lvl1;
+ return lvl1Controllers.at(instanceName);
}
void RobotUnit::setActivateControllersRequest(std::set<LVL1ControllerPtr, LVL1Controller::IdComp>&& ctrls)
@@ -1220,9 +1230,8 @@ namespace armarx
{
ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
publishNewSensorDataTime = TimeUtil::GetTime();
- for (ManagedIceObjectPtr& u : units)
+ for (RobotUnitSubUnitPtr& rsu : subUnits)
{
- RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(u);
if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
{
debugObserverMap["publishTimings_UnitUpdate_" + rsu->getName()] = new TimedVariant
@@ -1322,8 +1331,9 @@ namespace armarx
{
ARMARX_TIMER(TimestampVariant)
{
- for (const LVL1ControllerPtr& lvl1 : getMapValues(lvl1Controllers))
+ for (const auto& pair : lvl1Controllers)
{
+ const LVL1ControllerPtr& lvl1 = pair.second;
debugObserverMap["publishTimings_LVL1Updates_" + lvl1->getInstanceName()] = new TimedVariant
{
ARMARX_TIMER(TimestampVariant)
@@ -1401,10 +1411,11 @@ namespace armarx
void RobotUnit::removeAllUnits()
{
+ subUnits.clear();
for (ManagedIceObjectPtr& unit : units)
{
+ getArmarXManager()->removeObjectBlocking(unit->getName());
checkRefCountIsOne(unit, unit->getName());
- getArmarXManager()->removeObjectBlocking(unit);
checkRefCountIsOne(unit, unit->getName());
}
units.clear();
@@ -1452,7 +1463,7 @@ namespace armarx
for (auto& n2lvl1 : lvl1ControllersToBeDeleted)
{
LVL1ControllerPtr& lvl1 = n2lvl1.second;
- getArmarXManager()->removeObjectBlocking(lvl1);
+ getArmarXManager()->removeObjectBlocking(lvl1->getName());
checkRefCountIsOne(lvl1, lvl1->getName());
}
lvl1ControllersToBeDeleted.clear();
@@ -1462,7 +1473,7 @@ namespace armarx
for (auto& n2lvl1 : lvl1Controllers)
{
LVL1ControllerPtr& lvl1 = n2lvl1.second;
- getArmarXManager()->removeObjectBlocking(lvl1);
+ getArmarXManager()->removeObjectBlocking(lvl1->getName());
checkRefCountIsOne(lvl1, lvl1->getName());
}
lvl1Controllers.clear();
@@ -1918,29 +1929,31 @@ namespace armarx
ctrl.reserve(names.size());
for (const auto& name : names)
{
- auto it = lvl1Controllers.find(name);
- if (it == lvl1Controllers.end())
- {
- std::stringstream ss;
- ss << "RobotUnit: there is no LVL1Controller with name '" << name
- << "'. Existing LVL1Controllers are: " << getLVL1ControllerNames();
- throw InvalidArgumentException {ss.str()};
- }
- if (!it->second)
- {
- std::stringstream ss;
- ss << "RobotUnit: The LVL1Controller with name '" << name
- << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n" << lvl1Controllers;
- ARMARX_FATAL << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- ctrl.emplace_back(it->second);
+ ctrl.emplace_back(getLVL1ControllerNotNull(name));
}
return ctrl;
}
- LVL1ControllerPtr RobotUnit::getLVL1ControllerNotNull(const std::string& name) const
+ const LVL1ControllerPtr& RobotUnit::getLVL1ControllerNotNull(const std::string& name) const
{
- return getLVL1ControllersNotNull(std::vector<std::string> {name}).at(0);
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto it = lvl1Controllers.find(name);
+ if (it == lvl1Controllers.end())
+ {
+ std::stringstream ss;
+ ss << "RobotUnit: there is no LVL1Controller with name '" << name
+ << "'. Existing LVL1Controllers are: " << getLVL1ControllerNames();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (!it->second)
+ {
+ std::stringstream ss;
+ ss << "RobotUnit: The LVL1Controller with name '" << name
+ << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n" << lvl1Controllers;
+ ARMARX_FATAL << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ return it->second;
}
StringLVL1ControllerPrxDictionary RobotUnit::getAllLVL1Controllers(const Ice::Current&) const
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index a88ccc9f4..6e8db143e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -41,6 +41,7 @@
#include <ArmarXCore/observers/DebugObserver.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include "Units/RobotUnitSubUnit.h"
#include "LVL0Controllers/LVL0Controller.h"
#include "LVL1Controllers/LVL1Controller.h"
#include "Devices/ControlDevice.h"
@@ -166,19 +167,19 @@ namespace armarx
}
}
static void InitLVL1Controler(
- LVL1ControllerPtr lvl1,
+ const LVL1ControllerPtr& lvl1,
RobotUnit* ru,
std::size_t ctrlId,
StringStringDictionary ctrlDeviceControlModeMap,
std::vector<char> ctrlDeviceUsedBitmap,
std::vector<std::size_t> ctrlDeviceUsedIndices,
bool deletable
- )
+ ) noexcept
{
lvl1->robotUnitInit(ru, ctrlId, std::move(ctrlDeviceControlModeMap), std::move(ctrlDeviceUsedBitmap), std::move(ctrlDeviceUsedIndices), deletable);
}
static void PublishLVL1Controller(
- LVL1ControllerPtr lvl1,
+ const LVL1ControllerPtr& lvl1,
const SensorAndControl& sac,
const DebugDrawerInterfacePrx& draw,
const DebugObserverInterfacePrx& observer
@@ -186,26 +187,26 @@ namespace armarx
{
lvl1 ->publish(sac, draw, observer);
}
- bool GetLVL1ControllerStatusReportedActive(LVL1ControllerPtr lvl1)
+ bool GetLVL1ControllerStatusReportedActive(const LVL1ControllerPtr& lvl1)
{
return lvl1->statusReportedActive;
}
- bool GetLVL1ControllerStatusReportedRequested(LVL1ControllerPtr lvl1)
+ bool GetLVL1ControllerStatusReportedRequested(const LVL1ControllerPtr& lvl1)
{
return lvl1->statusReportedRequested;
}
- void UpdateLVL1ControllerStatusReported(LVL1ControllerPtr lvl1)
+ void UpdateLVL1ControllerStatusReported(const LVL1ControllerPtr& lvl1)
{
lvl1->statusReportedActive = lvl1->isActive.load();
lvl1->statusReportedRequested = lvl1->isRequested.load();
}
- void SetLVL1ControllerRequested(LVL1ControllerPtr lvl1, bool requested)
+ void SetLVL1ControllerRequested(const LVL1ControllerPtr& lvl1, bool requested)
{
lvl1->isRequested = requested;
}
void DeactivateLVL1ControllerPublishing(
- LVL1ControllerPtr lvl1,
+ const LVL1ControllerPtr& lvl1,
const DebugDrawerInterfacePrx& draw,
const DebugObserverInterfacePrx& observer)
{
@@ -305,7 +306,11 @@ namespace armarx
/// @brief The RobotUnit's state
std::atomic<RobotUnitState> state {RobotUnitState::PreComponentInitialization};
std::map<std::string, DynamicLibraryPtr> loadedLibs;
+ /// @brief holds all units managed by the RobotUnit
std::vector<ManagedIceObjectPtr> units;
+ /// @brief holds a copy of all units managed by the RobotUnit derived from RobotUnitSubUnit
+ /// this is done to prevent casting
+ std::vector<RobotUnitSubUnitPtr> subUnits;
std::thread::id rtThreadId;
mutable MutexType dataMutex;
static const std::set<RobotUnitState> devicesReadyStates;
@@ -469,7 +474,7 @@ namespace armarx
virtual ControlTargetBase* getControlTarget(const std::string& deviceName, const std::string& controlMode) override;
virtual LVL1ControllerInterfacePrx createLVL1Controller(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current&) override;
virtual LVL1ControllerInterfacePrx createLVL1ControllerFromVariantConfig(const std::string& className, const std::string& instanceName, const StringVariantBaseMap& variants, const Ice::Current&) override;
- LVL1ControllerPtr createLVL1Controller(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, bool deletable);
+ const LVL1ControllerPtr& createLVL1Controller(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, bool deletable);
//deleting lvl1 controllers
virtual void deleteLVL1Controller(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
virtual void deleteLVL1Controllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
@@ -526,7 +531,7 @@ namespace armarx
virtual LVL1ControllerInterfacePrx getLVL1Controller(const std::string& name, const Ice::Current&) const override;
virtual StringLVL1ControllerPrxDictionary getAllLVL1Controllers(const Ice::Current&) const override;
std::vector<LVL1ControllerPtr> getLVL1ControllersNotNull(const std::vector<std::string>& names) const;
- LVL1ControllerPtr getLVL1ControllerNotNull(const std::string& name) const;
+ const LVL1ControllerPtr& getLVL1ControllerNotNull(const std::string& name) const;
virtual LVL1ControllerStatus getLVL1ControllerStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
virtual LVL1ControllerStatusSeq getLVL1ControllerStatuses(const Ice::Current&) const override;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 5c0665d43..8103be99c 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -21,7 +21,11 @@
*/
#include "KinematicSubUnit.h"
-void armarx::KinematicSubUnit::setupData(std::string relRobFile, VirtualRobot::RobotPtr rob, std::map<std::string, ActuatorData> newDevs, RobotUnit* newRobotUnit)
+void armarx::KinematicSubUnit::setupData(
+ std::string relRobFile,
+ VirtualRobot::RobotPtr rob,
+ std::map<std::string, ActuatorData>&& newDevs,
+ RobotUnit* newRobotUnit)
{
std::lock_guard<std::mutex> guard {dataMutex};
ARMARX_CHECK_EXPRESSION(getState() == eManagedIceObjectCreated);
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 7bbf7d699..fbcc16ea6 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -55,7 +55,7 @@ namespace armarx
const LVL1ControllerPtr getController(ControlMode c) const;
};
- void setupData(std::string relRobFile, VirtualRobot::RobotPtr rob, std::map<std::string, ActuatorData> newDevs, RobotUnit* newRobotUnit);
+ void setupData(std::string relRobFile, VirtualRobot::RobotPtr rob, std::map<std::string, ActuatorData>&& newDevs, RobotUnit* newRobotUnit);
virtual void update(const SensorAndControl& sc, const LVL0AndLVL1Controllers& c) override;
// KinematicUnitInterface interface
--
GitLab