Add parsing of warning message for sw limits (#1055)

Author: MSECode

conf/iCubFindDependencies.cmake

@@ -64,7 +64,7 @@ checkandset_dependency(IPOPT)
 checkandset_dependency(OpenCV)
 checkandset_dependency(Qt5)
 
-set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.44.1)
+set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.44.2)
 
 if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
   if(icub_firmware_shared_VERSION VERSION_LESS ${MINIMUM_REQUIRED_icub_firmware_shared_VERSION})

src/libraries/icubmod/embObjLib/IethResource.cpp

@@ -81,6 +81,12 @@ bool IethResource::getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std:
     return true;
 }
 
+bool IethResource::getEntityControlModeName(uint32_t jomoId, eOenum08_t control_mode, std::string &controlModeName, eObool_t compact_string)
+{
+    controlModeName.clear();
+    return true;
+}
+
 
 // - end-of-file (leave a blank line after)----------------------------------------------------------------------------
 

src/libraries/icubmod/embObjLib/IethResource.h

@@ -109,6 +109,8 @@ namespace eth {
             */
             virtual bool getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName);
 
+            virtual bool getEntityControlModeName(uint32_t jomoId, eOenum08_t control_mode, std::string &controlModeName, eObool_t compact_string = eobool_true);
+
     private:
             static const char * names[iethresType_numberof+1];
     };

src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp

@@ -168,4 +168,16 @@ bool EntityNameProvider::getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos
     }
 
     return (m_MC_ethRes->getEncoderTypeName(jomoId, pos, encoderTypeName));
-}
+}
+
+bool EntityNameProvider::getEntityControlModeName(uint32_t jomoId, eOenum08_t control_mode, std::string &controlModeName, eObool_t compact_string)
+{
+    if (m_MC_ethRes == nullptr)  
+    {
+        controlModeName = "N/A";
+        return false;
+    }
+
+    return (m_MC_ethRes->getEntityControlModeName(jomoId, control_mode, controlModeName, compact_string));
+}
+/**************************************************************************************************************************/

src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp

@@ -12,12 +12,17 @@
 #include "embot_core_binary.h"
 #include "serviceParser.h"
 #include <yarp/os/Time.h>
-#include <algorithm>
 
+#include <algorithm>
+#include <sstream>
+#include <iomanip>
 
 using namespace Diagnostic::LowLevel;
 
-
+constexpr uint32_t iCubDegreesPerRevolution = 65536;
+constexpr uint32_t degreesPerRevolution = 360;
+constexpr float iCubDegreesToDegreesFactor = static_cast<float>(degreesPerRevolution) / static_cast<float>(iCubDegreesPerRevolution);
+constexpr float degreesToICubDegreesFactor = static_cast<float>(iCubDegreesPerRevolution) / static_cast<float>(degreesPerRevolution);
 
 
 /**************************************************************************************************************************/
@@ -849,6 +854,26 @@ void MotionControlParser::parseInfo()
             m_dnginfo.baseInfo.finalMessage.append(str);
         } break;
 
+        case eoerror_value_MC_joint_software_limit:
+        {
+            uint16_t joint_num = m_dnginfo.param16;
+            int8_t ref_controlmode = m_dnginfo.param64 & 0x00ff;
+            int32_t position_feedback = (m_dnginfo.param64 & 0xffffffff00000000) >> 32;
+            float position_feedback_converted = static_cast<float>(position_feedback) * iCubDegreesToDegreesFactor;
+            std::string ref_controlmode_str = {};
+            m_entityNameProvider.getEntityControlModeName(joint_num, static_cast<eOenum08_t>(ref_controlmode), ref_controlmode_str, false);
+
+            m_entityNameProvider.getAxisName(joint_num, m_dnginfo.baseInfo.axisName);
+
+            std::stringstream ss;
+            ss << " " << m_dnginfo.baseMessage
+               << " (Joint=" << m_dnginfo.baseInfo.axisName
+               << " (NIB=" << joint_num
+               << "), Position_feedback=" << std::fixed << std::setprecision(4) << position_feedback_converted
+               << ", Ref_controlmode=" << ref_controlmode_str << ")";
+            m_dnginfo.baseInfo.finalMessage.append(ss.str());
+            
+        } break;
         case EOERROR_VALUE_DUMMY:
         {
             m_dnginfo.baseInfo.finalMessage.append(": unrecognised eoerror_category_MotionControl error value.");

src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h

@@ -74,6 +74,7 @@ class Diagnostic::LowLevel::EntityNameProvider
 public:
     bool getAxisName(uint32_t entityId, std::string &axisName);
     bool getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName);
+    bool getEntityControlModeName(uint32_t jomoId, eOenum08_t control_mode, std::string &controlModeName, eObool_t compact_string = eobool_true);
 };
 
 

src/libraries/icubmod/embObjLib/hostTransceiver.cpp

