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 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);
}
int main(int argc, char **argv)
{
Aria::init();
ArClientBase client;
std::string host;
ArArgumentParser parser(&argc, argv);
ArClientSimpleConnector clientConnector(&parser);
parser.loadDefaultArguments();
/* Check for -help, and unhandled arguments: */
if (!clientConnector.parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
clientConnector.logOptions();
exit(0);
}
/* Connect our client object to the remote server: */
if (!clientConnector.connectClient(&client))
{
if (client.wasRejected())
printf("Server rejected connection, exiting\n");
else
printf("Could not connect to server, exiting\n");
exit(1);
}
client.addHandler("getCameraModeListCam1",
new ArGlobalFunctor1<ArNetPacket *>(&getCameraModeList));
client.requestOnce("getCameraModeListCam1");
ArNetPacket sending;
// This does the look at goal mode
sending.empty();
sending.strToBuf("lookAtGoal");
client.requestOnce("setCameraModeCam1", &sending);
// This does the look at point mode (at 0 0);
sending.empty();
sending.strToBuf("lookAtPoint");
sending.byte4ToBuf(1157);
sending.byte4ToBuf(-15786);
client.requestOnce("setCameraModeCam1", &sending);
while (Aria::getRunning() && client.isConnected())
{
client.loopOnce();
ArUtil::sleep(1000);
}
Aria::shutdown();
return 0;
};
AREXPORT void ArServerInfoDrawings::netGetDrawingList(ArServerClient *client,
ArNetPacket *packet)
{
ArNetPacket sendingPacket;
// TODO: Any need to protect the map by a mutex?
for (std::map<std::string, ArDrawingData *, ArStrCaseCmpOp>::iterator it =
myDrawingDatas.begin();
it != myDrawingDatas.end();
it++)
{
sendingPacket.empty();
sendingPacket.strToBuf((*it).first.c_str());
sendingPacket.strToBuf((*it).second->getShape());
sendingPacket.byte4ToBuf((*it).second->getPrimaryColor().colorToByte4());
sendingPacket.byte4ToBuf((*it).second->getSize());
sendingPacket.byte4ToBuf((*it).second->getLayer());
sendingPacket.uByte4ToBuf((*it).second->getDefaultRefreshTime());
sendingPacket.byte4ToBuf((*it).second->getSecondaryColor().colorToByte4());
sendingPacket.strToBuf((*it).second->getVisibility());
client->sendPacketTcp(&sendingPacket);
} // end for each drawing
sendingPacket.empty();
client->sendPacketTcp(&sendingPacket);
}
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);
}
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 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);
}
/** @internal */
AREXPORT void ArServerHandlerMap::serverGetMapName(ArServerClient *client,
ArNetPacket *packet)
{
ArNetPacket sendPacket;
if (myMap == NULL)
{
sendPacket.strToBuf("");
client->sendPacketTcp(&sendPacket);
}
else
{
sendPacket.strToBuf(myMap->getFileName());
client->sendPacketTcp(&sendPacket);
}
}
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 (!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;
// We have to tell it what section we're sending
sending.strToBuf("Section");
// The map is in the files section
sending.strToBuf("Files");
// The parameter name
sending.strToBuf("Map");
// The value of the parameter
sending.strToBuf("entire2.map");
// you could put in however many of these you want...
client.requestOnce("setConfig", &sending);
// you have to give the client some time to send the command
ArUtil::sleep(500);
Aria::shutdown();
return 0;
}
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 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");
}
}
/** @internal */
AREXPORT void ArServerHandlerMap::writeMapToClient(const char *line,
ArServerClient *client)
{
ArNetPacket sendPacket;
sendPacket.strToBuf(line);
client->sendPacketTcp(&sendPacket);
}
AREXPORT bool ArCentralForwarder::requestOnceWithString(const char *name, const char *str)
{
ArNetPacket tempPacket;
tempPacket.strToBuf(str);
return requestOnce(name, &tempPacket);
}
AREXPORT void ArServerHandlerMapping::serverMappingStatus(
ArServerClient *client, ArNetPacket *packet)
{
ArNetPacket sendPacket;
sendPacket.strToBuf(myMapName.c_str());
client->sendPacketTcp(&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);
}
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);
}
/** @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 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 ArServerInfoDrawings::netListDrawings(ArServerClient *client,
ArNetPacket *packet)
{
ArNetPacket sendingPacket;
std::map<std::string, ArDrawingData *, ArStrCaseCmpOp>::iterator it;
sendingPacket.byte4ToBuf(myDrawingDatas.size());
for (it = myDrawingDatas.begin(); it != myDrawingDatas.end(); it++)
{
sendingPacket.strToBuf((*it).first.c_str());
sendingPacket.strToBuf((*it).second->getShape());
sendingPacket.byte4ToBuf((*it).second->getPrimaryColor().colorToByte4());
sendingPacket.byte4ToBuf((*it).second->getSize());
sendingPacket.byte4ToBuf((*it).second->getLayer());
sendingPacket.uByte4ToBuf((*it).second->getDefaultRefreshTime());
sendingPacket.byte4ToBuf((*it).second->getSecondaryColor().colorToByte4());
}
client->sendPacketTcp(&sendingPacket);
}
AREXPORT void ArServerHandlerPopup::netPopupClicked(ArServerClient *client,
ArNetPacket *packet)
{
ArNetPacket sendingPacket;
PopupData *popupData;
std::map<ArTypes::Byte4, PopupData *>::iterator it;
ArTypes::Byte4 id;
ArTypes::Byte button;
id = packet->bufToByte4();
button = packet->bufToByte();
myDataMutex.lock();
if ((it = myMap.find(id)) == myMap.end())
{
ArLog::log(ArLog::Verbose,
"Cannot close popup %u for client %s as it doesn't exist (anymore at least)",
id, client->getIPString());
myDataMutex.unlock();
}
else
{
popupData = (*it).second;
sendingPacket.byte4ToBuf(id);
if (button == 0)
sendingPacket.strToBuf(popupData->myPopupInfo->getButton0Pressed());
else if (button == 1)
sendingPacket.strToBuf(popupData->myPopupInfo->getButton1Pressed());
else if (button == 2)
sendingPacket.strToBuf(popupData->myPopupInfo->getButton2Pressed());
else
sendingPacket.strToBuf("Popup closed because of odd click");
myMap.erase(id);
myDataMutex.unlock();
if (popupData->myCallback != NULL)
popupData->myCallback->invoke(popupData->myID, button);
delete popupData;
myServer->broadcastPacketTcp(&sendingPacket, "popupClose");
}
}
AREXPORT void ArCentralManager::forwarderServerClientRemovedCallback(
ArCentralForwarder *forwarder, ArServerClient *client)
{
// if this matches we're already closing this so don't do/print anything
if (myClosingConnectionID != 0 &&
myClosingConnectionID == client->getIdentifier().getConnectionID())
return;
ArLog::log(ArLog::Normal,
"Notifying main server of removal of serverClient for %s %s",
forwarder->getRobotName(), client->getIPString());
ArNetPacket sendPacket;
sendPacket.strToBuf("");
sendPacket.uByte2ToBuf(forwarder->getPort());
sendPacket.strToBuf(forwarder->getRobotName());
sendPacket.strToBuf("");
sendPacket.strToBuf(
forwarder->getClient()->getTcpSocket()->getIPString());
myClientServer->broadcastPacketTcpToMatching(&sendPacket, "clientRemoved",
client->getIdentifier(), true);
myClientServer->broadcastPacketTcpToMatching(&sendPacket, "clientAdded",
client->getIdentifier(), true);
}
void ArCentralManager::netClientList(ArServerClient *client, ArNetPacket *packet)
{
ArNetPacket sendPacket;
std::list<ArCentralForwarder *>::iterator fIt;
ArCentralForwarder *forwarder;
ArTypes::UByte2 sizeLen;
ArTypes::UByte2 realLen;
unsigned int numConnected = 0;
myDataMutex.lock();
sizeLen = sendPacket.getLength();
sendPacket.uByte2ToBuf(0);
for (fIt = myForwarders.begin(); fIt != myForwarders.end(); fIt++)
{
forwarder = (*fIt);
if (!forwarder->isConnected())
continue;
numConnected++;
sendPacket.strToBuf("");
sendPacket.uByte2ToBuf(forwarder->getPort());
sendPacket.strToBuf(forwarder->getRobotName());
sendPacket.strToBuf("");
sendPacket.strToBuf(
forwarder->getClient()->getTcpSocket()->getIPString());
}
realLen = sendPacket.getLength();
sendPacket.setLength(sizeLen);
sendPacket.uByte2ToBuf(numConnected);
sendPacket.setLength(realLen);
myDataMutex.unlock();
client->sendPacketTcp(&sendPacket);
}
AREXPORT void ArServerHandlerCamera::handleGetCameraModeList(
ArServerClient *client, ArNetPacket *packet)
{
ArNetPacket sending;
myModeMutex.lock();
sending.uByte2ToBuf(myCameraModeNameMap.size());
std::map<CameraMode, std::string>::iterator it;
for (it = myCameraModeNameMap.begin(); it != myCameraModeNameMap.end(); it++)
{
sending.strToBuf((*it).second.c_str());
}
myModeMutex.unlock();
client->sendPacketTcp(&sending);
}
AREXPORT void ArServerHandlerPopup::serverCycleCallback(void)
{
std::map<ArTypes::Byte4, PopupData *>::iterator it;
ArNetPacket sendingPacket;
PopupData *popupData;
int timeout;
std::list<ArTypes::Byte4> doneIDs;
std::list<PopupData *> donePopups;
myDataMutex.lock();
// only check it if we haven't checked it lately
if (myLastTimeCheck.mSecSince() > 1000)
{
myLastTimeCheck.setToNow();
for (it = myMap.begin(); it != myMap.end(); it++)
{
popupData = (*it).second;
if ((timeout = popupData->myPopupInfo->getTimeout()) > 0 &&
popupData->myStarted.secSince() >= timeout)
{
sendingPacket.empty();
sendingPacket.byte4ToBuf((*it).first);
sendingPacket.strToBuf(popupData->myPopupInfo->getTimeoutString());
myServer->broadcastPacketTcp(&sendingPacket, "popupClose");
doneIDs.push_back((*it).first);
donePopups.push_back(popupData);
}
}
}
while (doneIDs.begin() != doneIDs.end())
{
myMap.erase((*doneIDs.begin()));
doneIDs.pop_front();
}
myDataMutex.unlock();
std::list<PopupData *>::iterator donePopupIt;
while ((donePopupIt = donePopups.begin()) != donePopups.end())
{
popupData = (*donePopupIt);
if (popupData->myCallback != NULL)
popupData->myCallback->invoke(popupData->myID, -1);
delete popupData;
donePopups.pop_front();
}
}
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 (!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;
// We have to tell it what route we want to patrol
sending.strToBuf("hallways");
// tell it how much many times we want to patrol (<= 0 == forever)
sending.byte4ToBuf(0);
client.requestOnce("patrolRouteCount", &sending);
// note that there's also another call (patrol) that just has it always
// patrol forever but this gives you more options and is only
// slightly more complicated (just give it that byte4)
// you have to give the client some time to send the command
ArUtil::sleep(500);
Aria::shutdown();
return 0;
}
AREXPORT void ArServerHandlerConfig::getConfigDefaults(ArServerClient *client,
ArNetPacket *packet)
{
char sectionRequested[512];
sectionRequested[0] = '\0';
ArNetPacket sending;
if (myDefault == NULL)
{
ArLog::log(ArLog::Normal, "ArServerHandlerConfig::getConfigDefaults: No default config to get");
client->sendPacketTcp(&sending);
return;
}
myConfigMutex.lock();
// if we have a section name pick it up, otherwise we send everything
if (packet->getDataReadLength() < packet->getDataLength())
packet->bufToStr(sectionRequested, sizeof(sectionRequested));
//ArConfigArg param;
ArClientArg clientArg;
if (sectionRequested[0] == '\0')
ArLog::log(ArLog::Normal, "Sending all defaults to client");
else
ArLog::log(ArLog::Normal, "Sending defaults for section '%s' to client",
sectionRequested);
std::list<ArConfigSection *> *sections = myDefault->getSections();
for (std::list<ArConfigSection *>::iterator sIt = sections->begin();
sIt != sections->end();
sIt++)
{
ArConfigSection *section = (*sIt);
if (section == NULL) {
continue; // Should never happen...
}
// if we're not sending them all and not in the right section just cont
if (sectionRequested[0] != '\0' &&
ArUtil::strcasecmp(sectionRequested, section->getName()) != 0) {
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
myConfigMutex.unlock();
client->sendPacketTcp(&sending);
}
AriaClientDriver::AriaClientDriver(ArClientBase *client, ArKeyHandler *keyHandler, std::string robotName) :
myRobotStatePublisher(myKdlTree), myClient(client), myKeyHandler(keyHandler),
myTransRatio(0.0), myRotRatio(0.0), myLatRatio(0.0), myMaxVel(50),
myPrinting(false), myNeedToPrintHeader(false), myGotBatteryInfo(false),
/* Initialize functor objects with pointers to our handler methods: */
myUpCB(this, &AriaClientDriver::up),
myDownCB(this, &AriaClientDriver::down),
myLeftCB(this, &AriaClientDriver::left),
myRightCB(this, &AriaClientDriver::right),
myLateralLeftCB(this, &AriaClientDriver::lateralLeft),
myLateralRightCB(this, &AriaClientDriver::lateralRight),
mySafeDriveCB(this, &AriaClientDriver::safeDrive),
myUnsafeDriveCB(this, &AriaClientDriver::unsafeDrive),
myListDataCB(this, &AriaClientDriver::listData),
myLogTrackingTerseCB(this, &AriaClientDriver::logTrackingTerse),
myLogTrackingVerboseCB(this, &AriaClientDriver::logTrackingVerbose),
myResetTrackingCB(this, &AriaClientDriver::resetTracking),
myHandleOutputCB(this, &AriaClientDriver::handleOutput),
myHandleOutputNumbersCB(this, &AriaClientDriver::handleOutputNumbers),
myHandleOutputStringsCB(this, &AriaClientDriver::handleOutputStrings),
myHandleBatteryInfoCB(this, &AriaClientDriver::handleBatteryInfo),
myHandlePhysicalInfoCB(this, &AriaClientDriver::handlePhysicalInfo),
myHandleTemperatureInfoCB(this, &AriaClientDriver::handleTemperatureInfo),
myHandleSensorInfoCB(this, &AriaClientDriver::handleSensorInfo),
myHandleRangeDataCB(this, &AriaClientDriver::handleRangeData)
{
myRobotName=robotName; //string objects best handles the errors caused by unknown size
//printf("%s\n",(myRobotName+std::string("/Twist")).c_str());
//strcpy(myRobotName,robotName);
//memcpy(myRobotName,robotName, sizeof(robotName));
//KDL Tree and state publisher
std::string robot_desc_string;
myRosNodeHandle.getParam((std::string("robot_description_")+myRobotName).c_str(), robot_desc_string);
if (!kdl_parser::treeFromString(robot_desc_string, myKdlTree)){
ROS_ERROR("Failed to construct kdl tree\n please load urdf to parameter server:robot_description_<robotname>");
}
else{
printf("KDL parser::OK\n");}
myRobotStatePublisher=robot_state_publisher::RobotStatePublisher(myKdlTree);
//critical thread performing command updates
myCmdVelSubscribe = myRosNodeHandle.subscribe((myRobotName+std::string("/AriaCmdVel")).c_str(), 1, &AriaClientDriver::topicCallBack,this);
myTwistSubscribe = myRosNodeHandle.subscribe((myRobotName+std::string("/Twist")).c_str(), 1, &AriaClientDriver::topicCallBack2,this);
myNavdataPublish = myRosNodeHandle.advertise<ariaClientDriver::AriaNavData>((myRobotName+std::string("/AriaNavData")).c_str(), 1000);
myLaserPublish = myRosNodeHandle.advertise<sensor_msgs::LaserScan>((myRobotName+std::string("/LaserData")).c_str(), 50);
myOdomPublish = myRosNodeHandle.advertise<nav_msgs::Odometry>((myRobotName+std::string("/Odometry")).c_str(), 50);
// Critical thread performing sensor data acquisition
ArNetPacket sensorName;
sensorName.strToBuf("sim_lms2xx_1"); //lms2xx_1 for actual robot sim_lms2xx_1 for sim robot
myClient->addHandler("getSensorCurrent", &myHandleRangeDataCB);
myClient->request("getSensorCurrent",100,&sensorName);
myClient->addHandler("update", &myHandleOutputCB);
myClient->request("update", 100);
//myCmdVelSubscribe = myRosNodeHandle.subscribe<std_msgs::String>("AriaCmdVel", 1, Foo());
// Handlers(Threads) for Keyboard input
myKeyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB); //calls a method of an object pointer
myKeyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB);//double colon used for namespace
myKeyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);
myKeyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);
myKeyHandler->addKeyHandler('q', &myLateralLeftCB);
myKeyHandler->addKeyHandler('e', &myLateralRightCB);
myKeyHandler->addKeyHandler('s', &mySafeDriveCB);
myKeyHandler->addKeyHandler('u', &myUnsafeDriveCB);
myKeyHandler->addKeyHandler('l', &myListDataCB);
myKeyHandler->addKeyHandler('t', &myLogTrackingTerseCB);
myKeyHandler->addKeyHandler('v', &myLogTrackingVerboseCB);
myKeyHandler->addKeyHandler('r', &myResetTrackingCB);
myKeyHandler->addKeyHandler('o', &mySquareDriveCB);
// Handlers(Threads) for Service calls
myClient->addHandler("physicalInfo", &myHandlePhysicalInfoCB);
myClient->requestOnce("physicalInfo");
myClient->addHandler("batteryInfo", &myHandleBatteryInfoCB);
myClient->requestOnce("batteryInfo");
if (myClient->dataExists("temperatureInfo"))
{
myClient->addHandler("temperatureInfo", &myHandleTemperatureInfoCB);
myClient->requestOnce("temperatureInfo");
}
if (myClient->dataExists("getSensorList"))
{
myClient->addHandler("getSensorList", &myHandleSensorInfoCB);
myClient->requestOnce("getSensorList");
}
if (myClient->dataExists("updateNumbers") &&
myClient->dataExists("updateStrings"))
{
printf("Using new updates\n");
myClient->addHandler("updateStrings", &myHandleOutputStringsCB);
myClient->request("updateStrings", -1);
}
unsafeDrive();
printf("Aria Client Driver node started...\n");
printf("Debugging...\n");
}
bool ArServerHandlerConfig::internalSetConfig(ArServerClient *client,
ArNetPacket *packet)
{
char param[1024];
char argument[1024];
char errorBuffer[1024];
char firstError[1024];
ArNetPacket retPacket;
ArConfig *config;
bool ret = true;
if (client != NULL)
config = myConfig;
else
config = myDefault;
if (client != NULL)
lockConfig();
ArArgumentBuilder *builder = NULL;
if (client != NULL)
ArLog::log(ArLog::Normal, "Got new config from client %s", client->getIPString());
else
ArLog::log(ArLog::Verbose, "New default config");
errorBuffer[0] = '\0';
firstError[0] = '\0';
while (packet->getDataReadLength() < packet->getDataLength())
{
packet->bufToStr(param, sizeof(param));
packet->bufToStr(argument, sizeof(argument));
builder = new ArArgumentBuilder;
builder->setExtraString(param);
builder->add(argument);
ArLog::log(ArLog::Verbose, "Config: %s %s", param, argument);
// if the param name here is "Section" we need to parse sections,
// otherwise we parse the argument
if ((strcasecmp(param, "Section") == 0 &&
!config->parseSection(builder, errorBuffer, sizeof(errorBuffer))) ||
(strcasecmp(param, "Section") != 0 &&
!config->parseArgument(builder, errorBuffer, sizeof(errorBuffer))))
{
if (firstError[0] == '\0')
strcpy(firstError, errorBuffer);
}
delete builder;
builder = NULL;
}
if (firstError[0] == '\0')
{
if (config->callProcessFileCallBacks(true,
errorBuffer,
sizeof(errorBuffer)))
{
if (client != NULL)
ArLog::log(ArLog::Normal, "New config from client %s was fine.",
client->getIPString());
else
ArLog::log(ArLog::Verbose, "New default config was fine.");
retPacket.strToBuf("");
writeConfig();
}
else // error processing config callbacks
{
ret = false;
if (firstError[0] == '\0')
strcpy(firstError, errorBuffer);
// if its still empty it means we didn't have anything good in the errorBuffer
if (firstError[0] == '\0')
strcpy(firstError, "Error processing");
if (client != NULL)
ArLog::log(ArLog::Normal,
"New config from client %s had errors processing ('%s').",
client->getIPString(), firstError);
else
ArLog::log(ArLog::Normal,
"New default config had errors processing ('%s').",
firstError);
retPacket.strToBuf(firstError);
}
}
else
{
ret = false;
if (client != NULL)
ArLog::log(ArLog::Normal,
"New config from client %s had at least this problem: %s",
client->getIPString(), firstError);
else
ArLog::log(ArLog::Normal,
"New default config had at least this problem: %s",
firstError);
retPacket.strToBuf(firstError);
}
//printf("Sending ");
//retPacket.log();
if (client != NULL)
client->sendPacketTcp(&retPacket);
if (client != NULL)
unlockConfig();
if (client != NULL)
configUpdated(client);
return ret;
}
/// This should be its own thread here
void *ArCentralManager::runThread(void *arg)
{
std::list<ArSocket *>::iterator sIt;
std::list<std::string>::iterator nIt;
std::list<ArCentralForwarder *>::iterator fIt;
ArSocket *socket;
std::string robotName;
ArCentralForwarder *forwarder;
ArNetPacket *packet;
std::list<ArNetPacket *> addPackets;
std::list<ArNetPacket *> remPackets;
threadStarted();
while (getRunning())
{
int numForwarders = 0;
int numClients = 0;
myDataMutex.lock();
// this is where the original code to add forwarders was before we
// changed the unique behavior to drop old ones...
std::list<ArCentralForwarder *> connectedRemoveList;
std::list<ArCentralForwarder *> unconnectedRemoveList;
for (fIt = myForwarders.begin(); fIt != myForwarders.end(); fIt++)
{
forwarder = (*fIt);
numForwarders++;
if (forwarder->getServer() != NULL)
numClients += forwarder->getServer()->getNumClients();
bool connected = forwarder->isConnected();
bool removed = false;
if (!forwarder->callOnce(myHeartbeatTimeout, myUdpHeartbeatTimeout,
myRobotBackupTimeout, myClientBackupTimeout))
{
if (connected)
{
ArLog::log(ArLog::Normal, "Will remove forwarder from %s",
forwarder->getRobotName());
connectedRemoveList.push_back(forwarder);
removed = true;
}
else
{
ArLog::log(ArLog::Normal, "Failed to connect to forwarder from %s",
forwarder->getRobotName());
unconnectedRemoveList.push_back(forwarder);
removed = true;
}
}
if (!connected && !removed && forwarder->isConnected())
{
ArLog::log(ArLog::Normal, "Adding forwarder %s",
forwarder->getRobotName());
ArTime *newTime = new ArTime;
newTime->setSec(0);
myUsedPorts[forwarder->getPort()] = newTime;
std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
for (it = myForwarderAddedCBList.begin();
it != myForwarderAddedCBList.end();
it++)
{
if ((*it).second->getName() == NULL ||
(*it).second->getName()[0] == '\0')
ArLog::log(ArLog::Normal, "Calling unnamed add functor at %d",
-(*it).first);
else
ArLog::log(ArLog::Normal, "Calling %s add functor at %d",
(*it).second->getName(), -(*it).first);
(*it).second->invoke(forwarder);
}
ArLog::log(ArLog::Normal, "Added forwarder %s",
forwarder->getRobotName());
ArNetPacket *sendPacket = new ArNetPacket;
sendPacket->strToBuf("");
sendPacket->uByte2ToBuf(forwarder->getPort());
sendPacket->strToBuf(forwarder->getRobotName());
sendPacket->strToBuf("");
sendPacket->strToBuf(
forwarder->getClient()->getTcpSocket()->getIPString());
addPackets.push_back(sendPacket);
// MPL added this at the same time as the changes for the deadlock that happened down below
//myClientServer->broadcastPacketTcp(&sendPacket, "clientAdded");
}
}
while ((fIt = connectedRemoveList.begin()) != connectedRemoveList.end())
{
forwarder = (*fIt);
ArLog::log(ArLog::Normal, "Removing forwarder %s",
forwarder->getRobotName());
std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
for (it = myForwarderRemovedCBList.begin();
it != myForwarderRemovedCBList.end();
it++)
{
if ((*it).second->getName() == NULL ||
(*it).second->getName()[0] == '\0')
ArLog::log(ArLog::Normal, "Calling unnamed remove functor at %d",
-(*it).first);
else
ArLog::log(ArLog::Normal, "Calling %s remove functor at %d",
(*it).second->getName(), -(*it).first);
(*it).second->invoke(forwarder);
}
ArLog::log(ArLog::Normal, "Called forwarder removed for %s",
forwarder->getRobotName());
ArNetPacket *sendPacket = new ArNetPacket;
sendPacket->strToBuf("");
sendPacket->uByte2ToBuf(forwarder->getPort());
sendPacket->strToBuf(forwarder->getRobotName());
sendPacket->strToBuf("");
sendPacket->strToBuf(
forwarder->getClient()->getTcpSocket()->getIPString());
// MPL making this just push packets into a list to broadcast at the end since this was deadlocking
//myClientServer->broadcastPacketTcp(&sendPacket, "clientRemoved");
remPackets.push_back(sendPacket);
if (myUsedPorts.find(forwarder->getPort()) != myUsedPorts.end() &&
myUsedPorts[forwarder->getPort()] != NULL)
myUsedPorts[forwarder->getPort()]->setToNow();
myForwarders.remove(forwarder);
delete forwarder;
connectedRemoveList.pop_front();
ArLog::log(ArLog::Normal, "Removed forwarder");
}
while ((fIt = unconnectedRemoveList.begin()) !=
unconnectedRemoveList.end())
{
forwarder = (*fIt);
ArLog::log(ArLog::Normal, "Removing unconnected forwarder %s",
forwarder->getRobotName());
if (myUsedPorts.find(forwarder->getPort()) != myUsedPorts.end() &&
myUsedPorts[forwarder->getPort()] != NULL)
myUsedPorts[forwarder->getPort()]->setToNow();
myForwarders.remove(forwarder);
delete forwarder;
unconnectedRemoveList.pop_front();
ArLog::log(ArLog::Normal, "Removed unconnected forwarder");
}
// this code was up above just after the lock before we changed
// the behavior for unique names
while ((sIt = myClientSockets.begin()) != myClientSockets.end() &&
(nIt = myClientNames.begin()) != myClientNames.end())
{
socket = (*sIt);
robotName = (*nIt);
myClientSockets.pop_front();
myClientNames.pop_front();
ArLog::log(ArLog::Normal,
"New forwarder for socket from %s with name %s",
socket->getIPString(), robotName.c_str());
forwarder = new ArCentralForwarder(myClientServer, socket,
robotName.c_str(),
myClientServer->getTcpPort() + 1,
&myUsedPorts,
&myForwarderServerClientRemovedCB,
myEnforceProtocolVersion.c_str(),
myEnforceType);
myForwarders.push_back(forwarder);
}
numClients += myRobotServer->getNumClients();
if (myRobotServer != myClientServer)
numClients += myClientServer->getNumClients();
// end of code moved for change in unique behavior
if (numClients > myMostClients)
{
ArLog::log(ArLog::Normal, "CentralManager: New most clients: %d",
numClients);
myMostClients = numClients;
}
if (numForwarders > myMostForwarders)
myMostForwarders = numForwarders;
if (numClients > myMostClients)
{
ArLog::log(ArLog::Normal, "CentralManager: New most forwarders: %d",
numForwarders);
myMostForwarders = numForwarders;
}
myRobotServer->internalSetNumClients(numForwarders +
myClientSockets.size());
myDataMutex.unlock();
while (addPackets.begin() != addPackets.end())
{
packet = addPackets.front();
myClientServer->broadcastPacketTcp(packet, "clientAdded");
addPackets.pop_front();
delete packet;
}
while (remPackets.begin() != remPackets.end())
{
packet = remPackets.front();
myClientServer->broadcastPacketTcp(packet, "clientRemoved");
remPackets.pop_front();
delete packet;
}
ArUtil::sleep(1);
//make this a REALLY long sleep to test the duplicate pending
//connection code
//ArUtil::sleep(20000);
}
threadFinished();
return NULL;
}
AREXPORT bool ArCentralForwarder::gatheringCallOnce(
double heartbeatTimeout, double udpHeartbeatTimeout,
double robotBackupTimeout, double clientBackupTimeout)
{
// if we have a heartbeat timeout make sure we've heard the
// heartbeat within that range
if (heartbeatTimeout >= -.00000001 &&
myLastTcpHeartbeat.secSince() >= 5 &&
myLastTcpHeartbeat.secSince() / 60.0 > heartbeatTimeout)
{
ArLog::log(ArLog::Normal,
"%sHaven't connected in %g minutes, dropping connection",
myPrefix.c_str(), heartbeatTimeout);
return false;
}
if (!myClient->getReceivedDataList() ||
!myClient->getReceivedArgRetList() ||
!myClient->getReceivedGroupAndFlagsList())
{
myClient->loopOnce();
return true;
}
ArLog::log(ArLog::Normal, "%Connected to switching client %s from %s",
myPrefix.c_str(), myRobotName.c_str(), mySocket->getIPString());
//clientBase->logDataList();
char serverName[1024];
sprintf(serverName, "ArForwarderServer_%s", myRobotName.c_str());
myServer = new ArServerBase(false, serverName, false, "", "",
false, true,
false,
false, false);
myServer->enforceProtocolVersion(myEnforceProtocolVersion.c_str());
// there's no enforce of type here since this is the proxy for MP/ME
// and such (robots don't connect here)
myServer->addClientRemovedCallback(&myClientServerClientRemovedCB);
ArTime startedOpening;
startedOpening.setToNow();
int port;
bool foundPort;
std::map<int, ArTime *>::iterator usedIt;
// walk through our ports starting at our starting port
for (port = myStartingPort, foundPort = false;
!foundPort && port < 65536;
port++)
{
// if we've used the port in the last 2 minutes then skip it
if ((usedIt = myUsedPorts->find(port)) != myUsedPorts->end() &&
(*usedIt).second != NULL &&
((*usedIt).second->getSec() == 0 ||
(*usedIt).second->secSince() < 120))
{
ArLog::log(ArLog::Verbose, "%sSkipping port %d", myPrefix.c_str(), port);
continue;
}
// try to open it
if (myServer->open(port, myMainServer->getOpenOnIP()))
{
foundPort = true;
myPort = port;
}
}
if (!foundPort)
{
ArLog::log(ArLog::Normal, "%s Could not find port",
myPrefix.c_str());
}
myServer->setUserInfo(myMainServer->getUserInfo());
std::map<unsigned int, ArClientData *>::const_iterator dIt;
ArClientData *clientData;
myServer->addClientRemovedCallback(&myRobotServerClientRemovedCB);
myClient->addHandler("centralHeartbeat", &myNetCentralHeartbeatCB);
myClient->request("centralHeartbeat", 1000);
if (myClient->dataExists("identSetSelfIdentifier"))
{
ArNetPacket sending;
sending.strToBuf("CentralServer");
myClient->requestOnce("identSetSelfIdentifier", &sending);
}
myLastTcpHeartbeat.setToNow();
myLastUdpHeartbeat.setToNow();
for (dIt = myClient->getDataMap()->begin();
dIt != myClient->getDataMap()->end();
dIt++)
{
clientData = (*dIt).second;
if (myMainServer->dataHasFlag(clientData->getName(),
"MAIN_SERVER_ONLY"))
{
ArLog::log(ArLog::Normal,
"%sNot forwarding %s since it is MAIN_SERVER_ONLY",
myPrefix.c_str(), clientData->getName());
continue;
}
else if (clientData->hasDataFlag("DO_NOT_FORWARD"))
{
ArLog::log(ArLog::Normal,
"%sNot forwarding %s since it is DO_NOT_FORWARD",
myPrefix.c_str(), clientData->getName());
continue;
}
else if (clientData->hasDataFlag("RETURN_NONE"))
{
myReturnTypes[clientData->getCommand()] = RETURN_NONE;
}
else if (clientData->hasDataFlag("RETURN_SINGLE"))
{
myReturnTypes[clientData->getCommand()] = RETURN_SINGLE;
myRequestOnces[clientData->getCommand()] =
new std::list<ArServerClient *>;
}
else if (clientData->hasDataFlag("RETURN_VIDEO"))
{
ArLog::log(ArLog::Normal,
"%sForwarding %s that is RETURN_VIDEO",
myPrefix.c_str(), clientData->getName());
myReturnTypes[clientData->getCommand()] = RETURN_VIDEO;
myRequestOnces[clientData->getCommand()] =
new std::list<ArServerClient *>;
}
else if (clientData->hasDataFlag("RETURN_VIDEO_OPTIM"))
{
ArLog::log(ArLog::Normal,
"%sForwarding %s that is RETURN_VIDEO_OPTIM",
myPrefix.c_str(), clientData->getName());
myReturnTypes[clientData->getCommand()] = RETURN_VIDEO_OPTIM;
myRequestOnces[clientData->getCommand()] =
new std::list<ArServerClient *>;
}
else if (clientData->hasDataFlag("RETURN_UNTIL_EMPTY"))
{
myReturnTypes[clientData->getCommand()] = RETURN_UNTIL_EMPTY;
myRequestOnces[clientData->getCommand()] =
new std::list<ArServerClient *>;
}
else if (clientData->hasDataFlag("RETURN_COMPLEX"))
{
ArLog::log(ArLog::Normal,
"%sNot forwarding %s since it is a complex return",
myPrefix.c_str(), clientData->getName());
continue;
}
else
{
ArLog::log(ArLog::Normal,
"%sNot forwarding %s since it is an unknown return (data flags %s)",
myPrefix.c_str(), clientData->getName(),
clientData->getDataFlagsString());
continue;
}
setLastRequest(clientData->getCommand());
setLastBroadcast(clientData->getCommand());
myServer->addDataAdvanced(
clientData->getName(), clientData->getDescription(),
NULL, clientData->getArgumentDescription(),
clientData->getReturnDescription(), clientData->getCommandGroup(),
clientData->getDataFlagsString(), clientData->getCommand(),
&myInternalRequestChangedFunctor,
&myInternalRequestOnceFunctor);
myClient->addHandler(clientData->getName(), &myReceiveDataFunctor);
}
if (myClient->dataExists("centralServerHeartbeat"))
{
ArNetPacket sending;
myRobotHasCentralServerHeartbeat = true;
myLastSentCentralServerHeartbeat.setToNow();
myClient->requestOnce("centralServerHeartbeat", &sending, true);
myClient->requestOnceUdp("centralServerHeartbeat", &sending, true);
}
myState = STATE_CONNECTED;
return callOnce(heartbeatTimeout, udpHeartbeatTimeout,
robotBackupTimeout, clientBackupTimeout);
}
