diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 3cb09a7b0359bf73b5b577c48bf5545573c20b21..73305adb72fd432d59959e3a6c4f19b3b35d8e6a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -679,21 +679,25 @@ namespace armarx
CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
while (visu.updateTask && !visu.updateTask->isStopped())
{
- TIMING_START(Visu);
- if (visu.enabled)
{
- std::scoped_lock lock(dataMutex);
- visualizeCommit(data.objectPoses, data.robot);
- }
- if (visu.enabled)
- {
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
- if (debugObserver)
+ std::scoped_lock lock(visuMutex);
+
+ TIMING_START(Visu);
+ if (visu.enabled)
+ {
+ std::scoped_lock lock(dataMutex);
+ visualizeCommit(data.objectPoses, data.robot);
+ }
+ if (visu.enabled)
{
- debugObserver->setDebugChannel(getName(),
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ if (debugObserver)
{
- { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ });
+ }
}
}
cycle.waitForCycleDuration();
@@ -813,28 +817,32 @@ namespace armarx
CycleUtil cycle(static_cast<int>(1000 / decay.updateFrequencyHz));
while (decay.updateTask && !decay.updateTask->isStopped())
{
- TIMING_START(Decay);
- if (decay.enabled)
{
- std::scoped_lock lock(dataMutex);
- IceUtil::Time now = TimeUtil::GetTime();
- for (auto& [providerName, objectPoses] : data.objectPoses)
+ std::scoped_lock lock(decayMutex);
+
+ TIMING_START(Decay);
+ if (decay.enabled)
{
- for (objpose::ObjectPose& pose : objectPoses)
+ std::scoped_lock lock(dataMutex);
+ IceUtil::Time now = TimeUtil::GetTime();
+ for (auto& [providerName, objectPoses] : data.objectPoses)
{
- decay.updateConfidence(pose, now);
+ for (objpose::ObjectPose& pose : objectPoses)
+ {
+ decay.updateConfidence(pose, now);
+ }
}
}
- }
- if (decay.enabled)
- {
- TIMING_END_STREAM(Decay, ARMARX_VERBOSE);
- if (debugObserver)
+ if (decay.enabled)
{
- debugObserver->setDebugChannel(getName(),
+ TIMING_END_STREAM(Decay, ARMARX_VERBOSE);
+ if (debugObserver)
{
- { "Decay Update [ms]", new Variant(Decay.toMilliSecondsDouble()) },
- });
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Decay Update [ms]", new Variant(Decay.toMilliSecondsDouble()) },
+ });
+ }
}
}
@@ -973,7 +981,52 @@ namespace armarx
robotHeadGroup.addChild(grid);
}
- VBoxLayout root = {visuGroup, robotHeadGroup, VSpacer()};
+ GroupBox decayGroup;
+ {
+ tab.decay.enabled.setValue(decay.enabled);
+ {
+ float max = 1e6;
+ tab.decay.delaySeconds.setRange(0, max);
+ tab.decay.delaySeconds.setDecimals(2);
+ tab.decay.delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
+ tab.decay.delaySeconds.setValue(decay.delaySeconds);
+ }
+ {
+ float max = 1e6;
+ tab.decay.durationSeconds.setRange(0, max);
+ tab.decay.durationSeconds.setDecimals(2);
+ tab.decay.durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
+ tab.decay.durationSeconds.setValue(decay.durationSeconds);
+ }
+ {
+ tab.decay.maxConfidence.setRange(0, 1);
+ tab.decay.maxConfidence.setSteps(20);
+ tab.decay.maxConfidence.setValue(decay.maxConfidence);
+ }
+ {
+ tab.decay.minConfidence.setRange(0, 1);
+ tab.decay.minConfidence.setSteps(20);
+ tab.decay.minConfidence.setValue(decay.minConfidence);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Enabled"), {row, 0}).add(tab.decay.enabled, {row, 1});
+ row++;
+ grid.add(Label("Delay [sec]"), {row, 0}).add(tab.decay.delaySeconds, {row, 1});
+ row++;
+ grid.add(Label("Duration [sec]"), {row, 0}).add(tab.decay.durationSeconds, {row, 1});
+ row++;
+ grid.add(Label("Max Confidence"), {row, 0}).add(tab.decay.maxConfidence, {row, 1});
+ row++;
+ grid.add(Label("Min Confidence"), {row, 0}).add(tab.decay.minConfidence, {row, 1});
+ row++;
+
+ decayGroup.setLabel("Decay");
+ decayGroup.addChild(grid);
+ }
+
+ VBoxLayout root = {visuGroup, decayGroup, robotHeadGroup, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
}
@@ -981,7 +1034,7 @@ namespace armarx
{
// Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
{
- std::scoped_lock lock(dataMutex);
+ std::scoped_lock lock(visuMutex);
visu.enabled = tab.visu.enabled.getValue();
visu.inGlobalFrame = tab.visu.inGlobalFrame.getValue();
@@ -992,10 +1045,20 @@ namespace armarx
}
{
std::scoped_lock lock(robotHeadMutex);
+
robotHead.checkHeadVelocity = tab.robotHead.checkHeadVelocity.getValue();
robotHead.maxJointVelocity = tab.robotHead.maxJointVelocity.getValue();
robotHead.discardIntervalAfterMoveMS = tab.robotHead.discardIntervalAfterMoveMS.getValue();
}
+ {
+ std::scoped_lock lock(decayMutex);
+
+ decay.enabled = tab.decay.enabled.getValue();
+ decay.delaySeconds = tab.decay.delaySeconds.getValue();
+ decay.durationSeconds = tab.decay.durationSeconds.getValue();
+ decay.maxConfidence = tab.decay.maxConfidence.getValue();
+ decay.minConfidence = tab.decay.minConfidence.getValue();
+ }
}
void ObjectPoseObserver::Visu::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 1c4f032ff7e52c8ac1e770c77cbbf4ec8103a4f5..cb42484ce3f18ffb709b98fd066a8499f0f7dc06 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -192,11 +192,12 @@ namespace armarx
bool isAttachedCached(objpose::ObjectPose& objectPose) const;
};
- std::mutex dataMutex;
Data data;
+ std::mutex dataMutex;
objpose::ObjectPoseDecay decay;
+ std::mutex decayMutex;
objpose::RobotHeadMovement robotHead;
@@ -220,6 +221,7 @@ namespace armarx
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
};
Visu visu;
+ std::mutex visuMutex;
struct Calibration
{
@@ -236,6 +238,7 @@ namespace armarx
struct Visu
{
RemoteGui::Client::CheckBox enabled;
+
RemoteGui::Client::CheckBox inGlobalFrame;
RemoteGui::Client::FloatSlider alpha;
RemoteGui::Client::CheckBox alphaByConfidence;
@@ -252,6 +255,17 @@ namespace armarx
RemoteGui::Client::IntSpinBox discardIntervalAfterMoveMS;
};
RobotHead robotHead;
+
+ struct Decay
+ {
+ RemoteGui::Client::CheckBox enabled;
+
+ RemoteGui::Client::FloatSpinBox delaySeconds;
+ RemoteGui::Client::FloatSpinBox durationSeconds;
+ RemoteGui::Client::FloatSlider maxConfidence;
+ RemoteGui::Client::FloatSlider minConfidence;
+ };
+ Decay decay;
};
RemoteGuiTab tab;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
index e7406a5e30ad69ee6c421b38ec66b30bc72c53f2..234374a3c4dda96fd559e30f06e8aadc6334fb50 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
@@ -12,7 +12,7 @@ namespace armarx::objpose
{
defs->optional(enabled, prefix + "enabled",
"If true, object poses decay over time when not localized anymore.");
- defs->optional(noDecaySeconds, prefix + "noDecaySeconds",
+ defs->optional(delaySeconds, prefix + "delaySeconds",
"Duration after latest localization before decay starts.");
defs->optional(durationSeconds, prefix + "durationSeconds",
"How long to reach minimal confidence.");
@@ -31,11 +31,11 @@ namespace armarx::objpose
float ObjectPoseDecay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now)
{
float duration = (now - localization).toSeconds();
- if (duration < noDecaySeconds)
+ if (duration < delaySeconds)
{
return maxConfidence;
}
- else if (duration > noDecaySeconds + this->durationSeconds)
+ else if (duration > delaySeconds + this->durationSeconds)
{
return minConfidence;
}
@@ -43,7 +43,7 @@ namespace armarx::objpose
{
return simox::math::scale_value_from_to(
duration,
- noDecaySeconds, noDecaySeconds + this->durationSeconds,
+ delaySeconds, delaySeconds + this->durationSeconds,
maxConfidence, minConfidence);
}
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
index b55824a7cbfbf230d80a55dac74b924ba595106c..a66d9977097a354ca7eae420bce450296db925ab 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
@@ -33,7 +33,7 @@ namespace armarx::objpose
bool enabled = false;
/// Duration after latest localization before decay starts.
- float noDecaySeconds = 5.0;
+ float delaySeconds = 5.0;
/// How long to reach minConfidence.
float durationSeconds = 20.0;