vmcd.c 20.8 KB
Newer Older
Timothy Stack's avatar
   
Timothy Stack committed
1

Timothy Stack's avatar
   
Timothy Stack committed
2
#include <math.h>
Timothy Stack's avatar
   
Timothy Stack committed
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
#include <stdio.h>
#include <errno.h>
#include <assert.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/time.h>
#include <netdb.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include "log.h"
#include "mtp.h"

static int debug = 0;

static int looping = 1;

static char *emc_hostname = NULL;
static int emc_port = 0;

static char *logfile = NULL;

static char *pidfile = NULL;

David Johnson's avatar
   
David Johnson committed
31
32
33
34
35
36
37
38
39
40
41
#define MAX_VMC_CLIENTS 10
#define MAX_TRACKED_OBJECTS 10

struct robot_track {
    int updated;
    int valid;
    int robot_id;
    struct position position;
    int request_id;
};

Timothy Stack's avatar
   
Timothy Stack committed
42
43
44
45
struct vmc_client {
    char *vc_hostname;
    int vc_port;
    int vc_fd;
David Johnson's avatar
   
David Johnson committed
46
47
    /* we store object positions from individual vmc-clients in this array */
    struct robot_track tracks[MAX_TRACKED_OBJECTS];
Timothy Stack's avatar
   
Timothy Stack committed
48
49
50
51
52
53
54
};

static unsigned int vmc_client_count = 0;
static struct vmc_client vmc_clients[MAX_VMC_CLIENTS];

static struct mtp_config_vmc *vmc_config = NULL;

David Johnson's avatar
   
David Johnson committed
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
/* for tracking */
static int next_request_id = 0;

int get_next_request_id() {
    return next_request_id++;
}

/* we store actual robot positions in this array */
/* these are made from updates to the individual vmc_clients' arrays */
static struct robot_track real_robots[MAX_TRACKED_OBJECTS];

void vmc_handle_update_id(struct mtp_packet *p);
void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p);
int find_real_robot_id(struct position *p);


/**
 * Here's how the algorithm will lay out: when we get update_position packets
 * from a vmc-client, we will attempt to find a preexisting track for the given
 * position in vc_objects[n].  If there's nothing within the threshold, we 
 * start a new track for this object, and send a query to emc to get its id 
 * (this requires a request_id, which we create, and a client fd) (we continue
 * to update this new track, of course).  When the request comes back, IF there
 * is still a track for this client fd and request id, we associate the 
 * returned id with the track.  
 * Otherwise, we search for the track whose posit
 * and pose deltas are least for the new object.  If there are tracks that 
 * are not updated for some time (a two timestamp difference), we remove them.
 * The two-timestamp business: since we're getting update_position packets
 * from the vmc-clients for EACH id, we don't process everything at once.  
 * Thus, we have to ensure that all update_position packets for a specific
 * timestamp have been handled before we even try deleting stale tracks.  If
 * we place a MTP_POSITION_STATUS_CYCLE_COMPLETE in the status field for the 
 * last position update in a 
 * mezzanine cycle, we know we've seen the last packet for this iteration,
 * and we can clean out stale tracks.
 *
 * Every time we get 5 updates from each client, we update the robots array;
 * essentially, for each robot id we're supposed to know about (passed in the
 * config struct), we scan all vc_objects subarrays for the best position 
 * estimate for that id (given by confidence value, which we will add later --
 * for now, we just take the first one).
 */

/* TODO:
 * 1. fix timestamp encoding issue (change in protocol from int to double)
 * 2. add above hooks below
 * 3. add some support to emc for more vision querying and updating
 *
 * 4. test!
 *
 * 5. merge mezzanine-usb with mezzanine-current
 *
 * 6. test!
 *
 */


Timothy Stack's avatar
   
Timothy Stack committed
113
114
115
116
117
static void sigquit(int signal)
{
    looping = 0;
}

Timothy Stack's avatar
   
Timothy Stack committed
118
119
120
121
122
123
124
125
126
127
static void sigpanic(int sig)
{
  char buf[32];

  sprintf(buf, "sigpanic %d\n", sig);
  write(1, buf, strlen(buf));
  sleep(120);
  exit(1);
}

Timothy Stack's avatar
   
Timothy Stack committed
128
static int mygethostbyname(struct sockaddr_in *host_addr,
David Johnson's avatar
   
David Johnson committed
129
130
                           char *host,
                           int port)
