Commit Iniziale

com/roboglue_com.h

@@ -0,0 +1,157 @@
+#ifndef ROBOGLUE_COM_H
+#define ROBOGLUE_COM_H
+
+/*
+ * roboglue_com.cpp
+ *
+ *  Created on: Jan 21, 2019
+ *      Author: emanuele
+ *
+ *  Modulo di comunicazione del software roboglue, è un thread che continua a leggere le ibformazioni del robot
+ * e le scrive nella memoria condivisa tra i moduli, rivece comandi sotto forma di segnali per inviare
+ * comandi URscript al controller
+ *
+ *  20190121 - Prima Scittura, incorpora un thread che si occupa solo della lettura dati,
+ *              ad ora non finzionante.
+ *  20190122 - Il thread di lettura funziona correttamente ed è implementazione di QThread,
+ *              ad ogni nuovo pacchetto letto viene emess il segnale newData();
+ *              la classe readerThread contiene solo la funzione che viene spostata nel thread
+ *              di lettura.
+ *              Impostata la struttura comune a tutti i moduli.
+ *  20190123 - Impostati i segnalli per connessione e disconnessione comandata da GUI.
+ *  20190125 - Corretti alcuni bug che facevano crashare in caso di mancata connessione.
+ *              Aggiunto un parametero nella configurazione che definisce il massimo raggio di reach
+ *              del robot. Sarà sostituito da una tabella completa delle lunghezze dei bracci per
+ *              il calcolo della cinematica inversa nel main.
+ * 20190128 - Inizio scrittura modulo tcpsocket per lo scambio dei consensi.
+ * 20190129 - Continua scrittura del modulo socket, le variabili di scambio con il robot sono
+ *              in una mappa <NOME,VALORE> <std::string,int> per facilitaà di accesso
+ *              (rif documenti URscript get_var(), set_var())
+ * 20190131 - Modifiche alla parte di comunicazione per usare i comandi servoj del robot come
+ *              suggerito nelle librerie di ROS industrial.
+ *              Ricerche iniziate su come usare MoveIT e ros indusrtial con il driver UR per
+ *              la pianificazione e l'invio delle traiettorie. Il problema principale è che
+ *              il driver per UR di ROS è datato e non funziona con il controller versione 5.2
+ *              ROS indusrtial inoltre non è ancora compatibile con l'ultima versione di  ROS
+ *              e ubuntu 18.04. Altervativa valida porebbe essere usare solo MoveIT, vedrò.
+ * 20190208 - Implementazione della comunicazione ai moduli ros con mqtt
+ * 20190211 - Continua implementazione della comunicazione ai moduli ros (non ancora scritto)
+ *              con mqtt. Tra Qt e ROS ci saranno 3 code "bidirezionali": COMANDO, COORDINATE, STATO
+ *              per separare i flussi dei messaggi.
+ *              I messaggi sono strutturati come json, fare riferimento al file roboglue_messaggi.txt
+ *              per la specifica di formato.
+ *              I seganli che arrivano portando i messaggi sono strutturati diversamente a seconda
+ *              della coda di destinazione.
+ * 20190311 - La pubblicazione dei messaggi di coordinate verso ros deve essere a rateo costante
+ *              indipendentemente dalla frequenza di ingresso
+ */
+
+#include <QObject>
+#include <QThread>
+#include <QTcpServer>
+#include <QTcpSocket>
+#include <QJsonObject>
+#include <QJsonDocument>
+#include <QList>
+#include <QTimer>
+#include <ctime>
+#include <arpa/inet.h>
+#include <boost/thread.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/algorithm/clamp.hpp>
+#include <boost/chrono.hpp>
+#include <spdlog/spdlog.h>
+#include <spdlog/sinks/stdout_sinks.h>
+#include <mqtt/async_client.h>
+#include "mqtt_callback.h"
+#include <libURcom/URCLinterface.h>
+#include <shared/roboglue_shared.h>
+
+#define COMMAND 0
+#define VARNAME 1
+#define VALUE   2
+
+class RoboGlue_COM: public QThread {
+  Q_OBJECT
+
+  public:
+      RoboGlue_COM(RoboGlue_SHARED* mem = nullptr);
+      void run() override;
+
+  private:
+      shared_ptr<spdlog::logger> liblog;
+      shared_ptr<spdlog::logger> modlog;
+
+      RoboGlue_SHARED *m;
+      clientData_t localData;
+      QThread *readerThr = nullptr;
+      QTcpServer *server = nullptr;
+      QTcpSocket *robotIN = nullptr;
+      mqtt_callback *callback = nullptr;
+      mqtt::async_client* client = nullptr;
+      json baseMsg;
+      std::string robotRequest;
+      UR_CLinterface *interface = nullptr;
+
+      QTimer* coordSendTimer = nullptr;
+      QList<double> lastCoord;
+      bool isCoordNew = false;
+
+      bool isRunning = true;
+      bool isConnected = false;
+      bool isReading = false;
+      bool isNew = false;
+      bool isFirstMessage =  true;
+      bool robotConnected = false;
+
+      void saveKinematicInfo();
+      void saveConnectionInfo();
+      void processRobotRequest(std::string);
+
+      void MQTTpublish(json msg, std::string topic);
+
+  signals:
+      void com_newData();
+      void com_heartBeat();
+      void com_newROScommand(QString, QVariantMap);
+
+  private slots:
+      void on_internalRobotConnect();
+      void on_internalNewData();
+      void timed_sendCoordinates();
+
+  public slots:
+      void on_quit();
+      void on_commonStatusChange(void);
+      void on_connect(QString ip = "", uint port = 0, uchar retry = 1);
+      void on_disconnect();
+      void on_sendURscript(QString command);
+      void on_sendROScommand(QString, QVariantMap);
+      void on_sendROScoordinates(QList<double> sp);
+      void on_sendROSstate(QMap<std::string, bool> digital, QMap<std::string, uint16_t> analog);
+
+      //void on_sendSetpointOLD(QList<double> sp);
+};
+
+
+class readerThread: public QThread{
+    Q_OBJECT
+
+public:
+    readerThread(bool* r, UR_CLinterface* i, clientData_t* d){
+        this->isReading=r;
+        this->interface=i;
+        this->dataOut=d;
+    }
+    bool* isReading;
+    UR_CLinterface* interface;
+    clientData_t* dataOut;
+
+    void run() override;
+    void loopRead();
+
+signals:
+    void newDataAvailable();
+};
+
+#endif // ROBOGLUE_COM_H