From d13e875144711ca6c687b025a359d834194d5339 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 6 Mar 2023 14:53:57 +0100
Subject: [PATCH] aron conversions
---
.../manipulation/core/aron_conversions.cpp | 251 ++++++++++++------
.../manipulation/core/aron_conversions.h | 3 +
2 files changed, 172 insertions(+), 82 deletions(-)
diff --git a/source/armarx/manipulation/core/aron_conversions.cpp b/source/armarx/manipulation/core/aron_conversions.cpp
index 7e8c5521..9b1a6077 100644
--- a/source/armarx/manipulation/core/aron_conversions.cpp
+++ b/source/armarx/manipulation/core/aron_conversions.cpp
@@ -20,96 +20,111 @@
* GNU General Public License
*/
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include "aron_conversions.h"
+
+#include "ArmarXCore/core/logging/Logging.h"
+
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <armarx/manipulation/core/aron/Affordance.aron.generated.h>
-#include <armarx/manipulation/core/aron/ActionHypothesis.aron.generated.h>
-#include <armarx/manipulation/core/aron/ExecutableAction.aron.generated.h>
-#include <armarx/manipulation/core/aron/ExecutedAction.aron.generated.h>
-#include <armarx/manipulation/core/aron/FramedUncertainPose.aron.generated.h>
-#include <armarx/manipulation/core/aron/HandTrajectory.aron.generated.h>
-#include <armarx/manipulation/core/aron/SituatedAffordance.aron.generated.h>
-#include <armarx/manipulation/core/aron/Arm.aron.generated.h>
-#include <armarx/manipulation/core/aron/Hand.aron.generated.h>
-#include <armarx/manipulation/core/aron/ManipulationProcess.aron.generated.h>
-#include <armarx/manipulation/core/Affordance.h>
+
#include <armarx/manipulation/core/ActionHypothesis.h>
+#include <armarx/manipulation/core/Affordance.h>
#include <armarx/manipulation/core/AffordanceTypes.h>
#include <armarx/manipulation/core/ExecutableAction.h>
#include <armarx/manipulation/core/ExecutedAction.h>
#include <armarx/manipulation/core/FramedUncertainPose.h>
#include <armarx/manipulation/core/HandTrajectory.h>
-#include <armarx/manipulation/core/SituatedAffordance.h>
#include <armarx/manipulation/core/ManipulationProcess.h>
+#include <armarx/manipulation/core/SituatedAffordance.h>
#include <armarx/manipulation/core/arm.h>
+#include <armarx/manipulation/core/aron/ActionHypothesis.aron.generated.h>
+#include <armarx/manipulation/core/aron/Affordance.aron.generated.h>
+#include <armarx/manipulation/core/aron/Arm.aron.generated.h>
+#include <armarx/manipulation/core/aron/ExecutableAction.aron.generated.h>
+#include <armarx/manipulation/core/aron/ExecutedAction.aron.generated.h>
+#include <armarx/manipulation/core/aron/FramedUncertainPose.aron.generated.h>
+#include <armarx/manipulation/core/aron/Hand.aron.generated.h>
+#include <armarx/manipulation/core/aron/HandTrajectory.aron.generated.h>
+#include <armarx/manipulation/core/aron/ManipulationProcess.aron.generated.h>
+#include <armarx/manipulation/core/aron/SituatedAffordance.aron.generated.h>
#include <armarx/manipulation/meta/transform_container.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include "aron_conversions.h"
-
namespace armarx::manipulation::core
{
// namespace aron_helpers
// {
- armarx::FramedPose toFramedPose(const arondto::FramedUncertainPose& pose)
+ armarx::FramedPose
+ toFramedPose(const arondto::FramedUncertainPose& pose)
{
// TODO: should agent be reference frame. In FramedPose agent is not used for anything (as far as I can tell)
return {pose.uncertainPose.pose, pose.header.frame, pose.header.agent};
}
- armarx::FramedPose toFramedPose(const FramedUncertainPose& pose)
+ armarx::FramedPose
+ toFramedPose(const FramedUncertainPose& pose)
{
return toFramedPose(toAron(pose));
}
- arondto::Affordance toAron(const Affordance& bo)
+ arondto::Affordance
+ toAron(const Affordance& bo)
{
arondto::Affordance aff;
toAron(aff, bo);
return aff;
}
- Affordance fromAron(const arondto::Affordance& dto)
+ Affordance
+ fromAron(const arondto::Affordance& dto)
{
armem::robot::RobotDescription description;
armem::robot::fromAron(dto.robotDescription, description);
return {dto.actionType, description};
}
- arondto::FramedUncertainPose toAron(const FramedUncertainPose& bo)
+ arondto::FramedUncertainPose
+ toAron(const FramedUncertainPose& bo)
{
arondto::FramedUncertainPose pose;
toAron(pose, bo);
return pose;
}
- FramedUncertainPose fromAron(const arondto::FramedUncertainPose& dto)
+ FramedUncertainPose
+ fromAron(const arondto::FramedUncertainPose& dto)
{
return {toFramedPose(dto), dto.uncertainPose.covariance};
}
- arondto::ActionHypothesis toAron(const ActionHypothesis& bo)
+ arondto::ActionHypothesis
+ toAron(const ActionHypothesis& bo)
{
arondto::ActionHypothesis dto;
toAron(dto, bo);
return dto;
}
- ActionHypothesis fromAron(const arondto::ActionHypothesis& dto)
+ ActionHypothesis
+ fromAron(const arondto::ActionHypothesis& dto)
{
- return {fromAron(dto.affordance), fromAron(dto.framedPose), dto.observationTime, dto.handedness,
+ return {fromAron(dto.affordance),
+ fromAron(dto.framedPose),
+ dto.observationTime,
+ dto.handedness,
dto.existenceCertainty};
}
- arondto::ExecutableAction toAron(const ExecutableAction& bo)
+ arondto::ExecutableAction
+ toAron(const ExecutableAction& bo)
{
arondto::ExecutableAction dto;
toAron(dto, bo);
return dto;
}
- ExecutableAction fromAron(const arondto::ExecutableAction& dto)
+ ExecutableAction
+ fromAron(const arondto::ExecutableAction& dto)
{
std::optional<armarx::FramedPose> platformPose;
@@ -122,7 +137,7 @@ namespace armarx::manipulation::core
}
ARMARX_CHECK_NOT_EMPTY(dto.unimanual);
std::vector<ExecutableAction::Unimanual> unimanuals;
- for (const auto& um: dto.unimanual)
+ for (const auto& um : dto.unimanual)
{
std::optional<armarx::FramedPose> execPose;
std::optional<armarx::FramedPose> prePose;
@@ -162,35 +177,44 @@ namespace armarx::manipulation::core
}
// TODO: this shold work under c++20
// unimanuals.emplace_back(hypothesis, prePose, retractPose, trajectory);
- unimanuals.push_back(
- {.hypothesis=hypothesis, .execPose=execPose, .prePose=prePose, .retractPose=retractPose, .actionTrajectory=trajectory, .handedness=handedness});
+ unimanuals.push_back({.hypothesis = hypothesis,
+ .execPose = execPose,
+ .prePose = prePose,
+ .retractPose = retractPose,
+ .actionTrajectory = trajectory,
+ .handedness = handedness});
}
return {unimanuals,
- {.platformPose=platformPose, .platformPlanningStrategy=dto.common.platformPlanningStrategy},
+ {.platformPose = platformPose,
+ .platformPlanningStrategy = dto.common.platformPlanningStrategy},
dto.planningInformation};
}
- arondto::SituatedAffordance toAron(const SituatedAffordance& bo)
+ arondto::SituatedAffordance
+ toAron(const SituatedAffordance& bo)
{
arondto::SituatedAffordance dto;
toAron(dto, bo);
return dto;
}
- SituatedAffordance fromAron(const arondto::SituatedAffordance& dto)
+ SituatedAffordance
+ fromAron(const arondto::SituatedAffordance& dto)
{
return {dto.frame, fromAron(dto.affordance), dto.keyPoint, dto.pointCloud};
}
// }
- void toAron(arondto::Affordance& dto, const Affordance& bo)
+ void
+ toAron(arondto::Affordance& dto, const Affordance& bo)
{
dto.actionType = bo.affordance_type->getID();
toAron(dto.robotDescription, bo.getRobotDescription());
}
- void fromAron(const arondto::Affordance& dto, Affordance& bo)
+ void
+ fromAron(const arondto::Affordance& dto, Affordance& bo)
{
armem::robot::RobotDescription id;
fromAron(dto.robotDescription, id);
@@ -198,7 +222,8 @@ namespace armarx::manipulation::core
bo = Affordance(dto.actionType, id);
}
- void toAron(arondto::FramedUncertainPose& dto, const FramedUncertainPose& bo)
+ void
+ toAron(arondto::FramedUncertainPose& dto, const FramedUncertainPose& bo)
{
dto.header.frame = bo.pose->getFrame();
dto.header.agent = bo.pose->agent;
@@ -206,12 +231,14 @@ namespace armarx::manipulation::core
dto.uncertainPose.covariance = bo.covariance;
}
- void fromAron(const arondto::FramedUncertainPose& dto, FramedUncertainPose& bo)
+ void
+ fromAron(const arondto::FramedUncertainPose& dto, FramedUncertainPose& bo)
{
bo = fromAron(dto);
}
- void toAron(arondto::ActionHypothesis& dto, const ActionHypothesis& bo)
+ void
+ toAron(arondto::ActionHypothesis& dto, const ActionHypothesis& bo)
{
toAron(dto.affordance, *bo.affordance);
toAron(dto.framedPose, *bo.framedPose);
@@ -220,14 +247,16 @@ namespace armarx::manipulation::core
dto.handedness = bo.handedness;
}
- void fromAron(const arondto::ActionHypothesis& dto, ActionHypothesis& bo)
+ void
+ fromAron(const arondto::ActionHypothesis& dto, ActionHypothesis& bo)
{
bo = fromAron(dto);
}
- void toAron(arondto::ExecutableAction& dto, const ExecutableAction& bo)
+ void
+ toAron(arondto::ExecutableAction& dto, const ExecutableAction& bo)
{
- for (const auto& um: bo.unimanual)
+ for (const auto& um : bo.unimanual)
{
auto& dto_um = dto.unimanual.emplace_back();
if (um.hypothesis.has_value())
@@ -250,12 +279,14 @@ namespace armarx::manipulation::core
dto.planningInformation = bo.planningInformation;
}
- void fromAron(const arondto::ExecutableAction& dto, ExecutableAction& bo)
+ void
+ fromAron(const arondto::ExecutableAction& dto, ExecutableAction& bo)
{
bo = fromAron(dto);
}
- void toAron(arondto::SituatedAffordance& dto, const SituatedAffordance& bo)
+ void
+ toAron(arondto::SituatedAffordance& dto, const SituatedAffordance& bo)
{
dto.frame = bo.frame;
dto.pointCloud = bo.pointCloud;
@@ -266,19 +297,22 @@ namespace armarx::manipulation::core
toAron(dto.affordance, bo.affordance);
}
- void fromAron(const arondto::SituatedAffordance& dto, SituatedAffordance& bo)
+ void
+ fromAron(const arondto::SituatedAffordance& dto, SituatedAffordance& bo)
{
bo = fromAron(dto);
}
- arondto::ExecutedAction toAron(const ExecutedAction& bo)
+ arondto::ExecutedAction
+ toAron(const ExecutedAction& bo)
{
arondto::ExecutedAction exAction;
toAron(exAction, bo);
return exAction;
}
- void toAron(arondto::ExecutedAction& dto, const ExecutedAction& bo)
+ void
+ toAron(arondto::ExecutedAction& dto, const ExecutedAction& bo)
{
dto.action = toAron(*bo.action);
dto.startTime = *bo.startTime;
@@ -293,25 +327,34 @@ namespace armarx::manipulation::core
dto.debugOutput = bo.debugOutput;
}
- ExecutedAction fromAron(const arondto::ExecutedAction& dto)
+ ExecutedAction
+ fromAron(const arondto::ExecutedAction& dto)
{
- return {fromAron(dto.action), dto.startTime, dto.endTime, dto.success, dto.outcome, dto.comment,
+ return {fromAron(dto.action),
+ dto.startTime,
+ dto.endTime,
+ dto.success,
+ dto.outcome,
+ dto.comment,
dto.debugOutput};
}
- void fromAron(const arondto::ExecutedAction& dto, ExecutedAction& bo)
+ void
+ fromAron(const arondto::ExecutedAction& dto, ExecutedAction& bo)
{
bo = fromAron(dto);
}
- arondto::HandTrajectoryKeypoint toAron(const HandTrajectoryKeypoint& bo)
+ arondto::HandTrajectoryKeypoint
+ toAron(const HandTrajectoryKeypoint& bo)
{
auto kp = arondto::HandTrajectoryKeypoint();
toAron(kp, bo);
return kp;
}
- void toAron(arondto::HandTrajectoryKeypoint& dto, const HandTrajectoryKeypoint& bo)
+ void
+ toAron(arondto::HandTrajectoryKeypoint& dto, const HandTrajectoryKeypoint& bo)
{
dto.pose = bo.pose.toEigen();
dto.frame.frame = bo.pose.getFrame();
@@ -319,52 +362,58 @@ namespace armarx::manipulation::core
dto.fingerValues = bo.fingerValues;
}
- HandTrajectoryKeypoint fromAron(const arondto::HandTrajectoryKeypoint& dto)
+ HandTrajectoryKeypoint
+ fromAron(const arondto::HandTrajectoryKeypoint& dto)
{
return {dto.pose, dto.frame.frame, dto.fingerValues};
}
- void fromAron(const arondto::HandTrajectoryKeypoint& dto, HandTrajectoryKeypoint& bo)
+ void
+ fromAron(const arondto::HandTrajectoryKeypoint& dto, HandTrajectoryKeypoint& bo)
{
bo = fromAron(dto);
}
- arondto::HandTrajectory toAron(const HandTrajectory& bo)
+ arondto::HandTrajectory
+ toAron(const HandTrajectory& bo)
{
auto dto = arondto::HandTrajectory();
toAron(dto, bo);
return dto;
}
- void toAron(arondto::HandTrajectory& dto, const HandTrajectory& bo)
+ void
+ toAron(arondto::HandTrajectory& dto, const HandTrajectory& bo)
{
- std::transform(bo.keypoints.begin(), bo.keypoints.end(), std::back_inserter(dto.keypoints),
+ std::transform(bo.keypoints.begin(),
+ bo.keypoints.end(),
+ std::back_inserter(dto.keypoints),
[](const auto& kp) -> arondto::HandTrajectoryKeypoint
- {
- return toAron(kp);
- });
+ { return toAron(kp); });
dto.duration = bo.duration;
}
- HandTrajectory fromAron(const arondto::HandTrajectory& dto)
+ HandTrajectory
+ fromAron(const arondto::HandTrajectory& dto)
{
auto traj = HandTrajectory();
traj.duration = dto.duration;
- std::transform(dto.keypoints.begin(), dto.keypoints.end(), std::back_inserter(traj.keypoints),
- [](const auto& kp) -> HandTrajectoryKeypoint
- {
- return fromAron(kp);
- });
+ std::transform(dto.keypoints.begin(),
+ dto.keypoints.end(),
+ std::back_inserter(traj.keypoints),
+ [](const auto& kp) -> HandTrajectoryKeypoint { return fromAron(kp); });
return traj;
}
- void fromAron(const arondto::HandTrajectory& dto, HandTrajectory& bo)
+ void
+ fromAron(const arondto::HandTrajectory& dto, HandTrajectory& bo)
{
bo = fromAron(dto);
}
- void fromAron(const arondto::Arm& dto, Arm& bo)
+ void
+ fromAron(const arondto::Arm& dto, Arm& bo)
{
switch (dto.value)
{
@@ -377,7 +426,8 @@ namespace armarx::manipulation::core
}
}
- void fromAron(const arondto::Hand& dto, Hand& bo)
+ void
+ fromAron(const arondto::Hand& dto, Hand& bo)
{
switch (dto.value)
{
@@ -392,35 +442,72 @@ namespace armarx::manipulation::core
}
}
- arondto::ManipulationProcess toAron(const ManipulationProcess& bo)
+
+ void
+ toAron(arondto::Arm& dto, const Arm& bo)
+ {
+ switch (bo)
+ {
+ case Arm::Left:
+ dto = arondto::Arm::Left;
+ break;
+ case Arm::Right:
+ dto = arondto::Arm::Right;
+ break;
+ default:
+ ARMARX_ERROR << "Invalid arm";
+ }
+ }
+
+ void
+ toAron(arondto::Hand& dto, const Hand& bo)
+ {
+ switch (bo)
+ {
+ case Hand::Left:
+ dto = arondto::Hand::Left;
+ break;
+ case Hand::Right:
+ dto = arondto::Hand::Right;
+ break;
+ default:
+ ARMARX_ERROR << "Invalid hand";
+ }
+ }
+
+ arondto::ManipulationProcess
+ toAron(const ManipulationProcess& bo)
{
auto dto = arondto::ManipulationProcess();
toAron(dto, bo);
return dto;
}
- void toAron(arondto::ManipulationProcess& dto, const ManipulationProcess& bo)
+ void
+ toAron(arondto::ManipulationProcess& dto, const ManipulationProcess& bo)
{
- dto.actionHypotheses = meta::transform_container(bo.actionHypotheses, [](const auto& el)
- {
- return toAron(el);
- });
+ dto.actionHypotheses = meta::transform_container(bo.actionHypotheses,
+ [](const auto& el) { return toAron(el); });
dto.executedAction = toAron(*bo.executedAction);
dto.pointCloud = bo.pointCloud;
}
- ManipulationProcess fromAron(const arondto::ManipulationProcess& dto)
+ ManipulationProcess
+ fromAron(const arondto::ManipulationProcess& dto)
{
std::optional<armarx::manipulation::core::ExecutedAction> action;
- if (dto.executedAction.has_value()) action = fromAron(dto.executedAction.value());
- return ManipulationProcess{.actionHypotheses=meta::transform_container(dto.actionHypotheses, [](const auto& el)
- {
- return fromAron(el);
- }), .executedAction=action, .pointCloud=dto.pointCloud};
+ if (dto.executedAction.has_value())
+ action = fromAron(dto.executedAction.value());
+ return ManipulationProcess{
+ .actionHypotheses = meta::transform_container(
+ dto.actionHypotheses, [](const auto& el) { return fromAron(el); }),
+ .executedAction = action,
+ .pointCloud = dto.pointCloud};
}
- void fromAron(const arondto::ManipulationProcess& dto, ManipulationProcess& bo)
+ void
+ fromAron(const arondto::ManipulationProcess& dto, ManipulationProcess& bo)
{
bo = fromAron(dto);
}
-}
+} // namespace armarx::manipulation::core
diff --git a/source/armarx/manipulation/core/aron_conversions.h b/source/armarx/manipulation/core/aron_conversions.h
index 680686ea..f91ab090 100644
--- a/source/armarx/manipulation/core/aron_conversions.h
+++ b/source/armarx/manipulation/core/aron_conversions.h
@@ -78,6 +78,9 @@ namespace armarx::manipulation::core
void fromAron(const arondto::Arm& dto, Arm& bo);
void fromAron(const arondto::Hand& dto, Hand& bo);
+ void toAron(arondto::Arm& dto, const Arm& bo);
+ void toAron(arondto::Hand& dto, const Hand& bo);
+
namespace arondto
{
using ::armarx::manipulation::core::fromAron;
--
GitLab