391 lines
14 KiB
C++
391 lines
14 KiB
C++
#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();
|
|
//}
|
|
|
|
|