diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp index 21e579c803afd33d2ad5864de6c4b283ae69733c..1bf28496f697da38e2172a41836ecaccceaa5561 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 62744724f1d4c42ebe882cf4072e87d43428dd02..f2ea2ba959897ad48d9aed390a569d2e8009dd17 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 785d25bdca4757ebe1c07f0bab767aa08a02a24d..03a578fab3b9250f724efcd646c24400d8a898d2 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 202a81f089702e07155232200911fa0e0afa197d..17fe89152ef063ab932ace41a9ade84eb2f2f556 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 2264ce36467ab7701cb94c9b74cb4c577136c715..f0ac512724a4150658f84f01cb07e23dd016fca0 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;