pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFly.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp

@@ -36,7 +36,7 @@
 #include <QBrush>
 
 
-crazyFly::crazyFly(const CrazyflieData* p_crazyfly_msg, QString filename, QGraphicsItem * parent)
+crazyFly::crazyFly(const FlyingVehicleState* p_crazyfly_msg, QString filename, QGraphicsItem * parent)
     : QGraphicsSvgItem(filename)
 {
     updateCF(p_crazyfly_msg);
@@ -62,6 +62,18 @@ void crazyFly::setAddedToScene(bool added)
 
 void crazyFly::setScaleCFs(double scale)
 {
+    // Update the class variable for the baseline scale
+    m_baseline_scale = scale;
+
+    // Determine scaling based on height
+    float temp_z_scale = 0.75*m_z + 0.7;
+        if (temp_z_scale < 0.1)
+            temp_z_scale = 0.1;
+        else if (temp_z_scale > 2.0)
+            temp_z_scale = 2.0;
+        float temp_cf_scale = temp_z_scale * m_baseline_scale;
+
+    // Apply the scaling
     this->setScale(scale);
 }
 
@@ -71,11 +83,11 @@ std::string crazyFly::getName()
 }
 
 
-void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
+void crazyFly::updateCF(const FlyingVehicleState* p_crazyfly_msg)
 {
-    m_occluded = p_crazyfly_msg->occluded;
+    m_occluded = !(p_crazyfly_msg->isValid);
 
-    m_name = p_crazyfly_msg->crazyflieName;
+    m_name = p_crazyfly_msg->vehicleName;
     if(!m_occluded)             //if it is occluded, the info we got is useless
     {
         m_x = p_crazyfly_msg->x;
@@ -88,7 +100,18 @@ void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
 
 
         this->setPos(m_x * FROM_METERS_TO_UNITS, -m_y * FROM_METERS_TO_UNITS);    // - y because of coordinates
-        this->setRotation(- m_yaw * FROM_RADIANS_TO_DEGREES); //negative beacause anti-clock wise should be positive
+        this->setRotation(- m_yaw * RAD2DEG); //negative beacause anti-clock wise should be positive
+
+        // Determine scaling based on height
+        float temp_z_scale = 0.75*m_z + 0.7;
+        if (temp_z_scale < 0.1)
+            temp_z_scale = 0.1;
+        else if (temp_z_scale > 2.0)
+            temp_z_scale = 2.0;
+        float temp_cf_scale = temp_z_scale * m_baseline_scale;
+
+        // Apply the scaling
+        this->setScale(temp_cf_scale);
     }
 }
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/main.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/main.cpp

@@ -25,7 +25,7 @@
 //
 //
 //    DESCRIPTION:
-//    Main file of the project Teacher's GUI.
+//    Main file of the project System Config GUI.
 //
 //    ----------------------------------------------------------------------------------
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp

-//    Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
 //
 //    This file is part of D-FaLL-System.
 //    
@@ -25,7 +25,7 @@
 //
 //
 //    DESCRIPTION:
-//    Teacher's GUI main window header.
+//    System Config GUI main window header.
 //
 //    ----------------------------------------------------------------------------------
 
@@ -44,14 +44,15 @@
 #include <QMetaType>
 #include <QDir>
 #include <regex>
+#include <QShortcut>
 
 #ifdef CATKIN_MAKE
-#include "d_fall_pps/UnlabeledMarker.h"
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CrazyflieEntry.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
-#include "CentralManagerService.h"
+#include "dfall_pkg/UnlabeledMarker.h"
+#include "dfall_pkg/CMRead.h"
+#include "dfall_pkg/CrazyflieEntry.h"
+#include "dfall_pkg/CMUpdate.h"
+#include "dfall_pkg/CMCommand.h"
+#include "nodes/CentralManagerService.h"
 
 #include <ros/ros.h>
 #include <ros/network.h>
@@ -64,7 +65,7 @@
 #define N_MAX_CRAZYFLIES           20 // protection number
 
 #ifdef CATKIN_MAKE
-using namespace d_fall_pps;
+using namespace dfall_pkg;
 #endif
 
 MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
@@ -72,7 +73,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
     ui(new Ui::MainGUIWindow)
 {
     #ifdef CATKIN_MAKE
-    _rosNodeThread = new rosNodeThread(argc, argv, "my_GUI");
+    _rosNodeThread = new rosNodeThread(argc, argv, "SystemConfigGUI");
     #endif
     ui->setupUi(this);
     _init();
@@ -84,6 +85,33 @@ MainGUIWindow::~MainGUIWindow()
     delete ui;
 }
 
+float MainGUIWindow::validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value)
+{
+    // Initialise the value to the default
+    float return_value = default_value;
+
+    // Check that the field is NOT empty
+    if(!lineEdit->text().isEmpty())
+    {
+        // Extraxt the value from the field as a float
+        return_value = (lineEdit->text()).toFloat();
+        // Ensure that it is in the range specified
+        if (return_value < min)
+            return_value = min;
+        else if (return_value > max)
+            return_value = max;
+    }
+
+    // Clip the value to the specified decimal places
+
+
+    // Put the value back into the line edit
+    lineEdit->setText(QString::number( return_value, 'f', decimals));
+
+    // Return the value
+    return return_value;
+}
+
 int MainGUIWindow::getTabIndexFromName(QString name)
 {
     int found_name = -1;
@@ -144,13 +172,49 @@ void MainGUIWindow::doNumCrazyFlyZonesChanged(int n)
 
 void MainGUIWindow::_init()
 {
-    // initialize checkboxes, spinboxes,....
+    // INITIALISE VARIOUS GUI ELEMENTS
+
+	// The "CHECKBOX" for whether to display crazyflie icons
+	ui->checkBox_vicon_crazyflies->setEnabled(true);
+    ui->checkBox_vicon_crazyflies->setCheckable(true);
+    ui->checkBox_vicon_crazyflies->setChecked(true);
+
+    // The "SPIN BOX" for specifying the scaling of crazyflie icons
     ui->scaleSpinBox->setRange(0.1, 100);
     ui->scaleSpinBox->setSingleStep(0.1);
-    ui->scaleSpinBox->setValue(1);
+    ui->scaleSpinBox->setValue(5);
+    ui->scaleSpinBox->setEnabled(true);
+
+    // The "CHECKBOX" for whether to display markers
+    ui->checkBox_vicon_markers->setEnabled(true);
+	ui->checkBox_vicon_markers->setCheckable(true);
+    ui->checkBox_vicon_markers->setChecked(true);
+
+    // The "CHECKBOX" for whether to highlight the displayed markers
+    ui->checkBox_vicon_highlight_markers->setEnabled(true);
+    ui->checkBox_vicon_highlight_markers->setCheckable(true);
+    ui->checkBox_vicon_highlight_markers->setChecked(true);
+
+    // The "CHECKBOX" for whether to display the Crazyflie Zones
+    ui->checkBox_crazyfly_zones->setEnabled(true);
+    ui->checkBox_crazyfly_zones->setCheckable(true);
+    ui->checkBox_crazyfly_zones->setChecked(true);
 
-    ui->checkBox_vicon_crazyflies->setChecked(false);
-    ui->scaleSpinBox->setEnabled(false);
+    // The "CHECKBOX" for whether to display the x-y grid
+    ui->checkBox_grid->setEnabled(true);
+    ui->checkBox_grid->setCheckable(true);
+    ui->checkBox_grid->setChecked(true);
+
+    // The "CHECKBOX" for whether to display the Table
+    ui->checkBox_table->setEnabled(true);
+    ui->checkBox_table->setCheckable(true);
+    ui->checkBox_table->setChecked(true);
+
+
+    
+
+    
+    
 
 
     ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
@@ -221,9 +285,14 @@ void MainGUIWindow::_init()
     QObject::connect(scene, SIGNAL(modeChanged(int)), this, SLOT(transitionToMode(int)));
     QObject::connect(scene, SIGNAL(numTablePiecesChanged(int)), this, SLOT(handleTablePiecesNumChanged(int)));
 
-    ui->checkBox_vicon_highlight_markers->setEnabled(false);
+    // Add keyboard shortcuts
+    // > for "all motors off", press the space bar
+    ui->emergency_stop_button->setShortcut(tr("Space"));
+    // > for "kill GUI node", press "CTRL+C" while the GUI window is the focus
+    QShortcut* close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
 
-    #ifdef CATKIN_MAKE
+
+#ifdef CATKIN_MAKE
     _rosNodeThread->init();
     qRegisterMetaType<ptrToMessage>("ptrToMessage");
     QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
@@ -232,28 +301,13 @@ void MainGUIWindow::_init()
 
     ros_namespace = ros::this_node::getNamespace();
 
-    ros::NodeHandle nodeHandle("~");
-    DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
-    emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
-
-    // Initialise the publisher for sending "commands" from here (the master)
-    // to all of the agent nodes
-    commandAllAgentsPublisher = nodeHandle.advertise<std_msgs::Int32>("commandAllAgents", 1);
-
-    // Publisher for sending a request from here (the master) to all "Parameter Service" nodes
-    // that it should re-load parameters from the YAML file for the controllers.
-    // > This is recieved and acted on by both Coordinate and Agent type "Parameter Services",
-    // > A coordinator type "Parameter Service" will subsequently request the agents to fetch
-    //   the parameters from itself.
-    // > A agent type "Parameter Service" will subsequently request its own agent to fetch
-    //   the parameters from itself.
-    requestLoadControllerYamlPublisher = nodeHandle.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
-    
-    // Initialise the publisher for sending a request from here (the master)
-    // to all of the agents nodes that they should (re/dis)-connect from
-    // the Crazy-Radio
-    crazyRadioCommandAllAgentsPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommandAllAgents", 1);
-    #endif
+    //ros::NodeHandle nodeHandle("~");
+    //DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
+
+    // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+    ros::NodeHandle nodeHandle_dfall_root("/dfall");
+    emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1);
+#endif
 }
 
 void MainGUIWindow::doTabClosed(int tab_index)
@@ -298,13 +352,14 @@ void MainGUIWindow::updateComboBoxesCFZones()
     #ifdef CATKIN_MAKE
     for(int i = 0; i < scene->crazyfly_zones.size(); i++)
     {
-        if(!cf_linker->isCFZoneLinked(scene->crazyfly_zones[i]->getIndex()))
-        {
+    	// THIS IF CHECK WAS COMMENTED OUT TO ALLOW ASSIGNING MULTIPLE CRAYZFLIES TO ONE ZONE
+        //if(!cf_linker->isCFZoneLinked(scene->crazyfly_zones[i]->getIndex()))
+        //{
             int cf_zone_index = scene->crazyfly_zones[i]->getIndex();
             QString qstr = "CrazyFlyZone ";
             qstr.append(QString::number(cf_zone_index + 1));
             ui->comboBoxCFZones->addItem(qstr);
-        }
+        //}
     }
     #endif
 }
