2008-07-11 20:38:17 +00:00
|
|
|
#include "control.h"
|
|
|
|
|
2008-07-11 22:55:36 +00:00
|
|
|
|
|
|
|
#include <sys/socket.h>
|
|
|
|
#include <netdb.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <errno.h>
|
|
|
|
#include <string.h>
|
2008-07-12 00:17:29 +00:00
|
|
|
#include <stdlib.h>
|
|
|
|
#include <netinet/in.h>
|
|
|
|
#include <netinet/tcp.h>
|
|
|
|
#include <poll.h>
|
2008-07-11 22:55:36 +00:00
|
|
|
|
|
|
|
static int opensocket(const char *hostname, const char *port) {
|
|
|
|
struct addrinfo *res = NULL, pref;
|
2008-07-12 00:17:29 +00:00
|
|
|
int ret, s, val;
|
2008-07-11 22:55:36 +00:00
|
|
|
|
|
|
|
pref.ai_flags = 0;
|
|
|
|
pref.ai_family = PF_UNSPEC;
|
|
|
|
pref.ai_socktype = SOCK_STREAM;
|
|
|
|
pref.ai_protocol = 0;
|
|
|
|
pref.ai_addrlen = 0;
|
|
|
|
pref.ai_addr = NULL;
|
|
|
|
pref.ai_canonname = NULL;
|
|
|
|
pref.ai_next = NULL;
|
|
|
|
|
|
|
|
if (0 != (ret = getaddrinfo(hostname, port, &pref, &res)) || !res) {
|
|
|
|
fprintf(stderr, "Couldn't find host/port '%s:%s': %s\n", hostname, port, strerror(errno));
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (-1 == (s = socket(res->ai_family, res->ai_socktype, res->ai_protocol))) {
|
|
|
|
fprintf(stderr, "Couldn't create socket: %s\n", strerror(errno));
|
|
|
|
freeaddrinfo(res);
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (0 != connect(s, res->ai_addr, res->ai_addrlen)) {
|
|
|
|
fprintf(stderr, "Couldn't connect socket to '%s:%s': %s\n", hostname, port, strerror(errno));
|
|
|
|
freeaddrinfo(res);
|
2008-07-12 00:17:29 +00:00
|
|
|
close(s);
|
2008-07-11 22:55:36 +00:00
|
|
|
return -1;
|
|
|
|
}
|
2008-07-12 00:17:29 +00:00
|
|
|
|
|
|
|
val = 1;
|
|
|
|
if (0 != setsockopt(s, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val))) {
|
|
|
|
fprintf(stderr, "Couldn't set socket option TCP_NODELAY: %s\n", strerror(errno));
|
|
|
|
freeaddrinfo(res);
|
|
|
|
close(s);
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2008-07-11 22:55:36 +00:00
|
|
|
freeaddrinfo(res);
|
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2008-07-11 22:20:05 +00:00
|
|
|
static void telemetry_free(gpointer _te, gpointer x) {
|
|
|
|
telemetry *te = (telemetry*) _te;
|
2008-07-11 22:11:41 +00:00
|
|
|
UNUSED(x);
|
|
|
|
g_array_free(te->objects, TRUE);
|
2008-07-11 22:20:05 +00:00
|
|
|
g_slice_free(telemetry, te);
|
2008-07-11 22:11:41 +00:00
|
|
|
}
|
|
|
|
|
2008-07-12 01:29:24 +00:00
|
|
|
telemetry* telemetry_new() {
|
2008-07-11 22:11:41 +00:00
|
|
|
telemetry *te = g_slice_new0(telemetry);
|
|
|
|
te->objects = g_array_new(FALSE, FALSE, sizeof(object));
|
|
|
|
return te;
|
|
|
|
}
|
|
|
|
|
2008-07-12 01:10:38 +00:00
|
|
|
void telemetry_print(telemetry *t){
|
|
|
|
fprintf(stderr,"Telemetry object:\n");
|
|
|
|
fprintf(stderr,"\tTimestamp: %d Number of Objects: %d Vehicle:\n",t->ts,t->objects->len) ;
|
|
|
|
vehicle_print(&t->vehicle);
|
|
|
|
}
|
|
|
|
|
2008-07-13 16:42:05 +00:00
|
|
|
guint object_hash(gconstpointer _o) {
|
|
|
|
object *o = (object*) _o;
|
|
|
|
const gchar *p = (const gchar*) (&o->x);
|
|
|
|
gsize n = 2 * sizeof(double);
|
|
|
|
guint h = 0;
|
|
|
|
while (n--) { h = (h << 5) - h + *p; p++; }
|
|
|
|
return h;
|
|
|
|
}
|
|
|
|
|
|
|
|
gboolean object_equal(gconstpointer _a, gconstpointer _b) {
|
|
|
|
object *a = (object*) _a, *b = (object*) _b;
|
|
|
|
return (a->type == b->type && a->x == b->x && a->y == b->y && a->rad == b->rad);
|
|
|
|
}
|
|
|
|
|
|
|
|
object* object_new(object_t type, double x, double y, double rad, double dir, double speed) {
|
|
|
|
object *o = g_slice_new(object);
|
|
|
|
o->type = type; o->x = x; o->y = y; o->rad = rad; o->dir = dir; o->speed = speed;
|
|
|
|
return o;
|
|
|
|
}
|
|
|
|
|
|
|
|
void _object_free(gpointer p) {
|
|
|
|
g_slice_free(object, (object*) p);
|
|
|
|
}
|
|
|
|
|
2008-07-11 22:55:36 +00:00
|
|
|
trial *trial_new(const char *hostname, const char *port) {
|
|
|
|
trial *t;
|
|
|
|
int sock;
|
|
|
|
|
|
|
|
if (-1 == (sock = opensocket(hostname, port))) return NULL;
|
|
|
|
|
|
|
|
t = g_slice_new0(trial);
|
|
|
|
t->socket = sock;
|
2008-07-11 22:20:05 +00:00
|
|
|
g_queue_init(&t->telemetry);
|
2008-07-13 16:42:05 +00:00
|
|
|
t->map.solid_objects = g_hash_table_new_full(object_hash, object_equal, NULL, _object_free);
|
2008-07-13 13:19:43 +00:00
|
|
|
t->sim.tm.objects = g_array_new(FALSE, FALSE, sizeof(object));
|
2008-07-12 00:39:26 +00:00
|
|
|
control_parser_new(t);
|
2008-07-12 11:59:35 +00:00
|
|
|
t->runcnt = 0;
|
2008-07-11 22:11:41 +00:00
|
|
|
return t;
|
|
|
|
}
|
|
|
|
|
|
|
|
void trial_reset_run(trial *t) {
|
2008-07-11 22:20:05 +00:00
|
|
|
g_queue_foreach(&t->telemetry, telemetry_free, NULL);
|
|
|
|
g_queue_clear(&t->telemetry);
|
2008-07-11 22:11:41 +00:00
|
|
|
t->last_ts = 0;
|
2008-07-12 11:59:35 +00:00
|
|
|
t->runcnt++;
|
2008-07-12 00:39:26 +00:00
|
|
|
control_parser_reset(t);
|
2008-07-11 22:11:41 +00:00
|
|
|
}
|
|
|
|
|
2008-07-12 08:40:15 +00:00
|
|
|
int trial_wait_for_start(trial *t) {
|
|
|
|
while (!t->finished && !t->alive) {
|
|
|
|
if (-1 == trial_wait_for_input(t)) return -1;
|
2008-07-11 22:55:36 +00:00
|
|
|
}
|
2008-07-12 08:40:15 +00:00
|
|
|
return 0;
|
2008-07-11 22:11:41 +00:00
|
|
|
}
|
|
|
|
|
2008-07-12 00:17:29 +00:00
|
|
|
static char buffer[16*1024];
|
|
|
|
|
2008-07-12 08:40:15 +00:00
|
|
|
int trial_check_input(trial *t) {
|
2008-07-12 00:17:29 +00:00
|
|
|
ssize_t len;
|
|
|
|
struct pollfd p;
|
2008-07-12 08:40:15 +00:00
|
|
|
|
2008-07-12 00:17:29 +00:00
|
|
|
p.fd = t->socket;
|
|
|
|
p.events = POLLIN;
|
2008-07-13 21:12:52 +00:00
|
|
|
switch (poll(&p, 1, 1)) {
|
2008-07-12 00:17:29 +00:00
|
|
|
case -1:
|
|
|
|
switch (errno) {
|
|
|
|
case EAGAIN:
|
|
|
|
case EINTR:
|
2008-07-12 08:40:15 +00:00
|
|
|
return 0;
|
2008-07-12 00:17:29 +00:00
|
|
|
}
|
|
|
|
fprintf(stderr, "poll error: %s\n", strerror(errno));
|
2008-07-12 08:40:15 +00:00
|
|
|
return -1;
|
2008-07-12 00:17:29 +00:00
|
|
|
case 0:
|
2008-07-12 08:40:15 +00:00
|
|
|
return 0;
|
2008-07-12 00:17:29 +00:00
|
|
|
}
|
|
|
|
len = read(t->socket, buffer, sizeof(buffer));
|
|
|
|
if (len < 0) {
|
|
|
|
fprintf(stderr, "read error: %s\n", strerror(errno));
|
2008-07-12 08:40:15 +00:00
|
|
|
return -1;
|
|
|
|
} else if (len == 0) {
|
|
|
|
t->finished = 1;
|
|
|
|
if (t->alive) {
|
|
|
|
fprintf(stderr, "unexpected connection close\n");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
return 0;
|
2008-07-12 00:17:29 +00:00
|
|
|
}
|
|
|
|
g_string_append_len(t->parse_ctx->buffer, buffer, len);
|
2008-07-12 08:40:15 +00:00
|
|
|
return (RUN_ERROR != control_parse(t)) ? 0 : -1;
|
2008-07-11 22:11:41 +00:00
|
|
|
}
|
|
|
|
|
2008-07-12 08:40:15 +00:00
|
|
|
int trial_wait_for_input(trial *t) {
|
2008-07-12 10:12:18 +00:00
|
|
|
if (t->parse_ctx->buffer->len) return trial_check_input(t);
|
2008-07-12 00:17:29 +00:00
|
|
|
ssize_t len = read(t->socket, buffer, sizeof(buffer));
|
|
|
|
if (len < 0) {
|
2008-07-13 13:19:43 +00:00
|
|
|
if (errno == ECONNRESET) {
|
|
|
|
len = 0; /* treat as simply connection close */
|
|
|
|
} else {
|
|
|
|
fprintf(stderr, "read error: %s\n", strerror(errno));
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (len == 0) {
|
2008-07-12 08:40:15 +00:00
|
|
|
if (t->alive) {
|
|
|
|
fprintf(stderr, "unexpected connection close\n");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
t->finished = 1;
|
|
|
|
return 0;
|
2008-07-12 00:17:29 +00:00
|
|
|
}
|
|
|
|
g_string_append_len(t->parse_ctx->buffer, buffer, len);
|
2008-07-12 08:40:15 +00:00
|
|
|
return (RUN_ERROR != control_parse(t)) ? 0 : -1;
|
2008-07-11 22:11:41 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
void trial_free(trial *t) {
|
2008-07-11 22:55:36 +00:00
|
|
|
shutdown(t->socket, SHUT_RDWR);
|
|
|
|
close(t->socket);
|
2008-07-11 22:20:05 +00:00
|
|
|
g_queue_foreach(&t->telemetry, telemetry_free, NULL);
|
|
|
|
g_queue_clear(&t->telemetry);
|
2008-07-13 16:42:05 +00:00
|
|
|
g_hash_table_destroy(t->map.solid_objects);
|
2008-07-13 13:19:43 +00:00
|
|
|
g_array_free(t->sim.tm.objects, TRUE);
|
2008-07-12 00:39:26 +00:00
|
|
|
control_parser_free(t);
|
2008-07-11 22:20:05 +00:00
|
|
|
g_slice_free(trial, t);
|
2008-07-11 22:11:41 +00:00
|
|
|
}
|
2008-07-12 01:10:38 +00:00
|
|
|
|
|
|
|
|
|
|
|
void trial_print(trial *t){
|
|
|
|
fprintf(stderr,"Trial Alive: %d Last Timestep: %d Socket: %d\n",t->alive,t->last_ts,t->socket);
|
|
|
|
/*perhaps add map_print and vehicle_print*/
|
|
|
|
}
|
|
|
|
|
2008-07-12 09:32:41 +00:00
|
|
|
void telemetry_matlab(gpointer tele, gpointer file){
|
|
|
|
telemetry* t;
|
|
|
|
t = (telemetry*) tele;
|
2008-07-13 21:35:46 +00:00
|
|
|
vehicle_matlab((FILE*) file,t->ts,&t->vehicle);
|
|
|
|
}
|
2008-07-12 09:32:41 +00:00
|
|
|
|
2008-07-13 21:35:46 +00:00
|
|
|
void vehicle_matlab(FILE* out, timestamp ts, vehicle* v){
|
|
|
|
fprintf(out,"v(end+1,:) = [%d %f, %f, %f, %f];\n",ts,v->x,v->y,v->dir,v->speed);
|
2008-07-12 09:32:41 +00:00
|
|
|
}
|
2008-07-12 01:10:38 +00:00
|
|
|
|
2008-07-12 09:32:41 +00:00
|
|
|
/*prints coordinate history to matlab compatible file*/
|
|
|
|
void trial_matlab(trial *t){
|
|
|
|
FILE* outfile;
|
2008-07-12 11:59:35 +00:00
|
|
|
char name[80];
|
|
|
|
|
|
|
|
sprintf(name,"velocity%d.m",t->runcnt);
|
2008-07-12 09:32:41 +00:00
|
|
|
|
2008-07-12 11:59:35 +00:00
|
|
|
outfile = fopen(name,"w");
|
2008-07-12 09:32:41 +00:00
|
|
|
fprintf(outfile,"v = [];\n");
|
|
|
|
g_queue_foreach(&t->telemetry,telemetry_matlab,outfile);
|
|
|
|
fclose(outfile);
|
|
|
|
}
|
|
|
|
|
2008-07-12 01:10:38 +00:00
|
|
|
|
|
|
|
/* debugging stuff */
|
|
|
|
void object_print(object *o){
|
|
|
|
fprintf(stderr,"Object: Type: %d x: %f, y: %f, rad: %f dir: %f speed: %f\n",o->type,o->x,o->y,o->rad,o->dir,o->speed);
|
|
|
|
}
|
|
|
|
|
|
|
|
void vehicle_print(vehicle *v){
|
|
|
|
fprintf(stderr,"Vehicle: Accel: %d Turn: %d x: %f y: %f dir: %f speed: %f\n",v->accel,v->turn,v->x,v->y,v->dir,v->speed);
|
|
|
|
}
|
|
|
|
|
2008-07-13 13:19:43 +00:00
|
|
|
// void map_print(map *m){}
|