C++ ArSerialConnection::setBaud方法代码示例

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

void SetupRobot(void)
{
	puts("attempting to connect to robot");
	RobotConnectoin.setPort("COM8");
	RobotConnectoin.setBaud(9600);
	robot.setDeviceConnection(&RobotConnectoin);
	if(!robot.blockingConnect()){puts("not connected to robot");Aria::shutdown();}
	robot.addRangeDevice(&sonarDev);
	robot.addRangeDevice(&bumpers);
	robot.enableMotors();
	robot.enableSonar();
	robot.requestEncoderPackets();
	robot.setCycleChained(false);
//	robot.setRotVelMax(robot.getRotVelMax());
}

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

// Create an ArGPS object. If some options were obtained from command-line
// parameters by parseArgs(), use those, otherwise get values from robot
// parameters (the .p file) if we have a valid robot with valid parameters.
AREXPORT ArGPS* ArGPSConnector::createGPS(ArRobot *robot)
{
  // If we have a robot with parameters (i.e. have connected and read the .p
  // file), use those values unless already set by parseArgs() from command-line 
  if(robot && robot->getRobotParams())
  {
    if(myPort == NULL) {
      myPort = robot->getRobotParams()->getGPSPort();
      if(strcmp(myPort, "COM1") == 0)
        myPort = ArUtil::COM1;
      if(strcmp(myPort, "COM2") == 0)
        myPort = ArUtil::COM2;
      if(strcmp(myPort, "COM3") == 0)
        myPort = ArUtil::COM3;
      if(strcmp(myPort, "COM4") == 0)
        myPort = ArUtil::COM4;
    }
    if(myBaud == -1) {
      myBaud = robot->getRobotParams()->getGPSBaud();
    }
    if(myDeviceType == Invalid) {
      myDeviceType = deviceTypeFromString(robot->getRobotParams()->getGPSType());
    }
  }
  else
  {
    if(myPort == NULL) myPort = ARGPS_DEFAULT_SERIAL_PORT;
    if(myBaud == -1) myBaud = ARGPS_DEFAULT_SERIAL_BAUD;
    if(myDeviceType == Invalid) myDeviceType = Standard;
  }

  // Create gps:
  ArGPS* newGPS = NULL;
  switch (myDeviceType)
  {
    case Novatel:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using Novatel GPS");
      newGPS = new ArNovatelGPS;
      break;
    case Trimble:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using Trimble GPS");
      newGPS = new ArTrimbleGPS;
      break;
    case NovatelSPAN:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using Novatel SPAN GPS");
      newGPS = new ArNovatelSPAN;
      break;
    default:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using standard NMEA GPS");
      newGPS = new ArGPS;
      break;
  }

  if (myTCPHost == NULL)
  {
    // Setup serial connection
    ArSerialConnection *serialCon = new ArSerialConnection;
    ArLog::log(ArLog::Normal, "ArGPSConnector: Connecting to GPS on port %s at %d baud...", myPort, myBaud);
    if (!serialCon->setBaud(myBaud)) { delete serialCon; return false; }
    if (serialCon->open(myPort) != 0) {
      ArLog::log(ArLog::Terse, "ArGPSConnector: Error: could not open GPS serial port %s.", myPort);
      delete serialCon;
      return NULL;
    }
    newGPS->setDeviceConnection(serialCon);
    myDeviceCon = serialCon;
  }
  else
  {
    // Setup TCP connection
    ArTcpConnection *tcpCon = new ArTcpConnection;
    ArLog::log(ArLog::Normal, "ArGPSConnector: Opening TCP connection to %s:%d...", myTCPHost, myTCPPort);
    int openState = tcpCon->open(myTCPHost, myTCPPort);
    if (openState != 0) {
      ArLog::log(ArLog::Terse, "ArGPSConnector: Error: could not open TCP connection to %s port %d: %s", tcpCon->getOpenMessage(openState));
      delete tcpCon;
      return NULL;
    }
    newGPS->setDeviceConnection(tcpCon);
    myDeviceCon = tcpCon;
  }

  return newGPS;
}

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

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

AREXPORT bool ArUrg_2_0::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 we have a starting baud and are a serial port, then change the
  // baud rate... not by default this will set it to 0 baud which'll
  // cause the serial stuff not to touch it
  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();
  
  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;
  }
}

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

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