@@ -362,7 +417,7 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
         int index_name_found;
         for(int j = 0; j < crazyfly_vector_size_before; j++)
         {
-            if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
+            if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].vehicleName)
             {
                 name_found = true; // name found. This can only happen once per i-iteration, names are unique
                 index_name_found = j; // index in already existing vector, to update it later (really needed?)
@@ -376,24 +431,39 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
         else                    //name not found, newly arrived, add it to the vector
         {
             // now, if name follows our format, put the corresponding number. If not, put the unknown image
-            std::string s = p_msg->crazyflies[i].crazyflieName;
+            std::string s = p_msg->crazyflies[i].vehicleName;
             std::smatch m;
-            std::regex e ("PPS_CF([0-9]{2})");
+            std::regex e ("CF([0-9]{2})");
 
             QString filename(":/images/drone_fixed_");
 
-            if(std::regex_search(s, m, e))
-            {
-                std::string found_string = m[1].str();
-                filename.append(QString::fromStdString(found_string));
-                filename.append(".svg");
-            }
-            else
-            {
-                filename.append("unk.svg");
-            }
+			if(std::regex_search(s, m, e))
+			{
+				// Get the string that was found
+				// > it should be a zero-padded 2-digit number
+				std::string found_string = m[1].str();
+				// Convert to an integer
+				int found_string_as_int = QString::fromStdString(found_string).toInt();
+				if ((1 <= found_string_as_int) && (found_string_as_int <= 9))
+				{
+					filename.append(QString::fromStdString(found_string));
+					filename.append(".svg");
+				}
+				else
+				{
+					filename.append("unk.svg");
+				}
+			}
+			else
+			{
+				filename.append("unk.svg");
+			}
+
+            // Determine scaling based on height
+            float temp_cf_scale = (ui->scaleSpinBox->value());
 
             crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]), filename);
+            tmp_p_crazyfly->setScaleCFs(temp_cf_scale);
             crazyflies_vector.push_back(tmp_p_crazyfly);
         }
 
@@ -403,7 +473,6 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
             {
                 if(crazyflies_vector[i]->isOccluded())
                 {
-                    // ROS_INFO("===================OCCLUDED");
                     if(crazyflies_vector[i]->isAddedToScene())
                     {
                         scene->removeItem(crazyflies_vector[i]);
@@ -430,7 +499,7 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
         bool name_found = false;
         for(int i = 0; i < p_msg->crazyflies.size(); i++)
         {
-            if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
+            if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].vehicleName)
             {
                 name_found = true;
             }
@@ -695,16 +764,17 @@ void MainGUIWindow::on_refresh_student_ids_button_clicked()
     #ifdef CATKIN_MAKE
     ui->list_discovered_student_ids->clear();
 
-    // \/(\d)\/PPSClient
+    // \/(\d)\/FlyingAgentClient
     ros::V_string v_str;
     ros::master::getNodes(v_str);
     for(int i = 0; i < v_str.size(); i++)
     {
         std::string s = v_str[i];
         std::smatch m;
-        std::regex e ("\\/(\\d)\\/PPSClient");
+        //std::regex e ("\\/(\\d)\\/FlyingAgentClient");
+        std::regex e ("\\/agent(\\d\\d\\d)\\/FlyingAgentClient");
 
-        // std::regex e("\\/PPSClien(.)");
+        // std::regex e("\\/FlyingAgentClient(.)");
 
         // while(std::regex_search(s, m, e))
         // {
@@ -797,6 +867,11 @@ void MainGUIWindow::on_unlink_button_clicked()
 
 void MainGUIWindow::on_save_in_DB_button_clicked()
 {
+    // Get the zmin and zmax values from the line edits
+    float current_zmin = validate_and_get_value_from_lineEdit(ui->lineEdit_zmin,-10.0,10.0,2,-0.2);
+    float current_zmax = validate_and_get_value_from_lineEdit(ui->lineEdit_zmax,-10.0,10.0,2,1.8);
+
+#ifdef CATKIN_MAKE
     // we need to update and then save?
     CrazyflieDB tmp_db;
     for(int i = 0; i < cf_linker->links.size(); i++)
@@ -822,8 +897,8 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
                 tmp_entry.crazyflieContext.localArea.ymin = y_min * FROM_UNITS_TO_METERS;
                 tmp_entry.crazyflieContext.localArea.ymax = y_max * FROM_UNITS_TO_METERS;
 
-                tmp_entry.crazyflieContext.localArea.zmin = -0.2;
-                tmp_entry.crazyflieContext.localArea.zmax = 1.2;
+                tmp_entry.crazyflieContext.localArea.zmin = current_zmin;
+                tmp_entry.crazyflieContext.localArea.zmax = current_zmax;
             }
         }
         tmp_db.crazyflieEntries.push_back(tmp_entry);
@@ -838,11 +913,14 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
     fill_database_file();
 
     // Now also publish a ROS message stating that we changed the DB, so the nodes can update it
-    std_msgs::Int32 msg;
-    msg.data = 1;
-    this->DBChangedPublisher.publish(msg);
+    //std_msgs::Int32 msg;
+    //msg.data = 1;
+    //this->DBChangedPublisher.publish(msg);
+#endif
 }
 
+
+#ifdef CATKIN_MAKE
 void MainGUIWindow::clear_database_file()
 {
     CrazyflieDB tmp_db;
@@ -869,7 +947,10 @@ void MainGUIWindow::clear_database_file()
         ROS_INFO("Failed to read DB");
     }
 }
+#endif
 
+
+#ifdef CATKIN_MAKE
 void MainGUIWindow::fill_database_file()
 {
     clear_database_file();
@@ -882,7 +963,10 @@ void MainGUIWindow::fill_database_file()
     }
     save_database_file();
 }
+#endif
+
 
+#ifdef CATKIN_MAKE
 void MainGUIWindow::save_database_file()
 {
     CMCommand commandCall;
@@ -896,7 +980,10 @@ void MainGUIWindow::save_database_file()
         ROS_ERROR("failed to save db");
     }
 }
+#endif
+
 
+#ifdef CATKIN_MAKE
 void MainGUIWindow::insert_or_update_entry_database(CrazyflieEntry entry)
 {
     CMUpdate updateCall;
@@ -904,7 +991,10 @@ void MainGUIWindow::insert_or_update_entry_database(CrazyflieEntry entry)
     updateCall.request.crazyflieEntry = entry;
     _rosNodeThread->m_update_db_client.call(updateCall);
 }
+#endif
 
+
+#ifdef CATKIN_MAKE
 int MainGUIWindow::read_database_from_file(CrazyflieDB &read_db)
 {
     CMRead getDBCall;
@@ -919,9 +1009,12 @@ int MainGUIWindow::read_database_from_file(CrazyflieDB &read_db)
         return -1;
     }
 }
+#endif
+
 
 void MainGUIWindow::on_load_from_DB_button_clicked()
 {
+#ifdef CATKIN_MAKE
     CrazyflieDB tmp_db;
 
     if(read_database_from_file(tmp_db) == 0)
@@ -938,32 +1031,60 @@ void MainGUIWindow::on_load_from_DB_button_clicked()
 
         for(int i = 0; i < m_data_base.crazyflieEntries.size(); i++)
         {
+        	// Get the CF name from the database
             std::string cf_name = m_data_base.crazyflieEntries[i].crazyflieContext.crazyflieName;
+            // Get the radio address from the database
             std::string radio_address = m_data_base.crazyflieEntries[i].crazyflieContext.crazyflieAddress;
+            // Get the CF Zone Index from the database
             int cf_zone_index = m_data_base.crazyflieEntries[i].crazyflieContext.localArea.crazyfly_zone_index;
 
-            // we should first create the cf zones that are in the database?
-            bool cf_zone_exists;
-            qreal width = m_data_base.crazyflieEntries[i].crazyflieContext.localArea.xmax - m_data_base.crazyflieEntries[i].crazyflieContext.localArea.xmin;
-            qreal height = m_data_base.crazyflieEntries[i].crazyflieContext.localArea.ymax - m_data_base.crazyflieEntries[i].crazyflieContext.localArea.ymin;
-            QRectF tmp_rect(m_data_base.crazyflieEntries[i].crazyflieContext.localArea.xmin * FROM_METERS_TO_UNITS,
-                            - m_data_base.crazyflieEntries[i].crazyflieContext.localArea.ymax * FROM_METERS_TO_UNITS, // minus sign because qt has y-axis inverted
-                            width * FROM_METERS_TO_UNITS,
-                            height * FROM_METERS_TO_UNITS);
-            int student_id = m_data_base.crazyflieEntries[i].studentID;
-
-
-            scene->addCFZone(tmp_rect, cf_zone_index);
 
+            // THIS WAS ADDED TO ALLOW ASSIGNING MULTIPLE CRAYZFLIES TO ONE ZONE
+            // Check if a Zone with this index already exists.
+            bool cf_zone_exists;
+            scene->crazyfly_zones.size();
+            for(int i = 0; i < scene->crazyfly_zones.size(); i++)
+		    {
+	            int existing_cf_zone_index = scene->crazyfly_zones[i]->getIndex();
+	            if (cf_zone_index==existing_cf_zone_index)
+	            {
+	            	cf_zone_exists = true;
+	            }
+		    }
 
+            // we should first create the cf zones that are in the database?
+            if (!cf_zone_exists)
+            {
+            	// Get the size of this Zone from the database
+	            qreal width = m_data_base.crazyflieEntries[i].crazyflieContext.localArea.xmax - m_data_base.crazyflieEntries[i].crazyflieContext.localArea.xmin;
+	            qreal height = m_data_base.crazyflieEntries[i].crazyflieContext.localArea.ymax - m_data_base.crazyflieEntries[i].crazyflieContext.localArea.ymin;
+	            // Create a rectangle for disaplying the zone
+	            QRectF tmp_rect(m_data_base.crazyflieEntries[i].crazyflieContext.localArea.xmin * FROM_METERS_TO_UNITS,
+	                            - m_data_base.crazyflieEntries[i].crazyflieContext.localArea.ymax * FROM_METERS_TO_UNITS, // minus sign because qt has y-axis inverted
+	                            width * FROM_METERS_TO_UNITS,
+	                            height * FROM_METERS_TO_UNITS);
+	            // Add the zone to the scene
+	            scene->addCFZone(tmp_rect, cf_zone_index);
+        	}
+
+        	// Get the student ID for this entry in the database
+        	int student_id = m_data_base.crazyflieEntries[i].studentID;
+
+        	// Create the link
             cf_linker->link(student_id, cf_zone_index, cf_name, radio_address);
 
         }
+
+
+        // THIS WAS ADDED TO ALLOW ASSIGNING MULTIPLE CRAYZFLIES TO ONE ZONE
+        // Update the CF Zone combo box with the zones just added
+        updateComboBoxesCFZones();
     }
     else
     {
         ROS_ERROR("Failed to read DB");
     }
+#endif
 }
 
 void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
@@ -978,7 +1099,9 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
     }
     else
     {
+        #ifdef CATKIN_MAKE
         ROS_INFO("name not found in LUT");
+        #endif
     }
 }
 
@@ -987,312 +1110,47 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
 
 
 //    ----------------------------------------------------------------------------------
-//     CCCC   OOO   M   M  M   M    A    N   N  DDDD          A    L      L
-//    C      O   O  MM MM  MM MM   A A   NN  N  D   D        A A   L      L
-//    C      O   O  M M M  M M M  A   A  N N N  D   D       A   A  L      L
-//    C      O   O  M   M  M   M  AAAAA  N  NN  D   D       AAAAA  L      L
-//     CCCC   OOO   M   M  M   M  A   A  N   N  DDDD        A   A  LLLLL  LLLLL
+//    EEEEE  M   M  EEEEE  RRRR    GGGG  EEEEE  N   N   CCCC  Y   Y
+//    E      MM MM  E      R   R  G      E      NN  N  C       Y Y
+//    EEE    M M M  EEE    RRRR   G  GG  EEE    N N N  C        Y
+//    E      M   M  E      R   R  G   G  E      N  NN  C        Y
+//    EEEEE  M   M  EEEEE  R   R   GGG   EEEEE  N   N   CCCC    Y
 //
