//////////////////////////////////////////////////////////////////////////////// /// 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; }