You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

245 lines
5.9 KiB

  1. #include "control.h"
  2. #include <sys/socket.h>
  3. #include <netdb.h>
  4. #include <stdio.h>
  5. #include <errno.h>
  6. #include <string.h>
  7. #include <stdlib.h>
  8. #include <netinet/in.h>
  9. #include <netinet/tcp.h>
  10. #include <poll.h>
  11. static int opensocket(const char *hostname, const char *port) {
  12. struct addrinfo *res = NULL, pref;
  13. int ret, s, val;
  14. pref.ai_flags = 0;
  15. pref.ai_family = PF_UNSPEC;
  16. pref.ai_socktype = SOCK_STREAM;
  17. pref.ai_protocol = 0;
  18. pref.ai_addrlen = 0;
  19. pref.ai_addr = NULL;
  20. pref.ai_canonname = NULL;
  21. pref.ai_next = NULL;
  22. if (0 != (ret = getaddrinfo(hostname, port, &pref, &res)) || !res) {
  23. fprintf(stderr, "Couldn't find host/port '%s:%s': %s\n", hostname, port, strerror(errno));
  24. return -1;
  25. }
  26. if (-1 == (s = socket(res->ai_family, res->ai_socktype, res->ai_protocol))) {
  27. fprintf(stderr, "Couldn't create socket: %s\n", strerror(errno));
  28. freeaddrinfo(res);
  29. return -1;
  30. }
  31. if (0 != connect(s, res->ai_addr, res->ai_addrlen)) {
  32. fprintf(stderr, "Couldn't connect socket to '%s:%s': %s\n", hostname, port, strerror(errno));
  33. freeaddrinfo(res);
  34. close(s);
  35. return -1;
  36. }
  37. val = 1;
  38. if (0 != setsockopt(s, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val))) {
  39. fprintf(stderr, "Couldn't set socket option TCP_NODELAY: %s\n", strerror(errno));
  40. freeaddrinfo(res);
  41. close(s);
  42. return -1;
  43. }
  44. freeaddrinfo(res);
  45. return s;
  46. }
  47. static void telemetry_free(gpointer _te, gpointer x) {
  48. telemetry *te = (telemetry*) _te;
  49. UNUSED(x);
  50. g_array_free(te->objects, TRUE);
  51. g_slice_free(telemetry, te);
  52. }
  53. telemetry* telemetry_new() {
  54. telemetry *te = g_slice_new0(telemetry);
  55. te->objects = g_array_new(FALSE, FALSE, sizeof(object));
  56. return te;
  57. }
  58. void telemetry_print(telemetry *t){
  59. fprintf(stderr,"Telemetry object:\n");
  60. fprintf(stderr,"\tTimestamp: %d Number of Objects: %d Vehicle:\n",t->ts,t->objects->len) ;
  61. vehicle_print(&t->vehicle);
  62. }
  63. guint object_hash(gconstpointer _o) {
  64. object *o = (object*) _o;
  65. const gchar *p = (const gchar*) (&o->x);
  66. gsize n = 2 * sizeof(double);
  67. guint h = 0;
  68. while (n--) { h = (h << 5) - h + *p; p++; }
  69. return h;
  70. }
  71. gboolean object_equal(gconstpointer _a, gconstpointer _b) {
  72. object *a = (object*) _a, *b = (object*) _b;
  73. return (a->type == b->type && a->x == b->x && a->y == b->y && a->rad == b->rad);
  74. }
  75. object* object_new(object_t type, double x, double y, double rad, double dir, double speed) {
  76. object *o = g_slice_new(object);
  77. o->type = type; o->x = x; o->y = y; o->rad = rad; o->dir = dir; o->speed = speed;
  78. return o;
  79. }
  80. void _object_free(gpointer p) {
  81. g_slice_free(object, (object*) p);
  82. }
  83. trial *trial_new(const char *hostname, const char *port) {
  84. trial *t;
  85. int sock;
  86. if (-1 == (sock = opensocket(hostname, port))) return NULL;
  87. t = g_slice_new0(trial);
  88. t->socket = sock;
  89. g_queue_init(&t->telemetry);
  90. t->map.solid_objects = g_hash_table_new_full(object_hash, object_equal, NULL, _object_free);
  91. t->sim.tm.objects = g_array_new(FALSE, FALSE, sizeof(object));
  92. control_parser_new(t);
  93. t->runcnt = 0;
  94. return t;
  95. }
  96. void trial_reset_run(trial *t) {
  97. g_queue_foreach(&t->telemetry, telemetry_free, NULL);
  98. g_queue_clear(&t->telemetry);
  99. t->last_ts = 0;
  100. t->runcnt++;
  101. control_parser_reset(t);
  102. }
  103. int trial_wait_for_start(trial *t) {
  104. while (!t->finished && !t->alive) {
  105. if (-1 == trial_wait_for_input(t)) return -1;
  106. }
  107. return 0;
  108. }
  109. static char buffer[16*1024];
  110. int trial_check_input(trial *t) {
  111. ssize_t len;
  112. struct pollfd p;
  113. p.fd = t->socket;
  114. p.events = POLLIN;
  115. switch (poll(&p, 1, 0)) {
  116. case -1:
  117. switch (errno) {
  118. case EAGAIN:
  119. case EINTR:
  120. return 0;
  121. }
  122. fprintf(stderr, "poll error: %s\n", strerror(errno));
  123. return -1;
  124. case 0:
  125. return 0;
  126. }
  127. len = read(t->socket, buffer, sizeof(buffer));
  128. if (len < 0) {
  129. fprintf(stderr, "read error: %s\n", strerror(errno));
  130. return -1;
  131. } else if (len == 0) {
  132. t->finished = 1;
  133. if (t->alive) {
  134. fprintf(stderr, "unexpected connection close\n");
  135. return -1;
  136. }
  137. return 0;
  138. }
  139. g_string_append_len(t->parse_ctx->buffer, buffer, len);
  140. return (RUN_ERROR != control_parse(t)) ? 0 : -1;
  141. }
  142. int trial_wait_for_input(trial *t) {
  143. if (t->parse_ctx->buffer->len) return trial_check_input(t);
  144. ssize_t len = read(t->socket, buffer, sizeof(buffer));
  145. if (len < 0) {
  146. if (errno == ECONNRESET) {
  147. len = 0; /* treat as simply connection close */
  148. } else {
  149. fprintf(stderr, "read error: %s\n", strerror(errno));
  150. return -1;
  151. }
  152. }
  153. if (len == 0) {
  154. if (t->alive) {
  155. fprintf(stderr, "unexpected connection close\n");
  156. return -1;
  157. }
  158. t->finished = 1;
  159. return 0;
  160. }
  161. g_string_append_len(t->parse_ctx->buffer, buffer, len);
  162. return (RUN_ERROR != control_parse(t)) ? 0 : -1;
  163. }
  164. void trial_free(trial *t) {
  165. shutdown(t->socket, SHUT_RDWR);
  166. close(t->socket);
  167. g_queue_foreach(&t->telemetry, telemetry_free, NULL);
  168. g_queue_clear(&t->telemetry);
  169. g_hash_table_destroy(t->map.solid_objects);
  170. g_array_free(t->sim.tm.objects, TRUE);
  171. control_parser_free(t);
  172. g_slice_free(trial, t);
  173. }
  174. void trial_print(trial *t){
  175. fprintf(stderr,"Trial Alive: %d Last Timestep: %d Socket: %d\n",t->alive,t->last_ts,t->socket);
  176. /*perhaps add map_print and vehicle_print*/
  177. }
  178. void telemetry_matlab(gpointer tele, gpointer file){
  179. FILE* outfile;
  180. telemetry* t;
  181. vehicle * v;
  182. outfile = (FILE*) file;
  183. t = (telemetry*) tele;
  184. v = &t->vehicle;
  185. fprintf(outfile,"v(end+1,:) = [%d %f, %f, %f, %f];\n",t->ts,v->x,v->y,v->dir,v->speed);
  186. }
  187. /*prints coordinate history to matlab compatible file*/
  188. void trial_matlab(trial *t){
  189. FILE* outfile;
  190. char name[80];
  191. sprintf(name,"velocity%d.m",t->runcnt);
  192. outfile = fopen(name,"w");
  193. fprintf(outfile,"v = [];\n");
  194. g_queue_foreach(&t->telemetry,telemetry_matlab,outfile);
  195. fclose(outfile);
  196. }
  197. /* debugging stuff */
  198. void object_print(object *o){
  199. 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);
  200. }
  201. void vehicle_print(vehicle *v){
  202. 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);
  203. }
  204. // void map_print(map *m){}