Commit Iniziale

com/RoboGlue_Publisher.cpp

@@ -0,0 +1,25 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "RoboGlue_Publisher");
+    ros::NodeHandle nh;
+
+    ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
+
+    ros::Rate loop_rate(10);
+    while (ros::ok())
+    {
+        std_msgs::String msg;
+        msg.data = "hello world";
+
+        chatter_pub.publish(msg);
+
+        ros::spinOnce();
+
+        loop_rate.sleep();
+    }
+
+    return 0;
+}

com/mqtt_callback.cpp

@@ -0,0 +1,25 @@
+#include "mqtt_callback.h"
+
+mqtt_callback::mqtt_callback(mqtt::async_client *cli, std::shared_ptr<spdlog::logger> log) {
+    //initialize client pointer
+ cli_=cli;
+ log_=log;
+}
+
+void mqtt_callback::connected(const mqtt::string &cause) {
+    log_->info("MQTT client Connected: {}", cause.c_str());
+}
+
+void mqtt_callback::connection_lost(const mqtt::string &cause) {
+    log_->info("MQTT client Disconnected {}", cause.c_str());
+}
+
+void mqtt_callback::message_arrived(mqtt::const_message_ptr msg) {
+    log_->debug("Message Arrived: topic->{} - message->{}",
+               msg->get_topic(), msg->to_string());
+}
+
+void mqtt_callback::delivery_complete(mqtt::delivery_token_ptr tok) {
+    log_->trace("Message Delivery complete \n tok: {}", tok->get_message_id());
+}
+

com/mqtt_callback.h

@@ -0,0 +1,23 @@
+#ifndef MQTT_CALLBACK_H
+#define MQTT_CALLBACK_H
+
+#include <mqtt/client.h>
+#include <mqtt/callback.h>
+#include <spdlog/logger.h>
+
+class mqtt_callback : public virtual mqtt::callback {
+
+private:
+    mqtt::async_client *cli_ = nullptr;
+    std::shared_ptr<spdlog::logger> log_;
+
+public:
+    mqtt_callback(mqtt::async_client *cli, std::shared_ptr<spdlog::logger> log);
+
+    void connected(const mqtt::string &cause) override;
+    void connection_lost(const mqtt::string &cause) override;
+    void message_arrived(mqtt::const_message_ptr msg) override;
+    void delivery_complete(mqtt::delivery_token_ptr tok) override;
+};
+
+#endif

com/roboglue_com.cpp

