From 5819cddb9efacce754e76010d25dcef6effa651e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 27 Oct 2021 18:01:06 +0200
Subject: [PATCH] armem_robot_state: fixing getRobotJointStates
---
.../client/common/RobotReader.cpp | 72 +++++++++++++++++++
.../client/common/RobotReader.h | 10 +++
.../proprioception/RobotStateWriter.cpp | 2 +-
.../server/proprioception/RobotStateWriter.h | 1 -
4 files changed, 83 insertions(+), 2 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 5a5c279ad..2264ce364 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -203,6 +203,37 @@ namespace armarx::armem::robot_state
return getRobotJointState(qResult.memory, description.name);
}
+ RobotReader::JointTrajectory
+ RobotReader::queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin,
+ const armem::Time& end) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+ << "` on time interval [" << begin << "," << end << "]";
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().timeRange(begin, end);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return {};
+ }
+
+ return getRobotJointStates(qResult.memory, description.name);
+ }
+
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
@@ -275,6 +306,7 @@ namespace armarx::armem::robot_state
instance = &i;
return false; // break
});
+
if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
@@ -348,6 +380,46 @@ namespace armarx::armem::robot_state
return jointMap;
}
+ RobotReader::JointTrajectory
+ RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+
+ RobotReader::JointTrajectory jointTrajectory;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&jointTrajectory](const wm::Entity& entity)
+ {
+ entity.forEachSnapshot(
+ [&](const auto& snapshot)
+ {
+ if (not snapshot.hasInstance(0))
+ {
+ return;
+ }
+
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+ jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
+ });
+ });
+
+ ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
+
+ return jointTrajectory;
+ }
+
// force torque for left and right
std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
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 884c85fd9..20cafbd22 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -66,6 +66,12 @@ namespace armarx::armem::robot_state
queryJointState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+
+ JointTrajectory
+ queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin, const armem::Time& end) const;
+
std::optional<robot::RobotState::Pose>
queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
@@ -100,6 +106,10 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ JointTrajectory
+ getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
std::optional<robot::PlatformState>
getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index c2ed636a9..9b35f938d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::lock_guard lock{dataMutex};
queueSize = dataQueue.size();
- if (dataQueue.size() >= properties.memoryBatchSize)
+ if (!dataQueue.empty())
{
std::swap(batch, dataQueue);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 10c5a8b9e..1f66d5a70 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -84,7 +84,6 @@ namespace armarx::armem::server::robot_state::proprioception
struct Properties
{
- unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID;
};
Properties properties;
--
GitLab