C++ ArSensorReading类代码示例

/**
 * @brief publish sonar readings
 */
void AriaCore::publish_sonar()
{
    sensor_msgs::PointCloud msg;

    msg.header.stamp    = ros::Time::now();
    msg.header.frame_id = m_frameIDSonar;

    m_robot.lock();
    {
        int i;
        ArSensorReading* pRead;
        for (i=0; i<m_robot.getNumSonar(); ++i) {
            pRead = m_robot.getSonarReading(i);
            if (!pRead) {
                ROS_WARN("Did not receive a sonar reading on %d", i);
                continue;
            }

            geometry_msgs::Point32 point;
            point.x = pRead->getLocalX();
            point.y = pRead->getLocalY();
            point.z = 0.0;

            msg.points.push_back(point);
        }
    }
    m_robot.unlock();

    m_pubSonar.publish(msg);
}

void Mapping::keepRendering()
{
    resetMap();
    while(run)
    {
        if(lasers != NULL)
        {
            lasers->clear();
            delete lasers;
        }
        mRobot->readingSensors();
        lasers = mRobot->getLaserRanges();
        sonares = mRobot->getSonarRanges();
        sensores = sonares;

        ArSensorReading ar = sensores->at(0); //Apenas para pegar as informações de posição do robo no momento da tomada de informações.
        updateRoboPosition(ar.getXTaken(),ar.getYTaken(),ar.getThTaken());
        switch(this->technique)
        {
        case MappingTechnique::DeadReckoning:
            calculateMapDeadReckoning();
            break;
        case MappingTechnique::BAYES:
            calculateMapBayes();
            break;
        case MappingTechnique::HIMM:
            calculateMapHIMM();
            break;
        }
        ArUtil::sleep(33);
    }
    thread->exit();
    thread->wait();
}

void laserRequest_and_odom(ArServerClient *client, ArNetPacket *packet)
{ 
  robot.lock();
  ArNetPacket sending;
  sending.empty();
  ArLaser* laser = robot.findLaser(1);
  if(!laser){
      printf("Could not connect to Laser... exiting\n");
      Aria::exit(1);}	
  laser->lockDevice();
  const std::list<ArSensorReading*> *sensorReadings = laser->getRawReadings(); // see ArRangeDevice interface doc
  sending.byte4ToBuf((ArTypes::Byte4)(sensorReadings->size()));
  for (std::list<ArSensorReading*>::const_iterator it2= sensorReadings->begin(); it2 != sensorReadings->end(); ++it2){
	ArSensorReading* laserRead =*it2;
        sending.byte4ToBuf((ArTypes::Byte4)(laserRead->getRange()));
	//printf("%i,%i:",laserRead->getRange(),laserRead->getIgnoreThisReading());
  }
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getX()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getY()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getTh()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getVel()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getRotVel()));
  //printf("%1f,%1f,%1f\n",robot.getX(),robot.getY(),robot.getTh());
  laser->unlockDevice();
  robot.unlock();
  sending.finalizePacket();
  //sending.printHex();
  client->sendPacketTcp(&sending);
}

AREXPORT void ArLaserReflectorDevice::processReadings(void)
{
  //int i;
  ArSensorReading *reading;
  myLaser->lockDevice();
  lockDevice();
  
  const std::list<ArSensorReading *> *rawReadings;
  std::list<ArSensorReading *>::const_iterator rawIt;
  rawReadings = myLaser->getRawReadings();
  myCurrentBuffer.beginRedoBuffer();

  if (myReflectanceThreshold < 0 || myReflectanceThreshold > 255)
    myReflectanceThreshold = 0;

  if (rawReadings->begin() != rawReadings->end())
  {
    for (rawIt = rawReadings->begin(); rawIt != rawReadings->end(); rawIt++)
    {
      reading = (*rawIt);
      if (!reading->getIgnoreThisReading() && 
	  reading->getExtraInt() > myReflectanceThreshold)
	myCurrentBuffer.redoReading(reading->getPose().getX(), 
				    reading->getPose().getY());
    }
  }

  myCurrentBuffer.endRedoBuffer();

  unlockDevice();
  myLaser->unlockDevice();
}