@@ -0,0 +1,390 @@
+#include "roboglue_com.h"
+
+RoboGlue_COM::RoboGlue_COM (RoboGlue_SHARED *mem): m(mem) {
+
+    //////// SETUP LOGGER //////////
+    liblog = spdlog::stdout_logger_mt("URcom_liblog");
+    modlog = spdlog::stdout_logger_mt("RoboGlue_comlog");
+    liblog->set_level(spdlog::level::info);
+    modlog->set_level(spdlog::level::debug);
+
+    ///////// INITIALIZE TCP SERVER ///////////
+    server = new QTcpServer(this);
+    server->listen(QHostAddress::Any, 9000);
+    connect(server, SIGNAL(newConnection()), this, SLOT(on_internalRobotConnect()));
+    modlog->info("COM started");
+
+    //////// INITIALIZE MQTT CONNECTOR ///////
+    client = new mqtt::async_client(std::string("tcp://localhost:1883"),"");
+    callback = new mqtt_callback(client,modlog);////////////////////////////////////////////////
+    //////// END EXTERNAL PUBLIC SLOTS /////////////
+    ////////////////////////////////////////////////
+    baseMsg = m->commonSettings.baseMessage;
+    baseMsg["command"]=nullptr;
+    baseMsg["params"]=nullptr;
+
+    //////// INITIALIZE TIMED CALLBACK FUNCTIONS //////
+    coordSendTimer = new QTimer(this);
+    connect(coordSendTimer, SIGNAL(timeout()), this, SLOT(timed_sendCoordinates()));
+}
+
+void RoboGlue_COM::run() {
+    unsigned char attempt = 0;
+
+    /// START MQTT CLIENT ////
+    client->set_callback(*callback);
+    client->connect()->wait();
+    // subscrbe to mqtt topics
+    std::vector<std::string> subList = m->comSettings.subList;
+    for (uint i=0; i < subList.size(); i++){
+        client->subscribe(subList[i],0);
+    }
+    client->start_consuming();
+
+    /// AUTOCONNECT ROBOT INTERFACE ///
+    if(m->comSettings.connection.autoConnect) this->on_connect(QString(m->comSettings.connection.robotIp.c_str()),
+                                                               m->comSettings.connection.robotPort,
+                                                               m->comSettings.connection.retry);
+    /// THREAD LOOP ///
+    while(this->isRunning){
+        // if host is not connected and the reader thread is stopped do nothing
+        if (!this->isConnected & !this->isReading) {
+            msleep(100);
+        }
+        // if host is connected but reader thread is not started, start it
+        else if (this->isConnected & !this->isReading) {
+            this->isConnected=false;
+            // create new robot interface
+            interface = new UR_CLinterface(liblog);
+            // try connect to robot
+            while (!this->isConnected & (attempt++ < m->comSettings.connection.retry)){
+                attempt = 0;
+                modlog->debug("Connection Attempt:{0}",attempt);
+                try {
+                this->isConnected = interface->connect(m->comSettings.connection.robotIp.c_str(),
+                                                       m->comSettings.connection.robotPort);
+                } catch (boost::system::system_error &e){
+                    modlog->error("Connection Failed:\n {}", e.what());
+                }
+            }
+            if (this->isConnected & ! this->isReading) {
+                // create new reader thread and connect new data signal
+                modlog->info("Interface Connected");
+                readerThr = new readerThread(&this->isReading, interface, &localData);
+                // connect readerThread newdata signal
+                connect(readerThr, SIGNAL(newDataAvailable()), this, SLOT(on_internalNewData()));
+                readerThr->start();
+                modlog->debug("ReaderThread is Working");
+                this->isReading = true;
+            } else modlog->error("Connection failed");
+        }
+        // if host is connected and reader is running, do nothing
+        else if (this->isConnected & this->isReading) {
+            if (robotConnected){
+                if(robotIN->bytesAvailable()){
+                    robotRequest = robotIN->readAll().toStdString();
+                    robotRequest.pop_back();
+                    modlog->info("robotMessage {}", robotRequest.c_str());
+                    processRobotRequest(robotRequest);
+                }
+            }
+            emit com_heartBeat();
+            msleep(10);
+        }
+        // if host is to be disconnected but still reading, stop reading and disconnect host
+        else if (!this->isConnected & this->isReading){
+            // disable reading, should stop reader thread, disconnect new data signal
+            disconnect(readerThr, SIGNAL(newDataAvailable()), this, SLOT(on_internalNewData()));
+            this->isReading = false;
+            readerThr->wait();
+            modlog->debug("ReaderThread is Stopped");
+            // wait until thread exits
+            this->isConnected=false;
+            interface->disconnect();
+            modlog->info("Interface Disconnected");
+            // schedule readerThread for deletion;
+            readerThr->deleteLater();
+            // delete and rebuild interface for next connection;
+            delete interface;
+            interface = nullptr;
+        }
+    }
+    if (isConnected) {
+        robotIN->disconnectFromHost();
+        robotIN->deleteLater();
+        server->deleteLater();
+        on_disconnect();
+        readerThr->wait();
+        coordSendTimer->stop();
+    }
+    if (interface != nullptr) delete interface;
+    if (coordSendTimer != nullptr) delete coordSendTimer;
+    spdlog::drop("URcom_liblog");
+    spdlog::drop("RoboGlue_comlog");
+}
+
+
+/////////////////////////////////////////////////
+//////////////////// READER THREAD //////////////
+////////////////////////////////////////////////
+void readerThread::run(){
+    this->loopRead();
+}
+void readerThread::loopRead() {
+    while(*this->isReading){
+        interface->readData();
+        interface->parseData();
+        *dataOut=interface->clientData;
+        // emit signal every new data received
+        emit newDataAvailable();
+    }
+}
+/////////////////////////////////////////////////
+///////////////END READER THREAD////////////////
+////////////////////////////////////////////////
+
+////////////////////////////////////////////////
+/////////////// MQTT FUNCTIONS /////////////////
+////////////////////////////////////////////////
+void RoboGlue_COM::MQTTpublish(json msg, string topic) {
+    std::string msgstr = msg.dump();
+    mqtt::token_ptr tok = client->publish(mqtt::make_message(topic,msgstr));
+    modlog->trace("Published: id:{0}, topic:{1}, msg:{2}",
+                  tok->get_message_id(), tok->get_topics(), msgstr);
+}
+////////////////////////////////////////////////
+////////////END MQTT FUNCTIONS /////////////////
+////////////////////////////////////////////////
+
+void RoboGlue_COM::saveKinematicInfo() {
+    m->mutex.lock();
+    m->settings->beginGroup("robot");
+        m->settings->beginGroup("kine");
+            m->settings->beginWriteArray("dhtable");
+            for (int i=0; i<JOINT_NUMBER; i++) {
+                m->settings->setArrayIndex(i);
+                m->settings->setValue("dha", localData.configuration.kinematics.DHa[i]);
+                m->settings->setValue("dhd", localData.configuration.kinematics.DHd[i]);
+                m->settings->setValue("dhalpha", localData.configuration.kinematics.DHalpha[i]);
+                m->settings->setValue("dhtheta", localData.configuration.kinematics.DHtheta[i]);
+            }
+            m->settings->endArray();
+        m->settings->endGroup();
+    m->settings->endGroup();
+    m->mutex.unlock();
+}
+
+void RoboGlue_COM::saveConnectionInfo() {
+    m->mutex.lock();
+    m->settings->beginGroup("robot");
+        m->settings->beginGroup("connection");
+            m->settings->setValue("robotip",QString(m->comSettings.connection.robotIp.c_str()));
+            m->settings->setValue("robotport",m->comSettings.connection.robotPort);
+            m->settings->setValue("robotretry",m->comSettings.connection.retry);
+            m->settings->setValue("autoconnect",m->comSettings.connection.autoConnect);
+        m->settings->endGroup();
+    m->settings->endGroup();
+    m->mutex.unlock();
+}
+
+void RoboGlue_COM::processRobotRequest(std::string request) {
+    std::string response;
+    std::string varName;
+    std::string varValue;
+    std::vector<std::string> tokes;
+
+    m->mutex.lock();
+    // split command into tokes
+    boost::algorithm::split(tokes,request,boost::is_any_of(" "));
+    // process requested action
+    if (tokes[COMMAND] == "GET"){
+        modlog->debug("Robot GET Request");
+        varName = tokes[VARNAME];
+        response = varName + " ";
+        try {
+            response += std::to_string(m->robotVariables.at(varName));
+        } catch (std::out_of_range &e) {
+            modlog->error("{0}:{1}",e.what(), varName);
+        }
+        response.append("\n");
+    } else
+    if (tokes[COMMAND] == "SET") {
+        modlog->debug("Robot SET Request");
+        varName = tokes[VARNAME];
+        varValue = tokes[VALUE];
+        try {
+            // if variable does not exist insert it into map
+            m->robotVariables[varName] = std::stoi(varValue.c_str());
+        } catch (std::out_of_range &e) {
+            modlog->error("{0}:{1}",e.what(), varName);
+        }
+    } else {
+        modlog->debug("Robot GENERAL Request");
+        response = "generalrequest";
+        response.insert(0,&m->comSettings.connection.prefix);
+        response.append(&m->comSettings.connection.suffix);
+    }
+    m->mutex.unlock();
+    robotIN->write(response.c_str());
+    robotIN->flush();
+}
+
+////////////////////////////////////////////////
+////////////INTERNAL PRIVATE SLOTS//////////////
+////////////////////////////////////////////////
+void RoboGlue_COM::on_internalRobotConnect() {
+    if(robotIN != nullptr) {
+        robotConnected = false;
+        delete robotIN;
+    }
+    modlog->info("Robot request connected");
+    robotIN = server->nextPendingConnection();
+    robotConnected = true;
+}
+
+void RoboGlue_COM::on_internalNewData(){
+    this->m->mutex.lock();
+    m->robotData=this->localData;
+    this->m->mutex.unlock();
+    emit com_newData();
+    // save DH matrix data;
+    if(isFirstMessage) {
+        this->saveKinematicInfo();
+        isFirstMessage = false;
+    }
+}
+
+void RoboGlue_COM::timed_sendCoordinates(){
+    QList<double> sp = this->lastCoord;
+    if (this->isCoordNew)
+        this->isCoordNew = false;
+    if (!sp.empty()){
+        modlog->trace("timedSendCoordinate");
+        baseMsg.clear();
+        baseMsg["type"] = "cartesian";
+        baseMsg["mode"] = "pos";
+        baseMsg["value"] = json();
+        baseMsg["value"]["x"] = sp[0];
+        baseMsg["value"]["y"] = sp[1];
+        baseMsg["value"]["z"] = sp[2];
+        baseMsg["value"]["rx"] = sp[3];
+        baseMsg["value"]["ry"] = sp[4];
+        baseMsg["value"]["rz"] = sp[5];
+        MQTTpublish(baseMsg, m->comSettings.pubList[m->comSettings.COORD]);
+    }
+}
+////////////////////////////////////////////////
+////////END INTERNAL PRIVATE SLOTS//////////////
+////////////////////////////////////////////////
+
+void RoboGlue_COM::on_commonStatusChange() {
+    if (m->commonStatus.isRecording || m->commonStatus.isRealtime) {
+        if (!coordSendTimer->isActive()) coordSendTimer->start(100);
+    }
+    if (!m->commonStatus.isRecording && !m->commonStatus.isRealtime){
+        if (coordSendTimer->isActive()) coordSendTimer->stop();
+    }
+}
+
+void RoboGlue_COM::on_quit() {
+    this->isConnected=false;
+    client->disconnect();
+    while(this->isReading) msleep(100);
+    this->isRunning = false;
+}
+
+void RoboGlue_COM::on_connect(QString ip, uint port, uchar retry) {
+    modlog->debug("robotConnect Received");
+    if (!isConnected){
+        this->isConnected=true;
+        this->isReading=false;
+        this->isFirstMessage=true;
+        // update connection information for the thread
+        m->comSettings.connection.robotIp = ip.toStdString();
+        m->comSettings.connection.robotPort = port;
+        m->comSettings.connection.retry = retry;
+        // save connection info
+        saveConnectionInfo();
+    }
+}
+
+void RoboGlue_COM::on_disconnect() {
+    modlog->debug("robotDisconnect Received");
+    if(isConnected){
+        this->isConnected = false;
+    }
+}
+
+void RoboGlue_COM::on_sendURscript(QString command) {
+    modlog->debug("robotSendURscript Received");
+    if(isConnected){
+        //command sent directly trough interface, NOT ROS
+        this->interface->sendCommand(command.toStdString());
+    }
+}
+
+void RoboGlue_COM::on_sendROScommand(QString command, QVariantMap params) {
+    modlog->debug("robotSendCommand Received");
+    baseMsg.clear();
+    baseMsg["command"] = command.toStdString();
+    QJsonDocument jparam = QJsonDocument(QJsonObject::fromVariantMap(params));
+    baseMsg["params"] = json::parse(jparam.toJson().data());
+    MQTTpublish(baseMsg, m->comSettings.pubList[m->comSettings.COMM]);
+}
+
+void RoboGlue_COM::on_sendROScoordinates(QList<double> sp) {
+    this->lastCoord.clear();
+    this->lastCoord = sp;
+    this->isCoordNew = true;
+}
+
+void RoboGlue_COM::on_sendROSstate(QMap<std::string,bool> digital, QMap<std::string, uint16_t> analog){
+    modlog->debug("robotSendState Received");
+    baseMsg.clear();
+    baseMsg["digital"] = nlohmann::json();
+    baseMsg["analog"] = nlohmann::json();
+    for (auto di : digital.keys()) {
+        baseMsg["digital"][di] = digital[di];
+    }
+}
+////////////////////////////////////////////////
+//////// END EXTERNAL PUBLIC SLOTS /////////////
+////////////////////////////////////////////////
+//void RoboGlue_COM::on_sendSetpointOLD(QList<double> sp) {
+//    std::string setpoint;
+//    struct {
+//        union{
+//            struct{
+//                uint32_t x,y,z,a,b,c,keepalive;
+//            };
+//            char bytes[28];
+//        };
+//    } setPointBytes;
+//    setPointBytes.keepalive=1;
+//    int32_t v;
+//    setpoint = "(";
+//    for(uchar i=0; i<JOINT_NUMBER; i++){
+//        v=static_cast<int32_t>(sp[i]*1000);
+//        if (i>2) v=boost::algorithm::clamp(v, -2680,2680);
+//        setpoint.append(std::to_string(v));
+//        boost::algorithm::replace_all(setpoint, ",",".");
+//        if(i<JOINT_NUMBER) setpoint.append("|");
+//    }
+//    setpoint.append("1");
+//    boost::algorithm::replace_all(setpoint,"|",",");
+//    setpoint.append(")");
+
+//    setPointBytes.x=htonl(static_cast<uint32_t>(sp[0]*1000));
+//    setPointBytes.y=htonl(static_cast<uint32_t>(sp[1]*1000));
+//    setPointBytes.z=htonl(static_cast<uint32_t>(sp[2]*1000));
+//    setPointBytes.a=htonl(static_cast<uint32_t>(sp[3]*1000));
+//    setPointBytes.b=htonl(static_cast<uint32_t>(sp[4]*1000));
+//    setPointBytes.c=htonl(static_cast<uint32_t>(sp[5]*1000));
+//    setPointBytes.keepalive=htonl(setPointBytes.keepalive);
+
+//    if(robotIN->write(setPointBytes.bytes,28))
+//        modlog->trace("Sent Setpoint {}",setpoint.c_str());
+//    robotIN->flush();
+//}
+
+

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