doesnt compile, but close. Problem is insertItem in table

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

@@ -5,14 +5,15 @@
 #include "crazyFly.h"
 #include "crazyFlyZone.h"
 
+#include <QTableWidget>
+
 class CFLinker
 {
 public:
-    explicit CFLinker();
+    explicit CFLinker(QTableWidget* p_table);
     ~CFLinker();
 
-    void link(crazyFly* crazyfly, crazyFlyZone* crazyfly_zone);
-    void link(crazyFlyZone* crazyfly_zone, crazyFly* crazyfly);
+    void link(int student_id, crazyFly* crazyfly, crazyFlyZone* crazyfly_zone);
 
     void unlink(crazyFly* crazyfly,  crazyFlyZone* crazyfly_zone);
 
@@ -24,6 +25,9 @@ private:
     };
 
     std::vector<struct link> links;
+
+    // table
+    QTableWidget* m_p_table;
 };
 
 

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

@@ -34,12 +34,18 @@ public:
 
     void setScaleCFs(double scale);
 
+    bool isOccluded();
+
     // linking stuff
     void assignCFZone(int cf_zone_index);
     void removeAssigned();
     bool isAssigned();
+    bool isAddedToScene();
+    void setAddedToScene(bool added);
 
 private:
+    // item added to scene already:
+    bool m_added_to_scene;
 
     // info to fill by message
     std::string m_name;
@@ -51,6 +57,8 @@ private:
     qreal m_pitch;
     qreal m_yaw;
 
+    bool m_occluded;
+
     // info for plotting CF
     qreal m_width;
     qreal m_height;

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

 #include "CFLinker.h"
 
 
-CFLinker::CFLinker()
+CFLinker::CFLinker(QTableWidget* p_table)
 {
+    m_p_table = p_table;
 }
 
 CFLinker::~CFLinker()
 {
 }
 
-void CFLinker::link(crazyFly* crazyfly, crazyFlyZone* crazyfly_zone)
+void CFLinker::link(int student_id, crazyFly* crazyfly, crazyFlyZone* crazyfly_zone)
 {
     struct link tmp_link;
 
@@ -19,12 +20,15 @@ void CFLinker::link(crazyFly* crazyfly, crazyFlyZone* crazyfly_zone)
     crazyfly_zone->linkCF(tmp_link.cf_name);
     crazyfly->assignCFZone(tmp_link.cf_zone_index);
 
-    links.push_back(tmp_link);
-}
+    m_p_table->insertRow(m_p_table->rowCount());
+    QString str_id = QString::number(student_id);
+    m_p_table->insertItem(m_p_table->rowCount() - 1, 0, new QTableWidgetItem(str_id));
+    QString str_cf_name = QString::fromStdString(crazyfly->getName());
+    m_p_table->insertItem(m_p_table->rowCount() - 1, 1, new QTableWidgetItem(str_cf_name));
+    QString str_cf_zone_index = QString::number(crazyfly_zone->getIndex());
+    m_p_table->insertItem(m_p_table->rowCount() - 1, 2, new QTableWidgetItem(str_cf_zone_index));
 
-void CFLinker::link(crazyFlyZone* crazyfly_zone, crazyFly* crazyfly)
-{
-    link(crazyfly, crazyfly_zone);
+    links.push_back(tmp_link);
 }
 
 void CFLinker::unlink(crazyFly* crazyfly, crazyFlyZone* crazyfly_zone)

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

@@ -7,16 +7,28 @@
 crazyFly::crazyFly(const CrazyflieData* p_crazyfly_msg, QGraphicsItem * parent)
     : QGraphicsSvgItem(":/images/drone_fixed.svg")
 {
+
     updateCF(p_crazyfly_msg);
     m_width = DRONE_WIDTH;
     m_height = DRONE_HEIGHT;
     m_assigned = false;
+    m_added_to_scene = false;
 }
 
 crazyFly::~crazyFly()
 {
 }
 
