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 bool ArCentralForwarder::requestOnceUdp(const char *name,
ArNetPacket *packet,
bool quiet)
{
if (myClient != NULL)
{
unsigned int commandNum = myClient->findCommandFromName(name);
if (commandNum == 0)
{
if (!quiet)
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::requestOnce: Can't find commandNum for %s",
getRobotName(), name);
return false;
}
//return myClient->requestOnceUdp(name, packet, quiet);
if (packet != NULL)
{
packet->setCommand(commandNum);
packet->finalizePacket();
return internalRequestOnce(NULL, packet, false);
}
else
{
ArNetPacket tempPacket;
tempPacket.setCommand(commandNum);
tempPacket.finalizePacket();
return internalRequestOnce(NULL, &tempPacket, false);
}
}
else
{
if (!quiet)
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::requestOnce: Tried to call (for %s) while client was still NULL",
getRobotName(), name);
return false;
}
}
/// The packet handler for starting/stopping scans from the lcd
AREXPORT bool ArServerHandlerMapping::packetHandler(ArRobotPacket *packet)
{
// we return these as processed to help out the ArLaserLogger class
if (packet->getID() == 0x96)
return true;
if (packet->getID() != 0x97)
return false;
myRobot->unlock();
char argument = packet->bufToByte();
if (argument == 1)
{
// see if we're already mapping
if (myLaserLogger != NULL)
{
ArLog::log(ArLog::Normal,
"ArServerHandlerMapping::packetHandler: Start scan requested when already mapping");
}
else
{
// build a name
time_t now;
struct tm nowStruct;
now = time(NULL);
char buf[1024];
if (ArUtil::localtime(&now, &nowStruct))
{
sprintf(buf, "%02d%02d%02d_%02d%02d%02d",
(nowStruct.tm_year%100), nowStruct.tm_mon+1, nowStruct.tm_mday,
nowStruct.tm_hour, nowStruct.tm_min, nowStruct.tm_sec);
}
else
{
ArLog::log(ArLog::Normal,
"ArServerHandlerMapping::packetHandler: Could not make good packet name (error getting time), so just naming it \"lcd\"");
sprintf(buf, "lcd");
}
ArLog::log(ArLog::Normal, "Starting scan '%s' from LCD", buf);
ArNetPacket fakePacket;
fakePacket.strToBuf(buf);
fakePacket.finalizePacket();
serverMappingStart(NULL, &fakePacket);
}
}
else if (argument == 0)
{
if (myLaserLogger == NULL)
{
ArLog::log(ArLog::Normal,
"ArServerHandlerMapping::packetHandler: Stop scan requested when not mapping");
}
else
{
ArLog::log(ArLog::Normal, "Stopping scan from LCD");
serverMappingEnd(NULL, NULL);
}
}
else
{
ArLog::log(ArLog::Normal, "ArServerHandlerMapping::packetHandler: Unknown scan argument %d", argument);
}
myRobot->lock();
return true;
}
