C++ ArArgumentBuilder::add方法代码示例

/**
 * Search all locations for argument defaults and parse them.
 * These locations may be environment variables to read argument varues
 * from, or files to read.
 * @sa addDefaultArgumentLocation
 *
 * @note If you use this function your normal argc (passed into main()) will
 * have been modified, and won't reflect reality anymore. You'll have to use
 * getArgc() to get the actual original argument count.  This is a little wierd but is
 * this way so lots of people don't have to change lots of code.
 */
AREXPORT void ArArgumentParser::loadDefaultArguments(int position)
{
  std::list<std::string>::iterator it;
  std::list<bool>::iterator bIt;
  const char *str;
  char *argumentsPtr;
  char arguments[1024];

  if (!myUsingBuilder)
    {
      myBuilder = new ArArgumentBuilder;
      myBuilder->addStringsAsIs(*myArgc, myArgv);
      myOwnBuilder = true;
      myUsingBuilder = true;
    }

  for (it = ourDefaultArgumentLocs.begin(),
        bIt = ourDefaultArgumentLocIsFile.begin();
       it != ourDefaultArgumentLocs.end();
       it++, bIt++)
  {
    str = (*it).c_str();
    // see if its an environmental variable
    if (!(*bIt) && (argumentsPtr = getenv(str)) != NULL)
    {
      ArArgumentBuilder compressed;
      compressed.add(argumentsPtr);
      compressed.compressQuoted(true);
      myBuilder->addStringsAsIs(compressed.getArgc(), compressed.getArgv(),
                              position);
      ArLog::log(ArLog::Normal,
		 "Added arguments from environmental variable '%s'", str);
    }
    // see if we have a file
    else if ((*bIt) &&
	     ArUtil::getStringFromFile(str, arguments, sizeof(arguments)))
    {
      ArArgumentBuilder compressed;
      compressed.add(arguments);
      compressed.compressQuoted(true);
      myBuilder->addStringsAsIs(compressed.getArgc(), compressed.getArgv(),
                              position);
      ArLog::log(ArLog::Normal, "Added arguments from file '%s'",
		 str);
    }
    // the file or env didn't exit
    // this'll return true otherwise it'll return false)
    else
    {
      ArLog::log(ArLog::Verbose,
		 "Could not load from environmental variable or file '%s'",
		 str);
    }
  }
}

int RosAriaNode::Setup()
{
	ArArgumentBuilder *args;

	args = new ArArgumentBuilder();
	args->add("-rp"); //pass robot's serial port to Aria
	args->add(serial_port.c_str());
	conn = new ArSimpleConnector(args);


	robot = new ArRobot();
	robot->setCycleTime(1);

	ArLog::init(ArLog::File, ArLog::Verbose, "aria.log", true);

	//parse all command-line arguments
	if (!Aria::parseArgs())
	{
		Aria::logOptions();
		return 1;
	}

	// Connect to the robot
	if (!conn->connectRobot(robot)) {
		ArLog::log(ArLog::Terse, "rotate: Could not connect to robot! Exiting.");
		return 1;
	}

	//Sonar sensor
	sonar.setMaxRange(Max_Range);
	robot->addRangeDevice(&sonar);
	robot->enableSonar();

	// Enable the motors
	robot->enableMotors();

	robot->runAsync(true);

	return 0;
}

AREXPORT const std::list<ArArgumentBuilder *> *ArRobotParams::getSonarUnits(void)
{
  std::map<int, std::map<int, int> >::iterator it;
  int num, x, y, th;
  ArArgumentBuilder *builder;

  for (it = mySonarMap.begin(); it != mySonarMap.end(); it++)
  {
    num = (*it).first;
    x = (*it).second[SONAR_X];
    y = (*it).second[SONAR_Y];
    th = (*it).second[SONAR_TH];
    builder = new ArArgumentBuilder;
    builder->add("%d %d %d %d", num, x, y, th);
    myGetSonarUnitList.push_back(builder);
  }
  return &myGetSonarUnitList;
}

