From 53fdbd8a41ef0ae0c92be2f5b70a5746c8ccbc53 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Fri, 13 May 2022 11:09:51 +0200
Subject: [PATCH] fix timing bug for inverse-priority target request by
assuring that there is only one active control request at any time
---
scenarios/ViewScheduler/ViewScheduler.scx | 1 +
.../ViewScheduler/config/ArVizStorage.cfg | 212 ++++++++++++++++++
.../scheduler_example/Component.cpp | 52 +++--
.../gaze_scheduler/Scheduler.cpp | 43 ++--
.../view_selection/gaze_scheduler/Scheduler.h | 2 +-
5 files changed, 267 insertions(+), 43 deletions(-)
create mode 100644 scenarios/ViewScheduler/config/ArVizStorage.cfg
diff --git a/scenarios/ViewScheduler/ViewScheduler.scx b/scenarios/ViewScheduler/ViewScheduler.scx
index d29d557..59cfee3 100644
--- a/scenarios/ViewScheduler/ViewScheduler.scx
+++ b/scenarios/ViewScheduler/ViewScheduler.scx
@@ -4,5 +4,6 @@
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="view_memory_client" instance="" package="armarx_view_selection" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="view_memory" instance="" package="armarx_view_selection" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ViewScheduler/config/ArVizStorage.cfg b/scenarios/ViewScheduler/config/ArVizStorage.cfg
new file mode 100644
index 0000000..302ac28
--- /dev/null
+++ b/scenarios/ViewScheduler/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArVizStorage.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history is serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/source/armarx/view_selection/components/scheduler_example/Component.cpp b/source/armarx/view_selection/components/scheduler_example/Component.cpp
index 39407b1..a28b35f 100644
--- a/source/armarx/view_selection/components/scheduler_example/Component.cpp
+++ b/source/armarx/view_selection/components/scheduler_example/Component.cpp
@@ -82,41 +82,39 @@ namespace armarx::view_selection::components::scheduler_example
void
Component::onConnectComponent()
{
- // Do things after connecting to topics and components.
- ARMARX_INFO << "Testing Scheduler.";
viz::Layer layer = arviz.layer(properties.boxLayerName);
+ ARMARX_INFO << "Testing Scheduler.";
auto taskAttention = client::dto::AttentionType::AttentionTypeEnum::TaskDriven;
- auto stimulusAttention = client::dto::AttentionType::AttentionTypeEnum::StimulusDriven;
- auto duration = core::time::dto::Duration(1000000);
- auto duration_infinite = core::time::dto::Duration(-1000000);
- gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(0, 4000, 2000), "root", "Armar6"), {taskAttention, 0.4}, duration);
+ auto oneSecond = core::time::dto::Duration(1000000);
+ //the first request will be replaced by next target with higher priority
+ gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(0, 4000, 2000), "root", "Armar6"), {taskAttention, 0.4}, oneSecond);
layer.add(viz::Sphere("target_6")
.position(Eigen::Vector3f(0, 4000, 2000))
.radius(50).color(simox::Color::blue()));
- //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")
- .position(Eigen::Vector3f(500, 2000, 1000))
- .radius(10).color(simox::Color::blue()));
- gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(-500, 2000, 1000), "root", "Armar6"), {taskAttention, 0.7}, duration);
- layer.add(viz::Sphere("target_4")
- .position(Eigen::Vector3f(-500, 2000, 1000))
- .radius(10).color(simox::Color::blue()));
- gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, 1500, 1000), "root", "Armar6"), {taskAttention, 0.5}, duration);
+ gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, -2000, 1000), "Global", "Armar6"), {taskAttention, 1.5}, oneSecond);
+ layer.add(viz::Sphere("target_2")
+ .position(Eigen::Vector3f(500, -2000, 1000))
+ .radius(50).color(simox::Color::blue()));
+ //test inverse order
+ gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, -1000, 1000), "Global", "Armar6"), {taskAttention, 0.5}, oneSecond);
layer.add(viz::Sphere("target_5")
.position(Eigen::Vector3f(500, -1000, 1000))
.radius(20).color(simox::Color::blue()));
- //gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(0, 4000, 2000), "root", "Armar6"), {taskAttention, 0.4}, duration);
- //layer.add(viz::Sphere("target_6")
- // .position(Eigen::Vector3f(0, 4000, 2000))
- // .radius(50).color(simox::Color::blue()));
-
- //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);
+ gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(-500, 2000, 1000), "root", "Armar6"), {taskAttention, 0.7}, oneSecond);
+ layer.add(viz::Sphere("target_4")
+ .position(Eigen::Vector3f(-500, 2000, 1000))
+ .radius(10).color(simox::Color::blue()));
+ gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(500, 2000, 1000), "root", "Armar6"), {taskAttention, 0.8}, oneSecond);
+ layer.add(viz::Sphere("target_3")
+ .position(Eigen::Vector3f(500, 2000, 1000))
+ .radius(10).color(simox::Color::blue()));
+ //test infinite duration
+ auto stimulusAttention = client::dto::AttentionType::AttentionTypeEnum::StimulusDriven;
+ auto duration_infinite = core::time::dto::Duration(-1000000);
+ gazeScheduler->fixateFor(new FramedPosition(Eigen::Vector3f(1500, -2000, 1000), "Global", "Armar6"), {stimulusAttention, 10.0}, duration_infinite);
+ layer.add(viz::Sphere("target_infinite")
+ .position(Eigen::Vector3f(500, -2000, 1000))
+ .radius(50).color(simox::Color::blue()));
arviz.commit(layer);
drawRobot();
diff --git a/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp b/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp
index d667ac0..30e4f33 100644
--- a/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp
+++ b/source/armarx/view_selection/gaze_scheduler/Scheduler.cpp
@@ -124,7 +124,7 @@ namespace armarx::view_selection::gaze_scheduler
void Scheduler::fixateFor(const FramedPositionBasePtr& target, const client::dto::TargetPriority& priority,
const core::time::dto::Duration& duration, const Ice::Current& c)
{
- std::scoped_lock<std::mutex>targetLock(targetMutex);
+ std::lock_guard<std::mutex>targetLock(targetMutex);
client::TargetPriority requestedPriority;
client::fromIce(priority, requestedPriority);
core::time::Duration requestedDuration;
@@ -146,9 +146,15 @@ namespace armarx::view_selection::gaze_scheduler
auto newTarget = requestedTargets.top();
requestedTargets.pop();
submittedTargets.push_back(newTarget);
- armarx::core::time::dto::DateTime requestTimestamp;
- armarx::core::time::toIce(requestTimestamp, newTarget.requestTimestamp);
- controller->submitTarget(newTarget.target, requestTimestamp);
+ if (!openControlRequest)
+ {
+ openControlRequest = true;
+ armarx::core::time::dto::DateTime requestTimestamp;
+ armarx::core::time::toIce(requestTimestamp, newTarget.requestTimestamp);
+ controller->submitTarget(newTarget.target, requestTimestamp);
+ } else {
+ ARMARX_INFO << "The last controlRequest is still in progress, waiting for memoryUpdate.";
+ }
}
else if (submittedTargets.back().priority < requestedTargets.top().priority)
{
@@ -156,12 +162,14 @@ namespace armarx::view_selection::gaze_scheduler
<< submittedTargets.back().priority << " < " << requestedTargets.top().priority
<< ") removing it";
//replaced target needs to be stopped properly, then next target will be scheduled automatically
- if (!openStopRequest)
+ if (!openControlRequest)
{
- openStopRequest = true;
+ openControlRequest = true;
armarx::core::time::dto::Duration zero;
armarx::core::time::toIce(zero, armarx::core::time::Duration());
controller->removeTargetAfter(zero);
+ } else {
+ ARMARX_INFO << "The last controlRequest is still in progess, waiting for memoryUpdate.";
}
//TODO: does the target need to be reinserted in the request queue?
//requestedTargets.push(submittedTargets.back());
@@ -187,24 +195,29 @@ namespace armarx::view_selection::gaze_scheduler
{
submittedTargets.erase(submittedTargets.begin() + i);
//enable stop requests only after erasing their target to prevent multiple consecutive calls
- openStopRequest = false;
+ openControlRequest = false;
scheduleNextTarget();
return;
}
+ else if (memoryTarget.isAborted())
+ {
+ ARMARX_WARNING << "The target is out of reach and has been aborted.";
+ submittedTargets.erase(submittedTargets.begin() + i);
+ openControlRequest = false;
+ scheduleNextTarget();
+ return;
+ }
+ //targets with negative Duration continue until a target with higher Priority is submitted
if (memoryTarget.isReached() && submittedTargets[i].duration.toMicroSeconds() >= 0)
{
armarx::core::time::dto::Duration duration;
armarx::core::time::toIce(duration, submittedTargets[i].duration);
- openStopRequest = true;
+ openControlRequest = true;
controller->removeTargetAfter(duration);
+ return;
}
- else if (memoryTarget.isAborted())
- {
- armarx::core::time::dto::Duration zero;
- armarx::core::time::toIce(zero, armarx::core::time::Duration());
- openStopRequest = true;
- controller->removeTargetAfter(zero);
- }
+ //if none of the cases above match, the target has been successfully submitted
+ openControlRequest = false;
}
}
}
diff --git a/source/armarx/view_selection/gaze_scheduler/Scheduler.h b/source/armarx/view_selection/gaze_scheduler/Scheduler.h
index afa9511..4579545 100644
--- a/source/armarx/view_selection/gaze_scheduler/Scheduler.h
+++ b/source/armarx/view_selection/gaze_scheduler/Scheduler.h
@@ -152,7 +152,7 @@ namespace armarx::view_selection::gaze_scheduler
};
//to prevent multiple consecutive stop requests
- std::atomic<bool> openStopRequest = false;
+ std::atomic<bool> openControlRequest = false;
//target managemant needs to be synchronized
std::mutex targetMutex;
std::map<long, long> openTargets;
--
GitLab