当前位置: 首页>>代码示例>>C++>>正文


C++ ArTime类代码示例

本文整理汇总了C++中ArTime的典型用法代码示例。如果您正苦于以下问题:C++ ArTime类的具体用法?C++ ArTime怎么用?C++ ArTime使用的例子?那么, 这里精选的类代码示例或许可以为您提供帮助。


在下文中一共展示了ArTime类的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: getTimeRead

AREXPORT ArTime ArLogFileConnection::getTimeRead(int index)
{
  ArTime now;
  now.setToNow();
  return now;
}
开发者ID:Aharobot,项目名称:ArAndroidApp,代码行数:6,代码来源:ArLogFileConnection.cpp

示例2: sendCommandAndRecvStatus

bool ArUrg_2_0::internalConnect(void)

{
  bool ret = true;
  char buf[1024];
  

  ArSerialConnection *serConn = NULL;
  serConn = dynamic_cast<ArSerialConnection *>(myConn);

  bool alreadyAtAutobaud = false;



  // empty the buffer...
  /*
  sendCommandAndRecvStatus(
	  "RS", "reset", 
	  buf, sizeof(buf), 1000);
  readLine(buf, sizeof(buf), 1, true, false);

  sendCommandAndRecvStatus(
	  "SCIP2.0", "SCIP 2.0 request", 
	  buf, sizeof(buf), 1000);
  */

  writeLine("RS");
  ArUtil::sleep(100);

  writeLine("SCIP2.0");
  ArUtil::sleep(100);

  ArTime startedFlushing;

  while (readLine(buf, sizeof(buf), 1, true, false) ||
	 startedFlushing.mSecSince() < 1000);

  buf[0] = '\0';

  if (!(ret = sendCommandAndRecvStatus(
		"VV", "version request", 
		buf, sizeof(buf), 10000)) || 
      strcasecmp(buf, "00") != 0)      
  {
    // if we didn't get it and have an autobaud, try it at what the autobaud rate is
    if (serConn != NULL && atoi(getAutoBaudChoice()) > 0)
    {
      alreadyAtAutobaud = true;
      serConn->setBaud(atoi(getAutoBaudChoice()));
      ArUtil::sleep(100);

      writeLine("RS");
      ArUtil::sleep(100);
      
      writeLine("SCIP2.0");
      ArUtil::sleep(100);
      
      startedFlushing.setToNow();
      while (readLine(buf, sizeof(buf), 1, true, false) ||
	     startedFlushing.mSecSince() < 1000);
      
      if (!(ret = sendCommandAndRecvStatus(
		"VV", "version request after falling back to autobaudchoice", 
		buf, sizeof(buf), 10000)) || 
	  strcasecmp(buf, "00") != 0)      
      {
	if (ret && strcasecmp(buf, "00") != 0)      
	  ArLog::log(ArLog::Normal, 
		     "%s::blockingConnect: Bad status on version response after falling back to autobaudchoice", 
		     getName());
	return false;
      }
    }
    // if we don't have a serial port or no autobaud then we can't
    // change the baud, so just fail
    else
    {
      if (ret && strcasecmp(buf, "00") != 0)      
	ArLog::log(ArLog::Normal, 
		   "%s::blockingConnect: Bad status on version response (%s)",
		   getName(), buf);
      return false;
    }
  }

  // if we want to autobaud, then give it a whirl
  if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0)
  {

    // empty the buffer from the last version request
    while (readLine(buf, sizeof(buf), 100, true, false));

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

    ArUtil::sleep(100);

    //serConn->setBaud(115200);
//.........这里部分代码省略.........
开发者ID:sanyaade-research-hub,项目名称:aria,代码行数:101,代码来源:ArUrg_2_0.cpp

示例3: read

AREXPORT int ArLogFileConnection::read(const char *data, unsigned int size, 
				   unsigned int msWait)
{
  ArTime timeDone;
  unsigned int bytesRead = 0;
  int n;

  if (getStatus() != STATUS_OPEN) 
  {
    ArLog::log(ArLog::Terse, 
	       "ArLogFileConnection::read: Attempt to use port that is not open.");
    return -1;
  }
  
  timeDone.setToNow();
  timeDone.addMSec(msWait);

  if (stopAfter-- <= 0)
    {
      stopAfter= 1;
      return 0;
    }

  if (myFD != NULL)
    {
      char line[1000];
      if (fgets(line, 1000, myFD) == NULL) // done with file, close
        {
          close();
          return -1;
        }
      // parse the line
      int i=0;
      n = 0;
      while (line[i] != 0)
        {
          if (isdigit(line[i]))
            {
              if (isdigit(line[i+1]))
                {
                  if (isdigit(line[i+2]))
                    {
                      const_cast<char *>(data)[n++] = 
                        100 * (line[i]-'0') + 10*(line[i+1]-'0') + line[i+2]-'0';
                      i++;
                    }
                  else
                      const_cast<char *>(data)[n++] = 10*(line[i]-'0') + line[i+1]-'0';
                  i++;
                }
              else
                const_cast<char *>(data)[n++] = line[i]-'0';
            }
          i++;
        }
    }

#if 0
  if (n > 0)                    // add in checksum
    {
      int i;
      unsigned char nn;
      int c = 0;

      i = 3;
      nn = data[2] - 2;
      while (nn > 1) 
        {
          c += ((unsigned char)data[i]<<8) | (unsigned char)data[i+1];
          c = c & 0xffff;
          nn -= 2;
          i += 2;
        }
      if (nn > 0) 
        c = c ^ (int)((unsigned char) data[i]);

      const_cast<char *>(data)[n++] = (c << 8) & 0xff;
      const_cast<char *>(data)[n++] = c & 0xff;
    }
#endif

  bytesRead = n;
  return bytesRead;
}
开发者ID:Aharobot,项目名称:ArAndroidApp,代码行数:84,代码来源:ArLogFileConnection.cpp

示例4: main

int main(void)
{
  int ret;
  char bufWrite[1024];
  char bufRead[1024];
  bool verbose = false;
  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;

  ArSerialConnection ser1;
  ser1.setPort(ArUtil::COM1);
  //ser1.setBaud(115200);
  if (!ser1.openSimple())
  {
    printf("Exiting since open failed\n");
    exit(0);
  }
  printf("Port opened\n");
  lastPrint.setToNow();
  while (1)
  {
    ArUtil::sleep(1);
    //ArUtil::sleep(500);
/*
    bufWrite[0] = 0xfa;
    bufWrite[1] = 0xfb;
    bufWrite[2] = 0x3;
    bufWrite[3] = 0x0;
    bufWrite[4] = 0x0;
    bufWrite[5] = 0x0;
    ser1.write(bufWrite, 6);
*/
    ////ser1.write("a", 1);
    if ((ret = ser1.read(bufRead, sizeof(bufRead))) < 0)
      printf("Failed2 read\n");
    else if (ret > 0)
    {
      bufRead[ret] = '\0';
      if (verbose)
      {
	printf("%3d: ", ret);
	for (i = 0; i < ret; i++)
	  printf("%c(%x) ", bufRead[i], (unsigned char)bufRead[i]);
	printf("\n");
      }
      else
	printf("%s", bufRead);
    }
    else
      bufRead[0] = '\0';
    //ser1.write("a", 1);

  }
  
}
开发者ID:sauver,项目名称:sauver_sys,代码行数:64,代码来源:serialDump.cpp

示例5: main

int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  int dist;
  ArTime start;
  ArPose startPose;
  bool vel2 = false;

  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);
  
  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  if (argc != 2 || (dist = atoi(argv[1])) == 0)
    {
      printf("Usage: %s <distInMM>\n", argv[0]);
      exit(0);
    }
  if (dist < 1000)
    {
      printf("You must go at least a meter\n");
      exit(0);
    }
  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory

  robot.lock();

  /*
  robot.setAbsoluteMaxTransVel(2000);
  robot.setTransVelMax(2000);
  robot.setTransAccel(1000);
  robot.setTransDecel(1000);
  robot.comInt(82, 30); // rotkp
  robot.comInt(83, 200); // rotkv
  robot.comInt(84, 0); // rotki
  robot.comInt(85, 30); // transkp
  robot.comInt(86, 450); // transkv
  robot.comInt(87, 4); // transki

  */
  printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
  if (vel2)
    robot.setVel2(2200, 2200);
  else
    robot.setVel(2200);
  robot.unlock();
  start.setToNow();
  startPose = robot.getPose();
  while (1)
  {
    robot.lock();
    printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
	   robot.getVel(), robot.getX(), robot.getY(), 
	   startPose.findDistanceTo(robot.getPose()),
	   robot.getTh());
    if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
    {
      printf("\nFinished distance\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("\nDistance timed out\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }   
//.........这里部分代码省略.........
开发者ID:PipFall2015,项目名称:Ottos-Cloud,代码行数:101,代码来源:driveFast.cpp

示例6: if

ArLMS1XXPacket *ArLMS1XXPacketReceiver::receivePacket(unsigned int msWait,
						      bool scandataShortcut)
{
  ArLMS1XXPacket *packet;
  unsigned char c;
  long timeToRunFor;
  ArTime timeDone;
  ArTime lastDataRead;
  ArTime packetReceived;
  int numRead;
  int i;

  if (myConn == NULL || 
      myConn->getStatus() != ArDeviceConnection::STATUS_OPEN)
  {
    return NULL;
  }

  timeDone.setToNow();
  timeDone.addMSec(msWait);
  do
  {
    timeToRunFor = timeDone.mSecTo();
    if (timeToRunFor < 0)
      timeToRunFor = 0;

    //printf("%x\n", c);
    if (myState == STARTING)
    {
      if ((numRead = myConn->read((char *)&c, 1, timeToRunFor)) <= 0) 
	return NULL;
      if (c == '\002')
      {
	myState = DATA;
	myPacket.empty();
	myPacket.setLength(0);
	myPacket.rawCharToBuf(c);
	myReadCount = 0;
	packetReceived = myConn->getTimeRead(0);
	myPacket.setTimeReceived(packetReceived);
      }
      else
      {
	ArLog::log(ArLog::Normal, 
		   "ArLMS1XXPacketReceiver: Failed read (%d)", 
		   numRead);
      }
    }
    else if (myState == DATA)
    {
      numRead = myConn->read(&myReadBuf[myReadCount],
			     sizeof(myReadBuf) - myReadCount,
			     5);
      // trap if we failed the read
      if (numRead < 0)
      {
	ArLog::log(ArLog::Normal, 
		   "ArLMS1XXPacketReceiver: Failed read (%d)", 
		   numRead);
	myState = STARTING;
	return NULL;
      }
      // see if we found the end of the packet 
      for (i = myReadCount; i < myReadCount + numRead; i++)
      {
	if (myReadBuf[i] == '\002')
	{
	  ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Data found start of new packet...", 
		     myReadCount);
	  myPacket.empty();
	  myPacket.setLength(0);
	  memmove(myReadBuf, &myReadBuf[i], myReadCount + numRead - i);
	  numRead -= (i - myReadCount);
	  myReadCount -= i;
	  i = 0;
	  continue;
	}
	if (myReadBuf[i] == '\003')
	{
	  myPacket.dataToBuf(myReadBuf, i + 1);
	  myPacket.resetRead();
	  packet = new ArLMS1XXPacket;
	  packet->duplicatePacket(&myPacket);
	  myPacket.empty();
	  myPacket.setLength(0);

	  // if it's the end of the data just go back to the beginning
	  if (i == myReadCount + numRead - 1)
	  {
	    //ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Starting again");
	    myState = STARTING;
	  }
	  // if it isn't move the data up and start again	  
	  else
	  {
	    memmove(myReadBuf, &myReadBuf[i+1], myReadCount + numRead - i - 1);
	    myReadCount = myReadCount + numRead - i - 1;
	    myState = REMAINDER;
	    ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Got remainder, %d bytes beyond one packet ...", 
		       myReadCount);
//.........这里部分代码省略.........
开发者ID:Aharobot,项目名称:ArAndroidApp,代码行数:101,代码来源:ArLMS1XX.cpp

示例7: main

int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  ArTime start;
  
  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);

  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

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

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory
  printf("Telling the robot to go 300 mm for 5 seconds\n");
  robot.lock();
  robot.setVel(500);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }
  
  printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(50);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(100);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Done with tests, exiting\n");
  robot.disconnect();
//.........这里部分代码省略.........
开发者ID:sendtooscar,项目名称:ariaClientDriver,代码行数:101,代码来源:velTest.cpp

示例8: printf

void RosArnlNode::publish()
{
  // todo could only publish if robot not stopped (unless arnl has TriggerTime
  // set in which case it might update localization even ifnot moving), or
  // use a callback from arnl for robot pose updates rather than every aria
  // cycle.  In particular, getting the covariance is a bit computational
  // intensive and not everyone needs it.

  ArTime tasktime;

  // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
  ArPose pos = arnl.robot->getPose();

  // convert mm and degrees to position meters and quaternion angle in ros pose
  tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
    pos.getY()/1000, 0)), pose_msg.pose.pose); 

  pose_msg.header.frame_id = "map";

  // ARIA/ARNL times are in reference to an arbitrary starting time, not OS
  // clock, so find the time elapsed between now and last ARNL localization
  // to adjust the time stamp in ROS time vs. now accordingly.
  //pose_msg.header.stamp = ros::Time::now();
  ArTime loctime = arnl.locTask->getLastLocaTime();
  ArTime arianow;
  const double dtsec = (double) loctime.mSecSince(arianow) / 1000.0;
  //printf("localization was %f seconds ago\n", dtsec);
  pose_msg.header.stamp = ros::Time(ros::Time::now().toSec() - dtsec);

  // TODO if robot is stopped, ARNL won't re-localize (unless TriggerTime option is
  // configured), so should we just use Time::now() in that case? or do users
  // expect long ages for poses if robot stopped?

#if 0
  {
    printf("ros now is   %12d sec + %12d nsec = %f seconds\n", ros::Time::now().sec, ros::Time::now().nsec, ros::Time::now().toSec());
    ArTime t;
    printf("aria now is  %12lu sec + %12lu ms\n", t.getSec(), t.getMSec());
    printf("arnl loc is  %12lu sec + %12lu ms\n", loctime.getSec(), loctime.getMSec());
    printf("pose stamp:= %12d sec + %12d nsec = %f seconds\n", pose_msg.header.stamp.sec, pose_msg.header.stamp.nsec, pose_msg.header.stamp.toSec());
    double d = ros::Time::now().toSec() - pose_msg.header.stamp.toSec();
    printf("diff is  %12f sec, \n", d);
    puts("----");
  }
#endif

#ifndef ROS_ARNL_NO_COVARIANCE
  ArMatrix var;
  ArPose meanp;
  if(arnl.locTask->findLocalizationMeanVar(meanp, var))
  {
    // ROS pose covariance is 6x6 with position and orientation in 3
    // dimensions each x, y, z, roll, pitch, yaw (but placed all in one 1-d
    // boost::array container)
    //
    // ARNL has x, y, yaw (aka theta):
    //    0     1     2
    // 0  x*x   x*y   x*yaw
    // 1  y*x   y*y   y*yaw
    // 2  yaw*x yaw*y yaw*yaw
    //
    // Also convert mm to m and degrees to radians.
    //
    // all elements in pose_msg.pose.covariance were initialized to -1 (invalid
    // marker) in the RosArnlNode constructor, so just update elements that
    // contain x, y and yaw.
  
    pose_msg.pose.covariance[6*0 + 0] = var(0,0)/1000.0;  // x/x
    pose_msg.pose.covariance[6*0 + 1] = var(0,1)/1000.0;  // x/y
    pose_msg.pose.covariance[6*0 + 5] = ArMath::degToRad(var(0,2)/1000.0);    //x/yaw
    pose_msg.pose.covariance[6*1 + 0] = var(1,0)/1000.0;  //y/x
    pose_msg.pose.covariance[6*1 + 1] = var(1,1)/1000.0;  // y/y
    pose_msg.pose.covariance[6*1 + 5] = ArMath::degToRad(var(1,2)/1000.0);  // y/yaw
    pose_msg.pose.covariance[6*5 + 0] = ArMath::degToRad(var(2,0)/1000.0);  //yaw/x
    pose_msg.pose.covariance[6*5 + 1] = ArMath::degToRad(var(2,1)/1000.0);  // yaw*y
    pose_msg.pose.covariance[6*5 + 5] = ArMath::degToRad(var(2,2)); // yaw*yaw
  }
#endif
  
  pose_pub.publish(pose_msg);


  if(action_executing) 
  {
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position.pose = pose_msg.pose.pose;
    actionServer.publishFeedback(feedback);
  }


  // publishing transform map->base_link
  map_trans.header.stamp = ros::Time::now();
  map_trans.header.frame_id = frame_id_map;
  map_trans.child_frame_id = frame_id_base_link;
  
  map_trans.transform.translation.x = pos.getX()/1000;
  map_trans.transform.translation.y = pos.getY()/1000;
  map_trans.transform.translation.z = 0.0;
  map_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);

