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;
}
