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.

Author: rhaschke

core/include/moveit/task_constructor/storage.h

@@ -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) {}