int
EpuckDriver::MainSetup()
{
if(this->serialPort->initialize() == -1)
{
PLAYER_ERROR1("%s",this->serialPort->getError().c_str());
return -1;
}
// Request e-puck side program version
this->serialPort->sendInt(0x01);
if(this->serialPort->recvUnsigned() != this->EXPECTED_EPUCK_SIDE_VERSION)
{
PLAYER_ERROR("The e-puck side program version isn't the expected");
return -1;
}
if(epuckCamera.get() != NULL)
{
try
{
this->epuckCamera->Initialize();
std::string version = this->epuckCamera->GetCameraVersion();
PLAYER_MSG1(1,"E-puck camera initialized. Camera Version: %s", version.c_str());
}
catch(std::exception &e)
{
PLAYER_ERROR1("%s", e.what());
return -1;
}
}
return 0;
}
int
player_sd_unregister(player_sd_t* sd,
const char* name)
{
player_sd_mdns_t* mdns = (player_sd_mdns_t*)(sd->sdRef);
player_sd_mdns_dev_t* dev = NULL;
int i;
for(i=0;i<mdns->mdnsDevs_len;i++)
{
if(mdns->mdnsDevs[i].valid && !strcmp(name,mdns->mdnsDevs[i].sdDev.name))
{
dev = mdns->mdnsDevs + i;
break;
}
}
if(dev)
{
DNSServiceRefDeallocate(dev->regRef);
dev->valid = 0;
PLAYER_MSG1(2,"Unregistration of %s successful", name);
return(0);
}
else
{
PLAYER_ERROR1("Failed to find and unregister device %s", name);
return(-1);
}
}
void
resolveCB(DNSServiceRef sdRef,
DNSServiceFlags flags,
uint32_t interfaceIndex,
DNSServiceErrorType errorCode,
const char *fullname,
const char *hosttarget,
uint16_t port,
uint16_t txtLen,
const char *txtRecord,
void *context)
{
player_sd_dev_t* sddev = (player_sd_dev_t*)context;
const char* value;
uint8_t value_len;
char* colon;
char buf[PLAYER_SD_TXT_MAXLEN];
// Handle resolution result
if(errorCode == kDNSServiceErr_NoError)
{
// Fill in the address info
memset(sddev->hostname,0,sizeof(sddev->hostname));
strncpy(sddev->hostname,hosttarget,sizeof(sddev->hostname)-1);
sddev->robot = port;
if(!(value = (const char*)TXTRecordGetValuePtr(txtLen,
txtRecord,
PLAYER_SD_DEVICE_TXTNAME,
&value_len)))
{
PLAYER_ERROR1("Failed to find TXT info for service %s\n", sddev->name);
sddev->addr_fail = 1;
return;
}
if(!(colon = strchr(value,':')) ||
((colon-value) <= 0) ||
((value_len - (colon-value+1)) <= 0))
{
PLAYER_ERROR2("Failed to parse TXT info \"%s\" for service %s\n",
value, sddev->name);
sddev->addr_fail = 1;
return;
}
memset(buf,0,sizeof(buf));
strncpy(buf,value,(colon-value));
sddev->interf = str_to_interf(buf);
memset(buf,0,sizeof(buf));
strncpy(buf,colon+1,(value_len-(colon-value+1)));
sddev->index = atoi(buf);
sddev->addr_valid = 1;
}
else
{
// Something went wrong.
sddev->addr_fail = 1;
}
}
Property::Property (const char *newKey, bool readOnly)
: readonly(readOnly)
{
if ((key = strdup (newKey)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property key %s", newKey);
key = NULL;
}
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int CameraV4L2::Setup()
{
int err=0;
int i;
source = back_source;
norm = back_norm;
depth = back_depth;
width = back_width;
height = back_height;
fieldType = back_fieldType;
v4l2_type_id = back_v4l2_type_id;
PLAYER_MSG1(0,"Camerav4l2: Setting up device %s", this->device);
this->fg = fg2_createFrameGrabber();
// set initial settings
if (fg2_open(this->fg, this->device)!=0)
{
PLAYER_ERROR1("unable to open %s", this->device);
fg2_delete(&fg);
return -1;
}
PLAYER_MSG0(1,"Camerav4l2: device opened, aplying settings");
err |= fg2_set_source(this->fg, this->source, this->sourceCh);
err |= fg2_set_source_norm(this->fg, this->norm);
if (err!=0){
PLAYER_ERROR("Camerav4l2: failed source or norm");
fg2_delete(&fg);
return -1;
}
for(i=0; i<numOfCtls; i++){
fg2_setControlValue(fg, ctlNames[i], ctlVals[i]);
}
if (fg2_setPixelSettings(fg, this->width, this->height, this->v4l2_type_id, this->fieldType, this->depth)==0) {
} else {
PLAYER_ERROR("Camerav4l2: nie udalo sie ustawic podanych parametrow");
//fg2_delete(&fg);
//return -1;
}
// finally start streaming with new settings
if (fg2_startCapture(fg)!=0){
fg2_delete(&fg);
return -1;
}
// Start the driver thread.
this->StartThread();
return 0;
}
void Property::SetKey (const char *newKey)
{
if (key != NULL)
free (key);
if ((key = strdup (newKey)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property key %s", newKey);
key = NULL;
}
}
Property* PropertyBag::GetProperty (const char *key)
{
PropertyNode *currentProp = firstProperty;
while (currentProp != NULL)
{
if (strcmp (currentProp->key, key) == 0)
return currentProp->property;
currentProp = currentProp->next;
}
// Property wasn't found
PLAYER_ERROR1 ("Property not registered: %s", key);
return NULL;
}
const StringProperty& StringProperty::operator= (const StringProperty &rhs)
{
if (value != NULL)
free (value);
if (rhs.GetValue () == NULL)
value = NULL;
else if ((value = strdup (rhs.GetValue ())) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", rhs.GetValue ());
value = NULL;
}
return *this;
}
void StringProperty::GetValueToMessage (void *data) const
{
player_strprop_req_t *req = reinterpret_cast<player_strprop_req_t*> (data);
if (value == NULL)
req->value = NULL;
else if ((req->value = strdup (value)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", value);
req->value = NULL;
req->value_count = 0;
}
req->value_count = strlen (req->value) + 1; // +1 to make sure the NULL byte is copied by pack/unpack functions
}
StringProperty::StringProperty (const char *newKey, const char *newValue, bool readOnly)
: Property (newKey, readOnly)
{
if (newValue != NULL)
{
if ((value = strdup (newValue)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", newValue);
value = NULL;
}
}
else
value = NULL;
}
void StringProperty::SetValue (const char *newValue)
{
if (value != NULL)
free (value);
if (newValue != NULL)
{
if ((value = strdup (newValue)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", newValue);
value = NULL;
}
}
else
value = NULL;
}
StringProperty::StringProperty (const char *newKey, const char * newValue, bool readOnly, Driver * driver, ConfigFile*cf, int section)
: Property (newKey, readOnly)
{
if (newValue != NULL)
{
if ((value = strdup (newValue)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", newValue);
value = NULL;
}
}
else
value = NULL;
driver->RegisterProperty(newKey, this, cf, section);
}
void StringProperty::SetValueFromMessage (const void *data)
{
if (readonly)
{
PLAYER_WARN2 ("Property %s is read only, cannot change value %s", key, reinterpret_cast<const player_strprop_req_t*> (data)->value);
return;
}
const player_strprop_req_t *req = reinterpret_cast<const player_strprop_req_t*> (data);
if (value != NULL)
free (value);
if ((value = strdup (req->value)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", req->value);
value = NULL;
}
}
bool StringProperty::ReadConfig (ConfigFile *cf, int section)
{
const char *temp = cf->ReadString (section, key, NULL);
if (temp != NULL)
{
if (value != NULL)
free (value);
if ((value = strdup (temp)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", temp);
value = NULL;
return false;
}
}
return true;
}
const char* StringProperty::operator= (const char* rhs)
{
if (readonly)
{
PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %s", key, rhs);
return value;
}
if (value != NULL)
free (value);
if (rhs == NULL)
value = NULL;
else if ((value = strdup (rhs)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory to store property value %s", rhs);
value = NULL;
}
return *this;
}
/**
* handle command and generate reply
* @param cmd command in text representation
* @param len length of cmd (including '\0')
* @param reply buffer for reply
* @param rlen length of buffer for reply
* @return 0 ok, -1 failure
*/
int CameraV4L2::handleCommand(char *cmd, int len, char *reply, int rlen){
//enum v4l2_field fld;
int ret=-1;
unsigned int a; // backup
double dblVal;
ret = -1; // error on default
PLAYER_MSG1(2,"Got command get type %s", cmd);
switch(cmd[1]){
case 'c': // controls
case 'C':
if (len<3)
break;
if (cmd[2] == '\0'){
// count controls
PLAYER_MSG0(2,"counting controls");
a = fg2_countControls(fg);
ret = 0;
snprintf(reply, rlen, "%d", a);
break;
}
if (len<4)
break;
if (cmd[2] == 'i' || cmd[2] == 'I') {
if (sscanf(cmd+3, "%u", &a)!=1)
break;
dblVal = fg2_getControlValueI(fg, a);
ret = 0;
snprintf(reply, rlen, "%f", dblVal);
break;
}
if (cmd[2] == 'n' || cmd[2] == 'N' && cmd[3]==' '){
if (sscanf(cmd+3, "%u", &a)!=1)
break;
// get name
const char *name = fg2_getControlName(fg, a);
if (name==0)
break;
ret = 0;
snprintf(reply, rlen, "%s", name);
break;
}
if (cmd[2] != ' ')
break;
dblVal = fg2_getControlValue(fg, cmd + 3);
ret = 0;
snprintf(reply, rlen, "%f", dblVal);
break;
case 'f': // fields
case 'F':
{
int i=0;
for(i=0; fieldy[i].name; i++)
if (fieldy[i].fieldType == this->fieldType)
break;
if (fieldy[i].name) {
snprintf(reply, rlen, "%s", fieldy[i].name);
ret = 0;
} else {
PLAYER_WARN1("Unknown field type: %d", this->fieldType);
}
break;
}
case 'm': // modes
case 'M':
{
int i=0;
for(i=0; formaty[i].name; i++)
if (formaty[i].v4l2_type_id == this->v4l2_type_id)
break;
if (formaty[i].name) {
snprintf(reply, rlen, "%s", formaty[i].name);
ret = 0;
} else {
PLAYER_WARN1("Unknown pixel format type: %d", this->v4l2_type_id);
}
break;
}
case 'n': // norms
case 'N':
{
int i=0;
for(i=0; normy[i].name; i++)
if (normy[i].id == this->norm)
break;
if (formaty[i].name) {
snprintf(reply, rlen, "%s", normy[i].name);
ret = 0;
} else {
PLAYER_WARN1("Unknown pixel format type: %d", this->norm);
}
break;
}
case 'i': // source name
case 'I':
{
ret = 0;
snprintf(reply, rlen, "%s", fg2_get_source_name(fg));
break;
}
case '\0': // driver name
{
ret = 0;
snprintf(reply, rlen, "%s", fg2_get_device_name(fg));
break;
}
default:
PLAYER_ERROR1("Unknown command2 %s",cmd);
break;
}
return ret;
}
void
browseCB(DNSServiceRef sdRef,
DNSServiceFlags flags,
uint32_t interfaceIndex,
DNSServiceErrorType errorCode,
const char *serviceName,
const char *regtype,
const char *replyDomain,
void *context)
{
player_sd_t* sd = (player_sd_t*)context;
player_sd_mdns_t* mdns = (player_sd_mdns_t*)sd->sdRef;
player_sd_dev_t* sddev;
DNSServiceRef resolveRef;
DNSServiceErrorType sdErr;
struct pollfd ufds[1];
int numready;
mdns->flags = flags;
// Got a browse event.
if(flags & kDNSServiceFlagsAdd)
{
// A new service was added. First check whether we already have an
// entry for a device with this name.
if(!(sddev = player_sd_get_device(sd, serviceName)))
{
// No existing entry. Add one
sddev = _player_sd_add_device(sd, serviceName);
}
// Record the name
sddev->valid = 1;
sddev->addr_valid = 0;
sddev->addr_fail = 0;
memset(sddev->name,0,sizeof(sddev->name));
strncpy(sddev->name,serviceName,sizeof(sddev->name)-1);
// Resolve its address
if((sdErr = DNSServiceResolve(&resolveRef,
0,
interfaceIndex,
serviceName,
regtype,
replyDomain,
resolveCB,
(void*)sddev)) != kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceResolve returned error: %d\n", sdErr);
return;
}
ufds[0].fd = DNSServiceRefSockFD(resolveRef);
ufds[0].events = POLLIN;
while(!sddev->addr_valid && !sddev->addr_fail)
{
if((numready = poll(ufds,1,-1)) < 0)
{
if(errno == EAGAIN)
continue;
else
{
PLAYER_ERROR1("poll returned error: %s", strerror(errno));
sddev->addr_fail = 1;
}
}
else if(numready > 0)
{
if((sdErr = DNSServiceProcessResult(resolveRef)) !=
kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceProcessResult returned error: %d\n", sdErr);
sddev->addr_fail = 1;
}
}
}
DNSServiceRefDeallocate(resolveRef);
if(sddev->addr_valid)
{
// Invoke the users' callback
if(mdns->callb)
(mdns->callb)(sd,sddev);
}
}
else
{
// An existing service was removed. Delete our records for it.
if((sddev = player_sd_get_device(sd, serviceName)))
{
sddev->valid = 0;
sddev->addr_valid = 0;
// Invoke the users' callback
if(mdns->callb)
(mdns->callb)(sd,sddev);
}
}
//player_sd_printcache(sd);
}
int
player_sd_update(player_sd_t* sd, double timeout)
{
struct pollfd ufds[1];
int numready;
int polltime;
DNSServiceErrorType sdErr;
player_sd_mdns_t* mdns = (player_sd_mdns_t*)sd->sdRef;
if(!mdns->browseRef_valid)
{
PLAYER_ERROR("Can't update without a valid browsing session");
return(-1);
}
ufds[0].fd = DNSServiceRefSockFD(mdns->browseRef);
ufds[0].events = POLLIN;
if(timeout >= 0.0)
polltime = (int)rint(timeout * 1e3);
else
polltime = -1;
for(;;)
{
if((numready = poll(ufds,1,polltime)) < 0)
{
if(errno == EAGAIN)
{
// TODO: strictly speaking, we should decrement polltime here by
// the time that has elapsed so far
continue;
}
else
{
PLAYER_ERROR1("poll returned error: %s", strerror(errno));
return(-1);
}
}
else if(numready > 0)
{
// Read all queued up responses
if((sdErr = DNSServiceProcessResult(mdns->browseRef)) !=
kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceProcessResult returned error: %d", sdErr);
return(-1);
}
while(mdns->flags & kDNSServiceFlagsMoreComing)
{
if((sdErr = DNSServiceProcessResult(mdns->browseRef)) !=
kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceProcessResult returned error: %d", sdErr);
return(-1);
}
}
break;
}
else
{
// timeout
break;
}
}
return(0);
}
int
player_sd_browse(player_sd_t* sd,
double timeout,
int keepalive,
player_sd_browse_callback_fn_t cb)
{
DNSServiceErrorType sdErr;
player_sd_mdns_t* mdns = (player_sd_mdns_t*)(sd->sdRef);
int retval=0;
struct timeval curr;
double currtime, starttime;
double polltime;
// Close any previously open session
if(mdns->browseRef_valid)
{
DNSServiceRefDeallocate(mdns->browseRef);
mdns->browseRef_valid = 0;
}
mdns->flags = 0;
// Initiate the browse session
if((sdErr = DNSServiceBrowse(&(mdns->browseRef),
0,
0,
PLAYER_SD_SERVICENAME,
NULL,
browseCB,
(void*)sd)) != kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceBrowse returned error: %d", sdErr);
return(-1);
}
mdns->browseRef_valid = 1;
mdns->callb = cb;
// Should we wait here for responses?
if(timeout != 0.0)
{
// Record the current time
gettimeofday(&curr,NULL);
starttime = currtime = curr.tv_sec + curr.tv_usec / 1e6;
// Update until the requested time has elapsed
while((timeout < 0.0) || ((currtime - starttime) < timeout))
{
// Set up to poll on the DNSSD socket
if(timeout > 0.0)
polltime = timeout - (currtime - starttime);
else
polltime = -1.0;
if((retval = player_sd_update(sd,polltime)) != 0)
break;
gettimeofday(&curr,NULL);
currtime = curr.tv_sec + curr.tv_usec / 1e6;
}
}
// Should we leave the session open?
if(!keepalive)
{
DNSServiceRefDeallocate(mdns->browseRef);
mdns->browseRef_valid = 0;
}
return(retval);
}
bool PropertyBag::AddProperty (const char *key, Property *property)
{
if (firstProperty == NULL)
{
if ((firstProperty = new PropertyNode) == NULL)
{
PLAYER_ERROR ("Failed to allocate memory for property node");
return false;
}
if ((firstProperty->key = strdup (key)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory for property key: %s", key);
delete firstProperty;
firstProperty = NULL;
return false;
}
firstProperty->property = property;
firstProperty->next = NULL;
}
else
{
// Walk to the end of the list, checking for an existing property as we go
PropertyNode *lastProperty = firstProperty;
while (lastProperty != NULL)
{
if (strcmp (lastProperty->key, key) == 0)
{
PLAYER_ERROR1 ("Property already registered: %s", key);
return false;
}
if (lastProperty->next == NULL)
{
// This is the end of the list, break before we lose the pointer
// Note that the while loop doesn't do this check because then
// it wouldn't check the last item on the list to see if it's the
// same key as the one being registered
break;
}
lastProperty = lastProperty->next;
}
// Add the new property at this position (which should be the end of the list)
if ((lastProperty->next = new PropertyNode) == NULL)
{
PLAYER_ERROR ("Failed to allocate memory for property node");
return false;
}
if ((lastProperty->next->key = strdup (key)) == NULL)
{
PLAYER_ERROR1 ("Failed to allocate memory for property key: %s", key);
delete lastProperty->next;
lastProperty->next = NULL;
return false;
}
lastProperty = lastProperty->next;
lastProperty->property = property;
lastProperty->next = NULL;
}
return true;
}
int
player_sd_register(player_sd_t* sd,
const char* name,
player_devaddr_t addr)
{
DNSServiceErrorType sdErr;
char recordval[PLAYER_SD_TXT_MAXLEN];
int i,j;
player_sd_mdns_t* mdns = (player_sd_mdns_t*)(sd->sdRef);
player_sd_mdns_dev_t* dev;
char nameBuf[PLAYER_SD_NAME_MAXLEN];
// Find a spot for this device
for(i=0;i<mdns->mdnsDevs_len;i++)
{
if(!mdns->mdnsDevs[i].valid)
break;
}
if(i==mdns->mdnsDevs_len)
{
// Make the list bigger
if(!mdns->mdnsDevs_len)
mdns->mdnsDevs_len = PLAYER_SD_MDNS_DEVS_LEN_INITIAL;
else
mdns->mdnsDevs_len *= PLAYER_SD_MDNS_DEVS_LEN_MULTIPLIER;
mdns->mdnsDevs =
(player_sd_mdns_dev_t*)realloc(mdns->mdnsDevs,
(mdns->mdnsDevs_len *
sizeof(player_sd_mdns_dev_t)));
assert(mdns->mdnsDevs);
for(j=i;j<mdns->mdnsDevs_len;j++)
mdns->mdnsDevs[j].valid = 0;
}
dev = mdns->mdnsDevs + i;
dev->fail = 0;
memset(dev->sdDev.name,0,sizeof(dev->sdDev.name));
strncpy(dev->sdDev.name,name,sizeof(dev->sdDev.name)-1);
memset(dev->sdDev.hostname,0,sizeof(dev->sdDev.hostname));
packedaddr_to_dottedip(dev->sdDev.hostname,sizeof(dev->sdDev.hostname),
addr.host);
dev->sdDev.robot = addr.robot;
dev->sdDev.interf = addr.interf;
dev->sdDev.index = addr.index;
dev->nameIdx = 1;
TXTRecordCreate(&(dev->txtRecord),sizeof(dev->txtBuf),dev->txtBuf);
memset(recordval,0,sizeof(recordval));
snprintf(recordval, sizeof(recordval), "%s:%u",
interf_to_str(addr.interf), addr.index);
if((sdErr = TXTRecordSetValue(&(dev->txtRecord),
"device",
strlen(recordval),
recordval)))
{
PLAYER_ERROR1("TXTRecordSetValue returned error: %d", sdErr);
return(-1);
}
memset(nameBuf,0,sizeof(nameBuf));
strncpy(nameBuf,name,sizeof(nameBuf)-1);
sdErr = kDNSServiceErr_NameConflict;
// Avahi can return the kDNSServiceErr_NameConflict immediately.
while(sdErr == kDNSServiceErr_NameConflict)
{
sdErr = DNSServiceRegister(&(dev->regRef),
0,
0,
nameBuf,
PLAYER_SD_SERVICENAME,
NULL,
NULL,
addr.robot,
TXTRecordGetLength(&(dev->txtRecord)),
TXTRecordGetBytesPtr(&(dev->txtRecord)),
registerCB,
(void*)dev);
if(sdErr == kDNSServiceErr_NameConflict)
{
// Pick a new name
memset(nameBuf,0,sizeof(nameBuf));
snprintf(nameBuf,sizeof(nameBuf),"%s (%d)",
name,dev->nameIdx++);
}
}
if(sdErr != kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceRegister returned error: %d", sdErr);
return(-1);
}
else
{
dev->valid = 1;
if(strcmp(nameBuf,name))
PLAYER_WARN2("Changing service name of %s to %s\n",
name,nameBuf);
PLAYER_MSG1(2,"Registration of %s successful", name);
return(0);
}
}