Modifiche interfaccia

RoboGlue.pro.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.8.2, 2019-11-05T16:47:40. -->
+<!-- Written by QtCreator 4.8.2, 2019-11-18T18:58:05. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

com/mqtt_callback.cpp

@@ -41,6 +41,10 @@ void mqtt_callback::message_arrived(mqtt::const_message_ptr msg) {
                 mem_->commonStatus.recorderRunning = true;
                 mem_->commonStatus.recorderLast = std::stod(intMsg[mem_->comSettings.TS]);
                 log_->trace("HB RECORDER time: [{}]", mem_->commonStatus.recorderLast); 
+            } else if (!intMsg[mem_->comSettings.NODE].compare(mem_->comSettings.nodeNames["broadcaster"])) {
+                mem_->commonStatus.broadcasterRunning = true;
+                mem_->commonStatus.broadcasterLast = std::stod(intMsg[mem_->comSettings.TS]);
+                log_->trace("HB BROADCASTER time: [{}]", mem_->commonStatus.broadcasterLast);
             } else {
                 log_->error("Node {} unknown", intMsg[mem_->comSettings.NODE].c_str());
             }
@@ -53,7 +57,7 @@ void mqtt_callback::message_arrived(mqtt::const_message_ptr msg) {
     } else if (msg->get_topic() == mem_->comSettings.subMap.find("coordinates")->second){
         log_->debug("MQTT: Coordinate Message: {}", msg->get_payload().c_str());
     } else {
-        log_->error("Unrecognized topic");
+        log_->error("Unrecognized topic: {}", msg->get_topic().c_str());
     }
 }
 

com/roboglue_com.cpp