void readSonars(ArRobot& robot, int numSonar){
	char angle[64], value[64];
	G_id += 1;
	ArSensorReading* sonarReading;
	string res;
	sprintf(value,"id=%d;",G_id);
	res += value;
	for (int i=0; i < numSonar; i++){
		sonarReading = robot.getSonarReading(i);
		sprintf(value,"v%d=%05d;", i, sonarReading->getRange());
		res += value;
		//cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
	}
	res += "\n";
	fseek(G_SONAR_FD, SEEK_SET, 0);
	fwrite(res.c_str(), sizeof(char), res.size(), G_SONAR_FD);
}

/**
  This is the packet handler for the PB9 data, which is sent via the micro
  controller, to the client.  This will read the data from the packets,
  and then call processReadings to filter add the data to the current and
  cumulative buffers.
*/
AREXPORT bool ArIrrfDevice::packetHandler(ArRobotPacket *packet)
{
  int /*portNum,*/ i, dist, packetCounter;
  double conv;
  ArTransform packetTrans;
  std::list<ArSensorReading *>::iterator it;
  ArSensorReading *reading;
  ArPose pose;
  ArTransform encoderTrans;
  ArPose encoderPose;

  pose = myRobot->getPose();
  conv = 2.88;

  packetTrans.setTransform(pose);
  packetCounter = myRobot->getCounter();

  if (packet->getID() != 0x10)
    return false;

  // Which Aux port the IRRF is connected to
  //portNum =
  packet->bufToByte2();
  encoderTrans = myRobot->getEncoderTransform();
  encoderPose = encoderTrans.doInvTransform(pose);

  i = 0;
  for (i=0, it = myRawReadings->begin();it != myRawReadings->end();it++, i++)
  {
    reading = (*it);
    dist = (int) ((packet->bufToUByte2()) / conv);
    reading->newData(dist, pose, encoderPose, packetTrans, packetCounter, packet->getTimeReceived());
  }

  myLastReading.setToNow();

  processReadings();

  return true;
}

AREXPORT void ArSonarDevice::processReadings(void)
{
  int i;
  ArSensorReading *reading;
  lockDevice();

  for (i = 0; i < myRobot->getNumSonar(); i++)
  {
    reading = myRobot->getSonarReading(i);
    if (reading == NULL || !reading->isNew(myRobot->getCounter()))
      continue;
    addReading(reading->getX(), reading->getY());
  }

  // delete too-far readings
  std::list<ArPoseWithTime *> *readingList;
  std::list<ArPoseWithTime *>::iterator it;
  double dx, dy, rx, ry;
    
  myCumulativeBuffer.beginInvalidationSweep();
  readingList = myCumulativeBuffer.getBuffer();
  rx = myRobot->getX();
  ry = myRobot->getY();
  // walk through the list and see if this makes any old readings bad
  if (readingList != NULL)
    {
      for (it = readingList->begin(); it != readingList->end(); ++it)
	{
	  dx = (*it)->getX() - rx;
	  dy = (*it)->getY() - ry;
	  if ((dx*dx + dy*dy) > (myFilterFarDist * myFilterFarDist)) 
	    myCumulativeBuffer.invalidateReading(it);
	}
    }
  myCumulativeBuffer.endInvalidationSweep();
  // leave this unlock here or the world WILL end
  unlockDevice();
}

void sonar_stop(ArRobot* robot)
{
	int numSonar; //Number of sonar on the robot
	int i; //Counter for looping
	//int j;
	numSonar = robot->getNumSonar(); //Get number of sonar
	ArSensorReading* sonarReading; //To hold each reading
	//for (j = 1; j < 6; j++)
	//{
	robot->setVel(200); //Set translational velocity to 200 mm/s
	for (;;)
	{
		for (i = 0; i < numSonar; i++) //Loop through sonar
	
		
		{
			sonarReading = robot->getSonarReading(i); //Get each sonar reading
			cout << "Sonar reading " << i << " = " << sonarReading->getRange()
				<< " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
			//getchar();

			if (sonarReading->getSensorTh() > -90 && sonarReading->getSensorTh() < 90 && sonarReading->getRange() < 500)

				robot->setVel(0);
		}
	}
			//<< " Angle " << i << " = " <<
			//printf("Sonar Reading", i, "=",sonarReading)
			//robot->unlock(); //Lock robot during set up 18
			//robot->comInt(ArCommands::ENABLE, 1); //Turn on the motors 19	

		//robot->setVel(200); //Set translational velocity to 200 mm/s
		//if (sonarReading[1] < 500)
		//{
			//robot->setRotVel(0);
		//}
		
	}

