float GetRumboDst()
{
float b;
b=calcBearing(gpsinfo.lon, gpsinfo.lat, GetLonDst(), GetLatDst());
return b;
}
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;
}
}
}
}
