C++ ArSerialConnection类代码示例

int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  
  ArGlobalRetFunctor1<bool, ArRobotPacket *> tcm2PrinterCB(&tcm2Printer);
  ArSerialConnection con;
  Aria::init();
  
  robot = new ArRobot;

  robot->addPacketHandler(&tcm2PrinterCB, ArListPos::FIRST);
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    exit(0);
  }

  robot->setDeviceConnection(&con);
  if (!robot->blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    exit(0);
  }
  
  printf("%6s %6s %6s %6s %6s %6s %6s %10s %4s %4s %6s\n", 
	 "comp", "pitch", "roll", "magX", "magY", "magZ", "temp", "error",
	 "calH", "calV", "calM");
	 robot->comInt(ArCommands::TCM2, 3);

  robot->run(true);
  Aria::shutdown();
  
}

AREXPORT ArTCMCompassDirect::ArTCMCompassDirect(const char *serialPortName) :
  myCreatedOwnDeviceConnection(true),
  myNMEAParser("ArTCMCompassDirect"),
  myHCHDMHandler(this, &ArTCMCompassDirect::handleHCHDM)
{
  ArSerialConnection *newSerialCon = new ArSerialConnection();
  newSerialCon->setPort(serialPortName);
  newSerialCon->setBaud(9600);
  myDeviceConnection = newSerialCon;
  myNMEAParser.addHandler("HCHDM", &myHCHDMHandler);
}

int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;

  ArSerialConnection serialConnection;
  ArTcpConnection tcpConnection;
    
  if (tcpConnection.open("localhost", 8101)) {
    robot.setDeviceConnection(&tcpConnection);
  } else {
    serialConnection.setPort("/dev/ttyUSB0");
    robot.setDeviceConnection(&serialConnection);
  }
  robot.blockingConnect();
   
  printf("Setting robot to run async\n");
  robot.runAsync(false);

  printf("Turning off sound\n");
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Enabling motors\n");
  robot.enableMotors();

  // add a set of actions that combine together to effect the wander behavior
  /*ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;
  ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 75);
  robot.addAction(&avoidFrontNear, 50);
  robot.addAction(&avoidFrontFar, 49);
  robot.addAction(&constantVelocity, 25);*/

  printf("Locking\n");
  robot.lock();
  robot.setVel(100.0);
  robot.unlock();
  printf("Sleeping\n");
  ArUtil::sleep(3*1000);
  printf("Awake\n");

  
  // wait for robot task loop to end before exiting the program
  //while (true);
  //robot.waitForRunExit();
  

  Aria::exit(0);
  return 0;
}

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);

  int i;
  while (Aria::getRunning())
  {
    robot.lock();
    robot.comStr(ArCommands::TTY3, "1234567890");
    robot.unlock();
  }

  robot.disconnect();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv)
{
    // just some stuff for returns
    std::string str;
    int ret;

    // robots connection
    ArSerialConnection con;
    // the robot, this turns state reflection off
    ArRobot robot(NULL, false);
    // the joydrive as defined above, this also adds itself as a user task
    Joydrive joyd(&robot);

    // mandatory init
    Aria::init();

    // 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;
    }

    // set the connection o nthe robot
    robot.setDeviceConnection(&con);
    // connect, if we fail, exit
    if (!robot.blockingConnect())
    {
        printf("Could not connect to robot... exiting\n");
        Aria::shutdown();
        return 1;
    }

    // turn off the sonar, enable the motors, turn off amigobot sounds
    robot.comInt(ArCommands::SONAR, 0);
    robot.comInt(ArCommands::ENABLE, 1);
    robot.comInt(ArCommands::SOUNDTOG, 0);

    // run, if we lose connection to the robot, exit
    robot.run(true);

    // shutdown and go away
    Aria::shutdown();
    return 0;
}

int main(int argc, char **argv)
{
  Aria::init();
  ArLog::init(ArLog::StdErr, ArLog::Normal);
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }
  
  ArSerialConnection con;
  con.setPort(ArUtil::COM4);
  con.setBaud(19200);
  if(!con.openSimple())
    ArLog::log(ArLog::Terse, "could not open COM4");

  char buf[512];
  
  while (true)
  {
    int n = con.read(buf, 512, 10);
    if(n < 0)
    {
      ArLog::log(ArLog::Terse, "Error reading.");
      Aria::exit(n);
    }
    if(n == 0)
      continue;

    // log for debugging:
    char cmd = 0;
    int x = 0;
    int size = 0;
    for(int i = 0; i < n; ++i)
    {
      if(buf[i] == 0xc1 || buf[i] == 0x5a) puts("");
      printf("0x%hhX (%u)    ", buf[i], buf[i]);
  }
  
   }   
    

  Aria::exit(0);
}

