diff --git a/RoboGlue.pro.user b/RoboGlue.pro.user index ed803be..94b0f1e 100644 --- a/RoboGlue.pro.user +++ b/RoboGlue.pro.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/com/mqtt_callback.cpp b/com/mqtt_callback.cpp index 4e6c533..e389dc3 100644 --- a/com/mqtt_callback.cpp +++ b/com/mqtt_callback.cpp @@ -20,6 +20,29 @@ void mqtt_callback::message_arrived(mqtt::const_message_ptr msg) { msg->get_topic(), msg->to_string()); if(msg->get_topic() == mem_->comSettings.subMap.find("interface")->second) { log_->debug("MQTT: Interface Message: {}", msg->get_payload().c_str()); + // Gestione dei messaggi di interfaccia + std::vector intMsg; + boost::split(intMsg, static_cast(msg->to_string()), boost::is_any_of(":")); + if (!intMsg[mem_->comSettings.TYPE].compare("STARTUP") || + !intMsg[mem_->comSettings.TYPE].compare("HB")){ + // Received Startup Message + log_->debug("Startup/Heartbeat from node, ts: {},[{}]", + intMsg[mem_->comSettings.NODE].c_str(), + intMsg[mem_->comSettings.TS]); + if (!intMsg[mem_->comSettings.NODE].compare(mem_->comSettings.nodeNames["relay"])){ + mem_->commonStatus.relayRunning = true; + mem_->commonStatus.relayLast = std::stod(intMsg[mem_->comSettings.TS]); + } else if (!intMsg[mem_->comSettings.NODE].compare(mem_->comSettings.nodeNames["follower"])) { + mem_->commonStatus.followerRunning = true; + mem_->commonStatus.followerLast = std::stod(intMsg[mem_->comSettings.TS]); + } else if (!intMsg[mem_->comSettings.NODE].compare(mem_->comSettings.nodeNames["recorder"])) { + mem_->commonStatus.recorderRunning = true; + mem_->commonStatus.recorderLast = std::stod(intMsg[mem_->comSettings.TS]); + } else { + log_->error("Node {} unknown", intMsg[mem_->comSettings.NODE].c_str()); + } + emit mem_->commonStatusChange(); + } } else if (msg->get_topic() == mem_->comSettings.subMap.find("state")->second){ log_->debug("MQTT: State Message: {}", msg->get_payload().c_str()); } else if (msg->get_topic() == mem_->comSettings.subMap.find("commands")->second){ diff --git a/gui/roboglue_gui.cpp b/gui/roboglue_gui.cpp index 8ec1082..2c36832 100644 --- a/gui/roboglue_gui.cpp +++ b/gui/roboglue_gui.cpp @@ -26,6 +26,8 @@ RoboGlue_GUI::RoboGlue_GUI(RoboGlue_SHARED *mem) : m(mem) { this, SLOT(deleteLater()), Qt::ConnectionType::QueuedConnection); connect(this, SIGNAL(pad_hoverEvent(QEvent *)), this, SLOT(on_pad_hoverEvent(QEvent *))); + connect(m, SIGNAL(commonStatusChange()), + this, SLOT(on_commonStatusChange)); /////// READ NAME DEFINITION JSON FILE ////////// try { @@ -89,6 +91,7 @@ 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 } void RoboGlue_GUI::on_pad_hoverEvent(QEvent* e) { diff --git a/shared/roboglue_shared.cpp b/shared/roboglue_shared.cpp index 3c88b3a..11b3d80 100644 --- a/shared/roboglue_shared.cpp +++ b/shared/roboglue_shared.cpp @@ -13,6 +13,10 @@ RoboGlue_SHARED::RoboGlue_SHARED(QSettings *set){ 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 ///////////////////// diff --git a/shared/roboglue_shared.h b/shared/roboglue_shared.h index 9eb5411..4b36d04 100644 --- a/shared/roboglue_shared.h +++ b/shared/roboglue_shared.h @@ -31,6 +31,7 @@ #include #include #include +#include using namespace nlohmann; @@ -47,6 +48,14 @@ public: bool isPlaying = false; bool isRealtime = false; bool isFileOpen = false; + // ROS node status + bool relayRunning = false; + bool followerRunning = false; + bool recorderRunning = false; + // ROS node last seen + double relayLast = 0.0; + double followerLast = 0.0; + double recorderLast = 0.0; } commonStatus; /////////// PERMANENT SETTINGS VARIABLES ///////// @@ -79,10 +88,14 @@ public: } kine; 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; + enum { + TYPE, NODE, TS + }; } comSettings; struct {