add stopRequest synchronization to GazeScheduler

source/armarx/view_selection/components/scheduler_example/Component.cpp

@@ -96,6 +96,7 @@ namespace armarx::view_selection::components::scheduler_example
         //can not call consecutive targets with inverse order without getting stuck
         //TODO:resolve timing bug of inverse priority requests
         armarx::Clock::WaitFor(armarx::core::time::Duration::MilliSeconds(100));
+        gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, -2000, 1000), "Global", "Armar6"), {taskAttention, 1.5}, duration);
 
         gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, 2000, 1000), "root", "Armar6"), {taskAttention, 0.8}, duration);
         layer.add(viz::Sphere("target_3")
@@ -114,7 +115,6 @@ namespace armarx::view_selection::components::scheduler_example
         //          .position(Eigen::Vector3f(0, 4000, 2000))
         //          .radius(50).color(simox::Color::blue()));
 
-        gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, -2000, 1000), "Global", "Armar6"), {taskAttention, 1.5}, duration);
         //gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(1500, -2000, 1000), "Global", "Armar6"), {stimulusAttention, 10.0}, duration_infinite);
         //gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, -2000, 1000), "Global", "Armar6"), {stimulusAttention, 11.0}, duration);
         arviz.commit(layer);

source/armarx/view_selection/gaze_scheduler/Scheduler.cpp

@@ -87,13 +87,6 @@ namespace armarx::view_selection::gaze_scheduler
         controller = armarx::control::gaze_controller::GazeControllerInterfacePrx::checkedCast(
                          robotUnit->createNJointController("GazeController", "ViewController", config));
         controller->activateController();
-
-        //for testing
-        this->task = new armarx::SimpleRunningTask<>([this]()
-        {
-            this->run();
-        });
-        this->task->start();
     }
 
 
@@ -159,16 +152,20 @@ namespace armarx::view_selection::gaze_scheduler
         }
         else if (submittedTargets.back().priority < requestedTargets.top().priority)
         {
-            ARMARX_INFO << "current Target has lower priority ("
+            ARMARX_INFO << "current Target has lower priority than next in Queue ("
                         << submittedTargets.back().priority << " < " << requestedTargets.top().priority
-                        << ") replacing until later";
+                        << ") removing it";
             //replaced target needs to be stopped properly, then next target will be scheduled automatically
+            if (!openStopRequest)
+            {
+                openStopRequest = true;
+                armarx::core::time::dto::Duration zero;
+                armarx::core::time::toIce(zero, armarx::core::time::Duration());
+                controller->removeTargetAfter(zero);
+            }
             //TODO: does the target need to be reinserted in the request queue?
             //requestedTargets.push(submittedTargets.back());
             //submittedTargets.pop_back();
-            armarx::core::time::dto::Duration zero;
-            armarx::core::time::toIce(zero, armarx::core::time::Duration());
-            controller->removeTargetAfter(zero);
         }
         else
         {
@@ -189,6 +186,8 @@ namespace armarx::view_selection::gaze_scheduler
                 if (memoryTarget.isReleased())
                 {
                     submittedTargets.erase(submittedTargets.begin() + i);
+                    //enable stop requests only after erasing their target to prevent multiple consecutive calls
+                    openStopRequest = false;
                     scheduleNextTarget();
                     return;
                 }
@@ -196,12 +195,14 @@ namespace armarx::view_selection::gaze_scheduler
                 {
                     armarx::core::time::dto::Duration duration;
                     armarx::core::time::toIce(duration, submittedTargets[i].duration);
+                    openStopRequest = true;
                     controller->removeTargetAfter(duration);
                 }
                 else if (memoryTarget.isAborted())
                 {
                     armarx::core::time::dto::Duration zero;
                     armarx::core::time::toIce(zero, armarx::core::time::Duration());
+                    openStopRequest = true;
                     controller->removeTargetAfter(zero);
                 }
             }
@@ -245,7 +246,7 @@ namespace armarx::view_selection::gaze_scheduler
         {
             ARMARX_INFO << "Query failed: " << result.errorMessage;
         }
-        ARMARX_INFO << "Memory updated, " << submittedTargets.size() << " open Targets:";
+        ARMARX_INFO << "Memory updated, " << submittedTargets.size() << " open Target:";
         for (auto target : submittedTargets)
         {
             ARMARX_INFO << target;

source/armarx/view_selection/gaze_scheduler/Scheduler.h

@@ -29,11 +29,11 @@
 
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/time.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
-//#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
 
 #include <armarx/control/gaze_controller/gaze_target/GazeTarget.h>
 #include <armarx/control/gaze_controller/GazeControllerInterface.h>
@@ -41,9 +41,6 @@
 #include <armarx/view_selection/client/ice/GazeSchedulerInterface.h>
 #include <armarx/view_selection/client.h>
 
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
-
 namespace armarx::view_selection::gaze_scheduler
 {
 
@@ -105,10 +102,6 @@ namespace armarx::view_selection::gaze_scheduler
         RobotUnitInterfacePrx robotUnit;
         armarx::control::gaze_controller::GazeControllerInterfacePrx controller;
 
-        std::mutex targetMutex;
-        std::map<long, long> openTargets;
-        armarx::SimpleRunningTask<>::pointer_type task = nullptr;
-
         struct TargetRequest
         {
             FramedPositionBasePtr target;
@@ -158,8 +151,14 @@ namespace armarx::view_selection::gaze_scheduler
             
         };
 
+        //to prevent multiple consecutive stop requests
+        std::atomic<bool> openStopRequest = false;
+        //target managemant needs to be synchronized
+        std::mutex targetMutex;
+        std::map<long, long> openTargets;
         std::priority_queue<TargetRequest> requestedTargets;
         std::vector<TargetRequest> submittedTargets;
+
     };
 
 }  // namespace armarx::view_selection::gaze_scheduler