Fix gaze_targets outstream display bug

source/armarx/view_selection/gaze_controller/GazeController.cpp

@@ -31,19 +31,15 @@ namespace armarx::view_selection::gaze_controller
             ARMARX_CHECK_NOT_NULL(_rtYawNode);
             ARMARX_CHECK_NOT_NULL(_rtTorsoNode);
             // joint positions to be controlled
-            armarx::ControlTarget1DoFActuatorPosition* pitchCtrlTarget =
-                useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
-                    config->pitchNodeName, ControlModes::Position1DoF);
-            ARMARX_CHECK_NOT_NULL(pitchCtrlTarget);
-            _rtPitchCtrlPos = &(pitchCtrlTarget->position);
-            armarx::ControlTarget1DoFActuatorPosition* yawCtrlTarget =
-                useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
-                    _config->yawNodeName, ControlModes::Position1DoF);
-            ARMARX_CHECK_NOT_NULL(yawCtrlTarget);
-            _rtYawCtrlPos = &(yawCtrlTarget->position);
+            _rtPitchCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
+                _config->pitchNodeName, ControlModes::Position1DoF);
+            ARMARX_CHECK_NOT_NULL(_rtPitchCtrlTarget);
+            //_rtPitchCtrlPos = &(_pitchCtrlTarget->position);
+            _rtYawCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
+                _config->yawNodeName, ControlModes::Position1DoF);
+            ARMARX_CHECK_NOT_NULL(_rtYawCtrlTarget);
+            //_rtYawCtrlPos = &(yawCtrlTarget->position);
         }
-        _currentTarget = gaze_targets::GazeTarget();
-        ARMARX_IMPORTANT << _currentTarget;
         _controlActive = false;
     }
 
@@ -168,14 +164,14 @@ namespace armarx::view_selection::gaze_controller
             _publishTargetPitchAngle = pitch;
 
             // update control positions
-            *_rtYawCtrlPos = yaw;
-            *_rtPitchCtrlPos = pitch;
+            _rtYawCtrlTarget->position = yaw;
+            _rtPitchCtrlTarget->position = pitch;
         }
         else
         {
             // Needed to assure valid targets after deactivation
-            *_rtYawCtrlPos = 0;
-            *_rtPitchCtrlPos = 0;
+            _rtYawCtrlTarget->position = 0;
+            _rtPitchCtrlTarget->position = 0;
         }
     }
 
@@ -208,7 +204,7 @@ namespace armarx::view_selection::gaze_controller
         if (_tripRt2NonRt.updateReadBuffer())
         {
             gaze_targets::GazeTarget target = _tripRt2NonRt.getReadBuffer();
-            std::async(std::launch::async, [this, target]() { this->publishTarget(target); });
+            this->publishTarget(target);
         }
         debugObserver->setDebugChannel(getInstanceName(), datafields);
     }
@@ -237,7 +233,7 @@ namespace armarx::view_selection::gaze_controller
         w.controlTarget = target;
         _tripBufTarget.commitWrite();
         // feedback to scheduler
-        ARMARX_INFO << "new target: " << target;
+        ARMARX_INFO << "new target: " << target.name;
         publishTarget(target);
     }
 

source/armarx/view_selection/gaze_controller/GazeController.h

@@ -123,8 +123,10 @@ namespace armarx::view_selection::gaze_controller
         VirtualRobot::RobotNodePtr _rtCameraNode;
         VirtualRobot::RobotNodePtr _rtTorsoNode;
 
-        float* _rtPitchCtrlPos = nullptr;
-        float* _rtYawCtrlPos = nullptr;
+        armarx::ControlTarget1DoFActuatorPosition* _rtPitchCtrlTarget;
+        armarx::ControlTarget1DoFActuatorPosition* _rtYawCtrlTarget;
+        float* _rtPitchCtrlPos;
+        float* _rtYawCtrlPos;
 
         // std::unique_ptr<MultiDimPIDControllerTemplate<2>> _pid;
         gaze_targets::GazeTarget _currentTarget;

source/armarx/view_selection/gaze_targets/GazeTarget.cpp

@@ -26,6 +26,7 @@ namespace armarx::view_selection::gaze_targets
 {
     GazeTarget::GazeTarget()
     {
+        creationTimestamp = core::time::DateTime::Now();
     }
 
     GazeTarget::GazeTarget(std::string name,

source/armarx/view_selection/gaze_targets/GazeTarget.h

@@ -38,19 +38,19 @@ namespace armarx::view_selection::gaze_targets
     {
     public:
         // General properties
-        std::string name;
+        std::string name = "";
         FramedPosition position;
         // Scheduler values
-        TargetPriority priority;
+        TargetPriority priority = TargetPriority(AttentionType::RandomEvent, 0.0);
         armarx::core::time::Duration duration;
-        bool keepInQueue;
+        bool keepInQueue = false;
         // Event logging
-        TargetStatus status;
-        armarx::DateTime creationTimestamp;
-        armarx::DateTime activationTimestamp;
-        armarx::DateTime reachedTimestamp;
-        armarx::DateTime releasedTimestamp;
-        armarx::DateTime abortedTimestamp;
+        TargetStatus status = TargetStatus::Requested;
+        armarx::DateTime creationTimestamp = armarx::DateTime(0);
+        armarx::DateTime activationTimestamp = armarx::DateTime(0);
+        armarx::DateTime reachedTimestamp = armarx::DateTime(0);
+        armarx::DateTime releasedTimestamp = armarx::DateTime(0);
+        armarx::DateTime abortedTimestamp = armarx::DateTime(0);
 
         GazeTarget();
         explicit GazeTarget(std::string targetName,

source/armarx/view_selection/gaze_targets/TargetPriority.cpp

@@ -74,7 +74,8 @@ namespace armarx::view_selection::gaze_targets
     std::ostream&
     operator<<(std::ostream& os, const TargetPriority& prio)
     {
-        return os << "{ " << prio.attentionType << ", " << prio.priority << " }";
+        return os << "{ " << AttentionTypeNames.to_name(prio.attentionType) << ", " << prio.priority
+                  << " }";
     }
 
 } // namespace armarx::view_selection::gaze_targets