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
|
/*
* =====================================================================================
*
* Filename: main.cpp
*
* Description: Main code for reflections SLAM. While the code is primarily
* designed to run in ROS, an attempt is being made to allow some
* functionality without it. However, no guarantees are made that the two
* will produce the same outputs for a given input.
*
* Version: 1.0
* Created: 03/17/2017 04:12:33 PM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include "main.h"
int
parseLine(char *line, message *data)
{
char *time_string;
char *msg_type;
double t;
int s,ns;
time_string = strsep(&line, ",");
msg_type = strsep(&line, ",");
sscanf(time_string,"%lf", &t);
s = (int) t;
ns = (int) ((t-s)*1e9);
data->stamp.secs=s;
data->stamp.nsecs=ns;
if (!strcmp(msg_type,"bestutm")) {
data->msg_type = BESTUTM;
sscanf(line,"%d,%c,%lf,%lf,%lf",
&data->utm.zone_i,
&data->utm.zone_c,
&data->utm.northing,
&data->utm.easting,
&data->utm.up);
} else if (!strcmp(msg_type,"IMG")) {
data->msg_type = IMG;
char *lfn,*rfn;
lfn = strsep(&line, ",");
sscanf(lfn, "%s", &data->image_names);
rfn = strsep(&line, ",");
sscanf(rfn, "%s", &data->image_names+1);
} else if (!strcmp(msg_type,"inscovs")) {
data->msg_type = INSCOVS;
} else if (!strcmp(msg_type,"inspvas")) {
data->msg_type = INSPVAS;
sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
&data->position.latitude,
&data->position.longitude,
&data->position.altitude,
&data->velocity.north,
&data->velocity.east,
&data->velocity.up,
&data->attitude.roll,
&data->attitude.pitch,
&data->attitude.yaw);
} else if (!strcmp(msg_type,"rawimus")) {
data->msg_type = RAWIMUS;
double acc_y, ang_y;
sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf",
&data->linear_acceleration.z,
&acc_y,
&data->linear_acceleration.x,
&data->angular_velocity.z,
&ang_y,
&data->angular_velocity.x);
data->linear_acceleration.y = -acc_y;
data->angular_velocity.y = -ang_y;
} else {
fprintf(stderr, "Message type %s is unknown, quitting.\n", msg_type);
exit(1);
}
}
int main(int argc, char **argv)
{
#ifdef USE_ROS
#else /* ----- not USE_ROS ----- */
// Read sensors from file
char line[MAXLINE];
while (scanf("%s", line)!=EOF) {
message msg;
parseLine(line, &msg);
if (msg.msg_type==INSPVAS) {
}
}
#endif /* ----- not USE_ROS ----- */
return 0;
}
|
