bool GeoBrowse::ok() { /* cout << "Checking cursor, in state " << (int) _state << ", first call " << _firstCall << ", empty : " << _cur.isEmpty() << ", stack : " << _stack.size() << endl; */ bool first = _firstCall; if (_firstCall) { fillStack(maxPointsHeuristic); _firstCall = false; } if (_stack.size()) { if (first) { ++_nscanned; } return true; } while (moreToDo()) { fillStack(maxPointsHeuristic); if (! _cur.isEmpty()) { if (first) { ++_nscanned; } return true; } } return !_cur.isEmpty(); }
Waypointer::Waypointer(Waypoint waypoint_array[], int num_waypoints, std::string action_server) : base_client(action_server.c_str(), true) { waypoints = waypoint_array; waypoint_count = num_waypoints; fillStack(); while (!base_client.waitForServer(ros::Duration(5.0))) ROS_INFO("Waiting for action server..."); }
bool GeoBrowse::advance() { _cur._o = BSONObj(); if (_stack.size()) { _cur = _stack.front(); _stack.pop_front(); ++_nscanned; return true; } if (! moreToDo()) return false; while (_cur.isEmpty() && moreToDo()){ fillStack(maxPointsHeuristic); } return ! _cur.isEmpty() && ++_nscanned; }
void Waypointer::begin() { while(ros::ok()) { // If the stack is empty, kill the node since we can't continue. if (waypoint_stack.empty()) { ROS_ERROR("Waypoint stack is empty! Killing node..."); ros::shutdown(); } // Get the next waypoint from the top of the stack. Waypoint waypoint = waypoint_stack.top(); // Pop off the waypoint now that we are prepared to navigate to it. waypoint_stack.pop(); // Build the goal message. ROS_INFO("Building goal message..."); move_base_msgs::MoveBaseGoal goal = buildGoal(waypoint); // Publish the pose message. ROS_INFO("Sending goal message..."); base_client.sendGoal(goal); // Wait until we have a status for the current navigation attempt. ROS_INFO("Waiting for navigation result..."); base_client.waitForResult(); // Check the result of the navigation and send diagnostics to terminal. if (base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("Movement successful!"); else ROS_INFO("Movement failed!"); // If the stack is empty, refill it and continue. // Allows the turtlebot to loop through waypoints. if (waypoint_stack.empty()) { ROS_INFO("Waypoints exhausted, refilling stack..."); fillStack(); } } }
void GeoSearch::exec() { if(_numWanted == 0) return; /* * Search algorithm * 1) use geohash prefix to find X items * 2) compute max distance from want to an item * 3) find optimal set of boxes that complete circle * 4) use regular btree cursors to scan those boxes */ // Part 1 { do { long long f = found(); verify(f <= 0x7fffffff); fillStack(maxPointsHeuristic, _numWanted - static_cast<int>(f), true); processExtraPoints(); } while(_state != DONE && _state != DONE_NEIGHBOR && found() < _numWanted && (!_prefix.constrains() || _params.geoHashConverter->sizeEdge(_prefix) <= _scanDistance)); // If we couldn't scan or scanned everything, we're done if(_state == DONE){ expandEndPoints(); return; } } // Part 2 { // Find farthest distance for completion scan double farDist = farthest(); if(found() < _numWanted) { // Not enough found in Phase 1 farDist = _scanDistance; } else if (_type == GEO_PLANE) { // Enough found, but need to search neighbor boxes farDist += _params.geoHashConverter->getError(); } else if (_type == GEO_SPHERE) { // Enough found, but need to search neighbor boxes farDist = std::min(_scanDistance, computeXScanDistance(_near.y, rad2deg(farDist)) + 2 * _params.geoHashConverter->getError()); } verify(farDist >= 0); // Find the box that includes all the points we need to return _want = Box(_near.x - farDist, _near.y - farDist, farDist * 2); // Remember the far distance for further scans _scanDistance = farDist; // Reset the search, our distances have probably changed if(_state == DONE_NEIGHBOR){ _state = DOING_EXPAND; _neighbor = -1; } // Do regular search in the full region do { fillStack(maxPointsHeuristic); processExtraPoints(); } while(_state != DONE); } expandEndPoints(); }
// Fills the stack, but only checks a maximum number of maxToCheck points at a time. // Further calls to this function will continue the expand/check neighbors algorithm. void GeoBrowse::fillStack(int maxToCheck, int maxToAdd, bool onlyExpand) { if(maxToAdd < 0) maxToAdd = maxToCheck; int maxFound = _foundInExp + maxToCheck; verify(maxToCheck > 0); verify(maxFound > 0); verify(_found <= 0x7fffffff); // conversion to int int maxAdded = static_cast<int>(_found) + maxToAdd; verify(maxAdded >= 0); // overflow check bool isNeighbor = _centerPrefix.constrains(); // Starting a box expansion if (_state == START) { // Get the very first hash point, if required if(! isNeighbor) _prefix = expandStartHash(); if (!BtreeLocation::initial(_descriptor, _params, _min, _max, _prefix)) { _state = isNeighbor ? DONE_NEIGHBOR : DONE; } else { _state = DOING_EXPAND; _lastPrefix.reset(); } } // Doing the actual box expansion if (_state == DOING_EXPAND) { while (true) { // Record the prefix we're actively exploring... _expPrefix.reset(new GeoHash(_prefix)); // Find points inside this prefix while (checkAndAdvance(&_min, _prefix, _foundInExp) && _foundInExp < maxFound && _found < maxAdded) {} while (checkAndAdvance(&_max, _prefix, _foundInExp) && _foundInExp < maxFound && _found < maxAdded) {} if(_foundInExp >= maxFound || _found >= maxAdded) return; // We've searched this prefix fully, remember _lastPrefix.reset(new GeoHash(_prefix)); // If we've searched the entire space, we're finished. if (! _prefix.constrains()) { _state = DONE; notePrefix(); return; } // If we won't fit in the box, and we're not doing a sub-scan, increase the size if (! fitsInBox(_converter->sizeEdge(_prefix)) && _fringe.size() == 0) { // If we're still not expanded bigger than the box size, expand again _prefix = _prefix.up(); continue; } // We're done and our size is large enough _state = DONE_NEIGHBOR; // Go to the next sub-box, if applicable if(_fringe.size() > 0) _fringe.pop_back(); // Go to the next neighbor if this was the last sub-search if(_fringe.size() == 0) _neighbor++; break; } notePrefix(); } // If we doeighbors if(onlyExpand) return; // If we're done expanding the current box... if(_state == DONE_NEIGHBOR) { // Iterate to the next neighbor // Loop is useful for cases where we want to skip over boxes entirely, // otherwise recursion increments the neighbors. for (; _neighbor < 9; _neighbor++) { // If we have no fringe for the neighbor, make sure we have the default fringe if(_fringe.size() == 0) _fringe.push_back(""); if(! isNeighbor) { _centerPrefix = _prefix; _centerBox = makeBox(_centerPrefix); isNeighbor = true; } int i = (_neighbor / 3) - 1; int j = (_neighbor % 3) - 1; if ((i == 0 && j == 0) || (i < 0 && _centerPrefix.atMinX()) || (i > 0 && _centerPrefix.atMaxX()) || (j < 0 && _centerPrefix.atMinY()) || (j > 0 && _centerPrefix.atMaxY())) { continue; // main box or wrapped edge // TODO: We may want to enable wrapping in future, probably best as layer // on top of this search. } // Make sure we've got a reasonable center verify(_centerPrefix.constrains()); GeoHash _neighborPrefix = _centerPrefix; _neighborPrefix.move(i, j); while(_fringe.size() > 0) { _prefix = _neighborPrefix + _fringe.back(); Box cur(makeBox(_prefix)); double intAmt = intersectsBox(cur); // No intersection if(intAmt <= 0) { _fringe.pop_back(); continue; } else if(intAmt < 0.5 && _prefix.canRefine() && _fringe.back().size() < 4 /* two bits */) { // Small intersection, refine search string lastSuffix = _fringe.back(); _fringe.pop_back(); _fringe.push_back(lastSuffix + "00"); _fringe.push_back(lastSuffix + "01"); _fringe.push_back(lastSuffix + "11"); _fringe.push_back(lastSuffix + "10"); continue; } // Restart our search from a diff box. _state = START; verify(! onlyExpand); verify(_found <= 0x7fffffff); fillStack(maxFound - _foundInExp, maxAdded - static_cast<int>(_found)); // When we return from the recursive fillStack call, we'll either have // checked enough points or be entirely done. Max recurse depth is < 8 * // 16. // If we're maxed out on points, return if(_foundInExp >= maxFound || _found >= maxAdded) { // Make sure we'll come back to add more points verify(_state == DOING_EXPAND); return; } // Otherwise we must be finished to return verify(_state == DONE); return; } } // Finished with neighbors _state = DONE; } }
int main(int argc, char** argv) { FILE* fileIn = NULL; FILE* fileOut = NULL; Symbol symbols[ARRAY_LENGTH]; Symbol* symbolsOrd[ARRAY_LENGTH]; //ordered pointers (by frequency) u64 fileLen = 0; BinTree* temp[2]; BinTree* root; Stack stack; if ( !argv[1] ) { printHelp(); return EXIT_FAIL; } if ( !strcmp(argv[1], "-e") || !strcmp("--encode", argv[1]) ) { if ( !argv[2] || !argv[3] ) { printHelp(); return EXIT_FAIL; } fileIn = (FILE*)fopen(argv[2], "rt"); if ( !fileIn ) { printf("Error: file <<%s>> doesn't exists\n", argv[2]); return EXIT_FAIL; } stackInit(&stack); initializeArrays(symbols, symbolsOrd, ARRAY_LENGTH); fileLen = readFile(fileIn, symbols); //read symbols; first pass if ( fileLen == 0 ) { printf("Error: file <<%s>> is empty\n", argv[2]); fclose(fileIn); return EXIT_FAIL; } fileOut = (FILE*)fopen(argv[3], "wb"); if ( !fileOut ) { printf("Error: cannot create file <<%s>>\n", argv[3]); fclose(fileIn); return EXIT_FAIL; } //sort symbols by frequencies from lower to greatest qsort(symbolsOrd, ARRAY_LENGTH, sizeof(Symbol*), compareSym); fillStack(symbolsOrd, &stack, ARRAY_LENGTH); //prepare stack root = buildTree(&stack); createPath(root, 0); //0 - starting depth rewind(fileIn); //reopen fileIn; prepare second pass //write all compressed data saveToFile(fileIn, fileOut, symbols, root, argv[2], fileLen); //memory cleanup binTreeRemove(root); fclose(fileOut); fclose(fileIn); } else if ( !strcmp(argv[1], "-d") || !strcmp("--decode", argv[1]) ) { if ( !argv[2] ) { printHelp(); return EXIT_FAIL; } fileIn = fopen(argv[2], "rb"); if ( !fileIn ) { printf("Error: file <<%s>> doesn't exists\n", argv[2]); return EXIT_FAIL; } if ( !loadFromFile(fileIn, argv[3] ? argv[3] : NULL) ) { printf("Error: file <<%s>> corrupted or cannot create file <<%s>>\n", argv[2], argv[3]); fclose(fileIn); return EXIT_FAIL; } fclose(fileIn); } else { printHelp(); } return EXIT_SUCCESS; }