From c169e67efe409dac1f9bedcbe081d8a6746f498b Mon Sep 17 00:00:00 2001
From: Valerij Wittenbeck <valerij.wittenbeck@student.kit.edu>
Date: Wed, 21 May 2014 10:29:34 +0200
Subject: [PATCH] MovePlatformToLandmarkExampleGraph.xml - fixed an error with
the edge connections MovePlatformToLandmark.cpp - added more logging
---
.../configs/MovePlatformToLandmarkExampleGraph.xml | 2 +-
.../MovePlatformToLandmark.cpp | 12 ++++++------
2 files changed, 7 insertions(+), 7 deletions(-)
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml
index 068d84b24..9abf93e7e 100644
--- a/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml
+++ b/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>
diff --git a/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
index ddcd9e2a7..00935a56e 100644
--- a/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
+++ b/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++) {
--
GitLab