From 59f227f4caddafbca0c2872427b7a5539f1c95a3 Mon Sep 17 00:00:00 2001
From: Giuseppe Silano <g.silano89@gmail.com>
Date: Sun, 22 Mar 2020 22:21:57 +0100
Subject: [PATCH] Add drone position to data saving function and fix typo

---
 .../rotors_control/position_controller.h      |  1 +
 .../src/library/position_controller.cpp       | 20 +++++++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/rotors_control/include/rotors_control/position_controller.h b/rotors_control/include/rotors_control/position_controller.h
index 1fd5a89..ca6391c 100644
--- a/rotors_control/include/rotors_control/position_controller.h
+++ b/rotors_control/include/rotors_control/position_controller.h
@@ -85,6 +85,7 @@ namespace rotors_control {
             std::vector<string> listXeYe_;
             std::vector<string> listDeltaCommands_;
             std::vector<string> listPQCommands_;
+            std::vector<string> listDronePosition_;
 
             // Callbacks
             ros::NodeHandle n_;
diff --git a/rotors_control/src/library/position_controller.cpp b/rotors_control/src/library/position_controller.cpp
index 4ecb247..7787f6e 100644
--- a/rotors_control/src/library/position_controller.cpp
+++ b/rotors_control/src/library/position_controller.cpp
@@ -121,6 +121,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){
       ofstream fileXeYe;
       ofstream fileDeltaCommands;
       ofstream filePQCommands;
+      ofstream fileDronePosition;
 
       ROS_INFO("CallbackSavaData function is working. Time: %f seconds, %f nanoseconds", odometry_.timeStampSec, odometry_.timeStampNsec);
 
@@ -134,6 +135,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){
       fileXeYe.open("/home/" + user_ + "/XeYe.csv", std::ios_base::app);
       fileDeltaCommands.open("/home/" + user_ + "/DeltaCommands.csv", std::ios_base::app);
       filePQCommands.open("/home/" + user_ + "/PQCommands.csv", std::ios_base::app);
+      fileDronePosition.open("/home/" + user_ + "/DronePosition.csv", std::ios_base::app);
 
       // Saving control signals in a file
       for (unsigned n=0; n < listPropellersVelocity_.size(); ++n) {
@@ -176,6 +178,10 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){
           filePQCommands << listPQCommands_.at( n );
       }
 
+      for (unsigned n=0; n < listDronePosition_.size(); ++n) {
+          fileDronePosition << listDronePosition_.at( n );
+      }
+
       // Closing all opened files
       filePropellersVelocity.close();
       fileDroneAttiude.close();
@@ -187,6 +193,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){
       fileXeYe.close();
       fileDeltaCommands.close();
       filePQCommands.close();
+      fileDronePosition.close();
 
       // To have a one shot storing
       dataStoring_active_ = false;
@@ -213,6 +220,7 @@ void PositionController::SetLaunchFileParameters(){
     listXeYe_.clear();
     listDeltaCommands_.clear();
     listPQCommands_.clear();
+    listDronePosition_.clear();
 
 	}
 
@@ -352,6 +360,8 @@ void PositionController::ControlMixer(double* PWM_1, double* PWM_2, double* PWM_
       tempPWM << *PWM_1 << "," << *PWM_2 << "," << *PWM_3 << "," << *PWM_4 << ","
               << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n";
 
+      listPWM_.push_back(tempPWM.str());
+
       // Saving drone attitude in a file
       std::stringstream tempPWMComponents;
       tempPWMComponents << control_t_.thrust << "," << delta_theta << "," << delta_phi << "," << delta_psi << ","
@@ -544,6 +554,16 @@ void PositionController::SetOdometryWithoutStateEstimator(const EigenOdometry& o
 
     // Such function is invoked when the ideal odometry sensor is employed
     SetSensorData();
+
+    if(dataStoring_active_){
+
+      // Saving drone attitude in a file
+      std::stringstream tempDronePosition;
+      tempDronePosition << odometry_.position[0] << "," << odometry_.position[1] << "," << odometry_.position[2] << "," 
+              << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n";
+
+      listDronePosition_.push_back(tempDronePosition.str());
+    }
 }
 
 // Odometry values are put in the state structure. The structure contains the aircraft state