Timothy Stack's avatar
   
Timothy Stack committed
131
132
133
{
    struct hostent *host_ent;
    int retval = 0;
David Johnson's avatar
   
David Johnson committed
134
    
Timothy Stack's avatar
   
Timothy Stack committed
135
136
137
    assert(host_addr != NULL);
    assert(host != NULL);
    assert(strlen(host) > 0);
David Johnson's avatar
   
David Johnson committed
138
    
Timothy Stack's avatar
   
Timothy Stack committed
139
    memset(host_addr, 0, sizeof(struct sockaddr_in));
David Johnson's avatar
David Johnson committed
140
#ifndef linux
Timothy Stack's avatar
   
Timothy Stack committed
141
    host_addr->sin_len = sizeof(struct sockaddr_in);
David Johnson's avatar
David Johnson committed
142
#endif
Timothy Stack's avatar
   
Timothy Stack committed
143
144
145
    host_addr->sin_family = AF_INET;
    host_addr->sin_port = htons(port);
    if( (host_ent = gethostbyname(host)) != NULL ) {
David Johnson's avatar
   
David Johnson committed
146
147
148
149
        memcpy((char *)&host_addr->sin_addr.s_addr,
               host_ent->h_addr,
               host_ent->h_length);
        retval = 1;
Timothy Stack's avatar
   
Timothy Stack committed
150
151
    }
    else {
David Johnson's avatar
   
David Johnson committed
152
        retval = inet_aton(host, &host_addr->sin_addr);
Timothy Stack's avatar
   
Timothy Stack committed
153
154
155
156
157
158
159
    }
    return( retval );
}

static void usage(void)
{
    fprintf(stderr,
David Johnson's avatar
   
David Johnson committed
160
161
162
163
164
165
166
167
168
169
170
171
172
173
            "Usage: vmcd [-hd] [-l logfile] [-e emchost] [-p emcport]\n"
            "            [-c clienthost] [-P clientport]\n"
            "Vision master control daemon\n"
            "  -h\tPrint this message\n"
            "  -d\tTurn on debugging messages and do not daemonize\n"
            "  -l logfile\tSpecify the log file to use\n"
            "  -i pidfile\tSpecify the pid file name\n"
            "  -e emchost\tSpecify the host name where emc is running\n"
            "  -p emcport\tSpecify the port that emc is listening on\n"
            "  -c clienthost\tSpecify the host name where a vmc-client is\n"
            "  -P clientport\tSpecify the port the client is listening on\n"
            "\n"
            "Example:\n"
            "  $ vmcd -c foo -P 7070 -- -c foo -P 7071\n");
Timothy Stack's avatar
   
Timothy Stack committed
174
175
176
177
178
179
}

static int parse_client_options(int *argcp, char **argvp[])
{
    int c, argc, retval = EXIT_SUCCESS;
    char **argv;
David Johnson's avatar
   
David Johnson committed
180
    
Timothy Stack's avatar
   
Timothy Stack committed
181
182
    assert(argcp != NULL);
    assert(argvp != NULL);
David Johnson's avatar
   
David Johnson committed
183
    
Timothy Stack's avatar
   
Timothy Stack committed
184
185
186
187
    argc = *argcp;
    argv = *argvp;
    
    while ((c = getopt(argc, argv, "hdp:l:i:e:c:P:")) != -1) {
David Johnson's avatar
   
David Johnson committed
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
        switch (c) {
        case 'h':
            usage();
            exit(0);
            break;
        case 'd':
            debug += 1;
            break;
        case 'l':
            logfile = optarg;
            break;
        case 'i':
            pidfile = optarg;
            break;
        case 'e':
            emc_hostname = optarg;
            break;
        case 'p':
            if (sscanf(optarg, "%d", &emc_port) != 1) {
                error("-p option is not a number: %s\n", optarg);
                usage();
                exit(1);
            }
            break;
        case 'c':
            vmc_clients[vmc_client_count].vc_hostname = optarg;
            break;
        case 'P':
            if (sscanf(optarg,
                       "%d",
                       &vmc_clients[vmc_client_count].vc_port) != 1) {
                error("-P option is not a number: %s\n", optarg);
                usage();
                exit(1);
            }
            break;
        default:
            break;
        }
Timothy Stack's avatar
   
Timothy Stack committed
227
    }
David Johnson's avatar
   
David Johnson committed
228
    
Timothy Stack's avatar
   
Timothy Stack committed
229
    if (vmc_clients[vmc_client_count].vc_port != 0) {
David Johnson's avatar
   
David Johnson committed
230
        vmc_client_count += 1;
Timothy Stack's avatar
   
Timothy Stack committed
231
    }
David Johnson's avatar
   
David Johnson committed
232
    
Timothy Stack's avatar
   
Timothy Stack committed
233
234
    argc -= optind;
    argv += optind;
David Johnson's avatar
   
David Johnson committed
235
    
Timothy Stack's avatar
   
Timothy Stack committed
236
237
    *argcp = argc + 1;
    *argvp = argv - 1;
David Johnson's avatar
   
David Johnson committed
238
    
David Johnson's avatar
David Johnson committed
239
#ifndef linux
Timothy Stack's avatar
   
Timothy Stack committed
240
    optreset = 1;
David Johnson's avatar
David Johnson committed
241
#endif
Timothy Stack's avatar
   
Timothy Stack committed
242
    optind = 1;
David Johnson's avatar
   
David Johnson committed
243
    
Timothy Stack's avatar
   
Timothy Stack committed
244
245
246
    return retval;
}

