From 7539bf07437c5ca8a3dfeddb092433df4473c9f5 Mon Sep 17 00:00:00 2001
From: Markus Grotz <Markus Grotz markus.grotz@kit.edu>
Date: Mon, 25 Nov 2019 10:03:30 +0100
Subject: [PATCH] enable PlatformUnit::moveTo()
- moveTo() now uses the global position provided by the SelfLocalization
component to control the platform with an instance of
NJointHolonomicPlatformGlobalPositionController
---
...onomicPlatformGlobalPositionController.cpp | 8 ++---
...olonomicPlatformGlobalPositionController.h | 2 +-
.../units/RobotUnit/Units/PlatformSubUnit.cpp | 30 ++++++++++++++++++-
.../units/RobotUnit/Units/PlatformSubUnit.h | 15 ++++++++--
.../interface/units/PlatformUnitInterface.ice | 5 ++++
5 files changed, 52 insertions(+), 8 deletions(-)
mode change 100644 => 100755 source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
mode change 100644 => 100755 source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 222b7f42c..1a8943ff4 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -133,17 +133,17 @@ namespace armarx
}
- void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(float x, float y, float yaw, const IceUtil::Time& time = ::IceUtil::Time::now())
+ void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
{
// ..todo:: lock :)
- getWriterControlStruct().globalPosition << x, y;
- getWriterControlStruct().globalOrientation = yaw;
+ getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
+ getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation;
- getWriterControlStruct().lastUpdate = time;
+ 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 b151f3802..c6c7e6cd9 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -102,7 +102,7 @@ namespace armarx
void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
- void setGlobalPos(float x, float y, float yaw, const IceUtil::Time&);
+ void setGlobalPos(const PlatformPose& currentPose);
protected:
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
old mode 100644
new mode 100755
index 1201da179..f67bb14d7
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -45,6 +45,7 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
relY = s->relativePositionY;
relR = s->relativePositionRotation;
+
// @@@ CHECK BELOW:
if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
{
@@ -57,6 +58,7 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
currentPose.y = abs(1, 3);
currentPose.rotationAroundZ = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
listenerPrx->reportPlatformPose(currentPose);
+ globalPosCtrl->setGlobalPos(currentPose);
}
else
{
@@ -90,7 +92,11 @@ void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr,
void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
{
- ARMARX_WARNING << "NYI";
+ 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&)
@@ -138,3 +144,25 @@ Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
rp(1, 3) = relY;
return rp;
}
+
+
+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::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+{
+
+}
+
+
+
+void armarx::PlatformSubUnit::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+{
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
old mode 100644
new mode 100755
index 428cc7c29..7420f33e0
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -44,7 +44,8 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(PlatformSubUnit);
class PlatformSubUnit:
virtual public RobotUnitSubUnit,
- virtual public PlatformUnit
+ virtual public PlatformUnit,
+ virtual public PlatformSubUnitInterface
{
public:
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
@@ -59,8 +60,18 @@ namespace armarx
virtual void setGlobalPose(PoseBasePtr globalPose, const Ice::Current& = Ice::emptyCurrent) /*override*/;
virtual PoseBasePtr getGlobalPose(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;
+
+ void reportPlatformOdometryPose(::Ice::Float, ::Ice::Float, ::Ice::Float, const ::Ice::Current& = ::Ice::Current()) override;
+
// PlatformUnit interface
- void onInitPlatformUnit() override {}
+ void onInitPlatformUnit() override
+ {
+ usingTopic("PlatformState");
+ }
void onStartPlatformUnit() override {}
void onExitPlatformUnit() override {}
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index bec45c18e..5472c33e9 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -109,5 +109,10 @@ module armarx
void reportPlatformOdometryPose(float x, float y, float angle);
};
+ interface PlatformSubUnitInterface extends PlatformUnitInterface, PlatformUnitListener
+ {
+
+ };
+
};
--
GitLab