diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp index 9f04172c54061d70f6b1412c060a0cbfd5390e15..da872952c095ddca5d32a12b55e85952470432af 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp @@ -344,9 +344,7 @@ namespace armarx void RobotStateComponent::reportGlobalRobotPose( - const std::string& robotName, - const Eigen::Matrix4f& pose, - const long timestamp, + const TransformStamped& globalRobotPose, const Ice::Current&) { if (_synchronized) @@ -355,7 +353,7 @@ namespace armarx ARMARX_DEBUG << "Comparing " << localRobotName << " and " << robotName << "."; if (localRobotName == robotName) { - const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp); + const IceUtil::Time time = IceUtil::Time::microSeconds(globalRobotPose.header.timestampInMicroSeconds); insertPose(time, pose); _synchronized->setGlobalPose(pose); @@ -373,34 +371,6 @@ namespace armarx } - void RobotStateComponent::reportPlatformPose(const PoseStamped& platformPose, const Current&) - { - IceUtil::Time time = IceUtil::Time::microSeconds(platformPose.header.timestampInMicroSeconds); - insertPose(time, platformPose.pose.matrix()); - - if (_sharedRobotServant) - { - _sharedRobotServant->setTimestamp(time); - } - } - - - void RobotStateComponent::reportNewTargetPose(const PoseStamped&, const Current&) - { - // Unused. - } - - void RobotStateComponent::reportPlatformVelocity(const VelocityStamped&, const Current&) - { - // Unused. - } - - void RobotStateComponent::reportPlatformOdometryPose(const PoseStamped&, const Current&) - { - // Unused. - } - - std::vector<std::string> RobotStateComponent::getArmarXPackages(const Current&) const { std::vector<std::string> result; diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h index aba4d1e828a5b55b9165fb6681ca291f4d2bb5fb..881e97ddc17fc75f45a64e2048c620c17beee40f 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.h +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h @@ -122,19 +122,7 @@ namespace armarx RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override; // GlobalRobotPoseLocalizationListener - void reportGlobalRobotPose(const std::string& robotName, const Eigen::Matrix4f& pose, long timestamp, const Ice::Current& = Ice::Current()) override; - - // PlatformUnitListener interface - // TODO: Remove this interface and use GlobalRobotPoseLocalizationListener instead. - /// Stores the platform pose in the pose history. - void reportPlatformPose(const PoseStamped& platformPose, const Ice::Current&) override; - /// Does nothing. - void reportNewTargetPose(const PoseStamped& targetPose, const Ice::Current&) override; - /// Does nothing. - void reportPlatformVelocity(const VelocityStamped& platformPose, const Ice::Current&) override; - /// Does nothing. - void reportPlatformOdometryPose(const PoseStamped& odometryPose, const Ice::Current&) override; - + void reportGlobalRobotPose(const TransformStamped& globalRobotPose, const Ice::Current& = Ice::Current()) override; // Own interface. void setRobotStateObserver(RobotStateObserverPtr observer);