-//    BBBB   U   U  TTTTT  TTTTT   OOO   N   N   SSSS
-//    B   B  U   U    T      T    O   O  NN  N  S
-//    BBBB   U   U    T      T    O   O  N N N   SSS
-//    B   B  U   U    T      T    O   O  N  NN      S
-//    BBBB    UUU     T      T     OOO   N   N  SSSS
+//     SSSS  TTTTT   OOO   PPPP
+//    S        T    O   O  P   P
+//     SSS     T    O   O  PPPP
+//        S    T    O   O  P
+//    SSSS     T     OOO   P
 //    ----------------------------------------------------------------------------------
 
 
-// For the buttons that commands all of the agent nodes to:
-// > (RE)CONNECT THE RADIO
-void MainGUIWindow::on_all_connect_button_clicked()
+// > EMERGENCY STOP
+void MainGUIWindow::on_emergency_stop_button_clicked()
 {
-    std_msgs::Int32 msg;
-    msg.data = CMD_RECONNECT;
-    crazyRadioCommandAllAgentsPublisher.publish(msg);
-}
-// > DISCONNECT THE RADIO
-void MainGUIWindow::on_all_disconnect_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_DISCONNECT;
-    crazyRadioCommandAllAgentsPublisher.publish(msg);
-}
-// > TAKE-OFF
-void MainGUIWindow::on_all_take_off_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_CRAZYFLY_TAKE_OFF;
-    commandAllAgentsPublisher.publish(msg);
-}
-// > LAND
-void MainGUIWindow::on_all_land_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_CRAZYFLY_LAND;
-    commandAllAgentsPublisher.publish(msg);
-}
-// > MOTORS OFF
-void MainGUIWindow::on_all_motors_off_button_clicked()
-{
-    std_msgs::Int32 msg;
+#ifdef CATKIN_MAKE
+    dfall_pkg::IntWithHeader msg;
     msg.data = CMD_CRAZYFLY_MOTORS_OFF;
-    commandAllAgentsPublisher.publish(msg);
-    //emergencyStopPublisher.publish(msg);
-}
-// > ENABLE SAFE CONTROLLER
-void MainGUIWindow::on_all_enable_safe_controller_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_USE_SAFE_CONTROLLER;
-    commandAllAgentsPublisher.publish(msg);
-}
-// > ENABLE SAFE CONTROLLER
-void MainGUIWindow::on_all_enable_custom_controller_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_USE_CUSTOM_CONTROLLER;
-    commandAllAgentsPublisher.publish(msg);
-}
-// > LOAD THE YAML PARAMETERS FOR THE SAFE CONTROLLER
-void MainGUIWindow::on_all_load_safe_controller_yaml_own_agent_button_clicked()
-{
-	// Disable the button
-	ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
-	ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
-
-	// Send the message that the YAML paremters should be loaded
-    std_msgs::Int32 msg;
-    msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
-    requestLoadControllerYamlPublisher.publish(msg);
-
-    // Start a timer which will enable the button in its callback
-    // > This is required because the agent node waits some time between
-    //   re-loading the values from the YAML file and then assigning then
-    //   to the local variable of the agent.
-    // > Thus we use this timer to prevent the user from clicking the
-    //   button in the GUI repeatedly.
-    ros::NodeHandle nodeHandle("~");
-    m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::safeYamlFileTimerCallback, this, true);
-}
-// > LOAD THE YAML PARAMETERS FOR THE CUSTOM CONTROLLER
-void MainGUIWindow::on_all_load_custom_controller_yaml_own_agent_button_clicked()
-{
-	// Disable the button
-	ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
-	ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
-
-	// Send the message that the YAML paremters should be loaded
-    std_msgs::Int32 msg;
-    msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
-    requestLoadControllerYamlPublisher.publish(msg);
-
-    // Start a timer which will enable the button in its callback
-    // > This is required because the agent node waits some time between
-    //   re-loading the values from the YAML file and then assigning then
-    //   to the local variable of the agent.
-    // > Thus we use this timer to prevent the user from clicking the
-    //   button in the GUI repeatedly.
-    ros::NodeHandle nodeHandle("~");
-    m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
-
-}
-// > SEND THE YAML PARAMETERS FOR THE SAFE CONTROLLER
-void MainGUIWindow::on_all_load_safe_controller_yaml_coordinator_button_clicked()
-{
-	// Disable the button
-	ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
-	ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
-
-	// Send the message that the YAML paremters should be loaded
-    // by the coordinator (and then the agent informed)
-    std_msgs::Int32 msg;
-    msg.data = LOAD_YAML_SAFE_CONTROLLER_COORDINATOR;
-    requestLoadControllerYamlPublisher.publish(msg);
-
-    // Start a timer which will enable the button in its callback
-    // > This is required because the agent node waits some time between
-    //   re-loading the values from the YAML file and then assigning then
-    //   to the local variable of the agent.
-    // > Thus we use this timer to prevent the user from clicking the
-    //   button in the GUI repeatedly.
-    ros::NodeHandle nodeHandle("~");
-    m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::safeYamlFileTimerCallback, this, true);
-}
-// > SEND THE YAML PARAMETERS FOR THE CUSTOM CONTROLLER
-void MainGUIWindow::on_all_load_custom_controller_yaml_coordinator_button_clicked()
-{
-	// Disable the button
-	ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
-	ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
-
-    // Send the message that the YAML paremters should be loaded
-    // by the coordinator (and then the agent informed)
-    std_msgs::Int32 msg;
-    msg.data = LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR;
-    requestLoadControllerYamlPublisher.publish(msg);
-
-    // Start a timer which will enable the button in its callback
-    // > This is required because the agent node waits some time between
-    //   re-loading the values from the YAML file and then assigning then
-    //   to the local variable of the agent.
-    // > Thus we use this timer to prevent the user from clicking the
-    //   button in the GUI repeatedly.
-    ros::NodeHandle nodeHandle("~");
-    m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
-
-}
-// > CALLBACK TO RE-ENABLE THE SAFE CONTROLLER YAML BUTTONS
-void MainGUIWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
-{
-    // Enble the "load" and the "send" safe controller YAML button again
-    ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(true);
-	ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(true);
-}
-// > CALLBACK TO RE-ENABLE THE CUSTOM CONTROLLER YAML BUTTONS
-void MainGUIWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
-{
-    // Enble the "load" and the "send" custom controller YAML button again
-    ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(true);
-	ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(true);
-}
-
-/*
-// > CALLBACK TO SEND THE CUSTOM CONTROLLER YAML PARAMETERS AS A MESSAGE
-void MainGUIWindow::customSendYamlAsMessageTimerCallback(const ros::TimerEvent&)
-{
-	// Load the CUSTOM controller YAML parameters from file into a message for
-	// sending directly to the "CustonControllerService" node of every agent
-
-	ros::NodeHandle nodeHandle("~");
-
-	// Instaniate a local variable of the message type
-	CustomControllerYAML customControllerYamlMessage;
-
-	// Load the data directly from the YAML file into the message
-
-	// > For the mass
-	customControllerYamlMessage.mass = MainGUIWindow::getParameterFloat(nodeHandle, "mass");
-
-    // Debugging this this works
-    ROS_INFO_STREAM("DEBUGGING: mass loaded from the custom controller YAML = " << customControllerYamlMessage.mass );
-
-	// > For the control_frequency
-	customControllerYamlMessage.control_frequency = MainGUIWindow::getParameterFloat(nodeHandle, "control_frequency");
-
-	// > For the motorPoly coefficients
-    std::vector<float> temp_motorPoly(3);
-	MainGUIWindow::getParameterFloatVector(nodeHandle, "motorPoly", temp_motorPoly, 3);
-    // Copy the loaded data into the message
-    for ( int i=0 ; i<3 ; i++ )
-    {
-        customControllerYamlMessage.motorPoly.push_back( temp_motorPoly[i] );
-    }
-
-    // > For the boolean about whether to execute the convert into body frame function
-    customControllerYamlMessage.shouldPerformConvertIntoBodyFrame = MainGUIWindow::getParameterBool(nodeHandle, "shouldPerformConvertIntoBodyFrame");
-
-    // > For the boolean about publishing the agents current position
-	customControllerYamlMessage.shouldPublishCurrent_xyz_yaw = MainGUIWindow::getParameterBool(nodeHandle, "shouldPublishCurrent_xyz_yaw");
-
-	// > For the boolean about following another agent
-	customControllerYamlMessage.shouldFollowAnotherAgent = MainGUIWindow::getParameterBool(nodeHandle, "shouldFollowAnotherAgent");
-
-	// > For the ordered agent ID's for following eachother in a line
-    std::vector<int> temp_follow_in_a_line_agentIDs(100);
-	int temp_number_of_agents_in_a_line = MainGUIWindow::getParameterIntVectorWithUnknownLength(nodeHandle, "follow_in_a_line_agentIDs", temp_follow_in_a_line_agentIDs);
-	// > Double check that the sizes agree
-    if ( temp_number_of_agents_in_a_line != temp_follow_in_a_line_agentIDs.size() )
-    {
-    	// Update the user if the sizes don't agree
-    	ROS_ERROR_STREAM("parameter 'follow_in_a_line_agentIDs' was loaded with two different lengths, " << temp_number_of_agents_in_a_line << " versus " << temp_follow_in_a_line_agentIDs.size() );
-    }
-    // Copy the loaded data into the message
-    for ( int i=0 ; i<temp_number_of_agents_in_a_line ; i++ )
-    {
-        customControllerYamlMessage.follow_in_a_line_agentIDs.push_back( temp_follow_in_a_line_agentIDs[i] );
-    }    
-
-    // Publish the message containing the loaded YAML parameters
-    customYAMLasMessagePublisher.publish(customControllerYamlMessage);
-
-	// Start a timer which will enable the button in its callback
-    m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(0.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
+    msg.shouldCheckForAgentID = false;
+    emergencyStopPublisher.publish(msg);
+#endif
 }
-*/
-
-
 
 
 //    ----------------------------------------------------------------------------------
-//    L       OOO     A    DDDD        Y   Y    A    M   M  L
-//    L      O   O   A A   D   D        Y Y    A A   MM MM  L
-//    L      O   O  A   A  D   D         Y    A   A  M M M  L
-//    L      O   O  AAAAA  D   D         Y    AAAAA  M   M  L
-//    LLLLL   OOO   A   A  DDDD          Y    A   A  M   M  LLLLL
 //
-//    PPPP     A    RRRR     A    M   M  EEEEE  TTTTT  EEEEE  RRRR    SSSS
-//    P   P   A A   R   R   A A   MM MM  E        T    E      R   R  S
-//    PPPP   A   A  RRRR   A   A  M M M  EEE      T    EEE    RRRR    SSS
-//    P      AAAAA  R  R   AAAAA  M   M  E        T    E      R  R       S
-//    P      A   A  R   R  A   A  M   M  EEEEE    T    EEEEE  R   R  SSSS
+//
+//
+//
+//
 //    ----------------------------------------------------------------------------------
 
