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

// the important function
void KeyPTU::drive(void)
{
  // if the PTU isn't initialized, initialize it here... it has to be 
  // done here instead of above because it needs to be done when the 
  // robot is connected
  if (!myPTUInitRequested && !myPTU.isInitted() && myRobot->isConnected())
  {
    printf("\nWaiting for Camera to Initialize\n");
    myAbsolute = true;
    myPTUInitRequested = true;
    myPTU.init();
  }

  // if the camera hasn't initialized yet, then just return
  if (myPTUInitRequested && !myPTU.isInitted())
  {
    return;
  }

  if (myPTUInitRequested && myPTU.isInitted())
  {
    myPTUInitRequested = false;
    myPanSlew = myPTU.getPanSlew();
    myTiltSlew = myPTU.getTiltSlew();
    printf("Done.\n");
    question();
  }

  if (myExerciseTime.secSince() > 5 && myExercise)
  {
    int pan,tilt;

    if (ArMath::random()%2)
      pan = ArMath::random()%((int)myPTU.getMaxPosPan());
    else
      pan = -ArMath::random()%((int)myPTU.getMaxNegPan());

    if (ArMath::random()%2)
      tilt = ArMath::random()%((int)myPTU.getMaxPosTilt());
    else
      tilt = -ArMath::random()%((int)myPTU.getMaxNegTilt());

    myPTU.panTilt(pan, tilt);
    //printf("** %d\n", myRobot->getEstop());
    //printf("--> %x\n", myRobot->getFlags());
    myExerciseTime.setToNow();
  }

}

int main(int argc, char** argv)
{
  Aria::init();
  ArLog::init(ArLog::StdErr, ArLog::Normal);

  ArArgumentParser argParser(&argc, argv);
  argParser.loadDefaultArguments();
  ArSimpleConnector connector(&argParser);
  ArGPSConnector gpsConnector(&argParser);

  if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    ArLog::log(ArLog::Terse, "gpsExample options:\n  -printTable   Print data to standard output in regular columns rather than a refreshing terminal display, and print more digits of precision");
    return 1;
  }

  // Try connecting to robot 
  ArRobot robot;
  if(!connector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "gpsExample: Warning: Could not connect to robot.  Will not be able to switch GPS power on, or load GPS options from this robot's parameter file.");
  }
  else
  {
    ArLog::log(ArLog::Normal, "gpsExample: Connected to robot.");
    robot.runAsync(true);
  }

  // check command line arguments for -printTable
  bool printTable = argParser.checkArgument("printTable");

  // On the Seekur, power to the GPS receiver is switched on by this command.
  // (A third argument of 0 would turn it off). On other robots this command is
  // ignored.
  robot.com2Bytes(116, 6, 1);

  // Try connecting to a GPS. We pass the robot pointetr to the connector so it
  // can check the robot parameters for this robot type for default values for
  // GPS device connection information (receiver type, serial port, etc.)
  ArLog::log(ArLog::Normal, "gpsExample: Connecting to GPS, it may take a few seconds...");
  ArGPS *gps = gpsConnector.createGPS(&robot);
  if(!gps || !gps->connect())
  {
    ArLog::log(ArLog::Terse, "gpsExample: Error connecting to GPS device.  Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help.");
    return -1;
  }


  ArLog::log(ArLog::Normal, "gpsExample: Reading data...");
  ArTime lastReadTime;
  if(printTable)
    gps->printDataLabelsHeader();
  while(true)
  {
    int r = gps->read();
    if(r & ArGPS::ReadError)
    {
      ArLog::log(ArLog::Terse, "gpsExample: Warning: error reading GPS data.");
      ArUtil::sleep(1000);
      continue;
    }


    if(r & ArGPS::ReadUpdated)
    {
      if(printTable)
      {
        gps->printData(false);
        printf("\n");
      }
      else
      {
        gps->printData();
        printf("\r");
      }
      fflush(stdout);
      ArUtil::sleep(500);
      lastReadTime.setToNow();
      continue;
    } else {
      if(lastReadTime.secSince() >= 5) {
        ArLog::log(ArLog::Terse, "gpsExample: Warning: haven't recieved any data from GPS for more than 5 seconds!");
      }
      ArUtil::sleep(1000);
      continue;
    }

  }
  return 0;
}

