diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 652d51e31ea4343e8977c96df0b5c61eb4525e53..3d51ac4aa003553fa4ea9a5994951904001c29c2 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;
@@ -342,21 +344,19 @@ namespace armarx
void
RobotStateComponent::reportGlobalRobotPose(
- const std::string& robotName,
- const Eigen::Matrix4f& pose,
- const long timestamp,
+ const TransformStamped& globalRobotPose,
const Ice::Current&)
{
if (_synchronized)
{
std::string localRobotName = _synchronized->getName();
- ARMARX_DEBUG << "Comparing " << localRobotName << " and " << robotName << ".";
- if (localRobotName == robotName)
+ ARMARX_DEBUG << "Comparing " << localRobotName << " and " << globalRobotPose.header.agent << ".";
+ if (localRobotName == globalRobotPose.header.agent)
{
- const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+ const IceUtil::Time time = IceUtil::Time::microSeconds(globalRobotPose.header.timestampInMicroSeconds);
- insertPose(time, pose);
- _synchronized->setGlobalPose(pose);
+ insertPose(time, globalRobotPose.transform);
+ _synchronized->setGlobalPose(globalRobotPose.transform);
if (_sharedRobotServant)
{
@@ -371,49 +371,6 @@ namespace armarx
}
- void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, 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);
-
- if (_sharedRobotServant)
- {
- _sharedRobotServant->setTimestamp(time);
- }
- }
-
-
- void RobotStateComponent::reportNewTargetPose(
- Float newPlatformPositionX, Float newPlatformPositionY, Float newPlatformRotation,
- const Current&)
- {
- // Unused.
- (void) newPlatformPositionX, (void) newPlatformPositionY, (void) newPlatformRotation;
- }
-
-
- void RobotStateComponent::reportPlatformVelocity(
- Float currentPlatformVelocityX, Float currentPlatformVelocityY, Float currentPlatformVelocityRotation,
- const Current&)
- {
- // Unused.
- (void) currentPlatformVelocityX, (void) currentPlatformVelocityY, (void) currentPlatformVelocityRotation;
- }
-
-
- void RobotStateComponent::reportPlatformOdometryPose(Float x, Float y, Float angle, const Current&)
- {
- // Unused.
- (void) x, (void) y, (void) angle;
- }
-
-
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 517ed1a840e81837f4214e5b089460ca72757b64..881e97ddc17fc75f45a64e2048c620c17beee40f 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"
@@ -121,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 PlatformPose& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
- /// Does nothing.
- void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& = Ice::emptyCurrent) override;
- /// Does nothing.
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& = Ice::emptyCurrent) override;
- /// Does nothing.
- void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current& = Ice::emptyCurrent) override;
-
+ void reportGlobalRobotPose(const TransformStamped& globalRobotPose, const Ice::Current& = Ice::Current()) override;
// Own interface.
void setRobotStateObserver(RobotStateObserverPtr observer);
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index dcf0ebd846beb390d2bb1c29e511e14555e1cb42..59625f6aa98261c2f24c220a440a0225967a5974 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -854,7 +854,8 @@ armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
armarx::PropertyDefinitionsPtr
armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
{
- PropertyDefinitionsPtr def{new PlatformUnitPropertyDefinitions{getConfigIdentifier()}};
+ PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
+ def->setPrefix(getConfigIdentifier());
def->component(m_platform, "Platform");
def->component(m_obstacle_avoidance, "PlatformObstacleAvoidance");
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index 7afa0eefda2aa9201ed42c042c9e9543498b19e1..5f026957d977c4894264f06634d1b8ce036a1bde 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -25,34 +25,41 @@
#include "PlatformUnit.h"
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
namespace armarx
{
- void PlatformUnit::onInitComponent()
+ PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
{
- std::string platformName = getProperty<std::string>("PlatformName").getValue();
+ armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
+ def->topic(odometryPrx);
+ def->topic(globalPosePrx);
- // component dependencies
- listenerChannelName = platformName + "State";
- offeringTopic(listenerChannelName);
+ // legacy
+ // defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform (will publish values on PlatformName + 'State')");
+ def->topic(listenerPrx, "PlatformState");
+ return def;
+ }
+
+
+ void PlatformUnit::onInitComponent()
+ {
this->onInitPlatformUnit();
}
void PlatformUnit::onConnectComponent()
{
- ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
- listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
-
this->onStartPlatformUnit();
}
void PlatformUnit::onDisconnectComponent()
{
- listenerPrx = NULL;
-
this->onStopPlatformUnit();
}
@@ -67,10 +74,18 @@ namespace armarx
{
}
-
- PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
+ PlatformPose toPlatformPose(const TransformStamped& transformStamped)
{
- return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
- getConfigIdentifier()));
+ const float yaw = simox::math::mat4f_to_rpy(transformStamped.transform).z();
+ const Eigen::Affine3f pose(transformStamped.transform);
+
+ PlatformPose platformPose;
+ platformPose.x = pose.translation().x();
+ platformPose.y = pose.translation().y();
+ platformPose.rotationAroundZ = yaw;
+ platformPose.timestampInMicroSeconds = transformStamped.header.timestampInMicroSeconds;
+
+ return platformPose;
}
-}
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/PlatformUnit.h b/source/RobotAPI/components/units/PlatformUnit.h
index 5bedf16ac7a782f2c6c17d4fad32792d3700e99f..23f5f14fc2b0b842ffc08ba622b2717414528ae3 100644
--- a/source/RobotAPI/components/units/PlatformUnit.h
+++ b/source/RobotAPI/components/units/PlatformUnit.h
@@ -35,20 +35,6 @@
namespace armarx
{
- /**
- * \class PlatformUnitPropertyDefinitions
- * \brief Defines all necessary properties for armarx::PlatformUnit
- */
- class PlatformUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
- {
- public:
- PlatformUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform (will publish values on PlatformName + 'State')");
- }
- };
/**
@@ -118,10 +104,12 @@ namespace armarx
* PlatformUnitListener proxy for publishing state updates
*/
PlatformUnitListenerPrx listenerPrx;
- /**
- * Ice Topic name on which armarx::PlatformUnit::listenerPrx publishes information
- */
- std::string listenerChannelName;
+
+ GlobalRobotPoseLocalizationListenerPrx globalPosePrx;
+ OdometryListenerPrx odometryPrx;
+
};
+
+ PlatformPose toPlatformPose(const TransformStamped& transformStamped);
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 93b61e1d4f7338e888e55e0427e203a89843f731..f9beaced057488d00bc05de630b206c138953eff 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -24,14 +24,43 @@
#include "PlatformUnitSimulation.h"
+#include "RobotAPI/components/units/PlatformUnit.h"
+
+#include <RobotAPI/interface/core/GeometryBase.h>
+#include <cmath>
#include <Eigen/Geometry>
-#include <VirtualRobot/MathTools.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <cmath>
+
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include <VirtualRobot/MathTools.h>
namespace armarx
{
+ PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
+ {
+ auto def = PlatformUnit::createPropertyDefinitions();
+ def->setPrefix(getConfigIdentifier());
+
+ def->defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
+ def->defineOptionalProperty<float>("InitialRotation", 0, "Initial rotation of the platform.");
+ def->defineOptionalProperty<float>("InitialPosition.x", 0, "Initial x position of the platform.");
+ def->defineOptionalProperty<float>("InitialPosition.y", 0, "Initial y position of the platform.");
+ def->defineOptionalProperty<std::string>("ReferenceFrame", "Platform", "Reference frame in which the platform position is reported.");
+ def->defineOptionalProperty<float>("LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec).");
+ def->defineOptionalProperty<float>("MaxLinearAcceleration", 1000.0, "Linear acceleration of the platform (default: 1000 mm/sec).");
+ def->defineOptionalProperty<float>("Kp_Position", 5.0, "P value of the PID position controller");
+ def->defineOptionalProperty<float>("Kp_Velocity", 5.0, "P value of the PID velocity controller");
+ def->defineOptionalProperty<float>("AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec).");
+
+ def->component(robotStateComponent);
+
+ return def;
+ }
+
void PlatformUnitSimulation::onInitPlatformUnit()
{
platformMode = eUndefined;
@@ -56,15 +85,27 @@ namespace armarx
}
+ Eigen::Matrix4f PlatformUnitSimulation::currentPlatformPose() const
+ {
+ return VirtualRobot::MathTools::posrpy2eigen4f(currentPositionX, currentPositionY, 0, 0, 0, currentRotation);
+ }
+
void PlatformUnitSimulation::onStartPlatformUnit()
{
- 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);
+ agentName = robotStateComponent->getRobotName();
+ robotRootFrame = robotStateComponent->getSynchronizedRobot()->getRootNode()->getName();
+
+ TransformStamped currentPose;
+ currentPose.header.parentFrame = GlobalFrame;
+ currentPose.header.frame = robotRootFrame;
+ currentPose.header.agent = agentName;
+ currentPose.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose.transform = currentPlatformPose();
+
+ globalPosePrx->reportGlobalRobotPose(currentPose);
+
+ // legacy
+ listenerPrx->reportPlatformPose(toPlatformPose(currentPose));
simulationTask->start();
}
@@ -85,7 +126,6 @@ namespace armarx
}
}
-
void PlatformUnitSimulation::simulationFunction()
{
// the time it took until this method was called again
@@ -163,14 +203,39 @@ 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]);
+
+ // odom velocity is in local robot frame
+ FrameHeader odomVelocityHeader;
+ odomVelocityHeader.parentFrame = robotRootFrame;
+ odomVelocityHeader.frame = robotRootFrame;
+ odomVelocityHeader.agent = agentName;
+ odomVelocityHeader.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();;
+
+ // odom pose is in odom frame
+ FrameHeader odomPoseHeader;
+ odomPoseHeader.parentFrame = OdometryFrame;
+ odomPoseHeader.frame = robotRootFrame;
+ odomPoseHeader.agent = agentName;
+ odomPoseHeader.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();;
+
+ TransformStamped platformPose;
+ platformPose.header = odomPoseHeader;
+ platformPose.transform = currentPlatformPose();
+
+ TwistStamped platformVelocity;
+ platformVelocity.header = odomVelocityHeader;
+ platformVelocity.twist.linear << vel[0], vel[1], 0;
+ platformVelocity.twist.angular << 0, 0, vel[2];
+
+ odometryPrx->reportOdometryPose(platformPose);
+ odometryPrx->reportOdometryVelocity(platformVelocity);
+
+ // legacy
+ listenerPrx->reportPlatformPose(toPlatformPose(platformPose));
+
+ // legacy
+ const auto& velo = platformVelocity.twist;
+ listenerPrx->reportPlatformVelocity(velo.linear.x(), velo.linear.y(), velo.angular.z());
}
void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
@@ -185,16 +250,20 @@ namespace armarx
this->positionalAccuracy = positionalAccuracy;
this->orientationalAccuracy = orientationalAccuracy;
}
- listenerPrx->reportNewTargetPose(targetPositionX, targetPositionY, targetRotation);
- }
+ FrameHeader header;
+ header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ header.parentFrame = GlobalFrame;
+ header.frame = robotRootFrame;
+ header.agent = agentName;
- PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
- {
- return PropertyDefinitionsPtr(
- new PlatformUnitSimulationPropertyDefinitions(getConfigIdentifier()));
- }
+ TransformStamped targetPose;
+ targetPose.header = header;
+ targetPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
+ const auto tp = toPlatformPose(targetPose);
+ listenerPrx->reportNewTargetPose(tp.x, tp.y, tp.rotationAroundZ);
+ }
void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
{
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index c6d9851b1345a8aad3bd296b99f7c06ea774fec6..391331d4c45c4058ffbd16b8f1eb4637313b04cc 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -29,6 +29,7 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/interface/core/RobotState.h>
#include <IceUtil/Time.h>
@@ -36,30 +37,6 @@
namespace armarx
{
- /**
- * \class PlatformUnitSimulationPropertyDefinitions
- * \brief
- */
- class PlatformUnitSimulationPropertyDefinitions :
- public PlatformUnitPropertyDefinitions
- {
- public:
- PlatformUnitSimulationPropertyDefinitions(std::string prefix) :
- PlatformUnitPropertyDefinitions(prefix)
- {
- defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
- defineOptionalProperty<float>("InitialRotation", 0, "Initial rotation of the platform.");
- defineOptionalProperty<float>("InitialPosition.x", 0, "Initial x position of the platform.");
- defineOptionalProperty<float>("InitialPosition.y", 0, "Initial y position of the platform.");
- defineOptionalProperty<std::string>("ReferenceFrame", "Platform", "Reference frame in which the platform position is reported.");
- defineOptionalProperty<float>("LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec).");
- defineOptionalProperty<float>("MaxLinearAcceleration", 1000.0, "Linear acceleration of the platform (default: 1000 mm/sec).");
- defineOptionalProperty<float>("Kp_Position", 5.0, "P value of the PID position controller");
- defineOptionalProperty<float>("Kp_Velocity", 5.0, "P value of the PID velocity controller");
- defineOptionalProperty<float>("AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec).");
- }
- };
-
/**
* \class PlatformUnitSimulation
* \brief Simulates a robot platform.
@@ -134,6 +111,15 @@ namespace armarx
PeriodicTask<PlatformUnitSimulation>::pointer_type simulationTask;
+ private:
+ Eigen::Matrix4f currentPlatformPose() const;
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+
+ std::string agentName;
+ std::string robotRootFrame;
+
+
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 99ec57ddd276fe00bbc9ca9c76967d8e1d56ac11..809be741c620d0a5de372cb0276c19788aa2dc2d 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -21,150 +21,201 @@
*/
#include "PlatformSubUnit.h"
-#include <boost/algorithm/clamp.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
-void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+#include <RobotAPI/interface/core/GeometryBase.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
+namespace armarx
{
- if (!getProxy())
- {
- //this unit is not initialized yet
- ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet - skipping this update";
- return;
- }
- if (!listenerPrx)
+
+
+ PropertyDefinitionsPtr armarx::PlatformSubUnit::createPropertyDefinitions()
{
- ARMARX_INFO << deactivateSpam(1) << "listener is not set - skipping this update";
- return;
+ auto def = PlatformUnit::createPropertyDefinitions();
+ def->setPrefix(getConfigIdentifier());
+
+ def->component(robotStateComponent);
+
+ return def;
}
- const SensorValueBase* sensorValue = sc.sensors.at(platformSensorIndex).get();
- ARMARX_CHECK_EXPRESSION(sensorValue);
- std::lock_guard<std::mutex> guard {dataMutex};
- //pos
- {
- ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
- const SensorValueHolonomicPlatformRelativePosition* s = sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
- relX = s->relativePositionX;
- relY = s->relativePositionY;
- relR = s->relativePositionRotation;
- // @@@ CHECK BELOW:
- if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
+ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+ {
+ if (!getProxy())
+ {
+ //this unit is not initialized yet
+ ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet - skipping this update";
+ return;
+ }
+ if (!listenerPrx)
{
- const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
- abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
-
- PlatformPose currentPose;
- currentPose.timestampInMicroSeconds = sc.sensorValuesTimestamp.toMicroSeconds();
- currentPose.x = abs(0, 3);
- currentPose.y = abs(1, 3);
- currentPose.rotationAroundZ = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
- listenerPrx->reportPlatformPose(currentPose);
- globalPosCtrl->setGlobalPos(currentPose);
+ ARMARX_INFO << deactivateSpam(1) << "listener is not set - skipping this update";
+ return;
}
- else
+ const SensorValueBase* sensorValue = sc.sensors.at(platformSensorIndex).get();
+ ARMARX_CHECK_EXPRESSION(sensorValue);
+ std::lock_guard<std::mutex> guard {dataMutex};
+
+ const auto timestamp = sc.sensorValuesTimestamp.toMicroSeconds();;
+
+ // odom velocity is in local robot frame
+ FrameHeader odomVelocityHeader;
+ odomVelocityHeader.parentFrame = robotRootFrame;
+ odomVelocityHeader.frame = robotRootFrame;
+ odomVelocityHeader.agent = agentName;
+ odomVelocityHeader.timestampInMicroSeconds = timestamp;
+
+ // odom pose is in odom frame
+ FrameHeader odomPoseHeader;
+ odomPoseHeader.parentFrame = OdometryFrame;
+ odomPoseHeader.frame = robotRootFrame;
+ odomPoseHeader.agent = agentName;
+ odomPoseHeader.timestampInMicroSeconds = timestamp;
+
+ // odom velocity is in local robot frame
+ FrameHeader globalPoseHeader;
+ globalPoseHeader.parentFrame = GlobalFrame;
+ globalPoseHeader.frame = robotRootFrame;
+ globalPoseHeader.agent = agentName;
+ globalPoseHeader.timestampInMicroSeconds = timestamp;
+
+ //pos
{
- abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(relX, relY, 0, 0, 0, relR);
+ ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
+ const SensorValueHolonomicPlatformRelativePosition* s = sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
+ relX = s->relativePositionX;
+ relY = s->relativePositionY;
+ relR = s->relativePositionRotation;
+
+
+ // @@@ CHECK BELOW:
+ if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
+ {
+ const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
+ abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
+
+ TransformStamped currentPose;
+ currentPose.header = globalPoseHeader;
+ currentPose.transform = abs;
+
+ globalPosePrx->reportGlobalRobotPose(currentPose);
+
+ // legacy interfaces
+ PlatformPose platformPose = toPlatformPose(currentPose);
+ listenerPrx->reportPlatformPose(platformPose);
+ globalPosCtrl->setGlobalPos(platformPose);
+ }
+ else
+ {
+ abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(relX, relY, 0, 0, 0, relR);
+ }
+
+ TransformStamped odomPose;
+ odomPose.header = odomPoseHeader;
+ odomPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(relX, relY, 0, 0, 0, relR);
+
+ odometryPrx->reportOdometryPose(odomPose);
+
+ // legacy interface
+ const auto odomLegacyPose = toPlatformPose(odomPose);
+ listenerPrx->reportPlatformOdometryPose(odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
}
+ //vel
+ {
+ ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
+ const SensorValueHolonomicPlatformVelocity* s = sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
- listenerPrx->reportPlatformOdometryPose(relX, relY, relR);
- }
- //vel
- {
- ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
- const SensorValueHolonomicPlatformVelocity* s = sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
- listenerPrx->reportPlatformVelocity(s->velocityX, s->velocityY, s->velocityRotation);
+ TwistStamped odomVelocity;
+ odomVelocity.header = odomVelocityHeader;
+ odomVelocity.twist.linear << s->velocityX, s->velocityY, 0;
+ odomVelocity.twist.angular << 0, 0, s->velocityRotation;
+
+ odometryPrx->reportOdometryVelocity(odomVelocity);
+
+ // legacy interface
+ const auto& vel = odomVelocity.twist;
+ listenerPrx->reportPlatformVelocity(vel.linear.x(), vel.linear.y(), vel.angular.z());
+ }
}
-}
-void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
-{
- //holding the mutex here could deadlock
- if (!pt->isControllerActive())
+ void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
{
- pt->activateController();
+ //holding the mutex here could deadlock
+ if (!pt->isControllerActive())
+ {
+ pt->activateController();
+ }
+ std::lock_guard<std::mutex> guard {dataMutex};
+ pt->setVelocites(
+ std::clamp(vx, -maxVLin, maxVLin),
+ std::clamp(vy, -maxVLin, maxVLin),
+ std::clamp(vr, -maxVAng, maxVAng)
+ );
}
- std::lock_guard<std::mutex> guard {dataMutex};
- pt->setVelocites(
- boost::algorithm::clamp(vx, -maxVLin, maxVLin),
- boost::algorithm::clamp(vy, -maxVLin, maxVLin),
- boost::algorithm::clamp(vr, -maxVAng, maxVAng)
- );
-}
-
-void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
-{
- globalPosCtrl->setTarget(rx, ry, rr, lac, rac);
- if (!globalPosCtrl->isControllerActive())
- {
- globalPosCtrl->activateController();
- };
-}
-void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
-{
- relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
- if (!relativePosCtrl->isControllerActive())
+ void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
{
- relativePosCtrl->activateController();
+ globalPosCtrl->setTarget(rx, ry, rr, lac, rac);
+ if (!globalPosCtrl->isControllerActive())
+ {
+ globalPosCtrl->activateController();
+ };
}
- //holding the mutex here could deadlock
- // std::lock_guard<std::mutex> guard {dataMutex};
- ARMARX_INFO << "target orientation: " << rr;
-
-}
-
-void armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current&)
-{
- std::lock_guard<std::mutex> guard {dataMutex};
- maxVLin = std::abs(mxVLin);
- maxVAng = std::abs(mxVAng);
-}
-
-void armarx::PlatformSubUnit::setGlobalPose(armarx::PoseBasePtr globalPose, const Ice::Current&)
-{
- std::lock_guard<std::mutex> guard {dataMutex};
- PosePtr p = PosePtr::dynamicCast(globalPose);
- positionCorrection = p->toEigen() * abs.inverse();
-}
-
-armarx::PoseBasePtr armarx::PlatformSubUnit::getGlobalPose(const Ice::Current&)
-{
- std::lock_guard<std::mutex> guard {dataMutex};
- return new Pose {abs};
-}
-
-void armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
-{
- move(0, 0, 0);
-}
-
-Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
-{
- Eigen::Matrix4f rp = VirtualRobot::MathTools::rpy2eigen4f(0, 0, relR);
- rp(0, 3) = relX;
- rp(1, 3) = relY;
- return rp;
-}
+ void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
+ {
+ relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
+ if (!relativePosCtrl->isControllerActive())
+ {
+ relativePosCtrl->activateController();
+ }
+ //holding the mutex here could deadlock
+ // std::lock_guard<std::mutex> guard {dataMutex};
+ ARMARX_INFO << "target orientation: " << rr;
-void armarx::PlatformSubUnit::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
-{
- globalPosCtrl->setGlobalPos(currentPose);
-}
+ }
-void armarx::PlatformSubUnit::reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c)
-{
+ void armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current&)
+ {
+ std::lock_guard<std::mutex> guard {dataMutex};
+ maxVLin = std::abs(mxVLin);
+ maxVAng = std::abs(mxVAng);
+ }
-}
+ void armarx::PlatformSubUnit::setGlobalPose(armarx::PoseBasePtr globalPose, const Ice::Current&)
+ {
+ std::lock_guard<std::mutex> guard {dataMutex};
+ PosePtr p = PosePtr::dynamicCast(globalPose);
+ positionCorrection = p->toEigen() * abs.inverse();
+ }
-void armarx::PlatformSubUnit::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
-{
+ armarx::PoseBasePtr armarx::PlatformSubUnit::getGlobalPose(const Ice::Current&)
+ {
+ std::lock_guard<std::mutex> guard {dataMutex};
+ return new Pose {abs};
+ }
-}
+ void armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
+ {
+ move(0, 0, 0);
+ }
+ Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
+ {
+ Eigen::Matrix4f rp = VirtualRobot::MathTools::rpy2eigen4f(0, 0, relR);
+ rp(0, 3) = relX;
+ rp(1, 3) = relY;
+ return rp;
+ }
+ void armarx::PlatformSubUnit::reportGlobalRobotPose(const TransformStamped& currentPose, const Ice::Current&)
+ {
+ globalPosCtrl->setGlobalPos(toPlatformPose(currentPose));
+ }
-void armarx::PlatformSubUnit::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
-}
+}
\ No newline at end of file
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 7fb01dd2a3d2f49059a5906c7ea998ed8c95a4d9..df975ed0e999c8e19ed588dbb426ab7695643c89 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -32,6 +32,7 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/RobotState.h>
#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h"
@@ -58,19 +59,20 @@ namespace armarx
virtual void setGlobalPose(PoseBasePtr globalPose, const Ice::Current& = Ice::emptyCurrent) /*override*/;
virtual PoseBasePtr getGlobalPose(const Ice::Current& = Ice::emptyCurrent) /*override*/;
+ void reportGlobalRobotPose(const TransformStamped& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
- void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = ::Ice::Current()) override ;
- void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current()) override;
+ PropertyDefinitionsPtr createPropertyDefinitions() override;
- void reportPlatformOdometryPose(::Ice::Float, ::Ice::Float, ::Ice::Float, const ::Ice::Current& = ::Ice::Current()) override;
// PlatformUnit interface
void onInitPlatformUnit() override
{
- usingTopic("PlatformState");
}
- void onStartPlatformUnit() override {}
+ void onStartPlatformUnit() override
+ {
+ agentName = robotStateComponent->getRobotName();
+ robotRootFrame = robotStateComponent->getSynchronizedRobot()->getRootNode()->getName();
+ }
void onExitPlatformUnit() override {}
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
@@ -92,6 +94,17 @@ namespace armarx
Eigen::Matrix4f abs;
+ // TODO(fabian.reister): likely remove or adapt
Eigen::Matrix4f positionCorrection = Eigen::Matrix4f::Identity();
+
+ protected:
+ RobotStateComponentInterfacePrx robotStateComponent;
+
+ private:
+
+ std::string agentName;
+ std::string robotRootFrame;
+
+
};
}
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 06b9d9b21090a3149c26862033f87d2192775045..88db84d51e8fb8cce6e551e8a818936928e1c854 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -12,9 +12,11 @@ set(SLICE_FILES
core/BlackWhitelist.ice
core/PoseBase.ice
+ core/GeometryBase.ice
core/OrientedPoint.ice
core/LinkedPoseBase.ice
core/FramedPoseBase.ice
+ core/RobotLocalization.ice
core/RobotState.ice
core/RobotStateObserverInterface.ice
core/Trajectory.ice
diff --git a/source/RobotAPI/interface/core/GeometryBase.ice b/source/RobotAPI/interface/core/GeometryBase.ice
new file mode 100644
index 0000000000000000000000000000000000000000..3a3208019ebb02754b3cc2061ea9aea1e92fe9f5
--- /dev/null
+++ b/source/RobotAPI/interface/core/GeometryBase.ice
@@ -0,0 +1,46 @@
+
+#pragma once
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+/**
+* Messages that can also be found in ROS package 'geometry_msgs'
+* http://docs.ros.org/en/jade/api/geometry_msgs/html/index-msg.html
+*/
+module armarx
+{
+
+ struct FrameHeader
+ {
+ string parentFrame; // base frame
+ string frame; // child frame
+
+ string agent; // the robot
+
+ long timestampInMicroSeconds = 0;
+ }
+
+ struct TransformStamped
+ {
+ FrameHeader header;
+ Eigen::Matrix4f transform; // [mm]
+ }
+
+ struct Twist
+ {
+ // Linear (x,y,z) and angular (roll, pitch, yaw) velocities
+ Eigen::Vector3f linear; // [mm/s]
+ Eigen::Vector3f angular; // [rad/s]
+ }
+
+ struct TwistStamped
+ {
+ FrameHeader header;
+ Twist twist;
+ }
+
+} // module armarx
\ No newline at end of file
diff --git a/source/RobotAPI/interface/core/RobotLocalization.ice b/source/RobotAPI/interface/core/RobotLocalization.ice
new file mode 100644
index 0000000000000000000000000000000000000000..dbaee0f7a61dd6b95244e55e06b08bf50e4febec
--- /dev/null
+++ b/source/RobotAPI/interface/core/RobotLocalization.ice
@@ -0,0 +1,41 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI
+ * @author Fabian Reister (fabian dot reister at kit dot edu)
+ * @copyright 2021
+ * @license http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/core/GeometryBase.ice>
+
+module armarx{
+
+
+ interface GlobalRobotPoseLocalizationListener{
+ void reportGlobalRobotPose(TransformStamped currentPose);
+ }
+
+ interface OdometryListener{
+ void reportOdometryVelocity(TwistStamped platformVelocity);
+ void reportOdometryPose(TransformStamped odometryPose);
+ }
+
+} // module armarx
\ No newline at end of file
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 332959df89b7c0c4fe55ce2442f240d29e45213e..467b5c9552928a82c899642f2958b5c863d91993 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -27,6 +27,7 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
#include <RobotAPI/interface/core/FramedPoseBase.ice>
+
#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
@@ -179,10 +180,6 @@ module armarx
RobotInfoNodeList children;
};
- interface GlobalRobotPoseLocalizationListener
- {
- void reportGlobalRobotPose(string robotName, Eigen::Matrix4f pose, long timestamp);
- };
/**
* The interface used by the RobotStateComponent which allows creating
@@ -191,7 +188,6 @@ module armarx
*/
interface RobotStateComponentInterface extends
KinematicUnitListener,
- PlatformUnitListener,
GlobalRobotPoseLocalizationListener,
SimulatorResetEvent
{
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index c4afe4591d93d0da051824762cdf2d13195b259b..2d93b642a7658524d9b092cc569781c83797f71d 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -28,6 +28,9 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/core/GeometryBase.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
+
module armarx
{
@@ -90,26 +93,31 @@ module armarx
* reportPlatformPose reports current platform pose.
* @param currentPose Current global platform pose.
**/
+ ["deprecate:reportPlatformPose() has been deprecated, use GlobalRobotPoseLocalizationListener::reportGlobalRobotPose() instead."]
void reportPlatformPose(PlatformPose 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);
+ ["deprecate:reportNewTargetPose() has been deprecated, use ... instead."]
+ void reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation);
/**
* 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.
**/
+ // ["deprecate:reportPlatformVelocity() has been deprecated, use OdometryListener::reportOdometryVelocity() instead."]
void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation);
+ ["deprecate:reportPlatformOdometryPose() has been deprecated, use OdometryListener::reportOdometryPose() instead."]
void reportPlatformOdometryPose(float x, float y, float angle);
};
- interface PlatformSubUnitInterface extends PlatformUnitInterface, PlatformUnitListener
+ interface PlatformSubUnitInterface extends PlatformUnitInterface, GlobalRobotPoseLocalizationListener
{
};
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 9a1f92a6e262b920618086576b966e61542f1b96..601dc48bbb5df213edf8abbad0cf19bb487ae0b6 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -57,8 +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";
/**