diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index a827a7ea4b06898d3b41ff4692c112181e4f636e..76d042a9ecd96b1d6f03814efe6cfb8d609eb0d5 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -143,7 +143,8 @@ namespace armarx::armem::server::obj
// TODO: the instance segment should check the provided object poses and instantiate the needed robots by itself.
ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
- VirtualRobot::RobotPtr robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(robotName, armarx::Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure, true);
+ VirtualRobot::RobotPtr robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
+ robotName, VirtualRobot::RobotIO::RobotDescription::eStructure, true);
ARMARX_CHECK_NOT_NULL(robot);
getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
index 964aa2298bd839313f4f5df7b82b7d7ca7a26720..168686040e37f0346757b4ee259dd0107b0e4a37 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
@@ -48,7 +48,11 @@ namespace armarx::armem::client::plugins
ReaderWriterPlugin(ManagedIceObject& parent, const std::string pre) :
ComponentPlugin(parent, pre), readerWriter(memoryNameSystem())
{
- // addPlugin(armemPlugin);
+ if (not armemPlugin)
+ {
+ addPlugin(armemPlugin);
+ }
+ addPluginDependency(armemPlugin);
}
~ReaderWriterPlugin() override = default;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 4bedcf383fd89f0960753fa0e03f35df5244c3fb..79e199840991ff90bac951bbe1d3d670c7131026 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -190,7 +190,7 @@ namespace armarx::armem::robot_state
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
- ARMARX_WARNING << "Failed to query joint state for robot " << description.name;
+ ARMARX_WARNING << "Failed to query joint state for robot '" << description.name << "'.";
return std::nullopt;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 58f56a4d769d7bea7eaff44a7b556d0c863eed54..0eb7e2dde3c4e9e56342325c1069f499230186ee 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -47,8 +47,8 @@ namespace armarx::armem::robot_state
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / "
- << robot.getType() << "`!";
+ ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << "` "
+ << "(type `"<< robot.getType() << "`)!";
return false;
}
@@ -82,7 +82,7 @@ namespace armarx::armem::robot_state
const bool success = synchronizeRobot(*robot, timestamp);
- if(not success)
+ if (not success)
{
ARMARX_WARNING << "Could not synchronize robot `" << name << "` with the memory!";
return nullptr;
@@ -92,17 +92,40 @@ namespace armarx::armem::robot_state
}
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getSynchronizedRobot(
+ const std::string& name,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
+ {
+ return _getSynchronizedRobot(name, armem::Time::Invalid(), loadMode, blocking);
+ }
+
+
VirtualRobot::RobotPtr
VirtualRobotReader::getSynchronizedRobot(
const std::string& name,
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode,
const bool blocking)
+ {
+ return _getSynchronizedRobot(name, timestamp, loadMode, blocking);
+ }
+
+
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::_getSynchronizedRobot(
+ const std::string& name,
+ const Time& timestamp,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
while (blocking)
{
- VirtualRobot::RobotPtr robot = getRobot(name, timestamp, loadMode);
- if (robot and synchronizeRobot(*robot, timestamp))
+ const auto ts = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
+
+ VirtualRobot::RobotPtr robot = getRobot(name, ts, loadMode);
+ if (robot and synchronizeRobot(*robot, ts))
{
return robot;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index c14e60aee78a490bd1ffb0937ca008dc56895499..ea1d87b62fd2ad54b536b607d4ba316ae16e3777 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -55,12 +55,29 @@ namespace armarx::armem::robot_state
const VirtualRobot::RobotIO::RobotDescription& loadMode =
VirtualRobot::RobotIO::RobotDescription::eStructure);
+ [[nodiscard]] VirtualRobot::RobotPtr
+ getSynchronizedRobot(const std::string& name,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
+ bool blocking = true);
+
[[nodiscard]] VirtualRobot::RobotPtr
getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode =
VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
+
+
+ private:
+
+ [[nodiscard]] VirtualRobot::RobotPtr
+ _getSynchronizedRobot(const std::string& name,
+ const armem::Time& timestamp = armem::Time::Invalid(),
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
+ bool blocking = true);
+
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 1c4ae2923cb0d24f57fd5fda32524d6b5100eefe..1f6fb1582f3af4a98ba739a9621bd6db7988e160 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -73,6 +73,7 @@ set(LIB_FILES
data/rw/Reader.cpp
data/rw/reader/variant/VariantReader.cpp
data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+ data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
type/rw/Writer.cpp
type/rw/writer/variant/VariantWriter.cpp
@@ -211,6 +212,7 @@ set(LIB_HEADERS
data/rw/Reader.h
data/rw/reader/variant/VariantReader.h
data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
+ data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
type/rw/json/Data.h
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 5e91dfaca657f45e9ca1c9e15a044e6097f79775..21fdfc82b0fdec76910890100721d6f5ee19654c 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -39,13 +39,13 @@ namespace armarx::aron::data::reader
void getAronMetaInformationForType(const nlohmann::json& input, const std::string& expectedType, Path& p)
{
// check type
- if (input[rw::json::constantes::TYPE_SLUG] != expectedType)
+ if (input.at(rw::json::constantes::TYPE_SLUG) != expectedType)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input[rw::json::constantes::TYPE_SLUG], expectedType);
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input.at(rw::json::constantes::TYPE_SLUG), expectedType);
}
// set path
- std::vector<std::string> pathElements = input[rw::json::constantes::PATH_SLUG];
+ std::vector<std::string> pathElements = input.at(rw::json::constantes::PATH_SLUG);
p = Path(pathElements);
}
}
@@ -58,56 +58,56 @@ namespace armarx::aron::data::reader
void NlohmannJSONReader::readList(const nlohmann::json& input, std::vector<nlohmann::json>& elements, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::LIST_TYPENAME_SLUG, p);
- elements = input[rw::json::constantes::ELEMENTS_SLUG].get<std::vector<nlohmann::json>>();
+ elements = input.at(rw::json::constantes::ELEMENTS_SLUG).get<std::vector<nlohmann::json>>();
}
void NlohmannJSONReader::readDict(const nlohmann::json& input, std::map<std::string, nlohmann::json>& elements, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::DICT_TYPENAME_SLUG, p);
- elements = input[rw::json::constantes::ELEMENTS_SLUG].get<std::map<std::string, nlohmann::json>>();
+ elements = input.at(rw::json::constantes::ELEMENTS_SLUG).get<std::map<std::string, nlohmann::json>>();
}
void NlohmannJSONReader::readNDArray(const nlohmann::json& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::NDARRAY_TYPENAME_SLUG, p);
- shape = input[rw::json::constantes::DIMENSIONS_SLUG].get<std::vector<int>>();
- typeAsString = input[rw::json::constantes::USED_TYPE_SLUG];
- data = input[rw::json::constantes::DATA_SLUG].get<std::vector<unsigned char>>();
+ shape = input.at(rw::json::constantes::DIMENSIONS_SLUG).get<std::vector<int>>();
+ typeAsString = input.at(rw::json::constantes::USED_TYPE_SLUG);
+ data = input.at(rw::json::constantes::DATA_SLUG).get<std::vector<unsigned char>>();
}
void NlohmannJSONReader::readInt(const nlohmann::json& input, int& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::INT_TYPENAME_SLUG, p);
- i = input[rw::json::constantes::VALUE_SLUG];
+ i = input.at(rw::json::constantes::VALUE_SLUG);
}
void NlohmannJSONReader::readLong(const nlohmann::json& input, long& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::LONG_TYPENAME_SLUG, p);
- i = input[rw::json::constantes::VALUE_SLUG];
+ i = input.at(rw::json::constantes::VALUE_SLUG);
}
void NlohmannJSONReader::readFloat(const nlohmann::json& input, float& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::FLOAT_TYPENAME_SLUG, p);
- i = input[rw::json::constantes::VALUE_SLUG];
+ i = input.at(rw::json::constantes::VALUE_SLUG);
}
void NlohmannJSONReader::readDouble(const nlohmann::json& input, double& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::DOUBLE_TYPENAME_SLUG, p);
- i = input[rw::json::constantes::VALUE_SLUG];
+ i = input.at(rw::json::constantes::VALUE_SLUG);
}
void NlohmannJSONReader::readString(const nlohmann::json& input, std::string& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::STRING_TYPENAME_SLUG, p);
- i = input[rw::json::constantes::VALUE_SLUG];
+ i = input.at(rw::json::constantes::VALUE_SLUG);
}
void NlohmannJSONReader::readBool(const nlohmann::json& input, bool& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::BOOL_TYPENAME_SLUG, p);
- i = input[rw::json::constantes::VALUE_SLUG];
+ i = input.at(rw::json::constantes::VALUE_SLUG);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e773c517d9ceee0c8c166f327b639a2c8f799f6b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
@@ -0,0 +1,107 @@
+/*
+* 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 Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+#include <memory>
+#include <numeric>
+
+// Header
+#include "NlohmannJSONReaderWithoutTypeCheck.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
+
+#include "../../json/Data.h"
+
+
+namespace armarx::aron::data::reader
+{
+ data::Descriptor
+ NlohmannJSONReaderWithoutTypeCheck::getDescriptor(InputType& input)
+ {
+ return ConstNlohmannJSONVisitor::GetDescriptor(input);
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readList(const nlohmann::json& input,
+ std::vector<nlohmann::json>& elements,
+ Path& p)
+ {
+ elements = input.get<std::vector<nlohmann::json>>();
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readDict(const nlohmann::json& input,
+ std::map<std::string, nlohmann::json>& elements,
+ Path& p)
+ {
+ elements = input.get<std::map<std::string, nlohmann::json>>();
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readNDArray(const nlohmann::json& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path& p)
+ {
+ shape = input.at("dims").get<std::vector<int>>();
+ data = input.at("data").get<std::vector<unsigned char>>();
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readInt(const nlohmann::json& input, int& i, Path& p)
+ {
+ i = input;
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readLong(const nlohmann::json& input, long& i, Path& p)
+ {
+ i = input;
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readFloat(const nlohmann::json& input, float& i, Path& p)
+ {
+ i = input;
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readDouble(const nlohmann::json& input, double& i, Path& p)
+ {
+ i = input;
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readString(const nlohmann::json& input,
+ std::string& i,
+ Path& p)
+ {
+ i = input;
+ }
+
+ void
+ NlohmannJSONReaderWithoutTypeCheck::readBool(const nlohmann::json& input, bool& i, Path& p)
+ {
+ i = input;
+ }
+} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0bed7b71869e2abc074484fe3fb9774e0ef6730
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
@@ -0,0 +1,73 @@
+/*
+* 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 Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <stack>
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// BaseClass
+#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
+
+namespace armarx::aron::data::reader
+{
+ class NlohmannJSONReaderWithoutTypeCheck : public ReaderInterface<const nlohmann::json>
+ {
+ using Base = ReaderInterface<const nlohmann::json>;
+
+ public:
+ // constructors
+ NlohmannJSONReaderWithoutTypeCheck() = default;
+
+ data::Descriptor getDescriptor(InputType& input) final;
+
+ using Base::readBool;
+ using Base::readDict;
+ using Base::readDouble;
+ using Base::readFloat;
+ using Base::readInt;
+ using Base::readList;
+ using Base::readLong;
+ using Base::readNDArray;
+ using Base::readString;
+
+ void readList(InputType& input, std::vector<InputTypeNonConst>& elements, Path& p) override;
+ void readDict(InputType& input,
+ std::map<std::string, InputTypeNonConst>& elements,
+ Path& p) override;
+
+ void readNDArray(InputType& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path& p) override;
+
+ void readInt(InputType& input, int& i, Path& p) override;
+ void readLong(InputType& input, long& i, Path& p) override;
+ void readFloat(InputType& input, float& i, Path& p) override;
+ void readDouble(InputType& input, double& i, Path& p) override;
+ void readString(InputType& input, std::string& i, Path& p) override;
+ void readBool(InputType& input, bool& i, Path& p) override;
+ };
+} // namespace armarx::aron::data::reader