本文整理汇总了C++中AStarNode::getPose方法的典型用法代码示例。如果您正苦于以下问题：C++ AStarNode::getPose方法的具体用法？C++ AStarNode::getPose怎么用？C++ AStarNode::getPose使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类AStarNode的用法示例。
在下文中一共展示了AStarNode::getPose方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: while
QStack<AbstractAction *> * AStarAlgorithm::computePathToTheGoal(AStarNode *goalFound)
{
//I need the sequence of poses i will follow to correctly build the actions.
QStack<AStarNode *> path;
path.push(goalFound); //add the goal
AStarNode* nodeToProcess = goalFound->getParent();
while(nodeToProcess != startNode){
path.push(nodeToProcess);
nodeToProcess = nodeToProcess->getParent();
}
//Popping the node from the path's stack, i can compute the actions I need.
//Now i need to compute:
// - a rotation
// - the theta of the new pose
// - a transaltion
//then, i have to reverse the stack in order to pop the things in the correct order.
nodeToProcess = path.pop();
AStarNode *actualNode = startNode;
QStack<Data::Action *> tempActions;
do{
const PathNode *startPose = actualNode->getPose();
const PathNode *toReachPose = nodeToProcess->getPose();
double angle2 = computeRotationFromPoses(*startPose, *toReachPose);
Data::Action *rotAction = NULL;
if(angle2 != 0.0){
rotAction = new Data::Action();
rotAction->setType(Data::Action::Rotation);
rotAction->setValue(fromRadiantToDegree(angle2));
tempActions.push(rotAction);
}
//Set the theta value of the pose.
PathNode *newPathNodePose = new PathNode(nodeToProcess->getPose()->getTimestamp(),
Data::Pose(nodeToProcess->getPose()->getX(),
nodeToProcess->getPose()->getY(),
startPose->getTheta()+angle2));
const PathNode *prevPathNode = nodeToProcess->getPose();
nodeToProcess->setPose(newPathNodePose);
nodeToProcess->setOwnPose(true);
if(nodeToProcess->getOwnPose()){
delete prevPathNode;
}
toReachPose = newPathNodePose;
//compute the translation
double dx2 = pow(toReachPose->getX()-startPose->getX(), 2);
double dy2 = pow(toReachPose->getY()-startPose->getY(), 2);
double trasl = sqrt(dy2+dx2);
Data::Action *traAction = NULL;
if(fabs(trasl) > 0.1){
traAction = new Data::Action();
traAction->setType(Data::Action::Translation);
traAction->setValue(trasl);
tempActions.push(traAction);
}
//printPlan(actualNode, nodeToProcess, rotAction, traAction);
//nodes updates
actualNode = nodeToProcess;
if(!path.isEmpty())
nodeToProcess = path.pop();
} while(!path.isEmpty());
//I add the last rotation to let the robot to face the frontier.
double lastAngle = computeRotationFromPoses( *(nodeToProcess->getPose()) , *goalPose);
if(lastAngle != 0.0){
Data::Action *lastAction = new Data::Action();
lastAction->setType(Data::Action::Rotation);
lastAction->setValue(fromRadiantToDegree(lastAngle));
tempActions.push(lastAction);
}
//At last, i reverse the tempActions stack into the stack i must return.
QStack<AbstractAction *> *toRet = new QStack<AbstractAction *>();
while(!tempActions.isEmpty()){
toRet->push(tempActions.pop());
}
return toRet;
}