//.........这里部分代码省略.........
开发者ID:thentnucyborg,项目名称:ros-arnl,代码行数:101,代码来源:rosarnl_node.cpp

示例9: main

int main(int argc, char **argv)
{
  // this is how long to wait after there's been no data to close the
  // connection.. if its 0 and its using robot it'll set it to 5000 (5
  // seconds), if its 0 and using laser, it'll set it to 60000 (60
  // seconds, which is needed if the sick driver is controlling power
  int timeout = 0;
  // true will print out packets as they come and go, false won't
  bool tracePackets = false;

  // The socket objects
  ArSocket masterSock, clientSock;
  // The connections
  ArTcpConnection clientConn;
  ArSerialConnection robotConn;
  // the receivers, first for the robot
  ArRobotPacketReceiver clientRec(&clientConn);
  ArRobotPacketReceiver robotRec(&robotConn);
  // then for the laser
  ArSickPacketReceiver clientSickRec(&clientConn, 0, false, true);
  ArSickPacketReceiver robotSickRec(&robotConn);
  // how about a packet
  ArBasePacket *packet;
  // our timer for how often we test the client
  ArTime lastClientTest;
  ArTime lastData;
  // where we're forwarding from and to
  int portNumber;
  const char *portName;
  // if we're using the robot or the laser
  bool useRobot;
  
  if (argc == 1)
  {
    printf("Using robot and port 8101 and serial connection %s, by default.\n", ArUtil::COM1);
    useRobot = true;
    portNumber = 8101;
    portName = ArUtil::COM1;
  }
  else if (argc == 2)
  {
    // if laser isn't the last arg, somethings wrong
    if (strcmp(argv[1], "laser") != 0)
    {
      usage(argv[0]);
      return -1;
    }
    useRobot = false;
    portNumber = 8102;
    portName = ArUtil::COM3;
    printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
    printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
  }
  else if (argc == 3)
  {
    if ((portNumber = atoi(argv[1])) <= 0)
    {
      usage(argv[0]);
      return -1;
    }
    portName = argv[2];
    printf("Using robot and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
  }
  else if (argc == 4)
  {
    if ((portNumber = atoi(argv[1])) <= 0)
    {
      usage(argv[0]);
      return -1;
    }
    // if laser isn't the last arg, somethings wrong
    if (strcmp(argv[3], "laser") != 0)
    {
      usage(argv[0]);
      return -1;
    }
    useRobot = false;
    portName = argv[2];
    printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
    printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
  }
  else
  {
    usage(argv[0]);
    return -1;
  }
  if (timeout == 0 && useRobot)
    timeout = 5000;
  else if (timeout == 0)
    timeout = 60000;

  // Initialize Aria. For Windows, this absolutely must be done. Because
  // Windows does not initialize the socket layer for each program. Each
  // program must initialize the sockets itself.
  Aria::init(Aria::SIGHANDLE_NONE);

  // Lets open the master socket
  if (masterSock.open(portNumber, ArSocket::TCP))
    printf("Opened the master port at %d\n", portNumber);
  else
//.........这里部分代码省略.........
开发者ID:sanyaade-research-hub,项目名称:aria,代码行数:101,代码来源:ipthru.cpp

示例10: main

int main(void)
{
  printf("\nTesting platform localtime (broken-down) struct:\n");
  //struct tm t;
  //ArUtil::localtime(&t);

  struct tm t;
  ArUtil::localtime(&t);
  printf("ArUtil::localtime() returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n",
    t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
  time_t yesterday = time(NULL) - (24*60*60) ;
  ArUtil::localtime(&yesterday, &t);
  printf("ArUtil::localtime(time(NULL) - 24hours, struct tm*) returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n",
    t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);

  printf("\nCurrent time as strings:\n");
  char year[5], month[3], day[3], hour[3], min[3], sec[3];
  ArUtil::putCurrentYearInString(year, 5);
  ArUtil::putCurrentMonthInString(month, 3);
  ArUtil::putCurrentDayInString(day, 3);
  ArUtil::putCurrentHourInString(hour, 3);
  ArUtil::putCurrentMinuteInString(min, 3);
  ArUtil::putCurrentSecondInString(sec, 3);
  printf("  Year:%s, Month:%s, Day:%s, Hour:%s, Min:%s, Sec:%s\n",
      year, month, day, hour, min, sec);

  printf("\nTesting ArTime:\n");
  ArTime start, test;
  
  printf("Setting an ArTime object \"start\" to now...\n");
  start.setToNow();
  start.log();
  printf("Sleeping 4 secs\n");
  ArUtil::sleep(4000);
  printf("Setting an ArTime object \"test\" to now...\n");
  test.setToNow();
  test.log();

  printf("ms of \"test\" since start %ld\n", test.mSecSince(start));
  printf("seconds \"test\" since start %ld\n", test.secSince(start));
  printf("ms of start since \"test\" %ld\n", start.mSecSince(test));
  printf("seconds \"start\" test %ld\n", start.secSince(test));
  printf("\"start\" is before \"test\"? %d\n", test.isBefore(start));
  printf("\"start\" is after \"test\"? %d\n", test.isAfter(start));
  printf("\"test\" is before \"start\"? %d\n", start.isBefore(test));
  printf("\"test\" is after \"start\"? %d\n", start.isAfter(test));
  printf("ms from \"start\" to now %ld\n", start.mSecTo());
  printf("s from \"start\" to now %ld\n", start.secTo());
  printf("ms since \"start\" %ld\n", start.mSecSince());
  printf("s since \"start\" %ld\n", start.secSince());
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("s from \"test\" stamp to now %ld\n", test.secTo());

  printf("Testing addMSec, adding 200 mSec\n");
  test.addMSec(200);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("Testing addMSec, subtracting 300 mSec\n");
  test.addMSec(-300);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("Testing addMSec, adding 20.999 seconds\n");
  test.addMSec(20999);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("Testing addMSec, subtracting 23.5 seconds\n");
  test.addMSec(-23500);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  
  ArTime timeDone;
  printf("Setting ArTime object \"done\" to now.\n");
  timeDone.setToNow();
  timeDone.addMSec(1000);
  printf("Making sure the add works in the right direction, adding a second to a timestamp set now\n");
  printf("Reading: %ld\n", timeDone.mSecTo());
  printf("Sleeping 20 ms\n");
  ArUtil::sleep(20);
  printf("Reading: %ld\n", timeDone.mSecTo());
  printf("Sleeping 2 seconds\n");
  ArUtil::sleep(2000);
  printf("Reading: %ld\n", timeDone.mSecTo());

  puts("\nslamming ArUtil::localtime() from a bunch of threads with the same input time...");
  time_t now = time(NULL);
  class LocaltimeTestThread : public virtual ArASyncTask 
  {
  private:
    time_t time;
  public:
    LocaltimeTestThread(time_t t) : time(t) {}
    virtual void *runThread(void *) {
      struct tm t;
      ArUtil::localtime(&time, &t);
      printf("ArUtil::localtime() returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n", t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
      return 0;
    }
  };
  for(int i = 0; i < 200; ++i)
    (new LocaltimeTestThread(now))->runAsync();

  ArUtil::sleep(5000);

  printf("test is done.\n");
//.........这里部分代码省略.........
开发者ID:sauver,项目名称:sauver_sys,代码行数:101,代码来源:timeTest.cpp

示例11: threadStarted

AREXPORT void * ArSyncLoop::runThread(void *arg)
{
  threadStarted();

  long timeToSleep;
  ArTime loopEndTime;
  std::list<ArFunctor *> *runList;
  std::list<ArFunctor *>::iterator iter;
  ArTime lastLoop;
  bool firstLoop = true;
  bool warned = false;

  if (!myRobot)
  {
    ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Trying to run the synchronous loop without a robot.");
    return(0);
  }

  if (!myRobot->getSyncTaskRoot())
  {
    ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Can not run the synchronous loop without a task tree");
    return(0);
  }

  while (myRunning)
  {

    myRobot->lock();
    if (!firstLoop && !warned && !myRobot->getNoTimeWarningThisCycle() && 
	myRobot->getCycleWarningTime() != 0 && 
	myRobot->getCycleWarningTime() > 0 && 
	lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
    {
      ArLog::log(ArLog::Normal, 
 "Warning: ArRobot cycle took too long because the loop was waiting for lock.");
      ArLog::log(ArLog::Normal,
		 "\tThe cycle took %u ms, (%u ms normal %u ms warning)", 
		 lastLoop.mSecSince(), myRobot->getCycleTime(), 
		 myRobot->getCycleWarningTime());
    }
    myRobot->setNoTimeWarningThisCycle(false);
    firstLoop = false;
    warned = false;
    lastLoop.setToNow();

    loopEndTime.setToNow();
    loopEndTime.addMSec(myRobot->getCycleTime());
    myRobot->incCounter();
    myRobot->unlock();

    // note that all the stuff beyond here should maybe have a lock
    // but it should be okay because its just getting data
    myInRun = true;
    myRobot->getSyncTaskRoot()->run();
    myInRun = false;
    if (myStopRunIfNotConnected && !myRobot->isConnected())
    {
      if (myRunning)
	ArLog::log(ArLog::Normal, "Exiting robot run because of lost connection.");
      break;
    }
    timeToSleep = loopEndTime.mSecTo();
    // if the cycles chained and we're connected the packet handler will be 
    // doing the timing for us
    if (myRobot->isCycleChained() && myRobot->isConnected())
      timeToSleep = 0;

    if (!myRobot->getNoTimeWarningThisCycle() && 
	myRobot->getCycleWarningTime() != 0 && 
	myRobot->getCycleWarningTime() > 0 && 
	lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
    {
      ArLog::log(ArLog::Normal, 
	"Warning: ArRobot sync tasks too long at %u ms, (%u ms normal %u ms warning)", 
		 lastLoop.mSecSince(), myRobot->getCycleTime(), 
		 myRobot->getCycleWarningTime());
      warned = true;
    }
    

    if (timeToSleep > 0)
      ArUtil::sleep(timeToSleep);
  }   
  myRobot->lock();
  myRobot->wakeAllRunExitWaitingThreads();
  myRobot->unlock();

  myRobot->lock();
  runList=myRobot->getRunExitListCopy();
  myRobot->unlock();
  for (iter=runList->begin();
       iter != runList->end(); ++iter)
    (*iter)->invoke();
  delete runList;

  threadFinished();
  return(0);
}
开发者ID:sanyaade-research-hub,项目名称:aria,代码行数:98,代码来源:ArSyncLoop.cpp

示例12: _tWinMain

int APIENTRY _tWinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPTSTR    lpCmdLine,
                     int       nCmdShow)
{
	UNREFERENCED_PARAMETER(hPrevInstance);
	UNREFERENCED_PARAMETER(lpCmdLine);

  //-------------- M A I N    D E L   P R O G R A M A   D E L    R O B O T------------//
  //----------------------------------------------------------------------------------------------------------
	
  //inicializaion de variables
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArSimpleConnector simpleConnector(&parser);
  ArRobot robot;
  ArSonarDevice sonar;
  ArAnalogGyro gyro(&robot);
  robot.addRangeDevice(&sonar);

  // presionar tecla escape para salir del programa
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // uso de sonares para evitar colisiones con las paredes u 
  // obstaculos grandes, mayores a 8cm de alto
  ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
  ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
  ArActionLimiterTableSensor tableLimiterAction;
  robot.addAction(&tableLimiterAction, 100);
  robot.addAction(&limiterAction, 95);
  robot.addAction(&limiterFarAction, 90);


  // Inicializon la funcion de goto
  ArActionGoto gotoPoseAction("goto");
  robot.addAction(&gotoPoseAction, 50);
  
  // Finaliza el goto si es que no hace nada
  ArActionStop stopAction("stop");
  robot.addAction(&stopAction, 40);

  // Parser del CLI
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    exit(1);
  }
  
  // Conexion del robot
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);
  }
  robot.runAsync(true);

  // enciende motores, apaga sonidos
  robot.enableMotors();
  robot.comInt(ArCommands::SOUNDTOG, 0);

  const int duration = 100000; //msec
  ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);

  bool first = true;
  int horiz = 1800;
  int vert = 380;
  int goalNum = 0;
  ArTime start;
  start.setToNow();
  while (Aria::getRunning()) 
  {
    robot.lock();
    // inicia el primer punto 
    if (first || gotoPoseAction.haveAchievedGoal())
    {
      first = false;
	  
      goalNum++; //cambia de 0 a 1 el contador
      if (goalNum > 7)
        goalNum = 1;

	  //comienza la secuencia de puntos
      if (goalNum == 1)
        gotoPoseAction.setGoal(ArPose(horiz, vert*0));
      else if (goalNum == 2)
        gotoPoseAction.setGoal(ArPose(0, vert*1));
      else if (goalNum == 3)
        gotoPoseAction.setGoa l(ArPose(horiz, vert*2)+5);
      else if (goalNum == 4)
        gotoPoseAction.setGoal(ArPose(0, vert*3));
	  else if (goalNum == 5)
        gotoPoseAction.setGoal(ArPose(horiz, vert*4+5));
	  else if (goalNum == 6)
        gotoPoseAction.setGoal(ArPose(0, vert*5));
	  else if (goalNum == 7)
        gotoPoseAction.setGoal(ArPose(0, vert*0));
//.........这里部分代码省略.........
开发者ID:eilo,项目名称:Evolucion-Artificial-y-Robotica-Autonoma-en-Robots-Pioneer-P3-DX,代码行数:101,代码来源:GoPos.cpp

示例13: while

/**
 * moveRobotTo() rotates the robot by the mentioned theta and displaces the robot
 * by the given distance in the new heading.
 * @args:
 * 	double r - the distance in Millimeter by which robot is to be moved. Default=1000.
 * 	double th - the angle in degrees by which the robot is to be rotated. Default=45
 * @return: None
 */
void BotConnector::moveRobot(double r, double th)
{
	ArTime start;

	robot.runAsync(true);
	robot.enableMotors();

	//rotating robot
	//    robot.lock();
	//    robot.setDeltaHeading(th);
	//    robot.unlock();
	//    ArUtil::sleep(10000);

	if( th!=0 )
	{
		robot.lock();
		robot.setHeading(th+robot.getTh());
		robot.unlock();
		start.setToNow();
		while (1)
		{
			robot.lock();
			if (robot.isHeadingDone(1))
			{
				printf("directMotionExample: Finished turn\n");
				robot.unlock();
				break;
			}
			if (start.mSecSince() > 15000)
			{
				printf("directMotionExample: Turn timed out\n");
				robot.unlock();
				break;
			}
			robot.unlock();
			ArUtil::sleep(100);
		}
	}

	//moving robot
	if( r!=0 )
	{
		robot.lock();
		robot.move(r);
		robot.unlock();
		start.setToNow();
		while (1)
		{
			robot.lock();
			if (robot.isMoveDone())
			{
				printf("directMotionExample: Finished distance\n");
				robot.unlock();
				break;
			}
			if (start.mSecSince() > 15000)
			{
				printf("directMotionExample: Distance timed out\n");
				robot.unlock();
				break;
			}
			robot.unlock();
			ArUtil::sleep(50);
		}
	}

	//    ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
	//    ArTime start;
	//    start.setToNow();
	//    while(Aria::getRunning())
	//    {
	//        if(robot.isHeadingDone())
	//        {
	//            robot.unlock();
	//            printf("turned in %ld\n",start.mSecSince());
	//            break;
	//        }
	//        gotoPoseAction->setGoal(pos);

	//        if(gotoPoseAction->haveAchievedGoal())
	//        if(start.mSecSince() >= duration)
	//        {
	//            gotoPoseAction->cancelGoal();
	//            printf("time out :(\n");
	//            break;
	//        }
	//        else if(gotoPoseAction->haveAchievedGoal())
	//        {
	//            gotoPoseAction->cancelGoal();
	//            printf("task in time %ld\n",start.mSecSince());
	//            break;
	//        }
//.........这里部分代码省略.........
开发者ID:krishnatejakvs,项目名称:Velocity-Obstacle-Method-on-Differential-Drive-Robots,代码行数:101,代码来源:test.cpp

示例14: strcasecmp

bool ArUrg_2_0::internalGetReading(void)
{
  ArTime readingRequested;
  std::string reading;
  char buf[1024];

  reading = "";
  /*
  if (!writeLine(myRequestString))
  {
    ArLog::log(ArLog::Terse, "Could not send request distance reading to urg");
    return false;
  }
  */

  ArTime firstByte;

  if (!readLine(buf, sizeof(buf), 1000, true, false, &firstByte) || 
      strcasecmp(buf, myRequestString) != 0)
  {
    ArLog::log(ArLog::Normal, 
	       "%s: Did not get distance reading response (%s)",
	       getName(), buf);
    return false;
  }
  // TODO this isn't the right time, but for most of what we do that
  // won't matter
  readingRequested.setToNow();
  readingRequested.addMSec(-100);

  if (!readLine(buf, sizeof(buf), 100, false, false) || 
      strcasecmp(buf, "99") != 0)		  
  {
    ArLog::log(ArLog::Normal, 
	       "%s: Bad status on distance reading response (%s)",
	       getName(), buf);
    return false;
  }

  if (!readLine(buf, sizeof(buf), 100, false, false))
  {
    ArLog::log(ArLog::Normal, 
       "%s: Could not read timestamp in distance reading response (%s)",
	       getName(), buf);
    return false;
  }
  
  while (readLine(buf, sizeof(buf), 100, false, false))
  {
    if (strlen(buf) == 0)
    {
      myReadingMutex.lock();
      myReadingRequested = readingRequested;
      myReading = reading;
      myReadingMutex.unlock();	
      if (myRobot == NULL)
	sensorInterp();
      return true;
    }
    else
    {
      reading += buf;
    }
  }

  return false;
}
开发者ID:sanyaade-research-hub,项目名称:aria,代码行数:67,代码来源:ArUrg_2_0.cpp

示例15: main

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());
//.........这里部分代码省略.........
开发者ID:YGskty,项目名称:avoid_side_Aria,代码行数:101,代码来源:sickSimpleTest.cpp


注:本文中的ArTime类示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。