+bool crazyFly::isAddedToScene()
+{
+    return m_added_to_scene;
+}
+
+void crazyFly::setAddedToScene(bool added)
+{
+    m_added_to_scene = added;
+}
+
 void crazyFly::setScaleCFs(double scale)
 {
     this->setScale(scale);
@@ -30,16 +42,28 @@ std::string crazyFly::getName()
 
 void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
 {
+    m_occluded = p_crazyfly_msg->occluded;
+
     m_name = p_crazyfly_msg->crazyflieName;
-    m_x = p_crazyfly_msg->x;
-    m_y = p_crazyfly_msg->y;
-    m_z = p_crazyfly_msg->z;
-
-    m_yaw = p_crazyfly_msg->yaw;
-    m_pitch = p_crazyfly_msg->pitch;
-    m_roll = p_crazyfly_msg->roll;
-    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
+    if(!m_occluded)             //if it is occluded, the info we got is useless
+    {
+        m_x = p_crazyfly_msg->x;
+        m_y = p_crazyfly_msg->y;
+        m_z = p_crazyfly_msg->z;
+
+        m_yaw = p_crazyfly_msg->yaw;
+        m_pitch = p_crazyfly_msg->pitch;
+        m_roll = p_crazyfly_msg->roll;
+
+
+        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
+    }
+}
+
+bool crazyFly::isOccluded()
+{
+    return m_occluded;
 }
 
 

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

@@ -28,7 +28,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
     QMainWindow(parent),
     ui(new Ui::MainGUIWindow)
     #ifdef CATKIN_MAKE
-    ,cf_linker()
+    ,cf_linker(ui->table_links)
     #endif
 {
     #ifdef CATKIN_MAKE
@@ -152,18 +152,18 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
         for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
         {
             scene->removeItem(markers_vector[i]); // remove objects from scene
-            ROS_INFO_STREAM("element index: " << i << " removed");
+            // ROS_INFO_STREAM("element index: " << i << " removed");
         }
         markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end()); //delete them
     }
 
-    ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
+    // ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
 
     for(int i = 0; i < p_msg->markers.size(); i++) // here, or new markers message is equal to current messages, or greater (some new markers)
     {
         if(i >= markers_vector.size()) //some new markers coming
         {
-            ROS_INFO_STREAM("element index: " << i << " added");
+            // ROS_INFO_STREAM("element index: " << i << " added");
             Marker* tmp_p_marker = new Marker(&(p_msg->markers[i]));
             markers_vector.push_back(tmp_p_marker); // what happens with the new indexes? check if this is correct
 
@@ -178,7 +178,7 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
         }
         else
         {
-            ROS_INFO_STREAM("element index: " << i << " moved, already existed");
+            // ROS_INFO_STREAM("element index: " << i << " moved, already existed");
             markers_vector[i]->updateMarker(&(p_msg->markers[i]));
         }
     }
@@ -204,18 +204,38 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
         {
             crazyflies_vector[index_name_found]->updateCF(&(p_msg->crazyflies[i]));
         }
-        else
+        else                    //name not found, newly arrived, add it to the vector
         {
             crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]));
-            if(ui->checkBox_vicon_crazyflies->checkState() == Qt::Checked)
+            crazyflies_vector.push_back(tmp_p_crazyfly);
+        }
+
+        if(ui->checkBox_vicon_crazyflies->checkState() == Qt::Checked)
+        {
+            for(int i = 0; i < crazyflies_vector.size(); i++) //check for occlussion
             {
-                scene->addItem(tmp_p_crazyfly);
+                if(crazyflies_vector[i]->isOccluded())
+                {
+                    ROS_INFO("===================OCCLUDED");
+                    if(crazyflies_vector[i]->isAddedToScene())
+                    {
+                        scene->removeItem(crazyflies_vector[i]);
+                        crazyflies_vector[i]->setAddedToScene(false);
+                    }
+                }
+                else
+                {
+                    if(!crazyflies_vector[i]->isAddedToScene())
+                    {
+                        scene->addItem(crazyflies_vector[i]);
+                        crazyflies_vector[i]->setAddedToScene(true);
+                    }
+                }
             }
-            crazyflies_vector.push_back(tmp_p_crazyfly);
         }
     }
 
