C++ ArTime::mSecSince方法代码示例

void userTask(ArRobot *robot)
{
  int timeTaken = (requestTime.mSecSince() - 
		   robot->getIOPacketTime().mSecSince());

  if (timeTaken > 500)
  {
    ArLog::log(ArLog::Terse, "\nits been: %ld giving up and trying again (missed %d got %d)", timeTaken, numPacketsMissed, numPacketsGotten);
    numPacketsMissed++;
    robot->comInt(ArCommands::IOREQUEST, 1);
    requestTime.setToNow();
  }
  
  if (robot->getIOPacketTime().mSecSince() > requestTime.mSecSince())
    return;
  else
  {
    if (timeTaken > 30)
    {
      ArLog::log(ArLog::Terse, "\nlast packet time: %ld", timeTaken);
    }
    printf("\r%d packets gotten %d missed       ", numPacketsGotten, 
	   numPacketsMissed);
    numPacketsGotten++;
    fflush(stdout);
    robot->comInt(ArCommands::IOREQUEST, 1);
    requestTime.setToNow();
  }
}

void userTask(ArRobot *robot)
{
  if (robot->getIOPacketTime().mSecSince() > requestTime.mSecSince())
    return;
  else
  {
    ArLog::log(ArLog::Terse, "last packet time: %ld", requestTime.mSecSince() - robot->getIOPacketTime().mSecSince());
    fflush(stdout);
    robot->comInt(ArCommands::IOREQUEST, 1);
    requestTime.setToNow();
  }

  /*  Uncomment the next section to test response in velocity commands. */

  /*
  if (abs((int)(robot->getVel() - vel)) <= ABOUT_RIGHT &&
      velTime.secSince() > 5)
  {
    if (vel == 0)
      vel = 200;
    else if (vel == 200)
      vel = 400;
    else if (vel == 400)
      vel = 0;
    robot->setVel(vel);
    velTime.setToNow();
  }

  ArLog::log(ArLog::Terse, "vel requested: %d\trobot vel: %3.2f\tvel time: %d", vel, robot->getVel(), velTime.mSecSince());
  */

  ArLog::log(ArLog::Terse, "Aria cycle time: %d", robot->getCycleTime());
  fflush(stdout);
}

