MovePlatformToLandmark - added some error checking

source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp

@@ -108,36 +108,6 @@ namespace armarx
     void StateCalculatePath::onEnter()
     {
         ARMARX_LOG << eVERBOSE << "Entering StateCalculatePath";
-//        std::string targetLandmark = getInput<std::string>("targetLandmark");
-//        SingleTypeVariantListPtr landmarkTags = getInput<SingleTypeVariantList>("landmarkTags");
-
-//        int targetIndex = -1;
-//        for (int i = 0; i < landmarkTags->getSize(); i++) {
-//            if (landmarkTags->getVariant(i)->getString() == targetLandmark) {
-//                targetIndex = i;
-//                break;
-//            }
-//        }
-
-//        SingleTypeVariantList targetPositions(VariantType::Vector3);
-
-//        if (targetIndex >= 0) {
-//            Vector3Ptr v = getInput<SingleTypeVariantList>("landmarkPositions")->getVariant(targetIndex)->get<Vector3>();
-//            Vector3 vv;
-//            vv.x = v->x;
-//            vv.y = v->y;
-//            vv.z = v->z;
-//            ARMARX_LOG << eVERBOSE << vv.x << " " << vv.y << " " << vv.z << flush;
-//            targetPositions.addVariant(Variant(vv));
-////            targetPositions.addVariant(v);
-
-//            sendEvent<EvPathFound>();
-//        } else {
-//            ARMARX_LOG << eWARN << "Could not map from landmark tag to target position(s)";
-//            sendEvent<EvNoPathFound>();
-//        }
-
-//        setOutput("targetPositions", targetPositions);
 
         //read the nodes into a tag to node map
         SingleTypeVariantListPtr landmarkNodes = getInput<SingleTypeVariantList>("landmarkNodes");
@@ -192,23 +162,28 @@ namespace armarx
 
         std::string landmark = getInput<std::string>("targetLandmark");
         NodePtr goal = nameToNodeMap[landmark];
+        if (!goal) {
+            ARMARX_LOG << eWARN << "Target landmark doesn't exist in graph" << flush;
+            sendEvent<EvNoPathFound>();
+            return;
+        }
         std::list<NodePtr> path = aStar(closest, goal);
-        SingleTypeVariantList targetPositions(VariantType::Vector3);
         if (path.empty()) {
-            ARMARX_LOG << eWARN << "No path found";
+            ARMARX_LOG << eWARN << "No path found" << flush;
             sendEvent<EvNoPathFound>();
-        } else {
-            const float goalAlpha = path.back()->framedPos->z;
-            for (std::list<NodePtr>::iterator it = path.begin(); it != path.end(); it++) {
-                Vector3 v;
-                v.x = (*it)->framedPos->x;
-                v.y = (*it)->framedPos->y;
-                v.z = goalAlpha;
-                targetPositions.addVariant(v);
-            }
-            sendEvent<EvPathFound>();
-            setOutput("targetPositions", targetPositions);
+            return;
+        }
+        SingleTypeVariantList targetPositions(VariantType::Vector3);
+        const float goalAlpha = path.back()->framedPos->z;
+        for (std::list<NodePtr>::iterator it = path.begin(); it != path.end(); it++) {
+            Vector3 v;
+            v.x = (*it)->framedPos->x;
+            v.y = (*it)->framedPos->y;
+            v.z = goalAlpha;
+            targetPositions.addVariant(v);
         }
+        setOutput("targetPositions", targetPositions);
+        sendEvent<EvPathFound>();
         ARMARX_LOG << eVERBOSE << "StateCalculatePath: Done onEnter()" << flush;
     }