void *KinectArVideoServer::runThread(void*)
{
  // TODO might need to move initialization to separate function

  /* Open Kinect */

/*
  int ndevs = freenect2.enumerateDevices();
  printf("KinectArVideoServer: %d devices\n", ndevs);
  std::string serial = freenect2.getDefaultDeviceSerialNumber();
  printf("KinectArVideoServer: device serial number is %s\n", serial.c_str());
*/

  freenect_dev = freenect2.openDefaultDevice();

  if(!freenect_dev)
  {
    std::cout << "KinectArVideoServer: no kinect2 device connected or failure opening the default one!" << std::endl;
    return 0;
  }

  shutdown = false;


  /* Set up libfreenect listeners, start capturing  */

  //libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
  libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
  libfreenect2::FrameMap frames;

  freenect_dev->setColorFrameListener(&listener);
  freenect_dev->setIrAndDepthFrameListener(&listener);

  assert(freenect_dev);
  if(! freenect_dev->start() )
  {
    std::cout << "KinectArVideoServer: Error starting stream from kinect!" << std::endl;
    return NULL;
  }

  std::cout << "kinect device serial: " << freenect_dev->getSerialNumber() << std::endl;
  std::cout << "kinect device firmware: " << freenect_dev->getFirmwareVersion() << std::endl;

  // TODO set up registration in libfreenect2

  ArVideoOpenCV kinectDepthSource("Kinect_Depth|libfreenect2|OpenCV");
  ArVideo::createVideoServer(server, &kinectDepthSource, "Kinect_Depth|libfreenect2|OpenCV", "freenect2|Depth|OpenCV");

  ArVideoOpenCV kinectRGBSource("Kinect_RGB|libfreenect2|OpenCV");
  ArVideo::createVideoServer(server, &kinectRGBSource, "Kinect_RGB|libfreenect2|OpenCV", "freenect2|RGB|OpenCV");

//  ArVideoOpenCV kinectThreshSource("Kinect_Depth|libfreenect2|OpenCV_threshold");
//  ArVideo::createVideoServer(&server, &kinectThreshSource, "Kinect_Depth|libfreenect2|OpenCV_threshold", "Kinect depth data with basic threshold applied");


  /* Main loop, capture images from kinect, display, copy to ArVideo sources */

  bool first = true;
  cv::Mat rgbm_small(resize_to_width, resize_to_height, CV_8UC3);
  cv::Mat depthm_small(resize_to_width, resize_to_height, CV_8UC3);
  cv::Mat rgbm_flip(resize_to_width, resize_to_height, CV_8UC3);
  cv::Mat depthm_flip(resize_to_width, resize_to_height, CV_8UC3);

  while(!shutdown)
  {
//    std::cout << "." << std::flush;
    ArTime t;
    listener.waitForNewFrame(frames); //, 30000);
    if(t.secSince() >= 28) 
    {  
      std::cout << "KinectArVideoServer: Warning: took more than 30 seconds to receive a frame from Kinect!" << std::endl;
      //shutdown = true;
      //continue;
    }
    
    
//    printf("%d\n", t.secSince());
    libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
//    libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
    libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];

    // todo don't recreate Mat objects each time
    cv::Mat rgbm(rgb->height, rgb->width, CV_8UC4, rgb->data);
    cv::resize (rgbm, rgbm_small, cv::Size(resize_to_width, resize_to_height));
    cv::flip(rgbm_small, rgbm_flip, 1);

    cv::Mat depthm(depth->height, depth->width, CV_32FC1, depth->data);
    cv::resize(depthm, depthm_small, cv::Size(resize_to_width, resize_to_height));
    cv::flip(depthm_small, depthm_flip, 1);


//    cv::Mat depth_thresh(depth->height, depth->width, CV_32FC1, depth->data);
//    cv::threshold(depthm, depth_thresh, 0.4, 1.0, CV_THRESH_BINARY_INV);

//    cv::imshow("rgb", rgbm_small);
//    cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f);
//    cv::imshow("depth", depthm / 4500.0f);

    if(!kinectRGBSource.updateVideoDataCopy(rgbm_flip, 1, CV_BGR2RGB))
      std::cout << "KinectArVideoServer: Warning: error copying rgb data to ArVideo source" << std::endl;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    // empty the buffer from the last version request
    while (readLine(buf, sizeof(buf), 100));

    // now change the baud...
    sprintf(buf, "S%06d7654321", atoi(getAutoBaudChoice()));
    if (!writeLine(buf))
      return false;

    ArUtil::sleep(100);

    //serConn->setBaud(115200);
    serConn->setBaud(atoi(getAutoBaudChoice()));
    // wait a second for the baud to change...
    ArUtil::sleep(100);

    // empty the buffer from the baud change
    while (readLine(buf, sizeof(buf), 100));

    if (!(ret = sendCommandAndRecvStatus(
		  "V", "version request after switching to autobaudchoice", 
		  buf, sizeof(buf), 10000)) || 
	strcasecmp(buf, "0") != 0)      
    {
      if (ret && strcasecmp(buf, "0") != 0)      
	ArLog::log(ArLog::Normal, 
		   "%s::blockingConnect: Bad status on version response after switching to autobaudchoice", 
		   getName());
      return false;
    }

    ArLog::log(ArLog::Normal, "%s: Switched to %s baud rate",
	       getName(), getAutoBaudChoice());
  }

  while (readLine(buf, sizeof(buf), 10000))
  {
    /// MPL put this in instead of the following because of the
    /// behavior change of readline
    if (strlen(buf) == 0)
      break;

    if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
      myVendor = &buf[5];
    else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
      myProduct = &buf[5];
    else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
      myFirmwareVersion = &buf[5];
    else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
      myProtocolVersion = &buf[5];
    else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
      mySerialNumber = &buf[5];
    else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
      myStat = &buf[5];
  }

  if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() || 
      myProtocolVersion.empty() || mySerialNumber.empty())
  {
    ArLog::log(ArLog::Normal, 
	       "%s::blockingConnect: Missing information in version response",
	       getName());
    return false;
  }

  log();

  myLogMore = true;
  //  myLogMore = false;
  ArUtil::sleep(100);
  
  
  printf("myRequestString %s\n", myRequestString);


  if (!(ret = sendCommandAndRecvStatus(
		myRequestString, "request distance reading", 
		buf, sizeof(buf), 10000)) || 
      strcasecmp(buf, "0") != 0)
  {
    if (ret && strcasecmp(buf, "0") != 0) 
      ArLog::log(ArLog::Normal, 
	 "%s::blockingConnect: Bad status on distance reading response (%c)",
		 getName(), buf[0]);
    return false;
  }
  
  myLogMore = false;

  ArTime started;
  started.setToNow();
  while (started.secSince() < 10 && readLine(buf, sizeof(buf), 10000))
  {
    if (strlen(buf) == 0)
      return true;
  }

  ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
	     getName(), buf[0]);
  return false;
}

