コード例 #1
0
int main(void)
{
  int ret;
  std::string str;
  CBTest cbTest;

  ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected);
  ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect);
  ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected);
  ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError);

  ArSerialConnection con;
  ArRobot robot;

  printf("If a robot is attached to your port you should see:\n");
  printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n");
  printf("If no robot is attached you should see:\n");
  printf("Failed connect, Failed connect, Failed connect\n");
  printf("-------------------------------------------------------\n");
  ArLog::init(ArLog::None, ArLog::Terse);

  srand(time(NULL));

  robot.setDeviceConnection(&con);
  robot.addConnectCB(&connectCB, ArListPos::FIRST);
  robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
  robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST);
  robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST);
  
  // this should fail since there isn't an open port yet
  robot.blockingConnect();
  
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    exit(0);
  }
  
  robot.blockingConnect();

  con.close();
  robot.loopOnce();
  

  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    exit(0);
  }  
  robot.blockingConnect();
  robot.disconnect();

  exit(0);

}
コード例 #2
0
int main(int argc, char **argv)
{
  std::string str;
  int ret;
  int successes = 0, failures = 0;
  int action;
  bool exitOnFailure = true;
  
  ArSerialConnection con;
  ArRobot robot;
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  srand(time(NULL));
  robot.runAsync(false);
// if (!exitOnFailure)
//    ArLog::init(ArLog::None, ArLog::Terse);
  //else
  //ArLog::init(ArLog::None);
  while (1)
  {
    if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
	(ret = con.open()) != 0)
    {
      str = con.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      ++failures;
      if (exitOnFailure)
      {
	printf("Failed\n");
	exit(0);
      }
      else
      {
	ArUtil::sleep(200);
	robot.unlock();
	continue;
      }
    }
    robot.lock();
    robot.setDeviceConnection(&con);
    robot.unlock();
    ArUtil::sleep((rand() % 5) * 100);
    if (robot.asyncConnect())
    {
      robot.waitForConnectOrConnFail();
      robot.lock();
      if (!robot.isConnected())
      {
	if (exitOnFailure)
	{
	  printf("Failed after %d tries.\n", successes);
	  exit(0);
	}
	printf("Failed to connect successfully");
	++failures;
      }
      robot.comInt(ArCommands::SONAR, 0);
      robot.comInt(ArCommands::SOUNDTOG, 0);
      //robot.comInt(ArCommands::PLAYLIST, 0);
      robot.comInt(ArCommands::ENCODER, 1);
      ArUtil::sleep(((rand() % 20) + 3) * 100);
      ++successes;
      // okay, now try to leave it in a messed up state
      action = rand() % 8;
      robot.dropConnection();
      switch (action) {
      case 0:
	printf("Discon  0 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	break;
      case 1:
	printf("Discon  1 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	break;
      case 2:
	printf("Discon  2 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	ArUtil::sleep(100);
	robot.com(2);
	break;
      case 3:
	printf("Discon 10 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(10);
	break;
      case 4:
	printf("Discon    ");
	robot.disconnect();
	break;
      default:
	printf("Leave     ");
	break;
      }
      robot.unlock();
    }
    else
    {
      if (exitOnFailure)
      {
	printf("Failed after %d tries.\n", successes);
	exit(0);
      }
      printf("Failed to start connect ");
      ++failures;
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! RadioDisconnect ! ");
      con.write("|||\15", strlen("!!!\15"));
      
      ArUtil::sleep(100);
      con.write("WMD\15", strlen("WMD\15"));
      ArUtil::sleep(200);
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! ClosePort !\n");
      con.close();
    }
    else
      printf("\n");
    printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
	   (float)successes/(float)(successes+failures)*100);

    ArUtil::sleep((rand() % 2)* 1000);
  }
  return 0; 
}
コード例 #3
0
ファイル: ipthru.cpp プロジェクト: sanyaade-research-hub/aria
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
  {
    printf("Failed to open the master port at %d: %s\n",
	   portNumber, masterSock.getErrorStr().c_str());
    return -1;
  }

  // just go forever
  while (1)
  {
    // Lets wait for the client to connect to us.
    if (masterSock.accept(&clientSock))
      printf("Client has connected\n");
    else
      printf("Error in accepting a connection from the client: %s\n",
	     masterSock.getErrorStr().c_str());
   
    // now set up our connection so our packet receivers work
    clientConn.setSocket(&clientSock);
    clientConn.setStatus(ArDeviceConnection::STATUS_OPEN);
    lastClientTest.setToNow();
    lastData.setToNow();
    // open up the robot port
    if (robotConn.open(portName) != 0)
    {
      printf("Could not open robot port %s.\n", portName);
      return -1;
    }

    // while we're open, just read from one port and write to the other
    while (clientSock.getFD() >= 0)
    {
      // get our packet
      if (useRobot)
	packet = clientRec.receivePacket(1);
      else
	packet = clientSickRec.receivePacket(1);
      // see if we had one
      if (packet != NULL)
      {
	if (tracePackets)
	{
	  printf("Client ");
	  packet->log();
	}
	robotConn.writePacket(packet);
	lastData.setToNow();
      }
      // get our packet
      if (useRobot)
	packet = robotRec.receivePacket(1);
      else
	packet = robotSickRec.receivePacket(1);
      // see if we had one
      if (packet != NULL)
      {
	if (tracePackets)
	{
	  printf("Robot ");
	  packet->log();
	}
	clientConn.writePacket(packet);
	lastData.setToNow();
      }
      ArUtil::sleep(1);
      // If no datas gone by in timeout ms assume our connection is broken
      if (lastData.mSecSince() > timeout)
      {
	printf("No data received in %d milliseconds, closing connection.\n", 
	       timeout);
	clientConn.close();
      }
    }
    // Now lets close the connection to the client
    clientConn.close();
    printf("Socket to client closed\n");
    robotConn.close();
  }
  // And lets close the master port
  masterSock.close();
  printf("Master socket closed and program exiting\n");

  // Uninitialize Aria
  Aria::uninit();

  // All done
  return(0);
}