1 files changed, 102 insertions, 0 deletions
diff --git a/src/main.cpp b/src/main.cpp
new file mode 100644
index 0000000..d8dfdb9
--- /dev/null
+++ b/src/main.cpp
@@ -0,0 +1,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;
+}
+