Write C input parser.

Sensor inputs can be read in from stdin if compiled without USE_ROS.
Now "callback" functions can be written.

2 files changed, 181 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;
+}
+
diff --git a/src/main.h b/src/main.h
new file mode 100644
index 0000000..805e5a9
--- /dev/null
+++ b/src/main.h
@@ -0,0 +1,79 @@
+#ifndef  main_INC
+#define  main_INC
+
+//#define USE_ROS            /* Uncomment to use with ROS */
+
+#ifndef  USE_ROS
+#include <cstdio>
+#include <cstring>
+#endif     /* -----  USE_ROS  ----- */
+
+#include <iostream>
+#include <Eigen/Dense>
+
+#define MAXLINE 8192
+#define MAXFILENAME 1024
+
+// A struct for storing x,y,z data.
+typedef struct {
+    double x,y,z;
+} tuple;
+
+// A struct for storing velocity in world frame
+typedef struct {
+    double east, north, up;
+} velocity_t;
+
+// A struct for storing attitude data.
+typedef struct {
+    double roll,pitch,yaw;
+} attitude_t;
+
+// A struct for storing UTM data.
+typedef struct {
+    double northing,easting,up;
+    int zone_i;
+    char zone_c;
+} UTM;
+
+// A struct for storing GPS data.
+typedef struct {
+    double latitude, longitude, altitude;
+} gps;
+
+// Message types
+typedef enum {BESTUTM,IMG,INSCOVS,INSPVAS,RAWIMUS} message_type;
+
+/*
+ * The message struct is a general container for all message types. Not all
+ * members are used for all messages, so care must be taken not to use garbage
+ * values. 
+ */
+typedef struct {
+    int secs,nsecs;
+} timestamp;
+
+typedef struct {
+    // These should always be set.
+    timestamp stamp;
+    message_type msg_type;
+
+    // Only the members needed by the msg_type are stored.
+    tuple angular_velocity;
+    attitude_t attitude;
+    char image_names[2][MAXFILENAME];
+    tuple linear_acceleration;
+    gps position;
+    UTM utm;
+    velocity_t velocity;
+} message;
+
+int parseLine(char *line, message *msg);
+
+#ifdef  USE_ROS
+void imuCallback();
+#else      /* -----  not USE_ROS  ----- */
+void imuCallback(const message *msg);
+#endif     /* -----  not USE_ROS  ----- */
+
+#endif   /* ----- #ifndef main_INC  ----- */