ArActionDesired *Acquire::fire(ArActionDesired currentDesired)
{
  myDesired.reset();
  myDesired.setVel(0);
  switch (myState) {
  case STATE_START_LOOKING:
    myFirstTurn.clear();
    mySecondTurn.clear();
    myState = STATE_LOOKING;
    myGripper->liftUp();
    myGripper->gripClose();
    printf("Acquire: Raising lift\n");
    myStartUp.setToNow();
    myTryingGripper = true;
  case STATE_LOOKING:
    if (myTryingGripper && (myStartUp.mSecSince() < 600 ||
			    ((!myGripper->isLiftMaxed() ||
			      myGripper->getGripState() != 2) && 
			     myStartUp.mSecSince() < 5000)))
    {
      myGripper->liftUp();
      myGripper->gripClose();
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      return &myDesired;
    } 
    else if (myTryingGripper)
    {
      printf("Acquire: Done raising lift %ld after started.\n", 
	     myStartUp.mSecSince());
      myTryingGripper = false;
    }
    if (myActs->getNumBlobs(myChannel) > 0)
    {
      myDesired.setDeltaHeading(0);
      myState = STATE_SUCCEEDED;
      printf("Acquire: Succeeded!\n");
    }
    else if (myFirstTurn.didAll() && mySecondTurn.didAll())
    {
      myDesired.setDeltaHeading(0);
      myState = STATE_FAILED;
      printf("Acquire: Did two revolutions, didn't see the blob, Failed!\n");
    } 
    else
    {
      myFirstTurn.update(myRobot->getTh());
      if (myFirstTurn.didAll())
	mySecondTurn.update(myRobot->getTh());
      myDesired.setDeltaHeading(8);
    }
    return &myDesired;
  default:
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
}

void handleGetMap(ArNetPacket *packet)
{
  char buffer[512];

  if (packet->getDataReadLength() == packet->getDataLength())
  {
    printf("Empty packet signifying end of map (for central forward)\n");
    return;
  }
  
  packet->bufToStr(buffer, sizeof(buffer));
  // if we got an end of line char instead of a line it means the map is over
  if (buffer[0] == '\0')
  {
    printf("Map took %g seconds\n", start.mSecSince() / 1000.0);
    arMap.parsingComplete();
    arMap.writeFile("mapExample.map");
    //client.disconnect();
    //exit(0);
  }
  else
  {
    //printf("line '%s'\n", buffer);
    arMap.parseLine(buffer);
  }

}

ArActionDesired *JoydriveAction::fire(ArActionDesired currentDesired)
{
  int rot, trans;

  printf("%6ld ms since last loop. ms longer than desired:  %6ld.  mpac %d\n",
	 lastLoopTime.mSecSince(), 
	 lastLoopTime.mSecSince() - loopTime, myRobot->getMotorPacCount());
  lastLoopTime.setToNow();

  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myDesired.setVel(trans);
    myDesired.setDeltaHeading(-rot);
    return &myDesired;
  }
  else
  {
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
}

AREXPORT void ArServerClient::handleRequests(void)
{
  if (myState != STATE_CONNECTED)
    return;  

  std::list<ArServerClientData *>::iterator it;
  ArServerClientData *data;  
  ArServerData *serverData;
  ArTime lastSent;

  // walk through our list
  for (it = myRequested.begin(); it != myRequested.end(); ++it)
  {
    data = (*it);
    lastSent = data->getLastSent();
    // see if this needs to be called
    if (data->getMSec() != -1 && 
	(data->getMSec() == 0 || lastSent.mSecSince() > data->getMSec()))
    {
      serverData = data->getServerData();
      // call it, then set it so we know we did
      pushCommand(serverData->getCommand());
      pushForceTcpFlag(false);
      if (serverData->getFunctor() != NULL)
	serverData->getFunctor()->invoke(this, data->getPacket());
      popCommand();
      popForceTcpFlag();
      data->setLastSentToNow();
    }
  }
}

int main(int argc, char **argv)
{
  int ret;
  char bufWrite[1024];
  char bufRead[1024];
  int i, n;
  for (i = 0; i < 1024; i++)
    bufWrite[i] = 0x66;

  srand(time(NULL));
  
  int bytes1 = 0;
  //int bytes2 = 0;
  //int numToWrite = 1;

  ArTime lastPrint;
  
  if (argc < 2)
    {
      printf("Usage: %s <port>", argv[0]);
      exit(0);
    }


  ArSerialConnection ser1;
  ser1.setPort(argv[1]);
  ser1.setBaud(38400);
  if (!ser1.openSimple())
    {
      printf("Exiting since open failed\n");
      exit(0);
    }
  printf("Port opened");
  lastPrint.setToNow();
  while (1)
    {
      if (ser1.write(bufWrite, rand() % 1024) < 0)
	printf("Failed write\n");
      n = rand() % 1024;
      if ((ret = ser1.read(bufRead, n)) < 0)
	printf("Failed read\n");
      else if (ret > 0) 
	{
	  for (i = 0; i < ret; i++)
	    if (bufRead[i] != 0x66)
	      {
		printf("Failed\n");
		break;
	      }
	  bytes1 += ret;
	} 
       
      if (lastPrint.mSecSince() > 1000)
	{
	  printf("%d\n", bytes1);
	  lastPrint.setToNow();
	}
    }
	       
}

int go_to_position(ArRobot * robot, ArSick * sick, tracking_object position, float v = 300.0)
{

	(*robot).lock();
	(*robot).setDeltaHeading(position.degree - 90);
	(*robot).unlock();

	ArUtil::sleep(10);

	long int drive_time = (position.distance * 1000)/(v);
	ArTime start;

	(*robot).lock();
	(*robot).setVel(v);
	(*robot).unlock();


	start.setToNow();
	while(start.mSecSince() < drive_time)
	{
		;
	}

	(*robot).lock();
	(*robot).setVel(0);
	(*robot).unlock();

	return(0);
}

void *FillerThread::runThread(void *arg)
{
  while (1)
  {
    myStartBusyTime.setToNow();
    while (myStartBusyTime.mSecSince() < 150);
  }
}

std::vector<tracking_object> get_moving_objects(ArSick * sick, int ms_time, int num_scans, char user_command, int upper_angle = 10, int lower_angle = 350)
{
	std::vector<tracking_object> objects1;
	std::vector<tracking_object> objects2;
	std::vector<tracking_object> obj_vector;
	int t_elapsed;

	ArTime start;
	do
	{
		start.setToNow();

		objects1 = run_sick_scan(sick, num_scans);
		usleep(ms_time*1000);
		objects2 = run_sick_scan(sick, num_scans, user_command);

		t_elapsed = start.mSecSince();
	}while((objects1.size() != objects2.size())&&(objects1.size() > 0));

	unsigned int i = 0;
	unsigned int j = 0;

	for(j = 0; j < objects2.size(); j++)
	{
		tracking_object v;
		tracking_object new_v;
		v = objects2[j];
		v.vmag = 99999.9;
		for(i = 0; i < objects1.size(); i++)
		{
			new_v = v_calc(objects1[i],objects2[j],t_elapsed);

			if (new_v.vmag < v.vmag)
			{
				v = new_v;
			}
		}
		
		if(v.vmag > 3.0)
		{
			v.vmag = 0.0;
			v.vrad = 0.0;
			v.vtan = 0.0;
			v.theta_dot = 0.0;
		}

		obj_vector.push_back(v);

	}

	return(obj_vector);
}

AREXPORT bool ArTCMCompassDirect::blockingConnect(unsigned long connectTimeout)
{
  ArTime start;
  start.setToNow();
  if(!connect()) return false;
  ArLog::log(ArLog::Normal, "ArTCMCompassDirect: Opened connection, waiting for initial data...");
  while((unsigned long)start.mSecSince() <= connectTimeout)
  {
    if(read(0) > 0)
      return true;
    ArUtil::sleep(100);
  }
  ArLog::log(ArLog::Terse, "ArTCMCompassDirect: Error: No response from compass after %dms.", connectTimeout);
  return false;
}

AREXPORT bool ArMD5Calculator::calculateChecksum(const char *fileName,
                                                 unsigned char *md5DigestBuffer,
                                                 size_t md5DigestBufferLen)
{
  ArTime calcTime;

  if (ArUtil::isStrEmpty(fileName)) {
    // TODO ArLog
    return false;
  }
  FILE *file = fopen(fileName, "r");
  if (file == NULL) {
    // TODO ArLog
    return false;
  }

  ArMD5Calculator calculator;

  // TODO: Make this static and protect w/ mutex?
  char line[10000];
  bool ret = true;

  while (fgets(line, sizeof(line), file) != NULL)
  {
    calculator.append(line);
  }
  
  fclose(file);

  if (md5DigestBuffer != NULL) {
    if (md5DigestBufferLen != ArMD5Calculator::DIGEST_LENGTH) {
      // log warning
    }
    memset(md5DigestBuffer, 0, md5DigestBufferLen);
    memcpy(md5DigestBuffer, calculator.getDigest(), 
          ArUtil::findMin(md5DigestBufferLen, ArMD5Calculator::DIGEST_LENGTH));
  }

  int elapsed = calcTime.mSecSince();
  
  ArLog::log(ArLog::Normal,
             "ArMD5Calculator::calculateChecksum(%s) took %i msecs",
             fileName, elapsed);

  return true;


} // end method calculateChecksum

bool ArUrg::readLine(char *buf, unsigned int size, 
		     unsigned int msWait)
{
  if (myConn == NULL)
  {
    ArLog::log(ArLog::Terse, 
	       "%s: Attempt to read line from null connection", getName());
    return false;
  }

  ArTime started;
  started.setToNow();

  buf[0] = '\0';
  unsigned int onChar = 0;
  int ret;

  myConnMutex.lock();
  while ((msWait == 0 || started.mSecSince() < (int)msWait) && 
	 onChar < size)
  {
    if ((ret = myConn->read(&buf[onChar], 1, 0)) > 0)
    {
      if (buf[onChar] == '\n' || 
	  buf[onChar] == '\r')
      {
	//buf[onChar+1] = '\0';
	buf[onChar] = '\0';
	if (myLogMore)
	  ArLog::log(ArLog::Normal, "%s: '%s'", getName(), buf);
	myConnMutex.unlock();
	return true;
      }
      onChar++;
    }
    if (ret < 0)
    {
      ArLog::log(ArLog::Normal, "%s: bad ret", getName());
      myConnMutex.unlock();
      return false;
    }
    if (ret == 0)
      ArUtil::sleep(1);
  }
  myConnMutex.unlock();
  return false;
}

int main(int argc, char **argv)
{
  ArClientBase client;
  ArGlobalFunctor1<ArNetPacket *> testCB(&test);

  Aria::init();
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  ArTime startTime;
  startTime.setToNow();
  if (!client.blockingConnect("localhost", 7273))
  {
    printf("Could not connect to server, exiting\n");
    exit(1);
  }    
  printf("Took %ld msec to connect\n", startTime.mSecSince());
  
  client.runAsync();
  
  client.lock();
  client.addHandler("test", &testCB);
  client.addHandler("test2", &testCB);
  client.addHandler("test3", &testCB);
  client.logDataList();
  
  client.requestOnce("test");
  client.request("test2", 100);
  client.request("test3", -1);
  client.unlock();
  ArUtil::sleep(1000);
  printf("Changing speed\n");
  client.lock();
  client.request("test2", 300);
  client.unlock();
  ArUtil::sleep(1000);
  client.lock();
  client.requestStop("test2");
  client.unlock();
  
  ArUtil::sleep(1000);
  client.lock();
  client.disconnect();
  client.unlock();
  ArUtil::sleep(50);
  exit(0);
}

bool ArUrg::sendCommandAndRecvStatus(
	const char *command, const char *commandDesc, 
	char *buf, unsigned int size, unsigned int msWait)
{
  bool ret;
  ArTime start;

  // send the command
  if (!writeLine(command))
  {
    ArLog::log(ArLog::Normal, "%s: Could not send %s", getName(), 
	       commandDesc);
    return false;
  }

  // try and read the command back
  if (!readLine(buf, size, msWait))
  {
    ArLog::log(ArLog::Normal, "%s: Received no response to %s (in %d but really %d)", 
	       getName(), commandDesc, msWait, start.mSecSince());
    return false;
  }

  // make sure the command back matches
  if (strcasecmp(buf, command) != 0)
  {
    ArLog::log(ArLog::Normal, "%s: Received bad echo to %s (command %s got back %s)", 
	       getName(), commandDesc, command, buf);
    return false;
  }

  // get the status from that command back
  if (!readLine(buf, size, msWait))
  {
    ArLog::log(ArLog::Normal, 
	       "%s: Did not receive status back from %s",
	       getName(), commandDesc);
    return false;
  }

  return true;
}