@@ -6,7 +6,7 @@ RoboGlue_COM::RoboGlue_COM (RoboGlue_SHARED *mem): m(mem) {
     liblog = spdlog::stdout_logger_mt("URcom_liblog");
     modlog = spdlog::stdout_logger_mt("RoboGlue_comlog");
     liblog->set_level(spdlog::level::warn);
-    modlog->set_level(spdlog::level::trace);
+    modlog->set_level(spdlog::level::warn);
 
     ///////// INITIALIZE TCP SERVER ///////////
     server = new QTcpServer(this);

gui/roboglue_gui.cpp

@@ -91,22 +91,18 @@ 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(this->m->comSettings.nodeNames.find("relay")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.relayTime))));
-    this->ui->lbl_relay->setText(QString().fromStdString(std::to_string(m->commonStatus.relayTime)));
+    this->ui->lbl_follower->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("follower")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.followerTime))));
-    this->ui->lbl_follower->setText(QString().fromStdString(std::to_string(m->commonStatus.followerTime)));
+    this->ui->lbl_recorder->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("recorder")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.recorderTime))));
-    this->ui->lbl_recorder->setText(QString().fromStdString(std::to_string(m->commonStatus.recorderTime)));
+    this->ui->lbl_broadcaster->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("broadcaster")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.broadcasterTime))));
-
+    if (!m->commonStatus.relayRunning) this->ui->lbl_relay->setStyleSheet("QLabel { background-color : red; color : white; }");
-    if(m->commonStatus.relayTime > m->comSettings.deadTime ||
+    else this->ui->lbl_relay->setStyleSheet("QLabel { background-color : green; color : white; }");
-            m->commonStatus.followerTime > m->comSettings.deadTime ||
+    if (!m->commonStatus.recorderRunning) this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : red; color : white; }");
-            m->commonStatus.recorderTime > m->comSettings.deadTime) {
+    else this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : green; color : white; }");
-        this->ui->lbl_relay->setStyleSheet("QLabel { background-color : red; color : white; }");
+    if (!m->commonStatus.followerRunning) this->ui->lbl_follower->setStyleSheet("QLabel { background-color : red; color : white; }");
-        this->ui->lbl_follower->setStyleSheet("QLabel { background-color : red; color : white; }");
+    else this->ui->lbl_follower->setStyleSheet("QLabel { background-color : green; color : white; }");
-        this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : red; color : white; }");
+    if (!m->commonStatus.broadcasterRunning) this->ui->lbl_broadcaster->setStyleSheet("QLabel { background-color : red; color : white; }");
-    } else {
+    else this->ui->lbl_broadcaster->setStyleSheet("QLabel { background-color : green; color : white; }");
-        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) {
@@ -271,7 +267,7 @@ void RoboGlue_GUI::on_btn_open_clicked() {
         m->commonStatus.isFileOpen = true;
         fileOpen = true;
         metadata["name"] = ui->txt_fileName->text();
-        metadata["plot"] = true;
+        metadata["plot"] = ui->chk_plot->isChecked();
         param["action"] = "open";
         param["metadata"] = metadata;
         ui->btn_record->setEnabled(false);
@@ -329,6 +325,30 @@ void RoboGlue_GUI::on_btn_home_clicked() {
     emit sendROScommand("HOME", param);
 }
 
+void RoboGlue_GUI::on_btn_setFrame_clicked() {
+    QVariantMap param, frm;
+    QList<QString> tempPos, TempOr;
+    modlog->debug("setFrame");
+    param["action"]="set";
+    param["framename"]=ui->txt_frameName->text();
+    for(auto tp : ui->txt_framePos->text().split(",")){
+        frm[tp.split(":").first()]=tp.split(":").last().replace(" ", "").toDouble();
+    }
+    for(auto tp : ui->txt_frameOr->text().split(",")){
+        frm[tp.split(":").first()]=tp.split(":").last().replace(" ", "").toDouble();
+    }
+    param["frame"]=frm;
+    emit m->commonStatusChange();
+    emit sendROScommand("SETFRAME", param);
+}
+
+void RoboGlue_GUI::on_btn_stopRos_clicked() {
+    QVariantMap param;
+    modlog->debug("stopRos");
+    param["action"]=false;
+    emit m->commonStatusChange();
+    emit sendROScommand("QUIT", param);
+}
 ////////////////////////////////////////////////
 ////////END INTERNAL PRIVATE SLOTS//////////////
 ////////////////////////////////////////////////

gui/roboglue_gui.h

@@ -95,6 +95,10 @@ private slots:
     void on_btn_home_clicked();
     void on_btn_open_clicked();
 
+    void on_btn_setFrame_clicked();
+
+    void on_btn_stopRos_clicked();
+
 signals:
     void robotConnect(QString, uint port, uchar retry);
     void robotDisconnect();

gui/roboglue_gui.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1108</width>
+    <width>1018</width>
-    <height>760</height>
+    <height>826</height>
    </rect>
   </property>
   <property name="mouseTracking">
@@ -441,7 +441,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -460,7 +460,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -479,7 +479,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -501,7 +501,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -520,7 +520,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -539,7 +539,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -591,7 +591,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -610,7 +610,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -629,7 +629,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -648,7 +648,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -667,7 +667,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -686,7 +686,7 @@
                <string>-</string>
               </property>
               <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+               <set>Qt::AlignCenter</set>
               </property>
               <property name="textInteractionFlags">
                <set>Qt::TextSelectableByMouse</set>
@@ -920,7 +920,7 @@
         </item>
         <item row="0" column="0">
          <layout class="QVBoxLayout" name="verticalLayout_2">
-          <item>
+          <item alignment="Qt::AlignHCenter">
            <widget class="QFrame" name="frm_move">
             <property name="sizePolicy">
              <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -978,6 +978,9 @@
               <property name="text">
                <string>-</string>
               </property>
+              <property name="alignment">
+               <set>Qt::AlignCenter</set>
+              </property>
              </widget>
             </item>
             <item>
@@ -985,6 +988,9 @@
               <property name="text">
                <string>-</string>
               </property>
+              <property name="alignment">
+               <set>Qt::AlignCenter</set>
+              </property>
              </widget>
             </item>
             <item>
@@ -992,6 +998,9 @@
               <property name="text">
                <string>-</string>
               </property>
+              <property name="alignment">
+               <set>Qt::AlignCenter</set>
+              </property>
              </widget>
             </item>
            </layout>
@@ -1037,7 +1046,21 @@
              </widget>
             </item>
             <item>
-             <widget class="QLineEdit" name="txt_fileName"/>
+             <widget class="QLineEdit" name="txt_fileName">
+              <property name="minimumSize">
+               <size>
+                <width>50</width>
+                <height>0</height>
+               </size>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QCheckBox" name="chk_plot">
+              <property name="text">
+               <string>Plot</string>
+              </property>
+             </widget>
             </item>
            </layout>
           </item>
@@ -1211,33 +1234,17 @@
            </layout>
           </item>
           <item>
-           <layout class="QHBoxLayout" name="horizontalLayout_6">
+           <widget class="Line" name="line_12">
-            <item>
-             <spacer name="horizontalSpacer_4">
             <property name="orientation">
              <enum>Qt::Horizontal</enum>
             </property>
-              <property name="sizeHint" stdset="0">
-               <size>
-                <width>40</width>
-                <height>20</height>
-               </size>
-              </property>
-             </spacer>
-            </item>
-            <item>
-             <layout class="QGridLayout" name="gridLayout_2">
-              <item row="0" column="0">
-               <widget class="QLabel" name="label_18">
-                <property name="text">
-                 <string>dS</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignCenter</set>
-                </property>
            </widget>
           </item>
-              <item row="0" column="1">
+          <item>
+           <layout class="QHBoxLayout" name="horizontalLayout_6">
+            <item>
+             <layout class="QGridLayout" name="gridLayout_2">
+              <item row="0" column="2">
                <widget class="QLabel" name="label_19">
                 <property name="text">
                  <string>dT</string>
@@ -1247,7 +1254,7 @@
                 </property>
                </widget>
               </item>
-              <item row="1" column="0">
+              <item row="1" column="1">
                <widget class="QDoubleSpinBox" name="spn_ds">
                 <property name="maximum">
                  <double>100.000000000000000</double>
@@ -1263,7 +1270,7 @@
                 </property>
                </widget>
               </item>
-              <item row="1" column="1">
+              <item row="1" column="2">
                <widget class="QDoubleSpinBox" name="spn_dt">
                 <property name="maximum">
                  <double>240.000000000000000</double>
@@ -1276,10 +1283,108 @@
                 </property>
                </widget>
               </item>
+              <item row="0" column="1">
+               <widget class="QLabel" name="label_18">
+                <property name="text">
+                 <string>dS</string>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignCenter</set>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <spacer name="horizontalSpacer_4">
+                <property name="orientation">
+                 <enum>Qt::Horizontal</enum>
+                </property>
+                <property name="sizeHint" stdset="0">
+                 <size>
+                  <width>40</width>
+                  <height>20</height>
+                 </size>
+                </property>
+               </spacer>
+              </item>
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_22">
+                <property name="text">
+                 <string>Point Record Distance/Time</string>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignCenter</set>
+                </property>
+               </widget>
+              </item>
              </layout>
             </item>
            </layout>
           </item>
+          <item>
+           <widget class="Line" name="line_13">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <layout class="QGridLayout" name="gridLayout_5">
+            <item row="0" column="2">
+             <widget class="QLineEdit" name="txt_framePos">
+              <property name="inputMask">
+               <string>\x:000,y:000,z:000 </string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="1">
+             <widget class="QLabel" name="label_21">
+              <property name="text">
+               <string>Orientation</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="1">
+             <widget class="QLabel" name="label_20">
+              <property name="text">
+               <string>Position</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="2">
+             <widget class="QLineEdit" name="txt_frameOr">
+              <property name="inputMask">
+               <string>r\x:#00.0,ry:#00.0,rz:#00.0 </string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="0">
+             <widget class="QPushButton" name="btn_setFrame">
+              <property name="text">
+               <string>SetFrame</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="3">
+             <widget class="QLabel" name="label_24">
+              <property name="minimumSize">
+               <size>
+                <width>150</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="text">
+               <string>Name</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="3">
+             <widget class="QLineEdit" name="txt_frameName"/>
+            </item>
+           </layout>
+          </item>
           <item>
            <spacer name="verticalSpacer_2">
             <property name="orientation">
@@ -1339,6 +1444,20 @@
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QLabel" name="lbl_broadcaster">
+        <property name="text">
+         <string>-</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QPushButton" name="btn_stopRos">
+        <property name="text">
+         <string>Stop_ROS</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </item>
    </layout>
@@ -1348,7 +1467,7 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>1108</width>
+     <width>1018</width>
      <height>22</height>
     </rect>
    </property>
@@ -1371,11 +1490,11 @@
  <connections/>
  <buttongroups>
   <buttongroup name="grp_hilo"/>
-  <buttongroup name="grp_mod"/>
   <buttongroup name="gpr_lock">
    <property name="exclusive">
     <bool>false</bool>
    </property>
   </buttongroup>
+  <buttongroup name="grp_mod"/>
  </buttongroups>
 </ui>

roboglue.conf

@@ -8,7 +8,7 @@ automain=false
 autotrack=false
 
 [robot]
-connection\autoconnect=false
+connection\autoconnect=true
 connection\robotip=10.0.0.5
 connection\robotport=30002
 connection\robotretry=5
@@ -51,6 +51,7 @@ 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\broadcaster=roboglue_ros_broadcaster
 ros_nodes\node_names\follower=roboglue_ros_follower
 ros_nodes\node_names\recorder=roboglue_ros_recorder
 ros_nodes\node_names\relay=roboglue_ros_relay

shared/roboglue_shared.cpp

@@ -16,9 +16,10 @@ RoboGlue_SHARED::~RoboGlue_SHARED(){
 
 void RoboGlue_SHARED::on_statusTimer(void){
     double curTime = static_cast<double>(std::time(NULL));
-    commonStatus.relayTime = fabs(commonStatus.relayLast - curTime);
+    commonStatus.relayRunning = (commonStatus.relayTime = fabs(commonStatus.relayLast - curTime)) < comSettings.deadTime;
-    commonStatus.followerTime = fabs(commonStatus.followerLast - curTime);
+    commonStatus.followerRunning = (commonStatus.followerTime = fabs(commonStatus.followerLast - curTime)) < comSettings.deadTime;
-    commonStatus.recorderTime = fabs(commonStatus.recorderLast - curTime);
+    commonStatus.recorderRunning = (commonStatus.recorderTime = fabs(commonStatus.recorderLast - curTime)) < comSettings.deadTime;
+    commonStatus.broadcasterRunning = (commonStatus.broadcasterTime = fabs(commonStatus.broadcasterLast - curTime)) < comSettings.deadTime;
     emit this->commonStatusChange();
 }
 ////////////////////////////////////////////////////////////////
@@ -88,6 +89,7 @@ void RoboGlue_SHARED::restoreComSettings() {
                  comSettings.nodeNames["relay"] = settings->value("relay").toString().toStdString();
                  comSettings.nodeNames["follower"] = settings->value("follower").toString().toStdString();
                  comSettings.nodeNames["recorder"] = settings->value("recorder").toString().toStdString();
+                 comSettings.nodeNames["broadcaster"] = settings->value("broadcaster").toString().toStdString();
              settings->endGroup();
          settings->endGroup();
     settings->endGroup();
@@ -153,6 +155,7 @@ void RoboGlue_SHARED::saveComSettings() {
                 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->setValue("broadcaster", QString().fromStdString(comSettings.nodeNames.find("broadcaster")->second));
             settings->endGroup();
         settings->endGroup();
     settings->endGroup();

shared/roboglue_shared.h

@@ -30,6 +30,7 @@
 #include <QSize>
 #include <QTimer>
 #include <stdexcept>
+#include <ctime>
 #include <libJson/json.hpp>
 #include <libURcom/URCLinterface.h>
 #include <boost/algorithm/string.hpp>
@@ -55,13 +56,16 @@ public:
         bool relayRunning = false;
         bool followerRunning = false;
         bool recorderRunning = false;
+        bool broadcasterRunning = false;
         // ROS node last seen
-        double relayLast = 0.0;
+        double relayLast = static_cast<double>(time(NULL));
-        double followerLast = 0.0;
+        double followerLast = static_cast<double>(time(NULL));
-        double recorderLast = 0.0;
+        double recorderLast = static_cast<double>(time(NULL));
-        double relayTime = 0.0;
+        double broadcasterLast = static_cast<double>(time(NULL));
-        double followerTime = 0.0;
+        double relayTime = static_cast<double>(time(NULL));
-        double recorderTime = 0.0;
+        double followerTime = static_cast<double>(time(NULL));
+        double recorderTime = static_cast<double>(time(NULL));
+        double broadcasterTime = static_cast<double>(time(NULL));
     } commonStatus;
 
     /////////// PERMANENT SETTINGS VARIABLES /////////
@@ -92,7 +96,7 @@ public:
             kineDH_t DHtable;
             float maxReach = 1.2;
         } kine;
-        double deadTime = 10.0; //default value
+        double deadTime = 1.0; //default value
         std::map<std::string, std::string> subMap;
         std::map<std::string, std::string> pubMap;
         std::map<std::string, std::string> nodeNames;