//.........这里部分代码省略.........
    else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
      myStat = &buf[5];
  }

  if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() || 
      myProtocolVersion.empty() || mySerialNumber.empty())
  {
    ArLog::log(ArLog::Normal, 
	       "%s::blockingConnect: Missing information in version response",
	       getName());
    return false;
  }

  if (!(ret = sendCommandAndRecvStatus(
		"PP", "parameter info request", 
		buf, sizeof(buf), 10000)) || 
      strcasecmp(buf, "00") != 0)      
  {
    ArLog::log(ArLog::Normal, 
	       "%s::blockingConnect: Bad response to parameter info request",
	       getName());
    return false;
  }

  while (readLine(buf, sizeof(buf), 10000, false, true))
  {
    if (strlen(buf) == 0)
      break;

    if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0)
      myModel = &buf[5];
    else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0)
      myDMin = atoi(&buf[5]);
    else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0)
      myDMax = atoi(&buf[5]);
    else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0)
      myARes = atoi(&buf[5]);
    else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0)
      myAMin = atoi(&buf[5]);
    else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0)
      myAMax = atoi(&buf[5]);
    else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0)
      myAFront = atoi(&buf[5]);
    else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0)
      myScan = atoi(&buf[5]);
  }

  if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 ||
      myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0)
  {
    ArLog::log(ArLog::Normal, 
	       "%s::blockingConnect: Missing information in parameter info response",
	       getName());
    return false;
  }

  myStepSize = 360.0 / myARes;
  myStepFirst = myAFront * myStepSize;
  
  if (myMaxRange > myDMax)
    setMaxRange(myDMax);

  //log();

  setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());

  //myLogMore = true;
  //  myLogMore = false;
  ArUtil::sleep(100);
  
  
  //printf("myRequestString %s\n", myRequestString);

  if (!(ret = sendCommandAndRecvStatus(
		myRequestString, "request distance reading", 
		buf, sizeof(buf), 10000)) || 
      strcasecmp(buf, "00") != 0)
  {
    if (ret && strcasecmp(buf, "00") != 0) 
      ArLog::log(ArLog::Normal, 
	 "%s::blockingConnect: Bad status on distance reading response (%s)",
		 getName(), buf);
    return false;
  }
  
  //myLogMore = false;

  ArTime started;
  started.setToNow();
  while (started.secSince() < 10 && 
	 readLine(buf, sizeof(buf), 10000, true, false))
  {
    if (strlen(buf) == 0)
      return true;
  }

  ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
	     getName());
  return false;
}

