1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
|
// Client library for AR Drone control server
// Copyright (C) 2015 Florian Jung
// All rights reserved. Licensed under the 3-clause-BSD-license as follows:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this
// list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
// may be used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#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(720,1280,CV_8UC3, buffer);
}
void DroneConnection::fly(float x, float y, float z, float rot)
{
char buf[100];
int len = snprintf(buf, sizeof(buf), "fly %f %f %f %f\n", x,y,z,rot);
if (len >= sizeof(buf)-1)
{
printf("ERROR: buffer too small in DroneConnection::fly()!\n");
return;
}
printf("%s\n",buf);
write(sockfd, buf, len);
}
|
