C++ ArNetPacket::byte2ToBuf方法代码示例

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

AREXPORT void ArServerInfoSensor::getSensorCurrent(ArServerClient *client, 
					ArNetPacket *packet)
{
  ArRangeDevice *dev;
  char sensor[512];
  std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::iterator it;

  while (packet->getDataLength() > packet->getDataReadLength())
  {
    ArNetPacket sendPacket;

    // find out the sensor they want
    packet->bufToStr(sensor, sizeof(sensor));
    myRobot->lock();
    if ((dev = myRobot->findRangeDevice(sensor)) == NULL)
    {
      myRobot->unlock();
      ArLog::log(ArLog::Verbose, "ArServerInfoSensor::getSensorCurrent: No sensor %s", sensor);
      sendPacket.byte2ToBuf(-1);
      sendPacket.strToBuf(sensor);
      client->sendPacketUdp(&sendPacket);
      continue;
    }
    
    myRobot->unlock();
    dev->lockDevice();
    readings = dev->getCurrentBuffer();
    if (readings == NULL)
    {
      dev->unlockDevice();
      ArLog::log(ArLog::Verbose, "ArServerInfoSensor::getSensorCurrent: No current buffer for %s", sensor);
      sendPacket.byte2ToBuf(0);
      sendPacket.strToBuf(sensor);
      client->sendPacketUdp(&sendPacket);
      continue;
    } 
    
    sendPacket.byte2ToBuf(readings->size());
    sendPacket.strToBuf(sensor);
    for (it = readings->begin(); it != readings->end(); it++)
    {
      sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getX()));
      sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getY()));
    }
    dev->unlockDevice();
    client->sendPacketUdp(&sendPacket);
  }
  

}

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 void ArServerHandlerCamera::cameraUpdate(ArServerClient *client, 
				                                          ArNetPacket *packet)
{
  ArNetPacket send;

  myRobot->lock();
  int pan  = ArMath::roundInt(myCamera->getPan());
  int tilt = ArMath::roundInt(myCamera->getTilt());
  int zoom = ArMath::roundInt(myCamera->getZoom()/((double)(myCamera->getMaxZoom() - myCamera->getMinZoom()) + myCamera->getMinZoom()) * 100.0);

  myRobot->unlock();

  send.byte2ToBuf(pan);
  send.byte2ToBuf(tilt);
  send.byte2ToBuf(zoom);
  client->sendPacketUdp(&send);

} // end method cameraUpdate

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);
}

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 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 ArServerClient::processPacket(ArNetPacket *packet, bool tcp)
{
  std::string str;
  struct sockaddr_in sin;
  unsigned int clientUdpPort;
  ArNetPacket retPacket;

  //printf("Command number %d\n", packet->getCommand());
  // if we're in intro mode and received back the intro
  if (myState == STATE_SENT_INTRO && 
      packet->getCommand() == ArClientCommands::INTRODUCTION)
  {
    char user[512];
    unsigned char password[16];
    clientUdpPort = packet->bufToUByte2();
    packet->bufToStr(user, sizeof(user));
    packet->bufToData((char *)password, 16);

    if (myRejecting != 0)
    {
      retPacket.empty();
      retPacket.setCommand(ArServerCommands::REJECTED);
      retPacket.byte2ToBuf(myRejecting);
      retPacket.strToBuf(myRejectingString.c_str());
      sendPacketTcp(&retPacket);
      if (myRejecting == 2)
	ArLog::log(ArLog::Normal, 
   "%sRejected connection from %s since we're using a central server at %s",
		   myLogPrefix.c_str(), getIPString(), 
		   myRejectingString.c_str());
      internalSwitchState(STATE_REJECTED);
      return;
    }

    // if user info is NULL we're not checking passwords
    if (myUserInfo != NULL && 
	!myUserInfo->matchUserPassword(user, password, myPasswordKey.c_str(),
				       myServerKey.c_str(),
				       myLogPasswordFailureVerbosely))
    {
      retPacket.empty();
      retPacket.setCommand(ArServerCommands::REJECTED);
      retPacket.byte2ToBuf(1);
      retPacket.strToBuf("");
      sendPacketTcp(&retPacket);
      ArLog::log(ArLog::Normal, "%sRejected user '%s' or password from %s",
		 myLogPrefix.c_str(), user, getIPString());
      internalSwitchState(STATE_REJECTED);
      return;
    }
    if (myUserInfo != NULL)
      myGroups = myUserInfo->getUsersGroups(user);
    else
      myGroups.clear();
    sin.sin_family = AF_INET;
    sin.sin_addr = *myTcpSocket.inAddr();
    sin.sin_port = ArSocket::hostToNetOrder(clientUdpPort);
    if (myUserInfo != NULL)
      ArLog::log(ArLog::Normal, 
		 "%sClient connected from %s with user %s", 
		 myLogPrefix.c_str(), getIPString(), user);
    else
      ArLog::log(ArLog::Normal, 
		 "%sClient connected from %s", myLogPrefix.c_str(), 
		 getIPString());

    setUdpAddress(&sin);
    // send that we've connected
    retPacket.empty();
    retPacket.setCommand(ArServerCommands::CONNECTED);
    sendPacketTcp(&retPacket);
    // note that we're connected
    internalSwitchState(STATE_CONNECTED);

    // send them the list
    sendListPacket();
    // send the udp introduction if we're using udp
    if (!myTcpOnly)
    {
      retPacket.empty();
      retPacket.setCommand(ArServerCommands::UDP_INTRODUCTION);
      retPacket.byte4ToBuf(myIntroKey);
      sendPacketUdp(&retPacket);
    }
  }
  // if we aren't in intro mode and got an intro somethings wrong
  else if (packet->getCommand() == ArClientCommands::INTRODUCTION)
  {
    ArLog::log(ArLog::Terse, 
	       "%sReceived introduction when not in intro mode",
	       myLogPrefix.c_str());
    return;
  }
  // if we got this over tcp then they only want tcp
  else if (packet->getCommand() == ArClientCommands::UDP_INTRODUCTION)
  {
    if (!myTcpOnly)
    {
      ArLog::log(ArLog::Normal, "%sGot UDP introduction over tcp, assuming client only wants tcp data.", myLogPrefix.c_str());
      myTcpOnly = true;
//.........这里部分代码省略.........

AREXPORT void ArServerHandlerCameraCollection::getCameraList(ArServerClient *client,
        ArNetPacket *packet)
{
    if (client == NULL) {
        return; // Something very bad has happened...
    }

    ArNetPacket sendPacket;

    if (myCameraCollection == NULL) {
        sendPacket.byte2ToBuf(0);
        client->sendPacketTcp(&sendPacket);
    }

    // This lack of recursive locks is troublesome... Data might
    // change between calls...

    std::list<std::string> cameraNames;
    myCameraCollection->getCameraNames(cameraNames);

    sendPacket.byte2ToBuf(cameraNames.size());

    for (std::list<std::string>::iterator iter = cameraNames.begin();
            iter != cameraNames.end();
            iter++) {

        const char *curName = iter->c_str();

        sendPacket.strToBuf(curName);

        // TODO: ArNetPacket will NOT behave correctly if the given str is NULL
        // Fix this somehow...

        sendPacket.strToBuf(myCameraCollection->getCameraType(curName));
        sendPacket.strToBuf(myCameraCollection->getDisplayName(curName));
        sendPacket.strToBuf(myCameraCollection->getDisplayType(curName));

        // Send commands...

        std::list<std::string> commands;
        myCameraCollection->getCameraCommands(curName, commands);

        sendPacket.byte2ToBuf(commands.size());

        for (std::list<std::string>::iterator comIter = commands.begin();
                comIter != commands.end();
                comIter++) {

            const char *curCommand = comIter->c_str();

            sendPacket.strToBuf(curCommand);
            sendPacket.strToBuf(myCameraCollection->getCommandName(curName, curCommand));
            sendPacket.byte4ToBuf(myCameraCollection->getRequestInterval(curName, curCommand));

        } // end for each command

        // Send parameters...

        std::list<std::string> params;
        myCameraCollection->getParameterNames(curName, params);

        sendPacket.byte2ToBuf(params.size());

        ArConfigArg arg;
        ArClientArg clientArg;

        bool isSuccess = true;

        for (std::list<std::string>::iterator paramIter = params.begin();
                paramIter != params.end();
                paramIter++) {

            const char *paramName = paramIter->c_str();

            isSuccess = myCameraCollection->getParameter(curName,
                        paramName,
                        arg);
            if (!isSuccess) {
                ArLog::log(ArLog::Normal,
                           "ArServerHandlerCameraCollection::getCameraList() could not find param %s", paramName);
                continue;
            }

            // Add the current parameter to the packet
            isSuccess = clientArg.createPacket(arg, &sendPacket);

        } // end for each parameter

    } // end for each camera

    client->sendPacketTcp(&sendPacket);

} // end method getCameraList