1 files changed, 45 insertions, 39 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 62d2560..f4594fc 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -36,10 +36,6 @@ aboveWater(const Quaterniond &q)
     return tip[2];
 }
 
-#ifdef  USE_ROS
-
-#else      /* -----  not USE_ROS  ----- */
-
 /* 
  * ===  FUNCTION  ======================================================================
  *         Name:  covCallback
@@ -299,7 +295,7 @@ parseLine(char *line, message *data)
         exit(1);
     }
 }
-#endif     /* -----  not USE_ROS  ----- */
+
 /* 
  * ===  FUNCTION  ======================================================================
  *         Name:  update_dt
@@ -345,45 +341,55 @@ main(int argc, char **argv)
     // Open camera YAML
     Camera cam(argv[1]);
 
-#ifdef  USE_ROS
-    
-#else      /* -----  not USE_ROS  ----- */
-printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
-// Read sensors from file
-char line[MAXLINE];
-while (scanf("%s", line)!=EOF) {
-    message msg;
-    parseLine(line, &msg);
-    
-    switch ( msg.msg_type ) {
-        case BESTUTM:	
-            utmCallback(msg, mu,qbw);
-            break;
+#ifdef  ROS_PUBLISH
+    // Create ROS publisher
+    ros::init(argc, argv, "refslam");
+    ros::NodeHandle n;
+    ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100);
+#else      /* -----  not ROS_PUBLISH  ----- */
+    // Print unicsv header
+    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
+#endif     /* -----  not ROS_PUBLISH  ----- */
+    // Read sensors from file
+    char line[MAXLINE];
+    while (scanf("%s", line)!=EOF) {
+        message msg;
+        parseLine(line, &msg);
+        
+        switch ( msg.msg_type ) {
+            case BESTUTM:	
+                utmCallback(msg, mu,qbw);
+                break;
 
-        case IMG:	
-            imgCallback(msg, mu, cam, qbw);
-            break;
+            case IMG:	
+                imgCallback(msg, mu, cam, qbw);
+                break;
 
-        case INSPVAS:	
-            pvaCallback(msg, mu, qbw);
-            break;
+            case INSPVAS:	
+                pvaCallback(msg, mu, qbw);
+                break;
 
-        case RAWIMUS:	
-            dt = update_dt(msg.stamp, &t_old);
-            imuCallback(msg, mu, qbw, dt);
-            if (seenutm && seencov && seenpva)
-                mu.unicsv();
-            break;
+            case RAWIMUS:	
+                dt = update_dt(msg.stamp, &t_old);
+                imuCallback(msg, mu, qbw, dt);
+                if (seenutm && seencov && seenpva) {
+#ifdef  ROS_PUBLISH
+                    geometry_msgs::PoseStamped msg;
+                    ros::spinOnce();
+#else
+                    mu.unicsv();
+#endif
+                }
+                break;
 
-        case INSCOVS:
-            covCallback(msg, mu, qbw);
-            break;
+            case INSCOVS:
+                covCallback(msg, mu, qbw);
+                break;
 
-        default:	
-            break;
-    }				/* -----  end switch  ----- */
-}
-#endif     /* -----  not USE_ROS  ----- */
+            default:	
+                break;
+        }				/* -----  end switch  ----- */
+    }
 	return 0;
 }