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 visp_init()
{
com_in = jakopter_com_add_channel(CHANNEL_DISPLAY, VISP_COM_IN_SIZE);
if (com_in == NULL) {
fprintf(stderr, "[~][Display] Couldn't create com channel in.\n");
return -1;
}
com_out = jakopter_com_add_channel(CHANNEL_CALLBACK, VISP_COM_OUT_SIZE);
if (com_out == NULL) {
fprintf(stderr, "[~][Display] Couldn't create com channel out.\n");
return -1;
}
jakopter_com_write_float(com_out, 0, 0.0);
jakopter_com_write_float(com_out, 4, 0.0);
jakopter_com_write_float(com_out, 8, 0.0);
prev_update = 0;
return 0;
}
//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;
}
/**
* \brief Receive the navdata from the drone and write it in NAVDATA_CHANNEL.
* \return the result of recvfrom
*/
int recv_cmd()
{
pthread_mutex_lock(&mutex_navdata);
socklen_t len = sizeof(addr_drone_navdata);
int ret = recvfrom(sock_navdata, &data, sizeof(data), 0, (struct sockaddr*)&addr_drone_navdata, &len);
size_t offset = 0;
switch (data.demo.tag) {
case TAG_DEMO:
jakopter_com_write_int(nav_channel, offset, data.demo.vbat_flying_percentage);
offset += sizeof(data.demo.vbat_flying_percentage);
jakopter_com_write_int(nav_channel, offset, data.demo.altitude);
offset += sizeof(data.demo.altitude);
jakopter_com_write_float(nav_channel, offset, data.demo.theta);
offset += sizeof(data.demo.theta);
jakopter_com_write_float(nav_channel, offset, data.demo.phi);
offset += sizeof(data.demo.phi);
jakopter_com_write_float(nav_channel, offset, data.demo.psi);
offset += sizeof(data.demo.psi);
jakopter_com_write_float(nav_channel, offset, data.demo.vx);
offset += sizeof(data.demo.vx);
jakopter_com_write_float(nav_channel, offset, data.demo.vy);
offset += sizeof(data.demo.vy);
jakopter_com_write_float(nav_channel, offset, data.demo.vz);
break;
default:
break;
}
pthread_mutex_unlock(&mutex_navdata);
return ret;
}
int jakopter_com_write_float_lua(lua_State* L)
{
lua_Integer id = luaL_checkinteger(L, 1);
lua_Integer offset = luaL_checkinteger(L, 2);
float value = luaL_checknumber(L, 3);
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);
jakopter_com_write_float(cc, offset, value);
return 0;
}
int video_display_init()
{
com_in = jakopter_com_add_channel(CHANNEL_DISPLAY, DISPLAY_COM_IN_SIZE);
if (com_in == NULL) {
fprintf(stderr, "[~][Display] Couldn't create com channel.\n");
return -1;
}
com_out = jakopter_com_add_channel(CHANNEL_CALLBACK, DISPLAY_COM_OUT_SIZE);
if (com_out == NULL) {
fprintf(stderr, "[~][Display] Couldn't create com channel out.\n");
return -1;
}
jakopter_com_write_float(com_out, 0, 0.0);
jakopter_com_write_int(com_in, 28, -1);
jakopter_com_write_int(com_in, 32, -1);
jakopter_com_write_int(com_in, 36, -1);
jakopter_com_write_int(com_in, 40, -1);
prev_update = 0;
return 0;
}
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;
}
/**
* \brief Receive the navdata from the drone and write it in NAVDATA_CHANNEL.
* \return the result of recvfrom
*/
static int recv_cmd()
{
pthread_mutex_lock(&mutex_navdata);
socklen_t len = sizeof(addr_drone_navdata);
int ret = recvfrom(sock_navdata, &data, sizeof(data), 0, (struct sockaddr*)&addr_drone_navdata, &len);
size_t offset = 0;
//fprintf(stderr,"FBO: Receiving navdata (%d)\n", data.demo.altitude); // use altitude
switch (data.demo.tag) {
case TAG_DEMO:
jakopter_com_write_int(nav_channel, offset, data.demo.vbat_flying_percentage);
offset += sizeof(data.demo.vbat_flying_percentage);
jakopter_com_write_int(nav_channel, offset, data.demo.altitude);
offset += sizeof(data.demo.altitude);
jakopter_com_write_float(nav_channel, offset, data.demo.theta);
offset += sizeof(data.demo.theta);
jakopter_com_write_float(nav_channel, offset, data.demo.phi);
offset += sizeof(data.demo.phi);
jakopter_com_write_float(nav_channel, offset, data.demo.psi);
offset += sizeof(data.demo.psi);
jakopter_com_write_float(nav_channel, offset, data.demo.vx);
offset += sizeof(data.demo.vx);
jakopter_com_write_float(nav_channel, offset, data.demo.vy);
offset += sizeof(data.demo.vy);
jakopter_com_write_float(nav_channel, offset, data.demo.vz);
break;
default:
break;
}
pthread_mutex_unlock(&mutex_navdata);
// ADDED: Display navdatas for debug purpose
debug_navdata_demo();
navdata_timestamp();
return ret;
}
