Merge pull request RoboCup-SSL#144 from RoboCup-SSL/feature/simulator…

…-protocol

Add basic implementation of ssl-simulation-protocol

CMakeLists.txt

@@ -111,14 +111,20 @@ include_directories(${PROTOBUF_INCLUDE_DIRS})
 list(APPEND libs ${PROTOBUF_LIBRARIES})
 
 protobuf_generate_cpp(PROTO_CPP PROTO_H
-    src/proto/messages_robocup_ssl_detection.proto
-    src/proto/messages_robocup_ssl_geometry.proto
-    src/proto/messages_robocup_ssl_wrapper.proto
-    src/proto/messages_robocup_ssl_refbox_log.proto
-    src/proto/messages_robocup_ssl_robot_status.proto
-    src/proto/grSim_Replacement.proto
     src/proto/grSim_Commands.proto
     src/proto/grSim_Packet.proto
+    src/proto/grSim_Replacement.proto
+    src/proto/grSim_Robotstatus.proto
+    src/proto/ssl_gc_common.proto
+    src/proto/ssl_simulation_config.proto
+    src/proto/ssl_simulation_control.proto
+    src/proto/ssl_simulation_error.proto
+    src/proto/ssl_simulation_robot_control.proto
+    src/proto/ssl_simulation_robot_feedback.proto
+    src/proto/ssl_simulation_synchronous.proto
+    src/proto/ssl_vision_detection.proto
+    src/proto/ssl_vision_geometry.proto
+    src/proto/ssl_vision_wrapper.proto
 )
 
 qt5_add_resources(RESOURCES

include/configwidget.h

@@ -183,6 +183,9 @@ class ConfigWidget : public VarTreeView
   DEF_VALUE(int,Int,CommandListenPort)
   DEF_VALUE(int,Int,BlueStatusSendPort)
   DEF_VALUE(int,Int,YellowStatusSendPort)
+  DEF_VALUE(int,Int,SimControlListenPort)
+  DEF_VALUE(int,Int,BlueControlListenPort)
+  DEF_VALUE(int,Int,YellowControlListenPort)
   DEF_VALUE(int,Int,sendDelay)
   DEF_VALUE(bool,Bool,noise)
   DEF_VALUE(double,Double,noiseDeviation_x)

include/mainwindow.h

@@ -63,7 +63,13 @@ public slots:
     void reconnectYellowStatusSocket();
     void reconnectBlueStatusSocket();
     void reconnectVisionSocket();
+    void reconnectSimControlSocket();
+    void reconnectBlueControlSocket();
+    void reconnectYellowControlSocket();
     void recvActions();
+    void simControlSocketReady();
+    void blueControlSocketReady();
+    void yellowControlSocketReady();
     void setIsGlEnabled(bool value);
 
     int robotIndex(int robot,int team);
@@ -87,6 +93,9 @@ public slots:
     RoboCupSSLServer *visionServer;
     QUdpSocket *commandSocket;
     QUdpSocket *blueStatusSocket,*yellowStatusSocket;
+    QUdpSocket *simControlSocket;
+    QUdpSocket *blueControlSocket;
+    QUdpSocket *yellowControlSocket;
 };
 
 #endif // MAINWINDOW_H

include/net/robocup_ssl_client.h

@@ -23,10 +23,9 @@
 #define ROBOCUP_SSL_CLIENT_H
 #include <string>
 #include <QMutex>
-#include "messages_robocup_ssl_detection.pb.h"
-#include "messages_robocup_ssl_geometry.pb.h"
-#include "messages_robocup_ssl_wrapper.pb.h"
-#include "messages_robocup_ssl_refbox_log.pb.h"
+#include "ssl_vision_detection.pb.h"
+#include "ssl_vision_geometry.pb.h"
+#include "ssl_vision_wrapper.pb.h"
 using namespace std;
 
 class QUdpSocket;

include/net/robocup_ssl_server.h

@@ -24,9 +24,9 @@
 #include <string>
 #include <QMutex>
 #include <QObject>
