From 187c3ed9fc39a588ca459ab1b69b3f297867e017 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Tue, 5 Nov 2019 15:49:12 +0100 Subject: [PATCH] Aggiunta parametrizzazione topic mqtt e nomi dei nodi ros --- RoboGlue.pro.user | 2 +- com/roboglue_com.cpp | 8 +++-- gui/roboglue_gui.cpp | 17 ++++++++- init/roboglue_init.cpp | 4 ++- roboglue.conf | 16 ++++++++- shared/roboglue_shared.cpp | 71 ++++++++++++++++++++++++++++---------- shared/roboglue_shared.h | 2 +- 7 files changed, 95 insertions(+), 25 deletions(-) diff --git a/RoboGlue.pro.user b/RoboGlue.pro.user index 94b0f1e..427bf8b 100644 --- a/RoboGlue.pro.user +++ b/RoboGlue.pro.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/com/roboglue_com.cpp b/com/roboglue_com.cpp index e495662..4ab4a1e 100644 --- a/com/roboglue_com.cpp +++ b/com/roboglue_com.cpp @@ -35,8 +35,12 @@ void RoboGlue_COM::run() { client->set_callback(*callback); client->connect()->wait(); // subscrbe to mqtt topics - for (auto it=m->comSettings.subMap.begin(); it != m->comSettings.subMap.end(); ++it){ - client->subscribe(it->second,0); + try{ + for (auto it=m->comSettings.subMap.begin(); it != m->comSettings.subMap.end(); ++it){ + client->subscribe(it->second,0); + } + } catch (mqtt::exception &e) { + modlog->error("MQTT Error {}", e.what()); } client->start_consuming(); diff --git a/gui/roboglue_gui.cpp b/gui/roboglue_gui.cpp index 2c36832..5fa5b15 100644 --- a/gui/roboglue_gui.cpp +++ b/gui/roboglue_gui.cpp @@ -27,7 +27,7 @@ RoboGlue_GUI::RoboGlue_GUI(RoboGlue_SHARED *mem) : m(mem) { connect(this, SIGNAL(pad_hoverEvent(QEvent *)), this, SLOT(on_pad_hoverEvent(QEvent *))); connect(m, SIGNAL(commonStatusChange()), - this, SLOT(on_commonStatusChange)); + this, SLOT(on_commonStatusChange())); /////// READ NAME DEFINITION JSON FILE ////////// try { @@ -92,6 +92,21 @@ void RoboGlue_GUI::disableLockAxes(){ void RoboGlue_GUI::on_commonStatusChange() { modlog->trace("on_commonStatusChange Received"); // TODO: aggiungere e interpretare le flag per capire se i nodi ros sono tutti vivi dall'heartbeat + this->ui->lbl_relay->setText(QString().fromStdString(std::to_string(m->commonStatus.relayLast))); + this->ui->lbl_follower->setText(QString().fromStdString(std::to_string(m->commonStatus.followerLast))); + this->ui->lbl_recorder->setText(QString().fromStdString(std::to_string(m->commonStatus.recorderLast))); + + if(m->commonStatus.relayLast > m->comSettings.deadTime || + m->commonStatus.followerLast > m->comSettings.deadTime || + m->commonStatus.recorderLast > m->comSettings.deadTime) { + this->ui->lbl_relay->setStyleSheet("QLabel { background-color : red; color : white; }"); + this->ui->lbl_follower->setStyleSheet("QLabel { background-color : red; color : white; }"); + this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : red; color : white; }"); + } else { + this->ui->lbl_relay->setStyleSheet("QLabel { background-color : green; color : white; }"); + this->ui->lbl_follower->setStyleSheet("QLabel { background-color : green; color : white; }"); + this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : green; color : white; }"); + } } void RoboGlue_GUI::on_pad_hoverEvent(QEvent* e) { diff --git a/init/roboglue_init.cpp b/init/roboglue_init.cpp index 9f65858..e70a7af 100644 --- a/init/roboglue_init.cpp +++ b/init/roboglue_init.cpp @@ -5,7 +5,6 @@ RoboGlue_INIT::RoboGlue_INIT(QWidget *parent, const char*) : QMainWindow(parent) //////// SETUP LOGGER ////////// inilog = spdlog::stdout_logger_mt("RoboGlue_inilog"); - inilog->info("INIT Started"); //////// SETUP USER INTERFACE /////// ui = new Ui::RoboGlue_INIT; @@ -38,6 +37,9 @@ RoboGlue_INIT::RoboGlue_INIT(QWidget *parent, const char*) : QMainWindow(parent) //////// CONNECT SIGNALS & SLOTS /////// connect(this, SIGNAL(destroyed()), this, SLOT(deleteLater()), Qt::ConnectionType::QueuedConnection); + + //////// SIGNAL STARTED //////////////// + inilog->info("INIT Started"); } RoboGlue_INIT::~RoboGlue_INIT() { diff --git a/roboglue.conf b/roboglue.conf index 2e46f49..efb7da4 100644 --- a/roboglue.conf +++ b/roboglue.conf @@ -8,7 +8,7 @@ automain=false autotrack=false [robot] -connection\autoconnect=true +connection\autoconnect=false connection\robotip=10.0.0.5 connection\robotport=30002 connection\robotretry=5 @@ -40,3 +40,17 @@ kine\dhtable\size=6 kine\maxreach=@Variant(\0\0\0\x87?\x99\x99\x9a) kine\prefix=0 kine\suffix=0 + +[ros] +deadTime=10 +ros_nodes\mqtt_publishers\commands=roboglue_com/com2ros/commands +ros_nodes\mqtt_publishers\coordinates=roboglue_com/com2ros/coordinates +ros_nodes\mqtt_publishers\interface=roboglue_com/com2ros/interface +ros_nodes\mqtt_publishers\state=roboglue_com/com2ros/state +ros_nodes\mqtt_subscribers\commands=roboglue_com/ros2com/commands +ros_nodes\mqtt_subscribers\coordinates=roboglue_com/ros2com/coordinates +ros_nodes\mqtt_subscribers\interface=roboglue_com/ros2com/interface +ros_nodes\mqtt_subscribers\state=roboglue_com/ros2com/state +ros_nodes\node_names\follower=roboglue_ros_follower +ros_nodes\node_names\recorder=roboglue_ros_recorder +ros_nodes\node_names\relay=roboglue_ros_relay diff --git a/shared/roboglue_shared.cpp b/shared/roboglue_shared.cpp index 11b3d80..11240fb 100644 --- a/shared/roboglue_shared.cpp +++ b/shared/roboglue_shared.cpp @@ -4,23 +4,15 @@ RoboGlue_SHARED::RoboGlue_SHARED(QSettings *set){ settings=set; //FIXME: inserire la lettura di quesste informazioni da un file robotVariables.insert(std::pair("RDY",1)); - comSettings.subMap["commands"] = "roboglue_com/ros2com/commands"; - comSettings.subMap["coordinates"] = "roboglue_com/ros2com/coordinates"; - comSettings.subMap["state"] = "roboglue_com/ros2com/state"; - comSettings.subMap["interface"] = "roboglue_com/ros2com/interface"; - - comSettings.pubMap["commands"] = "roboglue_com/com2ros/commands"; - comSettings.pubMap["coordinates"] = "roboglue_com/com2ros/coordinates"; - comSettings.pubMap["state"] = "roboglue_com/com2ros/state"; - comSettings.pubMap["interface"] = "roboglue_com/com2ros/interface"; - - comSettings.nodeNames["relay"] = "roboglue_ros_relay"; - comSettings.nodeNames["follower"] = "roboglue_ros_follower"; - comSettings.nodeNames["recorder"] = "roboglue_ros_recorder"; } //////////////////////////////////////////////////////////////// /////////////////////// RESTORE FUNCTIONS ///////////////////// ///////////////////////////////////////////////////////////// +void RoboGlue_SHARED::restoreCommonSettings(){ + settings->beginGroup("common"); + settings->endGroup(); +} + void RoboGlue_SHARED::restoreInitSettings(){ settings->beginGroup("init"); initSettings.size=settings->value("size").toSize(); @@ -61,6 +53,28 @@ void RoboGlue_SHARED::restoreComSettings() { // TODO something settings->endGroup(); settings->endGroup(); + settings->beginGroup("ros"); + comSettings.deadTime = settings->value("deadTime").toDouble(); + settings->beginGroup("ros_nodes"); + settings->beginGroup("mqtt_publishers"); + comSettings.pubMap["commands"] = settings->value("commands").toString().toStdString(); + comSettings.pubMap["coordinates"] = settings->value("coordinates").toString().toStdString(); + comSettings.pubMap["state"] = settings->value("state").toString().toStdString(); + comSettings.pubMap["interface"] = settings->value("interface").toString().toStdString(); + settings->endGroup(); + settings->beginGroup("mqtt_subscribers"); + comSettings.subMap["commands"] = settings->value("commands").toString().toStdString(); + comSettings.subMap["coordinates"] = settings->value("coordinates").toString().toStdString(); + comSettings.subMap["state"] = settings->value("state").toString().toStdString(); + comSettings.subMap["interface"] = settings->value("interface").toString().toStdString(); + settings->endGroup(); + settings->beginGroup("node_names"); + comSettings.nodeNames["relay"] = settings->value("relay").toString().toStdString(); + comSettings.nodeNames["follower"] = settings->value("follower").toString().toStdString(); + comSettings.nodeNames["recorder"] = settings->value("recorder").toString().toStdString(); + settings->endGroup(); + settings->endGroup(); + settings->endGroup(); } void RoboGlue_SHARED::restoreGuiSettings() { @@ -85,7 +99,6 @@ void RoboGlue_SHARED::restoreTrackSettings() { void RoboGlue_SHARED::saveCommonSettings(){ settings->beginGroup("common"); settings->endGroup(); - } void RoboGlue_SHARED::saveInitSettings() { @@ -99,11 +112,33 @@ void RoboGlue_SHARED::saveInitSettings() { void RoboGlue_SHARED::saveComSettings() { settings->beginGroup("robot"); - settings->beginGroup("kine"); - settings->setValue("maxreach", comSettings.kine.maxReach); - settings->setValue("prefix", comSettings.connection.prefix); - settings->setValue("suffix", comSettings.connection.suffix); + settings->beginGroup("kine"); + settings->setValue("maxreach", comSettings.kine.maxReach); + settings->setValue("prefix", comSettings.connection.prefix); + settings->setValue("suffix", comSettings.connection.suffix); + settings->endGroup(); settings->endGroup(); + settings->beginGroup("ros"); + settings->setValue("deadTime", comSettings.deadTime); + settings->beginGroup("ros_nodes"); + settings->beginGroup("mqtt_publishers"); + settings->setValue("commands", QString().fromStdString(comSettings.pubMap.find("commands")->second)); + settings->setValue("coordinates", QString().fromStdString(comSettings.pubMap.find("coordinates")->second)); + settings->setValue("state", QString().fromStdString(comSettings.pubMap.find("state")->second)); + settings->setValue("interface", QString().fromStdString(comSettings.pubMap.find("interface")->second)); + settings->endGroup(); + settings->beginGroup("mqtt_subscribers"); + settings->setValue("commands", QString().fromStdString(comSettings.subMap.find("commands")->second)); + settings->setValue("coordinates", QString().fromStdString(comSettings.subMap.find("coordinates")->second)); + settings->setValue("state", QString().fromStdString(comSettings.subMap.find("state")->second)); + settings->setValue("interface", QString().fromStdString(comSettings.subMap.find("interface")->second)); + settings->endGroup(); + settings->beginGroup("node_names"); + settings->setValue("relay", QString().fromStdString(comSettings.nodeNames.find("relay")->second)); + settings->setValue("follower", QString().fromStdString(comSettings.nodeNames.find("follower")->second)); + settings->setValue("recorder", QString().fromStdString(comSettings.nodeNames.find("recorder")->second)); + settings->endGroup(); + settings->endGroup(); settings->endGroup(); } diff --git a/shared/roboglue_shared.h b/shared/roboglue_shared.h index 4b36d04..74b7098 100644 --- a/shared/roboglue_shared.h +++ b/shared/roboglue_shared.h @@ -86,10 +86,10 @@ public: kineDH_t DHtable; float maxReach = 1.2; } kine; + double deadTime = 10.0; //default value std::map subMap; std::map pubMap; std::map nodeNames; - //FIXME: riempire la mappa dinamicamente da un file, eliminare l'assegnazione manuale enum { COMM, COORD, STAT } tList;