-    // in this loop, clean the ones that are not present anymore
+    // in this loop, clean the ones that are not present anymore. UPDATE: this will apparently only happen when we tick and untick in Vicon
     int crazyfly_vector_size_after = crazyflies_vector.size();
 
     for(int j = 0; j < crazyfly_vector_size_after; j++)
@@ -441,7 +461,11 @@ void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
         #ifdef CATKIN_MAKE
         for(int i = 0; i < crazyflies_vector.size(); i++)
         {
-            scene->addItem(crazyflies_vector[i]);
+            if(!crazyflies_vector[i]->isAddedToScene())
+            {
+                scene->addItem(crazyflies_vector[i]);
+                crazyflies_vector[i]->setAddedToScene(true);
+            }
         }
         #endif
         ui->scaleSpinBox->setEnabled(true);
@@ -451,7 +475,11 @@ void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
         #ifdef CATKIN_MAKE
         for(int i = 0; i < crazyflies_vector.size(); i++)
         {
-            scene->removeItem(crazyflies_vector[i]);
+            if(crazyflies_vector[i]->isAddedToScene())
+            {
+                scene->removeItem(crazyflies_vector[i]);
+                crazyflies_vector[i]->setAddedToScene(false);
+            }
         }
         #endif
         ui->scaleSpinBox->setEnabled(false);

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

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>866</width>
-    <height>563</height>
+    <height>587</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -337,6 +337,12 @@
            </item>
            <item row="10" column="1" colspan="2">
             <widget class="QTableWidget" name="table_links">
+             <property name="minimumSize">
+              <size>
+               <width>273</width>
+               <height>0</height>
+              </size>
+             </property>
              <property name="wordWrap">
               <bool>true</bool>
              </property>

pps_ws/src/d_fall_pps/msg/CrazyflieData.msg

@@ -6,3 +6,5 @@ float64 roll
 float64 pitch
 float64 yaw
 float64 acquiringTime #delta t
+bool occluded
+

pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp

@@ -80,6 +80,9 @@ int main(int argc, char* argv[]) {
         i++;
         // TODO: Fake CF data
         CrazyflieData crazyfly;
+
+        crazyfly.occluded = false;
+
         crazyfly.crazyflieName = "CF1";
         crazyfly.x = 0;
         crazyfly.y = 0;
@@ -94,16 +97,20 @@ int main(int argc, char* argv[]) {
         crazyfly.yaw = -3.14159 * f;
         viconData.crazyflies.push_back(crazyfly);
 
-        if(i > 50 && i < 100)
+        crazyfly.crazyflieName = "CF3";
+        crazyfly.x = 1;
+        crazyfly.y = -1;
+        crazyfly.z = 0;
+        crazyfly.yaw = -3.14159 * f;
+
+
+        if(i > 50 && i < 200)
         {
-            crazyfly.crazyflieName = "CF3";
-            crazyfly.x = 1;
-            crazyfly.y = -1;
-            crazyfly.z = 0;
-            crazyfly.yaw = -3.14159 * f;
-            viconData.crazyflies.push_back(crazyfly);
+            crazyfly.occluded = true;
         }
 
+        viconData.crazyflies.push_back(crazyfly);
+
         viconDataPublisher.publish(viconData); // testing data
     }
     #else
@@ -153,7 +160,7 @@ int main(int argc, char* argv[]) {
         unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;
 
         UnlabeledMarker marker;
-        ROS_INFO_STREAM("unlabeledMarkerCount: " << unlabeledMarkerCount);
+        // ROS_INFO_STREAM("unlabeledMarkerCount: " << unlabeledMarkerCount);
 
         for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
         {
@@ -210,6 +217,8 @@ int main(int argc, char* argv[]) {
             CrazyflieData cfData;
             cfData.crazyflieName = subjectName;
 
+            cfData.occluded = outputTranslation.Occluded;
+
             cfData.x = outputTranslation.Translation[0] / 1000.0f;
             cfData.y = outputTranslation.Translation[1] / 1000.0f;
             cfData.z = outputTranslation.Translation[2] / 1000.0f;