From e2fc7e5f9701777652fc6a18d0e42cf23e84ecae Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Wed, 7 Jan 2015 14:00:04 +0100 Subject: client lib --- lib.cpp | 68 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 lib.cpp (limited to 'lib.cpp') diff --git a/lib.cpp b/lib.cpp new file mode 100644 index 0000000..fbbf338 --- /dev/null +++ b/lib.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include +#include +#include + +#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); +} -- cgit v1.2.3