int DynamicWindowController::ProcessMessage(QueuePointer & resp_queue, player_msghdr* hdr, void* data) {
// Handle the data from the position2d subscribed to and requests to this position2d
if (Message::MatchMessage(hdr, -1, -1, this->position_addr)) { // A message related to the subscribed position 2D
float robot_radius = 0;
player_position2d_geom_t* geom;
switch(hdr->type) {
case PLAYER_MSGTYPE_DATA:
switch (hdr->subtype) {
case PLAYER_POSITION2D_DATA_GEOM: // Underlying position 2D has change geometry.
printf("Underlying position geom changed\n");
geom = (player_position2d_geom_t*)data;
robot_radius = MAX(geom->size.sl,geom->size.sw);
robot_radius /= 2.0;
dw.setRadius(robot_radius);
Publish(device_addr,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_GEOM,data);
return 0;
case PLAYER_POSITION2D_DATA_STATE: // Underlying position 2D has change state
assert(hdr->size == sizeof(player_position2d_data_t));
processOdom(hdr, *reinterpret_cast<player_position2d_data_t *> (data));
return 0;
default:
printf("Another message type received?\n");
return 0;
}
break;
case PLAYER_MSGTYPE_RESP_ACK:
switch (hdr->subtype) {
case PLAYER_POSITION2D_REQ_GET_GEOM: // Our geometry reply from the underlying position driver
printf("DWC: receiced an ack, for what??\n");
geom = (player_position2d_geom_t*)data;
robot_radius = MAX(geom->size.sl,geom->size.sw);
robot_radius /= 2.0;
dw.setRadius(robot_radius);
Publish(device_addr,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_GEOM,data);
return 0;
default:
printf("I dont' know what is this, 1\n");
return -1;
}
default:
printf("I dont' know what is this, 2\n");
return -1;
}
}
//
else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_LASER_DATA_SCAN, this->laser_addr)) {
processLaser(*reinterpret_cast<player_laser_data_t *> (data));
return 0;
}
else if (Message::MatchMessage(hdr, -1, -1, device_addr)) { // A message related to this driver
switch(hdr->type) {
case PLAYER_MSGTYPE_CMD:
switch (hdr->subtype) {
case PLAYER_POSITION2D_CMD_CAR:
printf("Dynamic window controller has CMD_CAR");
return 0;
case PLAYER_POSITION2D_CMD_POS:
assert(hdr->size == sizeof(player_position2d_cmd_pos_t));
processCommand(*reinterpret_cast<player_position2d_cmd_pos_t *> (data));
return 0;
case PLAYER_POSITION2D_CMD_VEL:
assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
memcpy(&m_own_cmd,reinterpret_cast<player_position2d_cmd_pos_t *> (data),sizeof(m_own_cmd));
this->m_active_goal = false;
return 0;
case PLAYER_POSITION2D_CMD_VEL_HEAD:
printf("Dynamic window controller has CMD_VEL_HEAD");
return 0;
default:
printf("I dont' know what is this, 3\n");
return -1;
}
break;
case PLAYER_MSGTYPE_REQ:
switch (hdr->subtype) {
void* repdata;
player_msghdr_t* rephdr;
case PLAYER_POSITION2D_REQ_RESET_ODOM:
this->position_device->PutMsg(resp_queue,hdr,data);
printf("Dynamic window controller has REQ_RESET_ODOM\n");
return 0;
break;
case PLAYER_POSITION2D_REQ_SET_ODOM:
this->position_device->PutMsg(resp_queue,hdr,data);
printf("Dynamic window controller has REQ_SET_ODOM\n");
return 0;
break;
case PLAYER_POSITION2D_REQ_GET_GEOM:
case PLAYER_POSITION2D_REQ_MOTOR_POWER:
case PLAYER_POSITION2D_REQ_POSITION_MODE:
case PLAYER_POSITION2D_REQ_POSITION_PID:
case PLAYER_POSITION2D_REQ_SPEED_PID:
case PLAYER_POSITION2D_REQ_SPEED_PROF:
case PLAYER_POSITION2D_REQ_VELOCITY_MODE:
Message* msg;
if(!(msg = this->position_device->Request(this->InQueue, hdr->type, hdr->subtype,(void*)data,hdr->size, &hdr->timestamp)))
{
PLAYER_WARN1("Dynamic window controller failed to forward request with subtype: %d\n", hdr->subtype);
return(-1);
}
rephdr = msg->GetHeader();
repdata = msg->GetPayload();
// Copy in our address and forward the response
rephdr->addr = device_addr;
this->Publish(resp_queue, rephdr, repdata);
delete msg;
// header contains the device addr which makes player confused when reply comes from wrong device
return 0;
break;
default:
printf("I dont' know what is this, 4\n");
return -1;
}
break;
default:
printf("I dont' know what is this, 5\n");
return -1;
}
}
// Tell the caller that we don't know how to handle this message
printf("I really don't know what are you asking me\n");
return (-1);
}
/**
* handle command without reply
* @param cmd command in text representation
* @param len length of cmd (including '\0')
* @return 0 ok, -1 failure
*/
int CameraV4L2::handleCommand(char *cmd, int len){
int w;
int h;
enum v4l2_field fld;
int ret=-1;
unsigned int a,b,c; // backup
double dblVal;
ret = -1; // error on default
PLAYER_MSG1(2,"Got command %s", cmd);
switch(cmd[0]){
case 's':
case 'S':
if (sscanf(cmd+1," %d %d", &w, &h)!=2)
break;
fg2_stopCapture(fg);
ret = tryPixelSettings(fg, w, h, this->v4l2_type_id, fieldType);
fg2_startCapture(fg);
break;
case 'i':
case 'I':
// todo: rollback if needed
if (cmd[2]>='0' && cmd[2]<='9')
sscanf(cmd+2,"%d", &(this->source));
else {
this->source = -1;
}
fg2_stopCapture(fg);
ret = fg2_set_source(this->fg, this->source, cmd+2);
fg2_startCapture(fg);
break;
case 'n':
case 'N':
a = this->norm;
b = this->width;
c = this->height;
if (cmd[1]!=' ' || selectFormat(cmd+2)!=0)
break;
fg2_stopCapture(fg);
ret = fg2_set_source_norm(this->fg, this->norm);
ret |= tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
if (ret!=0) {
this->norm = a;
this->width = b;
this->height = c;
// known working?
fg2_set_source_norm(this->fg, this->norm);
tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
}
fg2_startCapture(fg);
break;
case 'm':
case 'M':
a = this->data.format;
b = this->depth;
c = this->v4l2_type_id;
if (cmd[1]!=' ' || selectFormat(cmd+2)!=0)
break;
fg2_stopCapture(fg);
ret = tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
if (ret!=0) {
this->data.format = a;
this->depth = b;
this->v4l2_type_id = c;
ret = tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
}
fg2_startCapture(fg);
break;
case 'w':
case 'W':
// todo: rollback if needed
if (cmd[2]>='0' && cmd[2]<='9'){
sscanf(cmd+2,"%d", &(this->flip_rb));
ret = 0;
}
break;
case 'f':
case 'F':
fld = fieldType;
if (cmd[1]!=' ' || selectField(cmd+2)!=0)
break;
fg2_stopCapture(fg);
ret = tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
fg2_startCapture(fg);
if (ret!=0) {
fieldType = fld;
}
break;
case 'c':
case 'C':
if (cmd[1] == 'i' || cmd[1] == 'I') {
if (sscanf(cmd+2, "%lf %u", &dblVal, &a)!=2)
break;
ret = fg2_setControlValueI(fg, a, dblVal);
break;
}
for(w=2; w<len-1; w++)
if (cmd[w]==' ' || cmd[w+1]=='\0')
break;
if (sscanf(cmd+2,"%lf",&dblVal) != 1)
break;
ret = fg2_setControlValue(fg, cmd+w+1, dblVal);
break;
default:
PLAYER_WARN1("Unknown command %s",cmd);
break;
}
return ret;
}
/**
* 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;
}
////////////////////////////////////////////////////////////////////////////////
// Constructor
CameraV4L2::CameraV4L2( ConfigFile* cf, int section)
: Driver(cf, section, PLAYER_CAMERA_CODE, PLAYER_ALL_MODE,
sizeof(player_camera_data_t), 128, 10, 10)
{
showFPS = 0;
sourceCh = 0;
const char *schary;
// Camera defaults to /dev/video0 and NTSC
this->device = cf->ReadString(section, "dev_file", "/dev/video0");
// Input source
this->sourceCh = NULL;
schary = cf->ReadString(section, "input", NULL);
if (schary!=NULL && schary[0]>='0' && schary[0]<='9')
this->source = cf->ReadInt(section, "input", 0);
else {
this->source = -1;
this->sourceCh = schary;
}
// NTSC or PAL
schary = cf->ReadString(section, "norm", "pal");
if (selectNorm(schary)!=0)
selectNorm(normy[0].name);
// Size
this->width = cf->ReadTupleInt(section, "size", 0, this->width);
this->height = cf->ReadTupleInt(section, "size", 1, this->height);
// Palette type
schary = cf->ReadString(section, "mode", "RGB24");
if (selectFormat(schary)!=0)
selectFormat(formaty[0].name);
schary = cf->ReadString(section, "field", "ANY");
if (selectField(schary)!=0)
selectField(fieldy[0].name);
flip_rb = cf->ReadInt(section, "swap_rb", 0);
showFPS = cf->ReadInt(section, "show_fps", -1);
if (showFPS>9)
showFPS = 9;
//printf("#controls: %d", cf->GetTupleCount(section, "controls") );
numOfCtls = cf->GetTupleCount(section, "controls")/2;
if (numOfCtls>0){
int i;
double k=0;
ctlNames = new const char*[numOfCtls];
ctlVals = new double[numOfCtls];
for(i=0; i<numOfCtls; i++){
ctlNames[i] = cf->ReadTupleString(section, "controls", i*2+1, "null");
k = cf->ReadTupleFloat(section, "controls", i*2, 200.0);
if (k>1.0){
numOfCtls = i;
PLAYER_WARN1("Wrong value format for control %s, need val <= 1.0", ctlNames[i]);
} else {
ctlVals[i]=(double)k;//??
}
}
}
back_source = source;
back_norm = norm;
back_depth = depth;
back_width = width;
back_height = height;
back_fieldType = fieldType;
back_v4l2_type_id = v4l2_type_id;
puts("Camerav4l2: Driver object created");
return;
}
int TE::ProcessMessage(MessageQueue* resp_queue,
player_msghdr * hdr,
void * data) {
// Is it new odometry data?
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
odom_addr)) {
ProcessOutputOdom(hdr, (player_position2d_data_t*) data);
// In case the input and output are the same device
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
localize_addr))
ProcessInputOdom(hdr, (player_position2d_data_t*) data);
return (0);
}// Is it new localization data?
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
localize_addr)) {
ProcessInputOdom(hdr, (player_position2d_data_t*) data);
return (0);
}// Is it a new laser scan?
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_LASER_DATA_SCAN,
laser_addr)) {
ProcessLaser(hdr, (player_laser_data_t*) data);
return (0);
}// Is it a new goal?
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_POS,
device_addr)) {
ProcessCommand(hdr, (player_position2d_cmd_pos_t*) data);
return 0;
} else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL,
device_addr)) {
ProcessCommand(hdr, (player_position2d_cmd_vel_t*) data);
return 0;
}// Is it a request for the underlying device?
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, device_addr)) {
// Pass the request on to the underlying position device and wait for
// the reply.
Message* msg;
if (!(msg = odom->Request(InQueue,
hdr->type,
hdr->subtype,
(void*) data,
hdr->size,
&hdr->timestamp))) {
PLAYER_WARN1("TE::ProcessCommand Failed to forward config request with subtype: %d\n",
hdr->subtype);
return (-1);
}
player_msghdr_t* rephdr = msg->GetHeader();
void* repdata = msg->GetPayload();
// Copy in our address and forward the response
rephdr->addr = device_addr;
Publish(resp_queue, rephdr, repdata);
delete msg;
return (0);
} else
return -1;
}
