PoslvControl::PoslvControl(bool showPath, bool showAxes, bool showVelocity, bool showAcceleration) : _ui(new Ui_PoslvControl()), _path(0), _linearVelocity(Eigen::Vector3d::Zero()), _angularVelocity(Eigen::Vector3d::Zero()), _acceleration(Eigen::Vector3d::Zero()), _T_w_i(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) * Eigen::Translation3d(0, 0, 0)) { _ui->setupUi(this); _ui->colorChooser->setPalette(&_palette); connect<BagControl>(SIGNAL(messageRead(const rosbag::MessageInstance&)), SLOT(messageRead(const rosbag::MessageInstance&))); connect<RosControl>( SIGNAL(messageRead(const poslv::VehicleNavigationSolutionMsgConstPtr&)), SLOT(messageRead(const poslv::VehicleNavigationSolutionMsgConstPtr&))); connect<View>(SIGNAL(render(View&)), SLOT(renderView(View&))); setPathColor(Qt::red); setShowPath(showPath); setAxesColor(Qt::green); setShowAxes(showAxes); setShowVelocity(showVelocity); setVelocityColor(Qt::cyan); setShowAcceleration(showAcceleration); setAccelerationColor(Qt::cyan); }
bool PathFindingApp::doPathfinding(int startX, int startY, int endX, int endY) { // Set color for start and end pos to path color setPathColor(m_texturePathFound, startX, startY); setPathColor(m_texturePathFound, endX, endY); bool done = true; // Some variables for path finding OpenList openList; ClosedList closedList; SearchLevel searchLevel(m_textureStartCase); SearchNode *result = 0; // Start A* search: // Add start node to open list. Position startPosition = Position(startX, startY); Position endPosition = Position(endX, endY); SearchNode *newNode = new SearchNode(startPosition, searchLevel.getH(startPosition, endPosition), 0, 0); openList.insertToOpenList(newNode); // 1. Get the square on the open list which has the lowest score. Let's call this square S (or prey) while (!openList.isEmpty()) { SearchNode *prev = openList.removeSmallestFFromOpenList(); if (prev->pos == endPosition) { // Goal found! result = prev; break; } else { // 2. Remove S from the open list and add S to the closed list. closedList.addToClosedList(prev); // 3. for each square T in S's walkable adjacent tiles: std::vector<Position> adjacentNodes = searchLevel.getAdjacentNodes(prev->pos.first, prev->pos.second); for (size_t i = 0; i < adjacentNodes.size(); ++i) { // If T is in the closed list : Ignore it. if (closedList.isInClosedList(adjacentNodes[i])) { continue; } SearchNode *n = openList.findFromOpenList(adjacentNodes[i]); if (n == 0) { // If T is not in the open list : Add it and compute its score SearchNode * newNode = new SearchNode(adjacentNodes[i], searchLevel.getH(adjacentNodes[i], endPosition), searchLevel.getG(prev, adjacentNodes[i]), prev); openList.insertToOpenList(newNode); } else { // If T is already in the open list : Check if the F score is lower // when we use the current generated path to get there. If it is update // it's score and update its parent as well. SearchNode *newNode = new SearchNode(adjacentNodes[i], searchLevel.getH(adjacentNodes[i], endPosition), searchLevel.getG(prev, adjacentNodes[i]), prev); if (n->distance() < newNode->distance()) { n->resetPrev(newNode->prevNode, searchLevel.getG(newNode->prevNode, n->pos)); } } } } } if (result == 0) { printf("Path not found!!!\n"); return true; } while (result != 0) { setPathColor(m_texturePathFound, result->pos.first, result->pos.second); result = result->prevNode; } return true; // TODO: Remove that search end and delay hack as seen below.. //static int i = 0; //i = ((i+1)%10); // 10*100ms = ~500 ms of total //Sleep(100); //return i==0; }