Make embedded diagnostic more human readable

conf/iCubFindDependencies.cmake

@@ -65,8 +65,8 @@ checkandset_dependency(OpenCV)
 checkandset_dependency(Qt5)
 
 if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
-  if(icub_firmware_shared_VERSION VERSION_LESS 1.35.0)
-    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.35.0 is required")
+  if(icub_firmware_shared_VERSION VERSION_LESS 1.35.1)
+    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.35.1 is required")
   endif()
 endif()
 

src/libraries/icubmod/embObjLib/CMakeLists.txt

@@ -44,7 +44,10 @@ set(EXTRA_SOURCE            ${CMAKE_CURRENT_SOURCE_DIR}/hostTransceiver.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/batteryInfo.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/theNVmanager.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/embObjGeneralDevPrivData.cpp
-                            ${CMAKE_CURRENT_SOURCE_DIR}/mcEventDownsampler.cpp)
+                            ${CMAKE_CURRENT_SOURCE_DIR}/mcEventDownsampler.cpp
+			    ${CMAKE_CURRENT_SOURCE_DIR}/diagnosticInfoFormatter.cpp
+			    ${CMAKE_CURRENT_SOURCE_DIR}/diagnosticInfoParsers.cpp
+			    ${CMAKE_CURRENT_SOURCE_DIR}/diagnosticInfo.cpp)
 
 set(NVS_CBK_SOURCE  ${CMAKE_CURRENT_SOURCE_DIR}/protocolCallbacks/EoProtocolMN_fun_userdef.c
                     ${CMAKE_CURRENT_SOURCE_DIR}/protocolCallbacks/EoProtocolMC_fun_userdef.c

src/libraries/icubmod/embObjLib/FeatureInterface.cpp

@@ -27,6 +27,8 @@
 #include "EOYtheSystem.h"
 #include "EOYmutex.h"
 
+#include "diagnosticLowLevelFormatter.h"
+
 
 using namespace eth;
 
@@ -339,6 +341,21 @@ void feat_PrintFatal(char *string)
 }
 
 
+void feat_manage_diagnostic(eOmn_info_basic_t* infobasic, uint8_t * extra, const EOnv* nv, const eOropdescriptor_t* rd)
+{
+    if(NULL == _interface2ethManager)
+    {
+        yError("the diagnostic service can not start. The interface to the eth manager is not working.");
+        return;
+    }
+    Diagnostic::LowLevel::InfoFormatter dngFormatter(_interface2ethManager, infobasic, extra, nv, rd);
+
+    Diagnostic::EmbeddedInfo info;
+    dngFormatter.getDiagnosticInfo(info);
+    info.printMessage();
+}
+
+
 // returns a void pointer to the allocated ACE_Recursive_Thread_Mutex
 void* ace_mutex_new(void)
 {

src/libraries/icubmod/embObjLib/FeatureInterface.h

@@ -75,6 +75,8 @@ void feat_PrintError(char *string);
 
 void feat_PrintFatal(char *string);
 
+void feat_manage_diagnostic(eOmn_info_basic_t* infobasic, uint8_t * extra, const EOnv* nv, const eOropdescriptor_t* rd);
+
 
 void* ace_mutex_new(void);
 

src/libraries/icubmod/embObjLib/IethResource.cpp

@@ -69,6 +69,12 @@ const char * IethResource::stringOfType()
     return names[t];
 }
 
+bool IethResource::getEntityName(uint32_t entityId, std::string &entityName)
+{
+    entityName.clear();
+    return true;
+}
+
 
 // - end-of-file (leave a blank line after)----------------------------------------------------------------------------
 

src/libraries/icubmod/embObjLib/IethResource.h

@@ -27,6 +27,7 @@
 
 
 #include "EoProtocol.h"
+#include <string>
 
 
 // marco.accame on 20 oct 2014.