AREXPORT void ArServerHandlerMapping::addStringsForStartOfLogToMap(
	ArMap *arMap)
{
  std::list<std::string>::iterator it;
  std::string str;
  ArArgumentBuilder *builder;

  for (it = myStringsForStartOfLog.begin(); 
       it != myStringsForStartOfLog.end(); 
       it++)
  {
    str = (*it);
    builder = new ArArgumentBuilder;
    builder->add(str.c_str());
    if (strcasecmp(builder->getArg(0), "LogInfoStrip:") == 0)
    {
      builder->removeArg(0, true);
      printf("lis %s\n", builder->getFullString());

    }
    std::list<std::string> infoNames = arMap->getInfoNames();
    for (std::list<std::string>::iterator iter = infoNames.begin();
         iter != infoNames.end();
         iter++) 
    {
      const char *curName = (*iter).c_str();

      if (strcasecmp(builder->getArg(0), curName) == 0)
      {
	      builder->removeArg(0, true);
	      arMap->getInfo(curName)->push_back(builder);
	      builder = NULL;
	      break;
      }
    }
    if (builder != NULL)
      delete builder;
  }
}

  const std::list<ArArgumentBuilder *> *getList(void) 
    {
      std::list<ArArgumentBuilder *>::iterator argIt;
      std::list<std::string>::iterator listIt;
      ArArgumentBuilder *builder;

      if (myArgList.size() != 0)
      {
	while ((argIt = myArgList.begin()) != myArgList.end())
	{
	  delete (*argIt);
	  myArgList.pop_front();
	}
      }
      for (listIt = myList.begin(); listIt != myList.end(); listIt++)
      {
	builder = new ArArgumentBuilder;
	builder->add((*listIt).c_str());
	myArgList.push_front(builder);
      }
      return &myArgList;
    }

