From 09f1281000ebd86b8d79ef4bd42751284793b6af Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 27 Oct 2021 20:45:15 +0200
Subject: [PATCH] armem_robot(_state): fixing circular dependency
---
.../armem_robot/aron_conversions.cpp | 25 --------
.../libraries/armem_robot/aron_conversions.h | 7 ---
.../armem_robot_state/aron_conversions.cpp | 62 ++++++++++++++-----
.../armem_robot_state/aron_conversions.h | 13 +++-
.../client/common/RobotReader.cpp | 12 ++--
5 files changed, 66 insertions(+), 53 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 21e579c80..1bf28496f 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -109,30 +109,5 @@ namespace armarx::armem
dto.objectJointValues = bo.jointMap;
}
- void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
- {
- bo.twist.linear.setZero();
- bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
-
- bo.twist.angular.setZero();
- bo.twist.angular.z() = dto.velocity.z(); // yaw
- }
-
- void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
- {
- ARMARX_ERROR << "Not implemented yet.";
- }
-
- void robot::fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
- {
- bo.force = dto.force;
- bo.torque = dto.torque;
- }
-
- void robot::toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
- {
- dto.force = bo.force;
- dto.torque = bo.torque;
- }
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 62744724f..f2ea2ba95 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,7 +8,6 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
@@ -27,12 +26,6 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
- void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
- void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
-
- void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo);
- void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo);
-
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 785d25bdc..03a578fab 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -1,31 +1,30 @@
#include "aron_conversions.h"
-// STL
#include <string>
-// Ice
#include <IceUtil/Time.h>
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-
namespace armarx::armem
{
/* Transform */
- void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ void
+ fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
{
fromAron(dto.header, bo.header);
aron::fromAron(dto.transform, bo.transform);
}
- void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ void
+ toAron(arondto::Transform& dto, const robot_state::Transform& bo)
{
toAron(dto.header, bo.header);
aron::toAron(dto.transform, bo.transform);
@@ -33,7 +32,8 @@ namespace armarx::armem
/* TransformHeader */
- void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ void
+ toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
{
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
@@ -41,7 +41,8 @@ namespace armarx::armem
dto.timestamp = bo.timestamp;
}
- void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ void
+ fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
{
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
@@ -51,16 +52,49 @@ namespace armarx::armem
/* JointState */
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+ void
+ fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
{
aron::fromAron(dto.name, bo.name);
aron::fromAron(dto.position, bo.position);
}
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+ void
+ toAron(arondto::JointState& dto, const robot_state::JointState& bo)
{
aron::toAron(dto.name, bo.name);
aron::toAron(dto.position, bo.position);
}
-} // namespace armarx::armem
+
+ void
+ fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
+
+ void
+ fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ {
+ bo.force = dto.force;
+ bo.torque = dto.torque;
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ {
+ dto.force = bo.force;
+ dto.torque = bo.torque;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 202a81f08..17fe89152 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,6 +21,9 @@
#pragma once
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
namespace armarx::armem
{
namespace robot_state
@@ -41,6 +44,7 @@ namespace armarx::armem
} // namespace arondto
+
void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
@@ -50,4 +54,11 @@ namespace armarx::armem
void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-} // namespace armarx::armem
\ No newline at end of file
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 2264ce364..f0ac51272 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -4,11 +4,10 @@
#include <optional>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
-#include "RobotAPI/libraries/armem_robot/types.h"
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -18,6 +17,7 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
namespace fs = ::std::filesystem;
@@ -508,7 +508,7 @@ namespace armarx::armem::robot_state
ARMARX_CHECK(proprioception.has_value());
platformState = robot::PlatformState(); // initialize optional
- robot::fromAron(proprioception->platform, platformState.value());
+ fromAron(proprioception->platform, platformState.value());
});
return platformState;
@@ -554,7 +554,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
robot::ForceTorque forceTorque;
- robot::fromAron(dtoFt, forceTorque);
+ fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
forceTorques.emplace(hand, forceTorque);
@@ -588,7 +588,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
robot::ForceTorque forceTorque;
- robot::fromAron(dtoFt, forceTorque);
+ fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
@@ -612,7 +612,7 @@ namespace armarx::armem::robot_state
const armem::wm::EntityInstance* instance = nullptr;
providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
{ instance = &i; });
- if (!instance)
+ if (instance == nullptr)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
--
GitLab