From 137d78b5992d393e7a6dca936e1a0bdfdf4ae154 Mon Sep 17 00:00:00 2001
From: Romain BERNARD <romain.bernard@uca.fr>
Date: Tue, 17 Sep 2024 16:57:46 +0200
Subject: [PATCH] separate bound propagation method from insertion method to
allow propagating changes from a given bound queue
---
src/routes/vehicle/SAEVRoute.cpp | 16 ++++++++++++----
src/routes/vehicle/SAEVRoute.h | 12 ++++++++++++
2 files changed, 24 insertions(+), 4 deletions(-)
diff --git a/src/routes/vehicle/SAEVRoute.cpp b/src/routes/vehicle/SAEVRoute.cpp
index ccdd0b1..b3053fc 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 f9ce97a..18016d8 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
--
GitLab