-#include "messages_robocup_ssl_detection.pb.h"
-#include "messages_robocup_ssl_geometry.pb.h"
-#include "messages_robocup_ssl_wrapper.pb.h"
+#include "ssl_vision_detection.pb.h"
+#include "ssl_vision_geometry.pb.h"
+#include "ssl_vision_wrapper.pb.h"
 using namespace std;
 
 class QUdpSocket;

include/sslworld.h

@@ -40,7 +40,11 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim)
 
 #include "config.h"
 
-#include "messages_robocup_ssl_robot_status.pb.h"
+#include "grSim_Robotstatus.pb.h"
+#include "ssl_simulation_config.pb.h"
+#include "ssl_simulation_control.pb.h"
+#include "ssl_simulation_robot_control.pb.h"
+#include "ssl_simulation_robot_feedback.pb.h"
 
 #define WALL_COUNT 10
 
@@ -62,10 +66,14 @@ class SSLWorld : public QObject
     dReal last_dt;
     dReal sim_time = 0;
     QList<SendingPacket*> sendQueue;
-    char packet[200]{};
-    char *in_buffer;
     bool lastInfraredState[TEAM_COUNT][MAX_ROBOT_COUNT]{};
     KickStatus lastKickState[TEAM_COUNT][MAX_ROBOT_COUNT]{};
+    void processSimControl(const SimulatorCommand &simulatorCommand, SimulatorResponse &simulatorResponse);
+    void processRobotControl(const RobotControl &robotControl, RobotControlResponse &robotControlResponse, Team team);
+    static void processMoveCommand(RobotControlResponse &robotControlResponse, const RobotMoveCommand &robotCommand,
+                            Robot *robot) ;
+    void processTeleportBall(SimulatorResponse &simulatorResponse, const TeleportBall &teleBall) const;
+    static void processTeleportRobot(const TeleportRobot &teleBot, Robot *robot);
 public:    
     dReal customDT;
     bool isGLEnabled;
@@ -98,11 +106,17 @@ class SSLWorld : public QObject
     RoboCupSSLServer *visionServer{};
     QUdpSocket *commandSocket{};
     QUdpSocket *blueStatusSocket{},*yellowStatusSocket{};
+    QUdpSocket *simControlSocket;
+    QUdpSocket *blueControlSocket;
+    QUdpSocket *yellowControlSocket;
     bool updatedCursor;
     Robot* robots[MAX_ROBOT_COUNT*2]{};
     int sendGeomCount;
 public slots:
     void recvActions();
+    void simControlSocketReady();
+    void blueControlSocketReady();
+    void yellowControlSocketReady();
 signals:
     void fpsChanged(int newFPS);
 };

src/configwidget.cpp

