From 8f91e5eded1909cd1372fcaa896c124cb8487c0a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 3 Mar 2021 18:13:00 +0100
Subject: [PATCH] new types: PoseStamped and VelocityStamped
---
.../RobotState/RobotStateComponent.cpp | 27 +++------
.../RobotState/RobotStateComponent.h | 11 ++--
.../components/units/PlatformUnit.cpp | 2 +-
.../components/units/PlatformUnitObserver.cpp | 20 ++++---
.../components/units/PlatformUnitObserver.h | 10 ++--
.../units/PlatformUnitSimulation.cpp | 28 ++++++---
...onomicPlatformGlobalPositionController.cpp | 12 ++--
...olonomicPlatformGlobalPositionController.h | 2 +-
.../interface/units/PlatformUnitInterface.ice | 58 +++++++++++++++----
source/RobotAPI/libraries/core/FramedPose.h | 2 +
10 files changed, 110 insertions(+), 62 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 652d51e31..2a625cb02 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -38,6 +38,8 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+
using namespace Eigen;
using namespace Ice;
@@ -371,16 +373,10 @@ namespace armarx
}
- void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&)
+ void RobotStateComponent::reportPlatformPose(const PoseStampedPtr& platformPose, 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);
- insertPose(time, globalPose);
+ const IceUtil::Time time = IceUtil::Time::microSeconds(platformPose->timestampInMicroSeconds);
+ insertPose(time, platformPose->pose);
if (_sharedRobotServant)
{
@@ -389,28 +385,21 @@ namespace armarx
}
- void RobotStateComponent::reportNewTargetPose(
- Float newPlatformPositionX, Float newPlatformPositionY, Float newPlatformRotation,
- const Current&)
+ void RobotStateComponent::reportNewTargetPose(const PoseStampedPtr&, const Current&)
{
// Unused.
- (void) newPlatformPositionX, (void) newPlatformPositionY, (void) newPlatformRotation;
}
- void RobotStateComponent::reportPlatformVelocity(
- Float currentPlatformVelocityX, Float currentPlatformVelocityY, Float currentPlatformVelocityRotation,
- const Current&)
+ void RobotStateComponent::reportPlatformVelocity(const VelocityStampedPtr&, const Current&)
{
// Unused.
- (void) currentPlatformVelocityX, (void) currentPlatformVelocityY, (void) currentPlatformVelocityRotation;
}
- void RobotStateComponent::reportPlatformOdometryPose(Float x, Float y, Float angle, const Current&)
+ void RobotStateComponent::reportPlatformOdometryPose(const PoseStampedPtr&, const Current&)
{
// Unused.
- (void) x, (void) y, (void) angle;
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 517ed1a84..5bce249a7 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -31,8 +31,9 @@
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <RobotAPI/libraries/core/remoterobot/RobotStateObserver.h>
#include "SharedRobotServants.h"
@@ -126,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 PlatformPose& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportPlatformPose(const PoseStampedPtr& platformPose, const Ice::Current&) override;
/// Does nothing.
- void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportNewTargetPose(const PoseStampedPtr& targetPose, const Ice::Current&) override;
/// Does nothing.
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportPlatformVelocity(const VelocityStampedPtr& platformPose, const Ice::Current&) override;
/// Does nothing.
- void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportPlatformOdometryPose(const PoseStampedPtr& odometryPose, const Ice::Current&) override;
// Own interface.
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index 7afa0eefd..ae00d8967 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -51,7 +51,7 @@ namespace armarx
void PlatformUnit::onDisconnectComponent()
{
- listenerPrx = NULL;
+ listenerPrx = nullptr;
this->onStopPlatformUnit();
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index d0e0cd8e0..9a7cb5fbb 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -82,23 +82,24 @@ namespace armarx
offerDataField("platformOdometryPose", "rotation", VariantType::Float, "Current Odometry Rotation of " + platformNodeName + " in radian");
// odometry pose is always zero in the beginning - set it so that it can be queried
- reportPlatformOdometryPose(0, 0, 0, Ice::emptyCurrent);
+ reportPlatformOdometryPose( ?? ?, Ice::emptyCurrent);
}
- void PlatformUnitObserver::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+
+ void PlatformUnitObserver::reportPlatformPose(const PoseStampedPtr& platformPose, const Ice::Current& c)
{
nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
updateChannel("platformPose");
}
- void PlatformUnitObserver::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
+ void PlatformUnitObserver::reportNewTargetPose(const PoseStampedPtr& targetPose, const Ice::Current& c)
{
nameValueMapToDataFields("targetPose", newPlatformPositionX, newPlatformPositionY, newPlatformRotation);
updateChannel("targetPose");
}
- void PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+ void PlatformUnitObserver::reportPlatformVelocity(const VelocityStampedPtr& platfromVelocity, const Ice::Current& c)
{
setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
@@ -106,6 +107,12 @@ namespace armarx
updateChannel("platformVelocity");
}
+ void armarx::PlatformUnitObserver::reportPlatformOdometryPose(const PoseStampedPtr& odomPose, const Ice::Current&)
+ {
+ nameValueMapToDataFields("platformOdometryPose", x, y, angle);
+ updateChannel("platformOdometryPose");
+ }
+
// ********************************************************************
// private methods
// ********************************************************************
@@ -123,9 +130,4 @@ namespace armarx
}
- void armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&)
- {
- nameValueMapToDataFields("platformOdometryPose", x, y, angle);
- updateChannel("platformOdometryPose");
- }
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 66b24989d..5c72ad09f 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -77,11 +77,11 @@ namespace armarx
void onInitObserver() override;
void onConnectObserver() 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 x, Ice::Float y, Ice::Float angle, const Ice::Current&) override;
+ // PlatformUnitInterface implementation
+ void reportPlatformPose(const PoseStampedPtr& platformPose, const Ice::Current& c) override;
+ void reportNewTargetPose(const PoseStampedPtr& targetPose, const Ice::Current& c) override;
+ void reportPlatformVelocity(const VelocityStampedPtr& platformPose, const Ice::Current& c) override;
+ void reportPlatformOdometryPose(const PoseStampedPtr& odometryPose, const Ice::Current& c) override;
std::string getDefaultName() const override
{
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 93b61e1d4..13ad38088 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -163,14 +163,26 @@ namespace armarx
break;
}
}
- PlatformPose currentPose;
- // FIXME: Take the timestamp from simulation
- currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- currentPose.x = currentPositionX;
- currentPose.y = currentPositionY;
- currentPose.rotationAroundZ = currentRotation;
- listenerPrx->reportPlatformPose(currentPose);
- listenerPrx->reportPlatformVelocity(vel[0], vel[1], vel[2]);
+
+ auto fillCommonPlatformFields = [&](FramedAndStamped & framedAndStamped)
+ {
+ platformPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ platformPose.parentFrame = GlobalFrame;
+ platformPose.childFrame = robotFrame;
+ };
+
+ PoseStamped platformPose;
+ fillCommonPlatformFields(platformPose);
+ // FIXME(fabian.reister): check if order is correct
+ platformPose.pose = (Eigen::Translation3f(currentPositionX, currentPositionY, 0.F) * Eigen::AngleAxisf(currentRotation, Eigen::Vector3f::UnitZ())).matrix();
+
+ VelocityStamped platformVelocity;
+ fillCommonPlatformFields(platformVelocity);
+ platformVelocity.velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
+
+
+ listenerPrx->reportPlatformPose(platformPose);
+ listenerPrx->reportPlatformVelocity(platformVelocity);
}
void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index bc246422b..3c4b3812f 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -120,15 +120,19 @@ namespace armarx
}
- void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
+ void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PoseStamped& currentPose)
{
// ..todo: check if norm is too large
- getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
- getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
+ // TODO(fabian.reister): add check: frame and agent have to "Global" and "robot"
+
+ const float yaw = simox::math::mat4f_2_rpy(currentPose.pose);
+
+ getWriterControlStruct().globalPosition << currentPose.translation().x(), currentPose.translation().y();
+ getWriterControlStruct().globalOrientation = yaw;
getWriterControlStruct().startPosition = currentPosition;
- getWriterControlStruct().startOrientation = currentOrientation;
+ getWriterControlStruct().startOrientation = yaw;
getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
writeControlStruct();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index 59838fb02..e8af657ed 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -108,7 +108,7 @@ namespace armarx
void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
- void setGlobalPos(const PlatformPose& currentPose);
+ void setGlobalPos(const PoseStamped& currentPose);
protected:
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index c4afe4591..28c780552 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -28,6 +28,7 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
module armarx
{
@@ -73,13 +74,50 @@ module armarx
void stopPlatform();
};
- struct PlatformPose
- {
+ // ["cpp:virtual"]
+ // class Framed
+ // {
+ // string frame; // source frame
+ // string agent; // child frame
+
+ // long timestampInMicroSeconds = 0;
+ // };
+
+ // ["cpp:virtual"]
+ // class Pose extends Framed
+ // {
+ // Eigen::Matrix4f pose; // [mm]
+ // };
+
+ // ["cpp:virtual"]
+ // class Stamped
+ // {
+ // };
+
+ ["cpp:virtual"]
+ class FramedAndStamped
+ {
+ string frame; // source frame
+ string agent; // child frame
+
long timestampInMicroSeconds = 0;
- float x = 0.0f;
- float y = 0.0f;
- float rotationAroundZ = 0.0f;
- };
+ }
+
+ ["cpp:virtual"]
+ class PoseStamped extends FramedAndStamped
+ {
+ Eigen::Matrix4f pose; // [mm]
+ }
+
+ ["cpp:virtual"]
+ class VelocityStamped extends FramedAndStamped
+ {
+ /**
+ * Linear (x,y,z) and angular (roll, pitch, yaw) velocities
+ */
+ Eigen::Vector6f velocity; // [mm, rad]
+ }
+
/**
* Implements an interface to an PlatformUnitListener.
@@ -90,23 +128,23 @@ module armarx
* reportPlatformPose reports current platform pose.
* @param currentPose Current global platform pose.
**/
- void reportPlatformPose(PlatformPose currentPose);
+ void reportPlatformPose(PoseStamped currentPose);
/**
* reportNewTargetPose reports target platform pose.
* @param newPlatformPositionX Global x-coordinate of target platform position.
* @param newPlatformPositionY Global y-coordinate of target platform position.
* @param newPlatformRotation Target global orientation of platform.
**/
- void reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation);
+ void reportNewTargetPose(PoseStamped targetPose);
/**
* reportPlatformVelocity reports current platform velocities.
* @param currentPlatformVelocityX x-coordinate of current velocity defined in platform root.
* @param currentPlatformVelocityY y-coordinate of current velocity defined in platform root.
* @param currentPlatformVelocityRotation Current orientational velocity defined in platform root.
**/
- void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation);
+ void reportPlatformVelocity(VelocityStamped platformVelocity);
- void reportPlatformOdometryPose(float x, float y, float angle);
+ void reportPlatformOdometryPose(PoseStamped odometryPose);
};
interface PlatformSubUnitInterface extends PlatformUnitInterface, PlatformUnitListener
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 9a1f92a6e..e76ccb78f 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -57,6 +57,8 @@ namespace armarx
* Variable of the global coordinate system. use this if you are specifying a global pose.
* */
std::string const GlobalFrame = "Global";
+ std::string const OdometryFrame = "Odom";
+ std::string const MapFrame = "Map";
--
GitLab