-// load parameters from corresponding YAML file
-//
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float MainGUIWindow::getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
-{
-    float val;
-    if(!nodeHandle.getParam(name, val))
-    {
-        ROS_ERROR_STREAM("missing parameter '" << name << "'");
-    }
-    return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void MainGUIWindow::getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
-{
-    if(!nodeHandle.getParam(name, val)){
-        ROS_ERROR_STREAM("missing parameter '" << name << "'");
-    }
-    if(val.size() != length) {
-        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
-    }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int MainGUIWindow::getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
-{
-    int val;
-    if(!nodeHandle.getParam(name, val))
-    {
-        ROS_ERROR_STREAM("missing parameter '" << name << "'");
-    }
-    return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void MainGUIWindow::getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+void MainGUIWindow::on_lineEdit_zmin_editingFinished()
 {
-    if(!nodeHandle.getParam(name, val)){
-        ROS_ERROR_STREAM("missing parameter '" << name << "'");
-    }
-    if(val.size() != length) {
-        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
-    }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int MainGUIWindow::getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
-{
-    if(!nodeHandle.getParam(name, val)){
-        ROS_ERROR_STREAM("missing parameter '" << name << "'");
-    }
-    return val.size();
+    validate_and_get_value_from_lineEdit(ui->lineEdit_zmin,-10.0,10.0,2,-0.2);
 }
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-bool MainGUIWindow::getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+
+void MainGUIWindow::on_lineEdit_zmax_editingFinished()
 {
-    bool val;
-    if(!nodeHandle.getParam(name, val))
-    {
-        ROS_ERROR_STREAM("missing parameter '" << name << "'");
-    }
-    return val;
+    validate_and_get_value_from_lineEdit(ui->lineEdit_zmax,-10.0,10.0,2,1.8);
 }
+

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/marker.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/marker.cpp

@@ -25,7 +25,7 @@
 //
 //
 //    DESCRIPTION:
-//    Teacher's GUI marker object, to represent unlabeled markers.
+//    System Config GUI marker object, to represent unlabeled markers.
 //
 //    ----------------------------------------------------------------------------------
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsRectItem.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsRectItem.cpp

@@ -101,12 +101,12 @@ QVariant myGraphicsRectItem::itemChange(GraphicsItemChange change, const QVarian
     {
         if(checkCornerGrabbers() == CornerGrabber::noCorner)
         {
-            qDebug("move now!");
+            //qDebug("move now!");
             return QPointF(value.toPointF().x(), value.toPointF().y());
         }
         else
         {
-            qDebug("dont move now!");
+            //qDebug("dont move now!");
             return QPointF(pos().x(), pos().y());
         }
     }

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsScene.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsScene.cpp

@@ -227,8 +227,8 @@ void myGraphicsScene::mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent)
             {
                 tmp_crazyfly_zone_item->setRect(QRectF(*p1, mouseEvent->scenePos()));
                 tmp_crazyfly_zone_item->updateCenterMarker();
-                qDebug("Mouse Position: %d, %d", (mouseEvent->scenePos()).toPoint().x(), (mouseEvent->scenePos()).toPoint().y());
-                qDebug("Rectangle BottomRight Position: %d, %d", tmp_crazyfly_zone_item->rect().bottomRight().x(), tmp_crazyfly_zone_item->rect().bottomRight().y());
+                //qDebug("Mouse Position: %d, %d", (mouseEvent->scenePos()).toPoint().x(), (mouseEvent->scenePos()).toPoint().y());
+                //qDebug("Rectangle BottomRight Position: %f, %f", tmp_crazyfly_zone_item->rect().bottomRight().x(), tmp_crazyfly_zone_item->rect().bottomRight().y());
                 break;
             }
         }

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread.cpp → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/rosNodeThread_for_systemConfigGUI.cpp

@@ -25,24 +25,23 @@
 //
 //
 //    DESCRIPTION:
-//    Takes care of creating a ros node thread.
+//    Creates a thread that runs the ros node.
 //
 //    ----------------------------------------------------------------------------------
 
 
-#include "rosNodeThread.h"
+#include "rosNodeThread_for_systemConfigGUI.h"
 
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
+#include "dfall_pkg/CMRead.h"
+#include "dfall_pkg/CMUpdate.h"
+#include "dfall_pkg/CMCommand.h"
 
 
-rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
+rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name,  QObject* parent)
     :   QObject(parent),
         m_Init_argc(argc),
         m_pInit_argv(pArgv),
         m_node_name(node_name)
-
 {
     /** Constructor for the node thread **/
 }
@@ -54,6 +53,7 @@ rosNodeThread::~rosNodeThread()
         ros::shutdown();
         ros::waitForShutdown();
     } // end if
+
     m_pThread->wait();
 } // end destructor
 
@@ -63,11 +63,13 @@ bool rosNodeThread::init()
     this->moveToThread(m_pThread); // QObject method
 
     connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
-    ros::init(m_Init_argc, m_pInit_argv, m_node_name); // student_GUI is the name of this node
+    ros::init(m_Init_argc, m_pInit_argv, m_node_name);
+    // Note that the variable "m_node_name" should be the
+    // string "SystemConfigGUI" in this case
 
     if (!ros::master::check())
     {
-        ROS_ERROR("Master not found, please check teacher computer is running and try again");
+        ROS_ERROR("No master found. Please make sure that there is a master roscore running");
         return false;           // do not start without ros.
     }
 
@@ -75,12 +77,14 @@ bool rosNodeThread::init()
     ros::Time::init();
     ros::NodeHandle nh("~");
 
-    m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
+    ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+    m_vicon_subscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
 
     // clients for db services:
-    m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
-    m_update_db_client = nh.serviceClient<CMUpdate>("/CentralManagerService/Update", false);
-    m_command_db_client = nh.serviceClient<CMCommand>("/CentralManagerService/Command", false);
+    m_read_db_client = nodeHandle_dfall_root.serviceClient<CMRead>("CentralManagerService/Read", false);
+    m_update_db_client = nodeHandle_dfall_root.serviceClient<CMUpdate>("CentralManagerService/Update", false);
+    m_command_db_client = nodeHandle_dfall_root.serviceClient<CMCommand>("CentralManagerService/Command", false);
 
     m_pThread->start();
     return true;
@@ -91,6 +95,23 @@ void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message
     emit newViconData(p_msg);   //pass the message to other places
 }
 
