diff --git a/src/routes/vehicle/SAEVRoute.cpp b/src/routes/vehicle/SAEVRoute.cpp index ccdd0b17b85cfddbcc89236ca6822330d668f523..b3053fc6a6666ed1b87e5d9b1e6fdb072ad97049 100644 --- a/src/routes/vehicle/SAEVRoute.cpp +++ b/src/routes/vehicle/SAEVRoute.cpp @@ -218,14 +218,15 @@ SAEVRoute::doNeighbouringTWChecks(const SAEVKeyPoint &originKP, const SAEVKeyPoi SAEVRouteChangelist SAEVRoute::insertRequestWithPropagation(SAEVKeyPoint &originKP, SAEVKeyPoint * originRequestPredecessorKP, SAEVKeyPoint * destinationRequestPredecessorKP) { //Init changelist and detour score - SAEVRouteChangelist changelist{this, &originKP, originRequestPredecessorKP, destinationRequestPredecessorKP, SAEVRouteChangelist::InsertionStatus::FAILURE_MIN}; + SAEVRouteChangelist changelist{this, &originKP, originRequestPredecessorKP, destinationRequestPredecessorKP, + SAEVRouteChangelist::InsertionStatus::FAILURE_MIN}; double detourScore = getDetourScore(originKP, originRequestPredecessorKP, destinationRequestPredecessorKP); //Properly insert the request to facilitate constraint propagation insertRequest(originKP, originRequestPredecessorKP, destinationRequestPredecessorKP); //Initialize bound propagation signal queue (each item signals a modification done on one of a KeyPoint - std::queue<std::pair<Bound, SAEVKeyPoint *>> boundPropagationQueue{}; - SAEVKeyPoint * destinationKP = originKP.getCounterpart(); + std::queue<std::pair<int, SAEVKeyPoint *>> boundPropagationQueue{}; + SAEVKeyPoint *destinationKP = originKP.getCounterpart(); boundPropagationQueue.emplace(Min, originKP.getPredecessor()); boundPropagationQueue.emplace(Max, originKP.getSuccessor()); boundPropagationQueue.emplace(Min, destinationKP->getPredecessor()); @@ -235,6 +236,14 @@ SAEVRouteChangelist SAEVRoute::insertRequestWithPropagation(SAEVKeyPoint &origin boundPropagationQueue.emplace(Min, destinationKP); boundPropagationQueue.emplace(Max, destinationKP); + doBoundPropagation(boundPropagationQueue, changelist); + changelist.setScore(detourScore); + return changelist; +} + + +SAEVRouteChangelist SAEVRoute::doBoundPropagation(std::queue<std::pair<int, SAEVKeyPoint *>> &boundPropagationQueue, + SAEVRouteChangelist &changelist) { //Pre-init variables used in the loop int oldValue; int newValue; @@ -311,7 +320,6 @@ SAEVRouteChangelist SAEVRoute::insertRequestWithPropagation(SAEVKeyPoint &origin } changelist.setStatus(SAEVRouteChangelist::InsertionStatus::SUCCESS); - changelist.setScore(detourScore); return changelist; } diff --git a/src/routes/vehicle/SAEVRoute.h b/src/routes/vehicle/SAEVRoute.h index f9ce97aea560b4cd1af12ddb9a0069939df584a7..18016d8117fe84750e53465960d0ba6222c1af0f 100644 --- a/src/routes/vehicle/SAEVRoute.h +++ b/src/routes/vehicle/SAEVRoute.h @@ -124,6 +124,18 @@ public: */ SAEVRouteChangelist insertRequestWithPropagation(SAEVKeyPoint &originKP, SAEVKeyPoint * originRequestPredecessorKP, SAEVKeyPoint * destinationRequestPredecessorKP); + /** + * Executes the bound propagation algorithm, starting from the given bound propagation queue. + * Each item in the queue references which bound (Min/Max) and what key point was affected, and from there we verify bounds for each predecessor/successor + * + * If the bounds become infeasible (min > max), then the propagation stops with a changelist with score= +Infinity and changes will be immediately reverted. + * Otherwise, it's the responsibility of this method's callers to revert changes if wanted (or to defer this responsibility to its caller) + * + * @param boundPropagationQueue a (Bound/Keypoint) pair queue giving a list of starting points for propagation. Typically when inserting a key point, the queue contains two items (one for each bound) + * @return A change list with every min/max bound change made during the insert procedure and a score estimating insertion quality (lower is better) + */ // FIXME: not a fan of using int instead of the Bound enum here, but I can't forward-declare it since it's part of SAEVRouteChange + SAEVRouteChangelist doBoundPropagation(std::queue<std::pair<int, SAEVKeyPoint *>> &boundPropagationQueue, + SAEVRouteChangelist &changelist); /** * Returns a score value equivalent to the detour implied by insertion of a request after the two given key point indexes. * The specific case of origin/destination being inserted one after another is taken into account