diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp index 3d51ac4aa003553fa4ea9a5994951904001c29c2..94b1deab94ab62fa3707552fea3ef92560ecba67 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp @@ -692,5 +692,24 @@ namespace armarx } + // legacy + void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&) + { + const float z = 0; + const Eigen::Vector3f position(currentPose.x, currentPose.y, z); + const Eigen::Matrix3f orientation = + Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix(); + const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation); + + IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds); + insertPose(time, globalPose); + + if (_sharedRobotServant) + { + _sharedRobotServant->setTimestamp(time); + } + } + + } diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h index 881e97ddc17fc75f45a64e2048c620c17beee40f..685567a476bd7ecfd8d97981fa2cc205d53c43bd 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.h +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h @@ -128,6 +128,20 @@ namespace armarx void setRobotStateObserver(RobotStateObserverPtr observer); + + // PlatformUnitListener interface + // TODO: Remove this interface and use GlobalRobotPoseLocalizationListener only. + /// Stores the platform pose in the pose history. + void reportPlatformPose(const PlatformPose& currentPose, const Ice::Current& = Ice::emptyCurrent) override; + /// Does nothing. + void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& = Ice::emptyCurrent) override {} + /// Does nothing. + void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& = Ice::emptyCurrent) override {} + /// Does nothing. + void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current& = Ice::emptyCurrent) override {} + + + protected: // Component interface. diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice index 467b5c9552928a82c899642f2958b5c863d91993..a80b9b22c09d331e30f726dfb751c45543319fc5 100644 --- a/source/RobotAPI/interface/core/RobotState.ice +++ b/source/RobotAPI/interface/core/RobotState.ice @@ -188,6 +188,7 @@ module armarx */ interface RobotStateComponentInterface extends KinematicUnitListener, + PlatformUnitListener, GlobalRobotPoseLocalizationListener, SimulatorResetEvent {