本文整理汇总了C++中Pose::isEqual方法的典型用法代码示例。如果您正苦于以下问题：C++ Pose::isEqual方法的具体用法？C++ Pose::isEqual怎么用？C++ Pose::isEqual使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Pose的用法示例。
在下文中一共展示了Pose::isEqual方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
//.........这里部分代码省略.........
// Old code
// TODO: leave it, I don't know what it does
// if ( act )
// {
// //NOTE; calculate path and turnings between actual position and goal
// /*string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
// travelledDistance = travelledDistance + astar.lengthPath ( path );
// cout << astar.lengthPath ( path ) << endl;
// numOfTurning = numOfTurning + astar.getNumberOfTurning ( path );
// numConfiguration++;
// */
// //totalAngle += scanAngle;
// }
// act = false;
// If the exploration just started
if ( count == 0 )
{
// Calculate other three pose given the strating one
string invertedPose = function.getEncodedKey ( invertedInitial,0 );
string eastPose = function.getEncodedKey ( eastInitial,0 );
string westPose = function.getEncodedKey ( westInitial,0 );
list<Pose> empty ;
std::pair<string,list<Pose>> pair1 = make_pair ( invertedPose,empty );
std::pair<string,list<Pose>> pair2 = make_pair ( eastPose,empty );
std::pair<string,list<Pose>> pair3 = make_pair ( westPose,empty );
// And add them (with empty candidates) to the graph structure
graph2.push_back ( pair1 );
graph2.push_back ( pair2 );
graph2.push_back ( pair3 );
}
// If it's not the first step but we are in one of the initial position (we come back here with backtracking)
if ( count != 0 && ( target.isEqual ( invertedInitial ) || target.isEqual ( eastInitial ) || target.isEqual ( westInitial ) ) )
{
// If there are no more destination in the graph, terminates the navigation
if ( graph2.size() == 0 ) break;
graph2.pop_back();
actualPose = function.getEncodedKey ( target,0 );
// Add to the graph the initial positions and the candidates from there (calculated inside the function)
pushInitialPositions ( map, x, y,orientation, range,FOV, threshold, actualPose, &graph2 );
}
// If there are no new candidate positions from the current pose of the robot
if ( candidatePosition.size() == 0 )
{
// Find candidates
ray.findCandidatePositions2 ( map,x,y,orientation,FOV,range );
candidatePosition = ray.getCandidatePositions();
ray.emptyCandidatePositions();
cout << "No other candidate position" << endl;
cout << "----- BACKTRACKING -----" << endl;
// If the graph contains cells that can be explored
if ( graph2.size() > 1 )
{
// Get the last position in the graph and then remove it
string targetString = graph2.at ( graph2.size()-1 ).first;
graph2.pop_back();
// EvaluationRecords record;
target = record.getPoseFromEncoding ( targetString );
// Add it to the history as cell visited more than once
history.push_back ( function.getEncodedKey ( target,2 ) );
cout << "[BT]No significative position reachable. Come back to previous position" << endl;
count = count + 1;