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;
}
return;
}
// if we're connected and got a udp confirmation
else if ((myState == STATE_CONNECTED ||
myState == STATE_SENT_INTRO) &&
packet->getCommand() == ArClientCommands::UDP_CONFIRMATION)
{
myUdpConfirmedTo = true;
ArLog::log(myVerboseLogLevel,
"%s: udp connection to client confirmed.", myLogPrefix.c_str());
return;
}
// if we're not connected (or close) and got a confirmation
else if (packet->getCommand() == ArClientCommands::UDP_CONFIRMATION)
{
ArLog::log(ArLog::Normal,
"%sReceived udp confirmation when not connected.",
myLogPrefix.c_str());
return;
}
else if (packet->getCommand() == ArClientCommands::TCP_ONLY)
{
ArLog::log(myVerboseLogLevel, "%sClient only wants tcp data.", myLogPrefix.c_str());
myTcpOnly = true;
return;
}
else if (packet->getCommand() == ArClientCommands::SHUTDOWN)
{
ArLog::log(ArLog::Normal, "%sClient from %s has disconnected.",
myLogPrefix.c_str(), getIPString());
internalSwitchState(STATE_DISCONNECTED);
return;
}
// if we're connected its a request, then set all that up
else if (myState == STATE_CONNECTED &&
packet->getCommand() == ArClientCommands::REQUEST)
{
std::list<ArServerClientData *>::iterator it;
ArServerClientData *data;
ArServerData *serverData;
unsigned int command;
long mSec;
// see which one they requested
command = packet->bufToUByte2();
mSec = packet->bufToByte4();
// first we see if we already have this one
for (it = myRequested.begin(); it != myRequested.end(); ++it)
{
data = (*it);
serverData = data->getServerData();
if (serverData->getCommand() == command)
{
trackPacketReceived(packet, command);
data->setMSec(mSec);
data->setPacket(packet);
data->getPacket()->setCommand(command);
serverData->callRequestChangedFunctor();
ArLog::log(myVerboseLogLevel,
"%sRevised request for command %s to %d mSec with new argument",
myLogPrefix.c_str(),
findCommandName(serverData->getCommand()), mSec);
return;
}
}
// we didn't have it, so make a new one
std::map<unsigned int, ArServerData *>::iterator sdit;
if ((sdit = myDataMap->find(command)) == myDataMap->end())
{
ArLog::log(ArLog::Terse,
"%sGot request for command %d which doesn't exist",
myLogPrefix.c_str(), command);
return;
}
serverData = (*sdit).second;
if (serverData == NULL)
{
ArLog::log(ArLog::Terse,
"%sprocessPackets request handler has NULL serverData",
myLogPrefix.c_str());
}
if (myUserInfo != NULL &&
serverData->getCommandGroup() != NULL &&
serverData->getCommandGroup()[0] != '\0' &&
myGroups.count(serverData->getCommandGroup()) == 0 &&
myGroups.count("all") == 0)
{
ArLog::log(ArLog::Normal,
"%s%s tried to request command '%s' but it doesn't have access to that command",
myLogPrefix.c_str(), getIPString(),
serverData->getName());
return;
}
trackPacketReceived(packet, command);
data = new ArServerClientData(serverData, mSec, packet);
data->getPacket()->setCommand(command);
ArLog::log(myVerboseLogLevel,
"%sadded request for command %s every %d mSec",
myLogPrefix.c_str(), serverData->getName(), mSec);
if (mSec == 0)
ArLog::log(ArLog::Normal, "%sClient from %s requested command %s every at 0 msec", myLogPrefix.c_str(),
getIPString(), serverData->getName());
myRequested.push_front(data);
serverData->callRequestChangedFunctor();
pushCommand(command);
pushForceTcpFlag(false);
if (serverData->getFunctor() != NULL)
serverData->getFunctor()->invoke(this, data->getPacket());
popCommand();
popForceTcpFlag();
return;
}
// if we got a request when we're not connected
else if (packet->getCommand() == ArClientCommands::REQUEST)
{
ArLog::log(ArLog::Normal,
"Got a request while not connected.",
myLogPrefix.c_str());
return;
}
// if we're connected its a requestStop, then set all that up
else if (myState == STATE_CONNECTED &&
packet->getCommand() == ArClientCommands::REQUESTSTOP)
{
std::list<ArServerClientData *>::iterator it;
ArServerClientData *data;
ArServerData *serverData;
unsigned int command;
// see which one they requested
command = packet->bufToUByte2();
// first we see if we have this one
for (it = myRequested.begin(); it != myRequested.end(); ++it)
{
data = (*it);
serverData = data->getServerData();
// we have a match, so set the new params then return
if (data->getServerData()->getCommand() == command)
{
trackPacketReceived(packet, command);
myRequested.erase(it);
ArLog::log(myVerboseLogLevel, "%sStopped request for command %s",
myLogPrefix.c_str(),
findCommandName(serverData->getCommand()));
delete data;
serverData->callRequestChangedFunctor();
return;
}
}
// if we don't have it... that means that it wasn't here
// find out what to call it
std::map<unsigned int, ArServerData *>::iterator sdit;
if ((sdit = myDataMap->find(command)) == myDataMap->end())
{
ArLog::log(ArLog::Terse,
"%sGot a requeststop for command %d which doesn't exist",
myLogPrefix.c_str(), command);
return;
}
trackPacketReceived(packet, command);
serverData = (*sdit).second;
if (serverData == NULL)
ArLog::log(ArLog::Terse,
"%srequeststop handler has NULL serverData on back command %d",
myLogPrefix.c_str(), command);
else
ArLog::log(ArLog::Normal, "%s: Got a stop request for command %s that isn't requested", myLogPrefix.c_str(), serverData->getName());
return;
}
// if we got a requestStop when we're not connected
else if (packet->getCommand() == ArClientCommands::REQUESTSTOP)
{
ArLog::log(ArLog::Normal,
"%sGot a requeststop while not connected.",
myLogPrefix.c_str());
return;
}
// if we're connected and its a command to execute just once
else if (myState == STATE_CONNECTED)
{
unsigned int command;
std::map<unsigned int, ArServerData *>::iterator it;
ArServerData *serverData;
command = packet->getCommand();
if ((it = myDataMap->find(command)) == myDataMap->end())
{
ArLog::log(ArLog::Terse,
"%sArServerClient got request for command %d which doesn't exist",
myLogPrefix.c_str(), command);
return;
}
serverData = (*it).second;
if (myUserInfo != NULL &&
serverData->getCommandGroup() != NULL &&
serverData->getCommandGroup()[0] != '\0' &&
myGroups.count(serverData->getCommandGroup()) == 0 &&
myGroups.count("all") == 0)
{
ArLog::log(ArLog::Normal,
"%s%s tried to request command '%s' once but it doesn't have access to that command", myLogPrefix.c_str(),
getIPString(),
serverData->getName());
return;
}
trackPacketReceived(packet, command);
// copy it out and return if its an idle packet
if (myAllowIdlePackets && serverData->isIdlePacket())
{
myHaveIdlePackets = true;
if (command <= 255)
ArLog::log(myVerboseLogLevel, "%sStoring idle command %d",
myLogPrefix.c_str(), command);
else
ArLog::log(myVerboseLogLevel, "%sStoring idle command %s",
myLogPrefix.c_str(), serverData->getName());
myIdlePacketsMutex.lock();
ArNetPacket *idlePacket = new ArNetPacket(packet->getLength() + 5);
idlePacket->duplicatePacket(packet);
myIdlePackets.push_back(idlePacket);
myIdlePacketsMutex.unlock();
return;
}
// If its a slow or an idle packet (and we're not allowing the
// idle behavior) and we allow slow packets then copy it
else if (myAllowSlowPackets && (serverData->isSlowPacket() ||
serverData->isIdlePacket()))
{
myHaveSlowPackets = true;
if (command <= 255)
ArLog::log(myVerboseLogLevel, "%sStoring slow command %d",
myLogPrefix.c_str(), command);
else
ArLog::log(myVerboseLogLevel, "%sStoring slow command %s",
myLogPrefix.c_str(), serverData->getName());
mySlowPacketsMutex.lock();
ArNetPacket *slowPacket = new ArNetPacket(packet->getLength() + 5);
slowPacket->duplicatePacket(packet);
mySlowPackets.push_back(slowPacket);
mySlowPacketsMutex.unlock();
return;
}
if (command <= 255)
ArLog::log(myVerboseLogLevel, "%sGot command %s",
myLogPrefix.c_str(), serverData->getName());
else
ArLog::log(ArLog::Verbose, "%sGot command %s",
myLogPrefix.c_str(), serverData->getName());
pushCommand(command);
pushForceTcpFlag(tcp);
if (serverData->getFunctor() != NULL)
serverData->getFunctor()->invoke(this, packet);
if (serverData->getRequestOnceFunctor() != NULL)
serverData->getRequestOnceFunctor()->invoke(this, packet);
popCommand();
popForceTcpFlag();
return;
}
else
{
ArLog::log(ArLog::Terse,
"%sRogue packet command %s in state %d", myLogPrefix.c_str(),
findCommandName(packet->getCommand()), myState);
}
}
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