diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp index 44f79520f9de6fefb25e46915b49b1b5b88cffff..2812e0782a59fad31cbccd09dc3b617888a8253a 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp +++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp @@ -293,20 +293,25 @@ VirtualRobot::RobotPtr RemoteRobot::createLocalClone() } VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string &filename) +{ + return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename); +} + +RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string &filename) { boost::recursive_mutex::scoped_lock cloneLock(m); ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl; VirtualRobot::RobotPtr result; - if (!robotStatePrx) + if (!sharedRobotPrx) { - ARMARX_ERROR_S << "NULL robotStatePrx. Aborting..." << endl; + ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..." << endl; return result; } if (filename.empty()) { - RemoteRobotPtr rob(new RemoteRobot(robotStatePrx->getSynchronizedRobot())); + RemoteRobotPtr rob(new RemoteRobot(sharedRobotPrx)); result = rob->createLocalClone(); if (!result) { @@ -322,20 +327,24 @@ VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfac return result; } } - synchronizeLocalClone(result,robotStatePrx); + synchronizeLocalClone(result,sharedRobotPrx); return result; } bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx) { - if (!robotStatePrx || !robot) + return synchronizeLocalClone(robot, robotStatePrx->getSynchronizedRobot()); +} + +bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx) +{ + if (!sharedRobotPrx || !robot) { ARMARX_ERROR_S << "NULL data. Aborting..." << endl; return false; } RobotConfigPtr c(new RobotConfig(robot,"synchronizeLocalClone")); - auto robotProxy = robotStatePrx->getSynchronizedRobot(); - NameValueMap jv = robotProxy->getConfig(); + NameValueMap jv = sharedRobotPrx->getConfig(); for ( NameValueMap::const_iterator it = jv.begin(); it!=jv.end(); it++ ) { // joint values @@ -347,7 +356,7 @@ bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotState } } robot->setConfig(c); - auto pose = PosePtr::dynamicCast(robotProxy->getGlobalPose()); + auto pose = PosePtr::dynamicCast(sharedRobotPrx->getGlobalPose()); robot->setGlobalPose(pose->toEigen()); return true; } diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h index fd7af89c404901bd64c3ce7358f74fc3ecf69a22..e5573185978d87565ff606285651844e22bed391 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h +++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h @@ -184,16 +184,21 @@ namespace armarx */ static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string &filename=std::string()); + static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string &filename=std::string()); + /*! Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone. The local clone must have the identical structure as the remote robot model, otherwise an error will be reported. */ static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx); + static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx); + // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging //! Clones the structure of this remote robot to a local instance VirtualRobot::RobotPtr createLocalClone(); + protected: /// Not implemented yet