From e38d8e82ca38764091161958251a361b495fbdcc Mon Sep 17 00:00:00 2001 From: mive93 Date: Wed, 8 May 2019 11:34:18 +0200 Subject: [PATCH] new send and submodule masa_protocol added --- .gitmodules | 3 + CMakeLists.txt | 2 - demo/demo/demo.cpp | 69 +++++++- demo/server/server_less_dummy.cpp | 283 ------------------------------ include/classutils.h | 138 +++++++++++++++ include/send.h | 268 ---------------------------- include/serialize.hpp | 38 ---- masa_protocol | 1 + 8 files changed, 206 insertions(+), 596 deletions(-) delete mode 100644 demo/server/server_less_dummy.cpp create mode 100644 include/classutils.h delete mode 100644 include/send.h delete mode 100644 include/serialize.hpp create mode 160000 masa_protocol diff --git a/.gitmodules b/.gitmodules index 0772b29..33a7887 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "tracker_CLASS"] path = tracker_CLASS url = https://github.com/mive93/tracker_CLASS.git +[submodule "masa_protocol"] + path = masa_protocol + url = https://git.hipert.unimore.it/rcavicchioli/masa_protocol.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 327742d..43cd091 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -107,8 +107,6 @@ add_executable(yolo3_demo demo/demo/demo.cpp target_link_libraries(yolo3_demo tkDNN) target_link_libraries(yolo3_demo python2.7 yaml-cpp) -add_executable(class_server demo/server/server_less_dummy.cpp) -target_link_libraries(class_server pthread tkDNN ) #install diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index b466905..a361b20 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -14,7 +14,10 @@ #include #include "Yolo3Detection.h" -#include "send.h" +#include "classutils.h" +#include "../masa_protocol/include/send.hpp" +#include "../masa_protocol/include/serialize.hpp" + #include "ekf.h" #include "trackutils.h" #include "plot.h" @@ -58,6 +61,52 @@ void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, c cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow! } +unsigned long long time_in_ms() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; + return t_stamp_ms; +} + +void prepare_message(Message *m, struct obj_coords *coords, int n_coords, int idx) +{ + m->cam_idx = idx; + m->t_stamp_ms = time_in_ms(); + m->num_objects = n_coords; + + m->objects.clear(); + for (int i = 0; i < n_coords ; i++) + { + Categories cat; + switch (static_cast(coords[i].cl)) + { + case 0: + cat = Categories::C_person; + break; + case 1: + cat = Categories::C_car; + break; + case 2: + cat = Categories::C_car; + break; + case 3: + cat = Categories::C_bus; + break; + case 4: + cat = Categories::C_motorbike; + break; + case 5: + cat = Categories::C_bycicle; + break; + } + RoadUser r{coords[i].LAT, coords[i].LONG, 0, 1, C_car}; + m->objects.push_back(r); + } + + m->lights.clear(); +} + int main(int argc, char *argv[]) { @@ -76,7 +125,7 @@ int main(int argc, char *argv[]) char *tiffile = "../demo/demo/data/map_b.tif"; if (argc > 4) tiffile = argv[4]; - int CAM_IDX = 0; + int CAM_IDX = 20936; if (argc > 5) CAM_IDX = atoi(argv[5]); bool to_show = true; @@ -143,8 +192,10 @@ int main(int argc, char *argv[]) struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords)); /*socket*/ - int sock; - int socket_opened = 0; + + Communicator Comm(SOCK_DGRAM); + Comm.open_client_socket("127.0.0.1", 8888); + Message *m = new Message; /*Conversion for tracker, from gps to meters and viceversa*/ geodetic_converter::GeodeticConverter gc; @@ -313,7 +364,15 @@ int main(int argc, char *argv[]) } frame_nbr++; - send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX); + prepare_message(m,coords, coord_i,CAM_IDX); + std::stringbuf s; + Comm.serialize_coords(m,&s); + + //std::cout< -#include //strlen -#include //strlen -#include -#include -#include //inet_addr -#include //write -#include //for threading , link with lpthread -#include -#include -#include -#include "serialize.hpp" -#include - -sem_t semaphore; - -struct obj_coords -{ - float LAT; - float LONG; - float cl; -}; - -int write_coords_to_file(std::string buffer, FILE *f) -{ - - std::istringstream is(buffer); - cereal::PortableBinaryInputArchive retrieve(is); - Message m; - retrieve(m); - - int cam_id = m.cam_idx; - unsigned long long t_stamp_ms = m.t_stamp_ms; - int obj_n = m.num_objects; - - struct obj_coords *c = (struct obj_coords *)malloc(obj_n * sizeof(struct obj_coords)); - - int i; - for (i = 0; i < obj_n; i++) - { - c[i].LAT = m.objects.at(i).latitude; - c[i].LONG = m.objects.at(i).longitude; - c[i].cl = m.objects.at(i).category; - //m.objects.at(i).speed; - //m.objects.at(i).orientation; - } - - char *to_print = (char *)malloc(100000); - memset(to_print, 0, 100000); - - /*int obj_n; - int cam_id; - unsigned long long t_stamp_ms; - char type_of_m; - - int consumed_chars = 0; - char *shifted_chars = (char*)malloc(4000); - char *to_print = (char*)malloc(100000); - memset(to_print,0,100000); - - sscanf(buffer, "%c %lld %d %d ", &type_of_m, &t_stamp_ms, &cam_id, &obj_n); - sprintf(shifted_chars, "%c %lld %d %d ", type_of_m, t_stamp_ms, cam_id, obj_n); - consumed_chars += strlen(shifted_chars); - //printf("%c %lld %d %d\n", type_of_m, t_stamp_ms, cam_id, *obj_n); - - struct obj_coords *c = (struct obj_coords *)malloc(obj_n * sizeof(struct obj_coords)); - - int i; - for (i = 0; i < obj_n; i++) - { - sscanf(buffer + consumed_chars, "%f %f %f ", &c[i].LAT, &c[i].LONG, &c[i].cl); - sprintf(shifted_chars, "%.9f %.9f %.0f ", c[i].LAT, c[i].LONG, c[i].cl); - consumed_chars += strlen(shifted_chars); - //printf("%Lf %f %f \n", c[i].LAT, c[i].LONG, c[i].cl); - }*/ - - char *command = (char *)malloc(4000); - for (i = 0; i < obj_n; i++) - { - sprintf(command, "%d %lld %.0f %.9f %.9f %f %f\n", cam_id, t_stamp_ms, c[i].cl, c[i].LAT, c[i].LONG, 0.0f, 0.0f); - strcat(to_print, command); - } - - printf("%s", to_print); - - fprintf(f, "%s", to_print); - free(to_print); - free(command); - free(c); - - return obj_n; -} - -//the thread function -void *connection_handler(void *); - -int main(int argc, char *argv[]) -{ - const int path_size = 400; - //const int message_size = 100000; - char path[path_size]; - //int obj_n; - const char *basepath = "/tmp/"; - struct stat st = {0}; - - sem_init(&semaphore, 0, 1); - - int socket_desc, client_sock, c, *new_sock; - struct sockaddr_in server, client; - - time_t rawtime; - time(&rawtime); - struct tm *tm_struct = localtime(&rawtime); - int tm_hour = tm_struct->tm_hour; - int tm_yday = tm_struct->tm_yday; - - sprintf(path, "%s%d/", basepath, tm_yday); - if (stat(path, &st) == -1) - { - mkdir(path, 0700); - } - memset(path, 0, path_size); - - sprintf(path, "%s%d/%d/", basepath, tm_yday, tm_hour); - if (stat(path, &st) == -1) - { - mkdir(path, 0700); - } - memset(path, 0, path_size); - - //Create socket - socket_desc = socket(AF_INET, SOCK_STREAM, 0); - if (socket_desc == -1) - { - printf("Could not create socket"); - } - puts("Socket created"); - - //Prepare the sockaddr_in structure - server.sin_family = AF_INET; - server.sin_addr.s_addr = INADDR_ANY; - server.sin_port = htons(8888); - - //Bind - if (bind(socket_desc, (struct sockaddr *)&server, sizeof(server)) < 0) - { - //print the error message - perror("bind failed. Error"); - return 1; - } - puts("bind done"); - - //Listen - listen(socket_desc, 3); - - //Accept and incoming connection - puts("Waiting for incoming connections..."); - c = sizeof(struct sockaddr_in); - while ((client_sock = accept(socket_desc, (struct sockaddr *)&client, (socklen_t *)&c))) - { - puts("Connection accepted"); - - pthread_t sniffer_thread; - new_sock = (int *)malloc(1); - *new_sock = client_sock; - - if (pthread_create(&sniffer_thread, NULL, connection_handler, (void *)new_sock) < 0) - { - perror("could not create thread"); - return 1; - } - - //Now join the thread , so that we dont terminate before the thread - //pthread_join( sniffer_thread , NULL); - puts("Handler assigned"); - } - - if (client_sock < 0) - { - perror("accept failed"); - return 1; - } - - sem_destroy(&semaphore); - - return 0; -} - -/* - * This will handle connection for each client - * */ -void *connection_handler(void *socket_desc) -{ - time_t rawtime; - struct tm *tm_struct; - int tm_hour; - int tm_yday; - const int path_size = 400; - const int message_size = 100000; - char path[path_size]; - int obj_n; - const char *basepath = "/tmp/"; - struct stat st = {0}; - FILE *f; - int mess_hour, mess_min, mess_yday; - - //Get the socket descriptor - int sock = *(int *)socket_desc; - int read_size; - - void *client_message = (void *)malloc(message_size); - - /* //Send some messages to the client - message = "Greetings! I am your connection handler\n"; - write(sock, message, strlen(message)); */ - - //Receive a message from client - while ((read_size = recv(sock, client_message, message_size, 0)) > 0) - { - - std::string s((char *)client_message, message_size); - //std::cout<<"Message received: "<tm_min; - mess_hour = tm_struct->tm_hour; - mess_yday = tm_struct->tm_yday; - if (mess_yday != tm_yday) - { - tm_yday = mess_yday; - sprintf(path, "%s%d/", basepath, tm_yday); - if (stat(path, &st) == -1) - { - mkdir(path, 0700); - } - memset(path, 0, path_size); - } - if (mess_hour != tm_hour) - { - tm_hour = mess_hour; - sprintf(path, "%s%d/%d/", basepath, tm_yday, tm_hour); - if (stat(path, &st) == -1) - { - mkdir(path, 0700); - } - memset(path, 0, path_size); - } - - sprintf(path, "%s%d/%d/%d.txt", basepath, tm_yday, tm_hour, mess_min); - - //CRITICAL SECTION - sem_wait(&semaphore); - f = fopen(path, "a"); - memset(path, 0, path_size); - - obj_n = write_coords_to_file(s, f); - - fclose(f); - sem_post(&semaphore); - } - - if (read_size == 0) - { - puts("Client disconnected"); - fflush(stdout); - } - else if (read_size == -1) - { - perror("recv failed"); - } - - //Free the socket pointer - free(socket_desc); - - free(client_message); - - return 0; -} diff --git a/include/classutils.h b/include/classutils.h new file mode 100644 index 0000000..d6aaa5e --- /dev/null +++ b/include/classutils.h @@ -0,0 +1,138 @@ +#ifndef CLASSUTILS_H +#define CLASSUTILS_H + +#include +#include +#include +#include +#include //socket +#include //inet_addr +#include //write + +#include +#include +#include "gdal.h" +#include +#include +#include "gdal/gdal_priv.h" +#include "gdal/cpl_conv.h" + + +struct obj_coords +{ + double LAT; + double LONG; + float cl; +}; + +void readTiff(char *filename, double *adfGeoTransform) +{ + GDALDataset *poDataset; + GDALAllRegister(); + poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly); + if (poDataset != NULL) + { + //int colms = poDataset->GetRasterXSize(); + //int rows = poDataset->GetRasterYSize(); + poDataset->GetGeoTransform(adfGeoTransform); + } +} + +void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform) +{ + //Returns global coordinates from pixel x, y coordinates + double xoff, a, b, yoff, d, e; + xoff = adfGeoTransform[0]; + a = adfGeoTransform[1]; + b = adfGeoTransform[2]; + yoff = adfGeoTransform[3]; + d = adfGeoTransform[4]; + e = adfGeoTransform[5]; + + //printf("%f %f %f %f %f %f\n",xoff, a, b, yoff, d, e ); + + lon = a * x + b * y + xoff; + lat = d * x + e * y + yoff; +} +void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform) +{ + x = int(round((lon - adfGeoTransform[0]) / adfGeoTransform[1])); + y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5])); +} + +void fillMatrix(cv::Mat &H, float *matrix, bool show = false) +{ + double *vals = (double *)H.data; + for (int i = 0; i < 9; i++) + { + vals[i] = matrix[i]; + } + if (show) + std::cout << H << "\n"; +} + + + + +//FILE *out_file = fopen("prova_pixel.txt", "w"); + +void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected_class, cv::Mat H, double *adfGeoTransform, int frame_nbr) +{ + double latitude, longitude; + std::vector x_y, ll; + x_y.push_back(cv::Point2f(x, y)); + + //transform camera pixel to map pixel + cv::perspectiveTransform(x_y, ll, H); + //tranform to map pixel to map gps + pixel2coord(ll[0].x, ll[0].y, latitude, longitude, adfGeoTransform); + //printf("lat: %f, long:%f \n", latitude, longitude); + coords[i].LAT = latitude; + coords[i].LONG = longitude; + coords[i].cl = detected_class; + + /*if (detected_class == 0) + { + + struct timeval tv; + gettimeofday(&tv, NULL); + unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; + + //printf(out_file, "%d %lld %d %d\n",frame_nbr, t_stamp_ms, int(ll[0].x), int(ll[0].y)); + fprintf(out_file, "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); + //printf( "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); + }*/ +} + +void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path) +{ + FILE *fp; + char *line = NULL; + size_t len = 0; + ssize_t read; + + float *proj_matrix = (float *)malloc(9 * sizeof(float)); + + fp = fopen(path, "r"); + if (fp == NULL) + exit(EXIT_FAILURE); + + int i = 0; + while ((read = getline(&line, &len, fp)) != -1) + { + if (3 == sscanf(line, "%f %f %f", &proj_matrix[i * 3 + 0], &proj_matrix[i * 3 + 1], &proj_matrix[i * 3 + 2])) + { + i++; + proj_matrix_read = 1; + } + } + free(line); + fclose(fp); + fillMatrix(H, proj_matrix); + + + + free(proj_matrix); +} + +#endif /*CLASSUTILS_H*/ \ No newline at end of file diff --git a/include/send.h b/include/send.h deleted file mode 100644 index 294572a..0000000 --- a/include/send.h +++ /dev/null @@ -1,268 +0,0 @@ -#ifndef SEND_H -#define SEND_H - -#include -#include -#include -#include -#include //socket -#include //inet_addr -#include //write - -#include -#include -#include "gdal.h" -#include -#include -#include "gdal/gdal_priv.h" -#include "gdal/cpl_conv.h" - -#include "serialize.hpp" - -struct obj_coords -{ - double LAT; - double LONG; - float cl; -}; - -void readTiff(char *filename, double *adfGeoTransform) -{ - GDALDataset *poDataset; - GDALAllRegister(); - poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly); - if (poDataset != NULL) - { - //int colms = poDataset->GetRasterXSize(); - //int rows = poDataset->GetRasterYSize(); - poDataset->GetGeoTransform(adfGeoTransform); - } -} - -void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform) -{ - //Returns global coordinates from pixel x, y coordinates - double xoff, a, b, yoff, d, e; - xoff = adfGeoTransform[0]; - a = adfGeoTransform[1]; - b = adfGeoTransform[2]; - yoff = adfGeoTransform[3]; - d = adfGeoTransform[4]; - e = adfGeoTransform[5]; - - //printf("%f %f %f %f %f %f\n",xoff, a, b, yoff, d, e ); - - lon = a * x + b * y + xoff; - lat = d * x + e * y + yoff; -} -void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform) -{ - x = int(round((lon - adfGeoTransform[0]) / adfGeoTransform[1])); - y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5])); -} - -void fillMatrix(cv::Mat &H, float *matrix, bool show = false) -{ - double *vals = (double *)H.data; - for (int i = 0; i < 9; i++) - { - vals[i] = matrix[i]; - } - if (show) - std::cout << H << "\n"; -} - -void serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX, std::stringbuf *buf) -{ - - std::ostream os(buf); - cereal::PortableBinaryOutputArchive archive(os); - - struct timeval tv; - gettimeofday(&tv, NULL); - unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; - - std::vector ruv; - - int i; - for (i = 0; i < obj_n; i++) - { - Road_User r{c[i].LAT, c[i].LONG, 0, 0, (int)c[i].cl}; - ruv.push_back(r); - } - - Message m{CAM_IDX, t_stamp_ms, ruv.size(), ruv}; - archive(m); - - //std::cout<str()< x_y, ll; - x_y.push_back(cv::Point2f(x, y)); - - //transform camera pixel to map pixel - cv::perspectiveTransform(x_y, ll, H); - //tranform to map pixel to map gps - pixel2coord(ll[0].x, ll[0].y, latitude, longitude, adfGeoTransform); - //printf("lat: %f, long:%f \n", latitude, longitude); - coords[i].LAT = latitude; - coords[i].LONG = longitude; - coords[i].cl = map_class_coco_to_voc(detected_class); - - if (detected_class == 0) - { - - struct timeval tv; - gettimeofday(&tv, NULL); - unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; - - //fprintf(out_file, "%d %lld %d %d\n",frame_nbr, t_stamp_ms, int(ll[0].x), int(ll[0].y)); - fprintf(out_file, "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); - } -} - -void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path) -{ - FILE *fp; - char *line = NULL; - size_t len = 0; - ssize_t read; - - float *proj_matrix = (float *)malloc(9 * sizeof(float)); - - fp = fopen(path, "r"); - if (fp == NULL) - exit(EXIT_FAILURE); - - int i = 0; - while ((read = getline(&line, &len, fp)) != -1) - { - if (3 == sscanf(line, "%f %f %f", &proj_matrix[i * 3 + 0], &proj_matrix[i * 3 + 1], &proj_matrix[i * 3 + 2])) - { - i++; - proj_matrix_read = 1; - } - } - free(line); - fclose(fp); - fillMatrix(H, proj_matrix); - - - - free(proj_matrix); -} - -int open_socket(char *ip, int &sock, int &socket_opened) -{ - struct sockaddr_in server; - sock = socket(AF_INET, SOCK_STREAM, 0); - if (sock == -1) - { - printf("Could not create socket"); - } - puts("Socket created"); - - server.sin_addr.s_addr = inet_addr(ip); - server.sin_family = AF_INET; - server.sin_port = htons(8888); - - /*connect to remote server*/ - if (connect(sock, (struct sockaddr *)&server, sizeof(server)) < 0) - { - perror("connect failed. Error"); - socket_opened = 0; - return 0; - } - puts("Connected\n"); - socket_opened = 1; - return 1; -} - -int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &socket_opened, int CAM_IDX) -{ - /*serialize coords*/ - std::stringbuf *message = new std::stringbuf(); - serialize_coords(coords, n_coords, CAM_IDX, message); - - //std::cout<str().length()<str().data(), message->str().length(), 0) < 0) - { - puts("Send failed"); - socket_opened = 0; - } - - delete message; - //free(message); - //close(sock); - return 1; -} - -#endif /*SEND_H*/ \ No newline at end of file diff --git a/include/serialize.hpp b/include/serialize.hpp deleted file mode 100644 index 975c3ec..0000000 --- a/include/serialize.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef SERIALIZE_H -#define SERIALIZE_H - -#include -#include - -struct Road_User{ - float latitude; - float longitude; - uint8_t speed; - uint8_t orientation; - uint8_t category; - - template - void serialize(Archive & archive) - { - archive( latitude, longitude, speed, orientation, category ); - } - -}; - -struct Message{ - uint32_t cam_idx; - uint64_t t_stamp_ms; - uint16_t num_objects; - std::vector objects; - - template - void serialize(Archive & archive) - { - archive( cam_idx, t_stamp_ms, num_objects, objects ); - } -}; - - - - -#endif \ No newline at end of file diff --git a/masa_protocol b/masa_protocol new file mode 160000 index 0000000..63f65f3 --- /dev/null +++ b/masa_protocol @@ -0,0 +1 @@ +Subproject commit 63f65f3397e29081ea7bc84447f4f9a0aa1dc2e3