示例#1
0
AREXPORT void ArLaser::laserConnect(void)
{
  // figure out how many readings we can have and set the current
  // buffer size to that
  double degrees;

  myLastReading.setToNow();

  if (canSetDegrees())
  {
    //degrees = fabs(ArMath::subAngle(getStartDegrees(), getEndDegrees()));
    degrees = fabs(getStartDegrees() - getEndDegrees());
    ArLog::log(myInfoLogLevel, 
	       "%s: Using degrees settings of %g to %g for %g degrees",
	       getName(), getStartDegrees(), getEndDegrees(), 
	       degrees);
  }
  else if (canChooseDegrees())
  {
    degrees = getDegreesChoiceDouble();
    ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
	       getName(), degrees);
  }
  else
  {
    degrees = 360;
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
  }

  double increment;
  if (canSetIncrement())
  {
    increment = getIncrement();
    ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
	       getName(), increment);
  }
  else if (canChooseIncrement())
  {
    increment = getIncrementChoiceDouble();
    ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
	       getName(), increment);
  }
  else
  {
	// PS 10/20/11 - This was missing causing buffer size to be very large
	// set this to the lowest, note both the SZ and S3 are setting the buffer
	// size but it's being overriden by this procedure - do we want to fix
	// this or just leave it at the max value 360/.25=1440???
	increment = .25;
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for increment, arbitrarily using .25", getName());
  }
  
  int size = (int)ceil(degrees / increment) + 1;
  ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d", 
	     getName(), size);
  setCurrentBufferSize(size);
  
  ArLog::log(myInfoLogLevel, "%s: Connected", getName());
  myConnectCBList.invoke();
}
示例#2
0
AREXPORT void ArLaser::laserConnect(void)
{
  // figure out how many readings we can have and set the current
  // buffer size to that
  double degrees;

  myLastReading.setToNow();

  if (canSetDegrees())
  {
    degrees = fabs(getStartDegrees() - getEndDegrees());
    ArLog::log(myInfoLogLevel, 
	       "%s: Using degrees settings of %g to %g for %g degrees",
	       getName(), getStartDegrees(), getEndDegrees(), 
	       degrees);
  }
  else if (canChooseDegrees())
  {
    degrees = getDegreesChoiceDouble();
    ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
	       getName(), degrees);
  }
  else
  {
    degrees = 360;
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
  }

  double increment;
  if (canSetIncrement())
  {
    increment = getIncrement();
    ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
	       getName(), increment);
  }
  else if (canChooseIncrement())
  {
    increment = getIncrementChoiceDouble();
    ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
	       getName(), increment);
  }
  else
  {
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 1000 for current buffer size", getName());
  }
  
  int size = (int)ceil(degrees / increment) + 1;
  ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d", 
	     getName(), size);
  setCurrentBufferSize(size);
  
  ArLog::log(myInfoLogLevel, "%s: Connected", getName());
  myConnectCBList.invoke();
}
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);
    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, true, false));
    
    if (!(ret = sendCommandAndRecvStatus(
		  "VV", "version request after switching 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 switching to autobaudchoice", 
		   getName());
      return false;
    }

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

  while (readLine(buf, sizeof(buf), 10000, false, true))
  {
    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;
  }

  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;
}
示例#5
0
void ArLMS1XX::sensorInterp(void)
{
  ArLMS1XXPacket *packet;
  
  while (1)
  {
    myPacketsMutex.lock();
    if (myPackets.empty())
    {
      myPacketsMutex.unlock();
      return;
    }
    packet = myPackets.front();
    myPackets.pop_front();
    myPacketsMutex.unlock();
	   
    // if its not a reading packet just skip it 
    if (strcasecmp(packet->getCommandName(), "LMDscandata") != 0)
    {
      delete packet;
      continue;
    }

    //set up the times and poses

    ArTime time = packet->getTimeReceived();
    ArPose pose;
    int ret;
    int retEncoder;
    ArPose encoderPose;
    
    // this value should be found more empirically... but we used 1/75
    // hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
    time.addMSec(-20);
    if (myRobot == NULL || !myRobot->isConnected())
    {
      pose.setPose(0, 0, 0);
      encoderPose.setPose(0, 0, 0);
    } 
    else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
	     (retEncoder = 
	      myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
    {
      ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
      delete packet;
      continue;
    }
    
    ArTransform transform;
    transform.setTransform(pose);
    
    unsigned int counter = 0; 
    if (myRobot != NULL)
      counter = myRobot->getCounter();
    
    lockDevice();
    myDataMutex.lock();
    
    int i;
    int dist;
    //int onStep;
    
    std::list<ArSensorReading *>::reverse_iterator it;
    ArSensorReading *reading;

    // read the extra stuff
    myVersionNumber = packet->bufToUByte2();
    myDeviceNumber = packet->bufToUByte2();
    mySerialNumber = packet->bufToUByte4();
    myDeviceStatus1 = packet->bufToUByte();
    myDeviceStatus2 = packet->bufToUByte();
    myMessageCounter = packet->bufToUByte2();
    myScanCounter = packet->bufToUByte2();
    myPowerUpDuration = packet->bufToUByte4();
    myTransmissionDuration = packet->bufToUByte4();
    myInputStatus1 = packet->bufToUByte();
    myInputStatus2 = packet->bufToUByte();
    myOutputStatus1 = packet->bufToUByte();
    
    myOutputStatus2 = packet->bufToUByte();
    myReserved = packet->bufToUByte2();
    myScanningFreq = packet->bufToUByte4();
    myMeasurementFreq = packet->bufToUByte4();

    if (myDeviceStatus1 != 0 || myDeviceStatus2 != 0)
      ArLog::log(myLogLevel, "%s: DeviceStatus %d %d", 
		 myDeviceStatus1, myDeviceStatus2); 

    /*
      printf("Received: %s %s ver %d devNum %d serNum %d scan %d sf %d mf %d\n", 
	   packet->getCommandType(), packet->getCommandName(), 
	   myVersionNumber, myDeviceNumber, 
	   mySerialNumber, myScanCounter, myScanningFreq, myMeasurementFreq);
    */
    myNumberEncoders = packet->bufToUByte2();
    //printf("\tencoders %d\n", myNumberEncoders);
    if (myNumberEncoders > 0)
      ArLog::log(myLogLevel, "%s: Encoders %d", getName(), myNumberEncoders);

    for (i = 0; i < myNumberEncoders; i++)
    {
      packet->bufToUByte4();
      packet->bufToUByte2();
      //printf("\t\t%d\t%d %d\n", i, eachEncoderPosition, eachEncoderSpeed);
    }

    myNumChans = packet->bufToUByte2();
    if (myNumChans > 1)
      ArLog::log(myLogLevel, "%s: NumChans %d", getName(), myNumChans);
    //printf("\tnumchans %d\n", myNumChans);

    char eachChanMeasured[1024];
    int eachScalingFactor;
    int eachScalingOffset;
    double eachStartingAngle;
    double eachAngularStepWidth;
    int eachNumberData;


    for (i = 0; i < myNumChans; i++)
    {
      eachChanMeasured[0] = '\0';
      packet->bufToStr(eachChanMeasured, sizeof(eachChanMeasured));
      
      // if this isn't the data we want then skip it
      if (strcasecmp(eachChanMeasured, "DIST1") != 0 &&
	  strcasecmp(eachChanMeasured, "DIST2") != 0)
	continue;

      eachScalingFactor = packet->bufToUByte4(); // FIX should be real
      eachScalingOffset = packet->bufToUByte4(); // FIX should be real
      eachStartingAngle = packet->bufToByte4() / 10000.0;
      eachAngularStepWidth = packet->bufToUByte2() / 10000.0;
      eachNumberData = packet->bufToUByte2();

      /*
      ArLog::log(myLogLevel, "%s: %s start %.1f step %.2f numReadings %d", 
		 getName(), eachChanMeasured,
		 eachStartingAngle, eachAngularStepWidth, eachNumberData);
      */

      /*
      printf("\t\t%s\tscl %d %d ang %g %g num %d\n", 
	     eachChanMeasured, 
	     eachScalingFactor, eachScalingOffset, 
	     eachStartingAngle, eachAngularStepWidth, 
	     eachNumberData);
      */
      // If we don't have any sensor readings created at all, make 'em all 
      if (myRawReadings->size() == 0)
	for (i = 0; i < eachNumberData; i++)
	  myRawReadings->push_back(new ArSensorReading);

      if (eachNumberData > myRawReadings->size())
      {
	ArLog::log(ArLog::Terse, "%s: Bad data, in theory have %d readings but can only have 541... skipping this packet\n", getName(), eachNumberData);
	printf("%s\n", packet->getBuf());
	continue;
      }
      
      std::list<ArSensorReading *>::iterator it;
      double atDeg;
      int onReading;

      double start;
      double increment;
      
      if (myFlipped)
      {
	start = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData;
	increment = -eachAngularStepWidth;
      }
      else
      {
	start = mySensorPose.getTh() + eachStartingAngle - 90.0;
	increment = eachAngularStepWidth;
      }
	
      bool ignore;
      for (//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0,
	   //atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData,
	   atDeg = start,
	   it = myRawReadings->begin(),
	   onReading = 0; 
	   
	   onReading < eachNumberData; 
	   
	   //atDeg += eachAngularStepWidth,
	   //atDeg -= eachAngularStepWidth,
	   atDeg += increment,
	   it++,
	   onReading++)
      {
	ignore = false;

	if (atDeg < getStartDegrees() || atDeg > getEndDegrees())
	  ignore = true;

	reading = (*it);
	dist = packet->bufToUByte2();

	if (dist == 0)
	{
	  ignore = true;
	}
	
	/*
	else if (!ignore && dist < 150)
	{
	  //ignore = true;

	  ArLog::log(ArLog::Normal, "%s: Reading at %.1f %s is %d (not ignoring, just warning)", 
		     getName(), atDeg, 
		     eachChanMeasured, dist);
	}
	*/

	reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
				     ArMath::roundInt(mySensorPose.getY()),
				     atDeg); 
	reading->newData(dist, pose, encoderPose, transform, counter, 
			 time, ignore, 0); // no reflector yet
      }
      /*
      ArLog::log(ArLog::Normal, 
		 "Received: %s %s scan %d numReadings %d", 
		 packet->getCommandType(), packet->getCommandName(), 
		 myScanCounter, onReading);
      */
    }
    
    myDataMutex.unlock(); 

    
    laserProcessReadings();
    unlockDevice();
    delete packet;
  }
}