// // Abort finding a location for a token // void Placement::AbortFind(Base::Token &token) { LOG_AI(("Placement of Token [%08X] is being aborted", U32(&token))) Locator *locator = locatorsActive.Find(U32(&token)); if (locator) { locatorsActive.Unlink(locator); locatorsActiveList.Unlink(locator); locatorsIdle.Add(U32(&token), locator); } else { if (!locatorsIdle.Exists(U32(&token))) { ERR_FATAL(("Token [%08X] is not idle or active", U32(&token))) } } }
// // Reset // void Water::Decomposition::Reset() { ASSERT(initialized) LOG_AI(("Starting Water Decomposition")) U32 numRegions = ConnectedRegion::GetNumRegions(traction); for (ConnectedRegion::Pixel pixel = 1; pixel <= numRegions; pixel++) { const ConnectedRegion::Region ®ion = ConnectedRegion::GetRegion(traction, pixel); // Only consider water regions with more than 200 cells of water if (region.area > 200) { bodies.Append(new Body(pixel, region)); } } // Given this region, calculate the map expanse and the map LOG_AI(("Finished Water Decomposition")) }
void SimpleNavigation::navigationStep() { mrpt::poses::CPose2D curPose; double yaw; double wantedYaw; float cur_w; float cur_v; interface->getCurrentPoseAndSpeeds(curPose, cur_v, cur_w); yaw = curPose.phi();//Robotpose returns yaw in radians LOG_AI(DEBUG4) << "Simple Nav Yaw: " << yaw << endl; wantedYaw = calcBearing(curPose);//AbstractNavigationInterface::calcBearing(curPose.x(), curPose.y(), navParams->target.x, navParams->target.y); LOG_AI(DEBUG4) << "Simple Nav Wanted Yaw: " << wantedYaw << endl; double dist = distance(curPose); double diffrence = wantedYaw - yaw; double minDiff = diffrence; if(diffrence > M_PI) { minDiff = 2*M_PI - diffrence; } else if(diffrence < -M_PI) { minDiff = 2*M_PI + diffrence; } LOG_AI(DEBUG4) << "Simple Nav Angular Difference: " << minDiff << endl; if(!dist > navParams->targetAllowedDistance) { LOG_AI(INFO) << "Reached Way Point" << endl; points.erase(points.begin()); if(points.size() != 0)//this->points) { gotoNextPoint(); } else { state = mrpt::reactivenav::CAbstractReactiveNavigationSystem::IDLE; } } if(abs((minDiff)) > .2) { bool turnRight = minDiff > 0; if(turnRight) { LOG_AI(DEBUG) << "\tTurning Right" << endl; interface->changeSpeeds(.5f,2); } else { LOG_AI(DEBUG) << "\tTurning Left" << endl; interface->changeSpeeds(.5f, -2); } } else { double dist = distance(curPose); LOG_AI(DEBUG) << "\tDriving Towards Target. Remaining Distance: " << dist << endl;//AbstractNavigationInterface::haversineDistance(curPose.x(), curPose.y(), navParams->target.x, navParams->target.y); if(dist > navParams->targetAllowedDistance) { interface->changeSpeeds(1.4,0); LOG_AI(DEBUG) << "\tDriving Towards Target. Remaining Distance: " << dist << endl; } else { LOG_AI(INFO) << "Reached Way Point" << endl; points.erase(points.begin()); if(points.size() != 0)//this->points) { gotoNextPoint(); } else { state = mrpt::reactivenav::CAbstractReactiveNavigationSystem::IDLE; } } } }