From 3a7c300cb0cb8956ca00dc1abb7aca111a0db8a0 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Fri, 17 Mar 2017 18:37:25 -0500 Subject: Write C input parser. Sensor inputs can be read in from stdin if compiled without USE_ROS. Now "callback" functions can be written. --- src/main.cpp | 102 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/main.h | 79 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 181 insertions(+) create mode 100644 src/main.cpp create mode 100644 src/main.h (limited to 'src') 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 +#include +#endif /* ----- USE_ROS ----- */ + +#include +#include + +#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 ----- */ -- cgit v1.1