@@ -125,6 +125,9 @@ ConfigWidget::ConfigWidget() {
     ADD_VALUE(comm_vars,Int,CommandListenPort,20011,"Command listen port")
     ADD_VALUE(comm_vars,Int,BlueStatusSendPort,30011,"Blue Team status send port")
     ADD_VALUE(comm_vars,Int,YellowStatusSendPort,30012,"Yellow Team status send port")
+    ADD_VALUE(comm_vars,Int,SimControlListenPort,10300,"Simulation control port")
+    ADD_VALUE(comm_vars,Int,BlueControlListenPort,10301,"Blue team control port")
+    ADD_VALUE(comm_vars,Int,YellowControlListenPort,10302,"Yellow team control port")
     ADD_VALUE(comm_vars,Int,sendDelay,0,"Sending delay (milliseconds)")
     ADD_VALUE(comm_vars,Int,sendGeometryEvery,120,"Send geometry every X frames")
     VarListPtr gauss_vars(new VarList("Gaussian noise"));

src/mainwindow.cpp

@@ -73,21 +73,33 @@ MainWindow::MainWindow(QWidget *parent)
     glwidget->setWindowTitle(tr("Simulator"));
     glwidget->resize(512,512);    
 
-    visionServer = NULL;
-    commandSocket = NULL;
-    blueStatusSocket = NULL;
-    yellowStatusSocket = NULL;
+    visionServer = nullptr;
+    commandSocket = new QUdpSocket(this);
+    blueStatusSocket = nullptr;
+    yellowStatusSocket = nullptr;
+    simControlSocket = new QUdpSocket(this);
+    blueControlSocket = new QUdpSocket(this);
+    yellowControlSocket = new QUdpSocket(this);
     reconnectVisionSocket();
     reconnectCommandSocket();
     reconnectBlueStatusSocket();
     reconnectYellowStatusSocket();
+    reconnectSimControlSocket();
+    reconnectBlueControlSocket();
+    reconnectYellowControlSocket();
 
+    QObject::connect(commandSocket,SIGNAL(readyRead()),this,SLOT(recvActions()));
+    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;
     glwidget->ssl->yellowStatusSocket = yellowStatusSocket;
-
-
+    glwidget->ssl->simControlSocket = simControlSocket;
+    glwidget->ssl->blueControlSocket = blueControlSocket;
+    glwidget->ssl->yellowControlSocket = yellowControlSocket;
 
     robotwidget = new RobotWidget(this, configwidget);
     robotwidget->setObjectName("RobotWidget");
@@ -249,6 +261,9 @@ MainWindow::MainWindow(QWidget *parent)
     QObject::connect(configwidget->v_CommandListenPort.get(), SIGNAL(wasEdited(VarPtr)), this, SLOT(reconnectCommandSocket()));
     QObject::connect(configwidget->v_BlueStatusSendPort.get(), SIGNAL(wasEdited(VarPtr)), this, SLOT(reconnectBlueStatusSocket()));
     QObject::connect(configwidget->v_YellowStatusSendPort.get(), SIGNAL(wasEdited(VarPtr)), this, SLOT(reconnectYellowStatusSocket()));
+    QObject::connect(configwidget->v_SimControlListenPort.get(), SIGNAL(wasEdited(VarPtr)), this, SLOT(reconnectSimControlSocket()));
+    QObject::connect(configwidget->v_BlueControlListenPort.get(), SIGNAL(wasEdited(VarPtr)), this, SLOT(reconnectBlueControlSocket()));
+    QObject::connect(configwidget->v_YellowControlListenPort.get(), SIGNAL(wasEdited(VarPtr)), this, SLOT(reconnectYellowControlSocket()));
     timer->start();
 
 
@@ -365,8 +380,7 @@ void MainWindow::update()
         lvv[2]=vv[2];
     }
 
-    QString ss;
-    fpslabel->setText(QString("Frame rate: %1 fps").arg(ss.sprintf("%06.2f",glwidget->getFPS())));        
+    fpslabel->setText(QString("Frame rate: %1 fps").arg(QString::asprintf("%06.2f",glwidget->getFPS())));        
     if (glwidget->ssl->selected!=-1)
     {
         selectinglabel->setVisible(true);
@@ -429,11 +443,8 @@ void MainWindow::restartSimulator()
     glwidget->ssl = new SSLWorld(glwidget,glwidget->cfg,glwidget->forms[FORMATION_INSIDE_1],glwidget->forms[FORMATION_INSIDE_1]);
     glwidget->ssl->glinit();
     glwidget->ssl->visionServer = visionServer;
-    glwidget->ssl->commandSocket = commandSocket;
     glwidget->ssl->blueStatusSocket = blueStatusSocket;
     glwidget->ssl->yellowStatusSocket = yellowStatusSocket;
-
-
 }
 
 void MainWindow::ballMenuTriggered(QAction* act)
