src/vrpn_tracker.cpp → groundStation/src/backend/vrpn_tracker.cpp

 #include <iostream>
 #include <algorithm>
+#include <functional>
 
 #include "vrpn_Tracker.h"
 #include "quat.h"
@@ -9,28 +10,25 @@
 namespace microcart
 {
 	static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
+	
+	TrackerData::TrackerData() :
+		x(0.0), y(0.0), z(0.0),
+		pitch(0.0), roll(0.0), yaw(0.0),
+		fps(0.0), timestamp()
+	{
+	}
 
-	Tracker::Tracker(std::string server) : Tracker(server.c_str())
+	Tracker::Tracker(std::string server) : 
+		remote(server.c_str()),
+		stop_flag(0),
+		trackerData()
 	{
 	}
 
 	Tracker::Tracker(const char * server) :
 		remote(server),
 		stop_flag(0),
-		trackerData({
-			.x = 0.0,
-			.y = 0.0,
-			.z = 0.0,
-			.pitch = 0.0,
-			.roll = 0.0,
-			.yaw = 0.0,
-			.fps = 0.0,
-			.timestamp = {
-				.tv_sec = 0,
-				.tv_usec = 0
-			}
-		})
-
+		trackerData()
 	{
 		remote.register_change_handler(this, vrpn_cb);
 
@@ -92,10 +90,9 @@ namespace microcart
 		trackerData.fps = 1.0 / elapsed_time_usec;
 
 		auto td = trackerData;
-		std::for_each(cb_vector.begin(), cb_vector.end(),
-			[td](std::function<void(const TrackerData &)> &fn){
-				fn(td);
-		});
+		for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
+			(*i)(td);
+		}
 	}
 
 	void Tracker::addCallback(std::function<void(const TrackerData&)> cb) 
@@ -115,6 +112,20 @@ struct ucart_vrpn_tracker {
 	microcart::Tracker * t;
 };
 
+void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
+		const microcart::TrackerData &td)
+{
+						struct ucart_vrpn_TrackerData data;
+						data.x = td.x;
+						data.y = td.y;
+						data.z = td.z;
+						data.pitch = td.pitch;
+						data.roll = td.roll;
+						data.yaw = td.yaw;
+						data.fps = td.fps;
+						(*cb)(&data);
+}
+
 extern "C"
 {
 	struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
@@ -123,6 +134,7 @@ extern "C"
 		try {
 			auto inst = new struct ucart_vrpn_tracker;
 			inst->t = new microcart::Tracker(server);
+			return inst;
 		} catch(...) {
 			return NULL;
 		}
@@ -138,16 +150,8 @@ extern "C"
 			void (*cb)(struct ucart_vrpn_TrackerData *))
 	{
 		try {
-			inst->t->addCallback([cb](const microcart::TrackerData & td) {
-						struct ucart_vrpn_TrackerData data;
-						data.x = td.x;
-						data.y = td.y;
-						data.z = td.z;
-						data.pitch = td.pitch;
-						data.roll = td.roll;
-						data.yaw = td.yaw;
-						(*cb)(&data);
-					});
+			auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
+			inst->t->addCallback(new_cb);
 		} catch(...) {
 			return -1;
 		}

src/vrpn_tracker.hpp → groundStation/src/backend/vrpn_tracker.hpp

@@ -54,6 +54,7 @@ extern "C"
 namespace microcart
 {
 	struct TrackerData {
+	public:
 		double x;
 		double y;
 		double z;
@@ -64,6 +65,8 @@ namespace microcart
 
 		double fps;
 		timeval timestamp;
+
+		TrackerData();
 	};
 
 	class Tracker {

groundStation/src/cli/cli.c

+#include <stdio.h>
+#include <string.h>
+#include <err.h>
+#include <libgen.h>
+
+#include "cli.h"
+
+int main(int argc, char **argv)
+{
+	int cmdID = -1;
+	char * command;
+	int i , useSymlink = 0;
+	struct backend_conn  *conn;
+
+	command = basename(argv[0]);
+	for(i = 0; i < MAX_COMMANDS; ++i) {
+		if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+		{
+			cmdID = i;
+			useSymlink = 1;
+		}
+	}
+
+	if(cmdID == -1) {
+		command = argv[1];
+		for(i = 0; i < MAX_COMMANDS; ++i) {
+			if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+			{
+				cmdID = i;
+			}	
+		}
+	}
+
+	if(cmdID == -1){
+		printf("Could not match input with a command. Please try again...\n");
+		return -1;
+	}
+
+	printf("Parsed Command : %s\n", commandNames[cmdID]);
+	conn = ucart_backendConnect();
+	if(conn == NULL) {
+		return -1;
+	}
+
+	if(useSymlink) {
+		(*cli_functions[cmdID]) (conn, argc, &argv[0]);
+	}else {
+		(*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
+	}
+
+	ucart_backendDisconnect(conn);
+    return 0;
+}

groundStation/src/cli/cli.h

+#ifndef __CLI_H
+#define __CLI_H
+
+#include "cli_monitor.h"
+// #include "cli_setpid.h"
+#include "cli_getpid.h"
+// #include "cli_getimu.h"
+enum CommandNameIds{
+	CMD_MONITOR,
+	CMD_SETPID,
+	CMD_GETPID,
+	CMD_GETIMU,
+	MAX_COMMANDS
+};
+
+typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
+static cli_function_ptr cli_functions[2] = {
+	&cli_monitor,
+	&cli_getpid
+	// &cli_setpid,
+	// &cli_getimu
+};
+
+static char* commandNames[MAX_COMMANDS] = {
+	"monitor",
+	"getPid",
+	"setPid",
+	"getImu"
+};
+
+
+#endif

vrpn @ 99c54dbe

+Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97