diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp index 5eb382bfd405d8c0c4888493038488c14b4cfb9d..2179e10a3ca2af214565d0c9ec61ee622d2222b8 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 10e39558bbc258f6d44aacc32bfbffc85c81b119..7177c1ca55c540b15e35d0aa872c3a55422a0dcb 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 a534de74f16aaff69a74622096430e2155faba08..c35160792695b1274623cbdd99a183d88ff888db 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__))