void ArServerInfoRobot::userTask(void)
{
  ArServerMode *netMode;
  ArNetPacket sending;
  
  if ((netMode = ArServerMode::getActiveMode()) != NULL)
  {
    myStatus = netMode->getStatus();
    myExtendedStatus = netMode->getExtendedStatus();
    if (myExtendedStatus.empty())
      myExtendedStatus = myStatus;
    myMode = netMode->getMode();
  }
  else 
  {
    myStatus = "Unknown status";
    myExtendedStatus = "Unknown extended status";
    myMode = "Unknown mode";
  }
  if (myStatus != myOldStatus || myMode != myOldMode || 
      myExtendedStatus != myOldExtendedStatus)
  {
    sending.strToBuf(myStatus.c_str());
    sending.strToBuf(myMode.c_str());
    sending.strToBuf(myExtendedStatus.c_str());
    myServer->broadcastPacketTcp(&sending, "updateStrings");
  }
  myOldStatus = myStatus;
  myOldMode = myMode;
  myOldExtendedStatus = myExtendedStatus;
}
Beispiel #2
0
AREXPORT void ArServerHandlerPopup::netPopupList(ArServerClient *client,
						 ArNetPacket *packet)
{
  ArLog::log(ArLog::Normal, "Sending popup list");

  std::map<ArTypes::Byte4, PopupData *>::iterator it;
  ArNetPacket sendingPacket;
  PopupData *popupData;

  myDataMutex.lock();
  for (it = myMap.begin(); it != myMap.end(); it++)
  {
    popupData = (*it).second;
    sendingPacket.empty();

    ArLog::log(ArLog::Normal, "Sending popup %d", popupData->myID);
    buildPacket(&sendingPacket, popupData);
    sendingPacket.setCommand(myServer->findCommandFromName("popupCreate"));

    client->sendPacketTcp(&sendingPacket);
  }

  sendingPacket.empty();
  client->sendPacketTcp(&sendingPacket);
  ArLog::log(ArLog::Normal, "Sent popups");
  myDataMutex.unlock();
}
AREXPORT void ArServerHandlerCamera::cameraInfo(ArServerClient *client, 
												                        ArNetPacket *packet)
{
  ArNetPacket send;

  myRobot->lock();

  int minPan = ArMath::roundInt(myCamera->getMaxNegPan());
  int maxPan = ArMath::roundInt(myCamera->getMaxPosPan());
  int minTilt = ArMath::roundInt(myCamera->getMaxNegTilt());
  int maxTilt = ArMath::roundInt(myCamera->getMaxPosTilt());
  int minZoom = 0;
  int maxZoom = 100;
  bool isZoomAvailable = myCamera->canZoom();
  //ArLog::log(ArLog::Normal, "minPan %d maxPan %d minTilt %d maxTilt %d minZoom %d maxZoom %d isZoomAvailable %d", minPan, maxPan, minTilt, maxTilt, minZoom, maxZoom, isZoomAvailable);

  myRobot->unlock();

  send.byte2ToBuf(minPan);
  send.byte2ToBuf(maxPan);
  send.byte2ToBuf(minTilt);
  send.byte2ToBuf(maxTilt);
  send.byte2ToBuf(minZoom);
  send.byte2ToBuf(maxZoom);
  send.byteToBuf(isZoomAvailable);

  client->sendPacketUdp(&send);

} // end method cameraInfo
Beispiel #4
0
AREXPORT void ArServerHandlerPopup::closePopup(ArTypes::Byte4 id, 
						const char *closeMessage)
{
  ArNetPacket sendingPacket;
  PopupData *popupData;
  std::map<ArTypes::Byte4, PopupData *>::iterator it;

  myDataMutex.lock();
  if ((it = myMap.find(id)) == myMap.end())
  {
    ArLog::log(ArLog::Verbose, 
       "Cannot close popup %u as it doesn't exist (anymore at least)", 
	       id);
    myDataMutex.unlock();
  }
  else
  {
    popupData = (*it).second;
    sendingPacket.byte4ToBuf(id);
    sendingPacket.strToBuf(closeMessage);
    myMap.erase(id);
    myDataMutex.unlock();
    delete popupData;
    if (popupData->myCallback != NULL)
      popupData->myCallback->invoke(popupData->myID, -2);
    myServer->broadcastPacketTcp(&sendingPacket, "popupClose");
  }
}
Beispiel #5
0
void sendPoseRobot(ArServerClient* client, ArNetPacket* packet) {
	ArNetPacket reply;
	ArPose pose = gotoGoal.getPose();
	reply.doubleToBuf(pose.getX());
	reply.doubleToBuf(pose.getY());
    client->sendPacketUdp(&reply);
}
AREXPORT void ArServerInfoSensor::getSensorList(ArServerClient *client, 
					  ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  std::list<ArRangeDevice *> *devList;
  std::list<ArRangeDevice *>::iterator it;

  myRobot->lock();
  devList = myRobot->getRangeDeviceList();
  
  if (devList == NULL)
  {
    myRobot->unlock();
    client->sendPacketUdp(&sendPacket);
    return;
  }
  
  sendPacket.byte2ToBuf(devList->size());

  for (it = devList->begin(); it != devList->end(); it++)
  {
    sendPacket.strToBuf((*it)->getName());
  }
  myRobot->unlock();
  client->sendPacketUdp(&sendPacket);
}
AREXPORT bool ArCentralForwarder::requestOnceWithString(const char *name, const char *str)
{
    ArNetPacket tempPacket;
    tempPacket.strToBuf(str);

    return requestOnce(name, &tempPacket);
}
void testFunction(ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sending;
  printf("responding to a packet of command %d\n", packet->getCommand());
  sending.strToBuf ("Laser data");
  client->sendPacketTcp(&sending);
}
AREXPORT void ArServerInfoDrawings::netRangeDeviceCumulative(
	ArServerClient *client, ArNetPacket *packet, ArRangeDevice *device)
{
  ArNetPacket sendPacket;
  std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::iterator it;

  device->lockDevice();
  readings = device->getCumulativeBuffer();
  if (readings == NULL)
  {
    ArLog::log(ArLog::Verbose, "ArServerInfoDrawing::netRangeDeviceCumulative: No cumulative buffer for %s", device->getName());
    device->unlockDevice();
    sendPacket.byte4ToBuf(0);
    client->sendPacketUdp(&sendPacket);
    return;
  } 
  
  sendPacket.byte4ToBuf(readings->size());
  for (it = readings->begin(); it != readings->end(); it++)
  {
    sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getX()));
    sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getY()));
  }
  device->unlockDevice();
  client->sendPacketUdp(&sendPacket);

}
/** @internal */
AREXPORT void ArServerHandlerMap::writeMapToClient(const char *line,
					 ArServerClient *client)
{
  ArNetPacket sendPacket;
  sendPacket.strToBuf(line);
  client->sendPacketTcp(&sendPacket);
}
/** @internal */
AREXPORT void ArServerHandlerMap::serverGetGoals(ArServerClient *client, 
						 ArNetPacket *packet)
{
  std::list<ArMapObject *>::iterator objIt;
  ArMapObject* obj;
  ArPose goal;  
  ArNetPacket sendPacket;

  for (objIt = myMap->getMapObjects()->begin(); 
       objIt != myMap->getMapObjects()->end(); 
       objIt++)
  {
    //
    // Get the forbidden lines and fill the occupancy grid there.
    //
    obj = (*objIt);
    if (obj == NULL)
      break;
    if (strcasecmp(obj->getType(), "GoalWithHeading") == 0 ||
	strcasecmp(obj->getType(), "Goal") == 0)
    {
      sendPacket.strToBuf(obj->getName());
    }
  }
  client->sendPacketTcp(&sendPacket);
}
AREXPORT void ArServerHandlerConfig::getConfigSectionFlags(
	ArServerClient *client, ArNetPacket *packet)
{
  ArLog::log(ArLog::Normal, "Config section flags requested.");

  ArNetPacket sending;

  std::list<ArConfigSection *> *sections = myConfig->getSections();
  std::list<ArConfigSection *>::iterator sIt;
  sending.byte4ToBuf(sections->size());

  for (sIt = sections->begin(); sIt != sections->end(); sIt++)
  {

    ArConfigSection *section = (*sIt);
    if (section == NULL) 
    {
      sending.strToBuf("");
      sending.strToBuf("");
    }
    else
    {
      sending.strToBuf(section->getName());
      sending.strToBuf(section->getFlags());
    }
  }
  client->sendPacketTcp(&sending);
}
void AriaClientDriver::sendInput()
{
  /* This method is called by the main function to send a ratioDrive
   * request with our current velocity values. If the server does
   * not support the ratioDrive request, then we abort now: */
  if(!myClient->dataExists("ratioDrive")) return;

  /* Construct a ratioDrive request packet.  It consists
   * of three doubles: translation ratio, rotation ratio, and an overall scaling
   * factor. */



  ArNetPacket packet;
  packet.doubleToBuf(myTransRatio);
  packet.doubleToBuf(myRotRatio);
  packet.doubleToBuf(myMaxVel); // use half of the robot's maximum.
  packet.doubleToBuf(myLatRatio);
  if (myPrinting)
    printf("Sending\n");
  myClient->requestOnce("ratioDrive", &packet);
  myTransRatio = 0;
  myRotRatio = 0;
  myLatRatio = 0;
}
void InputHandler::space(void)
{
  ArNetPacket packet;
  packet.doubleToBuf(0.00001);
  myClient->requestOnce("setVel", &packet);
  myClient->requestOnce("deltaHeading", &packet);
}
AREXPORT void ArServerHandlerMapping::serverMappingStatus(
	ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  sendPacket.strToBuf(myMapName.c_str());
  client->sendPacketTcp(&sendPacket);
}
void RobotClient::turnLeft()
{
    ArNetPacket sendPacket;
    this->setRadio(this->getRadio()+3);
    sendPacket.byte4ToBuf((ArTypes::Byte4)0.0);
    sendPacket.byte4ToBuf((ArTypes::Byte4)this->mRadio);
    pClient->requestOnce("setMoveInfo", &sendPacket);
}
void RobotClient::goBack()
{
    ArNetPacket sendPacket;
    this->setSpeed(this->getSpeed()-100);
    sendPacket.byte4ToBuf((ArTypes::Byte4)this->mSpeed);
    sendPacket.byte4ToBuf((ArTypes::Byte4)0.0);
    pClient->requestOnce("setMoveInfo", &sendPacket);
}
void exampleArrowsDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt) {
  // 1 Arrow that points at the robot
  ArNetPacket reply;
  reply.byte4ToBuf(1);
  reply.byte4ToBuf(0);      // Pos. X
  reply.byte4ToBuf(700);   // Pos. Y
  client->sendPacketUdp(&reply);
}
AREXPORT void ArServerClient::shutdown(void)
{
  ArNetPacket packet;
  
  packet.setCommand(ArServerCommands::SHUTDOWN);
  sendPacketTcp(&packet);
  myTcpSender.sendData();
}
/**
   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();
}
void RobotClient::stop()
{
    ArNetPacket sendPacket;
    this->setRadio(0);
    this->setSpeed(0);
    sendPacket.byte4ToBuf((ArTypes::Byte4)this->mSpeed);
    sendPacket.byte4ToBuf((ArTypes::Byte4)this->mRadio);
    pClient->requestOnce("setMoveInfo", &sendPacket);
}
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 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);
}
AREXPORT void ArHybridForwarderVideo::finishConstructor(void)
{
  myClient->lock();
  mySendVideoSizeCB = new ArFunctor2C<ArHybridForwarderVideo, ArServerClient*, 
  ArNetPacket *>(this, &ArHybridForwarderVideo::sendVideoSize);

  mySendVideoCB = new ArFunctor2C<ArHybridForwarderVideo, ArServerClient*, 
  ArNetPacket *>(this, &ArHybridForwarderVideo::sendVideo);
  
  myReceiveVideoSizeCB = new ArFunctor1C<ArHybridForwarderVideo, 
  ArNetPacket *>(this, &ArHybridForwarderVideo::receiveVideoSize);

  myReceiveVideoCB = new ArFunctor1C<ArHybridForwarderVideo, 
  ArNetPacket *>(this, &ArHybridForwarderVideo::receiveVideo);

  myClientCycleCB = new ArFunctorC<ArHybridForwarderVideo>(
	  this, &ArHybridForwarderVideo::clientCycleCallback);

  myReqSent = false;
  myLastReqSent.setToNow();
  myLastReceivedVideo.setToNow();
  myVideoRequestTime = 100;
  myForwardingVideo = false;

  if (myClient != NULL && myServer != NULL && myClient->isConnected())
  {
    myClient->addCycleCallback(myClientCycleCB);
    if (myClient->dataExists("videoSize"))
    {
      myServer->addData("videoSize", 
			"gets the width and height of the video data", 
			mySendVideoSizeCB, 
			"none", "uByte2: width, uByte2: height",
			"Video", "RETURN_SINGLE");
      myClient->addHandler("videoSize", myReceiveVideoSizeCB);
      myClient->requestOnce("videoSize");
    }
    if (myClient->dataExists("sendVideo"))
    {
      ArLog::log(ArLog::Normal, "Forwarding video.");
      myForwardingVideo = true;
      myClient->addHandler("sendVideo", myReceiveVideoCB);
      ArNetPacket packet;
      packet.uByteToBuf(90);
      myClient->requestOnce("sendVideo", &packet);

      myIsSendVideoAvailable = 
        myServer->addData("sendVideo", "gets video from the robot's camera (you should requestOnce this, you shouldn't request it, since you could easily fill the bandwidth that way)",
			                    mySendVideoCB, "uByte: quality (0 - 100)", 
			  "out: uByte2: width, uByte2: height, (len - readLen)*uByte: jpegData", "Video", "RETURN_VIDEO");
    }
  }
  myClient->unlock();
}
AREXPORT void ArServerInfoRobot::updateStrings(ArServerClient *client, 
					       ArNetPacket *packet)
{
  ArNetPacket sending;

  myRobot->lock();
  sending.strToBuf(myStatus.c_str());
  sending.strToBuf(myMode.c_str());
  myRobot->unlock();

  client->sendPacketTcp(&sending);
}
int main(int argc, char **argv)
{
  Aria::init();
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  ArClientBase client;

  ArArgumentParser parser(&argc, argv);

  ArClientSimpleConnector clientConnector(&parser);
  parser.loadDefaultArguments();

  if (!clientConnector.parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    clientConnector.logOptions();
    exit(0);
  }

  if (parser.getArgc() < 4 || parser.getArgc() > 6)
  {
    printf("usage: %s <x> <y> <th> <optional:xyspread> <optional:thspread>", argv[0]);
    exit(1);
  }

  if (!clientConnector.connectClient(&client))
  {
    if (client.wasRejected())
      printf("Server '%s' rejected connection, exiting\n", client.getHost());
    else
      printf("Could not connect to server '%s', exiting\n", client.getHost());
    exit(1);
  } 
  client.runAsync();

  ArNetPacket sending;
  // put in the arguments (you can see what they are from doing -lcl on clientDemo)
  sending.byte4ToBuf(atoi(parser.getArg(1)));
  sending.byte4ToBuf(atoi(parser.getArg(2)));
  sending.byte4ToBuf(atoi(parser.getArg(3)));
  if (parser.getArgc() > 4)
    sending.uByte4ToBuf(atoi(parser.getArg(4)));
  if (parser.getArgc() > 5)
    sending.uByte4ToBuf(atoi(parser.getArg(5)));
  // send the packet
  client.requestOnce("localizeToPose", &sending);

  // you have to give the client some time to send the command
  ArUtil::sleep(500);
  

  Aria::shutdown();
  return 0;
}
// Method called by accessor methods when properties changed. This reconstructs
// the myReply packet sent in response to requests from clients
void Circle::regenerate()
{
  myMutex.lock();
  myReply.empty();
  myReply.byte4ToBuf(myNumPoints);
  double a = 360.0/myNumPoints;
  for(unsigned int i = 0; i < myNumPoints; ++i)
  {
    myReply.byte4ToBuf(ArMath::roundInt(myPos.getX()+ArMath::cos(i*a)*myRadius)); // X
    myReply.byte4ToBuf(ArMath::roundInt(myPos.getY()+ArMath::sin(i*a)*myRadius)); // Y
  }
  myMutex.unlock();
}
AREXPORT void ArClientHandlerConfig::saveConfigToServer(
	  ArConfig *config, 
	  const std::set<std::string, ArStrCaseCmpOp> *ignoreTheseSections)
{
  //ArConfigArg param;
  ArClientArg clientArg;

  ArNetPacket sending;
  ArLog::log(ArLog::Normal, "%sSaving config to server", myLogPrefix.c_str());

  myDataMutex.lock();
  std::list<ArConfigSection *> *sections = config->getSections();
  for (std::list<ArConfigSection *>::iterator sIt = sections->begin(); 
       sIt != sections->end(); 
       sIt++)
  {
    ArConfigSection *section = (*sIt);
    // if we're ignoring sections and we're ignoring this one, then
    // don't send it
    if (ignoreTheseSections != NULL && 
	(ignoreTheseSections->find(section->getName()) != 
	 ignoreTheseSections->end()))
    {
      ArLog::log(ArLog::Verbose, "Not sending section %s", 
		 section->getName());
      continue;
    }
    sending.strToBuf("Section");
    sending.strToBuf(section->getName());
    std::list<ArConfigArg> *params = section->getParams();

    for (std::list<ArConfigArg>::iterator pIt = params->begin(); 
         pIt != params->end(); 
         pIt++)
    {
      ArConfigArg &param = (*pIt);

      if (!clientArg.isSendableParamType(param)) {
        continue;
      }

      sending.strToBuf(param.getName());

      clientArg.argTextToBuf(param, &sending);

    } // end for each param
  } // end for each section

  myDataMutex.unlock();
  myClient->requestOnce("setConfig", &sending);
}
AREXPORT void ArServerHandlerCommMonitor::handleGetHeartbeatInterval
                                              (ArServerClient *client, 
                                               ArNetPacket *packet)
{
  if (client == NULL) {
    return; // Something very bad has happened...
  }

  ArNetPacket sendPacket;
  sendPacket.uByte4ToBuf(myHeartbeatInterval);

  client->sendPacketTcp(&sendPacket);

} // end method handleGetHeartbeatInterval
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");
}