fix: CHECK_MESSAGE for VirtualRobot (simple checks)

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);

GeometricPlanning/assert/virtual_robot/assert.cpp

 #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

GeometricPlanning/assert/virtual_robot/assert.h

 #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__))