MovePlatformToLandmarkExampleGraph.xml - fixed an error with the edge connections

MovePlatformToLandmark.cpp - added more logging

scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml

@@ -71,7 +71,7 @@
     <!-- each edge is defined as "<nodename>;<nodename>" (";" is a delimiter between names, 
          so it shouldn't appear in any node name), all edges are bidirectional between the provided nodes -->
     <landmarkEdges>
-		<Item0>start;1</Item0>
+		<Item0>Start;1</Item0>
 		<Item1>1;2</Item1>
 		<Item2>2;Handover</Item2>
 		<Item3>2;3</Item3>

source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp

@@ -107,7 +107,7 @@ namespace armarx
 
     void StateCalculatePath::onEnter()
     {
-        ARMARX_LOG << eVERBOSE << "Entering StateCalculatePath";
+        ARMARX_LOG << eVERBOSE << "Entering StateCalculatePath" << flush;
 
         //read the nodes into a tag to node map
         SingleTypeVariantListPtr landmarkNodes = getInput<SingleTypeVariantList>("landmarkNodes");
@@ -120,6 +120,7 @@ namespace armarx
             nameToNodeMap[fv->frame] = n;
         }
 
+        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Retrieving edges" << flush;
         //process the edges by adding the nodes to each other's successors
         SingleTypeVariantListPtr landmarkEdges = getInput<SingleTypeVariantList>("landmarkEdges");
         for (int i = 0; i < landmarkEdges->getSize(); i++) {
@@ -132,6 +133,7 @@ namespace armarx
             right->successors.push_back(left);
         }
 
+        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Getting current pose from PlatformContext" << flush;
         PlatformContext* context = getContext<PlatformContext>();
         ChannelRefPtr poseRef = context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
         const float platformPositionX = context->getDatafieldRef(poseRef, "positionX")->getDataField()->getFloat();
@@ -147,6 +149,7 @@ namespace armarx
             sendEvent<EvNoPathFound>();
             return;
         }
+        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Finding closest point in graph" << flush;
         NodePtr closest = it->second;
         float minDist = getDist(platformPositionX, platformPositionY, closest->framedPos->x, closest->framedPos->y);
         it++;
@@ -158,7 +161,7 @@ namespace armarx
             }
         }
 
-        ARMARX_LOG << eVERBOSE << "Starting point: " << closest->name << flush;
+        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Starting point: " << closest->name << flush;
 
         std::string landmark = getInput<std::string>("targetLandmark");
         NodePtr goal = nameToNodeMap[landmark];
@@ -167,6 +170,7 @@ namespace armarx
             sendEvent<EvNoPathFound>();
             return;
         }
+        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Looking for path" << flush;
         std::list<NodePtr> path = aStar(closest, goal);
         if (path.empty()) {
             ARMARX_LOG << eWARN << "No path found" << flush;
@@ -207,10 +211,6 @@ namespace armarx
         cameFrom[goal] = start; //in case start==goal
 
         while (!openSet.empty()) {
-            for (size_t i = 0; i < openSet.size(); i++) {
-                std::cout << openSet[i] << ";";
-            }
-            std::cout << std::endl;
             float lowestScore = fScore[openSet[0]];
             std::vector<NodePtr>::iterator currentIT = openSet.begin();
             for (std::vector<NodePtr>::iterator it = openSet.begin() + 1; it != openSet.end(); it++) {