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 a355439ef72d713f64c9ec7be8675f71196d73f7..ec61aff0c4ae0e8aab22d6a05e90228600035145 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -3,6 +3,7 @@
#include <mutex>
#include <optional>
+#include "ArmarXCore/core/exceptions/LocalException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/PackagePath.h>
@@ -18,11 +19,35 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) : component(component), transformReader(component) {}
+ RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) : memoryClient(component), transformReader(component) {}
void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
transformReader.registerPropertyDefinitions(def);
+
+ def->optional(properties.memoryName, propertyPrefix + "Memory");
+ def->optional(properties.descriptionCoreSegment, propertyPrefix + "descriptionSegment");
+ def->optional(properties.localizationCoreSegment, propertyPrefix + "localizationSegment");
+ def->optional(properties.proprioceptionCoreSegment, propertyPrefix + "proprioceptionSegment");
+ }
+
+ void RobotReader::connect()
+ {
+ transformReader.connect();
+
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ auto result = memoryClient.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
}
std::optional<robot::Robot> RobotReader::get(const std::string& name, const armem::Time& timestamp)
@@ -76,34 +101,53 @@ namespace armarx::armem::robot_state
// clang-format off
qb
- .coreSegments().withName(properties.descriptionCoreSegment)
- .providerSegments().withName(name)
+ .coreSegments().all() // withName(properties.descriptionCoreSegment)
+ .providerSegments().all() // .withName(name)
.entities().all()
.snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ ARMARX_INFO << "Lookup query in reader";
- if (not qResult.success) /* c++20 [[unlikely]] */
+ if (not memoryReader)
{
+ ARMARX_WARNING << "Memory reader is null";
return std::nullopt;
}
- return getRobotDescription(qResult.memory, name);
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_INFO << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotDescription(qResult.memory, name);
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
+ }
+
+ return std::nullopt;
+
}
- std::optional<robot::RobotState> RobotReader::queryState(const robot::RobotDescription &description, const armem::Time ×tamp)
+ std::optional<robot::RobotState> RobotReader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
{
const auto jointMap = queryJointState(description, timestamp);
- if(not jointMap)
+ if (not jointMap)
{
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
- if(not globalPose)
+ if (not globalPose)
{
return std::nullopt;
}
@@ -146,9 +190,9 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const
{
const auto result = transformReader.getGlobalPose(description.name, "root", timestamp.toMicroSeconds());
- if(not result)
+ if (not result)
{
- return std::nullopt;
+ return std::nullopt;
}
return result.transform.transform;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index bc904e9fce5e0c72e40679af1adba252adad52d8..ba4d3d4177831c4609c65eedb5b51094b37d4e03 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -41,7 +41,8 @@ namespace armarx::armem::robot_state
RobotReader(armem::ClientReaderComponentPluginUser& component);
virtual ~RobotReader() = default;
- // TODO(fabian.reister): register property defs
+ void connect();
+
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
@@ -75,7 +76,7 @@ namespace armarx::armem::robot_state
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
- armem::ClientReaderComponentPluginUser& component;
+ armem::ClientReaderComponentPluginUser& memoryClient;
client::robot_state::localization::TransformReader transformReader;
};
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 3b91aac563d2b5ac909639680ba84b796394350b..97633b788dcfae4cd93d6a1fd7a72850a9a009b1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -1,27 +1,42 @@
#include "VirtualRobotReader.h"
-#include "ArmarXCore/core/PackagePath.h"
+
+#include <optional>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <optional>
+
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+
namespace armarx::armem::robot_state
{
- VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser &component) :
+ VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser& component) :
RobotReader(component)
{
}
+ void VirtualRobotReader::connect()
+ {
+ RobotReader::connect();
+ }
+
// TODO(fabian.reister): register property defs
- void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr &def) {}
+ void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ RobotReader::registerPropertyDefinitions(def);
+ }
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot &robot,
- const armem::Time ×tamp)
+ bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp)
{
+ const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", robot.getFilename()}};
+ .xml = PackagePath{package, robot.getFilename()}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
@@ -35,18 +50,20 @@ namespace armarx::armem::robot_state
return true;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string &name,
- const armem::Time ×tamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
+ ARMARX_INFO << "Querying robot description for robot '" << name << "'";
const auto description = queryDescription(name, timestamp);
- if(not description)
+ if (not description)
{
return nullptr;
}
- const std::string xmlFilename = ""; // description->xml;
+ const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
return VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
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 4ce18449012cf09a0a2cd2ce9a39f57b15f88e85..bdabb58068f81a9f92e56ba3931c02c85d4396e4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -30,19 +30,28 @@
namespace armarx::armem::robot_state
{
- class VirtualRobotReader:
- virtual public RobotReader
+ /**
+ * @brief The VirtualRobotReader class.
+ *
+ * The aim of this class is to obtain a virtual robot instance and synchronize it
+ * with the data (joint positions, global pose, ...) stored in the working memory.
+ *
+ */
+ class VirtualRobotReader : virtual public RobotReader
{
public:
VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
virtual ~VirtualRobotReader() = default;
// TODO(fabian.reister): register property defs
+ void connect();
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
- VirtualRobot::RobotPtr getRobot(const std::string& name, const armem::Time& timestamp, const VirtualRobot::RobotIO::RobotDescription& loadMode);
- };
+ VirtualRobot::RobotPtr getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode = VirtualRobot::RobotIO::RobotDescription::eStructure);
+ };
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
\ No newline at end of file