@@ -1053,18 +1053,20 @@ void HostTransceiver::eoprot_override_sk(void)
 void cpp_protocol_callback_incaseoferror_in_sequencenumberReceived(EOreceiver *r)
 {  
     const eOreceiver_seqnum_error_t * err = eo_receiver_GetSequenceNumberError(r);
+    char ipv4str[32] = {};
+    eo_common_ipv4addr_to_string(err->remipv4addr, ipv4str, sizeof(ipv4str));
+    std::string ipv4str_s(ipv4str);
     long long unsigned int exp = err->exp_seqnum;
     long long unsigned int rec = err->rec_seqnum;
     long long unsigned int timeoftxofcurrent = err->timeoftxofcurrent;
-    long long unsigned int timeoftxofprevious = err->timeoftxofprevious;
-    char *ipaddr = (char*)&err->remipv4addr;
-    //printf("\nERROR in sequence number from IP = %d.%d.%d.%d\t Expected: \t%llu,\t received: \t%llu\n", ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3], exp, rec);
-    char errmsg[256] = {0};
-    snprintf(errmsg, sizeof(errmsg), "hostTransceiver()::parse() detected an ERROR in sequence number from IP = %d.%d.%d.%d. Expected: %llu, Received: %llu, Missing: %llu, Prev Frame TX at %llu us, This Frame TX at %llu us",
-                                    ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3],
-                                    exp, rec, rec-exp,
-                                    timeoftxofprevious, timeoftxofcurrent);
-    yError() << errmsg;
+    long long unsigned int timeoftxofprevious = err->timeoftxofprevious; 
+    yError() << "hostTransceiver()::parse() detected an ERROR in sequence number from IP =" 
+             << ipv4str_s
+	     << "Expected:" << exp
+             << "- Received:" << rec
+             << "- Missing:" << (rec - exp)
+             << "- Prev Frame TX at" << timeoftxofprevious << "us"
+             << "- This Frame TX at" << timeoftxofcurrent << "us";	     
 }
 
 
@@ -1082,17 +1084,22 @@ void cpp_protocol_callback_incaseoferror_invalidFrame(EOreceiver *r)
 {
     const eOreceiver_invalidframe_error_t * err = eo_receiver_GetInvalidFrameError(r);
     char errmsg[256] = {0};
-    char *ipaddr = (char*)&err->remipv4addr;
+    char ipv4str[32] = {};
+    eo_common_ipv4addr_to_string(err->remipv4addr, ipv4str, sizeof(ipv4str));
+    std::string ipv4str_s(ipv4str);
     tmpStructROPframeHeader_t *header = (tmpStructROPframeHeader_t*)err->ropframe;
     long long unsigned int ageofframe = header->ageofframe;
     long long unsigned int sequencenumber = header->sequencenumber;
     uint16_t ropframesize = 0;
     eo_ropframe_Size_Get(err->ropframe, &ropframesize);
-    //snprintf(errmsg, sizeof(errmsg), "hostTransceiver()::parse() detected an ERROR of type INVALID FRAME from IP = TBD");
-    snprintf(errmsg, sizeof(errmsg), "hostTransceiver()::parse() detected an ERROR of type INVALID FRAME from IP = %d.%d.%d.%d", ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3]);
-    yError() << errmsg;
-    snprintf(errmsg, sizeof(errmsg), "hostTransceiver()::parse() detected: ropframesize = %d, ropsizeof = %d, ropsnumberof = %d, ageoframe = %llu, sequencenumber = %llu", ropframesize, header->ropssizeof, header->ropsnumberof, ageofframe, sequencenumber);
-    yDebug() << errmsg;
+
+    yError() << "hostTransceiver()::parse() detected an ERROR of type INVALID FRAME from IP =" << ipv4str_s;
+    yError() << "hostTransceiver()::parse() detected:"
+             << "ropframesize ="   << ropframesize
+             << "ropsizeof ="      << static_cast<int>(header->ropssizeof)
+             << "ropnumberof ="    << static_cast<int>(header->ropsnumberof)
+             << "ageofframe ="     << ageofframe
+             << "sequencenumber =" << sequencenumber;
 
 }
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -1831,6 +1831,22 @@ bool embObjMotionControl::getEncoderTypeName(uint32_t jomoId, eOmc_position_t po
     }
 }
 
+bool embObjMotionControl::getEntityControlModeName(uint32_t entityId, eOenum08_t control_mode, std::string &controlModeName, eObool_t compact_string)
+{
+    controlModeName.clear();
+    
+    if ((entityId>= 0) && (entityId< _njoints))
+    {
+        controlModeName = eomc_controlmode2string(control_mode, compact_string);
+        return true;
+    }
+    else
+    {
+        controlModeName = eomc_controlmode2string(eomc_ctrlmval_unknownError, compact_string);
+        return false;
+    }
+}
+
 ///////////// PID INTERFACE
 bool embObjMotionControl::setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid)
 {

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -459,6 +459,7 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata);
     virtual bool getEntityName(uint32_t entityId, std::string &entityName);
     virtual bool getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName) override;
+    virtual bool getEntityControlModeName(uint32_t entityId, eOenum08_t control_mode, std::string &controlModeName, eObool_t compact_string) override;
 
     /////////   PID INTERFACE   /////////
     virtual bool setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) override;