From 42527262d969eac97e6cb0e16b33d75f70a8244c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 3 Mar 2021 20:07:43 +0100
Subject: [PATCH] fixing platformunitguiplugin
build RobotAPI succeeds
---
.../RobotState/RobotStateComponent.cpp | 12 +++----
.../RobotState/RobotStateComponent.h | 8 ++---
.../components/units/PlatformUnitObserver.cpp | 23 ++++++++----
.../PlatformUnitGuiPlugin.cpp | 36 ++++++++++++-------
.../PlatformUnitGuiPlugin.h | 10 +++---
5 files changed, 55 insertions(+), 34 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 2a625cb02..73a75e56f 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -373,10 +373,10 @@ namespace armarx
}
- void RobotStateComponent::reportPlatformPose(const PoseStampedPtr& platformPose, const Current&)
+ void RobotStateComponent::reportPlatformPose(const PoseStamped& platformPose, const Current&)
{
- const IceUtil::Time time = IceUtil::Time::microSeconds(platformPose->timestampInMicroSeconds);
- insertPose(time, platformPose->pose);
+ const IceUtil::Time time = IceUtil::Time::microSeconds(platformPose.header.timestampInMicroSeconds);
+ insertPose(time, platformPose.pose);
if (_sharedRobotServant)
{
@@ -385,19 +385,19 @@ namespace armarx
}
- void RobotStateComponent::reportNewTargetPose(const PoseStampedPtr&, const Current&)
+ void RobotStateComponent::reportNewTargetPose(const PoseStamped&, const Current&)
{
// Unused.
}
- void RobotStateComponent::reportPlatformVelocity(const VelocityStampedPtr&, const Current&)
+ void RobotStateComponent::reportPlatformVelocity(const VelocityStamped&, const Current&)
{
// Unused.
}
- void RobotStateComponent::reportPlatformOdometryPose(const PoseStampedPtr&, const Current&)
+ void RobotStateComponent::reportPlatformOdometryPose(const PoseStamped&, const Current&)
{
// Unused.
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 5bce249a7..aba4d1e82 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -127,13 +127,13 @@ namespace armarx
// PlatformUnitListener interface
// TODO: Remove this interface and use GlobalRobotPoseLocalizationListener instead.
/// Stores the platform pose in the pose history.
- void reportPlatformPose(const PoseStampedPtr& platformPose, const Ice::Current&) override;
+ void reportPlatformPose(const PoseStamped& platformPose, const Ice::Current&) override;
/// Does nothing.
- void reportNewTargetPose(const PoseStampedPtr& targetPose, const Ice::Current&) override;
+ void reportNewTargetPose(const PoseStamped& targetPose, const Ice::Current&) override;
/// Does nothing.
- void reportPlatformVelocity(const VelocityStampedPtr& platformPose, const Ice::Current&) override;
+ void reportPlatformVelocity(const VelocityStamped& platformPose, const Ice::Current&) override;
/// Does nothing.
- void reportPlatformOdometryPose(const PoseStampedPtr& odometryPose, const Ice::Current&) override;
+ void reportPlatformOdometryPose(const PoseStamped& odometryPose, const Ice::Current&) override;
// Own interface.
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index e1cfd15d6..3bd23cdb3 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -33,6 +33,7 @@
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
namespace armarx
@@ -93,33 +94,41 @@ namespace armarx
reportPlatformOdometryPose(initialOdomPose, Ice::emptyCurrent);
-
}
void PlatformUnitObserver::reportPlatformPose(const PoseStamped& platformPose, const Ice::Current& c)
{
- nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
+ const float yaw = simox::math::mat4f_to_rpy(platformPose.pose).z();
+ const Eigen::Affine3f pose(platformPose.pose);
+
+ nameValueMapToDataFields("platformPose", pose.translation().x(), pose.translation().y(), yaw);
updateChannel("platformPose");
}
void PlatformUnitObserver::reportNewTargetPose(const PoseStamped& targetPose, const Ice::Current& c)
{
- nameValueMapToDataFields("targetPose", newPlatformPositionX, newPlatformPositionY, newPlatformRotation);
+ const float yaw = simox::math::mat4f_to_rpy(targetPose.pose).z();
+ const Eigen::Affine3f pose(targetPose.pose);
+
+ nameValueMapToDataFields("targetPose", pose.translation().x(), pose.translation().y(), yaw);
updateChannel("targetPose");
}
void PlatformUnitObserver::reportPlatformVelocity(const VelocityStamped& platfromVelocity, const Ice::Current& c)
{
- setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
- setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
- setDataField("platformVelocity", "velocityRotation", currentPlatformVelocityRotation);
+ setDataField("platformVelocity", "velocityX", platfromVelocity.velocity.x());
+ setDataField("platformVelocity", "velocityY", platfromVelocity.velocity.y());
+ setDataField("platformVelocity", "velocityRotation", platfromVelocity.velocity(5));
updateChannel("platformVelocity");
}
void armarx::PlatformUnitObserver::reportPlatformOdometryPose(const PoseStamped& odomPose, const Ice::Current&)
{
- nameValueMapToDataFields("platformOdometryPose", x, y, angle);
+ const float yaw = simox::math::mat4f_to_rpy(odomPose.pose).z();
+ const Eigen::Affine3f pose(odomPose.pose);
+
+ nameValueMapToDataFields("platformOdometryPose", pose.translation().x(), pose.translation().y(), yaw);
updateChannel("platformOdometryPose");
}
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 68a21112e..3ec9cf62b 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -38,6 +38,9 @@
#include <memory>
#include <cmath>
+// Simox
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
using namespace armarx;
@@ -198,27 +201,40 @@ void PlatformUnitWidget::stopControlTimer()
rotaCtrl->setNibble({0, 0});
}
-void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+void PlatformUnitWidget::reportPlatformPose(const PoseStamped& currentPose, const Ice::Current& c)
{
+ const float yaw = simox::math::mat4f_to_rpy(currentPose.pose).z();
+ const Eigen::Affine3f pose(currentPose.pose);
+
// moved to qt thread for thread safety
- QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPose.x), Q_ARG(float, currentPose.y), Q_ARG(float, currentPose.rotationAroundZ));
- platformRotation = currentPose.rotationAroundZ;
+ QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, pose.translation().x()), Q_ARG(float, pose.translation().y()), Q_ARG(float, yaw));
+ platformRotation = yaw;
}
-void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
+void PlatformUnitWidget::reportNewTargetPose(const PoseStamped& targetPlatformPose, const Ice::Current& c)
{
+ const float yaw = simox::math::mat4f_to_rpy(targetPlatformPose.pose).z();
+ const Eigen::Affine3f pose(targetPlatformPose.pose);
+
+
// moved to qt thread for thread safety
QMetaObject::invokeMethod(this, "setNewTargetPoseLabels",
- Q_ARG(float, newPlatformPositionX),
- Q_ARG(float, newPlatformPositionY),
- Q_ARG(float, newPlatformRotation));
+ Q_ARG(float, pose.translation().x()),
+ Q_ARG(float, pose.translation().y()),
+ Q_ARG(float, yaw));
}
-void PlatformUnitWidget::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+void PlatformUnitWidget::reportPlatformVelocity(const VelocityStamped& platformVelocity, const Ice::Current& c)
{
}
+void armarx::PlatformUnitWidget::reportPlatformOdometryPose(const PoseStamped&, const Ice::Current&)
+{
+ // ignore for now
+}
+
+
void PlatformUnitWidget::stopPlatform()
{
platformUnitProxy->stopPlatform();
@@ -392,7 +408,3 @@ void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
QWidget::keyReleaseEvent(event);
}
-void armarx::PlatformUnitWidget::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
- // ignore for now
-}
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index aeb8e8ce8..a94837926 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -115,11 +115,11 @@ namespace armarx
void onDisconnectComponent() override;
void onExitComponent() override;
- // slice interface implementation
- void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
+ // PlatformUnitInterface implementation
+ void reportPlatformPose(const PoseStamped& platformPose, const Ice::Current& c) override;
+ void reportNewTargetPose(const PoseStamped& targetPose, const Ice::Current& c) override;
+ void reportPlatformVelocity(const VelocityStamped& platformVelocity, const Ice::Current& c) override;
+ void reportPlatformOdometryPose(const PoseStamped& odometryPose, const Ice::Current& c) override;
// inherited of ArmarXWidget
static QString GetWidgetName()
--
GitLab