Johann Mantel · Fabian Reister
--- a/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
+++ b/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
                "The type of NJointController to use, e.g. GazeController");
  def->optional(properties.controllerName, "controllerName",
                "The name under which the controller is registered");
-  def->optional(properties.kp, "kp", "Control gain kp");
  def->optional(properties.robotName, "properties.RobotName",
                "Name of the robot for framedPosition.");
  def->optional(properties.robotFrame, "properties.robotFrameName",
                "Name of the local robotFrame for framedPosition.");
+  // listen to Controller updates
+  def->topic<GazeControllerListener>("GazeTargets");
--- a/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
+++ b/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
-  std::lock_guard<std::mutex> targetLock(targetMutex);

-  ARMARX_WARNING
-      << "Resetting PriorityQueue for targets, old requests are discarded";
-  requestedTargets = std::priority_queue<TargetRequest>();
+  ARMARX_INFO << "Resetting PriorityQueue, old requests are discarded";
+  std::lock_guard<std::mutex> targetLock(targetMutex);
+  requestedTargets.clear();
 }

 void Scheduler::scheduleNextTarget() {
  // prevent communicaton loss by waiting for controller feedback
  if (!openControlRequest) {
+
+    // reference to targetRequest with highest priority
+    auto nextTarget = requestedTargets.cbegin();
--- a/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
+++ b/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
 void Scheduler::resetPriorityQueue(const Ice::Current &) {
-  std::lock_guard<std::mutex> targetLock(targetMutex);

-  ARMARX_WARNING
-      << "Resetting PriorityQueue for targets, old requests are discarded";
-  requestedTargets = std::priority_queue<TargetRequest>();
+  ARMARX_INFO << "Resetting PriorityQueue, old requests are discarded";
+  std::lock_guard<std::mutex> targetLock(targetMutex);
+  requestedTargets.clear();
 }

 void Scheduler::scheduleNextTarget() {
  // prevent communicaton loss by waiting for controller feedback
  if (!openControlRequest) {
+
+    // reference to targetRequest with highest priority
--- a/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
+++ b/source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp
-      controller->submitTarget(newTarget.target, requestTimestamp);
-    } else if (requestedTargets.top().priority >=
-               submittedTargets.back().priority) {
-      ARMARX_INFO << "requested Target priority >= current Target priority ("
-                  << requestedTargets.top().priority
-                  << " >= " << submittedTargets.back().priority << ")";
+
+    } else if (nextTarget->priority > submittedTargets.back().priority) {
+      ARMARX_INFO << "requested Target priority > current Target priority ("
+                  << nextTarget->priority << " > "
+                  << submittedTargets.back().priority << ")";
      // replaced target needs to be stopped properly, then next target will be
      // scheduled automatically
-      if (requestedTargets.top().preemptable) {
-        ARMARX_INFO << "current Target is being preempted";
        armarx::core::time::dto::Duration zero;