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;
}
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
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");
}
}
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 ¶m = (*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");
}