fix: RobotReader: try-catch blocks

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>