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