int main(void)
{
  
  ArSerialConnection serConn;

  if (serConn.open(ArUtil::COM1) != 0)
  {
    printf("Could not open serial port\n");
    exit(1);
  }

  while (1)
  {
    printf("\rDCD %d", serConn.getDCD());
    fflush(stdout);
  }
  return 0;
}

int main()
{
  ArModuleLoader::Status status;
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;

  Aria::init();

  status=ArModuleLoader::load("./joydriveActionMod", &robot);
  printStatus(status);

  if (status == ArModuleLoader::STATUS_INIT_FAILED)
    return(1);

  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  robot.run(true);
  
  status=ArModuleLoader::close("./joydriveActionMod");
  printStatus(status);

  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv) 
{
  // just some stuff for returns
  std::string str;
  // robots connection
  ArSerialConnection con;

  // the robot, this turns state reflection off
  ArRobot robot(NULL, false);
  // the joydrive as defined above, this also adds itself as a user task
  KeyPTU ptu(&robot);

  // mandatory init
  Aria::init();

  ArLog::init(ArLog::StdOut, ArLog::Terse, NULL, true);

  con.setPort(ArUtil::COM1);
  // set the connection on the robot
  robot.setDeviceConnection(&con);

  // connect, if we fail, exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // turn off the sonar, enable the motors, turn off amigobot sounds
  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Press '?' for available commands\r\n");
  // run, if we lose connection to the robot, exit
  robot.run(true);
  
  // shutdown and go away
  Aria::shutdown();
  return 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);

}

bool ArUrg::internalConnect(void)

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

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

  bool alreadyAtAutobaud = false;

  // empty the buffer...
  buf[0] = '\0';
  while (readLine(buf, sizeof(buf), 1));

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

      if (!(ret = sendCommandAndRecvStatus(
		"V", "version request after falling back 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 falling back to autobaudchoice", 
		     getName());
	return false;
      }
    }
    // if we don't have a serial port, then we can't change the baud,
    // so just fail
    else
    {
      if (ret && strcasecmp(buf, "0") != 0)      
	ArLog::log(ArLog::Normal, 
		   "%s::blockingConnect: Bad status on version response (%s)",
		   getName(), buf);
      return false;
    }
  }

  if (!alreadyAtAutobaud && serConn != NULL)
  {

    // 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)
//.........这里部分代码省略.........

AREXPORT bool ArUrg::blockingConnect(void)
{
  if (!getRunning())
    runAsync();

  myConnMutex.lock();
  if (myConn == NULL)
  {
    ArLog::log(ArLog::Terse, 
	       "%s: Could not connect because there is no connection defined", 
	       getName());
    myConnMutex.unlock();
    failedToConnect();
    return false;
  }

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

  if (serConn != NULL)
    serConn->setBaud(atoi(getStartingBaudChoice()));

  if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN && 
      !myConn->openSimple())
  {
    ArLog::log(ArLog::Terse, 
	       "%s: Could not connect because the connection was not open and could not open it", 
	       getName());
    myConnMutex.unlock();
    failedToConnect();
    return false;
  }
  myConnMutex.unlock();

  lockDevice();
  myTryingToConnect = true;
  unlockDevice();

  laserPullUnsetParamsFromRobot();
  laserCheckParams();
  
  setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
  
  ArUtil::sleep(100);

  bool connected = false;

  if (internalConnect())
    connected = true;

  if (connected)
  {
    lockDevice();
    myIsConnected = true;
    myTryingToConnect = false;
    unlockDevice();
    ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
    laserConnect();
    return true;
  }
  else
  {
    failedToConnect();
    return false;
  }
}

