Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • sw/armarx/robot-api
  • uwkce_singer/robot-api
  • untcg_hofmann/robot-api
  • ulqba_korosakov/RobotAPI
4 results
Show changes
Showing
with 824 additions and 193 deletions
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::ArmarXObjects::MemoryToDebugObserver
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
* @date 2023
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
#include <SimoxUtility/json/json.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem::client::util
{
/**
* @brief ID of an ARON value in the memory.
*/
struct MemoryValueID
{
armem::MemoryID entityID;
aron::Path aronPath;
};
/**
* @brief Transfers data from memory servers to the DebugObserver.
*
* Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
* allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
*/
class MemoryToDebugObserver
{
public:
/**
* @brief Configuration.
*
* Can be converted to and from JSON.
*/
struct Properties
{
std::vector<MemoryValueID> plottedValues;
};
/**
* @brief Required services.
*/
struct Services
{
MemoryNameSystem& memoryNameSystem;
armarx::DebugObserverHelper& debugObserver;
};
/**
* @brief Constructor.
*/
MemoryToDebugObserver(const Properties& properties, const Services& services);
/**
* @brief Query values from the memory and send them to the debug observer.
*/
void pollOnce();
private:
static std::string makeChannelName(const armem::MemoryID& memoryID);
static std::string makeDatafieldName(const aron::Path& path);
armem::client::Reader* getReader(const armem::MemoryID& memoryID);
Properties properties;
Services services;
std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
};
void to_json(simox::json::json& j, const MemoryValueID& id);
void from_json(const simox::json::json& j, MemoryValueID& id);
void to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p);
void from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p);
} // namespace armarx::armem::client::util
......@@ -16,25 +16,28 @@ namespace armarx::armem::client::util
const std::string prefix = propertyPrefix();
props = defaultProperties();
if (not props.has_value())
{
props = defaultProperties();
}
def->optional(props.memoryName, prefix + "Memory");
def->optional(props.coreSegmentName, prefix + "CoreSegment");
def->optional(props->memoryName, prefix + "Memory");
def->optional(props->coreSegmentName, prefix + "CoreSegment");
// TODO(fabian.reister): this might also be part of the subclass
// if the provider name has to be derived from e.g. the component name
def->optional(props.providerName, prefix + "Provider", "Name of this provider");
def->optional(props->providerName, prefix + "Provider", "Name of this provider");
}
void
SimpleWriterBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << props.memoryName << "' ...";
ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << properties().memoryName << "' ...";
try
{
memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(props.memoryName));
ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName
memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(properties().memoryName));
ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << properties().memoryName
<< "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
......@@ -44,12 +47,6 @@ namespace armarx::armem::client::util
}
}
std::mutex&
SimpleWriterBase::memoryWriterMutex()
{
return memoryMutex;
}
armem::client::Writer&
SimpleWriterBase::memoryWriter()
{
......@@ -59,7 +56,17 @@ namespace armarx::armem::client::util
const SimpleWriterBase::Properties&
SimpleWriterBase::properties() const
{
return props;
if (not props.has_value())
{
const_cast<std::optional<Properties>&>(props) = defaultProperties();
}
return props.value();
}
void
SimpleWriterBase::setProperties(const Properties& p)
{
props = p;
}
} // namespace armarx::armem::client::util
......@@ -54,19 +54,18 @@ namespace armarx::armem::client::util
};
const Properties& properties() const;
void setProperties(const Properties& p);
virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
std::mutex& memoryWriterMutex();
armem::client::Writer& memoryWriter();
private:
Properties props;
std::optional<Properties> props;
armem::client::Writer memoryWriterClient;
std::mutex memoryMutex;
};
} // namespace armarx::armem::client::util
#include "MemoryID.h"
#include "error/ArMemError.h"
#include <SimoxUtility/algorithm/advanced.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <forward_list>
#include <boost/algorithm/string.hpp>
#include <forward_list>
#include <SimoxUtility/algorithm/advanced.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include "error/ArMemError.h"
namespace armarx::armem
{
......@@ -16,7 +15,6 @@ namespace armarx::armem
// This is constant, not a variable.
const std::string MemoryID::delimiter = "/";
MemoryID::MemoryID()
{
}
......@@ -80,14 +78,12 @@ namespace armarx::armem
this->instanceIndex = instanceIndexFromStr(*it);
}
MemoryID::MemoryID(
const std::string& memoryName,
const std::string& coreSegmentName,
const std::string& providerSegmentName,
const std::string& entityName,
Time timestamp,
int instanceIndex) :
MemoryID::MemoryID(const std::string& memoryName,
const std::string& coreSegmentName,
const std::string& providerSegmentName,
const std::string& entityName,
Time timestamp,
int instanceIndex) :
memoryName(memoryName),
coreSegmentName(coreSegmentName),
providerSegmentName(providerSegmentName),
......@@ -97,13 +93,14 @@ namespace armarx::armem
{
}
std::string MemoryID::str(bool escapeDelimiters) const
std::string
MemoryID::str(bool escapeDelimiters) const
{
return str(delimiter, escapeDelimiters);
}
std::string MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
std::string
MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
{
std::vector<std::string> items = getAllItems(escapeDelimiter);
while (items.size() > 0 && items.back().empty())
......@@ -113,7 +110,8 @@ namespace armarx::armem
return simox::alg::join(items, delimiter, false, false);
}
std::string MemoryID::getLeafItem() const
std::string
MemoryID::getLeafItem() const
{
std::vector<std::string> items = getAllItems();
for (auto it = items.rbegin(); it != items.rend(); ++it)
......@@ -126,7 +124,8 @@ namespace armarx::armem
return "";
}
bool MemoryID::hasGap() const
bool
MemoryID::hasGap() const
{
bool emptyFound = false;
for (const std::string& item : getAllItems())
......@@ -144,18 +143,59 @@ namespace armarx::armem
return false;
}
bool MemoryID::isWellDefined() const
bool
MemoryID::isWellDefined() const
{
return !hasGap();
}
MemoryID MemoryID::fromString(const std::string& string)
MemoryID
MemoryID::fromString(const std::string& string)
{
return MemoryID(string);
}
std::vector<std::string> MemoryID::getItems(bool escapeDelimiters) const
MemoryID
MemoryID::fromItems(const std::vector<std::string>& items)
{
MemoryID id;
std::size_t i = 0;
if (i < items.size())
{
id.memoryName = items[i];
++i;
}
if (i < items.size())
{
id.coreSegmentName = items[i];
++i;
}
if (i < items.size())
{
id.providerSegmentName = items[i];
++i;
}
if (i < items.size())
{
id.entityName = items[i];
++i;
}
if (i < items.size())
{
id.timestamp = timestampFromStr(items[i]);
++i;
}
if (i < items.size())
{
id.instanceIndex = instanceIndexFromStr(items[i]);
++i;
}
return id;
}
std::vector<std::string>
MemoryID::getItems(bool escapeDelimiters) const
{
std::vector<std::string> items;
......@@ -194,59 +234,69 @@ namespace armarx::armem
return items;
}
std::vector<std::string> MemoryID::getAllItems(bool escapeDelimiters) const
std::vector<std::string>
MemoryID::getAllItems(bool escapeDelimiters) const
{
return
{
escape(memoryName, escapeDelimiters), escape(coreSegmentName, escapeDelimiters),
escape(providerSegmentName, escapeDelimiters), escape(entityName, escapeDelimiters),
timestampStr(), instanceIndexStr()
return {
escape(memoryName, escapeDelimiters),
escape(coreSegmentName, escapeDelimiters),
escape(providerSegmentName, escapeDelimiters),
escape(entityName, escapeDelimiters),
timestampStr(),
instanceIndexStr(),
};
}
MemoryID MemoryID::getMemoryID() const
MemoryID
MemoryID::getMemoryID() const
{
MemoryID id;
id.memoryName = memoryName;
return id;
}
MemoryID MemoryID::getCoreSegmentID() const
MemoryID
MemoryID::getCoreSegmentID() const
{
MemoryID id = getMemoryID();
id.coreSegmentName = coreSegmentName;
return id;
}
MemoryID MemoryID::getProviderSegmentID() const
MemoryID
MemoryID::getProviderSegmentID() const
{
MemoryID id = getCoreSegmentID();
id.providerSegmentName = providerSegmentName;
return id;
}
MemoryID MemoryID::getEntityID() const
MemoryID
MemoryID::getEntityID() const
{
MemoryID id = getProviderSegmentID();
id.entityName = entityName;
return id;
}
MemoryID MemoryID::getEntitySnapshotID() const
MemoryID
MemoryID::getEntitySnapshotID() const
{
MemoryID id = getEntityID();
id.timestamp = timestamp;
return id;
}
MemoryID MemoryID::getEntityInstanceID() const
MemoryID
MemoryID::getEntityInstanceID() const
{
MemoryID id = getEntitySnapshotID();
id.instanceIndex = instanceIndex;
return id;
}
MemoryID MemoryID::removeLeafItem() const
MemoryID
MemoryID::removeLeafItem() const
{
if (instanceIndex != -1)
{
......@@ -268,118 +318,133 @@ namespace armarx::armem
{
return getMemoryID();
}
return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
}
void MemoryID::setMemoryID(const MemoryID& id)
void
MemoryID::setMemoryID(const MemoryID& id)
{
memoryName = id.memoryName;
}
void MemoryID::setCoreSegmentID(const MemoryID& id)
void
MemoryID::setCoreSegmentID(const MemoryID& id)
{
setMemoryID(id);
coreSegmentName = id.coreSegmentName;
}
void MemoryID::setProviderSegmentID(const MemoryID& id)
void
MemoryID::setProviderSegmentID(const MemoryID& id)
{
setCoreSegmentID(id);
providerSegmentName = id.providerSegmentName;
}
void MemoryID::setEntityID(const MemoryID& id)
void
MemoryID::setEntityID(const MemoryID& id)
{
setProviderSegmentID(id);
entityName = id.entityName;
}
void MemoryID::setEntitySnapshotID(const MemoryID& id)
void
MemoryID::setEntitySnapshotID(const MemoryID& id)
{
setEntityID(id);
timestamp = id.timestamp;
}
void MemoryID::setEntityInstanceID(const MemoryID& id)
void
MemoryID::setEntityInstanceID(const MemoryID& id)
{
setEntitySnapshotID(id);
instanceIndex = id.instanceIndex;
}
MemoryID MemoryID::withMemoryName(const std::string& name) const
MemoryID
MemoryID::withMemoryName(const std::string& name) const
{
MemoryID id = *this;
id.memoryName = name;
return id;
}
MemoryID MemoryID::withCoreSegmentName(const std::string& name) const
MemoryID
MemoryID::withCoreSegmentName(const std::string& name) const
{
MemoryID id = *this;
id.coreSegmentName = name;
return id;
}
MemoryID MemoryID::withProviderSegmentName(const std::string& name) const
MemoryID
MemoryID::withProviderSegmentName(const std::string& name) const
{
MemoryID id = *this;
id.providerSegmentName = name;
return id;
}
MemoryID MemoryID::withEntityName(const std::string& name) const
MemoryID
MemoryID::withEntityName(const std::string& name) const
{
MemoryID id = *this;
id.entityName = name;
return id;
}
MemoryID MemoryID::withTimestamp(Time time) const
MemoryID
MemoryID::withTimestamp(Time time) const
{
MemoryID id = *this;
id.timestamp = time;
return id;
}
MemoryID MemoryID::withInstanceIndex(int index) const
MemoryID
MemoryID::withInstanceIndex(int index) const
{
MemoryID id = *this;
id.instanceIndex = index;
return id;
}
std::string MemoryID::timestampStr() const
std::string
MemoryID::timestampStr() const
{
return hasTimestamp() ? std::to_string(timestamp.toMicroSecondsSinceEpoch()) : "";
}
std::string MemoryID::instanceIndexStr() const
std::string
MemoryID::instanceIndexStr() const
{
return hasInstanceIndex() ? std::to_string(instanceIndex) : "";
}
Time MemoryID::timestampFromStr(const std::string& string)
Time
MemoryID::timestampFromStr(const std::string& string)
{
return Time(Duration::MicroSeconds(parseInteger(string, "timestamp")));
}
int MemoryID::instanceIndexFromStr(const std::string& string)
int
MemoryID::instanceIndexFromStr(const std::string& string)
{
return int(parseInteger(string, "instance index"));
}
bool MemoryID::operator==(const MemoryID& other) const
bool
MemoryID::operator==(const MemoryID& other) const
{
return memoryName == other.memoryName
and coreSegmentName == other.coreSegmentName
and providerSegmentName == other.providerSegmentName
and entityName == other.entityName
and timestamp == other.timestamp
and instanceIndex == other.instanceIndex;
return memoryName == other.memoryName and coreSegmentName == other.coreSegmentName and
providerSegmentName == other.providerSegmentName and
entityName == other.entityName and timestamp == other.timestamp and
instanceIndex == other.instanceIndex;
}
bool MemoryID::operator<(const MemoryID& rhs) const
bool
MemoryID::operator<(const MemoryID& rhs) const
{
int c = memoryName.compare(rhs.memoryName);
if (c != 0)
......@@ -413,7 +478,8 @@ namespace armarx::armem
return instanceIndex < rhs.instanceIndex;
}
long long MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
long long
MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
{
if (string.empty())
{
......@@ -433,12 +499,14 @@ namespace armarx::armem
}
}
std::string MemoryID::escapeDelimiter(const std::string& name)
std::string
MemoryID::escapeDelimiter(const std::string& name)
{
return simox::alg::replace_all(name, delimiter, "\\" + delimiter);
}
std::string MemoryID::escape(const std::string& name, bool escapeDelimiters)
std::string
MemoryID::escape(const std::string& name, bool escapeDelimiters)
{
if (escapeDelimiters)
{
......@@ -450,24 +518,27 @@ namespace armarx::armem
}
}
std::ostream& operator<<(std::ostream& os, const MemoryID id)
std::ostream&
operator<<(std::ostream& os, const MemoryID id)
{
return os << "'" << id.str() << "'";
}
bool contains(const MemoryID& general, const MemoryID& specific)
bool
contains(const MemoryID& general, const MemoryID& specific)
{
if (!general.isWellDefined())
{
std::stringstream ss;
ss << "\nGeneral ID " << general << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
ss << "\nGeneral ID " << general << " is not well-defined, which is required for `"
<< __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(general, ss.str());
}
if (!specific.isWellDefined())
{
std::stringstream ss;
ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `"
<< __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(specific, ss.str());
}
......@@ -528,4 +599,4 @@ namespace armarx::armem
return true;
}
}
} // namespace armarx::armem
#pragma once
#include <functional> // for std::hash
#include <functional> // for std::hash
#include <string>
#include <vector>
#include "Time.h"
namespace armarx::armem
{
......@@ -48,7 +47,6 @@ namespace armarx::armem
class MemoryID
{
public:
std::string memoryName = "";
std::string coreSegmentName = "";
std::string providerSegmentName = "";
......@@ -58,7 +56,6 @@ namespace armarx::armem
public:
/// Construct a default (empty) memory ID.
MemoryID();
/// (Re-)Construct a memory ID from a string representation as returned by `str()`.
......@@ -72,6 +69,13 @@ namespace armarx::armem
int instanceIndex = -1);
/// Alias for constructor from string.
static MemoryID fromString(const std::string& string);
/// Constructor memory ID from items as returned by getItems().
static MemoryID fromItems(const std::vector<std::string>& items);
/**
* @brief Indicate whether this ID is well-defined.
*
......@@ -93,43 +97,56 @@ namespace armarx::armem
*/
bool isWellDefined() const;
// Checks whether a specific level is specified.
bool hasMemoryName() const
bool
hasMemoryName() const
{
return not memoryName.empty();
}
bool hasCoreSegmentName() const
bool
hasCoreSegmentName() const
{
return not coreSegmentName.empty();
}
bool hasProviderSegmentName() const
bool
hasProviderSegmentName() const
{
return not providerSegmentName.empty();
}
bool hasEntityName() const
bool
hasEntityName() const
{
return not entityName.empty();
}
bool hasTimestamp() const
bool
hasTimestamp() const
{
return timestamp.isValid();
}
void clearTimestamp()
void
clearTimestamp()
{
timestamp = Time::Invalid();
}
bool hasInstanceIndex() const
bool
hasInstanceIndex() const
{
return instanceIndex >= 0;
}
void clearInstanceIndex()
void
clearInstanceIndex()
{
instanceIndex = -1;
}
// Slice getters: Get upper part of the ID.
MemoryID getMemoryID() const;
......@@ -182,8 +199,6 @@ namespace armarx::armem
/// Get the instance index as string.
std::string instanceIndexStr() const;
/// Alias for constructor from string.
static MemoryID fromString(const std::string& string);
/// Reconstruct a timestamp from a string as returned by `timestampStr()`.
static Time timestampFromStr(const std::string& timestamp);
/// Reconstruct an instance index from a string as returned by `instanceIndexStr()`.
......@@ -206,31 +221,38 @@ namespace armarx::armem
// Operators
bool operator ==(const MemoryID& other) const;
inline bool operator !=(const MemoryID& other) const
bool operator==(const MemoryID& other) const;
inline bool
operator!=(const MemoryID& other) const
{
return not (*this == other);
return not(*this == other);
}
bool operator< (const MemoryID& rhs) const;
inline bool operator> (const MemoryID& rhs) const
bool operator<(const MemoryID& rhs) const;
inline bool
operator>(const MemoryID& rhs) const
{
return rhs < (*this);
}
inline bool operator<=(const MemoryID& rhs) const
inline bool
operator<=(const MemoryID& rhs) const
{
return not operator> (rhs);
return not operator>(rhs);
}
inline bool operator>=(const MemoryID& rhs) const
inline bool
operator>=(const MemoryID& rhs) const
{
return not operator< (rhs);
return not operator<(rhs);
}
friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
private:
static long long parseInteger(const std::string& string, const std::string& semanticName);
static std::string escapeDelimiter(const std::string& name);
static std::string escape(const std::string& name, bool escapeDelimiters);
......@@ -240,10 +262,8 @@ namespace armarx::armem
// Do not allow specifying the delimiter from outside.
std::string str(const std::string& delimiter, bool escapeDelimiter) const;
};
/**
* @brief Indicates whether `general` is "less specific" than, or equal to, `specific`,
* i.e. `general` "contains" `specific`.
......@@ -266,8 +286,7 @@ namespace armarx::armem
*/
bool contains(const MemoryID& general, const MemoryID& specific);
} // namespace armarx::armem
} // namespace armarx::armem
namespace std
{
......@@ -275,12 +294,12 @@ namespace std
template <>
struct hash<armarx::armem::MemoryID>
{
std::size_t operator()(const armarx::armem::MemoryID& id) const
std::size_t
operator()(const armarx::armem::MemoryID& id) const
{
const std::string sid = id.str();
return std::hash<string>()(sid);
}
};
}
} // namespace std
......@@ -157,6 +157,50 @@ namespace armarx::armem::server
return this->_commit(commit, false);
}
data::CommitResult
MemoryToIceAdapter::commitLocking(const data::Commit& commitIce, Time timeArrived)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
auto handleException = [](const std::string& what)
{
data::CommitResult result;
data::EntityUpdateResult& r = result.results.emplace_back();
r.success = false;
r.errorMessage = what;
return result;
};
armem::Commit commit;
try
{
::armarx::armem::fromIce(commitIce, commit, timeArrived);
}
catch (const aron::error::AronNotValidException& e)
{
throw;
return handleException(e.what());
}
catch (const Ice::Exception& e)
{
throw;
return handleException(e.what());
}
armem::CommitResult result = this->commitLocking(commit);
data::CommitResult resultIce;
toIce(resultIce, result);
return resultIce;
}
data::CommitResult
MemoryToIceAdapter::commitLocking(const data::Commit& commitIce)
{
ARMARX_TRACE;
return commitLocking(commitIce, armem::Time::Now());
}
armem::CommitResult
MemoryToIceAdapter::commitLocking(const armem::Commit& commit)
{
......
......@@ -37,6 +37,8 @@ namespace armarx::armem::server
data::CommitResult commit(const data::Commit& commitIce, Time timeArrived);
data::CommitResult commit(const data::Commit& commitIce);
armem::CommitResult commit(const armem::Commit& commit);
data::CommitResult commitLocking(const data::Commit& commitIce, Time timeArrived);
data::CommitResult commitLocking(const data::Commit& commitIce);
armem::CommitResult commitLocking(const armem::Commit& commit);
......
......@@ -48,7 +48,7 @@ namespace armarx::armem::server::plugins
ReadWritePluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
ARMARX_TRACE;
return iceAdapter().commit(commitIce);
return iceAdapter().commitLocking(commitIce);
}
// READING
......
#pragma once
#include "BaseQueryProcessorBase.h"
#include <regex>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <regex>
#include "BaseQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::base
{
......@@ -16,11 +15,10 @@ namespace armarx::armem::server::query_proc::base
public BaseQueryProcessorBase<_MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>
{
protected:
using Base = BaseQueryProcessorBase<_MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>;
using Base =
BaseQueryProcessorBase<_MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>;
public:
using MemoryT = _MemoryT;
using CoreSegmentT = typename MemoryT::CoreSegmentT;
......@@ -31,25 +29,26 @@ namespace armarx::armem::server::query_proc::base
public:
MemoryQueryProcessorBase()
{
}
MemoryQueryProcessorBase(ChildProcessorT&& childProcessor) :
childProcessor(childProcessor)
MemoryQueryProcessorBase(ChildProcessorT&& childProcessor) : childProcessor(childProcessor)
{
}
using Base::process;
ResultMemoryT process(const armem::query::data::Input& input, const MemoryT& memory) const
ResultMemoryT
process(const armem::query::data::Input& input, const MemoryT& memory) const
{
return this->process(input.memoryQueries, memory);
}
virtual void process(ResultMemoryT& result,
const armem::query::data::MemoryQuery& query,
const MemoryT& memory) const override
virtual void
process(ResultMemoryT& result,
const armem::query::data::MemoryQuery& query,
const MemoryT& memory) const override
{
if (auto q = dynamic_cast<const armem::query::data::memory::All*>(&query))
{
......@@ -69,19 +68,19 @@ namespace armarx::armem::server::query_proc::base
}
}
virtual void process(ResultMemoryT& result,
const armem::query::data::memory::All& query,
const MemoryT& memory) const
virtual void
process(ResultMemoryT& result,
const armem::query::data::memory::All& query,
const MemoryT& memory) const
{
memory.forEachCoreSegment([this, &result, &query](const CoreSegmentT& coreSegment)
{
this->_processResult(result, coreSegment, query);
});
{ this->_processResult(result, coreSegment, query); });
}
virtual void process(ResultMemoryT& result,
const armem::query::data::memory::Single& query,
const MemoryT& memory) const
virtual void
process(ResultMemoryT& result,
const armem::query::data::memory::Single& query,
const MemoryT& memory) const
{
if (auto coreSegment = memory.findCoreSegment(query.coreSegmentName))
{
......@@ -89,32 +88,35 @@ namespace armarx::armem::server::query_proc::base
}
}
virtual void process(ResultMemoryT& result,
const armem::query::data::memory::Regex& query,
const MemoryT& memory) const
virtual void
process(ResultMemoryT& result,
const armem::query::data::memory::Regex& query,
const MemoryT& memory) const
{
const std::regex regex(query.coreSegmentNameRegex);
memory.forEachCoreSegment([this, &result, &query, &regex](const CoreSegmentT& coreSegment)
{
if (std::regex_search(coreSegment.name(), regex))
memory.forEachCoreSegment(
[this, &result, &query, &regex](const CoreSegmentT& coreSegment)
{
this->_processResult(result, coreSegment, query);
}
});
if (std::regex_search(coreSegment.name(), regex))
{
this->_processResult(result, coreSegment, query);
}
});
}
protected:
virtual bool _processAllowed(const armem::query::data::MemoryQuery& query) const
virtual bool
_processAllowed(const armem::query::data::MemoryQuery& query) const
{
// always execute query. Override if you want to execute the quey only if a special condition is fulfilled (e.g. querytargets)
return true;
}
void _processResult(ResultMemoryT& result,
const CoreSegmentT& coreSegment,
const armem::query::data::MemoryQuery& query) const
void
_processResult(ResultMemoryT& result,
const CoreSegmentT& coreSegment,
const armem::query::data::MemoryQuery& query) const
{
ResultCoreSegmentT* child = result.findCoreSegment(coreSegment.name());
if (child == nullptr)
......@@ -126,8 +128,6 @@ namespace armarx::armem::server::query_proc::base
protected:
ChildProcessorT childProcessor;
};
}
} // namespace armarx::armem::server::query_proc::base
......@@ -207,7 +207,6 @@ namespace armarx::armem::server::query_proc::wm_server
MemoryQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
};
}
......@@ -97,6 +97,12 @@ namespace armarx::aron
type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type);
data::IntPtr enumData = data::Int::DynamicCast(data);
if (enumType == nullptr or enumData == nullptr)
{
ARMARX_WARNING << "Enum Type or Data is NULL";
return;
}
std::string name = enumType->getValueName(enumData->getValue());
value << name;
}
......
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
<Include include="RobotAPI/libraries/aron/common/aron/Names.xml" />
</AronIncludes>
<GenerateTypes>
<!--
ToDo: Model regions. Ideas:
- Polygon (convex, non-convex)
-
-->
<Object name='armarx::navigation::location::arondto::Location'>
<ObjectChild key='framedPose'>
<FramedPose />
</ObjectChild>
<ObjectChild key='names'>
<armarx::arondto::Names optional="true" />
</ObjectChild>
</Object>
</GenerateTypes>
......
......@@ -36,6 +36,8 @@ armarx_add_library(
server/attachments/Segment.h
client/class/ClassReader.h
client/class/ClassWriter.h
client/articulated_object/Reader.h
client/articulated_object/Writer.h
client/articulated_object/ArticulatedObjectReader.h
......@@ -55,6 +57,8 @@ armarx_add_library(
types.cpp
utils.cpp
client/class/ClassReader.cpp
client/class/ClassWriter.cpp
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
client/articulated_object/ArticulatedObjectReader.cpp
......
......@@ -8,11 +8,35 @@ Core segment type of Object/Class.
<PackagePath package="RobotAPI" path="libraries/aron/common/aron/OrientedBox.xml" />
<PackagePath package="RobotAPI" path="libraries/aron/common/aron/PackagePath.xml" />
<PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectID.xml" />
<PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectNames.xml" />
<PackagePath package="RobotAPI" path="libraries/aron/common/aron/Names.xml" />
<PackagePath package="RobotAPI" path="libraries/armem/aron/MemoryID.xml" />
</AronIncludes>
<GenerateTypes>
<Object name="armarx::armem::arondto::Feature">
<ObjectChild key="angle">
<float32 />
</ObjectChild>
<ObjectChild key="scale">
<float32 />
</ObjectChild>
<ObjectChild key="point2d">
<vector2f />
</ObjectChild>
<ObjectChild key="point3d">
<vector3f />
</ObjectChild>
<ObjectChild key="feature">
<List>
<float32 />
</List>
</ObjectChild>
</Object>
<Object name="armarx::armem::arondto::ObjectClass">
<ObjectChild key="id">
......@@ -59,7 +83,13 @@ Core segment type of Object/Class.
</ObjectChild>
<ObjectChild key="names">
<armarx::arondto::ObjectNames />
<armarx::arondto::Names />
</ObjectChild>
<ObjectChild key="ivtFeatures">
<List>
<armarx::armem::arondto::Feature />
</List>
</ObjectChild>
</Object>
......
......@@ -4,50 +4,53 @@
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem
namespace armarx
{
void
fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
armem::fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
{
bo = dto.pose;
}
void
toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
armem::toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
{
dto.pose = bo;
}
void
fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
armem::fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
{
objpose::fromAron(dto.pose, bo);
}
void
toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
{
objpose::toAron(dto.pose, bo);
}
/* Attachments */
void
fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
armem::fromAron(const arondto::attachment::AgentDescription& dto,
attachment::AgentDescription& bo)
{
fromAron(dto.id, bo.id);
aron::fromAron(dto.frame, bo.frame);
}
void
toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
armem::toAron(arondto::attachment::AgentDescription& dto,
const attachment::AgentDescription& bo)
{
toAron(dto.id, bo.id);
aron::toAron(dto.frame, bo.frame);
}
void
fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
armem::fromAron(const arondto::attachment::ObjectAttachment& dto,
attachment::ObjectAttachment& bo)
{
fromAron(dto.agent, bo.agent);
aron::fromAron(dto.transformation, bo.transformation);
......@@ -57,7 +60,8 @@ namespace armarx::armem
}
void
toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
armem::toAron(arondto::attachment::ObjectAttachment& dto,
const attachment::ObjectAttachment& bo)
{
toAron(dto.agent, bo.agent);
aron::toAron(dto.transformation, bo.transformation);
......@@ -67,8 +71,8 @@ namespace armarx::armem
}
void
fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
attachment::ArticulatedObjectAttachment& bo)
armem::fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
attachment::ArticulatedObjectAttachment& bo)
{
fromAron(dto.agent, bo.agent);
aron::fromAron(dto.transformation, bo.transformation);
......@@ -78,8 +82,8 @@ namespace armarx::armem
}
void
toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
const attachment::ArticulatedObjectAttachment& bo)
armem::toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
const attachment::ArticulatedObjectAttachment& bo)
{
toAron(dto.agent, bo.agent);
aron::toAron(dto.transformation, bo.transformation);
......@@ -89,7 +93,7 @@ namespace armarx::armem
}
void
toAron(arondto::Marker& dto, const marker::Marker& bo)
armem::toAron(arondto::Marker& dto, const marker::Marker& bo)
{
dto.name = bo.name;
armarx::toAron(dto.robotGlobal, bo.robotGlobal);
......@@ -100,7 +104,7 @@ namespace armarx::armem
}
void
fromAron(const arondto::Marker& dto, marker::Marker& bo)
armem::fromAron(const arondto::Marker& dto, marker::Marker& bo)
{
bo.name = dto.name;
armarx::fromAron(dto.robotGlobal, bo.robotGlobal);
......@@ -109,6 +113,73 @@ namespace armarx::armem
armarx::fromAron(dto.markerPose, bo.markerPose);
}
} // namespace armarx
namespace armarx::armem
{
void
clazz::fromAron(const arondto::Feature& dto, Feature& bo)
{
bo.angle = dto.angle;
bo.scale = dto.scale;
bo.feature = dto.feature;
bo.point2d = dto.point2d;
bo.point3d = dto.point3d;
}
void
clazz::toAron(arondto::Feature& dto, const Feature& bo)
{
dto.angle = bo.angle;
dto.scale = bo.scale;
dto.feature = bo.feature;
dto.point2d = bo.point2d;
dto.point3d = bo.point3d;
}
void
clazz::fromAron(const arondto::ObjectClass& dto, ObjectClass& bo)
{
armarx::fromAron(dto.id, bo.id);
armarx::fromAron(dto.simoxXmlPath, bo.simoxXmlPath);
armarx::fromAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
armarx::fromAron(dto.urdfPath, bo.urdfPath);
armarx::fromAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
armarx::fromAron(dto.sdfPath, bo.sdfPath);
armarx::fromAron(dto.meshWrlPath, bo.meshWrlPath);
armarx::fromAron(dto.meshObjPath, bo.meshObjPath);
armarx::fromAron(dto.aabb, bo.aabb);
armarx::fromAron(dto.oobb, bo.oobb);
fromAron(dto.names, bo.names);
bo.ivtFeatures.clear();
for (const auto& i : dto.ivtFeatures)
{
fromAron(i, bo.ivtFeatures.emplace_back());
}
}
void
clazz::toAron(arondto::ObjectClass& dto, const ObjectClass& bo)
{
armarx::toAron(dto.id, bo.id);
armarx::toAron(dto.simoxXmlPath, bo.simoxXmlPath);
armarx::toAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
armarx::toAron(dto.urdfPath, bo.urdfPath);
armarx::toAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
armarx::toAron(dto.sdfPath, bo.sdfPath);
armarx::toAron(dto.meshWrlPath, bo.meshWrlPath);
armarx::toAron(dto.meshObjPath, bo.meshObjPath);
armarx::toAron(dto.aabb, bo.aabb);
armarx::toAron(dto.oobb, bo.oobb);
toAron(dto.names, bo.names);
dto.ivtFeatures.clear();
for (const auto& i : bo.ivtFeatures)
{
toAron(dto.ivtFeatures.emplace_back(), i);
}
}
} // namespace armarx::armem
armarx::armem::MemoryID
......
#pragma once
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem
......@@ -18,24 +18,37 @@ namespace armarx::armem
/* Attachments */
void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
void fromAron(const arondto::attachment::AgentDescription& dto,
attachment::AgentDescription& bo);
void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
void fromAron(const arondto::attachment::ObjectAttachment& dto,
attachment::ObjectAttachment& bo);
void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
attachment::ArticulatedObjectAttachment& bo);
void toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
const attachment::ArticulatedObjectAttachment& bo);
void fromAron(const arondto::Marker& dto, marker::Marker&bo);
void toAron(arondto::Marker& dto, const marker::Marker&bo);
} // namespace armarx::armem
void fromAron(const arondto::Marker& dto, marker::Marker& bo);
void toAron(arondto::Marker& dto, const marker::Marker& bo);
} // namespace armarx::armem
#include <RobotAPI/libraries/armem/core/MemoryID.h>
namespace armarx::armem::clazz
{
void fromAron(const armarx::armem::arondto::Feature& dto, Feature& bo);
void toAron(armarx::armem::arondto::Feature& dto, const Feature& bo);
void fromAron(const armarx::armem::arondto::ObjectClass& dto, ObjectClass& bo);
void toAron(armarx::armem::arondto::ObjectClass& dto, const ObjectClass& bo);
} // namespace armarx::armem::clazz
namespace armarx::armem::obj
{
/// Make a Memory ID for the object instance snapshot representing this pose.
MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
}
} // namespace armarx::armem::obj
#include "ClassReader.h"
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::obj::clazz
{
std::optional<armem::clazz::ObjectClass>
ClassReader::getObjectClass(const std::string& providerName, const ObjectID& id)
{
auto mid = armarx::armem::MemoryID();
mid.memoryName = properties().memoryName;
mid.coreSegmentName = properties().coreSegmentName;
mid.providerSegmentName = providerName;
mid.entityName = id.str();
auto i = this->memoryReader().getLatestSnapshotIn(mid);
if (i.has_value() && i->size() == 1)
{
auto& instance = i->getInstance(0);
auto aron = instance.dataAs<armarx::armem::arondto::ObjectClass>();
armem::clazz::ObjectClass objClass;
armarx::armem::clazz::fromAron(aron, objClass);
return objClass;
}
return std::nullopt;
}
std::map<ObjectID, armem::clazz::ObjectClass>
ClassReader::getObjectClasses(const std::vector<ObjectID>& objectIDs)
{
armem::client::query::Builder builder;
auto& entities = builder.coreSegments()
.withName(properties().coreSegmentName)
.providerSegments()
.all()
.entities();
for (const ObjectID& objectID : objectIDs)
{
entities.withName(objectID.getClassID().str()).snapshots().latest();
}
const armem::client::QueryResult result = memoryReader().query(builder);
if (not result.success)
{
throw armem::error::QueryFailed(result.errorMessage);
}
std::map<ObjectID, armem::clazz::ObjectClass> objectClasses;
result.memory.forEachInstance(
[&objectClasses](const armem::wm::EntityInstance& instance) -> bool
{
const ObjectID classID = ObjectID::FromString(instance.id().entityName);
auto aron = instance.dataAs<armarx::armem::arondto::ObjectClass>();
armarx::armem::clazz::fromAron(aron, objectClasses[classID]);
return true;
});
return objectClasses;
}
std::string
ClassReader::propertyPrefix() const
{
return "classReader.";
}
client::util::SimpleReaderBase::Properties
ClassReader::defaultProperties() const
{
client::util::SimpleReaderBase::Properties p;
p.memoryName = "Object";
p.coreSegmentName = "Class";
return p;
}
} // namespace armarx::armem::obj::clazz
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
#include <map>
#include <optional>
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::obj::clazz
{
class ClassReader : public armem::client::util::SimpleReaderBase
{
public:
ClassReader() = default;
std::optional<armem::clazz::ObjectClass> getObjectClass(const std::string& providerName,
const armarx::ObjectID& id);
/**
* @brief Get object class information for object class IDs.
* @param objectIDs The object class IDs.
* @return The corresponding object classes.
* @throw armarx::armem::error::QueryFailed If the memory query failed.
*/
std::map<armarx::ObjectID, armem::clazz::ObjectClass>
getObjectClasses(const std::vector<armarx::ObjectID>& objectIDs);
protected:
std::string propertyPrefix() const final;
Properties defaultProperties() const final;
};
} // namespace armarx::armem::obj::clazz
#include "ClassWriter.h"
namespace armarx::armem::obj::clazz
{
ClassWriter::ClassWriter(const std::string& p) :
providerName(p)
{
}
void ClassWriter::setProviderName(const std::string& pName)
{
this->providerName = pName;
}
bool
ClassWriter::commitObjectClass(const armarx::armem::clazz::ObjectClass& c,
const armarx::core::time::DateTime& referenceTime)
{
armarx::armem::arondto::ObjectClass objClassAron;
armarx::armem::clazz::toAron(objClassAron, c);
armarx::armem::MemoryID mid;
mid.memoryName = properties().memoryName;
mid.coreSegmentName = properties().coreSegmentName;
mid.providerSegmentName = properties().providerName;
mid.entityName = c.id.str();
auto commit = armarx::armem::Commit();
auto& update = commit.add();
update.confidence = 1.0f;
update.referencedTime = referenceTime;
update.sentTime = armarx::core::time::DateTime::Now();
update.entityID = mid;
update.instancesData = {objClassAron.toAron()};
auto res = this->memoryWriter().commit(commit);
return res.allSuccess();
}
std::string
ClassWriter::propertyPrefix() const
{
return "classWriter.";
}
client::util::SimpleWriterBase::Properties
ClassWriter::defaultProperties() const
{
client::util::SimpleWriterBase::Properties p;
p.memoryName = "Object";
p.coreSegmentName = "Class";
p.providerName = providerName;
return p;
}
} // namespace armarx::armem::obj::clazz
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
#include <mutex>
#include <optional>
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::obj::clazz
{
class ClassWriter : public armem::client::util::SimpleWriterBase
{
public:
ClassWriter() = default;
ClassWriter(const std::string& p);
void setProviderName(const std::string& pName);
bool commitObjectClass(const armarx::armem::clazz::ObjectClass& c,
const armarx::core::time::DateTime& referenceTime);
protected:
std::string propertyPrefix() const final;
Properties defaultProperties() const final;
private:
std::string providerName;
};
} // namespace armarx::armem::obj::clazz