From 7a5609f9d9c179c39ab212fadf45769ca33c4c0f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 15:48:55 +0100
Subject: [PATCH] LegacyRobotStateMemoryAdapter: now needs
GlobalRobotPoseLocalizationListener
---
.../LegacyRobotStateMemoryAdapter.cpp | 36 +++++++++++++++----
.../LegacyRobotStateMemoryAdapter.h | 5 +--
...LegacyRobotStateMemoryAdapterInterface.ice | 3 +-
3 files changed, 34 insertions(+), 10 deletions(-)
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index b82eefe89..b8d85a279 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -21,6 +21,7 @@
*/
#include "LegacyRobotStateMemoryAdapter.h"
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -211,17 +212,36 @@ namespace armarx::armem
}
- void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
+ // void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
+ // {
+ // ARMARX_DEBUG << "Got an update for platform pose";
+ // std::lock_guard l(updateMutex);
+ // update.platformPose = p;
+ // updateTimestamps(p.timestampInMicroSeconds);
+ // }
+
+ void LegacyRobotStateMemoryAdapter::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
{
+
ARMARX_DEBUG << "Got an update for platform pose";
- std::lock_guard l(updateMutex);
- update.platformPose = p;
- updateTimestamps(p.timestampInMicroSeconds);
- }
- void LegacyRobotStateMemoryAdapter::reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &)
- {
+ const Eigen::Isometry3f global_T_robot(transformStamped.transform);
+ const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
+
+ armarx::PlatformPose p;
+ p.x = global_T_robot.translation().x();
+ p.y = global_T_robot.translation().y();
+ p.rotationAroundZ = yaw;
+ p.timestampInMicroSeconds = transformStamped.header.timestampInMicroSeconds;
+
+ {
+ std::lock_guard l(updateMutex);
+ update.platformPose = p;
+ updateTimestamps(p.timestampInMicroSeconds);
+ }
}
+
+
void LegacyRobotStateMemoryAdapter::reportPlatformVelocity(Ice::Float f1, Ice::Float f2, Ice::Float f3, const Ice::Current &)
{
ARMARX_DEBUG << "Got an update for platform vels";
@@ -240,6 +260,8 @@ namespace armarx::armem
update.platformOdometryPose = {f1, f2, f3};
updateTimestamps(now);
}
+
+
void LegacyRobotStateMemoryAdapter::commitArmar3RobotDescription()
{
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
index 9680d8c27..71ab81f1a 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
@@ -59,11 +59,12 @@ namespace armarx::armem
void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportPlatformPose(const PlatformPose &, const Ice::Current &) override;
- void reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ // void reportPlatformPose(const PlatformPose &, const Ice::Current &) override;
void reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
index ffd72e20c..baad0e798 100644
--- a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
+++ b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
@@ -2,6 +2,7 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
module armarx
{
@@ -9,7 +10,7 @@ module armarx
{
module robot_state
{
- interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener, armarx::PlatformUnitListener
+ interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener, armarx::PlatformUnitListener, armarx::GlobalRobotPoseLocalizationListener
{
}
}
--
GitLab