void ArLaserFilter::processReadings(void)
{
  myLaser->lockDevice();
  selfLockDevice();

  const std::list<ArSensorReading *> *rdRawReadings;
  std::list<ArSensorReading *>::const_iterator rdIt;
  
  if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
  {
    selfUnlockDevice();
    myLaser->unlockDevice();
    return;
  }

  size_t rawSize = myRawReadings->size();
  size_t rdRawSize = myLaser->getRawReadings()->size();
  
  while (rawSize < rdRawSize)
  {
    myRawReadings->push_back(new ArSensorReading);
    rawSize++;
  }

  // set where the pose was taken
  myCurrentBuffer.setPoseTaken(
	  myLaser->getCurrentRangeBuffer()->getPoseTaken());
  myCurrentBuffer.setEncoderPoseTaken(
	  myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());


  std::list<ArSensorReading *>::iterator it;
  ArSensorReading *rdReading;
  ArSensorReading *reading;

#ifdef DEBUGRANGEFILTER
  FILE *file = NULL;
  file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
#endif

  std::map<int, ArSensorReading *> readingMap;
  int numReadings = 0;

  // first pass to copy the readings and put them into a map
  for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
       rdIt != rdRawReadings->end() && it != myRawReadings->end();
       rdIt++, it++)
  {
    rdReading = (*rdIt);
    reading = (*it);
    *reading = *rdReading;
    
    readingMap[numReadings] = reading;
    numReadings++;
  }
  
  char buf[1024];
  int i;
  int j;
  ArSensorReading *lastAddedReading = NULL;
  
  // now walk through the readings to filter them
  for (i = 0; i < numReadings; i++)
  {
    reading = readingMap[i];

    // if we're ignoring this reading then just get on with life
    if (reading->getIgnoreThisReading())
      continue;

    if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
    {
      reading->setIgnoreThisReading(true);
      continue;
    }
      
    if (lastAddedReading != NULL)
    {

      if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
      {
#ifdef DEBUGRANGEFILTER
	if (file != NULL)
	  fprintf(file, "%.1f too close from last %6.0f\n", 
		  reading->getSensorTh(),
		  lastAddedReading->getPose().findDistanceTo(
			  reading->getPose()));
#endif
	reading->setIgnoreThisReading(true);
	continue;
      }
#ifdef DEBUGRANGEFILTER
      else if (file != NULL)
	fprintf(file, "%.1f from last %6.0f\n", 
		reading->getSensorTh(),
		lastAddedReading->getPose().findDistanceTo(
			reading->getPose()));
#endif
    }

//.........这里部分代码省略.........

void ArUrg::sensorInterp(void)
{
  ArTime readingRequested;
  std::string reading;
  myReadingMutex.lock();
  if (myReading.empty())
  {
    myReadingMutex.unlock();
    return;
  }

  readingRequested = myReadingRequested;
  reading = myReading;
  myReading = "";
  myReadingMutex.unlock();

  ArTime time = readingRequested;
  ArPose pose;
  int ret;
  int retEncoder;
  ArPose encoderPose;

  //time.addMSec(-13);
  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());
    return;
  }

  ArTransform transform;
  transform.setTransform(pose);

  unsigned int counter = 0; 
  if (myRobot != NULL)
    counter = myRobot->getCounter();

  lockDevice();
  myDataMutex.lock();

  //double angle;
  int i;
  int len = reading.size();

  int range;
  int big; 
  int little;
  //int onStep;

  std::list<ArSensorReading *>::reverse_iterator it;
  ArSensorReading *sReading;

  bool ignore;
  for (it = myRawReadings->rbegin(), i = 0; 
       it != myRawReadings->rend() && i < len - 1; 
       it++, i += 2)
  {
    ignore = false;
    big = reading[i] - 0x30;
    little = reading[i+1] - 0x30;
    range = (big << 6 | little);
    if (range < 20)
    {
      /* Well that didn't work...
      // if the range is 1 to 5 that means it has low intensity, which
      // could be black or maybe too far... try ignoring it and see if
      // it helps with too much clearing
      if (range >= 1 || range <= 5)
	ignore = true;
      */
      range = 4096;
    }
    sReading = (*it);
    sReading->newData(range, pose, encoderPose, transform, counter, 
		      time, ignore, 0);
  }

  myDataMutex.unlock();

  laserProcessReadings();
  unlockDevice();
}

