Robot: getToFInHand

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

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;