void update_infos() { //base y position of the text int base_y = 0; //height of a line of text with the current font. int line_height = TTF_FontLineSkip(font); //buffer to hold the current textto be drawn char buf[TEXT_BUF_SIZE]; //retrieve navdata one by one int bat = jakopter_com_read_int(com_in, 0); //and format it for textual display snprintf(buf, TEXT_BUF_SIZE, "Battery : %d%%", bat); buf[TEXT_BUF_SIZE-1] = '\0'; //finally, print it onto a texture using SDL_ttf graphs[0].tex = video_make_text(buf, &graphs[0].pos.w, &graphs[0].pos.h); //and set its draw position accordingly graphs[0].pos.x = 0; graphs[0].pos.y = base_y; //go to a new line base_y += line_height; int alt = jakopter_com_read_int(com_in, 4); snprintf(buf, TEXT_BUF_SIZE, "Altitude : %d", alt); buf[TEXT_BUF_SIZE-1] = '\0'; graphs[1].tex = video_make_text(buf, &graphs[1].pos.w, &graphs[1].pos.h); graphs[1].pos.x = 0; graphs[1].pos.y = base_y; //update pitch, roll, yaw and speed pitch = jakopter_com_read_float(com_in, 8); roll = jakopter_com_read_float(com_in, 12); yaw = jakopter_com_read_float(com_in, 16); speed = jakopter_com_read_float(com_in, 20); }
static int blob_process(uint8_t* frame, int width, int height, int size) { init_display(width, height); if (!initialized || process_changed) { blob.setGraphics(true); blob.setGraphicsThickness(2); initialized = true; process_changed = false; } prepare_display(frame, width, height, size); //compute blob if (!init_track) { blob.initTracking(*grey_img, germ); init_track = true; } else { blob.track(*grey_img); } int bat = jakopter_com_read_int(com_in, 0); std::ostringstream bat_str; bat_str << bat; vpDisplay::displayText(*display_img, (int)display_img->getHeight()-25, 10, "Battery : " + bat_str.str() + " %%", vpColor::red); vpDisplay::flush(*display_img); return 0; }
int jakopter_com_read_int_lua(lua_State* L) { lua_Integer id = luaL_checkinteger(L, 1); lua_Integer offset = luaL_checkinteger(L, 2); jakopter_com_channel_t* cc = jakopter_com_get_channel(id); if (cc == NULL) return luaL_error(L, "[~][lua] com_channel of id %d doesn't exist", id); lua_pushinteger(L, jakopter_com_read_int(cc, offset)); return 1; }
int count_blue_px(uint8_t* frame, int width, int height, int size) { //size = width*height*3/2 int y_size = width*height; int param = jakopter_com_read_int(com_in, 28); int brightness = (param > -1 && param <= 256) ? param : 70; param = jakopter_com_read_int(com_in, 32); int darkness = (param > -1 && param <= 256) ? param : 256; param = jakopter_com_read_int(com_in, 36); int cr_u = (param > -1 && param <= 256) ? param : 127; param = jakopter_com_read_int(com_in, 40); int cr_v = (param > -1 && param <= 256) ? param : 127; int h = 0; float count = 0.0; while (h < height) { int w = 0; while (w < width) { int y = *(frame + (h * width + w)); int u = *(frame + ((h / 2) * (width / 2) + (w / 2) + y_size)); int v = *(frame + ((h / 2) * (width / 2) + (w / 2) + y_size + (y_size / 4))); if (y > brightness && y < darkness && u >= cr_u && v <= cr_v) count += 1.0; w++; } //printf("%.2f\n", count); h++; } count = count / (float)width; //printf("%.2f\n", count); jakopter_com_write_float(com_out, 0, count); return 0; }
static int qrcode_process(uint8_t* frame, int width, int height, int size) { init_display(width, height); if (!initialized || process_changed) { initialized = true; process_changed = false; } prepare_display(frame, width, height, size); if (qr_detector.detect(*grey_img)) { std::vector<vpImagePoint> pol = qr_detector.getPolygon(0); vpRect bbox = qr_detector.getBBox(0); vpDisplay::displayRectangle(*display_img, bbox, vpColor::green); vpDisplay::displayText(*display_img, (int)(bbox.getTop()-10), (int)bbox.getLeft(), "Message: \"" + qr_detector.getMessage(0) + "\"", vpColor::red); for (size_t j = 0; j < pol.size(); j++) { vpDisplay::displayCross(*display_img, pol[j], 14, vpColor::red, 3); std::ostringstream number; number << j; vpDisplay::displayText(*display_img, pol[j]+vpImagePoint(10,0), number.str(), vpColor::blue); } double x = 0; double y = 0; bbox.getCenter(x, y); std::string msg = qr_detector.getMessage(0); char cstr[SIZE_MESSAGE]; strncpy(cstr, msg.c_str(),SIZE_MESSAGE); jakopter_com_write_float(com_out, 0, (float)bbox.getSize()); jakopter_com_write_float(com_out, 4, (float)x); jakopter_com_write_float(com_out, 8, (float)y); jakopter_com_write_int(com_out, 12, strlen(cstr)+1); jakopter_com_write_buf(com_out, 16, cstr, SIZE_MESSAGE); } int bat = jakopter_com_read_int(com_in, 0); std::ostringstream bat_str; bat_str << bat; vpDisplay::displayText(*display_img, (int)display_img->getHeight()-25, 10, "Battery : " + bat_str.str() + " %", vpColor::red); vpDisplay::flush(*display_img); return 0; }
static int face_process(uint8_t* frame, int width, int height, int size) { //alt_tree, lbpcascade std::string face_cascade_script = "/usr/share/opencv/lbpcascades/lbpcascade_frontalface.xml"; init_display(width, height); if (!initialized || process_changed) { face_detector.setCascadeClassifierFile(face_cascade_script); initialized = true; process_changed = false; } prepare_display(frame, width, height, size); //compute face detection bool face_found = face_detector.detect(*grey_img); if (face_found) { std::ostringstream text; text << "Found " << face_detector.getNbObjects() << " face(s)"; vpDisplay::displayText(*display_img, 10, 10, text.str(), vpColor::red); //we get the first face in the list, which is also the biggest std::vector<vpImagePoint> p = face_detector.getPolygon(0); vpRect bbox = face_detector.getBBox(0); vpDisplay::displayRectangle(*display_img, bbox, vpColor::green, false, 4); vpDisplay::displayText(*display_img, (int)bbox.getTop()-10, (int)bbox.getLeft(), "Message: \"" + face_detector.getMessage(0) + "\"", vpColor::red); double x = 0; double y = 0; bbox.getCenter(x, y); jakopter_com_write_float(com_out, 0, (float)bbox.getSize()); jakopter_com_write_float(com_out, 4, (float)x); jakopter_com_write_float(com_out, 8, (float)y); } int bat = jakopter_com_read_int(com_in, 0); std::ostringstream bat_str; bat_str << bat; vpDisplay::displayText(*display_img, (int)display_img->getHeight()-25, 10, "Battery : " + bat_str.str() + " %", vpColor::red); vpDisplay::flush(*display_img); return 0; }
int jakopter_com_read_string_lua(lua_State* L) { lua_Integer id = luaL_checkinteger(L, 1); lua_Integer offset = luaL_checkinteger(L, 2); jakopter_com_channel_t* cc = jakopter_com_get_channel(id); if (cc == NULL) return luaL_error(L, "[~][lua] com_channel of id %d doesn't exist", id); size_t size = jakopter_com_read_int(cc, offset); char* string = malloc(size); jakopter_com_read_buf(cc, offset+4, size, string); if (size == 0) lua_pushstring(L, NULL); else lua_pushstring(L, string); free(string); return 1; }
//FBO analyse frame on the server once a while // port is 5000 // WARNING this routine is not thread safe int analyse_video(uint8_t* frame, int width, int height, int size){ static int call_count = 0; static int init_callback_done=0; static int sockfd = 0; //FBO DO THE JOB HERE //fprintf(stderr,"Calling sample_video (call count = %d)\n",call_count); jakopter_com_write_int(com_out, 0, call_count); // in all cases need to return the call back instance if (init_callback_done){ call_count++; int doit = jakopter_com_read_int(com_in, 28); if (doit) { copyDataInBuffer(imageBuffer,frame,IMAGE_TO_ANALYZE,width,height,width); sendLargeData(sockfd,imageBuffer,IMAGE_BUFFER_SIZE); receiveLargeData(sockfd, resultBuffer, ANALYSIS_BUFFER_SIZE); int coord_i_min = *((int *) &(resultBuffer[0])); int coord_j_min = *((int *) &(resultBuffer[4])); double valx = *((double *) &(resultBuffer[8])); int samplenum = *((int *) &(resultBuffer[16])); // show it on the video also //printf("server answer sample %d result %d %d %f\n",samplenum, coord_i_min, coord_j_min, valx); //Return result and status here jakopter_com_write_int(com_out, 4, samplenum); jakopter_com_write_int(com_out, 8, coord_i_min); jakopter_com_write_int(com_out, 12, coord_j_min); jakopter_com_write_float(com_out, 16, valx); } } else { call_count = 0; init_callback_done = 1; // Open the socket sockfd = openSocket((char *) "localhost",5000); jakopter_com_write_int(com_out, 4, -1); jakopter_com_write_int(com_out, 8, 0); jakopter_com_write_int(com_out, 12, 0); jakopter_com_write_float(com_out, 16, 0.0); } return 0; }