diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp index 4d4d2399bb3ee5059d9788fb60f61cf23f78c2b0..a17005e6d573ff98accd7b40e9c45376ed8e85c4 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp @@ -38,8 +38,6 @@ #include <ArmarXCore/core/system/ArmarXDataPath.h> #include <ArmarXCore/core/time/TimeUtil.h> -#include <RobotAPI/interface/units/PlatformUnitInterface.h> - using namespace Eigen; using namespace Ice; @@ -72,7 +70,6 @@ namespace armarx defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history").setMin(0); defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model"); defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name."); - defineOptionalProperty<std::string>("PlatformTopicName", "PlatformState", "Topic where platform state is published."); defineOptionalProperty<std::string>("GlobalRobotPoseLocalizationTopicName", "GlobalRobotPoseLocalization", "Topic where the global robot pose can be reported."); } @@ -149,7 +146,6 @@ namespace armarx usingTopic(topicPrefix + robotNodeSetName + "State"); usingTopic(topicPrefix + getProperty<std::string>("GlobalRobotPoseLocalizationTopicName").getValue()); - usingTopic(topicPrefix + getProperty<std::string>("PlatformTopicName").getValue()); try { @@ -692,39 +688,4 @@ namespace armarx return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")}; } - - // 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); - // ARMARX_IMPORTANT << VAROUT(currentPose.timestampInMicroSeconds); - - TransformStamped stamped; - stamped.header.frame = armarx::GlobalFrame; - stamped.header.agent = _synchronized->getName(); - stamped.header.timestampInMicroSeconds = time.toMicroSeconds(); - stamped.header.parentFrame = ""; - stamped.transform = globalPose; - - this->reportGlobalRobotPose(stamped); - - /* - * old: - 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 6a3b527630a594e5a12ed53a03690fa1a48cde0e..d786dd0d67efc4b5e8fe07e02bef4f4b370588da 100644 --- a/source/RobotAPI/components/RobotState/RobotStateComponent.h +++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h @@ -128,20 +128,6 @@ 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. @@ -239,4 +225,3 @@ namespace armarx }; } -