From 54e653ebdbdee4085c2e0dba040fd4f037f5df9e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 24 May 2024 12:02:20 +0200 Subject: [PATCH] Fix failing assertion The cost of a newly created Priority does not need to be finite. An example occurs in the Pick+Place demo: the CurrentState solution is filtered by an applicibility test, which may set the cost to infinity while lifting the solution to the wrapper stage. In this case, new InterfaceStates are created from the infinite cost solution. Consequently, the state should be marked as PRUNED. --- core/include/moveit/task_constructor/storage.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 0b7dd54cb..e0796980d 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -94,10 +94,9 @@ class InterfaceState */ struct Priority : std::tuple { - Priority(unsigned int depth, double cost, Status status = ENABLED) - : std::tuple(status, depth, cost) { - assert(std::isfinite(cost)); - } + Priority(unsigned int depth, double cost, Status status) + : std::tuple(status, depth, cost) {} + Priority(unsigned int depth, double cost) : Priority(depth, cost, std::isfinite(cost) ? ENABLED : PRUNED) {} // Constructor copying depth and cost, but modifying its status Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {}