diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp index 1365ca76a761f5956aaebc5d96ef1adcca8034f5..934b88073535870c139d3677e45d8dcdc19bbf57 100644 --- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp +++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp @@ -187,7 +187,7 @@ namespace armarx return entries.at(i); } } - ScopedLock lock(mutex); + std::unique_lock lock(mutex); ARMARX_INFO << "registering '" << name << "'"; entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000); validEntries++; @@ -219,7 +219,7 @@ namespace armarx e.isRunning = true; e.enabled = true; { - ScopedLock lock(e.messageMutex); + std::unique_lock lock(e.messageMutex); e.message = args.message; } } @@ -285,7 +285,7 @@ namespace armarx } ARMARX_TRACE; - ScopedLock lock(e.messageMutex); + std::unique_lock lock(e.messageMutex); if (e.message.size()) { ss << " - " << e.message; diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h index 97852dab631cc1e6e1df0c4f738585f43395b0f4..49ca51167f87d7c83636c7d7d8f744797dd2d5e2 100644 --- a/source/RobotAPI/components/RobotHealth/RobotHealth.h +++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h @@ -31,10 +31,10 @@ #include <ArmarXCore/core/time/TimeUtil.h> #include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/services/tasks/PeriodicTask.h> -#include <ArmarXCore/core/system/Synchronization.h> #include <ArmarXCore/interface/components/EmergencyStopInterface.h> #include <atomic> +#include <mutex> namespace armarx { @@ -111,7 +111,7 @@ namespace armarx bool isRunning = false; bool required = false; bool enabled = true; - Mutex messageMutex; + std::mutex messageMutex; std::vector<long> history; size_t historyIndex = 0; }; @@ -145,7 +145,7 @@ namespace armarx void monitorHealthTaskClb(); Entry& findOrCreateEntry(const std::string& name); - Mutex mutex; + std::mutex mutex; std::deque<Entry> entries; std::atomic_size_t validEntries {0}; PeriodicTask<RobotHealth>::pointer_type monitorHealthTask; diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp index 3cb860a4dbc16e48e0c5d60ba0b6985f0d99990c..a44d697794ceaf47d46266b6e6b515dec9acf84d 100644 --- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp +++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp @@ -31,6 +31,8 @@ #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h> #include <RobotAPI/libraries/core/math/ColorUtils.h> +#include <thread> + namespace armarx { void ViewSelection::onInitComponent() @@ -142,7 +144,7 @@ namespace armarx void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps) { - boost::mutex::scoped_lock lock(syncMutex); + std::unique_lock lock(syncMutex); IceUtil::Time currentTime = armarx::TimeUtil::GetTime(); for (const auto& p : saliencyMaps) @@ -180,7 +182,7 @@ namespace armarx int maxIndex = -1; float maxSaliency = std::numeric_limits<float>::min(); - boost::mutex::scoped_lock lock(syncMutex); + std::unique_lock lock(syncMutex); hasNewSaliencyMap = false; @@ -284,7 +286,7 @@ namespace armarx if (!manualViewTargets.empty()) { - ScopedLock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); ViewTargetBasePtr manualViewTarget = manualViewTargets.top(); @@ -332,11 +334,11 @@ namespace armarx viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration); - boost::this_thread::sleep(boost::posix_time::milliseconds(duration)); + std::this_thread::sleep_for(std::chrono::milliseconds(duration)); } else { - boost::this_thread::sleep(boost::posix_time::milliseconds(5)); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); } } @@ -344,7 +346,7 @@ namespace armarx void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c) { - boost::mutex::scoped_lock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot(); @@ -356,14 +358,14 @@ namespace armarx manualViewTargets.push(viewTarget); - boost::mutex::scoped_lock lock2(syncMutex); + std::unique_lock lock2(syncMutex); condition.notify_all(); } void ViewSelection::clearManualViewTargets(const Ice::Current& c) { - boost::mutex::scoped_lock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); while (!manualViewTargets.empty()) { @@ -376,7 +378,7 @@ namespace armarx ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c) { - boost::mutex::scoped_lock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); ViewTargetList result; @@ -396,7 +398,7 @@ namespace armarx void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&) { - boost::mutex::scoped_lock lock(syncMutex); + std::unique_lock lock(syncMutex); hasNewSaliencyMap = true; @@ -439,7 +441,7 @@ namespace armarx } - boost::mutex::scoped_lock lock(syncMutex); + std::unique_lock lock(syncMutex); TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes(); DebugDrawer24BitColoredPointCloud cloud; @@ -492,7 +494,7 @@ namespace armarx void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c) { - boost::mutex::scoped_lock lock(syncMutex); + std::unique_lock lock(syncMutex); if (saliencyMaps.count(name)) { @@ -506,7 +508,7 @@ namespace armarx { std::vector<std::string> names; - boost::mutex::scoped_lock lock(syncMutex); + std::unique_lock lock(syncMutex); for (const auto& p : saliencyMaps) { diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h index f8e6cbdc93a26500fef455bc989b2149bd1554b4..98beeedd5e6d9a176d9b76e83b4b9659d99ef8d5 100644 --- a/source/RobotAPI/components/ViewSelection/ViewSelection.h +++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h @@ -46,6 +46,8 @@ #include <Eigen/Geometry> +#include <condition_variable> +#include <mutex> #include <queue> namespace armarx @@ -151,7 +153,7 @@ namespace armarx void activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override { - boost::mutex::scoped_lock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); ARMARX_INFO << "activating automatic view selection"; @@ -161,7 +163,7 @@ namespace armarx void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override { - boost::mutex::scoped_lock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); ARMARX_INFO << "deactivating automatic view selection"; @@ -171,7 +173,7 @@ namespace armarx bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override { - boost::mutex::scoped_lock lock(manualViewTargetsMutex); + std::unique_lock lock(manualViewTargetsMutex); return doAutomaticViewSelection; } @@ -214,16 +216,16 @@ namespace armarx bool drawViewDirection; - armarx::Mutex manualViewTargetsMutex; + std::mutex manualViewTargetsMutex; std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets; bool doAutomaticViewSelection; Eigen::Vector3f offsetToHeadCenter; - boost::condition_variable condition; + std::condition_variable condition; bool hasNewSaliencyMap; - boost::mutex syncMutex; + std::mutex syncMutex; std::map<std::string, SaliencyMapBasePtr> saliencyMaps; diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index 60225861162fbfa91b23798437784f8d054ec02d..b4d6e62f6f1e203f4930e2ea56b424b08455c838 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/source/RobotAPI/components/units/HeadIKUnit.cpp @@ -50,7 +50,7 @@ namespace armarx void HeadIKUnit::onInitComponent() { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); usingProxy(getProperty<std::string>("KinematicUnitName").getValue()); usingProxy(getProperty<std::string>("RobotStateComponentName").getValue()); @@ -65,7 +65,7 @@ namespace armarx void HeadIKUnit::onConnectComponent() { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates"); @@ -103,7 +103,7 @@ namespace armarx { release(); - //ScopedLock lock(accessMutex); + //std::unique_lock lock(accessMutex); if (drawer) { drawer->removeSphereDebugLayerVisu("HeadViewTarget"); @@ -126,7 +126,7 @@ namespace armarx void HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c) { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); cycleTime = milliseconds; @@ -139,7 +139,7 @@ namespace armarx void HeadIKUnit::setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c) { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); this->robotNodeSetNames = armarx::Split(robotNodeSetName, ","); for (auto& setName : robotNodeSetNames) @@ -208,7 +208,7 @@ namespace armarx void HeadIKUnit::request(const Ice::Current& c) { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); requested = true; ARMARX_IMPORTANT << "Requesting HeadIKUnit"; @@ -228,7 +228,7 @@ namespace armarx void HeadIKUnit::release(const Ice::Current& c) { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); ARMARX_INFO << "Releasing HeadIKUnit"; requested = false; @@ -260,9 +260,9 @@ namespace armarx bool doCalculation = false; { - ScopedTryLock lock(accessMutex); + std::unique_lock lock(accessMutex, std::defer_lock); - if (lock.owns_lock()) + if (lock.try_lock()) { doCalculation = requested && newTargetSet; newTargetSet = false; @@ -276,7 +276,7 @@ namespace armarx if (doCalculation) { - ScopedLock lock(accessMutex); + std::unique_lock lock(accessMutex); VirtualRobot::RobotNodeSetPtr kinematicChain; bool foundSolution = false; diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h index 251c88d4d5a1cfd0822f79a744b3757a23cd451d..b814b519aae4f377a55e0610759e52663007ad50 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.h +++ b/source/RobotAPI/components/units/HeadIKUnit.h @@ -25,13 +25,14 @@ #include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/services/tasks/PeriodicTask.h> -#include <ArmarXCore/core/system/Synchronization.h> #include <RobotAPI/interface/visualization/DebugDrawerInterface.h> #include <RobotAPI/libraries/core/FramedPose.h> #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h> #include <RobotAPI/interface/units/HeadIKUnit.h> +#include <mutex> + namespace armarx { @@ -98,7 +99,7 @@ namespace armarx private: void periodicExec(); - Mutex accessMutex; + std::mutex accessMutex; bool requested; int cycleTime; PeriodicTask<HeadIKUnit>::pointer_type execTask; diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp index 99bbab48c3d6aa7835e515dac18ed5e2fd2c5354..2e3f100c89d4e1b7e26d37e72ef075dda0ff8e4c 100644 --- a/source/RobotAPI/components/units/TCPControlUnit.cpp +++ b/source/RobotAPI/components/units/TCPControlUnit.cpp @@ -69,7 +69,7 @@ namespace armarx void TCPControlUnit::onConnectComponent() { - ScopedLock lock(dataMutex); + std::unique_lock lock(dataMutex); debugObs = getTopic<DebugObserverInterfacePrx>("DebugObserver"); kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue()); @@ -173,7 +173,7 @@ namespace armarx void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c) { - ScopedLock lock(taskMutex); + std::unique_lock lock(taskMutex); cycleTime = milliseconds; if (execTask) @@ -190,7 +190,7 @@ namespace armarx request(); } - ScopedLock lock(dataMutex); + std::unique_lock lock(dataMutex); ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName); @@ -269,7 +269,7 @@ namespace armarx } ARMARX_IMPORTANT << "Requesting TCPControlUnit"; - ScopedLock lock(dataMutex); + std::unique_lock lock(dataMutex); requested = true; execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator"); @@ -281,7 +281,7 @@ namespace armarx void TCPControlUnit::release(const Ice::Current& c) { ARMARX_IMPORTANT << "Releasing TCPControlUnit"; - ScopedLock lock(dataMutex); + std::unique_lock lock(dataMutex); // while (calculationRunning) // { @@ -305,9 +305,9 @@ namespace armarx void TCPControlUnit::periodicExec() { - ScopedTryLock lock(dataMutex); + std::unique_lock lock(dataMutex, std::defer_lock); - if (lock.owns_lock()) + if (lock.try_lock()) { { @@ -1304,7 +1304,7 @@ void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&, void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&) { - ScopedLock lock(reportMutex); + std::unique_lock lock(reportMutex); FramedPoseBaseMap tcpPoses; std::string rootFrame = localReportRobot->getRootNode()->getName(); auto it = jointAngles.find("timestamp"); @@ -1337,7 +1337,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel, } lastTopicReportTime = TimeUtil::GetTime(); - ScopedLock lock(reportMutex); + std::unique_lock lock(reportMutex); if (!localVelReportRobot) { diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h index cc984d4358f9e0b1dbec9455f6cb26283aa876bf..e24e2f8dc344b1b44a4bd24ca2240506d0c329c8 100644 --- a/source/RobotAPI/components/units/TCPControlUnit.h +++ b/source/RobotAPI/components/units/TCPControlUnit.h @@ -25,7 +25,6 @@ #include <RobotAPI/interface/units/TCPControlUnit.h> #include <RobotAPI/libraries/core/FramedPose.h> #include <ArmarXCore/core/services/tasks/PeriodicTask.h> -#include <ArmarXCore/core/system/Synchronization.h> #include <ArmarXCore/core/Component.h> #include <VirtualRobot/IK/DifferentialIK.h> @@ -211,9 +210,9 @@ namespace armarx FramedPoseBaseMap lastTCPPoses; IceUtil::Time lastTopicReportTime; - Mutex dataMutex; - Mutex taskMutex; - Mutex reportMutex; + std::mutex dataMutex; + std::mutex taskMutex; + std::mutex reportMutex; bool calculationRunning; double syncTimeDelta; diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp index 38e01c30f7cb4cfdd13ae802c6e3c7dc31e714b2..31ed73e85ab61b61ed48ede725bc03447114e05f 100644 --- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp +++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp @@ -44,7 +44,7 @@ void GamepadUnit::onConnectComponent() sendTask = new SimplePeriodicTask<>([&] { ARMARX_TRACE; - ScopedLock lock(mutex); + std::unique_lock lock(mutex); if (!js.opened()) { return; @@ -114,7 +114,7 @@ void GamepadUnit::run() } } ARMARX_TRACE; - ScopedLock lock(mutex); + std::unique_lock lock(mutex); IceUtil::Time now = IceUtil::Time::now(); dataTimestamp = new TimestampVariant(now); diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h index 85ac99dc262ec38a2b1e4e3b1464e97e4866f22e..109ce81a17a98c5154b396e09064d9943be6ff0a 100644 --- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h +++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h @@ -31,7 +31,6 @@ #include <ArmarXCore/core/services/tasks/RunningTask.h> #include <ArmarXCore/core/services/tasks/TaskUtil.h> -#include <ArmarXCore/core/system/Synchronization.h> #include <RobotAPI/interface/units/GamepadUnit.h> @@ -115,7 +114,7 @@ namespace armarx SimplePeriodicTask<>::pointer_type sendTask; void run(); - Mutex mutex; + std::mutex mutex; std::string deviceName; Joystick js; GamepadData data; diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp index 41701aaf3f7953445affcfd0292a581120637905..dcbd9ca9d926d65487dbe3d2427118f89c6bdb46 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp @@ -1387,7 +1387,7 @@ bool EtherCAT::generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex, ARMARX_WARNING << "Bus is not running no write"; return false; } - ScopedLock lock(etherCatMutex); + std::unique_lock lock(etherCatMutex); IceUtil::Time start = IceUtil::Time::now(); int workCount = ec_SDOwrite(slave, index, subindex, (boolean) ca, buflen, buf, SDO_WRITE_TIMEOUT); IceUtil::Time end = IceUtil::Time::now(); @@ -1431,7 +1431,7 @@ bool EtherCAT::generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex, { return false; } - ScopedLock lock(etherCatMutex); + std::unique_lock lock(etherCatMutex); IceUtil::Time start = IceUtil::Time::now(); int workCount = ec_SDOread(slave, index, subindex, (boolean) ca, &buflen, buf, SDO_READ_TIMEOUT); IceUtil::Time end = IceUtil::Time::now(); diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h index e9bc549d71890e6345cd2ef8706b0d338baafcc6..f103df995c10cb2210071b2b58c8a368231d06d9 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h @@ -3,8 +3,6 @@ #include "AbstractFunctionalDevice.h" -#include <ArmarXCore/core/system/Synchronization.h> - #include <IceUtil/Time.h> #include <iostream> @@ -13,6 +11,7 @@ #include <memory> #include <sstream> #include <atomic> +#include <mutex> /** @@ -236,7 +235,7 @@ namespace armarx RobotUnit* mainUnitPtr; std::string ifname; - mutable Mutex etherCatMutex; + mutable std::mutex etherCatMutex; bool emergencyStopWasActivated = false; IceUtil::Time busUpdateLastUpdateTime; IceUtil::Time ermergencyStopRecoverStartpoint; diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h index 56ff6d28cd17522453014f9b880d8e88294ea3dc..f25048c8fec61c0923c8da06532f1a05570fba87 100644 --- a/source/RobotAPI/libraries/core/MultiDimPIDController.h +++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h @@ -28,10 +28,11 @@ #include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <ArmarXCore/core/time/TimeUtil.h> -#include <ArmarXCore/core/system/Synchronization.h> #include <Eigen/Core> +#include <mutex> + namespace armarx { template <int dimensions = Eigen::Dynamic> @@ -218,7 +219,7 @@ namespace armarx double maxControlValue; double maxDerivation; bool firstRun; - mutable RecursiveMutex mutex; + mutable std::recursive_mutex mutex; bool threadSafe = true; std::vector<bool> limitless; private: @@ -231,6 +232,8 @@ namespace armarx PIDVectorX zeroVec; } stackAllocations; + using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>; + using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>; ScopedRecursiveLockPtr getLock() const { if (threadSafe)