int main(int argc, char **argv)
{
  int ret;
  std::string str;
  // the serial connection (robot)
  ArSerialConnection serConn;
  // tcp connection (sim)
  ArTcpConnection tcpConn;
  // the robot
  ArRobot robot;
  // the laser
  ArSick sick;
  // the laser connection
  ArSerialConnection laserCon;

  bool useSimForLaser = false;


  std::string hostname = "prod.local.net";

  // timeouts in minutes
  int wanderTime = 0;
  int restTime = 0;


  // check arguments
  if (argc == 3 || argc == 4)
  {
    wanderTime = atoi(argv[1]);
    restTime = atoi(argv[2]);
    if (argc == 4)
      hostname = argv[3];
  }
  else
  {
    printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n");
    printf("Times are in minutes.  Hostname is the machine to pipe the ACTS display to\n\n");
    wanderTime = 15;
    restTime = 45;
  }

  printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime);
  printf("Sending display to %s.\n\n", hostname.c_str());

  // sonar, must be added to the robot
  ArSonarDevice sonar;

  // the actions we'll use to wander
  ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;

  // mandatory init
  Aria::init();

  // Add the key handler to Aria so other things can find it
  Aria::setKeyHandler(&keyHandler);

  // Attach the key handler to a robot now, so that it actually gets
  // some processing time so it can work, this will also make escape
  // exit
  robot.attachKeyHandler(&keyHandler);


  // First we see if we can open the tcp connection, if we can we'll
  // assume we're connecting to the sim, and just go on...  if we
  // can't open the tcp it means the sim isn't there, so just try the
  // robot

  // modify this next line if you're not using default tcp connection
  tcpConn.setPort();

  // see if we can get to the simulator  (true is success)
  if (tcpConn.openSimple())
  {
    // we could get to the sim, so set the robots device connection to the sim
    printf("Connecting to simulator through tcp.\n");
    robot.setDeviceConnection(&tcpConn);
  }
  else
  {
    // we couldn't get to the sim, so set the port on the serial
    // connection and then set the serial connection as the robots
    // device

    // modify the next line if you're not using the first serial port
    // to talk to your robot
    serConn.setPort();
    printf(
      "Could not connect to simulator, connecting to robot through serial.\n");
    robot.setDeviceConnection(&serConn);
  }
  
  
  // add the sonar to the robot
//.........这里部分代码省略.........

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:
//.........这里部分代码省略.........

AREXPORT bool ArSZSeries::blockingConnect(void) {

	if (!getRunning())
		runAsync();

	myConnMutex.lock();
	if (myConn == NULL) {
		ArLog::log(ArLog::Terse,
				"%s: Could not connect because there is no connection defined",
				getName());
		myConnMutex.unlock();
		failedToConnect();
		return false;
	}


	//				myPrevSensorIntTime = myConn->getTimeRead(0);

	// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
	laserPullUnsetParamsFromRobot();
	laserCheckParams();

	// PS 9/9/11 - add setting baud
    ArSerialConnection *serConn = NULL;
	serConn = dynamic_cast<ArSerialConnection *>(myConn);
	if (serConn != NULL)
		serConn->setBaud(atoi(getStartingBaudChoice()));

	if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN
			&& !myConn->openSimple()) {
		ArLog::log(
				ArLog::Terse,
				"%s: Could not connect because the connection was not open and could not open it",
				getName());
		myConnMutex.unlock();
		failedToConnect();
		return false;
	}

	// PS - set logging level and laser type in packet receiver class
	myReceiver.setmyInfoLogLevel(myInfoLogLevel);
	myReceiver.setmyName(getName());

	myReceiver.setDeviceConnection(myConn);
	myConnMutex.unlock();

	lockDevice();
	myTryingToConnect = true;
	unlockDevice();

	// PS 9/9/11 - moved up top
	//laserPullUnsetParamsFromRobot();
	//laserCheckParams();

	int size = ArMath::roundInt((270/.3) + 1);
	ArLog::log(myInfoLogLevel,
			"%s::blockingConnect() Setting current buffer size to %d",
			getName(), size);
	setCurrentBufferSize(size);

	ArTime timeDone;
	if (myPowerControlled)
	{
		if (!timeDone.addMSec(60 * 1000))
		{
			ArLog::log(ArLog::Normal,
					"%s::blockingConnect() error adding msecs (60 * 1000)",
					getName());
		}
	}
	else
	{
		if (!timeDone.addMSec(30 * 1000))
		{
			ArLog::log(ArLog::Normal,
					"%s::blockingConnect() error adding msecs (30 * 1000)",
					getName());
		}
	}


	ArSZSeriesPacket *packet;

	ArSZSeriesPacket sendPacket;

#if 0
	sendPacket.empty();
	sendPacket.uByteToBuf(0xA0); // stop continous sending
	sendPacket.uByteToBuf(0x00);
	sendPacket.uByteToBuf(0x1D);
	sendPacket.uByteToBuf(0x7E);

	sendPacket.finalizePacket();

	if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
	{
		ArLog::log(ArLog::Terse,
				"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
		failedToConnect();
		return false;
//.........这里部分代码省略.........