scheduler: using new controller api

source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp

@@ -24,7 +24,6 @@
 
 #include "ArmarXCore/core/logging/Logging.h"
 
-// Include headers you only need in function definitions in the .cpp.
 #include <chrono>
 #include <thread>
 
@@ -37,9 +36,10 @@
 #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 
-#include "armarx/view_selection/gaze_targets.h"
+#include <armarx/view_selection/gaze_targets.h>
 #include <armarx/view_selection/gaze_controller/aron/GazeControllerConfig.aron.generated.h>
 #include <armarx/view_selection/memory/constants.h>
+#include <armarx/view_selection/gaze_controller/aron_conversions.h>
 
 namespace armarx::view_selection::gaze_scheduler
 {
@@ -101,7 +101,7 @@ namespace armarx::view_selection::gaze_scheduler
         //populate the robot
         RobotNodeNames robotNodeNames = getRobotNodeNames(properties.robotName);
 
-        gaze_controller::arondto::Config config;
+        gaze_controller::Config config;
         config.params.yawNodeName = robotNodeNames.yawNodeName;
         config.params.pitchNodeName = robotNodeNames.pitchNodeName;
         config.params.cameraNodeName = robotNodeNames.cameraNodeName;
@@ -117,9 +117,24 @@ namespace armarx::view_selection::gaze_scheduler
             auto builder = getControlComponentPlugin().createControllerBuilder<
                 armarx::view_selection::common::ControllerType::GazeController>();
 
-            auto ctrlWrapper = builder.withNodeSet("GazeController").withConfig(config).create();
+            //const armarx::PackagePath configPath(
+            //    "armarx_view_selection", "controller_config/GazeController/default.json");
+
+            ARMARX_INFO << "Initializing controller now";
+            // auto ctrlWrapper = builder.withNodeSet("Head").withConfig(configPath.toSystemPath()).create();
+            
+            gaze_controller::arondto::Config dtoConfig;
+            toAron(dtoConfig, config);
+            
+            ARMARX_IMPORTANT << "builder.withNodeSet().withConfig";
+            builder.withNodeSet("Head").withConfig(dtoConfig);
+
+            ARMARX_IMPORTANT << "builder.create()";
+            auto ctrlWrapper = builder.create();
             ARMARX_TRACE;
             ARMARX_CHECK(ctrlWrapper.has_value());
+
+            ARMARX_INFO << "Successfully created controller";
             ctrl.emplace(std::move(ctrlWrapper.value()));
         }
 
@@ -127,6 +142,7 @@ namespace armarx::view_selection::gaze_scheduler
         ARMARX_CHECK(ctrl.has_value());
 
         //Properties are loaded OK
+        ARMARX_INFO << "Activating controller";
         ctrl->activate();
 
         // subscribe to memory updates
@@ -203,7 +219,10 @@ namespace armarx::view_selection::gaze_scheduler
     
     void Scheduler::updateControllerTarget(const gaze_targets::GazeTarget& gazeTarget)
     {
-        toAron(ctrl->config.targets.gazeTarget, gazeTarget);
+        armarx::view_selection::gaze_targets::arondto::GazeTarget dto;
+        toAron(dto, gazeTarget);
+
+        ctrl->config.targets.gazeTarget = dto;
         ctrl->updateConfig();
     }
 
@@ -218,10 +237,6 @@ namespace armarx::view_selection::gaze_scheduler
                 if (currentTarget.name == target.name)
                 {
                     ARMARX_VERBOSE << "Updating active target: " << target.name;
-                    submittedTargets.back() = target;
-
-                    openControlRequest = true;
-
                     updateControllerTarget(target);
                     
                     return;
@@ -279,39 +294,7 @@ namespace armarx::view_selection::gaze_scheduler
             }
             else
             {
-<<<<<<< HEAD
-                // reference to targetRequest with highest priority
-                // this will be only used in one of the if-else cases so that they do not
-                // influence each other
-                auto nextTarget = requestedTargets.cbegin();
-
-                if (submittedTargets.empty())
-                {
-                    ARMARX_INFO << "No currently active Targets, submitting new target: "
-                                << *nextTarget;
-
-                    // keep track of submittedTargets, copy requestedTarget to submitted
-                    submittedTargets.push_back(*nextTarget);
-
-                    openControlRequest = true;
-                    
-                    updateControllerTarget(*nextTarget);
-
-                    // keep or erase target after submission based on keepInQueue
-                    if (nextTarget->keepInQueue)
-                    {
-                        ARMARX_VERBOSE << "The target is kept in the queue.";
-                    }
-                    else
-                    {
-                        requestedTargets.erase(nextTarget);
-                        ARMARX_VERBOSE << "The target has been removed from the queue.";
-                    }
-                }
-                else if (nextTarget->priority > submittedTargets.back().priority)
-=======
                 if (nextTarget->priority > currentTarget.priority)
->>>>>>> origin/master
                 {
                     ARMARX_INFO << "requested Target priority > current Target priority ("
                                 << nextTarget->priority << " > " << currentTarget.priority << ")";
@@ -319,7 +302,6 @@ namespace armarx::view_selection::gaze_scheduler
                     // scheduled automatically
                     armarx::core::time::dto::Duration zero;
                     armarx::core::time::toIce(zero, armarx::core::time::Duration());
-                    openControlRequest = true;
                     // controller->removeTargetAfter(zero);
                 }
                 else
@@ -347,9 +329,7 @@ namespace armarx::view_selection::gaze_scheduler
         currentTarget = target;
         activeTarget = true;
 
-        gaze_targets::arondto::GazeTarget dto;
-        toAron(dto, target);
-        controller->submitTarget(dto.toAronDTO());
+        updateControllerTarget(target);
     }
 
     void

source/armarx/view_selection/components/gaze_scheduler/Scheduler.h

@@ -22,7 +22,6 @@
 
 #pragma once
 
-#include <iostream>
 #include <mutex>
 #include <queue>
 
@@ -38,7 +37,6 @@
 #include "armarx/view_selection/gaze_targets/GazeTarget.h"
 #include <armarx/view_selection/components/gaze_scheduler/GazeSchedulerInterface.h>
 #include <armarx/view_selection/gaze_controller/GazeController.h>
-#include <armarx/view_selection/gaze_controller/GazeControllerInterface.h>
 #include <armarx/view_selection/gaze_targets.h>
 #include <armarx/view_selection/common/controller_types.h>