@@ -510,42 +521,56 @@ void MainWindow::showAbout()
 
 void MainWindow::reconnectBlueStatusSocket()
 {
-    if (blueStatusSocket!=NULL)
-    {
-        delete blueStatusSocket;
-    }
+    delete blueStatusSocket;
     blueStatusSocket = new QUdpSocket(this);
-    // if (blueStatusSocket->bind(QHostAddress::Any,configwidget->BlueStatusSendPort()))
-    //     logStatus(QString("Status send port binded for Blue Team on: %1").arg(configwidget->BlueStatusSendPort()),QColor("green"));
 }
 
 void MainWindow::reconnectYellowStatusSocket()
 {
-    if (yellowStatusSocket!=NULL)
-    {
-        delete yellowStatusSocket;
-    }
+    delete yellowStatusSocket;
     yellowStatusSocket = new QUdpSocket(this);
-    // if (yellowStatusSocket->bind(QHostAddress::Any,configwidget->YellowStatusSendPort()))
-    //     logStatus(QString("Status send port binded for Yellow Team on: %1").arg(configwidget->YellowStatusSendPort()),QColor("green"));
 }
 
 void MainWindow::reconnectCommandSocket()
 {
-    if (commandSocket!=NULL)
-    {
-        QObject::disconnect(commandSocket,SIGNAL(readyRead()),this,SLOT(recvActions()));
-        delete commandSocket;
-    }
-    commandSocket = new QUdpSocket(this);
+    commandSocket->disconnectFromHost();
     if (commandSocket->bind(QHostAddress::Any,configwidget->CommandListenPort()))
-        logStatus(QString("Command listen port binded on: %1").arg(configwidget->CommandListenPort()),QColor("green"));
-    QObject::connect(commandSocket,SIGNAL(readyRead()),this,SLOT(recvActions()));
+        logStatus(QString("Command listen port bound on: %1").arg(configwidget->CommandListenPort()),QColor("green"));
+    else
+        logStatus(QString("Command listen port could not be bound on: %1").arg(configwidget->YellowControlListenPort()),QColor("red"));
+}
+
+void MainWindow::reconnectSimControlSocket()
+{
+    simControlSocket->disconnectFromHost();
+    if (simControlSocket->bind(QHostAddress::Any,configwidget->SimControlListenPort()))
+        logStatus(QString("Sim control listen port bound on: %1").arg(configwidget->SimControlListenPort()),QColor("green"));
+    else
+        logStatus(QString("Sim control listen port could not be bound on: %1").arg(configwidget->YellowControlListenPort()),QColor("red"));
+    QObject::connect(simControlSocket,SIGNAL(readyRead()),this,SLOT(simControlSocketReady()));
+}
+
+void MainWindow::reconnectBlueControlSocket()
+{
+    blueControlSocket->disconnectFromHost();
+    if (blueControlSocket->bind(QHostAddress::Any,configwidget->BlueControlListenPort()))
+        logStatus(QString("Blue control listen port bound on: %1").arg(configwidget->BlueControlListenPort()),QColor("green"));
+    else
+        logStatus(QString("Blue control listen port could not be bound on: %1").arg(configwidget->YellowControlListenPort()),QColor("red"));
+}
+
+void MainWindow::reconnectYellowControlSocket()
+{
+    yellowControlSocket->disconnectFromHost();
+    if (yellowControlSocket->bind(QHostAddress::Any,configwidget->YellowControlListenPort()))
+        logStatus(QString("Yellow control listen port bound on: %1").arg(configwidget->YellowControlListenPort()),QColor("green"));
+    else
+        logStatus(QString("Yellow control listen port could not be bound on: %1").arg(configwidget->YellowControlListenPort()),QColor("red"));
 }
 
 void MainWindow::reconnectVisionSocket()
 {
-    if (visionServer == NULL) {
+    if (visionServer == nullptr) {
         visionServer = new RoboCupSSLServer(this);
     }
     visionServer->change_address(configwidget->VisionMulticastAddr());
@@ -558,6 +583,21 @@ void MainWindow::recvActions()
     glwidget->ssl->recvActions();
 }
 
+void MainWindow::simControlSocketReady()
+{
+    glwidget->ssl->simControlSocketReady();
+}
+
+void MainWindow::blueControlSocketReady()
+{
+    glwidget->ssl->blueControlSocketReady();
+}
+
+void MainWindow::yellowControlSocketReady()
+{
+    glwidget->ssl->yellowControlSocketReady();
+}
+
 void MainWindow::setIsGlEnabled(bool value)
 {
   glwidget->ssl->isGLEnabled = value;