AREXPORT void ArClientHandlerConfig::handleGetConfigDefaults(
	ArNetPacket *packet)
{
  ArLog::log(ArLog::Normal, "%sreceived default config %s", 
		             myLogPrefix.c_str(), 
                 ((myHaveRequestedDefaultCopy) ? "(copy)" : "(reset)"));

  char param[1024];
  char argument[1024];
  char errorBuffer[1024];
  
  myDataMutex.lock();

  ArConfig *config = NULL;

  // If the config (or a section) is being reset to its default values,
  // then we don't want to remove any parameters that are not set -- i.e.
  // any parameters that are not contained in the default config.
  bool isClearUnsetValues = false;

  if (myHaveRequestedDefaults) {

    config = &myConfig;
  }
  else if (myHaveRequestedDefaultCopy) {

    // If we have requested a copy of the default configuration, then we
    // will want to remove any parameters that haven't been explicitly set.
    // (This is because of the next line, which copies the current config 
    // to the default config.)
    isClearUnsetValues = true;

    // The default config is transmitted in an "abbreviated" form -- just 
    // the section/param names and values.  Copy the current config to the
    // default before processing the packet so that the parameter types, etc.
    // can be preserved.
    if (myDefaultConfig == NULL) {
      myDefaultConfig = new ArConfig(myConfig);
      myDefaultConfig->setConfigName("Default", myRobotName.c_str());
      myDefaultConfig->setQuiet(myIsQuiet);
    }
    else {
      *myDefaultConfig = myConfig;
    }


    config = myDefaultConfig;
  }
  // if we didn't ask for any of these, then just return since the
  // data is for someone else
  else
  {
    myDataMutex.unlock();
    return;
  }

  if (config == NULL) {
    ArLog::log(ArLog::Normal,
               "%serror determining config to populate with default values",
               myLogPrefix.c_str());
  myDataMutex.unlock();
    return;
  }

  ArArgumentBuilder *builder = NULL;
  ArLog::log(ArLog::Normal, "%sGot defaults", myLogPrefix.c_str());
  errorBuffer[0] = '\0';
  
  //myDataMutex.lock();
  if (isClearUnsetValues) {
    config->clearAllValueSet();
  }

  while (packet->getDataReadLength() < packet->getDataLength())
  {
    packet->bufToStr(param, sizeof(param));  
    packet->bufToStr(argument, sizeof(argument));  


    builder = new ArArgumentBuilder;
    builder->setQuiet(myIsQuiet);
    builder->setExtraString(param);
    builder->add(argument);

    if ((strcasecmp(param, "Section") == 0 && 
        !config->parseSection(builder, errorBuffer, sizeof(errorBuffer))) ||
        (strcasecmp(param, "Section") != 0 &&
        !config->parseArgument(builder, errorBuffer, sizeof(errorBuffer))))
    {
      ArLog::log(ArLog::Terse, "%shandleGetConfigDefaults: Hideous problem getting defaults, couldn't parse '%s %s'", 
		             myLogPrefix.c_str(), param, argument);
    }
    else {
      IFDEBUG(if (strlen(param) > 0) {
                 ArLog::log(ArLog::Normal, "%shandleGetConfigDefaults: added default '%s %s'", 
                 myLogPrefix.c_str(), param, argument); } );
    }
    delete builder;
    builder = NULL;
  }

bool ArServerHandlerConfig::internalSetConfig(ArServerClient *client, 
					      ArNetPacket *packet)
{
  char param[1024];
  char argument[1024];
  char errorBuffer[1024];
  char firstError[1024];
  ArNetPacket retPacket;
  ArConfig *config;
  bool ret = true;

  if (client != NULL)
    config = myConfig;
  else
    config = myDefault;

  if (client != NULL)
    lockConfig();
  ArArgumentBuilder *builder = NULL;
  if (client != NULL)
    ArLog::log(ArLog::Normal, "Got new config from client %s", client->getIPString());
  else
    ArLog::log(ArLog::Verbose, "New default config");
  errorBuffer[0] = '\0';
  firstError[0] = '\0';

  while (packet->getDataReadLength() < packet->getDataLength())
  {
    packet->bufToStr(param, sizeof(param));  
    packet->bufToStr(argument, sizeof(argument));  

    builder = new ArArgumentBuilder;
    builder->setExtraString(param);
    builder->add(argument);
    ArLog::log(ArLog::Verbose, "Config: %s %s", param, argument);
    // if the param name here is "Section" we need to parse sections,
    // otherwise we parse the argument
    if ((strcasecmp(param, "Section") == 0 && 
        !config->parseSection(builder, errorBuffer, sizeof(errorBuffer))) ||
        (strcasecmp(param, "Section") != 0 &&
        !config->parseArgument(builder, errorBuffer, sizeof(errorBuffer))))
    {
      if (firstError[0] == '\0')
        strcpy(firstError, errorBuffer);
    }
    delete builder;
    builder = NULL;
  }
  if (firstError[0] == '\0')
  {
    if (config->callProcessFileCallBacks(true, 
                                           errorBuffer, 
                                           sizeof(errorBuffer)))
    {
      if (client != NULL)
	ArLog::log(ArLog::Normal, "New config from client %s was fine.",
		   client->getIPString());
      else
	ArLog::log(ArLog::Verbose, "New default config was fine.");
      retPacket.strToBuf("");
      writeConfig();
    }
    else // error processing config callbacks
    {
      ret = false;
      if (firstError[0] == '\0')
        strcpy(firstError, errorBuffer);
      // if its still empty it means we didn't have anything good in the errorBuffer
      if (firstError[0] == '\0')
        strcpy(firstError, "Error processing");

      if (client != NULL)
	ArLog::log(ArLog::Normal, 
		   "New config from client %s had errors processing ('%s').",
		   client->getIPString(), firstError);
      else
	ArLog::log(ArLog::Normal, 
		   "New default config had errors processing ('%s').",
		   firstError);
      retPacket.strToBuf(firstError);
    }
  }
  else
  {
    ret = false;
    if (client != NULL)
      ArLog::log(ArLog::Normal, 
		 "New config from client %s had at least this problem: %s", 
		 client->getIPString(), firstError);
    else
      ArLog::log(ArLog::Normal, 
		 "New default config had at least this problem: %s", 
		 firstError);
    retPacket.strToBuf(firstError);
  }
  //printf("Sending ");
  //retPacket.log();
  if (client != NULL)
    client->sendPacketTcp(&retPacket);
  if (client != NULL)
//.........这里部分代码省略.........

bool ArSoundPlayer::playWavFile(const char* filename, const char* params)
{
  ArArgumentBuilder builder;
  //builder.addPlain("sleep .35; play");
  builder.addPlain("play");
  builder.add("-v %.2f", ourVolume);
  builder.addPlain(filename);
  builder.addPlain(params);
  ArLog::log(ArLog::Normal, "ArSoundPlayer: Playing file \"%s\" with \"%s\"", 
	     filename, builder.getFullString());
  
  int ret;
  if ((ret = system(builder.getFullString())) != -1)
  {
    ArLog::log(ArLog::Normal, "ArSoundPlayer: Played file \"%s\" with \"%s\" (got %d)", 
	       filename, builder.getFullString(), ret);
    return true;
  }
  else
  {
    ArLog::logErrorFromOS(ArLog::Normal, 
			  "ArSoundPlayer::playWaveFile: system call failed");
    return false;
  }


  /*
    This was an old mechanism for doing a fork then exec in order to
    mimic a system call, so that it could have the child PID for
    killing... however in our embedded linux killing the play command
    doesn't also kill the sox command, so the kill doesn't work at
    all... and the fork() would also reserve a lot of memory for the new process
    ...
   so it was replaced with the above system call

  const char* prog = NULL;
  prog = getenv("PLAY_WAV");
  if(prog == NULL)
    prog = "play";
  char volstr[4];
  if(strcmp(prog, "play") == 0)
  {
    snprintf(volstr, 4, "%f", ourVolume);
  }  

  ArLog::log(ArLog::Normal, "ArSoundPlayer: Playing file \"%s\" using playback program \"%s\" with argument: -v %s", filename, prog, volstr);
  ourPlayChildPID = fork();

  //ourPlayChildPID = vfork(); // XXX rh experimental, avoids the memory copy cost of fork()
  // NOTE: after vfork() you can ONLY safely use an exec function or _exit() in the
  // child process. Any other call, if it modifies memory still shared by the
  // parent, will result in problems.
  if(ourPlayChildPID == -1) 
  {
    ArLog::log(ArLog::Terse, "ArSoundPlayer: error forking! (%d: %s)", errno, 
      (errno == EAGAIN) ? "EAGAIN reached process limit, or insufficient memory to copy page tables" : 
        ( (errno == ENOMEM) ? "ENOMEM out of kernel memory" : "unknown error" ) );

    ArLog::logErrorFromOS(ArLog::Normal, 
			  "ArSoundPlayer::playWaveFile: fork failed");
    return false;
  }
  if(ourPlayChildPID == 0)
  {
    // child process: execute sox
    int r = -1;
    r = execlp(prog, prog, "-v", volstr, filename, (char*)0);
    if(r < 0)
    {
      int err = errno;
      const char *errstr = strerror(err);
      printf("ArSoundPlayer (child process): Error executing Wav file playback program \"%s %s\" (%d: %s)\n", prog, filename, err, errstr);
      //_exit(-1);   // need to use _exit with vfork
      exit(-1);
    }
  } 
  // parent process: wait for child to finish
  ArLog::log(ArLog::Verbose, "ArSoundPlayer: created child process %d to play wav file \"%s\".", 
      ourPlayChildPID, filename);
  int status;
  waitpid(ourPlayChildPID, &status, 0);
  if(WEXITSTATUS(status) != 0) {
    ArLog::log(ArLog::Terse, "ArSoundPlayer: Error: Wav file playback program \"%s\" with file \"%s\" exited with error code %d.", prog, filename, WEXITSTATUS(status));
    ourPlayChildPID = -1;
    return false;
  }
  ArLog::log(ArLog::Verbose, "ArSoundPlayer: child process %d finished.", ourPlayChildPID);
  ourPlayChildPID = -1;
  return true;
  */
}

int main(int argc, char **argv)
{

  Aria::init();
  ArLog::init(ArLog::StdOut, ArLog::Verbose);

  ArMap testMap;

  ArTime timer;

  ArGlobalFunctor mapChangedCB(&mapChanged);
  testMap.addMapChangedCB(&mapChangedCB);


  if (argc <= 1)
  {
    printf("mapTest: Usage %s <map> <map2:optional>\n", argv[0]);
    Aria::exit(1);
  }

  timer.setToNow();
  if (!testMap.readFile(argv[1]))
  {
    printf("mapTest: Could not read map '%s'\n", argv[1]);
    Aria::exit(2);
  }
  printf("mapTest: Took %ld ms to read file\n", timer.mSecSince());
/*
  printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
	 testMap.getMapObjectsChanged().mSecSince(),
	 testMap.getPointsChanged().mSecSince(),
	 testMap.getMapInfoChanged().mSecSince());
*/
  timer.setToNow();
  if(!testMap.writeFile("mapTest.map"))
  {
    printf("mapTest: Error could not write new map to mapTest.map");
    Aria::exit(3);
  }
  printf("mapTest: Took %ld ms to write file mapTest.map\n", timer.mSecSince());

  std::list<ArMapObject *>::iterator objIt;
  ArMapObject *obj;
  for (objIt = testMap.getMapObjects()->begin(); 
       objIt != testMap.getMapObjects()->end(); 
       objIt++)
  {
    obj = (*objIt);
    printf("mapTest: Map object: %s named \"%s\". Pose: %0.2f,%0.2f,%0.2f. ", obj->getType(), obj->getName(), obj->getPose().getX(), obj->getPose().getY(), obj->getPose().getTh());
    if(obj->hasFromTo())
      printf("mapTest: Extents: From %0.2f,%0.2f to %0.2f,%0.2f.", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY());
    printf("mapTest: \n");

/*
    if (strcasecmp(obj->getType(), "Goal") == 0 ||
	strcasecmp(obj->getType(), "GoalWithHeading") == 0)
    {
      printf("mapTest: Map object: Goal %s\n", obj->getName());
    }
    else if (strcasecmp(obj->getType(), "ForbiddenLine") == 0 &&
	obj->hasFromTo())
    {
      printf("mapTest: Map object: Forbidden line from %.0f %.0f to %.0f %.0f\n",
	     obj->getFromPose().getX(), obj->getFromPose().getY(),
	     obj->getToPose().getX(), obj->getToPose().getY());
    }

*/

  }

  std::list<ArArgumentBuilder*>* objInfo = testMap.getMapInfo();
  for(std::list<ArArgumentBuilder*>::const_iterator i = objInfo->begin();
      i != objInfo->end();
      i++)
  {
    printf("mapTest: MapInfo object definition:\n----\n");
    (*i)->log();
    printf("mapTest: ----\n");
  }

  printf("mapTest: First 5 data points:\n");
  std::vector<ArPose>::iterator pIt;
  ArPose pose;
  int n = 0;
  for (pIt = testMap.getPoints()->begin(); 
       pIt != testMap.getPoints()->end(); 
       pIt++)
  {
    pose = (*pIt);
    if (n > 5)
      exit(0);
    printf("mapTest: \t%.0f %.0f\n", pose.getX(), pose.getY());
    n++;
    // the points are gone through
  }

  if (argc >= 3)
  {
    timer.setToNow();
//.........这里部分代码省略.........

int RosAriaNode::Setup()
{
  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
  // called once per instance, and these objects need to persist until the process terminates.

  robot = new ArRobot();
  ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

  // Now add any parameters given via ros params (see RosAriaNode constructor):

  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
  // for wireless serial connection. Otherwise, interpret it as a serial port name.
  size_t colon_pos = serial_port.find(":");
  if (colon_pos != std::string::npos)
  {
    args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
    args->add(serial_port.substr(0, colon_pos).c_str());
    //ROS_INFO( "RosAria: using IP: [%s]", serial_port.substr(0, colon_pos).c_str() );
    args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
    args->add(serial_port.substr(colon_pos+1).c_str());
    //ROS_INFO( "RosAria: using port: [%s]", serial_port.substr(colon_pos+1).c_str() );
  }
  else
  {
    args->add("-robotPort"); // pass robot's serial port to Aria
    args->add(serial_port.c_str());
  }

  // if a baud rate was specified in baud parameter
  if(serial_baud != 0)
  {
    args->add("-robotBaud");
    char tmp[100];
    snprintf(tmp, 100, "%d", serial_baud);
    args->add(tmp);
  }
  
  if( debug_aria )
  {
    // turn on all ARIA debugging
    args->add("-robotLogPacketsReceived"); // log received packets
    args->add("-robotLogPacketsSent"); // log sent packets
    args->add("-robotLogVelocitiesReceived"); // log received velocities
    args->add("-robotLogMovementSent");
    args->add("-robotLogMovementReceived");
    ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
  }


  // Connect to the robot
  conn = new ArRobotConnector(argparser, robot); // warning never freed
  if (!conn->connectRobot()) {
    ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
    return 1;
  }

  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
  if(!Aria::parseArgs())
  {
    ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
    return 1;
  }

  readParameters();

  // Start dynamic_reconfigure server
  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
  

  /*
   * 横向速度和纵向速度单位为米
*/

  /*
   * 初始化参数的最小值 其中TickMM=10 DriftFactor=-200 Revount=-32760
   */

  // Setup Parameter Minimums
  rosaria::RosAriaConfig dynConf_min;
  dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
  dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
  // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
  // Until then, set unit length interval.
  dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
  dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
  dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
  dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_min.TicksMM     = 10;
  dynConf_min.DriftFactor = -200;
  dynConf_min.RevCount    = -32760;
  
  dynamic_reconfigure_server->setConfigMin(dynConf_min);
  
  
  // 初始化参数的最大值 其中TickMM=200 DriftFactor=200 Revount=32760
  rosaria::RosAriaConfig dynConf_max;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    }
    // if not its part of the keyword
    else
      keyword[i-textStart] = line[i];
  }

  keyword[i-textStart] = '\0';
  //ArLog::log(ArLog::Verbose, "line %d: keyword %s", lineNumber, keyword);
  // now find the start of the value (first non whitespace)
  for (; i < len; i++)
  {
    // if its not a space we're done
    if (!isspace(line[i]))
    {
      valueStart = &line[i];
      break;
    };
  }
  // lower that keyword
  ArUtil::lower(keyword, keyword, 512);

  // a variable for if we're using the remainder handler or not (don't
  // do a test just because someone could set the remainder handler to
  // some other handler they're using)
  bool usingRemainder = false;
  // see if we have a handler for the keyword
  if ((it = myMap.find(keyword)) != myMap.end())
  {
    //printf("have handler for keyword %s\n", keyword);
    // we have a handler, so pull that out
    handler = (*it).second;
    // valueStart was set above but make sure there's an argument
    if (i == len)
      noArgs = true;
  }
  // if we don't then check for a remainder handler
  else
  {
    //printf("no handler for keyword %s\n", keyword);
    // if we have one set it
    if (myRemainderHandler != NULL)
    {
      usingRemainder = true;
      handler = myRemainderHandler;
      // reset the value to the start of the text
      valueStart = &line[textStart];
    }
    // if we don't just keep going
    else
    {
      ArLog::log(ArLog::Verbose,
		 "line %d: unknown keyword '%s' line '%s', continuing",
		 myLineNumber, keyword, &line[textStart]);
      return true;
    }
  }
  /*
  if (noArgs)
    ArLog::log(ArLog::Verbose, "line %d: firstword '%s' no argument",
	       myLineNumber, keyword);
  else
    ArLog::log(ArLog::Verbose, "line %d: firstword '%s' argument '%s'",
	       myLineNumber, keyword, valueStart);
  */
  // now toss the rest of the argument into an argument builder then
  // form it up to send to the functor

  ArArgumentBuilder builder;
  // if we have arguments add them
  if (!noArgs)
    builder.add(valueStart);
  // if not we still set the name of whatever we parsed (unless we
  // didn't have a param of course)
  if (!usingRemainder)
    builder.setExtraString(keyword);

  // make sure we don't overwrite any errors
  if (errorBuffer != NULL && errorBuffer[0] != '\0')
  {
    errorBuffer = NULL;
    errorBufferLen = 0;
  }

  // call the functor and see if there are errors;
  // if we had an error and aren't continuing on errors then we keep going
  if (!handler->call(&builder, errorBuffer, errorBufferLen))
  {
    // put the line number in the error message (this won't overwrite
    // anything because of the check above
    if (errorBuffer != NULL)
    {
      std::string errorString = errorBuffer;
      snprintf(errorBuffer, errorBufferLen, "Line %d: %s", myLineNumber,
	       errorString.c_str());

    }
    return false;
  }
  return true;
}

int RosAriaNode::Setup()
{
  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
  // called once per instance, and these objects need to persist until the process terminates.

  robot = new ArRobot();

  ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

  // Now add any parameters given via ros params (see RosAriaNode constructor):

  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
  // for wireless serial connection. Otherwise, interpret it as a serial port name.
  size_t colon_pos = serial_port.find(":");
  if (colon_pos != std::string::npos)
  {
    args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
    args->add(serial_port.substr(0, colon_pos).c_str());
    args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
    args->add(serial_port.substr(colon_pos+1).c_str());
  }
  else
  {
    args->add("-robotPort"); // pass robot's serial port to Aria
    args->add(serial_port.c_str());
  }

  // if a baud rate was specified in baud parameter
  if(serial_baud != 0)
  {
    args->add("-robotBaud");
    char tmp[100];
    snprintf(tmp, 100, "%d", serial_baud);
    args->add(tmp);
  }
  
  if( debug_aria )
  {
    // turn on all ARIA debugging
    args->add("-robotLogPacketsReceived"); // log received packets
    args->add("-robotLogPacketsSent"); // log sent packets
    args->add("-robotLogVelocitiesReceived"); // log received velocities
    args->add("-robotLogMovementSent");
    args->add("-robotLogMovementReceived");
    ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
  }

  // Connect to the robot
  conn = new ArRobotConnector(argparser, robot); // warning never freed
  if (!conn->connectRobot()) {
    ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
    return 1;
  }

  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
  if(!Aria::parseArgs())
  {
    ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
    return 1;
  }

  // Start dynamic_reconfigure server
  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;

  robot->lock();

  // Setup Parameter Minimums
  rosaria::RosAriaConfig dynConf_min;

  //arbitrary non-zero values so dynamic reconfigure isn't STUPID
  dynConf_min.trans_vel_max = 0.1; 
  dynConf_min.rot_vel_max = 0.1; 
  dynConf_min.trans_accel = 0.1;
  dynConf_min.trans_decel = 0.1;
  dynConf_min.rot_accel = 0.1;
  dynConf_min.rot_decel = 0.1; 
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_min.TicksMM     = 10;
  dynConf_min.DriftFactor = -200;
  dynConf_min.RevCount    = -32760;
  
  dynamic_reconfigure_server->setConfigMin(dynConf_min);
  
  rosaria::RosAriaConfig dynConf_max;
  dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0; 
  dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0; 
  dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0;
  dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0;
  dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0;
  dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_max.TicksMM     = 200;
  dynConf_max.DriftFactor = 200;
  dynConf_max.RevCount    = 32760;
  
  dynamic_reconfigure_server->setConfigMax(dynConf_max);
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
  Aria::init();
  char *worldName;
  char *mapName;

  if (argc != 3)
  {
    ArLog::log(ArLog::Normal, "Usage: %s <WorldFile> <MapFile>", argv[0]);
    ArLog::log(ArLog::Normal, "Example: %s columbia.wld columbia.map", argv[0]);
    exit(1);
  }

  worldName = argv[1];
  mapName = argv[2];
  
  FILE *file;
  if ((file = ArUtil::fopen(worldName, "r")) == NULL)
  {
    ArLog::log(ArLog::Normal, "Could not open world file '%s' to convert", worldName);
    exit(1);
  }
  
  char line[10000];
  
  std::vector<ArLineSegment> lines;

  bool haveHome = false;
  ArPose homePose;

  // read until the end of the file
  while (fgets(line, sizeof(line), file) != NULL)
  {
    ArArgumentBuilder builder;
    builder.add(line);

    // Four ints is a line
    if (builder.getArgc() == 4 && builder.isArgInt(0) && 
	builder.isArgInt(1) && builder.isArgInt(2) && 
	builder.isArgInt(3))
    {
      lines.push_back(
	      ArLineSegment(builder.getArgInt(0), builder.getArgInt(1),
			    builder.getArgInt(2), builder.getArgInt(3)));
    }

    // "position X Y Th" becomes a RobotHome
    if( !strcmp(builder.getArg(0), "position") &&
        builder.getArgc() == 4 && builder.isArgInt(1) && 
        builder.isArgInt(2) && builder.isArgInt(3) )
    {
      haveHome = true;
      homePose.setX(builder.getArgInt(1));
      homePose.setY(builder.getArgInt(2));
      homePose.setTh(builder.getArgInt(3));
      printf("Will make a Home point out of start position: ");
      homePose.log();
    }
  }

    
  ArMap armap;
  armap.setLines(&lines);

  ArPose nopose;
  ArMapObject home("RobotHome", homePose, NULL, "ICON", "Home", false, nopose, nopose);
  std::list<ArMapObject*> objects;
  if(haveHome)
  {
    objects.push_back(&home);
    armap.setMapObjects(&objects);
  }

  if (!armap.writeFile(mapName))
  {
    ArLog::log(ArLog::Normal, "Could not save map file '%s'", mapName);
    exit(1);
  }

  ArLog::log(ArLog::Normal, "Converted %s world file to %s map file.", worldName, mapName);
  exit(0);
}