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