David Johnson's avatar
   
David Johnson committed
247
248
249
/* promoted to global so that handlers can access it */
int emc_fd;

Timothy Stack's avatar
   
Timothy Stack committed
250
251
int main(int argc, char *argv[])
{
David Johnson's avatar
   
David Johnson committed
252
    int lpc, i , retval = EXIT_SUCCESS;
Timothy Stack's avatar
   
Timothy Stack committed
253
254
255
    struct sockaddr_in sin;
    fd_set readfds;

David Johnson's avatar
   
David Johnson committed
256
257
    emc_fd = 0;
    
Timothy Stack's avatar
   
Timothy Stack committed
258
259
260
    FD_ZERO(&readfds);
    
    do {
David Johnson's avatar
   
David Johnson committed
261
        retval = parse_client_options(&argc, &argv);
Timothy Stack's avatar
   
Timothy Stack committed
262
    } while ((argc > 0) && strcmp(argv[0], "--") == 0);
David Johnson's avatar
   
David Johnson committed
263
    
Timothy Stack's avatar
   
Timothy Stack committed
264
    if (debug) {
David Johnson's avatar
   
David Johnson committed
265
        loginit(0, logfile);
Timothy Stack's avatar
   
Timothy Stack committed
266
267
    }
    else {
David Johnson's avatar
   
David Johnson committed
268
269
270
271
272
273
274
        /* Become a daemon */
        daemon(0, 0);
        
        if (logfile)
            loginit(0, logfile);
        else
            loginit(1, "vmcclient");
Timothy Stack's avatar
   
Timothy Stack committed
275
    }
David Johnson's avatar
   
David Johnson committed
276
    
Timothy Stack's avatar
   
Timothy Stack committed
277
    if (pidfile) {
David Johnson's avatar
   
David Johnson committed
278
279
280
281
282
283
        FILE *fp;
        
        if ((fp = fopen(pidfile, "w")) != NULL) {
            fprintf(fp, "%d\n", getpid());
            (void) fclose(fp);
        }
Timothy Stack's avatar
   
Timothy Stack committed
284
    }
David Johnson's avatar
   
David Johnson committed
285
    
Timothy Stack's avatar
   
Timothy Stack committed
286
287
288
    signal(SIGQUIT, sigquit);
    signal(SIGTERM, sigquit);
    signal(SIGINT, sigquit);
David Johnson's avatar
   
David Johnson committed
289
    
Timothy Stack's avatar
   
Timothy Stack committed
290
291
292
    signal(SIGSEGV, sigpanic);
    signal(SIGBUS, sigpanic);
    
David Johnson's avatar
   
David Johnson committed
293
294
295
296
    /* connect to emc and send init packet */
    /* in future, vmc will be a separate identity that emc can connect to
     * as necessary; vmc will not connect to emc.
     */
Timothy Stack's avatar
   
Timothy Stack committed
297
    if (emc_hostname != NULL) {
David Johnson's avatar
   
David Johnson committed
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
        struct mtp_packet *mp = NULL, *rmp = NULL;
        struct mtp_control mc;
        
        mc.id = -1;
        mc.code = -1;
        mc.msg = "vmcd init";
        if (mygethostbyname(&sin, emc_hostname, emc_port) == 0) {
            pfatal("bad emc hostname: %s\n", emc_hostname);
        }
        else if ((emc_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
            pfatal("socket");
        }
        else if (connect(emc_fd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
            pfatal("connect");
        }
        else if ((mp = mtp_make_packet(MTP_CONTROL_INIT,
                                       MTP_ROLE_VMC,
                                       &mc)) == NULL) {
            pfatal("mtp_make_packet");
        }
        else if (mtp_send_packet(emc_fd, mp) != MTP_PP_SUCCESS) {
            pfatal("could not configure with emc");
        }
        else if (mtp_receive_packet(emc_fd, &rmp) != MTP_PP_SUCCESS) {
            pfatal("could not get vmc config packet");
        }
        else {
            FD_SET(emc_fd, &readfds);
            vmc_config = rmp->data.config_vmc;
            
            for (lpc = 0; lpc < vmc_config->num_robots; lpc++) {
                info(" robot[%d] = %d %s\n",
                     lpc,
                     vmc_config->robots[lpc].id,
                     vmc_config->robots[lpc].hostname);
            }
        }
        
        free(mp);
        mp = NULL;
    }

    /* setup real_robots */
    for (i = 0; i < MAX_TRACKED_OBJECTS; ++i) {
        real_robots[i].valid = 0;
Timothy Stack's avatar
   
Timothy Stack committed
343
344
    }
    
David Johnson's avatar
   
David Johnson committed
345
    /* connect to all specified clients */
Timothy Stack's avatar
   
Timothy Stack committed
346
    for (lpc = 0; lpc < vmc_client_count; lpc++) {
David Johnson's avatar
   
David Johnson committed
347
348
349
350
351
352
353
354
355
356
357
358
359
360
        struct vmc_client *vc = &vmc_clients[lpc];
        
        if (mygethostbyname(&sin, vc->vc_hostname, vc->vc_port) == 0) {
            pfatal("bad vmc-client hostname: %s\n", vc->vc_hostname);
        }
        else if ((vc->vc_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
            pfatal("socket");
        }
        else if (connect(vc->vc_fd,
                         (struct sockaddr *)&sin,
                         sizeof(sin)) == -1) {
            pfatal("connect");
        }
        else {
Timothy Stack's avatar
   
Timothy Stack committed
361
362
            int i;
	    
David Johnson's avatar
   
David Johnson committed
363
364
365
366
367
368
369
370
371
372
            FD_SET(vc->vc_fd, &readfds);

            // we now init the robot_track list for this struct:
            for (i = 0; i < MAX_TRACKED_OBJECTS; ++i) {
                vc->tracks[i].updated = 0;
                vc->tracks[i].valid = 0;
                vc->tracks[i].robot_id = -1;
                vc->tracks[i].request_id = -1;
            }
        }
Timothy Stack's avatar
   
Timothy Stack committed
373
    }
David Johnson's avatar
   
David Johnson committed
374
375
    
    /* handle input from emc and vmc */
Timothy Stack's avatar
   
Timothy Stack committed
376
    while (looping) {
David Johnson's avatar
   
David Johnson committed
377
378
379
380
381
382
383
384
385
386
387
388
389
390
        fd_set rreadyfds;
        int rc;
        
        rreadyfds = readfds;
        rc = select(FD_SETSIZE, &rreadyfds, NULL, NULL, NULL);
        if (rc > 0) {
            if ((emc_fd != 0) && FD_ISSET(emc_fd, &rreadyfds)) {
                struct mtp_packet *mp = NULL;
                
                if ((rc = mtp_receive_packet(emc_fd, &mp)) != MTP_PP_SUCCESS) {
                    fatal("bad packet from emc %d\n", rc);
                }
                else {
                    /* XXX Need to handle update_id msgs here. */
Timothy Stack's avatar
   
Timothy Stack committed
391
                    mtp_print_packet(stdout, mp);
David Johnson's avatar
   
David Johnson committed
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409

                    vmc_handle_update_id(mp);
                }
                
                mtp_free_packet(mp);
                mp = NULL;
            }
            
            for (lpc = 0; lpc < vmc_client_count; lpc++) {
                struct vmc_client *vc = &vmc_clients[lpc];
                
                if (FD_ISSET(vc->vc_fd, &rreadyfds)) {
                    struct mtp_packet *mp = NULL;
                    
                    if (mtp_receive_packet(vc->vc_fd, &mp) != MTP_PP_SUCCESS) {
                        fatal("bad packet from vmc-client\n");
                    }
                    else {
Timothy Stack's avatar
   
Timothy Stack committed
410
411
412
                        mtp_print_packet(stdout, mp);
			fflush(stdout);
			
David Johnson's avatar
   
David Johnson committed
413
414
415
416
417
418
419
420
421
                        vmc_handle_update_position(vc,mp);

                    }
                    
                    mtp_free_packet(mp);
                    mp = NULL;
                }
            }
        }
Timothy Stack's avatar
   
Timothy Stack committed
422
        else if (rc == -1 && errno != EINTR) {
David Johnson's avatar
   
David Johnson committed
423
424
            perror("select");
        }
Timothy Stack's avatar
   
Timothy Stack committed
425
426
427
428
    }
    
    return retval;
}
David Johnson's avatar
   
David Johnson committed
429
430
431
432
433
434
435
436
437

void vmc_handle_update_id(struct mtp_packet *p) {
    int i,j,k;
    if (p == NULL) {
        return;
    }
    // we have to search all of the vmc_clients' tracks arrays for this
    // request_id, IF the returned robot_id isn't -1
    if (p->data.update_id->robot_id == -1) {
Timothy Stack's avatar
   
Timothy Stack committed
438
      error("no robot id?\n");
David Johnson's avatar
   
David Johnson committed
439
440
441
        return;
    }

Timothy Stack's avatar
   
Timothy Stack committed
442
443
    info("update id %d\n", p->data.update_id->request_id);

David Johnson's avatar
   
David Johnson committed
444
445
    // now check through all the possible tracks:
    for (i = 0; i < MAX_VMC_CLIENTS; ++i) {
Timothy Stack's avatar
   
Timothy Stack committed
446
        if (vmc_clients[i].vc_fd > -1) {
David Johnson's avatar
   
David Johnson committed
447
448
449
450
            struct vmc_client *vc = &(vmc_clients[i]);
            for (j = 0; j < MAX_TRACKED_OBJECTS; ++j) {
                if (vc->tracks[j].valid &&
                    vc->tracks[j].request_id == p->data.update_id->request_id) {
Timothy Stack's avatar
   
Timothy Stack committed
451
452
                    int first_invalid_idx = -1;
		    
David Johnson's avatar
   
David Johnson committed
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
                    vc->tracks[j].robot_id = p->data.update_id->robot_id;

                    // before we return, we should also try to update
                    // real_robots, and make a new track if one isn't there
                    // for this robot
                    // in truth, there should not be a real_robot matching
                    // this id in the list, or we would've found it... but
                    // not everything happens in perfect sync, so we have to
                    // check for this possibility
                    for (k = 0; k < MAX_TRACKED_OBJECTS; ++k) {
                        if (real_robots[k].valid == 1) {
                            if (real_robots[k].robot_id == vc->tracks[j].robot_id) {
                                // update the posit and return.
                                real_robots[k].position = vc->tracks[j].position;
                                return;
                            }
                        }
                        else if (first_invalid_idx == -1) {
                            first_invalid_idx = k;
                        }
                    }

                    // if we get here, we have to add a new track to the 
                    // real_robots list:
                    real_robots[first_invalid_idx].valid = 1;
                    real_robots[first_invalid_idx].robot_id = vc->tracks[j].robot_id;
                    real_robots[first_invalid_idx].position = vc->tracks[j].position;

                    return;
                }
            }
        }
    }

}
/* we allow a track to be continued if there is a match within 5 cm */
#define DIST_THRESHOLD 0.05
/* and if pose difference is less than 45 degress */
#define POSE_THRESHOLD 0.785

void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
    int i,j;

    // first we run through the current track list for this vmc_client:
    float best_dist_delta = DIST_THRESHOLD;
    float best_pose_delta = POSE_THRESHOLD;
    int best_track_idx = -1;
    int first_invalid_track = -1;

Timothy Stack's avatar
   
Timothy Stack committed
502
503
504
505
    if (p == NULL || vc == NULL) {
        return;
    }

David Johnson's avatar
   
David Johnson committed
506
507
508
509
510
511
    for (i = 0; i < MAX_TRACKED_OBJECTS; ++i) {
        if (vc->tracks[i].valid) {
            float dx,dy;
            float my_dist_delta;
            float my_pose_delta;

Timothy Stack's avatar
   
Timothy Stack committed
512
513
514
515
            dx = fabsf(p->data.update_position->position.x -
		       vc->tracks[i].position.x);
            dy = fabsf(p->data.update_position->position.y -
		       vc->tracks[i].position.y);
David Johnson's avatar
   
David Johnson committed
516
517
518
519
            my_dist_delta = sqrt(dx*dx + dy*dy);
            my_pose_delta = p->data.update_position->position.theta - 
                vc->tracks[i].position.theta;

Timothy Stack's avatar
   
Timothy Stack committed
520
521
            if (fabsf(my_dist_delta) > DIST_THRESHOLD || 
                fabsf(my_dist_delta) > best_dist_delta) {
David Johnson's avatar
   
David Johnson committed
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
                continue;
            }
            if (fabsf(my_pose_delta) > POSE_THRESHOLD ||
                fabsf(my_pose_delta) > best_pose_delta) {
                continue;
            }

            // if we've gotten here, we've beat the current best!
            best_dist_delta = my_dist_delta;
            best_pose_delta = my_pose_delta;
            best_track_idx = i;
        }
        else if (first_invalid_track == -1) {
            first_invalid_track = i;
        }
Timothy Stack's avatar
   
Timothy Stack committed
537
	
David Johnson's avatar
   
David Johnson committed
538
539
540
541
542
    }

    // now check to see if we found a match.
    // if we did, update the track:
    if (best_track_idx > -1) {
Timothy Stack's avatar
   
Timothy Stack committed
543
544
545
546
	info("got a match\n");
	
        vc->tracks[best_track_idx].position =
	    p->data.update_position->position;
David Johnson's avatar
   
David Johnson committed
547
548
549
550
        // now we try to update real_robots
        if (vc->tracks[best_track_idx].robot_id != -1) {
            // we want to update the real_robots list!
            for (j = 0; j < MAX_TRACKED_OBJECTS; ++j) {
Timothy Stack's avatar
   
Timothy Stack committed
551
552
553
554
                if (real_robots[j].robot_id ==
		    vc->tracks[best_track_idx].robot_id) {
                    real_robots[j].position =
			vc->tracks[best_track_idx].position;
David Johnson's avatar
   
David Johnson committed
555
556
557
558
559
560
561
562
563
564
565
566
567
                }
            }
        }

        vc->tracks[best_track_idx].updated = 1;
    }
    else {
        // need to start a new track and possibly issue a request_id request:
        if (first_invalid_track == -1) {
            // no more room to store tracks!
            printf("OUT OF TRACKING ROOM IN VMCD!\n");
        }
        else {
Timothy Stack's avatar
   
Timothy Stack committed
568
	    int retval;
Timothy Stack's avatar
   
Timothy Stack committed
569
	    
David Johnson's avatar
   
David Johnson committed
570
            vc->tracks[first_invalid_track].valid = 1;
Timothy Stack's avatar
   
Timothy Stack committed
571
572
            vc->tracks[first_invalid_track].position =
		p->data.update_position->position;
David Johnson's avatar
   
David Johnson committed
573
574
575
576
577
578
            vc->tracks[first_invalid_track].robot_id = -1;
            vc->tracks[first_invalid_track].request_id = -1;

            // now, if we can find a match for this track close enough to
            // a track in the real_robots list, we can steal the robot_id from
            // there!
David Johnson's avatar
David Johnson committed
579
            retval = find_real_robot_id(&(vc->tracks[i].position));
David Johnson's avatar
   
David Johnson committed
580
581
582
            if (retval == -1) {
                // we have to send our request_id packet, now:
                struct mtp_request_id rid;
David Johnson's avatar
David Johnson committed
583
                struct mtp_packet *mp;
Timothy Stack's avatar
   
Timothy Stack committed
584
585
		
                rid.position = vc->tracks[first_invalid_track].position;
David Johnson's avatar
   
David Johnson committed
586
                rid.request_id = get_next_request_id();
Timothy Stack's avatar
   
Timothy Stack committed
587
588
                mp = mtp_make_packet(MTP_REQUEST_ID, MTP_ROLE_VMC, &rid);
                retval = mtp_send_packet(emc_fd,mp);
David Johnson's avatar
   
David Johnson committed
589
                if (retval == MTP_PP_SUCCESS) {
Timothy Stack's avatar
   
Timothy Stack committed
590
591
592
		    info("sent request %d\n", rid.request_id);
                    vc->tracks[first_invalid_track].request_id =
			rid.request_id;
David Johnson's avatar
   
David Johnson committed
593
594
595
596
597
598
599
                }
                else {
                    vc->tracks[first_invalid_track].valid = 0;
                }
            }
            else {
                // we can update the real_robots list!
Timothy Stack's avatar
   
Timothy Stack committed
600
601
602
603
                real_robots[retval].position =
		    p->data.update_position->position;
                vc->tracks[first_invalid_track].robot_id =
		    real_robots[retval].robot_id;
David Johnson's avatar
   
David Johnson committed
604
605
606
607
608
609
610
611
612
613
            }

            vc->tracks[first_invalid_track].updated = 1;

        }
    }

    // now, if the update_position.status field is MTP_POSITION_STATUS_CYCLE_
    // COMPLETE, we want to go though and weed out any valid tracks that
    // were not updated:
Timothy Stack's avatar
   
Timothy Stack committed
614
    if (p->data.update_position->status == MTP_POSITION_STATUS_CYCLE_COMPLETE) {
Timothy Stack's avatar
   
Timothy Stack committed
615
616
617
618
	struct mtp_update_position mup;
	struct mtp_packet *mp;

	mp = mtp_make_packet(MTP_UPDATE_POSITION, MTP_ROLE_VMC, &mup);
David Johnson's avatar
   
David Johnson committed
619
620
621
622
        for (i = 0; i < MAX_TRACKED_OBJECTS; ++i) {
            if (vc->tracks[i].valid && !(vc->tracks[i].updated)) {
                vc->tracks[i].valid = -1;
            }
Timothy Stack's avatar
   
Timothy Stack committed
623
	    else if (vc->tracks[i].valid && vc->tracks[i].robot_id != -1) {
Timothy Stack's avatar
   
Timothy Stack committed
624
625
626
627
628
		info("sending update for %d -> %f %f\n",
		     vc->tracks[i].robot_id,
		     vc->tracks[i].position.x,
		     vc->tracks[i].position.y);
		
Timothy Stack's avatar
   
Timothy Stack committed
629
630
631
632
633
		mup.robot_id = vc->tracks[i].robot_id;
		mup.position = vc->tracks[i].position;
		mup.status = MTP_POSITION_STATUS_UNKNOWN;
		mtp_send_packet(emc_fd, mp);
	    }
David Johnson's avatar
   
David Johnson committed
634
        }
Timothy Stack's avatar
   
Timothy Stack committed
635
636
	mtp_free_packet(mp);
	mp = NULL;
David Johnson's avatar
   
David Johnson committed
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
    }

    // finis!
                                                        
            
}

int find_real_robot_id(struct position *p) {
    int i;

    // we look through real_robots:
    float best_dist_delta = DIST_THRESHOLD;
    float best_pose_delta = POSE_THRESHOLD;
    int best_track_idx = -1;

Timothy Stack's avatar
   
Timothy Stack committed
652
653
654
655
    if (p == NULL) {
        return -1;
    }

David Johnson's avatar
   
David Johnson committed
656
657
658
659
660
661
    for (i = 0; i < MAX_TRACKED_OBJECTS; ++i) {
        if (real_robots[i].valid) {
            float dx,dy;
            float my_dist_delta;
            float my_pose_delta;

David Johnson's avatar
David Johnson committed
662
            dx = p->x - real_robots[i].position.x;
David Johnson's avatar
David Johnson committed
663
            dy = p->y - real_robots[i].position.y;
David Johnson's avatar
   
David Johnson committed
664
            my_dist_delta = sqrt(dx*dx + dy*dy);
David Johnson's avatar
David Johnson committed
665
            my_pose_delta = p->theta - real_robots[i].position.theta;
David Johnson's avatar
   
David Johnson committed
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686

            if (fabsf(my_dist_delta) > DIST_THRESHOLD || 
                fabsf(my_dist_delta) > best_dist_delta) {
                continue;
            }
            if (fabsf(my_pose_delta) > POSE_THRESHOLD ||
                fabsf(my_pose_delta) > best_pose_delta) {
                continue;
            }

            // if we've gotten here, we've beat the current best!
            best_dist_delta = my_dist_delta;
            best_pose_delta = my_pose_delta;
            best_track_idx = i;
        }   
    }

    return best_track_idx;
}