From 93d46c46511e2d769eed124eaa8da314f0ab9a82 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 3 Mar 2021 19:44:56 +0100
Subject: [PATCH] fix: interface changed
---
.../units/PlatformUnitSimulation.cpp | 35 ++++++++++---------
.../components/units/PlatformUnitSimulation.h | 2 +-
...onomicPlatformGlobalPositionController.cpp | 2 +-
3 files changed, 20 insertions(+), 19 deletions(-)
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index d32b20d3e..f917c29d0 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -67,10 +67,11 @@ namespace armarx
void PlatformUnitSimulation::onStartPlatformUnit()
{
- PoseStampedPtr currentPose(new PoseStamped);
- // FIXME: Take the timestamp from simulation
- currentPose->timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- currentPose->pose = currentPlatformPose();
+ PoseStamped currentPose;
+ // FIXME(fabian.reister): set frame and agent
+ // FIXME(fabian.reister): Take the timestamp from simulation
+ currentPose.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose.pose = currentPlatformPose();
listenerPrx->reportPlatformPose(currentPose);
simulationTask->start();
@@ -93,11 +94,11 @@ namespace armarx
}
}
- void PlatformUnitSimulation::fillCommonPlatformFields(FramedAndStamped & framedAndStamped)
+ void PlatformUnitSimulation::fillCommonPlatformFields(FrameHeader& header)
{
- framedAndStamped.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- framedAndStamped.frame = GlobalFrame;
- framedAndStamped.agent = RobotFrame;
+ header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ header.frame = GlobalFrame;
+ header.agent = RobotFrame;
}
void PlatformUnitSimulation::simulationFunction()
@@ -179,13 +180,13 @@ namespace armarx
}
- PoseStampedPtr platformPose(new PoseStamped);
- fillCommonPlatformFields(*platformPose);
- platformPose->pose = currentPlatformPose();
+ PoseStamped platformPose;
+ fillCommonPlatformFields(platformPose.header);
+ platformPose.pose = currentPlatformPose();
- VelocityStampedPtr platformVelocity(new VelocityStamped);
- fillCommonPlatformFields(*platformVelocity);
- platformVelocity->velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
+ VelocityStamped platformVelocity;
+ fillCommonPlatformFields(platformVelocity.header);
+ platformVelocity.velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
listenerPrx->reportPlatformPose(platformPose);
listenerPrx->reportPlatformVelocity(platformVelocity);
@@ -204,9 +205,9 @@ namespace armarx
this->orientationalAccuracy = orientationalAccuracy;
}
- PoseStampedPtr targetPose(new PoseStamped);
- fillCommonPlatformFields(*targetPose);
- targetPose->pose = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
+ PoseStamped targetPose;
+ fillCommonPlatformFields(targetPose.header);
+ targetPose.pose = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
listenerPrx->reportNewTargetPose(targetPose);
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index c2161535f..504c4f2f1 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -136,7 +136,7 @@ namespace armarx
private:
Eigen::Matrix4f currentPlatformPose() const;
- void fillCommonPlatformFields(FramedAndStamped & framedAndStamped);
+ void fillCommonPlatformFields(FrameHeader& header);
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index cde9b632f..4e305e90e 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -138,7 +138,7 @@ namespace armarx
getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation;
- getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+ getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.header.timestampInMicroSeconds);
writeControlStruct();
--
GitLab