+// void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
+// {
+//     QMutex * pMutex = new QMutex();
+//     pMutex->lock();
+//     ROS_INFO_STREAM("ViconData: " << data.x << ", " << data.y << ", " << data.z);
+//     m_vicon_data.x = data.x;
+//     m_vicon_data.y = data.y;
+//     m_vicon_data.z = data.z;
+//     m_vicon_data.yaw = data.yaw;
+//     m_vicon_data.pitch = data.pitch;
+//     m_vicon_data.roll = data.roll;
+//     pMutex->unlock();
+//     delete pMutex;
+//     // Q_EMIT newViconData(m_vicon_data.x, m_vicon_data.y, m_vicon_data.z, m_vicon_data.yaw, m_vicon_data.pitch, m_vicon_data.roll);
+//     emit newViconData(m_vicon_data.x, m_vicon_data.y);
+// }
+
 void rosNodeThread::run()
 {
     ros::Rate loop_rate(100);
@@ -104,7 +125,6 @@ void rosNodeThread::run()
         // cmd_msg.linear.x = m_speed;
         // cmd_msg.angular.z = m_angle;
         pMutex->unlock();
-        // ROS_INFO("RUNNING");
 
         // sim_velocity.publish(cmd_msg);
         ros::spinOnce();

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro → dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/systemConfigGUI.pro

@@ -8,18 +8,20 @@ QT       += core gui
 
 greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
 
-TARGET = CrazyFlyGUI
+TARGET = systemConfigGUI
 TEMPLATE = app
 
 INCLUDEPATH += $$PWD/include
 CONFIG += c++11
 
-RESOURCES = CrazyFlyGUI.qrc
+RESOURCES = systemconfiggui.qrc
 
 QT+= svg
 
 
 SOURCES += \
+    src/centerMarker.cpp \
+    src/channelLUT.cpp \
     src/cornergrabber.cpp \
     src/crazyFlyZone.cpp \
     src/crazyFlyZoneTab.cpp \
@@ -31,6 +33,8 @@ SOURCES += \
     src/tablePiece.cpp
 
 HEADERS  += \
+    include/centerMarker.h \
+    include/channelLUT.h \
     include/cornergrabber.h \
     include/crazyFlyZone.h \
     include/crazyFlyZoneTab.h \
@@ -41,7 +45,9 @@ HEADERS  += \
     include/tablePiece.h \
     include/globalDefinitions.h \
     include/marker.h \
-    include/crazyFly.h
+    include/crazyFly.h \
+    \
+    include/constants_for_qt_compile.h
 
 FORMS    += \
-    src/mainguiwindow.ui
+    forms/mainguiwindow.ui

pps_ws/src/d_fall_pps/PPS.perspective → dfall_ws/src/dfall_pkg/PPS.perspective

@@ -191,7 +191,7 @@
               "keys": {
                 "publishers": {
                   "type": "repr", 
-                  "repr": "u\"[{'type_name': 'std_msgs/Int32', 'topic_name': '/PPSClient/Command', 'enabled': False, 'rate': 1.0, 'expressions': {u'/PPSClient/Command/data': '3'}, 'publisher_id': 0, 'counter': 0}]\""
+                  "repr": "u\"[{'type_name': 'std_msgs/Int32', 'topic_name': '/FlyingAgentClient/Command', 'enabled': False, 'rate': 1.0, 'expressions': {u'/FlyingAgentClient/Command/data': '3'}, 'publisher_id': 0, 'counter': 0}]\""
                 }
               }, 
               "groups": {}

dfall_ws/src/dfall_pkg/crazyradio/CONTRIBUTING.md

+Contribution guide
+==================
+
+👍🎉 Thanks a lot for considering contributing 🎉👍
+
+We welcome and encourage contribution. There is many way to contribute: you can
+write bug report, contribute code or documentation.
+You can also go to the [bitcraze forum](https://forum.bitcraze.io) and help others.
+
+## Reporting issues
+
+When reporting issues the more information you can supply the better.
+Since the lib runs on many different OSes, can connect to multiple versions of the Crazyflie and you could use our official releases or clone directly from Git, it can be hard to figure out what's happening.
+
+ - **Information about the environment:**
+   - What OS are your running on
+   - What version of Python are you using
+   - If relevant, what version of the Crazyflie firmware are you using
+ - **How to reproduce the issue:** Step-by-step guide on how the issue can be reproduced (or at least how you reproduce it).
+ Include everything you think might be useful, the more information the better.
+
+## Improvements request and proposal
+
+We and the community are continuously working to improve the lib.
+Feel free to make an issue to request a new functionality.
+
+## Contributing code/Pull-Request
+
+We welcome code contribution, this can be done by starting a pull-request.
+
+If the change is big, typically if the change span to more than one file, consider starting an issue first to discuss the improvement.
+This will makes it much easier to make the change fit well into the lib.
+
+There is some basic requirement for us to merge a pull request:
+ - Describe the change
+ - Refer to any issues it effects
+ - Separate one pull request per functionality: if you start writing "and" in the feature description consider if it could be separated in two pull requests.
+ - The pull request must pass the automated test (see test section bellow)
+
+In your code:
+- Don't include name, date or information about the change in the code. That's what Git is for.
+- CamelCase classes, but not functions and variables
+- Private variables and functions should start with _
+- 4 spaces indentation
+- When catching exceptions try to make it as specific as possible, it makes it harder for bugs to hide
+- Short variable and function names are OK if the scope is small
+- The code should pass flake8
+
+### Run test
+
+In order to run the tests you can run:
+```
+python tools/build/build
+python3 tools/build/build
+```

dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+#    Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+#
+#    This file is part of D-FaLL-System.
+#    
+#    D-FaLL-System is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#    
+#    D-FaLL-System is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU General Public License for more details.
+#    
+#    You should have received a copy of the GNU General Public License
+#    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
+#    
+#
+#    ----------------------------------------------------------------------------------
+#    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
+#    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
+#    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
+#    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
+#    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
+#
+#
+#    DESCRIPTION:
+#    The service that manages the context of the student groups.
+#
+#    ----------------------------------------------------------------------------------
+
+
+# Import the ROS library package
+import roslib
+# Load the "dfall package" manifest for ROS
+roslib.load_manifest('dfall_pkg')
+# Import the ROS-Python package
+import rospy
+# Import standard messages
+from std_msgs.msg  import Int32
+# Import dfall_pkg messages
+from dfall_pkg.msg import ControlCommand
+from dfall_pkg.msg import IntWithHeader
+from dfall_pkg.msg import FlyingVehicleState
+#from dfall_pkg.msg import GyroMeasurements
+# Import dfall_pkg services
+from dfall_pkg.srv import IntIntService
+
+# General import
+import time, sys
+import struct
+import logging
+
+# Import the ROS bag package for logging
+#import rosbag
+#from rospkg import RosPack
+
+# Import the ROS standard message types
+from std_msgs.msg import Float32
+from std_msgs.msg import String
+
+# Add library
+#sys.path.append("lib")
+
+# CrazyFlie client imports
+import cflib
+
+from cflib.crazyflie import Crazyflie
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+
+import cflib.drivers.crazyradio
+
+# Import the package for logging data back from the Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+# Logging settings
+logging.basicConfig(level=logging.ERROR)
+
+# CONTROLLER_MOTOR = 2
+# CONTROLLER_ANGLE = 1
+# CONTROLLER_RATE = 0
+
+# CF_COMMAND_TYPE_MOTORS =  8
+# CF_COMMAND_TYPE_RATE   =  9
+# CF_COMMAND_TYPE_ANGLE  = 10
+
+CF_ONBOARD_CONTROLLER_MODE_OFF            = 0
+CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE   = 1
+CF_ONBOARD_CONTROLLER_MODE_ANGLE          = 2
+CF_ONBOARD_CONTROLLER_MODE_VELOCITY       = 3
+CF_ONBOARD_CONTROLLER_MODE_POSITION       = 4
+
+# CF_PACKET_DECODER_STOP_TYPE              = 0
+# CF_PACKET_DECODER_VELOCITY_WORLD_TYPE    = 1
+# CF_PACKET_DECODER_Z_DISTANCE_TYEP        = 2
+# CF_PACKET_DECODER_CPPM_EMU_TYPE          = 3
+# CF_PACKET_DECODER_ALT_HOLD_TYPE          = 4
+# CF_PACKET_DECODER_HOVER_TYPE             = 5
+# CF_PACKET_DECODER_FULL_STATE_TYPE        = 6
+# CF_PACKET_DECODER_POSITION_TYPE          = 7
+CF_PACKET_DECODER_DFALL_TYPE             = 8
+
+RAD_TO_DEG = 57.296
+
+# CrazyRadio states:
+CONNECTED    = 0
+CONNECTING   = 1
+DISCONNECTED = 2
+
+# Commands coming
+CMD_RECONNECT  = 0
+CMD_DISCONNECT = 1
+
+# Commands for FlyingAgentClient
+#CMD_USE_SAFE_CONTROLLER    = 1
+#CMD_USE_DEMO_CONTROLLER    = 2
+#CMD_USE_STUDENT_CONTROLLER = 3
+#CMD_USE_MPC_CONTROLLER     = 4
+#CMD_USE_REMOTE_CONTROLLER  = 5
+#CMD_USE_TUNING_CONTROLLER  = 6
+
+CMD_CRAZYFLY_TAKE_OFF   = 11
+CMD_CRAZYFLY_LAND       = 12
+CMD_CRAZYFLY_MOTORS_OFF = 13
+
+# Types of flying vehicle states:
+#FLYING_VEHICLE_STATE_TYPE_NONE                        = 0
+#FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT           = 1
+FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE    = 2
+#FLYING_VEHICLE_STATE_TYPE_12_STATE_ESTIMATE           = 3
+
+# Flying states
+STATE_MOTORS_OFF     = 1
+#STATE_TAKE_OFF       = 2
+#STATE_FLYING         = 3
+#STATE_LAND           = 4
+#STATE_UNAVAILABLE    = 5
+
+
+
+# Open the ROS bag for logging
+#rp = RosPack()
+#record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
+#rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
+#rospy.loginfo(record_file)
+#bag = rosbag.Bag(record_file, 'w')
+
+class CrazyRadioClient:
+    """
+       CrazyRadio client that recieves the commands from the controller and
+       sends them in a CRTP package to the crazyflie with the specified
+       address.
+    """
+    def __init__(self):
+
+        # INITIALISE THE PROPERTIES OF THE OBJECT
+        # > For the status of the radio link
+        self._status = DISCONNECTED
+        # > For the addess of the radio link
+        self.link_uri = ""
+        # > For the name of the connected crazyflie
+        self.crazyflie_name = ""
+        # > For keeping track of the "flying state"
+        #   of the "FlyingAgentClient"
+        self._flyingState_of_flyingAgentClient = STATE_MOTORS_OFF
+
+        # > INIIALISE PUBLISHERS
+        # > For informing the network of the status of
+        #   the radio link
+        self.radio_status_publisher = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
+        # > For publishing commands on behalf of the
+        #   FlyingAgentClient, only used for publishing
+        #   "motor off" commands
+        self.flyingAgentClient_command_publisher = rospy.Publisher('FlyingAgentClient/Command', IntWithHeader, queue_size=1)
+
+        # PAUSE SHORTLY FOR THE PUBLISHERS TO BE REGISTERED IN ROS
+        time.sleep(1.0)
+
+        # Initialize the CrazyFlie and add callbacks
+        self._init_cf()
+
+        # Connect to the Crazyflie
+        self.connect()
+
+    def _init_cf(self):
+        self._cf = Crazyflie()
+
+        # Add callbacks that get executed depending on the connection _status.
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        self.change_status_to(DISCONNECTED)
+
+    def change_status_to(self, new_status):
+        print "[CRAZY RADIO] status changed to: %s" % new_status
+        self._status = new_status
+        self.radio_status_publisher.publish(new_status)
+
+    def get_status(self):
+        return self._status
+
+    def update_link_uri(self):
+        self.link_uri = "radio://" + rospy.get_param("~crazyflieAddress")
+
+    def update_crazyflie_name(self):
+        self.crazyflie_name = rospy.get_param("~crazyflieName")
+
+    def connect(self):
+        # update link from ros params
+        self.update_link_uri()
+
+        print "[CRAZY RADIO] Connecting to %s" % self.link_uri
+        self.change_status_to(CONNECTING)
+        rospy.loginfo("[CRAZY RADIO] connecting...")
+        self._cf.open_link(self.link_uri)
+
+    def disconnect(self):
+        print "[CRAZY RADIO] sending Motors OFF command before disconnecting"
+        cf_client._send_to_commander(0, 0, 0, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0)
+        # change state to motors OFF
+        msg = IntWithHeader()
+        msg.shouldCheckForAgentID = False
+        msg.data = CMD_CRAZYFLY_MOTORS_OFF
+        self.flyingAgentClient_command_publisher.publish(msg)
+        time.sleep(0.1)
+        print "[CRAZY RADIO] Disconnecting from %s" % self.link_uri
+        self._cf.close_link()
+        self.change_status_to(DISCONNECTED)
+
+
+
+    def _data_received_battery_callback(self, timestamp, data, logconf):
+        # Initialise the variable for the data
+        batteryVolt = Float32()
+        # Retrieve the values from the data packet received
+        batteryVolt.data = data["pm.vbat"]
+
+        # Write data to the ROS bag
+        #bag.write('batteryVoltage', batteryVolt)
+
+        # Print out one value for DEBUGGING
+        #print "[CRAZY RADIO] DEBUGGING, received pm.vbat = %f" % batteryVolt.data
+
+        # Publish the battery voltage
+        cfbattery_pub.publish(batteryVolt)
+
+
+
+    def _data_received_stateEstimate_callback(self, timestamp, data, logconf):
+        # Perform safety check if required
+        if (isEnabled_strictSafety):
+            # Estimate the height at the next measurement
+            height_at_next_measurement = data["stateEstimateZ.z"] / 1000.0 + (cfStateEstimate_polling_period / 1000.0) * (data["stateEstimateZ.vz"] / 1000.0)
+            # Turn-off if too high
+            if (height_at_next_measurement > maxHeight_for_strictSafety_meters):
+                # Publish a motors OFF command
+                msg = IntWithHeader()
+                msg.shouldCheckForAgentID = False
+                msg.data = CMD_CRAZYFLY_MOTORS_OFF
+                self.flyingAgentClient_command_publisher.publish(msg)
+                # Inform the user
+                rospy.logerr("[CRAZY RADIO] Height safety check failed, measured = %f, max allowed = %f" % (height_at_next_measurement, maxHeight_for_strictSafety_meters))
+
+        # Initialise the variable for the flying vehicle state
+        cfStateEstimate = FlyingVehicleState()
+
+        # Retrieve the values from the data packet received
+        # > (x,y,z) positions
+        cfStateEstimate.x  = data["stateEstimateZ.x"] / 1000.0
+        cfStateEstimate.y  = data["stateEstimateZ.y"] / 1000.0
+        cfStateEstimate.z  = data["stateEstimateZ.z"] / 1000.0
+        # > (x,y,z) velocities
+        cfStateEstimate.vx = data["stateEstimateZ.vx"] / 1000.0
+        cfStateEstimate.vy = data["stateEstimateZ.vy"] / 1000.0
+        cfStateEstimate.vz = data["stateEstimateZ.vz"] / 1000.0
+        # > (x,y,z) accelerations
+        #cfStateEstimate.ax = data["stateEstimateZ.ax"] / 1000.0
+        #cfStateEstimate.ay = data["stateEstimateZ.ay"] / 1000.0
+        #cfStateEstimate.az = data["stateEstimateZ.az"] / 1000.0
+        # > (roll,pitch,yaw) angles
+        cfStateEstimate.roll  =  data["stateEstimateZ.roll"]  / 1000.0
+        cfStateEstimate.pitch = -data["stateEstimateZ.pitch"] / 1000.0
+        cfStateEstimate.yaw   =  data["stateEstimateZ.yaw"]   / 1000.0
+        # > (roll,pitch,yaw) anglular rates (direct from gryo)
+        #cfStateEstimate.rollRate  =  data["stateEstimateZ.rateRoll"]  / 1000.0
+        #cfStateEstimate.pitchRate = -data["stateEstimateZ.ratePitch"] / 1000.0
+        #cfStateEstimate.yawRate   =  data["stateEstimateZ.rateYaw"]   / 1000.0
+
+        # Print out one value for DEBUGGING
+        #print "[CRAZY RADIO] received (z,vz)  = ( %6.3f , %6.3f )" %( cfStateEstimate.z , cfStateEstimate.vz )
+        #print "[CRAZY RADIO] received (r,p,y) = ( %6.3f , %6.3f , %6.3f )" %( cfStateEstimate.roll*RAD_TO_DEG , cfStateEstimate.pitch*RAD_TO_DEG , cfStateEstimate.yaw*RAD_TO_DEG )
+
+        # Fill in the remaining details:
+        # > For the type
+        cfStateEstimate.type = FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE
+        # > For the name
+        cfStateEstimate.vehicleName = self.crazyflie_name
+        # > For the validity flag
+        cfStateEstimate.isValid = True
+        # > For the acquiring time
+        cfStateEstimate.acquiringTime = 0.0
+
+        # Publish the state estimate
+        cfstateEstimate_pub.publish(cfStateEstimate)
+
+
+
+        # # Initialise the variable for the gyroscope data
+        # gyroMeasurements = GyroMeasurements()
+
+        # # Retrieve the values from the data packet received
+        # # > (roll,pitch,yaw) anglular rates (direct from gryo)
+        # gyroMeasurements.rollrate  =  data["stateEstimateZ.rateRoll"] / 1000.0
+        # gyroMeasurements.pitchrate = -data["stateEstimateZ.ratePitch"] / 1000.0
+        # gyroMeasurements.yawrate   =  data["stateEstimateZ.rateYaw"] / 1000.0
+
+        # # Fill in the name
+        # gyroMeasurements.vehicleName = self.crazyflie_name
+
+        # # Print out one value for DEBUGGING
+        # #print "[CRAZY RADIO] gyro (r,p,y)     = ( %6.3f , %6.3f , %6.3f )" %( gyroMeasurements.rollrate*RAD_TO_DEG , gyroMeasurements.pitchrate*RAD_TO_DEG , gyroMeasurements.yawrate*RAD_TO_DEG )
+        
+        # # Publish the gyroscope measurements
+        # cfgyroMeasurements_pub.publish(gyroMeasurements)
+
+
+
+    def _logging_error(self, logconf, msg):
+        print "[CRAZY RADIO] Error when logging %s" % logconf.name
+
+
+
+    def _start_logging(self):
+
+        # NOTE: that logging can only be started when
+        # connected to a Crazyflie is becuse "add_config(...)"
+        # function check that the variables requested are
+        # available in the onboard TOC (table of contents).
+
+        # A "LOGGING GROUP" FOR THE BATTERY VOLTAGE:
+
+        # Initialise a log config object
+        self.logconf_battery = LogConfig("LoggingBattery", period_in_ms=battery_polling_period)
+        # Add the varaibles to be logged
+        self.logconf_battery.add_variable("pm.vbat", "float");
+        #
+        # Try to add the log config to the crazyflie object
+        try:
+            self._cf.log.add_config(self.logconf_battery)
+            # Add the callback for received data
+            self.logconf_battery.data_received_cb.add_callback(self._data_received_battery_callback)
+            # Add the callback for errors
+            self.logconf_battery.error_cb.add_callback(self._logging_error)
+            # Check the log config is valid
+            if self.logconf_battery.valid:
+                # Start the logging
+                self.logconf_battery.start()
+                rospy.loginfo("[CRAZY RADIO] Started the log config for the battery voltage")
+            else:
+                rospy.loginfo("[CRAZY RADIO] The log config for the battery voltage is invalid, hence logging has not been started.")
+        # Handle "key error" exceptions:
+        except KeyError as e:
+            rospy.logerr("[CRAZY RADIO] For the battery voltage, the following error occurred while trying to add the log config: %s" % str(e) ) 
+        # Handle "attribution error" exceptions:
+        except AttributeError:
+            rospy.logerr("[CRAZY RADIO] The battery voltage log config has a bad configuration.")
+
+        # A "LOGGING GROUP" FOR THE STATE ESTIMATE:
+
+        # Initialise a log config object
+        self.logconf_stateEstimate = LogConfig("LoggingStateEstimate", period_in_ms=cfStateEstimate_polling_period)
+        # Add the varaibles to be logged
+        # > (x,y,z) positions
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.x",    "int16_t");
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.y",    "int16_t");
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.z",    "int16_t");
+        # > (x,y,z) velocities
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.vx",   "int16_t");
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.vy",   "int16_t");
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.vz",   "int16_t");
+        # > (x,y,z) accelerations
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.ax",   "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.ay",   "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.az",   "int16_t");
+        # > (roll,pitch,yaw) angles
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.roll",        "int16_t");
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.pitch",       "int16_t");
+        self.logconf_stateEstimate.add_variable("stateEstimateZ.yaw",         "int16_t");
+        # > (roll,pitch,yaw) anglular rates (direct from gryo)
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.rateRoll",    "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.ratePitch",   "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.rateYaw",     "int16_t");
+        #
+        # Try to add the log config to the crazyflie object
+        try:
+            self._cf.log.add_config(self.logconf_stateEstimate)
+            # Add the callback for received data
+            self.logconf_stateEstimate.data_received_cb.add_callback(self._data_received_stateEstimate_callback)
+            # Add the callback for errors
+            self.logconf_stateEstimate.error_cb.add_callback(self._logging_error)
+            # Check the log config is valid
+            if self.logconf_stateEstimate.valid:
+                # Start the logging
+                self.logconf_stateEstimate.start()
+                rospy.loginfo("[CRAZY RADIO] Started the log config for the state estimate")
+            else:
+                rospy.loginfo("[CRAZY RADIO] The log config for the state estimate is invalid, hence logging has not been started.")
+        # Handle "key error" exceptions:
+        except KeyError as e:
+            rospy.logerr("[CRAZY RADIO] For the state estimate, the following error occurred while trying to add the log config: %s" % str(e) ) 
+        # Handle "attribution error" exceptions:
+        except AttributeError:
+            rospy.logerr("[CRAZY RADIO] The state estimate log config has a bad configuration.")
+
+
+
+    def _connected(self, link_uri):
+        """
+            This callback is executed as soon as the connection to the
+            quadrotor is established.
+        """
+        self.change_status_to(CONNECTED)
+        # change state to motors OFF
+        msg = IntWithHeader()
+        msg.shouldCheckForAgentID = False
+        msg.data = CMD_CRAZYFLY_MOTORS_OFF
+        cf_client.flyingAgentClient_command_publisher.publish(msg)
+
+        rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
+        # Config for Logging
+        self._start_logging()
+
+
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the specified address)"""
+        self.change_status_to(DISCONNECTED)
+        rospy.logerr("[CRAZY RADIO] Connection to %s failed: %s" % (link_uri, msg))
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        self.change_status_to(DISCONNECTED)
+        rospy.logerr("[CRAZY RADIO] Connection to %s lost: %s" % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        self.change_status_to(DISCONNECTED)
+        rospy.logwarn("[CRAZY RADIO] Disconnected from %s" % link_uri)
+
+        # CHANGE THE STATE TO "MOTORS OFF"
+        msg = IntWithHeader()
+        msg.shouldCheckForAgentID = False
+        msg.data = CMD_CRAZYFLY_MOTORS_OFF
+        self.flyingAgentClient_command_publisher.publish(msg)
+
+        # STOP AND DELETE ALL THE "LOGGING GROUPS"
+        # > For the battery voltage
+        self.logconf_battery.stop()
+        rospy.loginfo("The log config for the battery voltage was stopped")
+        self.logconf_battery.delete()
+        rospy.loginfo("The log config for the battery voltage was deleted")
+        # > For the state estimate
+        self.logconf_stateEstimate.stop()
+        rospy.loginfo("The log config for the state estimate was stopped")
+        self.logconf_stateEstimate.delete()
+        rospy.loginfo("The log config for the state estimate was deleted")
+
+
+
+    def _send_to_commander(self, cmd1, cmd2, cmd3, cmd4, x_mode , x_value, y_mode, y_value, z_mode, z_value, yaw_mode, yaw_value ):
+        # CONSTRUCT THE CONDENSED 16-BIT INTEGER THAT ENCODES
+        # THE REQUESTED {x,y,z,yaw} MODES
+        # > Each mode is encoded using 3 bits, stacked together as follows:
+        # ---------------------------------------------------------------
+        # |BIT   | 15 14 13 12 | 11 10  9 | 8  7  6 | 5  4  3 | 2  1  0 |
+        # |DESC. | free bits   | yaw_mode | z_mode  | y_mode  | x_mode  |
+        # ---------------------------------------------------------------
+        # > The python bitwise operators "<<" and "|" are used
+        #   to construct the 16-bit "cumulative" mode
+        modes_as_uint16 = (yaw_mode << 9) | (z_mode << 6) | (y_mode << 3) | (x_mode << 0)
+        #
+        # > To test the behaviour of this command, enter the following
+        #   into a python shell:
+        # >>> modes = (4 << 9) | (2 << 6) | (1 << 3) | (7 << 0)
+        # >>> "{0:b}".format(modes)
+        # > The second line should display the result '100010001111'
+        #   which encodes:
+        # --------------------------------------------------
+        # |BIT    | 11 10  9 | 8  7  6 | 5  4  3 | 2  1  0 |
+        # |O or 1 |  1  0  0 | 0  1  0 | 0  0  1 | 1  1  1 |
+        # |Value  | mode=4   | mode=2  | mode=1  | mode=7  |
+        # --------------------------------------------------
+        #
+        # > To extract a mode, an example code snippet is:
+        # >>> yaw_mode = (modes >> 9) & 7
+        # > The bitwise & operator with the number 7 serves to
+        #   select only the right-hand most 3 bits of (modes >> 9),
+        #   the last 3 bits of the variable modes after is has been
+        #   shifted to the right by 9 bits.
+        # > This syntax works in both python and plain-c
+
+        
+
+        # CONSTRUCT AND SEND THE COMMANDER PACKET
+        # > See hints at end of this file for an explanation
+        #   of the struct packing format, i.e., of '<HHHHHffff'
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.data = struct.pack('<BHHHHHffff', CF_PACKET_DECODER_DFALL_TYPE, modes_as_uint16, cmd1, cmd2, cmd3, cmd4, x_value, y_value, z_value, yaw_value)
+        self._cf.send_packet(pk)
+
+        # NOTES ON THE PACKING OF THE "pk.data" STRUCT
+        # > The "B" is the decoder type packed into 1 btye
+        # > The first "H" is the condense 2-byte integer
+        #   of four onboard controller modes
+        # > The next four "H" are the 2-byte per motor commands
+        # > The four "f" are the setpoints for each onboard controller
+        # > Hence the total size of the data is:
+        #   sizs('HHHHHffff') = 5*2bytes + 4*4bytes = 26bytes
+        # > This leaves 3 spare bytes before reaching the maximum
+        #   of 29 bytes as specified in the comment below
+
+        # DESCRIPTION OF THE CRAZYFLIE PACKETS
+        # > This is taken directly from the file
+        #   "crtp_commander_generic.c" in the
+        #   Crazyflie firmware
+        #
+        # The generic commander format contains a packet type and data that has to be
+        # decoded into a setpoint_t structure. The aim is to make it future-proof
+        # by easily allowing the addition of new packets for future use cases.
+        #
+        # The packet format is:
+        # +------+==========================+
+        # | TYPE |     DATA                 |
+        # +------+==========================+
+        #
+        # The type is defined bellow together with a decoder function that should take
+        # the data buffer in and fill up a setpoint_t structure.
+        # The maximum data size is 29 bytes.
+
+
+
+
+
+    def crazyRadioCommandCallback(self, msg):
+        """Callback to tell CrazyRadio to reconnect"""
+
+        # Initialise a boolean flag that the command is NOT relevant
+        command_is_relevant = False
+
+        # Check the header details of the message for it relevance
+        if (msg.shouldCheckForAgentID == False):
+            command_is_relevant = True
+        else:
+            for this_ID in msg.agentIDs:
+                if (this_ID == m_agentID):
+                    command_is_relevant = True
+                    break
+
+        # Only consider the command if it is relevant
+        if (command_is_relevant):
+            #print "[CRAZY RADIO] received command to: %s" % msg.data
+            if msg.data == CMD_RECONNECT:
+                if self.get_status() == DISCONNECTED:
+                    print "[CRAZY RADIO] received command to CONNECT (current status is DISCONNECTED)"
+                    self.connect()
+                elif self.get_status() == CONNECTING:
+                    print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTING)"
+                    #self.radio_status_publisher.publish(CONNECTING)
+                elif self.get_status() == CONNECTED:
+                    print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTED)"
+                    #self.radio_status_publisher.publish(CONNECTED)
+
+            elif msg.data == CMD_DISCONNECT:
+                if self.get_status() == CONNECTED:
+                    print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTED)"
+                    self.disconnect()
+                elif self.get_status() == CONNECTING:
+                    print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTING)"
+                    #self.radio_status_publisher.publish(CONNECTING)
+                elif self.get_status() == DISCONNECTED:
+                    print "[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
+                    #self.radio_status_publisher.publish(DISCONNECTED)
+
+
+
+    def getCurrentCrazyRadioStatusServiceCallback(self, req):
+        """Callback to service the request for the connection status"""
+        # Directly return the current status
+        return self._status
+
+
+
+    def flyingStateCallback(self, msg):
+        """Callback for keeping track of the flying state"""
+
+        # > This is used to know when to reset the Crazyflie's
+        #   onboard Kalman filter.
+        # > The filter is reset when the state changes from
+        #   "motors off" to something else
+        # > This is required because the onboard state estimate
+        #   tends to drift when the Crazyflie stays on the ground
+        #   for an even a short amount of time
+
+        # The data in this message is always relevant because
+        # of the namespace of the message, hence there is no
+        # need to check the header.
+
+        # Get the new state from the message
+        new_flying_state = msg.data
+
+        # Get the current flying state into a local variable
+        current_flying_state = self._flyingState_of_flyingAgentClient
+
+        # Only consider the command if it is relevant
+        if ( \
+            (current_flying_state==STATE_MOTORS_OFF)
+            and \
+            not(new_flying_state==STATE_MOTORS_OFF) \
+        ):
+            # Call the CrazyRadio function that sets the
+            # paramter for reseting the onboard estimate
+            self._cf.param.set_value("kalman.resetEstimation", 1)
+            # Inform the user
+            #print "[CRAZY RADIO] reqested the Crazyflie to reset the onboard estimate"
+
+        # Update the class variable for keeping track of the
+        # current state
+        self._flyingState_of_flyingAgentClient = new_flying_state
+
+
+# END OF: class CrazyRadioClient:
+# ============================= #
+
+
+# ============================= #
+# CALL BACK WHEN A MESSAGE IS RECEIVED ON:
+# "FlyingAgentClient/ControlCommand"
+def controlCommandCallback(data):
+    """Callback for controller actions"""
+
+    # NOTE: cmd{1,...,4} must NOT be all 0, as this causes the
+    #       crazyflie onboard controller to reset!
+    # TODO: check if this note is still true!
+
+    # CHECK THE X-CONTROLLER MODE
+    # > Because the pitch is inverted on the crazyflie
+    # > And because the angles need to be converted to degrees
+    if ( \
+        (data.xControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+        or \
+        (data.xControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+    ):
+        # Negate the setpoint and convert to degrees
+        x_controller_setpoint = -data.xControllerSetpoint * RAD_TO_DEG;
+    #
+    else:
+        # Take the setpoint as is
+        x_controller_setpoint = data.xControllerSetpoint;
+
+    # CHECK THE Y-CONTROLLER MODE
+    # > Because the pitch is inverted on the crazyflie
+    # > And because the angles need to be converted to degrees
+    if ( \
+        (data.yControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+        or \
+        (data.yControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+    ):
+        # Convert the setpoint to degrees
+        y_controller_setpoint = data.yControllerSetpoint * RAD_TO_DEG;
+    #
+    else:
+        # Take the setpoint as is
+        y_controller_setpoint = data.yControllerSetpoint;
+
+    # CHECK THE Z-CONTROLLER MODE
+    # > Because the angle and angular rate modes are not allowed
+    if ( \
+        (data.zControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+        or \
+        (data.zControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+    ):
+        # Turn the z-controller off and inform the user
+        rospy.logerr("[CRAZY RADIO] z controller requested disallowed mode, now turning OFF the z-controller.")
+        z_controller_mode     = CF_ONBOARD_CONTROLLER_MODE_OFF;
+        z_controller_setpoint = 0.0;
+    #
+    else:
+        # Take the mode and setpoint as is
+        z_controller_mode     = data.zControllerMode;
+        z_controller_setpoint = data.zControllerSetpoint;
+
+    # CHECK THE YAW-CONTROLLER MODE
+    # > Because the yaw is inverted on the crazyflie
+    # > And because the angles need to be converted to degrees
+    # > And because the position and velocity modes are not allowed
+    if ( \
+        (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+        or \
+        (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+    ):
+        # Negate the setpoint and convert to degrees
+        yaw_controller_mode     = data.yawControllerMode;
+        yaw_controller_setpoint = data.yawControllerSetpoint * RAD_TO_DEG;
+    #
+    elif (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_OFF):
+        # Set the setpoint to zero
+        yaw_controller_mode     = data.yawControllerMode;
+        yaw_controller_setpoint = 0.0;
+    #
+    else:
+        # Set the yaw-controller  to zero angle setpoint and inform the user
+        rospy.logerr("[CRAZY RADIO] yaw controller requested disallowed mode, now turning to ANGULAR_RATE mode with zero setpoint.")
+        yaw_controller_mode     = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+        yaw_controller_setpoint = 0.0;
+
+
+    # PASS THE COMMAND TO THE SENDING FUNCTION
+    cf_client._send_to_commander(
+        data.motorCmd1,
+        data.motorCmd2,
+        data.motorCmd3,
+        data.motorCmd4,
+        data.xControllerMode,
+        x_controller_setpoint,
+        data.yControllerMode,
+        y_controller_setpoint,
+        z_controller_mode,
+        z_controller_setpoint,
+        yaw_controller_mode,
+        yaw_controller_setpoint,
+        )
+
+# END OF: def controlCommandCallback(data):
+# ============================= #
+
+
+
+
+
+if __name__ == '__main__':
+
+    # Starting the ROS-node
+    global node_name
+    node_name = "CrazyRadio"
+    rospy.init_node(node_name, anonymous=True)
+
+    # Get the namespace of this node
+    global ros_namespace
+    ros_namespace = rospy.get_namespace()
+
+
+
+    # Initialize the low-level drivers
+    # > with the option set to NOT list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    # Wait until address parameter is set by FlyingAgentClient
+    while not rospy.has_param("~crazyflieAddress"):
+        time.sleep(0.05)
+
+    # Wait until name parameter is set by FlyingAgentClient
+    # > Should be set at the same timie as the address
+    while not rospy.has_param("~crazyflieName"):
+        time.sleep(0.05)
+
+    # Use the following lines of code to connect without data
+    # from the "Central Manager":
+    # radio_address = "radio://0/72/2M"
+    # rospy.loginfo("manual address loaded")
+
+    # Fetch the YAML paramter "battery polling period"
+    global battery_polling_period
+    battery_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioCopyOfBatteryMonitor/battery_polling_period")
+
+    global cfStateEstimate_polling_period
+    cfStateEstimate_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/cfStateEstimate_polling_period")
+
+    global isEnabled_strictSafety
+    isEnabled_strictSafety = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/isEnabled_strictSafety")
+
+    global maxHeight_for_strictSafety_meters
+    maxHeight_for_strictSafety_meters = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/maxHeight_for_strictSafety_meters")
+
+    # Fetch the YAML paramter "agentID" and "coordID"
+    global m_agentID
+    m_agentID = rospy.get_param(ros_namespace + "/FlyingAgentClient/agentID")
+    coordID   = rospy.get_param(ros_namespace + "/FlyingAgentClient/coordID")
+    # Convert the coordinator ID to a zero-padded string
+    coordID_as_string = format(coordID, '03')
+
+    # Initialise a publisher for the battery voltage
+    global cfbattery_pub
+    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
+
+    # Initialise a publisher for the state estimate
+    global cfstateEstimate_pub
+    cfstateEstimate_pub = rospy.Publisher(node_name + '/CFStateEstimate', FlyingVehicleState, queue_size=10)
+
+    # Initialise a "CrazyRadioClient" type variable that handles
+    # all communication over the CrazyRadio
+    global cf_client
+    cf_client = CrazyRadioClient()
+
+    # Subscribe to the commands for when to (dis-)connect the
+    # CrazyRadio connection with the Crazyflie
+    # > For the radio commands from the FlyingAgentClient of this agent
+    rospy.Subscriber("FlyingAgentClient/CrazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
+    # > For the radio command from the coordinator
+    rospy.Subscriber("/dfall/coord" + coordID_as_string + "/FlyingAgentClient/CrazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
+
+
+    # Advertise a Serice for the current status
+    # of the Crazy Radio connect
+    getCurrentCrazyRadioStatusService = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback)
+
+
+    # Subscribe to the "flying state" of the FlyingAgentClient
+    # > This is used to determine when to reset the Crazyflie's
+    #   onboard Kalman filter
+    rospy.Subscriber("FlyingAgentClient/FlyingState", Int32, cf_client.flyingStateCallback)
+
+    # Sleep briefly to allow everything to start-up
+    time.sleep(1.0)
+
+
+
+    # Subscribe to the "control commands" that are to be sent
+    # to the Crazyflie, i.e., commands of motor thrust and
+    # setpoints for the onboard controllers
+    rospy.Subscriber("FlyingAgentClient/ControlCommand", ControlCommand, controlCommandCallback)
+
+
+
+    # Print out some information to the user.
+    print "[CRAZY RADIO] Node READY :-)"
+
+    # Enter an endless while loop to keep the node alive.
+    rospy.spin()
+
+
+
+    # SHUT DOWN THE NODE IF THE "ros::spin" IS CANCELLED
+
+    # Inform the user
+    rospy.loginfo("[CRAZY RADIO] Turning off crazyflie")
+
+    # Send the motors off command
+    cf_client._send_to_commander(0, 0, 0, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0)
+
+    # Send a "Motors OFF" on behalf of the "FlyingAgentClient"
+    # > This serves the purpose of changing the flying state
+    #   to the "Motors OFF" state
+    msg = IntWithHeader()
+    msg.shouldCheckForAgentID = False
+    msg.data = CMD_CRAZYFLY_MOTORS_OFF
+    cf_client.flyingAgentClient_command_publisher.publish(msg)
+
+    # Allow some time for the command to be sent
+    time.sleep(1.0)
+
+
+
+    # Close the ROS loggin bag
+    #bag.close()
+    #rospy.loginfo("[CRAZY RADIO] Logging bag closed")
+
+    # Close the CrazyRadio link
+    cf_client._cf.close_link()
+    rospy.loginfo("[CRAZY RADIO] Link closed")
+
+
+# ===================================== #
+# HINTS FOR THE FORMAT STRING USED IN
+# THE "struct.pack(...)" COMMANDS FOR
+# CONSTRUCTING THE DATA PACKAGES SENT
+# TO THE CRAZYFLIE
+#
+# COPIED FROM:
+# https://docs.python.org/3/library/struct.html#format-characters
+#
+# FORMAT CHARACTERS:
+# Format characters have the following meaning; the conversion
+# between C and Python values should be obvious given their
+# types. The ‘Standard size’ column refers to the size of the
+# packed value in bytes when using standard size; that is, when
+# the format string starts with one of '<', '>', '!' or '='.
+# When using native size, the size of the packed value is
+# platform-dependent.
+#
+# |----------------------------------------------------------|
+# | Format | C Type             | Python type | Size | Notes |
+# |--------------------------------------------------|-------|
+# | x      | pad byte           | no value    | 1    |       |
+# | c      | char               | bytes len 1 | 1    |       |
+# | b      | signed char        | integer     | 1    | (1)(2)|
+# | B      | unsigned char      | integer     | 1    | (2)   |
+# | ?      | _Bool              | bool        | 1    | (1)   |
+# | h      | short              | integer     | 2    | (2)   |
+# | H      | unsigned short     | integer     | 2    | (2)   |
+# | i      | int                | integer     | 4    | (2)   |
+# | I      | unsigned int       | integer     | 4    | (2)   |
+# | l      | long               | integer     | 4    | (2)   |
+# | L      | unsigned long      | integer     | 4    | (2)   |
+# | q      | long long          | integer     | 8    | (2)   |
+# | Q      | unsigned long long | integer     | 8    | (2)   |
+# | n      | ssize_t            | integer     |      | (3)   |
+# | N      | size_t             | integer     |      | (3)   |
+# | e      | (6)                | float       | 2    | (4)   |
+# | f      | float              | float       | 4    | (4)   |
+# | d      | double             | float       | 8    | (4)   |
+# | s      | char[]             | bytes       |      |       |
+# | p      | char[]             | bytes       |      |       |
+# | P      | void *             | integer     |      | (5)   |
+# |--------------------------------------------------|-------|
+#
+# Changed in version 3.3: Added support for the 'n'
+# and 'N' formats.
+#
+# Changed in version 3.6: Added support for the 'e'
+# format.
+#
+# Notes:
+#
+# (1) The '?' conversion code corresponds to the _Bool
+#     type defined by C99. If this type is not available,
+#     it is simulated using a char. In standard mode, it
+#     is always represented by one byte.
+#
+# (2) When attempting to pack a non-integer using any of
+#     the integer conversion codes, if the non-integer has
+#     a __index__() method then that method is called to
+#     convert the argument to an integer before packing.
+#
+#     Changed in version 3.2: Use of the __index__() method
+#     for non-integers is new in 3.2.
+#
+# (3) The 'n' and 'N' conversion codes are only available
+#     for the native size (selected as the default or with
+#     the '@' byte order character). For the standard size,
+#     you can use whichever of the other integer formats
+#     fits your application.
+#
+# (4) For the 'f', 'd' and 'e' conversion codes, the packed
+#     representation uses the IEEE 754 binary32, binary64 or
+#     binary16 format (for 'f', 'd' or 'e' respectively),
+#     regardless of the floating-point format used by the
+#     platform.
+#
+# (5) The 'P' format character is only available for the
+#     native byte ordering (selected as the default or with
+#     the '@' byte order character). The byte order character
+#     '=' chooses to use little- or big-endian ordering based
+#     on the host system. The struct module does not interpret
+#     this as native ordering, so the 'P' format is not
+#     available.
+#
+# (6) The IEEE 754 binary16 “half precision” type was
+#     introduced in the 2008 revision of the IEEE 754 standard.
+#     It has a sign bit, a 5-bit exponent and 11-bit precision
+#     (with 10 bits explicitly stored), and can represent
+#     numbers between approximately 6.1e-05 and 6.5e+04 at
+#     full precision. This type is not widely supported by C
+#     compilers: on a typical machine, an unsigned short can
+#     be used for storage, but not for math operations. See
+#     the Wikipedia page on the half-precision floating-point
+#     format for more information.
+#
+# A format character may be preceded by an integral repeat
+# count. For example, the format string '4h' means exactly the
+# same as 'hhhh'.
+#
+# Whitespace characters between formats are ignored; a count
+# and its format must not contain whitespace though.
+#
+# For the 's' format character, the count is interpreted as
+# the length of the bytes, not a repeat count like for the
+# other format characters; for example, '10s' means a single
+# 10-byte string, while '10c' means 10 characters. If a count
+# is not given, it defaults to 1. For packing, the string is
+# truncated or padded with null bytes as appropriate to make
+# it fit. For unpacking, the resulting bytes object always has
+# exactly the specified number of bytes. As a special case,
+# '0s' means a single, empty string (while '0c' means 0
+# characters).
+#
+# When packing a value x using one of the integer formats
+# ('b', 'B', 'h', 'H', 'i', 'I', 'l', 'L', 'q', 'Q'), if x is
+# outside the valid range for that format then struct.error is
+# raised.
+#
+# Changed in version 3.1: In 3.0, some of the integer formats
+# wrapped out-of-range values and raised DeprecationWarning
+# instead of struct.error.
+#
+# The 'p' format character encodes a “Pascal string”, meaning
+# a short variable-length string stored in a fixed number of
+# bytes, given by the count. The first byte stored is the
+# length of the string, or 255, whichever is smaller. The
+# bytes of the string follow. If the string passed in to
+# pack() is too long (longer than the count minus 1), only
+# the leading count-1 bytes of the string are stored. If the
+# string is shorter than count-1, it is padded with null bytes
+# so that exactly count bytes in all are used. Note that for
+# unpack(), the 'p' format character consumes count bytes,
+# but that the string returned can never contain more than
+# 255 bytes.
+
+# For the '?' format character, the return value is either
+# True or False. When packing, the truth value of the argument
+# object is used. Either 0 or 1 in the native or standard bool
+# representation will be packed, and any non-zero value will
+# be True when unpacking.
+#
+# ===================================== #
\ No newline at end of file

pps_ws/src/d_fall_pps/crazyradio/README.md → dfall_ws/src/dfall_pkg/crazyradio/README.md

@@ -13,26 +13,37 @@ For more info see our [wiki](http://wiki.bitcraze.se/ "Bitcraze Wiki").
 * [Fork the cflib](https://help.github.com/articles/fork-a-repo/)
 * [Clone the cflib](https://help.github.com/articles/cloning-a-repository/), `git clone git@github.com:YOUR-USERNAME/crazyflie-lib-python.git`
 * [Install the cflib in editable mode](http://pip-python3.readthedocs.org/en/latest/reference/pip_install.html?highlight=editable#editable-installs), `pip install -e path/to/cflib` 
-* [Uninstall the cflib](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib`
 
-Note: If you are developing for the [cfclient][cfclient] you must use python3.
+
+* [Uninstall the cflib if you don't want it any more](http://pip-python3.readthedocs.org/en/latest/reference/pip_uninstall.html), `pip uninstall cflib`
+
+Note: If you are developing for the [cfclient][cfclient] you must use python3. On Ubuntu (16.04, 18.08) use `pip3` instead of `pip`.
 
 ### Linux, OSX, Windows
-* Build a [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) with package dependencies
-  * `pip install virtualenv`
-  * `virtualenv venv`
-  * `source venv/bin/activate`
-* `pip install -r requirements.txt`
+
+The following should be executed in the root of the crazyflie-lib-python file tree.
+
+#### Virtualenv
+This section contains a very short description of how to use [virtualenv (local python environment)](https://virtualenv.pypa.io/en/latest/) 
+with package dependencies. If you don't want to use virualenv and don't mind installing cflib dependencies system-wide
+you can skip this section.
+
+* Install virtualenv: `pip install virtualenv`
+* create an environment: `virtualenv venv`
 * Activate the environment: `source venv/bin/activate`
-* Connect the crazyflie and run an example: `python -m examples.basiclog`
-* Deactivate the virtualenv if you activated it `deactivate`
 
-Note: For systems that support [make](https://www.gnu.org/software/make/manual/html_node/Simple-Makefile.html), the first four steps can be replaced with `make venv`
 
-Note: The first three steps can be skipped if you don't mind installing cflib dependencies system-wide.
+* To deactivate the virtualenv when you are done using it `deactivate`
+
+Note: For systems that support [make](https://www.gnu.org/software/make/manual/html_node/Simple-Makefile.html), you can use `make venv` to
+create an environment, activate it and install dependencies.
 
+#### Install cflib dependencies
+Install dependencies required by the lib: `pip install -r requirements.txt`
 
-# Testing
+To verify the installation, connect the crazyflie and run an example: `python examples/basiclog`
+
+## Testing
 ### With docker and the toolbelt
 
 For information and installation of the 
@@ -45,9 +56,12 @@ Note: Docker and the toolbelt is an optional way of running tests and reduces th
 work needed to maintain your python environmet. 
 
 ### Native python on Linux, OSX, Windows
-* [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox`
+ [Tox](http://tox.readthedocs.org/en/latest/) is used for native testing: `pip install tox`
+* If test fails after installing tox with `pip install tox`, installing with  `sudo apt-get install tox`result a successful test run
+
 * Test package in python2.7 `TOXENV=py27 tox`
 * Test package in python3.4 `TOXENV=py34 tox`
+* Test package in python3.6 `TOXENV=py36 tox`
 
 Note: You must have the specific python versions on your machine or tests will fail. (ie. without specifying the TOXENV, `tox` runs tests for python2.7, 3.3, 3.4 and would require all python versions to be installed on the machine.)
 
@@ -62,13 +76,18 @@ The following steps make it possible to use the USB Radio without being root.
 
 ```
 sudo groupadd plugdev
-sudo usermod -a -G plugdev <username>
+sudo usermod -a -G plugdev $USER
 ```
 
+You will need to log out and log in again in order to be a member of the plugdev group.
+
 Create a file named ```/etc/udev/rules.d/99-crazyradio.rules``` and add the
 following:
 ```
+# Crazyradio (normal operation)
 SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="7777", MODE="0664", GROUP="plugdev"
+# Bootloader
+SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="0101", MODE="0664", GROUP="plugdev"
 ```
 
 To connect Crazyflie 2.0 via usb, create a file name ```/etc/udev/rules.d/99-crazyflie.rules``` and add the following:
@@ -76,6 +95,12 @@ To connect Crazyflie 2.0 via usb, create a file name ```/etc/udev/rules.d/99-cra
 SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE="0664", GROUP="plugdev"
 ```
 
+You can reload the udev-rules using the following:
+```
+sudo udevadm control --reload-rules
+sudo udevadm trigger
+```
+
 [cfclient]: https://www.github.com/bitcraze/crazyflie-clients-python