diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp index 94109774002f45404c6f46b26708de87882d5e60..6488c07c182a94dcdcf1f50aa4ff9d1038ef6284 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp @@ -131,7 +131,7 @@ namespace armarx ARMARX_VERBOSE << "Node: " << node->getName() << endl; } }*/ - _sharedRobotServant = new SharedRobotServant(this->_synchronized); + _sharedRobotServant = new SharedRobotServant(this->_synchronized, RobotStateComponentInterfacePrx::uncheckedCast(getProxy())); _sharedRobotServant->ref(); } @@ -172,7 +172,7 @@ namespace armarx auto clone = this->_synchronized->clone(_synchronized->getName()); - SharedRobotServantPtr p = new SharedRobotServant(clone); + SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy())); p->setTimestamp(IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp)); auto result = getObjectAdapter()->addWithUUID(p); // virtal robot clone is buggy -> set global pose here @@ -197,7 +197,7 @@ namespace armarx clone->setJointValues(config.jointMap); clone->setGlobalPose(FramedPosePtr::dynamicCast(config.globalPose)->toEigen()); - SharedRobotServantPtr p = new SharedRobotServant(clone); + SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy())); p->setTimestamp(IceUtil::Time::microSecondsDouble(time)); auto result = getObjectAdapter()->addWithUUID(p); // virtal robot clone is buggy -> set global pose here diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp index 38ead5c0fb93347009670fa4653dc1c52bdab4db..e1fa94fd5708ba936f3f23128848adace2d5b5a0 100644 --- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp +++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp @@ -15,9 +15,9 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. * - * @package - * @author - * @date + * @package + * @author + * @date * @copyright http://www.gnu.org/licenses/gpl-2.0.txt * GNU General Public License */ @@ -282,8 +282,9 @@ namespace armarx // SharedRobotServant /////////////////////////////// - SharedRobotServant::SharedRobotServant(RobotPtr robot) - : _robot(robot) + SharedRobotServant::SharedRobotServant(RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent) + : _robot(robot), + robotStateComponent(robotStateComponent) { #ifdef VERBOSE ARMARX_WARNING_S << "construct " << this << flush; @@ -307,6 +308,11 @@ namespace armarx } } + void SharedRobotServant::setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent) + { + this->robotStateComponent = robotStateComponent; + } + SharedRobotNodeInterfacePrx SharedRobotServant::getRobotNode(const string& name, const Current& current) { // ARMARX_LOG_S << "Looking for node: " << name << flush; @@ -445,6 +451,11 @@ namespace armarx return new TimestampVariant(updateTimestamp); } + RobotStateComponentInterfacePrx SharedRobotServant::getRobotStateComponent(const Current&) const + { + return robotStateComponent; + } + void SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr& pose, const Current&) { WriteLockPtr lock = _robot->getWriteLock(); diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h index e8ec1fd3e390547c5a605894b14a87d3742d3e7e..3eabd92556dc2c501f3ebbca1f309fca94a401dc 100644 --- a/source/RobotAPI/components/RobotState/SharedRobotServants.h +++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h @@ -15,9 +15,9 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. * - * @package - * @author - * @date + * @package + * @author + * @date * @copyright http://www.gnu.org/licenses/gpl-2.0.txt * GNU General Public License */ @@ -72,7 +72,7 @@ namespace armarx */ class SharedRobotNodeServant : virtual public SharedRobotNodeInterface, - public SharedObjectBase + public SharedObjectBase { public: SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::Current()*/); @@ -117,9 +117,9 @@ namespace armarx public SharedObjectBase { public: - SharedRobotServant(VirtualRobot::RobotPtr robot); + SharedRobotServant(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent); ~SharedRobotServant(); - + void setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent); virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string& name, const Ice::Current& current = Ice::Current()); virtual SharedRobotNodeInterfacePrx getRootNode(const Ice::Current& current = Ice::Current()); virtual bool hasRobotNode(const std::string& name, const Ice::Current& current = Ice::Current()); @@ -140,11 +140,14 @@ namespace armarx void setTimestamp(const IceUtil::Time& updateTime); TimestampBasePtr getTimestamp(const Ice::Current& = Ice::Current()) const; + RobotStateComponentInterfacePrx getRobotStateComponent(const Ice::Current&) const; protected: VirtualRobot::RobotPtr _robot; boost::recursive_mutex m; std::map<std::string, SharedRobotNodeInterfacePrx> _cachedNodes; IceUtil::Time updateTimestamp; + RobotStateComponentInterfacePrx robotStateComponent; + }; typedef IceInternal::Handle<SharedRobotServant> SharedRobotServantPtr; diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice index 936fd47c8e5bc433d50ee06bbcd90ee454485f4c..e010537604e34d0b714473a91f271deed56326e2 100644 --- a/source/RobotAPI/interface/core/RobotState.ice +++ b/source/RobotAPI/interface/core/RobotState.ice @@ -118,12 +118,20 @@ module armarx }; + interface RobotStateComponentInterface; + /** * The SharedRobotInterface provides access to a limited amount of * VirtualRobot::Robot methods over the Ice network. */ interface SharedRobotInterface extends SharedObjectInterface { + /** + * @return returns the RobotStateComponent this robot belongs to + */ + ["cpp:const"] idempotent + RobotStateComponentInterface* getRobotStateComponent(); + SharedRobotNodeInterface* getRobotNode(string name); SharedRobotNodeInterface* getRootNode(); bool hasRobotNode(string name); @@ -143,6 +151,7 @@ module armarx void setGlobalPose(PoseBase globalPose); PoseBase getGlobalPose(); NameValueMap getConfig(); + };