From d56da92bc6ec006df45dcd19b55ed13715f24376 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 27 May 2021 08:12:56 +0200
Subject: [PATCH] RobotReader: implemented getRobotJointState()
---
.../client/common/RobotReader.cpp | 140 +++++++++++-------
1 file changed, 85 insertions(+), 55 deletions(-)
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 525365942..2d8319dc9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -7,19 +7,23 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) : memoryClient(component), transformReader(component) {}
+ RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) :
+ memoryClient(component), transformReader(component)
+ {
+ }
void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
@@ -28,7 +32,8 @@ namespace armarx::armem::robot_state
def->optional(properties.memoryName, propertyPrefix + "Memory");
def->optional(properties.descriptionCoreSegment, propertyPrefix + "descriptionSegment");
def->optional(properties.localizationCoreSegment, propertyPrefix + "localizationSegment");
- def->optional(properties.proprioceptionCoreSegment, propertyPrefix + "proprioceptionSegment");
+ def->optional(properties.proprioceptionCoreSegment,
+ propertyPrefix + "proprioceptionSegment");
}
void RobotReader::connect()
@@ -36,8 +41,7 @@ namespace armarx::armem::robot_state
transformReader.connect();
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName << "' ...";
auto result = memoryClient.useMemory(properties.memoryName);
if (not result.success)
{
@@ -50,7 +54,8 @@ namespace armarx::armem::robot_state
memoryReader.setReadingMemory(result.proxy);
}
- std::optional<robot::Robot> RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ std::optional<robot::Robot> RobotReader::get(const std::string& name,
+ const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -63,17 +68,13 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
-
robot::Robot RobotReader::get(const robot::RobotDescription& description,
const armem::Time& timestamp)
{
- robot::Robot robot
- {
- .description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp
- };
+ robot::Robot robot{.description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
synchronize(robot, timestamp);
@@ -94,7 +95,8 @@ namespace armarx::armem::robot_state
return true;
}
- std::optional<robot::RobotDescription> RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ std::optional<robot::RobotDescription>
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -127,7 +129,6 @@ namespace armarx::armem::robot_state
}
return getRobotDescription(qResult.memory, name);
-
}
catch (...)
{
@@ -135,32 +136,34 @@ namespace armarx::armem::robot_state
}
return std::nullopt;
-
}
- std::optional<robot::RobotState> RobotReader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+ std::optional<robot::RobotState>
+ RobotReader::queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
{
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
+ ARMARX_WARNING << "Failed to query joint state";
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
+ ARMARX_WARNING << "Failed to query global pose";
return std::nullopt;
}
return robot::RobotState
{
- .timestamp = timestamp,
- .globalPose = *globalPose,
- .jointMap = *jointMap
- };
+ .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
- std::optional<robot::RobotState::JointMap> RobotReader::queryJointState(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ std::optional<robot::RobotState::JointMap>
+ RobotReader::queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -181,13 +184,16 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
+ ARMARX_WARNING << qResult.errorMessage;
return std::nullopt;
}
return getRobotJointState(qResult.memory, description.name);
}
- std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ 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);
if (not result)
@@ -198,8 +204,9 @@ namespace armarx::armem::robot_state
return result.transform.transform;
}
-
- std::optional<robot::RobotState> RobotReader::getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ std::optional<robot::RobotState>
+ RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -231,44 +238,67 @@ namespace armarx::armem::robot_state
return robot::convertRobotState(instance);
}
+ // FIXME remove this, use armem/util/util.h
+ template <typename AronClass>
+ std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ {
+ static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ AronClass>::value);
+
+ try
+ {
+ AronClass t;
+ t.fromAron(item.data());
+ return t;
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ return std::nullopt;
+ }
+ }
- std::optional<robot::RobotState::JointMap> RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ std::optional<robot::RobotState::JointMap>
+ RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
- // // clang-format off
- // const armem::wm::ProviderSegment& providerSegment = memory
- // .getCoreSegment(properties.proprioceptionCoreSegment)
- // .getProviderSegment(name);
- // // clang-format on
- // const auto entities = simox::alg::get_values(providerSegment.entities());
- // // TODO entitiesToRobotState()
+ robot::RobotState::JointMap jointMap;
- // if (entities.empty())
- // {
- // ARMARX_WARNING << "No entity found";
- // return std::nullopt;
- // }
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
- // const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ {
- // if (entitySnapshots.empty())
- // {
- // ARMARX_WARNING << "No entity snapshots found";
- // return std::nullopt;
- // }
+ for (const auto &[name, entity] : providerSegment.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // // TODO(fabian.reister): check if 0 available
- // const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // // Here, we access the RobotUnit streaming data stored in the proprioception segment.
- // return robot::convertRobotState(instance);
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
- return std::nullopt; // TODO implement
- }
+ jointMap.emplace(jointState->name, jointState->position);
+ }
+ }
+ if (jointMap.empty())
+ {
+ return std::nullopt;
+ }
+ return jointMap;
+ }
- std::optional<robot::RobotDescription> RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ std::optional<robot::RobotDescription>
+ RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -297,4 +327,4 @@ namespace armarx::armem::robot_state
return robot::convertRobotDescription(instance);
}
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
\ No newline at end of file
--
GitLab