new send and submodule masa_protocol added

This commit is contained in:
mive93
2019-05-08 11:34:18 +02:00
parent caf4ddbce2
commit e38d8e82ca
8 changed files with 206 additions and 596 deletions
+3
View File
@@ -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
-2
View File
@@ -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
+64 -5
View File
@@ -14,7 +14,10 @@
#include <opencv2/imgproc/imgproc.hpp>
#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<int>(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<<s.str()<<std::endl;
//std::cout<<s.str().length()<<std::endl;
Comm.send_message(m);
}
free(coords);
-283
View File
@@ -1,283 +0,0 @@
/*
C socket server example, handles multiple clients using threads
*/
#include <stdio.h>
#include <string.h> //strlen
#include <stdlib.h> //strlen
#include <time.h>
#include <sys/socket.h>
#include <arpa/inet.h> //inet_addr
#include <unistd.h> //write
#include <pthread.h> //for threading , link with lpthread
#include <semaphore.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "serialize.hpp"
#include <string>
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: "<<s<<std::endl;
time(&rawtime);
tm_struct = localtime(&rawtime);
mess_min = tm_struct->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;
}
+138
View File
@@ -0,0 +1,138 @@
#ifndef CLASSUTILS_H
#define CLASSUTILS_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/socket.h> //socket
#include <arpa/inet.h> //inet_addr
#include <unistd.h> //write
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include "gdal.h"
#include <gdal_priv.h>
#include <gdal/gdal.h>
#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<cv::Point2f> 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*/
-268
View File
@@ -1,268 +0,0 @@
#ifndef SEND_H
#define SEND_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/socket.h> //socket
#include <arpa/inet.h> //inet_addr
#include <unistd.h> //write
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include "gdal.h"
#include <gdal_priv.h>
#include <gdal/gdal.h>
#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<Road_User> 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<<buf->str()<<std::endl;
//return buf;
}
struct obj_coords *deserialize_coords(char *buffer, int *obj_n)
{
std::stringbuf buf(buffer);
std::istream is(&buf);
cereal::PortableBinaryInputArchive retrieve(is);
Message m;
retrieve(m);
int cam_id = m.cam_idx;
unsigned long long t_stamp_ms = m.t_stamp_ms;
*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;
}
return c;
}
int map_class_coco_to_voc(int coco_class)
{
switch (coco_class)
{
case 0:
return 14; //person
case 1:
return 1; //bicycle
case 2:
return 6; //car
case 3:
return 13; //motorkbike
case 5:
return 5; //bus
}
return -1;
}
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<cv::Point2f> 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<<message->str().length()<<std::endl;
/*open socket if not already opened*/
if (socket_opened == 0)
{
int res = open_socket("127.0.0.1", sock, socket_opened);
if (res)
printf("Socket opened!\n");
else
{
printf("Problem: socket NOT opened!\n");
return 0;
}
}
/*send message to server*/
if (send(sock, message->str().data(), message->str().length(), 0) < 0)
{
puts("Send failed");
socket_opened = 0;
}
delete message;
//free(message);
//close(sock);
return 1;
}
#endif /*SEND_H*/
-38
View File
@@ -1,38 +0,0 @@
#ifndef SERIALIZE_H
#define SERIALIZE_H
#include <cereal/archives/portable_binary.hpp>
#include <cereal/types/vector.hpp>
struct Road_User{
float latitude;
float longitude;
uint8_t speed;
uint8_t orientation;
uint8_t category;
template<class Archive>
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<Road_User> objects;
template<class Archive>
void serialize(Archive & archive)
{
archive( cam_idx, t_stamp_ms, num_objects, objects );
}
};
#endif
Submodule
+1
Submodule masa_protocol added at 63f65f3397