diff options
| -rw-r--r-- | Makefile | 4 | ||||
| -rw-r--r-- | client2.c | 62 | ||||
| -rw-r--r-- | lib.cpp | 68 | ||||
| -rw-r--r-- | lib.h | 28 |
4 files changed, 104 insertions, 58 deletions
@@ -1,4 +1,4 @@ -client2: client2.c - g++ client2.c `pkg-config --libs opencv` -lm -o client2 +client2: client2.c lib.cpp + g++ client2.c lib.cpp `pkg-config --libs opencv` -lm -o client2 client: client.c gcc client.c -lX11 -lXi -lXmu -lglut -lGL -lGLU -lm -o client @@ -7,63 +7,20 @@ #include <time.h> #include <opencv2/opencv.hpp> +#include "lib.h" using namespace cv; #define SOCKETPATH "/home/flo/uds_socket" -void die(const char* msg){perror(msg); exit(1);} -void suicide(const char* msg){ fprintf(stderr, "%s\n", msg); exit(1); } - - - -unsigned char buffer[67108864]; // must be unsigned. because reasons -_- - -int read_completely(int fd, void* buf, size_t len) -{ - size_t n_read; - for (n_read = 0; n_read < len;) - { - size_t tmp = read(fd, buf, len-n_read); - n_read+=tmp; - buf = ((char*)buf)+tmp; - } - return n_read; -} - int main(int argc, const char** argv) { - struct sockaddr_un my_sockaddr; - struct navdata_t - { - double phi; - double theta; - double psi; - double batt; - }; - - my_sockaddr.sun_family=AF_UNIX; - strcpy(my_sockaddr.sun_path, SOCKETPATH); - int sockaddrlen = strlen(my_sockaddr.sun_path) + sizeof(my_sockaddr.sun_family); - - int sockfd = socket(AF_UNIX, SOCK_STREAM, 0); - if (sockfd == -1) die("socket"); - - - if (connect(sockfd, (struct sockaddr*) &my_sockaddr, sockaddrlen) == -1) - die("connect"); + DroneConnection drone(SOCKETPATH); printf("GO!!\n"); time_t t = time(NULL); int nframes=0; - write(sockfd,"get\n",4); - read_completely(sockfd, buffer, 4); - int framelen = ((buffer[0]*256+buffer[1])*256+buffer[2])*256+buffer[3]; - if (framelen + sizeof(navdata_t) > sizeof(buffer)) suicide("buffer too small"); - read_completely(sockfd, buffer, framelen+sizeof(navdata_t)); - const navdata_t* navdata = (navdata_t*)(buffer + framelen); - printf("%lf\t%lf\t%lf\t%lf\n", navdata->phi, navdata->theta, navdata->psi, navdata->batt); char key; bool go=true; bool new_vals=true; @@ -93,6 +50,8 @@ int main(int argc, const char** argv) //col[0]=-0.01; //col[1]=0.0; //col[2]=0.01; + + Mat dingens; while ((key = waitKey(1)) != 'x') { @@ -146,18 +105,10 @@ int main(int argc, const char** argv) } if (go) { - write(sockfd,"get\n",4); - read_completely(sockfd, buffer, 4); - int framelen = ((buffer[0]*256+buffer[1])*256+buffer[2])*256+buffer[3]; - if (framelen + sizeof(navdata_t) > sizeof(buffer)) suicide("buffer too small"); - read_completely(sockfd, buffer, framelen+sizeof(navdata_t)); - - const navdata_t* navdata = (navdata_t*)(buffer + framelen); - printf("%lf\t%lf\t%lf\t%lf\n", navdata->phi, navdata->theta, navdata->psi, navdata->batt); + navdata_t navdata; + drone.get(dingens, &navdata); } - //Mat dingens=Mat::eye(100,100,CV_8UC1) * 244; - Mat dingens(768,1280,CV_8UC3, buffer); for (int i=0; i< 1280; i+=50) dingens.col(i)=Vec3b(255,192,128); for (int i=0; i< 768; i+=50) @@ -197,6 +148,5 @@ int main(int argc, const char** argv) nframes++; } - close(sockfd); return 0; } @@ -0,0 +1,68 @@ +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <sys/socket.h> +#include <string.h> +#include <sys/un.h> +#include <time.h> + +#include "lib.h" + +using namespace cv; + +#define BUFSIZE 67108864 + +static void die(const char* msg){perror(msg); exit(1);} +static void suicide(const char* msg){ fprintf(stderr, "%s\n", msg); exit(1); } + +static int read_completely(int fd, void* buf, size_t len) +{ + size_t n_read; + for (n_read = 0; n_read < len;) + { + size_t tmp = read(fd, buf, len-n_read); + n_read+=tmp; + buf = ((char*)buf)+tmp; + } + return n_read; +} + + +DroneConnection::DroneConnection(const char* sockpath) +{ + struct sockaddr_un my_sockaddr; + buffer = new unsigned char[BUFSIZE]; + + my_sockaddr.sun_family=AF_UNIX; + strcpy(my_sockaddr.sun_path, sockpath); + int sockaddrlen = strlen(my_sockaddr.sun_path) + sizeof(my_sockaddr.sun_family); + + sockfd = socket(AF_UNIX, SOCK_STREAM, 0); + if (sockfd == -1) die("socket"); + + + if (connect(sockfd, (struct sockaddr*) &my_sockaddr, sockaddrlen) == -1) + die("connect"); +} + +DroneConnection::~DroneConnection() +{ + close(sockfd); + delete [] buffer; +} + +void DroneConnection::get(Mat& frame, navdata_t* nav) +{ + write(sockfd,"get\n",4); + + read_completely(sockfd, buffer, 4); + int framelen = ((buffer[0]*256+buffer[1])*256+buffer[2])*256+buffer[3]; + if (framelen + sizeof(navdata_t) > BUFSIZE) suicide("buffer too small"); + + read_completely(sockfd, buffer, framelen+sizeof(navdata_t)); + + const navdata_t* navdata = (navdata_t*)(buffer + framelen); + memcpy(nav, navdata, sizeof(*navdata)); + + frame = Mat(768,1280,CV_8UC3, buffer); +} @@ -0,0 +1,28 @@ +#ifndef __LIB_H__ +#define __LIB_H__ + +#include <opencv2/opencv.hpp> + +struct navdata_t +{ + double phi; + double theta; + double psi; + double batt; +}; + + +class DroneConnection +{ + public: + DroneConnection(const char* sockpath); + ~DroneConnection(); + void get(cv::Mat& frame, navdata_t* navdata); + + private: + unsigned char* buffer; + int sockfd = -1; + +}; + +#endif |