//
  // 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 &region = 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"))
  }
Exemplo n.º 3
0
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;
			}
		}
	}
}