diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 2a625cb02b9e3c0feb0bad0c8132737ea6c1d06a..73a75e56fe8083732813634c63148ca54b1c3f1d 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 5bce249a787e6dc1a26cd4048adc6027bbaa4db0..aba4d1e828a5b55b9165fb6681ca291f4d2bb5fb 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 e1cfd15d655ebce8ba65d49f58771f8ae5acbeeb..3bd23cdb3401eb739ce506bf3cb6c58ab12ec4dd 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 68a21112e18da9b40ea282a899b5802d003279eb..3ec9cf62b1f5b71f0213ba3bbadb44860ea7787d 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 aeb8e8ce8f414bac77ec83e9e9237679b76873c8..a948379260a7f4cce2049320d5a59de5bc97c0b9 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()