diff --git a/source/armarx/view_selection/components/scheduler_example/Component.cpp b/source/armarx/view_selection/components/scheduler_example/Component.cpp
index 9fd0b17be733b60c1d7e27b464d42ec1eba189be..39407b1addcc34b3b6bf703ba2de6469f84d06c0 100644
--- a/source/armarx/view_selection/components/scheduler_example/Component.cpp
+++ b/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);
diff --git a/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp b/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp
index cb41c52ff547051d943cef3329c297306486c63e..d667ac09301f4628f946c9fd776bfc5b3d8fe76c 100644
--- a/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp
+++ b/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;
diff --git a/source/armarx/view_selection/gaze_scheduler/Scheduler.h b/source/armarx/view_selection/gaze_scheduler/Scheduler.h
index 077f4b4ab4f8ebda8bc4dc549c67515c4882a1da..afa9511cb47a562ca6052695cff9cd9bd2819c4a 100644
--- a/source/armarx/view_selection/gaze_scheduler/Scheduler.h
+++ b/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