Write initial "callback" functions for no ROS.

1 files changed, 77 insertions, 2 deletions

diff --git a/src/main.cpp b/src/main.cpp
index d8dfdb9..150a141 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -20,6 +20,59 @@
  */
 
 #include "main.h"
+bool seenutm=false;
+bool seenpva=false;
+
+#ifdef  USE_ROS
+
+#else      /* -----  not USE_ROS  ----- */
+void
+imgCallback(const message *msg)
+{
+    return;
+}
+void 
+imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+{
+    if (!seenutm || !seenpva) return;
+    using Eigen::Vector3d;
+    Vector3d acc(msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z);
+    Vector3d ang(msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z);
+}
+
+void
+pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+{
+    using Eigen::AngleAxisd;
+    using Eigen::Vector3d;
+    double roll, pitch, yaw;
+    attitude_t A=msg.attitude;
+    roll = M_PI*msg.attitude.roll/180.;
+    pitch = M_PI*msg.attitude.pitch/180.;
+    yaw = M_PI*(90.-msg.attitude.yaw)/180.;
+    Eigen::Matrix3d R;
+    R = AngleAxisd(yaw, Vector3d::UnitZ())
+      * AngleAxisd(roll, Vector3d::UnitY())
+      *  AngleAxisd(pitch, Vector3d::UnitX());
+    q = Eigen::Quaternion<double>(R);
+    if (!seenpva) {
+        Vector3d vw(msg.velocity.east,msg.velocity.north,msg.velocity.up);
+        X.segment(3,3) = R.transpose()*vw;
+        seenpva=true;
+    }
+    return;
+}
+
+void
+utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
+{
+    seenutm=true;
+    X[0] = msg.utm.easting;
+    X[1] = msg.utm.northing;
+    X[2] = msg.utm.up;
+    return;
+}
+#endif     /* -----  not USE_ROS  ----- */
 
 int 
 parseLine(char *line, message *data)
@@ -85,6 +138,8 @@ parseLine(char *line, message *data)
 
 int main(int argc, char **argv)
 {
+    Eigen::Quaternion<double> qbw;
+    Eigen::Matrix<double,9,1> bodyStateVector;
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
@@ -93,8 +148,28 @@ char line[MAXLINE];
 while (scanf("%s", line)!=EOF) {
     message msg;
     parseLine(line, &msg);
-    if (msg.msg_type==INSPVAS) {
-    }
+    
+    switch ( msg.msg_type ) {
+        case BESTUTM:	
+            utmCallback(msg,bodyStateVector);
+            break;
+
+        case IMG:	
+            imgCallback(&msg);
+            break;
+
+        case INSPVAS:	
+            pvaCallback(msg, bodyStateVector, qbw);
+            std::cout << bodyStateVector << std::endl;
+            break;
+
+        case RAWIMUS:	
+            imuCallback(msg, bodyStateVector, qbw);
+            break;
+
+        default:	
+            break;
+    }				/* -----  end switch  ----- */
 }
 #endif     /* -----  not USE_ROS  ----- */
 	return 0;