@@ -91,6 +92,13 @@ namespace eth {
 
     public:
             const char * stringOfType();
+            /* the entityName there will be the name of the entity, or it can be an empty string if the enity han't name
+            The entity could be:
+              - axis in case the ethResource type is iethres_motioncontrol
+              - sensor id in case of, skin, mais and strain 
+              The defiult value is an empty string 
+              return false in case of error, like entityId is not present. Aniway the entity name is initialize to empty string.*/
+            virtual bool getEntityName(uint32_t entityId, std::string &entityName);
 
     private:
             static const char * names[iethresType_numberof+1];

src/libraries/icubmod/embObjLib/diagnosticInfo.cpp

@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include <string>
+#include "diagnosticInfo.h"
+#include <yarp/os/LogStream.h>
+
+
+using namespace Diagnostic;
+
+
+/**************************************************************************************************************************/
+/******************************************        TimeOfInfo       ***************************************************/
+/**************************************************************************************************************************/
+
+void TimeOfInfo::toString(std::string &str_toi)
+{
+    char str[50];
+    snprintf(str, sizeof(str), "%ds %dm %du", sec, msec, usec);
+    str_toi.clear();
+    str_toi.append(str);
+}
+
+
+
+/**************************************************************************************************************************/
+/******************************************        Info       ***************************************************/
+/**************************************************************************************************************************/
+
+void EmbeddedInfo::printMessage()
+{
+   std::string str_toi;
+   timeOfInfo.toString(str_toi);
+
+    std::string final_str = "from BOARD " + sourceBoardIpAddrStr + " (" + sourceBoardName +") time=" + str_toi + " : " + finalMessage;
+    switch(severity)
+    {
+        case SeverityOfError::info:
+        {
+            yInfo() << final_str;
+        } break;
+
+        case SeverityOfError::debug:
+        {
+            yDebug() << final_str;
+        } break;
+
+        case SeverityOfError::warning:
+        {
+            yWarning() << final_str;
+        } break;
+
+        case SeverityOfError::error:
+        {
+            yError() << final_str;
+        } break;
+
+        case SeverityOfError::fatal:
+        {
+            yError() << "EMS received the following FATAL error: " <<final_str;
+        } break;
+
+        default:
+        {
+            yError() << final_str;
+        } break;
+    }
+
+
+}
+

src/libraries/icubmod/embObjLib/diagnosticInfo.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+
+#ifndef __diagnosticInfo_h__
+#define __diagnosticInfo_h__
+
+#include <string>
+
+
+namespace Diagnostic {
+    enum class SeverityOfError {info=0, debug =1, warning =2, error=3, fatal=4}; //TBD
+    class EmbeddedInfo;
+    class TimeOfInfo;
+}
+
+class Diagnostic::TimeOfInfo
+{
+public:
+    uint32_t    sec;
+    uint32_t    msec;
+    uint32_t    usec;
+
+    void toString(std::string &str_toi);
+
+};
+
+
+class Diagnostic::EmbeddedInfo
+{
+public:
+    std::string                   sourceBoardIpAddrStr; // is the ipv4 address, in string, of the board that sends the diagnostic information 
+    std::string                   sourceBoardName; // is the name of the board that sends the diagnostic information
+    std::string                   sourceCANPortStr; // if the diagnostic info si sent by a board on CAN, this field contains the CAN port expressed in string
+    uint8_t                       sourceCANBoardAddr; // if the diagnostic info is sent by a board on CAN, this field contains the CAN address of the board
+    std::string                   finalMessage; // contains the final diagostic message after the parsering
+    Diagnostic::SeverityOfError   severity; // is the severity of the message
+    Diagnostic::TimeOfInfo        timeOfInfo; //it is the time of the board when it sent the message
+    std::string                   axisName; //if the error contains the joint number, then axisName contain its name.
+
+public:
+    void printMessage();
+};
+
+
+
+#endif //__diagnosticInfo_h__

src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp

@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include <string>
+#include "diagnosticLowLevelFormatter.h"
+#include "diagnosticLowLevelFormatter_hid.h"
+
+
+
+using namespace Diagnostic::LowLevel;
+using namespace Diagnostic;
+
+
+InfoFormatter::InfoFormatter(eth::TheEthManager* ethManager, eOmn_info_basic_t* infobasic, uint8_t * extra, const EOnv* nv, const eOropdescriptor_t* rd) :
+        m_ethManager(ethManager),m_infobasic(infobasic), m_extra(extra), m_nv(nv), m_rd(rd)
+{;}
+
+
+bool InfoFormatter::getDiagnosticInfo(EmbeddedInfo &info)
+{
+    //==> firts of all fill all the common info to all messages and then parser the parameter depending on the specific message
+
+    AuxEmbeddedInfo dnginfo;
+
+    //1. fill all the common info to all messages
+    dnginfo.sourceBoardIpAddr = eo_nv_GetIP(m_nv);
+    dnginfo.baseInfo.sourceBoardName = m_ethManager->getName(eo_nv_GetIP(m_nv));
+    getTimeOfInfo(dnginfo.baseInfo.timeOfInfo);
+    getSourceOfMessage(dnginfo.baseInfo);
+    getSeverityOfError(dnginfo.baseInfo);
+
+    dnginfo.baseMessage = std::string(eoerror_code2string(m_infobasic->properties.code));
+
+    eOmn_info_extraformat_t extraf  = static_cast<eOmn_info_extraformat_t>EOMN_INFO_PROPERTIES_FLAGS_get_extraformat(m_infobasic->properties.flags);
+    dnginfo.extraMessage.clear();
+
+    if(eomn_info_extraformat_verbal == extraf)
+    {
+        dnginfo.extraMessage.append((NULL == m_extra) ? ("no extra info despite we are in verbal mode") : ((const char *)m_extra));
+    }
+    else
+    {
+        dnginfo.extraMessage.append(".");
+    }
+
+
+    ipv4ToString(dnginfo.baseInfo);
+    dnginfo.param64 = m_infobasic->properties.par64; 
+    dnginfo.param16 = m_infobasic->properties.par16;
+    dnginfo.errorCode = m_infobasic->properties.code;
+
+    EntityNameProvider entityNameProvider{dnginfo.sourceBoardIpAddr, m_ethManager};
+
+    //2. create the parser related to the error cod family
+    std::unique_ptr<DefaultParser> parser_ptr;
+
+    eOerror_category_t category = eoerror_code2category(m_infobasic->properties.code);
+    switch(category)
+    {
+        case eoerror_category_Config: parser_ptr = std::make_unique<ConfigParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_MotionControl: parser_ptr = std::make_unique<MotionControlParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_HardWare: parser_ptr = std::make_unique<HwErrorParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_System: parser_ptr = std::make_unique<SysParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_ETHmonitor: parser_ptr = std::make_unique<EthMonitorParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_Skin: parser_ptr = std::make_unique<SkinParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_InertialSensor: parser_ptr = std::make_unique<InertialSensorParser>(dnginfo, entityNameProvider); break;
+
+        case eoerror_category_AnalogSensor: parser_ptr = std::make_unique<AnalogSensorParser>(dnginfo, entityNameProvider); break;
+
+        default:                      parser_ptr = std::make_unique<DefaultParser>(dnginfo, entityNameProvider); break;
+    };
+
+    parser_ptr->parseInfo();
+
+    //3. return the parsered info
+    info = dnginfo.baseInfo;
+
+    return true;
+
+}
+
+void InfoFormatter::getSourceOfMessage(EmbeddedInfo &info)
+{
+    const char * const sourcenames[] =
+    {
+        "LOCAL",
+        "CAN1",
+        "CAN2",
+        "UNKNOWN"
+    };
+
+    eOmn_info_source_t source = static_cast<eOmn_info_source_t>EOMN_INFO_PROPERTIES_FLAGS_get_source(m_infobasic->properties.flags);
+
+    info.sourceCANBoardAddr = EOMN_INFO_PROPERTIES_FLAGS_get_address(m_infobasic->properties.flags);
+
+    info.sourceCANPortStr = std::string(((source > eomn_info_source_can2) ? (sourcenames[3]) : (sourcenames[source])));
+
+}
+
+
+
+void InfoFormatter::getTimeOfInfo(TimeOfInfo &timeOfInfo)
+{
+    timeOfInfo.sec  = m_infobasic->timestamp / 1000000;
+    timeOfInfo.msec = (m_infobasic->timestamp % 1000000) / 1000;
+    timeOfInfo.usec = m_infobasic->timestamp % 1000;
+}
+
+
+void InfoFormatter::ipv4ToString(EmbeddedInfo &info)
+{
+    char ipinfo[20] = {0};
+    eo_common_ipv4addr_to_string(eo_nv_GetIP(m_nv), ipinfo, sizeof(ipinfo));
+    info.sourceBoardIpAddrStr.clear();
+    info.sourceBoardIpAddrStr.append(ipinfo);
+}
+
+void InfoFormatter::getSeverityOfError(EmbeddedInfo &info)
+{
+    eOmn_info_type_t type = static_cast<eOmn_info_type_t>EOMN_INFO_PROPERTIES_FLAGS_get_type(m_infobasic->properties.flags);
+    switch(type)
+    {
+        case eomn_info_type_info: {info.severity =  SeverityOfError::info;return;}
+        case eomn_info_type_debug: {info.severity =  SeverityOfError::debug;return;}
+        case eomn_info_type_warning: {info.severity =  SeverityOfError::warning;return;}
+        case eomn_info_type_error: {info.severity =  SeverityOfError::error;return;}
+        case eomn_info_type_fatal: {info.severity =  SeverityOfError::fatal;return;}
+
+
+    };
+}
+
+/**************************************************************************************************************************/
+/******************************************        EntityNameProvider       ***************************************************/
+/**************************************************************************************************************************/
+EntityNameProvider::EntityNameProvider(eOipv4addr_t boardAddr, eth::TheEthManager* ethManager):m_ethManager(ethManager)
+{
+       m_MC_ethRes = m_ethManager->getInterface(boardAddr, eth::iethresType_t::iethres_motioncontrol);
+}
+
+bool EntityNameProvider::getAxisName(uint32_t entityId, std::string &axisName)
+{
+    if(m_MC_ethRes == nullptr)
+    {
+        axisName = "N/A";
+        return false;
+    }
+
+    return (m_MC_ethRes->getEntityName(entityId, axisName));
+}