diff options
Diffstat (limited to 'src/main.cpp')
| -rw-r--r-- | src/main.cpp | 21 |
1 files changed, 19 insertions, 2 deletions
diff --git a/src/main.cpp b/src/main.cpp index cde65f5..af602c5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -43,8 +43,21 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) } void -imgCallback(const message &msg) +imgCallback(message &msg) { + int id,sx,sy,rx,ry; + strcat(msg.image_names[0],".txt"); + FILE *fin; + if ((fin=fopen(msg.image_names[0], "r"))==NULL) { + err_sys("fopen: %s", msg.image_names[0]); + } + while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) { + printf("id: %d source: (%d, %d) reflection: (%d, %d)\n", id, sx, sy, rx, ry); + } + if (fclose(fin)==EOF) { + err_sys("fclose"); + } + return; } @@ -100,7 +113,6 @@ utmCallback(const message &msg, State &mu) } return; } -#endif /* ----- not USE_ROS ----- */ int parseLine(char *line, message *data) @@ -192,6 +204,7 @@ parseLine(char *line, message *data) exit(1); } } +#endif /* ----- not USE_ROS ----- */ timestamp update_dt(const timestamp t, timestamp *t_old) @@ -215,6 +228,10 @@ update_dt(const timestamp t, timestamp *t_old) int main(int argc, char **argv) { + if (argc!=2) { + fprintf(stderr, "Usage: %s camera\n", argv[0]); + exit(1); + } State mu; Quaterniond qbw; // bias in FRD coordinates |