int main(int argc, char **argv)
{
  int ret;
  std::string str;
  ArSerialConnection con;
  ArSickPacket sick;
  ArSickPacket *packet;
  ArSickPacketReceiver receiver(&con);
  ArTime start;
  unsigned int value;
  int numReadings;
  ArTime lastReading;
  ArTime packetTime;

  start.setToNow();

  // open the connection, if it fails, exit
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  start.setToNow();

  printf("Waiting for laser to power on\n");
  sick.empty();
  sick.uByteToBuf(0x10);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  while (start.secSince() < 70 && 
	 ((packet = receiver.receivePacket(100)) == NULL
	  ||  (packet->getID() != 0x90)));
  if (packet != NULL)
    printf("Laser powered on\n");
  else
    exit(1);

  printf("Changing baud\n");
  sick.empty();
  sick.byteToBuf(0x20);
  sick.byteToBuf(0x40);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  ArUtil::sleep(10);
  if (!con.setBaud(38400))
  {
    printf("Could not set baud, exiting\n");
  }
  
  
  /*packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();
  */
  sick.empty();
  sick.uByteToBuf(0x3B);
  sick.uByte2ToBuf(180);
  sick.uByte2ToBuf(100);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();

  sick.empty();
  sick.byteToBuf(0x20);
  sick.byteToBuf(0x24);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();



  printf("Starting to report back from port, it took %ld ms to get here:\n",
	 start.mSecSince());
  start.setToNow();
  while (start.secSince() < 6)
  {
    packetTime.setToNow();
    packet = receiver.receivePacket();
    if (packet != NULL)
      printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
    if (packet != NULL)
    {
      if (packet->getLength() < 10)
	packet->log();
      else if (packet->getID() == 0x90)
      {
	char strBuf[512];
	packet->log();
	//printf("%x\n", packet->bufToUByte());
//.........这里部分代码省略.........

bool tryport(const char *port, bool is422, int baud, char **argv, int argc, int maxread, int timeout)
{
  ArSerialConnection ser1(is422);
  ser1.setPort(port);
  ser1.setBaud(baud);
  printf("Trying %s port %s at baud rate %d:\n", is422?"RS-422":"RS-232", port, baud);

  if (!ser1.openSimple())
  {
    ArLog::logErrorFromOS(ArLog::Terse, "Error opening %s.", port);
    return false;
  }

  if (argc > 0)
  {
    printf("-> %3d bytes: ", argc);
    fflush(stdout);
  }

  for (int i = 0; i < argc; ++i)
  {
    int d = strtol(argv[i], NULL, 0);
    if (d == 0 && errno != 0)
    {
      ArLog::logErrorFromOS(ArLog::Terse, "error parsing command argument %d (\"%s\"). Must be decimal, hexidecimal, or octal number.", i, argv[i]);
      Aria::exit(3);
    }
    unsigned char c = (unsigned char)d;
    printf("0x%.2X %c ", (unsigned char) c, (c >= ' ' && c <= '~') ? c : ' ');
    fflush(stdout);
    ser1.write((char*)&c, 1);
  }

  if (argc > 0)
    puts("");

  char buf[256];
  int n = 0;
  ArTime t;
  while (1)
  {
    ArUtil::sleep(1);
    int r = 0;
    if ((r = ser1.read(buf, sizeof(buf))) < 0)
    {
      puts("Error reading data from serial port.");
      return false;
    }
    else if (r > 0)
    {
      printf("<- %3d bytes: ", r);
      for (int i = 0; i < r; ++i)
      {
        //printf("[%d] ", i); fflush(stdout);
        char c = buf[i];
        printf("0x%.2X %c ", (unsigned char)c, (c >= ' ' && c <= '~') ? c : ' ');
        if ((i+1) % 8 == 0)
          printf("\n              ");
      }
      printf("\n");
    }
    if ((n += r) >= maxread)
    {
      puts("max");
      return true;
    }
    if (t.secSince() > timeout)
    {
      puts("timeout");
      return false;
    }
  }
  return true;
}