AREXPORT void ArServerInfoRobot::updateNumbers(ArServerClient *client, 
					       ArNetPacket *packet)
{
  ArNetPacket sending;

  myRobot->lock();

  if (myRobot->haveStateOfCharge())
    sending.byte2ToBuf(ArMath::roundInt(myRobot->getStateOfCharge() * 10));
  else if (myRobot->getRealBatteryVoltage() > 0)
    sending.byte2ToBuf(ArMath::roundInt(
	    myRobot->getRealBatteryVoltage() * 10));
  else
    sending.byte2ToBuf(ArMath::roundInt(
	    myRobot->getBatteryVoltage() * 10));
  sending.byte4ToBuf((int)myRobot->getX());
  sending.byte4ToBuf((int)myRobot->getY());
  sending.byte2ToBuf((int)myRobot->getTh());
  sending.byte2ToBuf((int)myRobot->getVel());
  sending.byte2ToBuf((int)myRobot->getRotVel());
  sending.byte2ToBuf((int)myRobot->getLatVel());
  sending.byteToBuf((char)myRobot->getTemperature());
  myRobot->unlock();

  client->sendPacketUdp(&sending);
}
/**
   This requests a config from the server and resets it so we haven't
   gotten a config.
 **/
AREXPORT void ArClientHandlerConfig::requestConfigFromServer(void)
{

  char *getConfigPacketName = "getConfigBySections";
  bool isInsertPriority = true;

  ArFunctor1C<ArClientHandlerConfig, ArNetPacket *> *getConfigCB = &myHandleGetConfigBySectionsCB;

  if (!myClient->dataExists(getConfigPacketName)) {
    getConfigPacketName = "getConfig";
    isInsertPriority = false;

    getConfigCB = &myHandleGetConfigCB;
  }

  myDataMutex.lock();
  ArLog::log(ArLog::Verbose, "%sRequesting config from server, and clearing sections...", 
              myLogPrefix.c_str());
  myConfig.clearSections();
  myDataMutex.unlock();

  myClient->remHandler(getConfigPacketName, getConfigCB);
  myClient->addHandler(getConfigPacketName, getConfigCB);
  myClient->remHandler("setConfig", &myHandleSetConfigCB);
  myClient->addHandler("setConfig", &myHandleSetConfigCB);

  if (myClient->dataExists("getConfigDefaults")) {
    myClient->remHandler("getConfigDefaults", &myHandleGetConfigDefaultsCB);
    myClient->addHandler("getConfigDefaults", &myHandleGetConfigDefaultsCB);
  }
  if (myClient->dataExists("getConfigSectionFlags"))
  {
    myClient->remHandler("getConfigSectionFlags", 
			 &myHandleGetConfigSectionFlagsCB);
    myClient->addHandler("getConfigSectionFlags", 
			 &myHandleGetConfigSectionFlagsCB);
    myClient->requestOnce("getConfigSectionFlags");
  }

  if (isInsertPriority) {
    ArLog::log(ArLog::Verbose,
               "%sRequesting that config has last priority value %i",
               myLogPrefix.c_str(),
               ArPriority::LAST_PRIORITY);

    ArNetPacket packet;
    packet.byteToBuf(ArPriority::LAST_PRIORITY);
    myClient->requestOnce(getConfigPacketName, &packet);
  }
  else { // don't insert priority
    myClient->requestOnce(getConfigPacketName);
  } // end else don't insert priority

  myDataMutex.lock();
  myHaveGottenConfig = false;  
  myDataMutex.unlock();
}
AREXPORT void ArServerInfoRobot::physicalInfo(ArServerClient *client, 
					     ArNetPacket *packet)
{
  ArNetPacket sending;

  myRobot->lock();
  sending.strToBuf(myRobot->getRobotType());
  sending.strToBuf(myRobot->getRobotSubType());
  sending.byte2ToBuf((int)myRobot->getRobotWidth());
  sending.byte2ToBuf((int)myRobot->getRobotLengthFront());
  sending.byte2ToBuf((int)myRobot->getRobotLengthRear());
  if (!myRobot->hasLatVel())
    sending.byteToBuf(0);
  else
    sending.byteToBuf(1);
  myRobot->unlock();
  
  client->sendPacketTcp(&sending);
}
void AriaClientDriver::unsafeDrive()
{
  /* Construct a request packet. The data is a single byte, with value
   * 1 to enable safe drive, 0 to disable. */
  ArNetPacket p;
  p.byteToBuf(0);

  /* Send the packet as a single request: */
  if(myPrinting)
    printf("Sending setSafeDrive 0.\n");
  myClient->requestOnce("setSafeDrive",&p);
  if(myPrinting)
    printf("\nSent disable safe drive command. Your robot WILL run over things if you're not careful.\n");
}
void AriaClientDriver::safeDrive()
{
  /* Construct a request packet. The data is a single byte, with value
   * 1 to enable safe drive, 0 to disable. */
  ArNetPacket p;
  p.byteToBuf(1);

  /* Send the packet as a single request: */
  if(myPrinting)
    printf("Sending setSafeDrive 1.\n");
  myClient->requestOnce("setSafeDrive",&p);
  if(myPrinting)
    printf("\nSent enable safe drive.\n");
}
Пример #6
0
AREXPORT void ArServerInfoRobot::update(ArServerClient *client, 
					ArNetPacket *packet)
{
  ArNetPacket sending;

  myRobot->lock();

  ArServerMode *netMode;
  if ((netMode = ArServerMode::getActiveMode()) != NULL)
  {
    sending.strToBuf(netMode->getStatus());
    sending.strToBuf(netMode->getMode());
  }
  else 
  {
    sending.strToBuf("Unknown status");
    sending.strToBuf("Unknown mode");
  }


	//ArLog::log(ArLog::Normal,
  //                     "ArServerInfoRobot::update() havestateofcharge = %d, soc = %f, real = %f, reg = %f",
	//											myRobot->haveStateOfCharge(),
	//											myRobot->getStateOfCharge(),
	//											myRobot->getRealBatteryVoltage(),
	//											myRobot->getBatteryVoltage());

  if (myRobot->haveStateOfCharge())
    sending.byte2ToBuf(ArMath::roundInt(myRobot->getStateOfCharge() * 10));
  else if (myRobot->getRealBatteryVoltage() > 0)
    sending.byte2ToBuf(ArMath::roundInt(
	    myRobot->getRealBatteryVoltage() * 10));
  else
    sending.byte2ToBuf(ArMath::roundInt(
	    myRobot->getBatteryVoltage() * 10));

  sending.byte4ToBuf((int)myRobot->getX());
  sending.byte4ToBuf((int)myRobot->getY());
  sending.byte2ToBuf((int)myRobot->getTh());
  sending.byte2ToBuf((int)myRobot->getVel());
  sending.byte2ToBuf((int)myRobot->getRotVel());
  sending.byte2ToBuf((int)myRobot->getLatVel());
  sending.byteToBuf((char)myRobot->getTemperature());
	//sending.byte2ToBuf((int)myRobot->getPayloadNumSlots());
  myRobot->unlock();

  client->sendPacketUdp(&sending);
}
AREXPORT void ArServerHandlerCamera::handleGetCameraInfo(ArServerClient *client, 
                                                         ArNetPacket *packet)
{
  if ((client == NULL) || (myRobot == NULL) || (myCamera == NULL)) {
    return;
  }

  ArNetPacket sendPacket;

  myRobot->lock();

  double minPan  = myCamera->getMaxNegPan();
  double maxPan  = myCamera->getMaxPosPan();
  double minTilt = myCamera->getMaxNegTilt();
  double maxTilt = myCamera->getMaxPosTilt();
  double minZoom = 0;
  double maxZoom = 100;
  bool isZoomAvailable = myCamera->canZoom();

  ArLog::log(ArLog::Verbose, 
             "ArServerHandlerCamera: client requested camera info (my camera name is %s), returning: minPan %f maxPan %f minTilt %f maxTilt %f minZoom %f maxZoom %f isZoomAvailable %d", 
             myCameraName.c_str(), minPan, maxPan, minTilt, maxTilt, minZoom, maxZoom, isZoomAvailable);

  myRobot->unlock();

  addDoubleToPacket(minPan, sendPacket);
  addDoubleToPacket(maxPan, sendPacket);
  addDoubleToPacket(minTilt, sendPacket);
  addDoubleToPacket(maxTilt, sendPacket);
  addDoubleToPacket(minZoom, sendPacket);
  addDoubleToPacket(maxZoom, sendPacket);

  sendPacket.byteToBuf(isZoomAvailable);

  client->sendPacketUdp(&sendPacket);

} // end method handleGetCameraInfo
/**
 * @param client the ArServerClient * to which to send the config
 * @param packet the ArNetPacket * which accompanied the client's request
 * @param isMultiplePackets a bool set to true if the server should send a
 * packet for each config section followed by the empty packet; false if 
 * the server should send the entire config in one packet (i.e. the old style)
 * @param lastPriority the last ArPriority::Priority that should be sent 
 * to the client (this is the greatest numerical value and the least 
 * semantic priority).
**/
AREXPORT void ArServerHandlerConfig::handleGetConfig(ArServerClient *client, 
                                                     ArNetPacket *packet,
                                                     bool isMultiplePackets,
                                                     ArPriority::Priority lastPriority)
{

  ArConfigArg param;

 
  // The multiple packets method also sends display hints with the parameters;
  // the old single packet method does not.
  ArClientArg clientArg(isMultiplePackets,
                        lastPriority);

  std::set<std::string> sent;

  ArNetPacket sending;
  ArLog::log(ArLog::Normal, "Config requested.");

  std::list<ArConfigSection *> *sections = myConfig->getSections();
  for (std::list<ArConfigSection *>::iterator sIt = sections->begin(); 
       sIt != sections->end(); 
       sIt++)
  {
    // Clear the packet...
    if (isMultiplePackets) {
      sending.empty();
    }

    // clear out the sent list between sections
    sent.clear();

    ArConfigSection *section = (*sIt);
    if (section == NULL) {
      continue;
    }

    sending.byteToBuf('S');
    sending.strToBuf(section->getName());
    sending.strToBuf(section->getComment());

    ArLog::log(ArLog::Verbose, "Sending config section %s...", section->getName());

    //printf("S %s %s\n", section->getName(), section->getComment());
    std::list<ArConfigArg> *params = section->getParams();
    for (std::list<ArConfigArg>::iterator pIt = params->begin(); 
         pIt != params->end(); 
         pIt++)
    {
      param = (*pIt);

      bool isCheckableName = 
      (param.getType() != ArConfigArg::DESCRIPTION_HOLDER && 
       param.getType() != ArConfigArg::SEPARATOR &&
       param.getType() != ArConfigArg::STRING_HOLDER);

      // if we've already sent it don't send it again
      if (isCheckableName &&
          sent.find(param.getName()) != sent.end()) {
        continue;
      }
      else if (isCheckableName) {
        sent.insert(param.getName());
      }

      if (clientArg.isSendableParamType(param))
      {
        sending.byteToBuf('P');

        bool isSuccess = clientArg.createPacket(param,
                                                &sending);

      }
    } // end for each parameter

    if (!sending.isValid()) {

      ArLog::log(ArLog::Terse, "Config section %s cannot be sent; packet size exceeded",
                 section->getName());

    } // end if length exceeded...
    else if (isMultiplePackets) {

      client->sendPacketTcp(&sending);

    } // end else send in chunks...

  } // end for each section

  // If sending each section in individual packets, then send an empty packet 
  // to indicate the end of the config data.
  if (isMultiplePackets) {

    sending.empty();
    client->sendPacketTcp(&sending);
  }
  else { //  send the entire config in one packet

    // If the config is too big to fit in the packet, then just send an empty
    // packet (to try to prevent an older client from crashing)
    // TODO: Is there any better way to notify the user of an error....
    if (!sending.isValid()) {
      ArLog::log(ArLog::Terse, "Error sending config; packet size exceeded");
      sending.empty();
    }

    client->sendPacketTcp(&sending);

  } // end else send the entire packet

} // end method getConfigBySections
AREXPORT void ArServerHandlerMapping::serverMappingStart(
	ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  char buf[512];
  packet->bufToStr(buf, sizeof(buf));

  // see if we're already mapping
  if (myLaserLogger != NULL)
  {
    ArLog::log(ArLog::Normal, "MappingStart: Map already being made");
    sendPacket.byteToBuf(1);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    return;
  }


  myRobot->lock();

  // lower case everything (to avoid case conflicts)
  ArUtil::lower(buf, buf, sizeof(buf));
  myMapName = buf;

  myFileName = myMapName;
  if (!mySuffix.empty())
    myFileName += mySuffix;
  if (strstr(myMapName.c_str(), ".2d") == NULL)
    myFileName += ".2d";

  if (myLaser2 != NULL)
  {
    myFileName2 = myMapName;
    if (!mySuffix2.empty())
      myFileName2 += mySuffix2;
    if (strstr(myMapName.c_str(), ".2d") == NULL)
      myFileName2 += ".2d";
  }


  // call our mapping started callbacks
  std::list<ArFunctor *>::iterator fit;
  ArFunctor *functor;
  for (fit = myMappingStartCallbacks.begin(); 
       fit != myMappingStartCallbacks.end(); 
       fit++)
  {
    functor = (*fit);
    functor->invoke();
  }


  myLaserLogger = new ArLaserLogger(myRobot, myLaser, 300, 25, myFileName.c_str(),
				    true, Aria::getJoyHandler(), 
				    myTempDirectoryHelper.getTempDirectory(),
				    myUseReflectorValues,
				    Aria::getRobotJoyHandler(),
				    &myLocationDataMap,
				    myExtraLasers);
  if (myLaserLogger == NULL)
  {
    ArLog::log(ArLog::Normal, "MappingStart: myLaserLogger == NULL");
    sendPacket.byteToBuf(2);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    myMapName = "";
    myFileName = "";
    myRobot->unlock();
    return;
  }
  if (!myLaserLogger->wasFileOpenedSuccessfully())
  {
    ArLog::log(ArLog::Normal, "MappingStart: Cannot open map file %s", 
	       myFileName.c_str());
    sendPacket.byteToBuf(2);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    myMapName = "";
    myFileName = "";
    delete myLaserLogger;
    myLaserLogger = NULL;
    myRobot->unlock();
    return;
  }

  if (myLaser2 != NULL)
  {
    myLaserLogger2 = new ArLaserLogger(myRobot, myLaser2, 300, 25, 
				     myFileName2.c_str(),
				     true, Aria::getJoyHandler(), 
				     myTempDirectoryHelper.getTempDirectory(),
				     myUseReflectorValues,
				     Aria::getRobotJoyHandler(),
				     &myLocationDataMap);
  }

  // toss our strings for the start on there
  std::list<std::string>::iterator it;
  
  for (it = myStringsForStartOfLog.begin(); 
       it != myStringsForStartOfLog.end(); 
       it++)
  {
    myLaserLogger->addInfoToLogPlain((*it).c_str());
    if (myLaserLogger2 != NULL)
      myLaserLogger2->addInfoToLogPlain((*it).c_str());
  }

  // call our mapping started callbacks
  for (fit = myMappingBegunCallbacks.begin(); 
       fit != myMappingBegunCallbacks.end(); 
       fit++)
  {
    functor = (*fit);
    functor->invoke();
  }


  myRobot->unlock();
  
  
  ArLog::log(ArLog::Normal, "MappingStart: Map %s started", 
	     myMapName.c_str());
  sendPacket.byteToBuf(0);
  if (client != NULL)
    client->sendPacketTcp(&sendPacket);
  
  ArNetPacket broadcastPacket;
  broadcastPacket.strToBuf(myFileName.c_str());
  myServer->broadcastPacketTcp(&broadcastPacket, "mappingStatusBroadcast");
}
AREXPORT void ArServerHandlerMapping::serverMappingEnd(
	ArServerClient *client, ArNetPacket *packet)
{
  std::list<ArFunctor *>::iterator fit;

  ArNetPacket sendPacket;
  if (myLaserLogger == NULL)
  {
    ArLog::log(ArLog::Normal, "MappingEnd: No map being made");
    sendPacket.byteToBuf(1);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    return;
  }

  myRobot->lock();

  delete myLaserLogger;
  myLaserLogger = NULL;

  bool haveFile2 = false;

  if (myLaserLogger2 != NULL)
  {
    haveFile2 = true;
    delete myLaserLogger2;
    myLaserLogger2 = NULL;
  }
    
    
  std::list<std::string> sourceFileNameList;
  sourceFileNameList.push_back(myFileName);
  if (haveFile2) {
    sourceFileNameList.push_back(myFileName2);
  }

  bool isSuccess = myTempDirectoryHelper.moveFilesToBaseDirectory
                                           (sourceFileNameList);
  
  if (isSuccess) {
    sendPacket.uByte2ToBuf(0);
  }
  else {
    sendPacket.uByte2ToBuf(2);
  } 


  ArLog::log(ArLog::Normal, "MappingEnd: Stopped mapping %s", 
	           myFileName.c_str());

  // call our mapping end callbacks
  for (fit = myMappingEndCallbacks.begin(); 
       fit != myMappingEndCallbacks.end(); 
       fit++)
    (*fit)->invoke();
  
  
  // Call the mapping ended callbacks
  for (fit = myMappingEndedCallbacks.begin(); 
       fit != myMappingEndedCallbacks.end(); 
       fit++) {
    (*fit)->invoke();
  }

  // Clear the internal file names
  myMapName = "";
  myFileName = "";

  myRobot->unlock();
  if (client != NULL)
    client->sendPacketTcp(&sendPacket);

  ArNetPacket broadcastPacket;
  broadcastPacket.strToBuf(myFileName.c_str());
  myServer->broadcastPacketTcp(&broadcastPacket, "mappingStatusBroadcast");
}
Пример #11
0
void sendEmpty(ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendingPacket;
  sendingPacket.byteToBuf(0);
  client->sendPacketTcp(&sendingPacket);
}
AREXPORT void ArServerHandlerMapping::serverMappingEnd(
	ArServerClient *client, ArNetPacket *packet)
{
  std::list<ArFunctor *>::iterator fit;

  ArNetPacket sendPacket;
  if (myLaserLogger == NULL)
  {
    ArLog::log(ArLog::Normal, "MappingEnd: No map being made");
    sendPacket.byteToBuf(1);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    return;
  }

  myRobot->lock();
  delete myLaserLogger;
  myLaserLogger = NULL;

  bool haveFile2 = false;

  if (myLaserLogger2 != NULL)
  {
    haveFile2 = true;
    delete myLaserLogger2;
    myLaserLogger2 = NULL;
  }
    
  // now, if our temp directory and base directory are different we
  // need to move it and put the result in the packet, otherwise put
  // in we're okay
  if (ArUtil::strcasecmp(myBaseDirectory, myTempDirectory) != 0)
  {
#ifndef WIN32
    char *mvName = "mv";
#else
    char *mvName = "move";
#endif
    char systemBuf[6400];
    char fromBuf[1024];
    char toBuf[1024];

    char systemBuf2[6400];
    char fromBuf2[1024];
    char toBuf2[1024];

    if (myTempDirectory.size() > 0)
      snprintf(fromBuf, sizeof(fromBuf), "%s%s", 
	       myTempDirectory.c_str(), myFileName.c_str());
    else
      snprintf(fromBuf, sizeof(fromBuf), "%s", myFileName.c_str());
    ArUtil::fixSlashes(fromBuf, sizeof(fromBuf));

    if (myTempDirectory.size() > 0)
      snprintf(toBuf, sizeof(toBuf), "%s%s", 
	       myBaseDirectory.c_str(), myFileName.c_str());
    else
      snprintf(toBuf, sizeof(toBuf), "%s", myFileName.c_str());
    ArUtil::fixSlashes(toBuf, sizeof(toBuf));

    sprintf(systemBuf, "%s \"%s\" \"%s\"", mvName, fromBuf, toBuf);




    ArLog::log(ArLog::Verbose, "Moving with '%s'", systemBuf);
    // call our pre move callbacks
    for (fit = myPreMoveCallbacks.begin(); 
	 fit != myPreMoveCallbacks.end(); 
	 fit++)
      (*fit)->invoke();

    // move file
    int ret;
    if ((ret = system(systemBuf)) == 0)
    {
      ArLog::log(ArLog::Verbose, "ArServerHandlerMapping: Moved file %s (with %s)", 
		 myFileName.c_str(), systemBuf);
      sendPacket.byteToBuf(0);
    }
    else
    {
      ArLog::log(ArLog::Normal, 
		 "ArServerHandlerMapping: Couldn't move file for %s (ret of '%s' is %d) removing file", 
		 myFileName.c_str(), systemBuf, ret);
      unlink(fromBuf);
      sendPacket.uByte2ToBuf(2);
    }

    if (haveFile2)
    {
      if (myTempDirectory.size() > 0)
	snprintf(fromBuf2, sizeof(fromBuf2), "%s%s", 
		 myTempDirectory.c_str(), myFileName2.c_str());
      else
	snprintf(fromBuf2, sizeof(fromBuf2), "%s", myFileName2.c_str());
      ArUtil::fixSlashes(fromBuf2, sizeof(fromBuf2));
      
      if (myTempDirectory.size() > 0)
	snprintf(toBuf2, sizeof(toBuf2), "%s%s", 
		 myBaseDirectory.c_str(), myFileName2.c_str());
      else
	snprintf(toBuf2, sizeof(toBuf2), "%s", myFileName2.c_str());
      ArUtil::fixSlashes(toBuf2, sizeof(toBuf2));
      
      sprintf(systemBuf2, "%s \"%s\" \"%s\"", mvName, fromBuf2, toBuf2);
      
      if ((ret = system(systemBuf2)) == 0)
      {
	ArLog::log(ArLog::Verbose, "ArServerHandlerMapping: Moved file2 %s (with %s)", 
		   myFileName2.c_str(), systemBuf2);
      }
      else
      {
	ArLog::log(ArLog::Normal, 
		   "ArServerHandlerMapping: Couldn't move file2 for %s (ret of '%s' is %d) removing file", 
		   myFileName2.c_str(), systemBuf2, ret);
	unlink(fromBuf2);
      }
    }
    
    // call our post move callbacks
    for (fit = myPostMoveCallbacks.begin(); 
	 fit != myPostMoveCallbacks.end(); 
	 fit++)
      (*fit)->invoke();

  }
  else
  {
    // just put in things are okay, it'll get sent below
    sendPacket.byteToBuf(0);
  }


  ArLog::log(ArLog::Normal, "MappingEnd: Stopped mapping %s", 
	     myFileName.c_str());

  // call our mapping started callbacks
  for (fit = myMappingEndCallbacks.begin(); 
       fit != myMappingEndCallbacks.end(); 
       fit++)
    (*fit)->invoke();

  myMapName = "";
  myFileName = "";

  myRobot->unlock();
  if (client != NULL)
    client->sendPacketTcp(&sendPacket);

  ArNetPacket broadcastPacket;
  broadcastPacket.strToBuf(myFileName.c_str());
  myServer->broadcastPacketTcp(&broadcastPacket, "mappingStatusBroadcast");
}