From f6373adb29f7d777c801df9b23a6b4dd531d2baa Mon Sep 17 00:00:00 2001
From: armar-user <armar-user@h2t.kit.edu>
Date: Mon, 8 Nov 2021 14:49:21 +0100
Subject: [PATCH] made the robot unit optional to robotstatememory
---
.../RobotAPI/components/armem/CMakeLists.txt | 2 +-
.../RobotStateMemory/RobotStateMemory.cpp | 72 ++++++++++---------
.../RobotStateMemory/RobotStateMemory.h | 11 ++-
source/RobotAPI/interface/CMakeLists.txt | 3 +
.../interface/units/LaserScannerUnit.ice | 2 +-
.../RobotUnitComponentPlugin.cpp | 51 ++++++++++---
.../RobotUnitComponentPlugin.h | 12 ++--
7 files changed, 94 insertions(+), 59 deletions(-)
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index 526448e13..f6c875f0b 100644
--- a/source/RobotAPI/components/armem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/CMakeLists.txt
@@ -2,7 +2,7 @@
add_subdirectory(server)
# memory server addons
-
+add_subdirectory(addon)
# clients
add_subdirectory(client)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 27aaa07dd..95018ad3c 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -45,11 +45,10 @@ namespace armarx::armem::server::robot_state
{
addPlugin(debugObserver);
ARMARX_CHECK_NOT_NULL(debugObserver);
+
addPlugin(robotUnit.plugin);
ARMARX_CHECK_NOT_NULL(robotUnit.plugin);
-
- robotUnit.reader.setTag(getName());
- robotUnit.writer.setTag(getName());
+ robotUnit.plugin->setRobotUnitAsOptionalDependency(true);
}
@@ -62,7 +61,7 @@ namespace armarx::armem::server::robot_state
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- const std::string robotUnitPrefix = sensorValuePrefix;
+ const std::string robotUnitPrefix = robotUnit.sensorValuePrefix;
defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
"Prefix of all sensor values.");
@@ -71,13 +70,13 @@ namespace armarx::armem::server::robot_state
.setMin(1);
defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
"The frequency to store values in Hz. All other values get discarded. "
- "Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
- .setMin(1).setMax(ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ "Minimum is 1, max is " + std::to_string(robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
+ .setMin(1).setMax(robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY);
const std::string prefix = "mem.";
- workingMemory().name() = "RobotState";
+ setMemoryName("RobotState");
descriptionSegment.defineProperties(defs, prefix + "desc.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
@@ -101,7 +100,7 @@ namespace armarx::armem::server::robot_state
localizationSegment.onInit();
commonVisu.init();
- robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY);
robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
std::vector<std::string> includePaths;
@@ -127,38 +126,43 @@ namespace armarx::armem::server::robot_state
{
ARMARX_CHECK_NOT_NULL(debugObserver->getDebugObserver());
- if (robotUnit.plugin->getRobotUnit())
+ // 1. General setup
+ localizationSegment.onConnect();
+ commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
+
+ // 2. Check for RobotUnit. If RobotUnit is enabled. Otherwise we do not wait for a streaming service and do not setup the segments
+ if (robotUnit.plugin->hasRobotUnitName())
{
robotUnit.plugin->waitUntilRobotUnitIsRunning();
- }
- RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
+ RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
- descriptionSegment.onConnect(robotUnitPrx);
- proprioceptionSegment.onConnect(robotUnitPrx);
- localizationSegment.onConnect();
+ robotUnit.reader.setTag(getName());
+ robotUnit.writer.setTag(getName());
- commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
+ descriptionSegment.onConnect(robotUnitPrx);
+ proprioceptionSegment.onConnect(robotUnitPrx);
- robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
- proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
- robotUnit.writer.connect(*debugObserver);
- robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
+ robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
+ proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
+ robotUnit.writer.connect(*debugObserver);
+ robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
- robotUnit.reader.task = new SimpleRunningTask<>([this]()
- {
- robotUnit.reader.run(
- robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
- );
- }, "Robot Unit Reader");
- robotUnit.writer.task = new SimpleRunningTask<>([this]()
- {
- robotUnit.writer.run(
- robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
- iceAdapter(), localizationSegment
- );
- }, "Robot State Writer");
+ robotUnit.reader.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.reader.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
+ );
+ }, "Robot Unit Reader");
+ robotUnit.writer.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.writer.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
+ iceAdapter(), localizationSegment
+ );
+ }, "Robot State Writer");
- startRobotUnitStream();
+ startRobotUnitStream();
+ }
}
@@ -181,6 +185,8 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::startRobotUnitStream()
{
+ ARMARX_CHECK(robotUnit.plugin->robotUnitIsRunning());
+
if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
{
if (robotUnit.reader.task->isRunning() && robotUnit.writer.task->isRunning())
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 74dd4a3fa..6459b2aac 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -111,6 +111,10 @@ namespace armarx::armem::server::robot_state
struct RobotUnit
{
+ // params
+ static constexpr float ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
+ static constexpr const char* sensorValuePrefix = "RobotUnit.";
+
float pollFrequency = 50;
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
@@ -122,13 +126,6 @@ namespace armarx::armem::server::robot_state
std::mutex dataMutex;
};
RobotUnit robotUnit;
-
-
- // params
- static constexpr float ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
- static constexpr const char* sensorValuePrefix = "RobotUnit.";
-
-
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index a7cc9ca79..a4810afdc 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -126,6 +126,9 @@ set(SLICE_FILES
# Special Servers
armem/server/ObjectMemoryInterface.ice
+ # Special Addons
+ armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
+
armem/mns.ice
armem/mns/MemoryNameSystemInterface.ice
diff --git a/source/RobotAPI/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index 208f51515..631974396 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -48,7 +48,7 @@ module armarx
{
float angle;
float distance;
- float intensity;
+// float intensity;
};
struct LaserScannerInfo
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 6647cf385..2bc74d3ea 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -80,16 +80,29 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::preOnConnectComponent()
{
- parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ if (not _robotUnitName.empty())
+ {
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ }
}
void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
- properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the RobotUnit");
+ if (_isRobotUnitOptionalDependency)
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "",
+ "Name of the RobotUnit");
+ }
+ else
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the RobotUnit");
+ }
}
}
@@ -100,7 +113,6 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
- ARMARX_CHECK_NOT_EMPTY(name);
ARMARX_CHECK_EMPTY(_robotUnitName);
_robotUnitName = name;
}
@@ -110,17 +122,28 @@ namespace armarx::plugins
return _robotUnitName;
}
- void RobotUnitComponentPlugin::deactivate()
+ bool RobotUnitComponentPlugin::hasRobotUnitName() const
{
- _isRobotUnitOptionalDependency = true;
+ return not _robotUnitName.empty();
}
+ void RobotUnitComponentPlugin::setRobotUnitAsOptionalDependency(bool isOptional)
+ {
+ _isRobotUnitOptionalDependency = isOptional;
+ }
- void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond) const
+ void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond)
{
ARMARX_INFO << "Waiting until robot unit is running ...";
- while (not(termCond() or not isNullptr(getRobotUnit()) or getRobotUnit()->isRunning()))
+ if (not hasRobotUnitName())
+ {
+ ARMARX_ERROR << "Could not wait for a robotUnit without a name!";
+ return;
+ }
+
+ parent<Component>().usingProxy(_robotUnitName);
+ while (not(termCond() or not robotUnitIsRunning()))
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
@@ -128,7 +151,15 @@ namespace armarx::plugins
ARMARX_INFO << "Robot unit is up and running.";
}
-
+ bool RobotUnitComponentPlugin::robotUnitIsRunning() const
+ {
+ if (not hasRobotUnitName())
+ {
+ // An empty robotUnit can never run
+ return false;
+ }
+ return not(isNullptr(getRobotUnit()) or not getRobotUnit()->isRunning());
+ }
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 4371934b9..6edd2d836 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -26,13 +26,9 @@ namespace armarx
void setRobotUnitName(const std::string& name);
const std::string& getRobotUnitName() const;
+ bool hasRobotUnitName() const;
- void deactivate();
-
- void setRobotUnitAsOptionalDependency(bool isOptional = true)
- {
-
- }
+ void setRobotUnitAsOptionalDependency(bool isOptional = true);
/**
* @brief Waits until the robot unit is running.
@@ -44,7 +40,9 @@ namespace armarx
* If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
* for the robot unit to become available.
*/
- void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;});
+
+ bool robotUnitIsRunning() const;
--
GitLab