fix

source/armarx/view_selection/target_provider/handover/RobotReceiver.cpp

@@ -28,10 +28,29 @@
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include "ArmarXCore/core/logging/Logging.h"
 
+#include "RobotAPI/libraries/core/FramedPose.h"
+
 #include "armarx/view_selection/target_provider/handover/util.h"
 
 namespace armarx::view_selection::target_provider::handover
 {
+    std::vector<gaze_targets::GazeTarget>
+    RobotReceiver::updateTargets(const armem::human::HumanPose& human)
+    {
+        std::vector<gaze_targets::GazeTarget> targets;
+
+        switch (phase_)
+        {
+            case Phase::PreHandover:
+                targets = updateTargetsPreHandover(human);
+                break;
+            case Phase::PostHandover:
+                targets = updateTargetsPreHandover(human);
+                break;
+        }
+
+        return targets;
+    }
 
 
     std::vector<gaze_targets::GazeTarget>
@@ -172,13 +191,6 @@ namespace armarx::view_selection::target_provider::handover
             const armem::human::PoseKeypoint rightWristKp =
                 human.keypoints.at(rightWristKpName.value());
 
-            const float handLeftRobotDistance =
-                leftWristKp.positionRobot->toEigen().head<2>().norm();
-            const float handRightRobotDistance =
-                rightWristKp.positionRobot->toEigen().head<2>().norm();
-
-            const float minDistance = std::min(handLeftRobotDistance, handRightRobotDistance);
-
             const float alpha = 0.5; // might be improved in the future
 
             const Eigen::Vector3f centerHandsPosition =
@@ -186,19 +198,23 @@ namespace armarx::view_selection::target_provider::handover
                 alpha * (rightWristKp.positionGlobal->toEigen() -
                          leftWristKp.positionGlobal->toEigen());
 
-            const float centerHandsRobotDistance = (centerHandsPosition - robot_->getGlobalPosition()).head<2>().norm();
+            const float centerHandsRobotDistance =
+                (centerHandsPosition - robot_->getGlobalPosition()).head<2>().norm();
 
             const float priority = handsPriorityFromDistance(centerHandsRobotDistance);
 
             const gaze_targets::TargetPriority targetPriority(
                 gaze_targets::AttentionType::TaskDriven, priority);
 
-            gaze_targets::GazeTarget handTarget("center_hands",
-                                                centerHandsPosition,
-                                                targetPriority,
-                                                Duration::Seconds(1),
-                                                false);
-            gazeTargets.push_back(handTarget);
+            const std::string& agent = leftWristKp.positionGlobal->agent;
+
+            gaze_targets::GazeTarget handsTarget(
+                "center_hands",
+                FramedPosition(centerHandsPosition, GlobalFrame, agent),
+                targetPriority,
+                Duration::Seconds(1),
+                false);
+            gazeTargets.push_back(handsTarget);
         }
 
         // combined wrist