From bb8b53b4a32b8b0f792f18704fbdaee249206b3f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 3 Jan 2023 20:23:52 +0100
Subject: [PATCH] fix: CHECK_MESSAGE for VirtualRobot (simple checks)
---
...ticulatedObjectGeometricPlanningHelper.cpp | 4 ++-
.../assert/virtual_robot/assert.cpp | 22 ++++++++++--
.../assert/virtual_robot/assert.h | 35 ++++++++++++++++++-
3 files changed, 57 insertions(+), 4 deletions(-)
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index 5eb382bfd..2179e10a3 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -183,7 +183,9 @@ namespace simox::geometric_planning
subpart->registerRobotNode(jointReferenceNode);
CHECK_MESSAGE(jointReferenceNode->initialize(subpart->getRootNode()),
- "Failed to initialize node `" << jointReferenceNode->getName() << "`!");
+ "Failed to initialize node `",
+ jointReferenceNode->getName(),
+ "`!");
// reset joint state
joint->setJointValue(initialJointValue);
diff --git a/GeometricPlanning/assert/virtual_robot/assert.cpp b/GeometricPlanning/assert/virtual_robot/assert.cpp
index 10e39558b..7177c1ca5 100644
--- a/GeometricPlanning/assert/virtual_robot/assert.cpp
+++ b/GeometricPlanning/assert/virtual_robot/assert.cpp
@@ -1,9 +1,11 @@
#include "assert.h"
#include <sstream>
-#include <experimental/source_location>
+
#include <VirtualRobot/VirtualRobotException.h>
+#include <experimental/source_location>
+
namespace simox::geometric_planning::assert::virtual_robot::detail
{
void
@@ -21,4 +23,20 @@ namespace simox::geometric_planning::assert::virtual_robot::detail
}
}
-} // namespace simox::geometric_planning::assert::virtual_robot::detail
+ void
+ checkMessage(bool expressionResult,
+ const std::string& expression,
+ const std::function<std::string()>& messageFtor,
+ const std::experimental::source_location& sourceLocation)
+ {
+ if (not expressionResult)
+ {
+ std::stringstream stream;
+ stream << "Check `" << expression << "` failed in function `"
+ << sourceLocation.function_name() << "` line " << sourceLocation.line() << " at "
+ << sourceLocation.file_name() << ". " << messageFtor();
+ throw VirtualRobot::VirtualRobotException(stream.str());
+ }
+ }
+
+} // namespace simox::geometric_planning::assert::virtual_robot::detail
diff --git a/GeometricPlanning/assert/virtual_robot/assert.h b/GeometricPlanning/assert/virtual_robot/assert.h
index a534de74f..c35160792 100644
--- a/GeometricPlanning/assert/virtual_robot/assert.h
+++ b/GeometricPlanning/assert/virtual_robot/assert.h
@@ -1,6 +1,9 @@
#pragma once
+#include <sstream>
+
#include <VirtualRobot/VirtualRobot.h>
+
#include <experimental/source_location>
#define REQUIRE(a) VR_ASSERT(a)
@@ -13,6 +16,36 @@ namespace simox::geometric_planning::assert::virtual_robot::detail
const std::experimental::source_location& sourceLocation =
std::experimental::source_location::current());
-} // namespace simox::geometric_planning::assert
+ /**
+ * @brief Helper function for lazy string/stream concatenation of var args
+ *
+ */
+ template <typename... Args>
+ auto
+ messageFn(Args... args)
+ {
+ return [=]() -> std::string
+ {
+ std::stringstream stream;
+
+ // fold expression: https://en.cppreference.com/w/cpp/language/fold
+ ([&] { stream << args; }(), ...);
+
+ return stream.str();
+ };
+ }
+
+
+ void checkMessage(bool expressionResult,
+ const std::string& expression,
+ const std::function<std::string()>& messageFtor,
+ const std::experimental::source_location& sourceLocation =
+ std::experimental::source_location::current());
+
+
+} // namespace simox::geometric_planning::assert::virtual_robot::detail
#define CHECK(a) ::simox::geometric_planning::assert::virtual_robot::detail::check(a, #a)
+#define CHECK_MESSAGE(a, ...) \
+ ::simox::geometric_planning::assert::virtual_robot::detail::checkMessage( \
+ a, #a, ::simox::geometric_planning::assert::virtual_robot::detail::messageFn(__VA_ARGS__))
--
GitLab