From d4d24f767ca26dcf3c7a7ec3026761733511442a Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Sat, 25 Feb 2023 20:30:53 +0100
Subject: [PATCH] Robot: getToFInHand
---
source/armarx/manipulation/core/Robot.cpp | 35 +++++++++++++++++++++++
source/armarx/manipulation/core/Robot.h | 4 +++
2 files changed, 39 insertions(+)
diff --git a/source/armarx/manipulation/core/Robot.cpp b/source/armarx/manipulation/core/Robot.cpp
index 6ff81e26..ace89131 100644
--- a/source/armarx/manipulation/core/Robot.cpp
+++ b/source/armarx/manipulation/core/Robot.cpp
@@ -21,6 +21,7 @@
#include "Robot.h"
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
namespace armarx::manipulation::core
{
@@ -89,6 +90,40 @@ namespace armarx::manipulation::core
};
}
+ std::optional<armem::robot::ToFArray>
+ Robot::getToFInHand(const Hand hand, const armarx::core::time::DateTime& timestamp)
+ {
+ const armem::robot::RobotDescription desc{.name = robotName};
+
+ const auto tof = robotReader.queryToF(desc, timestamp);
+
+ if (not tof.has_value())
+ {
+ return std::nullopt;
+ }
+
+ armem::robot::ToFArray tofArray;
+ std::string frame;
+
+ switch (hand)
+ {
+ case Hand::Left:
+ ARMARX_CHECK(tof->count(armem::robot_state::RobotReader::Hand::Left) > 0);
+ tofArray = tof->at(armem::robot_state::RobotReader::Hand::Left);
+ frame = "TimeOfFlightLeft";
+ // frame = leftArmHelper.getTimeOfFlightSensorFrame();
+ break;
+ case Hand::Right:
+ ARMARX_CHECK(tof->count(armem::robot_state::RobotReader::Hand::Right) > 0);
+ tofArray = tof->at(armem::robot_state::RobotReader::Hand::Right);
+ frame = "TimeOfFlightRight";
+ // frame = rightArmHelper.getTimeOfFlightSensorFrame();
+ break;
+ }
+
+ return tofArray;
+ }
+
RobotUnitInterfacePrx
Robot::robotUnit() const
diff --git a/source/armarx/manipulation/core/Robot.h b/source/armarx/manipulation/core/Robot.h
index fc2b55d0..5d10f1eb 100644
--- a/source/armarx/manipulation/core/Robot.h
+++ b/source/armarx/manipulation/core/Robot.h
@@ -37,6 +37,7 @@
#include "RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h"
#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include "RobotAPI/libraries/core/FramedPose.h"
@@ -82,6 +83,9 @@ namespace armarx::manipulation::core
std::optional<ForceTorque>
getForceTorqueInHand(Hand hand, const armarx::DateTime& timestamp = armarx::Clock::Now());
+ std::optional<armem::robot::ToFArray>
+ getToFInHand(Hand hand, const armarx::DateTime& timestamp = armarx::Clock::Now());
+
armarx::RobotUnitInterfacePrx robotUnit() const;
armarx::HandUnitInterfacePrx handUnit(Hand hand) const;
--
GitLab