Stop the robots when the stop receiving packages for 1 sec

include/sslworld.h

@@ -111,6 +111,10 @@ class SSLWorld : public QObject
     QUdpSocket *simControlSocket;
     QUdpSocket *blueControlSocket;
     QUdpSocket *yellowControlSocket;
+
+    QElapsedTimer elapsedLastPackageBlue;
+    QElapsedTimer elapsedLastPackageYellow;
+
     bool updatedCursor;
     Robot* robots[MAX_ROBOT_COUNT*2]{};
     int sendGeomCount;

src/mainwindow.cpp

@@ -93,7 +93,7 @@
     QObject::connect(simControlSocket,SIGNAL(readyRead()),this,SLOT(simControlSocketReady()));
     QObject::connect(blueControlSocket,SIGNAL(readyRead()),this,SLOT(blueControlSocketReady()));
     QObject::connect(yellowControlSocket,SIGNAL(readyRead()),this,SLOT(yellowControlSocketReady()));
-    
+
     glwidget->ssl->visionServer = visionServer;
     glwidget->ssl->commandSocket = commandSocket;
     glwidget->ssl->blueStatusSocket = blueStatusSocket;
@@ -362,56 +362,86 @@
     return s;
 }
 
 void MainWindow::update()
 {
     if (glwidget->ssl->isGLEnabled) {
         glwidget->ssl->g->enableGraphics();
         glwidget->updateGL();
     } else {
         glwidget->ssl->g->disableGraphics();
         glwidget->step();
     }
 
     int R = robotIndex(glwidget->Current_robot,glwidget->Current_team);
 
     if(0 <= R)
     {
         const dReal* vv = dBodyGetLinearVel(glwidget->ssl->robots[R]->chassis->body);
         static dVector3 lvv;
         dVector3 aa;
         aa[0]=(vv[0]-lvv[0])/configwidget->DeltaTime();
         aa[1]=(vv[1]-lvv[1])/configwidget->DeltaTime();
         aa[2]=(vv[2]-lvv[2])/configwidget->DeltaTime();
         robotwidget->vellabel->setText(QString::number(sqrt(vv[0]*vv[0]+vv[1]*vv[1]+vv[2]*vv[2]),'f',3));
         robotwidget->acclabel->setText(QString::number(sqrt(aa[0]*aa[0]+aa[1]*aa[1]+aa[2]*aa[2]),'f',3));
         lvv[0]=vv[0];
         lvv[1]=vv[1];
         lvv[2]=vv[2];
     }
 
     fpslabel->setText(QString("Frame rate: %1 fps").arg(QString::asprintf("%06.2f",glwidget->getFPS())));        
     if (glwidget->ssl->selected!=-1)
     {
         selectinglabel->setVisible(true);
         if (glwidget->ssl->selected==-2)
         {            
             selectinglabel->setText("Ball");
         }
         else
         {            
             int R = glwidget->ssl->selected%configwidget->Robots_Count();
             int T = glwidget->ssl->selected/configwidget->Robots_Count();
             if (T==0) selectinglabel->setText(QString("%1:Blue").arg(R));
             else selectinglabel->setText(QString("%1:Yellow").arg(R));
         }
     }
     else selectinglabel->setVisible(false);
     vanishlabel->setVisible(configwidget->vanishing());
     noiselabel->setVisible(configwidget->noise());
     cursorlabel->setText(QString("Cursor: [X=%1;Y=%2;Z=%3]").arg(dRealToStr(glwidget->ssl->cursor_x)).arg(dRealToStr(glwidget->ssl->cursor_y)).arg(dRealToStr(glwidget->ssl->cursor_z)));
     statusWidget->update();
+
+    if(glwidget != nullptr && glwidget->ssl != nullptr)
+    {
+        // Stops blue robots from moving if no package has been received for 1 second
+        if(glwidget->ssl->elapsedLastPackageBlue.nsecsElapsed()*1e-9 > 1)
+        {
+            for(int i=0; i < glwidget->cfg->Robots_Count(); ++i)
+            {
+                const int index = glwidget->ssl->robotIndex(i, BLUE-1);
+
+                if(index == -1 || glwidget->ssl->robots[index] == nullptr)
+                    continue;
+
+                glwidget->ssl->robots[index]->resetSpeeds();
+            }
+        }
+        // Stops yellow robots from moving if no package has been received for 1 second
+        if(glwidget->ssl->elapsedLastPackageYellow.nsecsElapsed()*1e-9 > 1)
+        {
+            for(int i=0; i < glwidget->cfg->Robots_Count(); ++i)
+            {
+                const int index = glwidget->ssl->robotIndex(i, YELLOW-1);
+
+                if(index == -1 || glwidget->ssl->robots[index] == nullptr)
+                    continue;
+
+                glwidget->ssl->robots[index]->resetSpeeds();
+            }
+        }
+    }
 }
 
 void MainWindow::updateRobotLabel()
 {
     robotwidget->teamCombo->setCurrentIndex(glwidget->Current_team);

src/sslworld.cpp

@@ -313,6 +313,9 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1
     }
 
     restartRequired = false;
+
+    elapsedLastPackageBlue.start();
+    elapsedLastPackageYellow.start();
 }
 
 int SSLWorld::robotIndex(int robot,int team) {
@@ -705,6 +708,8 @@ void SSLWorld::blueControlSocketReady() {
         robotControlResponse.SerializeToArray(buffer.data(), buffer.size());
         blueControlSocket->writeDatagram(buffer.data(), buffer.size(), datagram.senderAddress(), datagram.senderPort());
     }
+
+    elapsedLastPackageBlue.start();
 }
 
 void SSLWorld::yellowControlSocketReady() {
@@ -723,6 +728,8 @@ void SSLWorld::yellowControlSocketReady() {
         robotControlResponse.SerializeToArray(buffer.data(), buffer.size());
         yellowControlSocket->writeDatagram(buffer.data(), buffer.size(), datagram.senderAddress(), datagram.senderPort());
     }
+
+    elapsedLastPackageYellow.start();
 }