diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp index eacd69f2afe16f464dc4486665e7c4e6674a7147..94318354a993c7837eb2fac200c879e26d1c9258 100644 --- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp +++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp @@ -878,7 +878,7 @@ namespace armarx d.color.b, d.color.transparency, 16, 30); - SoNode* circle = boost::dynamic_pointer_cast<CoinVisualizationNode>(node)->getCoinVisualization(); + SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization(); circle->setName(SELECTION_NAME(d.layerName, d.name)); sep->addChild(circle); @@ -1315,7 +1315,7 @@ namespace armarx nodeMat->setOverride(false); nodeSep->addChild(nodeMat); - boost::shared_ptr<CoinVisualization> robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType); + CoinVisualizationPtr robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType); if (robNodeVisu) { diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp index e49aac6a5abb3f596201cdc23f823805eba10654..da99bba0da26c375c7e1ad0d03936611dd2abae5 100644 --- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp +++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp @@ -184,7 +184,7 @@ namespace armarx Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const { ReadLockPtr lock = _node->getRobot()->getReadLock(); - boost::shared_ptr<RobotNodePrismatic> prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(_node); + RobotNodePrismatic* prismatic = dynamic_cast<RobotNodePrismatic*>(_node.get()); if (prismatic) { @@ -199,7 +199,7 @@ namespace armarx Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current& current) const { ReadLockPtr lock = _node->getRobot()->getReadLock(); - boost::shared_ptr<RobotNodeRevolute> revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(_node); + RobotNodeRevolute* revolute = dynamic_cast<RobotNodeRevolute*>(_node.get()); if (revolute) { diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h index 922588ddbdbf15100fa4a3e358a5abd232de3546..76addb6ea428eacfbf37548067956fe33fd6d1f3 100644 --- a/source/RobotAPI/components/RobotState/SharedRobotServants.h +++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h @@ -26,19 +26,11 @@ #include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/system/ImportExportComponent.h> #include <RobotAPI/libraries/core/FramedPose.h> - - #include <RobotAPI/interface/core/RobotState.h> -#include <boost/thread.hpp> +#include <VirtualRobot/VirtualRobot.h> -namespace VirtualRobot -{ - class Robot; - using RobotPtr = boost::shared_ptr<Robot>; - class RobotNode; - using RobotNodePtr = boost::shared_ptr<RobotNode>; -} +#include <boost/thread.hpp> namespace armarx { diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h index 6f7cc714ab242930d2a2a32d6022b77889a473bc..3816605266f1bc44dce40a752bfe94b14af8177b 100644 --- a/source/RobotAPI/components/units/HandUnit.h +++ b/source/RobotAPI/components/units/HandUnit.h @@ -32,15 +32,9 @@ #include <ArmarXCore/core/system/ImportExportComponent.h> #include <ArmarXCore/observers/variant/SingleTypeVariantList.h> +#include <VirtualRobot/VirtualRobot.h> -namespace VirtualRobot -{ - class Robot; - using RobotPtr = boost::shared_ptr<Robot>; - class EndEffector; - using EndEffectorPtr = boost::shared_ptr<EndEffector>; -} namespace armarx { /** diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index d240a3c60d2fa0a382ae4fe69cb33df69a643c9c..df43463d53c642d9953382872fa37c6caa519372 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/source/RobotAPI/components/units/HeadIKUnit.cpp @@ -31,6 +31,9 @@ #include <VirtualRobot/IK/GazeIK.h> #include <VirtualRobot/LinkedCoordinate.h> +#include <boost/shared_ptr.hpp> +#include <memory> + namespace armarx { @@ -305,7 +308,9 @@ namespace armarx VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint; - virtualPrismaticJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode); + using boost::dynamic_pointer_cast; + using std::dynamic_pointer_cast; + virtualPrismaticJoint = dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode); if (!virtualPrismaticJoint) { ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set"; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h index 1f6e157696ee50f33a216cf6665d271b11be7939..1ca9843eb3dd70b97900d04020c286a8c2d27588 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h @@ -53,13 +53,7 @@ #include "../util/JointAndNJointControllers.h" #include "../util/ControlThreadOutputBuffer.h" -namespace VirtualRobot -{ - class Robot; - class RobotNode; - using RobotNodePtr = boost::shared_ptr<RobotNode>; - using RobotPtr = boost::shared_ptr<Robot>; -} +#include <VirtualRobot/VirtualRobot.h> namespace armarx::RobotUnitModule { diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp index 8f4bd0e84990ce14bdc686609e3fdbb317321247..41022170c200a160b5f794da0973d1202600dae6 100644 --- a/source/RobotAPI/components/units/TCPControlUnit.cpp +++ b/source/RobotAPI/components/units/TCPControlUnit.cpp @@ -34,6 +34,7 @@ #include <Eigen/Core> +#include <boost/shared_ptr.hpp> #include <memory> using namespace VirtualRobot; @@ -358,7 +359,9 @@ namespace armarx localDIKMap[data.nodeSetName] = dIk; } - EDifferentialIKPtr dIk = boost::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]); + using std::dynamic_pointer_cast; + using boost::dynamic_pointer_cast; + auto dIk = dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]); dIk->clearGoals(); } @@ -476,7 +479,9 @@ namespace armarx { RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first); - EDifferentialIKPtr dIK = boost::dynamic_pointer_cast<EDifferentialIK>(itDIK->second); + using std::dynamic_pointer_cast; + using boost::dynamic_pointer_cast; + auto dIK = dynamic_pointer_cast<EDifferentialIK>(itDIK->second); // ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose(); // dIK->setVerbose(true); Eigen::VectorXf jointDelta; @@ -656,7 +661,7 @@ namespace armarx } else { - VR_ERROR << "Internal error?!" << endl; // Error + VR_ERROR << "Internal error?!" << std::endl; // Error } @@ -754,7 +759,7 @@ namespace armarx if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i])) { - ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << endl; + ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << std::endl; dofWeights = tempDOFWeights; return false; } @@ -776,7 +781,7 @@ namespace armarx { if (verbose) { - ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << endl; + ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << std::endl; } break; @@ -789,14 +794,14 @@ namespace armarx if (dTheta.norm() < mininumChange) { // if (verbose) - ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl; + ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << std::endl; break; } if (checkImprovement && posDist > lastDist) { // if (verbose) - ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << endl; + ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << std::endl; robot->setJointValues(rns, jvBest); break; } @@ -835,9 +840,9 @@ namespace armarx if (step >= maxNStep && verbose) { - ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << endl; - ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << endl; - ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << endl; + ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << std::endl; + ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << std::endl; + ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << std::endl; return false; } @@ -901,7 +906,7 @@ namespace armarx } else { - VR_ERROR << "Internal error?!" << endl; // Error + VR_ERROR << "Internal error?!" << std::endl; // Error } @@ -1051,7 +1056,7 @@ namespace armarx } else { - VR_ERROR << "Internal error?!" << endl; // Error + VR_ERROR << "Internal error?!" << std::endl; // Error } @@ -1221,7 +1226,7 @@ namespace armarx } Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1); - //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << endl; + //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << std::endl; //cout << boost::format("Error Position < %1% >: %2%") % tcp->getName() % position.norm() << std::endl; float result = 0.0f; Eigen::VectorXf tcpWeight(3); diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp index 76b9edea96974cce02ccb474e0a0e8452a5fe122..2dada5d9cfc755d806b548ae65185a17e63aa8bb 100644 --- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp +++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp @@ -668,7 +668,7 @@ VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string return robot; } -CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot) +VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot) { CoinVisualizationPtr coinVisualization; diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h index 047655ab2555949865fa9cc0b13b9dd4cda48642..41d50d3d08e5f394d2a9df8603656f595bed9f47 100644 --- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h +++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h @@ -54,13 +54,10 @@ #include <QStyledItemDelegate> #include <ArmarXCore/core/util/IceReportSkipper.h> -#include <boost/shared_ptr.hpp> -#include <boost/cstdint.hpp> +#include <VirtualRobot/VirtualRobot.h> namespace armarx { - using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>; - class KinematicUnitConfigDialog; /*! @@ -245,7 +242,7 @@ namespace armarx VirtualRobot::RobotNodeSetPtr robotNodeSet; VirtualRobot::RobotNodePtr currentNode; - CoinVisualizationPtr kinematicUnitVisualization; + VirtualRobot::CoinVisualizationPtr kinematicUnitVisualization; SoNode* kinematicUnitNode; SoSeparator* rootVisu; SoSeparator* robotVisu; @@ -276,7 +273,7 @@ namespace armarx boost::recursive_mutex mutexNodeSet; // init stuff VirtualRobot::RobotPtr loadRobotFile(std::string fileName); - CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot); + VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot); VirtualRobot::RobotNodeSetPtr getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName); bool initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet); bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet); diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h index 9292a3666978a4741ef8f6a75dd6da4021d8ef8d..70bfba4dd96690a4f7e133d0164de2a3ff87dd05 100644 --- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h +++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h @@ -44,14 +44,10 @@ #include <VirtualRobot/Visualization/VisualizationFactory.h> #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> -#include <boost/shared_ptr.hpp> - #include <Inventor/sensors/SoTimerSensor.h> namespace armarx { - using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>; - class RobotViewerConfigDialog; /** @@ -171,7 +167,7 @@ namespace armarx // init stuff VirtualRobot::RobotPtr loadRobotFile(std::string fileName); - CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot); + VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot); QPointer<QWidget> __widget; QPointer<SimpleConfigDialog> dialog; diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h index e7a796a6eb65e02cb59ac06751278d73912ca8cc..319e542d33664ec0a7d19c54422814369172435f 100644 --- a/source/RobotAPI/libraries/core/FramedPose.h +++ b/source/RobotAPI/libraries/core/FramedPose.h @@ -33,6 +33,9 @@ #include <ArmarXCore/observers/AbstractObjectSerializer.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/LinkedCoordinate.h> + #include <Eigen/Core> #include <Eigen/Geometry> @@ -40,13 +43,6 @@ #include <sstream> -namespace VirtualRobot -{ - class Robot; - using RobotPtr = boost::shared_ptr<Robot>; - class LinkedCoordinate; -} - namespace armarx::VariantType { // variant types diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp index 154cfb2890c9fb2f0f5e6a8b2867bf82ea68d775..2b9444a4d594a6483c435638fad3385f9b544f57 100644 --- a/source/RobotAPI/libraries/core/RobotPool.cpp +++ b/source/RobotAPI/libraries/core/RobotPool.cpp @@ -43,7 +43,7 @@ namespace armarx VirtualRobot::RobotPtr RobotPool::getRobot(int inflation) { - ScopedLock lock(mutex); + std::scoped_lock lock(mutex); for (auto& r : robots[inflation]) { if (r.use_count() == 1) @@ -61,7 +61,7 @@ namespace armarx size_t RobotPool::getPoolSize() const { - ScopedLock lock(mutex); + std::scoped_lock lock(mutex); size_t size = 0; for (auto& pair : robots) { @@ -72,7 +72,7 @@ namespace armarx size_t RobotPool::getRobotsInUseCount() const { - ScopedLock lock(mutex); + std::scoped_lock lock(mutex); size_t count = 0; for (auto& pair : robots) { @@ -89,7 +89,7 @@ namespace armarx size_t RobotPool::clean() { - ScopedLock lock(mutex); + std::scoped_lock lock(mutex); size_t count = 0; for (auto& pair : robots) { diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h index 6aa0ccb24c917a6e76c2d2a0e1d06f9cc8ae16be..4d6db6fc1ed2ba14a1f3c9bffe2b7557deaf0508 100644 --- a/source/RobotAPI/libraries/core/RobotPool.h +++ b/source/RobotAPI/libraries/core/RobotPool.h @@ -22,8 +22,11 @@ * GNU General Public License */ #pragma once + #include <VirtualRobot/Robot.h> -#include <ArmarXCore/core/system/Synchronization.h> + +#include <mutex> + namespace armarx { @@ -53,7 +56,7 @@ namespace armarx private: std::map<int, std::vector<VirtualRobot::RobotPtr>> robots; VirtualRobot::RobotPtr baseRobot; - mutable Mutex mutex; + mutable std::mutex mutex; }; using RobotPoolPtr = std::shared_ptr<RobotPool>; } diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp index 6e4988445a86ee0a2e42efc1ddcc4da2600d3609..cb3a549b7d45fdc48d727da8a46db0586512917e 100644 --- a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp +++ b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp @@ -28,6 +28,10 @@ #include <ArmarXCore/core/system/ImportExportComponent.h> #include <ArmarXCore/statechart/Statechart.h> +#include <boost/algorithm/string/split.hpp> +#include <boost/algorithm/string/classification.hpp> +#include <boost/algorithm/string/trim.hpp> + namespace armarx { // **************************************************************** diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp index dfc84e32aa84f0bba6e323322cd78cab998473be..82b34bed30972c62afb143272525a261f43c1823 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp +++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp @@ -22,8 +22,12 @@ * GNU General Public License */ #include "RemoteRobot.h" + #include <boost/foreach.hpp> #include <boost/format.hpp> +#include <boost/algorithm/string/split.hpp> +#include <boost/algorithm/string/classification.hpp> + #include <VirtualRobot/VirtualRobot.h> #include <VirtualRobot/XML/RobotIO.h> #include <VirtualRobot/RobotConfig.h> @@ -38,7 +42,7 @@ #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h> #include <ArmarXCore/core/system/ArmarXDataPath.h> -//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj << endl; +//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj; #define DMES(Obj) ; using namespace VirtualRobot; @@ -47,7 +51,7 @@ using namespace Eigen; namespace armarx { - boost::recursive_mutex RemoteRobot::m; + std::recursive_mutex RemoteRobot::m; RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) : Robot(), @@ -199,12 +203,12 @@ namespace armarx VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, RobotPtr robo) { - boost::recursive_mutex::scoped_lock cloneLock(m); + std::scoped_lock cloneLock(m); static int nonameCounter = 0; if (!remoteNode || !robo) { - ARMARX_ERROR_S << " NULL data " << endl; + ARMARX_ERROR_S << " NULL data "; return VirtualRobot::RobotNodePtr(); } @@ -217,7 +221,7 @@ namespace armarx if (name.empty()) { - ARMARX_LOG_S << "Node without name!!!" << endl; + ARMARX_LOG_S << "Node without name!!!"; std::stringstream ss; ss << "robot_node_" << nonameCounter; nonameCounter++; @@ -285,7 +289,7 @@ namespace armarx break; default: - ARMARX_ERROR_S << "JointType nyi..." << endl; + ARMARX_ERROR_S << "JointType nyi..."; return VirtualRobot::RobotNodePtr(); break; } @@ -308,14 +312,14 @@ namespace armarx /* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase); if (!rnRemote) { - ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i] << endl; + ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i]; continue; }*/ if (!localNode) { - ARMARX_ERROR_S << "Could not create local node: " << children[i] << endl; + ARMARX_ERROR_S << "Could not create local node: " << children[i]; continue; } @@ -329,8 +333,7 @@ namespace armarx VirtualRobot::RobotPtr RemoteRobot::createLocalClone() { - //ARMARX_IMPORTANT_S << "RemoteRobot local clone" << flush; - boost::recursive_mutex::scoped_lock cloneLock(m); + std::scoped_lock cloneLock(m); std::string robotType = getName(); std::string robotName = getName(); VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName, robotType)); @@ -347,7 +350,7 @@ namespace armarx if (!res) { - ARMARX_ERROR_S << "Failed to initialize local robot..." << endl; + ARMARX_ERROR_S << "Failed to initialize local robot..."; return VirtualRobot::RobotPtr(); } @@ -375,24 +378,24 @@ namespace armarx { RobotPtr result; - boost::recursive_mutex::scoped_lock cloneLock(m); - ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl; + std::scoped_lock cloneLock(m); + ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")"; if (!sharedRobotPrx) { - ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..." << endl; + ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..."; return result; } if (filename.empty()) { - RemoteRobotPtr rob(new RemoteRobot(sharedRobotPrx)); + std::shared_ptr<RemoteRobot> rob(new RemoteRobot(sharedRobotPrx)); result = rob->createLocalClone(); if (!result) { - ARMARX_ERROR_S << "Could not create local clone. Aborting..." << endl; + ARMARX_ERROR_S << "Could not create local clone. Aborting..."; return result; } } @@ -429,7 +432,7 @@ namespace armarx if (!result) { - ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl; + ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..."; return result; } } @@ -460,12 +463,12 @@ namespace armarx { if (!robot) { - ARMARX_ERROR << "Robot is NULL! Aborting..." << endl; + ARMARX_ERROR << "Robot is NULL! Aborting..."; return false; } if (!sharedRobotPrx) { - ARMARX_ERROR_S << "shared robot prx is NULL! Aborting..." << endl; + ARMARX_ERROR_S << "shared robot prx is NULL! Aborting..."; return false; } PoseBasePtr globalPose; @@ -480,7 +483,7 @@ namespace armarx if (!c->setConfig(jointName, jointAngle)) { - ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl; + ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..."; } } @@ -518,7 +521,7 @@ namespace armarx if (!c->setConfig(jointName, jointAngle)) { - ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl; + ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..."; } } diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h index 815d81841a7b09abb3650da0ce98fcd8b357a700..bdcec086f7caada926363468166bd7ff854346b6 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h +++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h @@ -37,17 +37,8 @@ #include <VirtualRobot/Nodes/RobotNodeFixed.h> #include <VirtualRobot/XML/RobotIO.h> #include <VirtualRobot/CollisionDetection/CollisionChecker.h> -// boost -#include <boost/thread/mutex.hpp> -#include <boost/utility/enable_if.hpp> -#include <boost/type_traits/is_base_of.hpp> - -//namespace VirtualRobot -//{ -// class RobotNodeRevolute; -// class RobotNodePrismatic; -// class RobotNodeFixed; -//} + +#include <mutex> namespace armarx { @@ -150,13 +141,10 @@ namespace armarx */ class RemoteRobot : public VirtualRobot::Robot { - template<class T> - friend void boost::checked_delete(T*); - protected: - ~RemoteRobot() override; public: RemoteRobot(SharedRobotInterfacePrx robot); + ~RemoteRobot() override; VirtualRobot::RobotNodePtr getRootNode() const override; @@ -270,12 +258,12 @@ namespace armarx mutable std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes; mutable VirtualRobot::RobotNodePtr _root; - static boost::recursive_mutex m; + static std::recursive_mutex m; static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr); }; - using RemoteRobotPtr = boost::shared_ptr<RemoteRobot>; + //using RemoteRobotPtr = boost::shared_ptr<RemoteRobot>; } #endif diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h index e8c1146821c153ab5767805431e2a8c8a9d53163..b604011aec6843d69bc58a1cd8e0b335345344d0 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h +++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h @@ -31,15 +31,9 @@ #include <ArmarXCore/observers/ConditionCheck.h> #include <ArmarXCore/observers/Observer.h> -#include <string> +#include <VirtualRobot/VirtualRobot.h> -namespace VirtualRobot -{ - class Robot; - using RobotPtr = boost::shared_ptr<Robot>; - class RobotNode; - using RobotNodePtr = boost::shared_ptr<RobotNode>; -} +#include <string> namespace armarx {