Make sure state is sent before the goal is defined!

naderman · Jul 15, 2010 · 537a6e7 · 537a6e7
1 parent fa9cb32
commit 537a6e7
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 5 deletions.
diff --git a/src/spoac/stm/PKSService.cpp b/src/spoac/stm/PKSService.cpp
@@ -47,6 +47,7 @@ PKSService::PKSService(STMPtr stm, ice::IceHelperPtr iceHelper):
             "PlanController");
     }
     stmSize = -1;
+    sentGoal = false;
 }
 
 void PKSService::setScenario(const LTMSlice::Scenario& scenario)
@@ -90,16 +91,30 @@ void PKSService::sendScenario()
 
     planner->setActionDefinitions(actions);
 
+    sentGoal = false;
+}
+
+void PKSService::sendGoal()
+{
     if (!currentGoal.goalExpression.empty())
     {
         planner->setGoal(currentGoal);
     }
+    sentGoal = true;
 }
 
-void PKSService::checkScenario()
+void PKSService::updateState(const StateUpdate& state)
 {
     if (stm->extractPlanConstants().size() != stmSize)
     {
         sendScenario();
     }
+
+    planner->updateState(state);
+
+    if (!sentGoal)
+    {
+        sendGoal();
+    }
 }
+
diff --git a/src/spoac/stm/PKSService.h b/src/spoac/stm/PKSService.h
@@ -59,14 +59,16 @@ namespace spoac
         void setGoal(const PlanningSlice::Goal& goal);
 
         void sendScenario();
+        void sendGoal();
 
-        void checkScenario();
+        void updateState(const PlanningSlice::StateUpdate& state);
 
     protected:
         STMPtr stm;
         ice::IceHelperPtr iceHelper;
 
         size_t stmSize;
+        bool sentGoal;
 
         PlanningSlice::PlanControllerTopicPrx planner;
         LTMSlice::Scenario currentScenario;

diff --git a/src/spoac/stm/PlanNetworkPerceptionHandler.cpp b/src/spoac/stm/PlanNetworkPerceptionHandler.cpp
@@ -67,9 +67,7 @@ void PlanNetworkPerceptionHandler::update(spoac::STMPtr stm)
 {
     if (wait <= 0)
     {
-        pksService->checkScenario();
-
-        planner->updateState(getState(stm));
+        pksService->updateState(getState(stm));
     }
     else
     {