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 bool ArServerInfoDrawings::addRobotsRangeDevices(ArRobot *robot)
{
std::list<ArRangeDevice *>::iterator it;
bool ret = true;
ArRangeDevice *device;
//printf("0\n");
if (robot == NULL || robot->getRangeDeviceList() == NULL)
{
ArLog::log(ArLog::Terse, "InfoDrawings::addRobotsRangeDevices: Robot or robot's range device list is NULL");
return false;
}
//printf("1\n");
for (it = robot->getRangeDeviceList()->begin();
it != robot->getRangeDeviceList()->end();
it++)
{
device = (*it);
//printf("2 %s\n", device->getName());
device->lockDevice();
if (!addRangeDevice(device))
ret = false;
device->unlockDevice();
}
//printf("3\n");
return ret;
}