AREXPORT bool ArUrg::setParamsBySteps(int startingStep, int endingStep, 
				      int clusterCount, bool flipped)
{
  if (startingStep < 0 || startingStep > 768 || 
      endingStep < 0 || endingStep > 768 || 
      startingStep > endingStep || 
      clusterCount < 1)
  {
    ArLog::log(ArLog::Normal, 
	       "%s::setParamsBySteps: Bad params (starting %d ending %d clusterCount %d)",
	       getName(), startingStep, endingStep, clusterCount);
    return false;
  }
  
  myDataMutex.lock();
  myStartingStep = startingStep;
  myEndingStep = endingStep;
  myClusterCount = clusterCount;
  myFlipped = flipped;
  sprintf(myRequestString, "G%03d%03d%02d", myStartingStep, endingStep,
	  clusterCount);
  myClusterMiddleAngle = 0;
  if (myClusterCount > 1)
    myClusterMiddleAngle = myClusterCount * 0.3515625 / 2.0;

  if (myRawReadings != NULL)
  {
    ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
    myRawReadings->clear();
    delete myRawReadings;
    myRawReadings = NULL;
  }

  myRawReadings = new std::list<ArSensorReading *>;
  

  ArSensorReading *reading;
  int onStep;
  double angle;

  for (onStep = myStartingStep; 
       onStep < myEndingStep; 
       onStep += myClusterCount)
  {
    /// FLIPPED
    if (!myFlipped)
      angle = ArMath::subAngle(ArMath::subAngle(135, onStep * 0.3515625),
			       myClusterMiddleAngle);
    else
      angle = ArMath::addAngle(ArMath::addAngle(-135, onStep * 0.3515625), 
			       myClusterMiddleAngle);
			       
    reading = new ArSensorReading;
    reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
				 ArMath::roundInt(mySensorPose.getY()),
				 ArMath::addAngle(angle, 
						  mySensorPose.getTh()));
    myRawReadings->push_back(reading);
    //printf("%.1f ", reading->getSensorTh());
  }


  myDataMutex.unlock();
  return true;
}

