From 6f836688b2f6ac1723a744d4c22889c4793e62f4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 16 Mar 2021 16:42:21 +0100
Subject: [PATCH] robot state component: using new interface
---
.../RobotState/RobotStateComponent.cpp | 34 ++-----------------
.../RobotState/RobotStateComponent.h | 14 +-------
2 files changed, 3 insertions(+), 45 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 9f04172c5..da872952c 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 aba4d1e82..881e97ddc 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);
--
GitLab