////////////////////////////////////////////////////////////////////////////////
/// Process incoming requests
int CameraV4L2::HandleRequests()
{
void *client;
char request[PLAYER_MAX_REQREP_SIZE];
char outBuf[PLAYER_MAX_REQREP_SIZE];
int len;
while ((len = GetConfig(&client, &request, sizeof(request),NULL)) > 0)
{
int ret = -1;
PLAYER_MSG2(2,"Got Reguest %c (size: %d)", request[0], len);
if (len>1 && (request[0]=='g' || request[0]=='G')){
// pobranie jakiejs wartosci
PLAYER_MSG0(2,"Get type request");
ret = handleCommand(request, len, outBuf, PLAYER_MAX_REQREP_SIZE-1);
if (ret==0) {
outBuf[PLAYER_MAX_REQREP_SIZE-1] = '\0';
PLAYER_MSG1(2,"Sending Ack: %s", outBuf);
if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, outBuf, strlen(outBuf)+1,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
} else {
PLAYER_MSG0(2,"Sendinf NACK");
if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
}
} else {
if (len>0) {
ret = handleCommand(request, len);
}
if (ret == 0){
PLAYER_MSG0(2,"Sending Ack");
if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
} else {
PLAYER_MSG0(2,"Sending Nack");
if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
}
}
// if framegrabber is slow(1-10s), it is worth handling all commands at once
//usleep(30000);
}
return 0;
}
void
TE::ProcessLaser(player_msghdr_t*, player_laser_data_t* scan) {
double r, b, db;
double dmin = obs_dist;
obstacle = false;
beta = M_PI / 2;
db = scan->resolution;
for (unsigned int i = 0; i < scan->ranges_count; i++) {
b = scan->min_angle + (i * db);
r = scan->ranges[i];
if (b >= laser_min_angle && b <= laser_max_angle) {
if (r < dmin) {
obstacle = true;
dmin = r;
beta = b;
}
}
}
nearObstDist = dmin;
if (dmin <= min_dist) {
if (verbose)
PLAYER_MSG0(0, "TE::ProcessLaser Stoped");
waiting = true;
PutPositionCmd(0, 0);
stall = true;
}
}
////////////////////////////////////////////////////////////////////////////////
// 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;
}
/**
* 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;
}
int player_driver_init(DriverTable* table) {
PLAYER_MSG0(0, "TE driver initializing");
TE_Register(table);
PLAYER_MSG0(0, "TE initialization done");
return (0);
}
void
TE::Main() {
double g_dx, g_da;
double alpha, theta, phi;
double vx, va;
player_pose_t relativeGoal;
player_pose_t currGoal;
// Fill in the TE's parameter structure
current_dir = 1;
for (;;) {
usleep(200000); // 200 ms delay
pthread_testcancel();
// call
ProcessMessages();
// are we waiting for a stall to clear?
if (waiting) {
PutPositionCmd(0, 0);
stall = true;
if (verbose)
PLAYER_MSG0(0, "TE::Main waiting");
continue;
}
// do we have a goal?
if (!active_goal) {
continue;
}
// wzgledne polozenie celu
relativeGoal.px = goal.px - odom_pose.px;
relativeGoal.py = goal.py - odom_pose.py;
relativeGoal.pa = goal.pa;
// angle from 0 to the goal (theta)
theta = atan2(relativeGoal.py, relativeGoal.px);
// diff betwean robot orientation angle (psi) and goal vector (theta)
alpha = angle_diff(theta, odom_pose.pa);
g_dx = hypot(relativeGoal.px, relativeGoal.py);
if (obstacle && g_dx > dist_eps) {
//PLAYER_MSG0(1, "TE: obstacle avoidance");
if (fabs(beta) > ang_eps)
phi = angle_diff(fabs(beta) / beta * M_PI / 2,
angle_diff(beta, alpha));
else
phi = angle_diff(M_PI / 2, angle_diff(beta, alpha));
currGoal.px = cos(phi) * relativeGoal.px +
sin(phi) * relativeGoal.py;
currGoal.py = -sin(phi) * relativeGoal.px +
cos(phi) * relativeGoal.py;
currGoal.pa = relativeGoal.pa;
} else
currGoal = relativeGoal;
// angle from 0 to the goal (theta)
theta = atan2(currGoal.py, currGoal.px);
// diff betwean robot orientation angle (psi) and goal vector (theta)
alpha = angle_diff(theta, odom_pose.pa);
// are we at the goal?
g_dx = hypot(currGoal.px, currGoal.py);
g_da = angle_diff(currGoal.pa, odom_pose.pa);
if ((g_dx < dist_eps)) { // jestesmy bliko celu
if (verbose)
PLAYER_MSG0(0, "TE::Main Close to goal");
if (fabs(g_da) < ang_eps) { // z wlasciwa orientacja
active_goal = false;
PutPositionCmd(0.0, 0.0);
if (verbose)
PLAYER_MSG0(0, "TE::Main At goal");
continue;
} else { // trzeba poprawić orientację po dojechaniu do celu
if (verbose)
PLAYER_MSG0(0, "TE::Main Correcting orientation");
vx = 0;
va = k_a * va_max * tanh(10 * g_da);
}
} else {
// sterowanie
vx = vx_max * tanh(fabs(g_dx)) * fabs(cos(alpha));
va = k_a * va_max * tanh(alpha);
}
if (nearObstDist <= obs_dist) {
vx = vx * (nearObstDist - min_dist) / (obs_dist - min_dist);
}
if (nearObstDist <= min_dist) {
vx = 0;
va = 0;
}
PutPositionCmd(vx, va);
}
}
////////////////////////////////////////////////////////////////////////////////
// Clean up resources.
int LocalBB::Shutdown()
{
PLAYER_MSG0(2, "LocalBB shut down");
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Load resources.
int LocalBB::Setup()
{
PLAYER_MSG0(2, "LocalBB ready");
return 0;
}