void Joydrive::drive(void)
{
  int trans, rot;
  ArPose pose;
  ArPose rpose;
  ArTransform transform;
  ArRangeDevice *dev;
  ArSensorReading *son;

  if (!myRobot->isConnected())
  {
    printf("Lost connection to the robot, exiting\n");
    exit(0);
  }
  printf("\rx %6.1f  y %6.1f  th  %6.1f", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh());
  fflush(stdout);
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
      time(NULL) - myLastPress > 1)
  {
    myLastPress = time(NULL);
    printf("\n");
    switch (myTest)
    {
    case 1:
      printf("Moving back to the origin.\n");
      pose.setPose(0, 0, 0);
      myRobot->moveTo(pose);
      break;
    case 2:
      printf("Moving over a meter.\n");
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
//.........这里部分代码省略.........

void ArLaser::laserProcessReadings(void)
{
  // if we have no readings... don't do anything
  if (myRawReadings == NULL || myRawReadings->begin() == myRawReadings->end())
    return;

  std::list<ArSensorReading *>::iterator sensIt;
  ArSensorReading *sReading;
  double x, y;
  double lastX = 0.0, lastY = 0.0;
  //unsigned int i = 0;
  ArTime len;
  len.setToNow();

  bool clean;
  if (myCumulativeCleanInterval <= 0 ||
      (myCumulativeLastClean.mSecSince() > 
       myCumulativeCleanInterval))
  {
    myCumulativeLastClean.setToNow();
    clean = true;
  }
  else
  {
    clean = false;
  }
  
  myCurrentBuffer.setPoseTaken(myRawReadings->front()->getPoseTaken());
  myCurrentBuffer.setEncoderPoseTaken(
	  myRawReadings->front()->getEncoderPoseTaken());
  myCurrentBuffer.beginRedoBuffer();	  

  // walk the buffer of all the readings and see if we want to add them
  for (sensIt = myRawReadings->begin(); 
       sensIt != myRawReadings->end(); 
       ++sensIt)
  {
    sReading = (*sensIt);

    // if we have ignore readings then check them here
    if (!myIgnoreReadings.empty() && 
	(myIgnoreReadings.find(
		(int) ceil(sReading->getSensorTh())) != 
	 myIgnoreReadings.end()) || 
	myIgnoreReadings.find(
		(int) floor(sReading->getSensorTh())) != 
	myIgnoreReadings.end())
      sReading->setIgnoreThisReading(true);

    // see if the reading is valid
    if (sReading->getIgnoreThisReading())
      continue;

    // if we have a max range then check it here... 
    if (myMaxRange != 0 && 
	sReading->getRange() > myMaxRange)
    {
      sReading->setIgnoreThisReading(true);
    }

    // see if the reading is valid... this is set up this way so that
    // max range readings can cancel out other readings, but will
    // still be ignored other than that... ones ignored for other
    // reasons were skipped above
    if (sReading->getIgnoreThisReading())
    {
      internalProcessReading(sReading->getX(), sReading->getY(), 
			     sReading->getRange(), clean, true);
      continue;
    }

    // get our coords
    x = sReading->getX();
    y = sReading->getY();
    
    // see if we're checking on the filter near dist... if we are
    // and the reading is a good one we'll check the cumulative
    // buffer
    if (myMinDistBetweenCurrentSquared > 0.0000001)
    {
      // see where the last reading was
      //squaredDist = (x-lastX)*(x-lastX) + (y-lastY)*(y-lastY);
      // see if the reading is far enough from the last reading
      if (ArMath::squaredDistanceBetween(x, y, lastX, lastY) > 
	  myMinDistBetweenCurrentSquared)
      {
	lastX = x;
	lastY = y;
	// since it was a good reading, see if we should toss it in
	// the cumulative buffer... 
	internalProcessReading(x, y, sReading->getRange(), clean, false);
	
	/* we don't do this part anymore since it wound up leaving
	// too many things not really tehre... if its outside of our
	// sensor angle to use to filter then don't let this one
	// clean  (ArMath::fabs(sReading->getSensorTh()) > 50)
	// filterAddAndCleanCumulative(x, y, false); else*/
      }
      // it wasn't far enough, skip this one and go to the next one
      else
//.........这里部分代码省略.........

void ArSickLogger::internalTakeReading(void)
{
    const std::list<ArSensorReading *> *readings;
    std::list<ArSensorReading *>::const_iterator it;
    std::list<ArSensorReading *>::const_reverse_iterator rit;
    ArPose poseTaken;
    time_t msec;
    ArSensorReading *reading;
    bool usingAdjustedReadings;

    // we take readings in any of the following cases if we haven't
    // taken one yet or if we've been explicitly told to take one or if
    // we've gone further than myDistDiff if we've turned more than
    // myDegDiff if we've switched sign on velocity and gone more than
    // 50 mm (so it doesn't oscilate and cause us to trigger)

    if (myRobot->isConnected() && (!myFirstTaken || myTakeReadingExplicit ||
                                   myLast.findDistanceTo(myRobot->getEncoderPose()) > myDistDiff ||
                                   fabs(ArMath::subAngle(myLast.getTh(),
                                           myRobot->getEncoderPose().getTh())) > myDegDiff ||
                                   (( (myLastVel < 0 && myRobot->getVel() > 0) ||
                                      (myLastVel > 0 && myRobot->getVel() < 0)) &&
                                    myLast.findDistanceTo(myRobot->getEncoderPose()) > 50)))
    {
        myWrote = true;
        mySick->lockDevice();
        /// use the adjusted raw readings if we can, otherwise just use
        /// the raw readings like before
        if ((readings = mySick->getAdjustedRawReadings()) != NULL)
        {
            usingAdjustedReadings = true;
        }
        else
        {
            usingAdjustedReadings = false;
            readings = mySick->getRawReadings();
        }
        if (readings == NULL || (it = readings->begin()) == readings->end() ||
                myFile == NULL)
        {
            mySick->unlockDevice();
            return;
        }
        myTakeReadingExplicit = false;
        myScanNumber++;
        if (usingAdjustedReadings)
            ArLog::log(ArLog::Normal,
                       "Taking adjusted readings from the %d laser values",
                       readings->size());
        else
            ArLog::log(ArLog::Normal,
                       "Taking readings from the %d laser values",
                       readings->size());
        myFirstTaken = true;
        myLast = myRobot->getEncoderPose();
        poseTaken = (*readings->begin())->getEncoderPoseTaken();
        myLastVel = myRobot->getVel();
        msec = myStartTime.mSecSince();
        fprintf(myFile, "scan1Id: %d\n", myScanNumber);
        fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000);
        /* ScanStudio isn't using these yet so don't print them
          fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(),
          myRobot->getVel());*/
        internalPrintPos(poseTaken);

        if (myUseReflectorValues)
        {
            fprintf(myFile, "reflector1: ");

            if (!mySick->isLaserFlipped())
            {
                // make sure that the list is in increasing order
                for (it = readings->begin(); it != readings->end(); it++)
                {
                    reading = (*it);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%d ", reading->getExtraInt());
                    else
                        fprintf(myFile, "0 ");
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%d ", reading->getExtraInt());
                    else
                        fprintf(myFile, "0 ");
                }
            }
            fprintf(myFile, "\n");
        }
        /**
           Note that the the sick1: or scan1: must be the last thing in
           that timestamp, ie that you should put any other data before
           it.
         **/
        if (myOldReadings)
//.........这里部分代码省略.........

//.........这里部分代码省略.........

  std::stringstream bumper_info(std::stringstream::out);
  // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
  for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
  {
    bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
    bumper_info << " " << (front_bumpers & (1 << (i+1)));
  }
  ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());

  bumper_info.str("");
  // Rear bumpers have reverse order (rightmost is LSB)
  unsigned int numRearBumpers = robot->getNumRearBumpers();
  for (unsigned int i=0; i<numRearBumpers; i++)
  {
    bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
    bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
  }
  ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
  
  bumpers_pub.publish(bumpers);

  //Publish battery information
  // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
  std_msgs::Float64 batteryVoltage;
  batteryVoltage.data = robot->getRealBatteryVoltageNow();
  voltage_pub.publish(batteryVoltage);

  if(robot->haveStateOfCharge())
  {
    std_msgs::Float32 soc;
    soc.data = robot->getStateOfCharge()/100.0;
    state_of_charge_pub.publish(soc);
  }

  // publish recharge state if changed
  char s = robot->getChargeState();
  if(s != recharge_state.data)
  {
    ROS_INFO("RosAria: publishing new recharge state %d.", s);
    recharge_state.data = s;
    recharge_state_pub.publish(recharge_state);
  }

  // publish motors state if changed
  bool e = robot->areMotorsEnabled();
  if(e != motors_state.data || !published_motors_state)
  {
	ROS_INFO("RosAria: publishing new motors state %d.", e);
	motors_state.data = e;
	motors_state_pub.publish(motors_state);
	published_motors_state = true;
  }

  // Publish sonar information, if enabled.
  if (use_sonar) {
    sensor_msgs::PointCloud cloud;	//sonar readings.
    cloud.header.stamp = position.header.stamp;	//copy time.
    // sonar sensors relative to base_link
    cloud.header.frame_id = frame_id_sonar;
    

    // Log debugging info
    std::stringstream sonar_debug_info;
    sonar_debug_info << "Sonar readings: ";
    for (int i = 0; i < robot->getNumSonar(); i++) {
      ArSensorReading* reading = NULL;
      reading = robot->getSonarReading(i);
      if(!reading) {
        ROS_WARN("RosAria: Did not receive a sonar reading.");
        continue;
      }
      
      // getRange() will return an integer between 0 and 5000 (5m)
      sonar_debug_info << reading->getRange() << " ";

      // local (x,y). Appears to be from the centre of the robot, since values may
      // exceed 5000. This is good, since it means we only need 1 transform.
      // x & y seem to be swapped though, i.e. if the robot is driving north
      // x is north/south and y is east/west.
      //
      //ArPose sensor = reading->getSensorPosition();  //position of sensor.
      // sonar_debug_info << "(" << reading->getLocalX() 
      //                  << ", " << reading->getLocalY()
      //                  << ") from (" << sensor.getX() << ", " 
      //                  << sensor.getY() << ") ;; " ;
      
      //add sonar readings (robot-local coordinate frame) to cloud
      geometry_msgs::Point32 p;
      p.x = reading->getLocalX() / 1000.0;
      p.y = reading->getLocalY() / 1000.0;
      p.z = 0.0;
      cloud.points.push_back(p);
    }
    ROS_DEBUG_STREAM(sonar_debug_info.str());
    
    sonar_pub.publish(cloud);
  }

}