gaze controller: cleanup

source/armarx/view_selection/gaze_controller/GazeController.cpp

 
 #include "GazeController.h"
 
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/interface/observers/VariantBase.h>
 
-#include "RobotAPI/components/units/RobotUnit/ControlModes.h"
-#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h"
+#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
 
-#include "armarx/view_selection/common/controller_types.h"
+#include <armarx/view_selection/common/controller_types.h>
 #include <armarx/view_selection/gaze_controller/aron/GazeControllerConfig.aron.generated.h>
 #include <armarx/view_selection/gaze_controller/aron_conversions.h>
 
@@ -21,7 +23,7 @@ namespace armarx::view_selection::gaze_controller
                                    const NJointControllerConfigPtr& config,
                                    const VirtualRobot::RobotPtr&)
     {
-        ARMARX_INFO << "Creating gaze controller";
+        ARMARX_RT_LOGF("Creating gaze controller");
         // config
         ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
         ARMARX_CHECK_EXPRESSION(cfg);
@@ -32,7 +34,7 @@ namespace armarx::view_selection::gaze_controller
         const auto configData = arondto::Config::FromAron(cfg->config);
 
         // robot
-        ARMARX_INFO << "Setting up nodes";
+        ARMARX_RT_LOGF("Setting up nodes");
         {
             _rtRobot = useSynchronizedRtRobot();
             ARMARX_CHECK_NOT_NULL(_rtRobot);
@@ -54,15 +56,17 @@ namespace armarx::view_selection::gaze_controller
             ARMARX_CHECK_NOT_NULL(_rtYawCtrlTarget);
             //_rtYawCtrlPos = &(yawCtrlTarget->position);
         }
+
+        ARMARX_RT_LOGF("Nodes set up successfully");
     }
 
     GazeController::~GazeController() = default;
 
     void
     GazeController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
-                                 const Ice::Current& iceCurrent)
+                                 const Ice::Current&  /*iceCurrent*/)
     {
-        ARMARX_INFO << "Controller::updateConfig";
+        // ARMARX_VERBOSE << "Controller::updateConfig";
 
         auto updateConfigDto = arondto::Config::FromAron(dto);
 
@@ -89,19 +93,21 @@ namespace armarx::view_selection::gaze_controller
     }
 
     void
-    GazeController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
-                          const IceUtil::Time& timeSinceLastIteration)
+    GazeController::rtRun(const IceUtil::Time&  /*sensorValuesTimestamp*/,
+                          const IceUtil::Time&  /*timeSinceLastIteration*/)
     {
-        const auto now = armarx::core::time::DateTime::Now();
+        auto target = rtGetControlStruct().targets.gazeTarget.position;
 
-        ARMARX_INFO << deactivateSpam(1) << "Current target is "
-                    << rtGetControlStruct().targets.gazeTarget;
+        if(target.frame.empty())
+        {
+            target.frame = GlobalFrame;
+        }
 
         // calculate inverse kinematic
         const float currentYawAngle = _rtYawNode->getJointValue();
         const float currentPitchAngle = _rtPitchNode->getJointValue();
-        const Eigen::Vector3f targetPoint =
-            rtGetControlStruct().targets.gazeTarget.position.toRootEigen(_rtRobot);
+
+        const Eigen::Vector3f targetPoint = target.toRootEigen(_rtRobot);
 
         const float h = _rtCameraNode->getPositionInRootFrame().z();
         float targetYawAngle = -std::atan2(targetPoint.x(), targetPoint.y());