merging

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/rosNodeThread.h

@@ -11,13 +11,13 @@
 
 #include <ros/ros.h>
 #include <ros/network.h>
-#include "d_fall_pps/ViconData.h"
 #include "d_fall_pps/UnlabeledMarker.h"
-#include "d_fall_pps/UnlabeledMarkersArray.h"
+#include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/ViconData.h"
 
 using namespace d_fall_pps;
 
-typedef UnlabeledMarkersArray::ConstPtr ptrToMessage;
+typedef ViconData::ConstPtr ptrToMessage;
 
 Q_DECLARE_METATYPE(ptrToMessage)
 
@@ -31,12 +31,10 @@ public:
     bool init();
 
     // void messageCallback(const ViconData& data);
-    void messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg);
+    void messageCallback(const ptrToMessage& p_msg);
 
 
 signals:
-    // void newViconData(double, double, double, double, double, double);
-    // void newViconData(double, double);
 
     void newViconData(const ptrToMessage& p_msg);
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp

@@ -25,7 +25,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
     ui(new Ui::MainGUIWindow)//,
     // _rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData")
 {
-    _rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/UnlabeledMarkersArray");
+    _rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
 
     ui->setupUi(this);
     _init();

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/rosNodeThread.cpp

@@ -44,7 +44,7 @@ bool rosNodeThread::init()
     return true;
 } // set up the thread
 
-void rosNodeThread::messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg) // When a message arrives to the topic, this callback is executed
+void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
 {
     emit newViconData(p_msg);   //pass the message to other places
 }

pps_ws/src/d_fall_pps/include/CentralManagerService.h

+//for field command in CMCommand
+#define CMD_SAVE    1
+#define CMD_RELOAD  2
+
+//for field mode in CMUpdate
+#define ENTRY_INSERT_OR_UPDATE  1
+#define ENTRY_REMOVE            2
+

pps_ws/src/d_fall_pps/launch/Crazyflie.launch

 <launch>
 	<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
 	</node>
-	
+
 	<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
 		<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
 	</node>
 
 	<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
-		<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />	
+		<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
 	</node>
 
 	<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
-		<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />	
+		<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
 	</node>
 
 
@@ -25,7 +25,10 @@
 
 	<node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
 
-   	<!-- When we have a GUI, this has to be adapted and included -->
+        <node pkg="rosbag" type="record" name="testbag" />
+
+
+	<!-- When we have a GUI, this has to be adapted and included -->
 	<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
 		<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
 	</node>

pps_ws/src/d_fall_pps/src/CentralManagerService.cpp

@@ -16,6 +16,8 @@
 
 #include <stdlib.h>
 #include "ros/ros.h"
+#include <ros/package.h>
+#include "rosbag/bag.h"
 #include "d_fall_pps/CentralManager.h"
 #include "d_fall_pps/CrazyflieContext.h"
 #include "d_fall_pps/CrazyflieDB.h"
@@ -31,6 +33,18 @@ using namespace d_fall_pps;
 using namespace std;
 
 CrazyflieDB crazyflieDB;
+rosbag::Bag bag;
+
+void saveCrazyflieDB() {
+    string packagePath = ros::package::getPath("d_fall_pps") + "/";
+    string dbFile = packagePath + "crazyflie.db";
+    bag.open(dbFile, rosbag::bagmode::Write);
+    bag.write("crazyflie_db", ros::Time::now(), crazyflieDB);
+}
+
+void loadCrazyflieDB() {
+
+}
 
 bool cmRead(CMRead::Request &request, CMRead::Response &response) {
     response.crazyflieDB = crazyflieDB;
@@ -99,12 +113,12 @@ bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
 bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
     switch(request.command) {
         case CMD_SAVE: {
-            //writeCrazyflieDB(crazyflieDB);
+            saveCrazyflieDB();
             return true;
         }
 
         case CMD_RELOAD: {
-            //crazyflieDB = readCrazyflieDB();
+            loadCrazyflieDB();
             return true;
         }
 

pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp

@@ -20,7 +20,7 @@
 #include "d_fall_pps/ViconData.h"
 #include "d_fall_pps/UnlabeledMarker.h"
 
-// #define TESTING_FAKE_DATA
+#define TESTING_FAKE_DATA
 
 // notice that unit here are in milimeters
 using namespace ViconDataStreamSDK::CPP;
@@ -48,22 +48,23 @@ int main(int argc, char* argv[]) {
     	}
 
         // Testing piece of code
+        ViconData viconData;
         UnlabeledMarker marker;
-        UnlabeledMarkersArray markersArray;
 
         marker.index = 0;
         marker.x = f;
         marker.y = 0;
         marker.z = 0;
 
-        markersArray.markers.push_back(marker);
+        viconData.markers.push_back(marker);
+
 
         marker.index = 1;
         marker.x = 0;
         marker.y = f;
         marker.z = 0;
 
-        markersArray.markers.push_back(marker);
+        viconData.markers.push_back(marker);
 
         if(i > 50 && i < 100)
         {
@@ -71,13 +72,15 @@ int main(int argc, char* argv[]) {
             marker.x = f;
             marker.y = f;
             marker.z = 0;
-            markersArray.markers.push_back(marker);
+            viconData.markers.push_back(marker);
         }
 
-        unlabeledMarkersPublisher.publish(markersArray);
         ros::Duration(0.1).sleep();
         f += 10;
         i++;
+        // TODO: Fake CF data
+
+        viconDataPublisher.publish(viconData); // testing data
     }
     #else