Skip to content

Commit

Permalink
Fix failing assertion
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
rhaschke committed May 24, 2024
1 parent 9c05305 commit 54e653e
Showing 1 changed file with 3 additions and 4 deletions.
7 changes: 3 additions & 4 deletions core/include/moveit/task_constructor/storage.h
Original file line number Diff line number Diff line change
Expand Up @@ -94,10 +94,9 @@ class InterfaceState
*/
struct Priority : std::tuple<Status, unsigned int, double>
{
Priority(unsigned int depth, double cost, Status status = ENABLED)
: std::tuple<Status, unsigned int, double>(status, depth, cost) {
assert(std::isfinite(cost));
}
Priority(unsigned int depth, double cost, Status status)
: std::tuple<Status, unsigned int, double>(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) {}

Expand Down

0 comments on commit 54e653e

Please sign in to comment.