Adjusted duplicate waypoint adjustment to avoid loops

afrl-rq · Aug 29, 2018 · cb561cf · cb561cf
1 parent 487c6eb
commit cb561cf
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/src/Tasks/TaskServiceBase.cpp b/src/Tasks/TaskServiceBase.cpp
@@ -55,7 +55,7 @@ const int64_t TaskOptionClass::m_firstImplementationRouteId{2}; // first id to u
 //XML STRINGS    
 const std::string TaskServiceBase::m_taskOptions_XmlTag{"TaskOptions"};
 
-bool isCollocated(afrl::cmasi::Location3D* a, afrl::cmasi::Location3D* b)
+bool isColocated(afrl::cmasi::Location3D* a, afrl::cmasi::Location3D* b)
 {
     if(!a || !b) return false;
 
@@ -943,14 +943,19 @@ void TaskServiceBase::processImplementationRoutePlanResponseBase(const std::shar
                                                 itTaskOptionClass->second->m_firstTaskActiveWaypointID = waypointId;
                                             }
                                         }
+
+                                        // if a waypoint is co-located with the previous waypoint, replace the previous waypoint
                                         if(!taskImplementationResponse->getTaskWaypoints().empty() &&
-                                            isCollocated(taskImplementationResponse->getTaskWaypoints().back(), waypoint))
+                                            isColocated(taskImplementationResponse->getTaskWaypoints().back(), waypoint))
                                         {
                                             waypointId = taskImplementationResponse->getTaskWaypoints().back()->getNumber();
+                                            int64_t nextId = taskImplementationResponse->getTaskWaypoints().back()->getNextWaypoint();
                                             waypoint->setNumber(waypointId);
+                                            waypoint->setNextWaypoint(nextId);
                                             delete taskImplementationResponse->getTaskWaypoints().back();
-                                            taskImplementationResponse->getTaskWaypoints().pop_back();;
+                                            taskImplementationResponse->getTaskWaypoints().pop_back();
                                         }
+
                                         taskImplementationResponse->getTaskWaypoints().push_back(waypoint);
                                         waypointId++;
                                         waypoint = nullptr; // gave up ownership