From 0b5808d803eee45e6140d096768fd173990f1c50 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Fri, 24 Nov 2023 09:04:07 +0100
Subject: [PATCH] add locks to robot state reader and transform reader to
secure the prx. Make transform reader copyable.
---
.../client/common/RobotReader.cpp | 114 +++++++++++++-----
.../client/common/RobotReader.h | 2 +-
.../client/localization/TransformReader.cpp | 56 ++++++---
.../client/localization/TransformReader.h | 3 +-
4 files changed, 125 insertions(+), 50 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 a8280d7e9..8f7c057e4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -162,6 +162,7 @@ namespace armarx::armem::robot_state
try
{
+ std::scoped_lock l(memoryReaderMutex);
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -256,6 +257,7 @@ namespace armarx::armem::robot_state
try
{
+ std::scoped_lock l(memoryReaderMutex);
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -294,17 +296,27 @@ namespace armarx::armem::robot_state
.snapshots().timeRange(begin, end);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return {};
+ }
- if (not qResult.success) /* c++20 [[unlikely]] */
+ return getRobotJointStates(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint states. Reason: "
+ << GetHandledExceptionString();
return {};
}
-
- return getRobotJointStates(qResult.memory, description.name);
}
std::optional<robot::PlatformState>
@@ -326,17 +338,27 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- if (not qResult.success) /* c++20 [[unlikely]] */
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotPlatformState(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query platform state. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getRobotPlatformState(qResult.memory, description.name);
}
std::optional<robot::RobotState::Pose>
@@ -503,17 +525,27 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
- if (not qResult.success) /* c++20 [[unlikely]] */
+ return getForceTorque(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torque. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getForceTorque(qResult.memory, description.name);
}
// force torque for left and right
@@ -536,17 +568,27 @@ namespace armarx::armem::robot_state
.snapshots().timeRange(start, end);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
- if (not qResult.success) /* c++20 [[unlikely]] */
+ return getForceTorques(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torques. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getForceTorques(qResult.memory, description.name);
}
std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
@@ -566,17 +608,27 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- if (not qResult.success) /* c++20 [[unlikely]] */
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getToF(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1)
+ << "Failed to query ToF. Reason: " << GetHandledExceptionString();
return std::nullopt;
}
-
- return getToF(qResult.memory, description.name);
}
std::optional<robot::PlatformState>
@@ -652,7 +704,6 @@ namespace armarx::armem::robot_state
tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
ARMARX_CHECK(proprioception.has_value());
-
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
robot::ForceTorque forceTorque;
@@ -809,6 +860,7 @@ namespace armarx::armem::robot_state
try
{
+ std::scoped_lock l(memoryReaderMutex);
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
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 dbf81e69a..8397ea9a8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -163,7 +163,7 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
armem::client::Reader memoryReader;
- mutable std::mutex memoryWriterMutex;
+ mutable std::mutex memoryReaderMutex;
client::robot_state::localization::TransformReader transformReader;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 51739021f..5a221a342 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -59,6 +59,12 @@
namespace armarx::armem::client::robot_state::localization
{
+ TransformReader::TransformReader(const TransformReader& t) :
+ memoryReader(t.memoryReader), properties(t.properties), propertyPrefix(t.propertyPrefix)
+ {
+
+ }
+
TransformReader::~TransformReader() = default;
void
@@ -147,25 +153,41 @@ namespace armarx::armem::client::robot_state::localization
.snapshots().beforeOrAtTime(query.header.timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
-
- if (not qResult.success)
+ try
{
- return {.transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage};
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success)
+ {
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
+ }
+
+ const auto& localizationCoreSegment =
+ qResult.memory.getCoreSegment(properties.localizationSegment);
+
+ using armarx::armem::common::robot_state::localization::TransformHelper;
+ return TransformHelper::lookupTransform(localizationCoreSegment, query);
+ }
+ catch (...)
+ {
+ ARMARX_VERBOSE << "Lookup Transform failure" << GetHandledExceptionString();
+ return TransformResult{.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::Error,
+ .errorMessage = "Error in tf lookup '" +
+ query.header.parentFrame + " -> " +
+ query.header.frame + "' : Memory exception."};
}
-
- const auto& localizationCoreSegment =
- qResult.memory.getCoreSegment(properties.localizationSegment);
-
- using armarx::armem::common::robot_state::localization::TransformHelper;
- return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 4d9ee5983..32b3c2fc1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -44,7 +44,7 @@ namespace armarx::armem::client::robot_state::localization
{
public:
TransformReader() = default;
- TransformReader(const TransformReader&) = default;
+ TransformReader(const TransformReader&);
~TransformReader() override;
@@ -60,6 +60,7 @@ namespace armarx::armem::client::robot_state::localization
private:
armem::client::Reader memoryReader;
+ mutable std::mutex memoryReaderMutex;
// Properties
struct Properties
--
GitLab