From d910118800bb0605e326998562134f14e71b0b40 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 7 Jun 2022 16:38:52 +0200
Subject: [PATCH] fix: RobotReader: try-catch blocks
---
.../client/common/RobotReader.cpp | 60 ++++++++++++-------
1 file changed, 38 insertions(+), 22 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 f8eaaa643..4bedcf383 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -210,25 +210,23 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) const
{
- common::robot_state::localization::TransformQuery query
- {
- .header = {
- .parentFrame = OdometryFrame,
- .frame = "root",
- .agent = description.name,
- .timestamp = timestamp
- }
- };
+ common::robot_state::localization::TransformQuery query{
+ .header = {.parentFrame = OdometryFrame,
+ .frame = "root",
+ .agent = description.name,
+ .timestamp = timestamp}};
- try {
+ try
+ {
const auto result = transformReader.lookupTransform(query);
if (not result)
{
return std::nullopt;
}
return result.transform;
-
- } catch (...) {
+ }
+ catch (...)
+ {
ARMARX_WARNING << GetHandledExceptionString();
return std::nullopt;
}
@@ -254,17 +252,26 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ 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_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotJointState(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_WARNING << deactivateSpam(1) << "Failed to query joint state. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getRobotJointState(qResult.memory, description.name);
}
RobotReader::JointTrajectory
@@ -335,13 +342,22 @@ namespace armarx::armem::robot_state
RobotReader::queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const
{
- const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
- if (not result)
+ try
{
+ const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
+ if (not result)
+ {
+ return std::nullopt;
+ }
+
+ return result.transform.transform;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Failed to query global pose. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return result.transform.transform;
}
std::optional<robot::RobotState>
--
GitLab