Feat/ft service multiple sensors

.clang-format

@@ -0,0 +1,12 @@
+# Common settings
+TabWidth:        4
+IndentWidth:     4
+UseTab:          Never
+ColumnLimit: 200
+
+Language: Cpp
+
+DisableFormat:   false
+Standard: Cpp11
+SortIncludes: false
+

.github/workflows/ci.yml

@@ -126,7 +126,7 @@ jobs:
               -DENABLE_icubmod_canBusVirtualAnalogSensor:BOOL=ON -DENABLE_icubmod_embObjFTsensor:BOOL=ON \
               -DENABLE_icubmod_embObjIMU:BOOL=ON -DENABLE_icubmod_embObjInertials:BOOL=ON \
               -DENABLE_icubmod_embObjMais:BOOL=ON -DENABLE_icubmod_embObjMotionControl:BOOL=ON \
-              -DENABLE_icubmod_embObjSkin:BOOL=ON -DENABLE_icubmod_embObjStrain:BOOL=ON \
+              -DENABLE_icubmod_embObjSkin:BOOL=ON -DENABLE_icubmod_embObjStrain:BOOL=ON -DENABLE_icubmod_embObjMultipleFTsensors:BOOL=ON \
               -DENABLE_icubmod_parametricCalibrator:BOOL=ON -DENABLE_icubmod_parametricCalibratorEth:BOOL=ON \
               -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
 
@@ -145,9 +145,11 @@ jobs:
               -DENABLE_icubmod_canBusVirtualAnalogSensor:BOOL=ON -DENABLE_icubmod_embObjFTsensor:BOOL=ON \
               -DENABLE_icubmod_embObjIMU:BOOL=ON -DENABLE_icubmod_embObjInertials:BOOL=ON \
               -DENABLE_icubmod_embObjMais:BOOL=ON -DENABLE_icubmod_embObjMotionControl:BOOL=ON \
-              -DENABLE_icubmod_embObjSkin:BOOL=ON -DENABLE_icubmod_embObjStrain:BOOL=ON \
+              -DENABLE_icubmod_embObjSkin:BOOL=ON -DENABLE_icubmod_embObjStrain:BOOL=ON -DENABLE_icubmod_embObjMultipleFTsensors:BOOL=ON \
               -DENABLE_icubmod_parametricCalibrator:BOOL=ON -DENABLE_icubmod_parametricCalibratorEth:BOOL=ON \
+              -DBUILD_TESTING:BOOL=ON \
               -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
+
     - name: Enable python bindings on Ubuntu
       if: matrix.os == 'ubuntu-latest'
       run: |
@@ -164,6 +166,13 @@ jobs:
         export PATH=$PATH:${GITHUB_WORKSPACE}/install/bin:${VCPKG_INSTALLATION_ROOT}/installed/x64-windows/bin
         cmake --build . --config ${{ matrix.build_type }}
 
+    - name: Test
+      if: matrix.os == 'ubuntu-latest'
+      shell: bash
+      run: |
+        cd build
+        ctest --output-on-failure -C ${{ matrix.build_type }} .
+
     - name: Install
       shell: bash
       run: |

CMakeLists.txt

@@ -20,6 +20,8 @@ set(YARP_REQUIRED_VERSION 3.6.0)
 find_package(YCM ${YCM_REQUIRED_VERSION} REQUIRED)
 find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED)
 
+enable_testing()
+
 if(YARP_math_FOUND)
   set(ICUB_HAS_YARP TRUE CACHE BOOL "" FORCE)
   message(STATUS "found libYARP_math")

src/CMakeLists.txt

@@ -24,6 +24,8 @@ mark_as_advanced(ICUBMAIN_COMPILE_SIMULATORS)
 option(ICUBMAIN_COMPILE_MODULES "Enable icub-main modules." ON)
 mark_as_advanced(ICUBMAIN_COMPILE_MODULES)
 
+option(BUILD_TESTING "Enable unittest." OFF)
+
 if (ICUBMAIN_COMPILE_LIBRARIES)
 add_subdirectory(libraries)
 endif()
@@ -44,5 +46,9 @@ if (ICUBMAIN_COMPILE_MODULES)
 add_subdirectory(modules)
 endif()
 
-
+if (BUILD_TESTING)
+  if(NOT BUILD_SHARED_LIBS)
+    add_subdirectory(unittest)
+  endif()
+endif()
 

src/libraries/icubmod/CMakeLists.txt

@@ -45,6 +45,7 @@ yarp_begin_plugin_library(icubmod QUIET)
   add_subdirectory(embObjAnalog)
   add_subdirectory(embObjStrain)
   add_subdirectory(embObjFTsensor)
+  add_subdirectory(embObjMultipleFTsensors)
   add_subdirectory(embObjMais)
   add_subdirectory(embObjMultiEnc)
   add_subdirectory(embObjInertials)

src/libraries/icubmod/embObjLib/CMakeLists.txt

@@ -38,6 +38,8 @@ set(EXTRA_SOURCE            ${CMAKE_CURRENT_SOURCE_DIR}/hostTransceiver.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/IethResource.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/fakeEthResource.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/serviceParser.cpp
+                            ${CMAKE_CURRENT_SOURCE_DIR}/serviceParserMultipleFt.cpp
+                            ${CMAKE_CURRENT_SOURCE_DIR}/ftInfo.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/theNVmanager.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/embObjGeneralDevPrivData.cpp
                             ${CMAKE_CURRENT_SOURCE_DIR}/mcEventDownsampler.cpp)
@@ -81,7 +83,7 @@ SOURCE_GROUP("EmbObj Source Files" FILES ${embobj_source})
 SOURCE_GROUP("EmbObj Header Files" FILES ${embobj_header})
 
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
-add_library(${PROJECT_NAME} SHARED ${embobj_source} ${embobj_header})
+add_library(${PROJECT_NAME} ${embobj_source} ${embobj_header})
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/../motionControlLib
                                                 ${PATH_TO_CALLBACK}/embObjAnalog/usrcbk/

src/libraries/icubmod/embObjLib/IethResource.cpp

@@ -53,7 +53,7 @@ const char * IethResource::names[iethresType_numberof+1] =
 {
     "resManagement", "resAnalogMais", "resAnalogStrain", "resMotionControl",
     "resSkin", "resAnalogVirtual", "resAnalogInertial", "resAnalogmultienc",
-    "resAnalogInertial3", "resAnalogTemperature", "resAnalogPSC", "resAnalogPOS",
+    "resAnalogInertial3", "resAnalogTemperature", "resAnalogPSC", "resAnalogPOS","resAnalogFT",
     "resNONE"
 };
 

src/libraries/icubmod/embObjLib/IethResource.h

@@ -71,10 +71,11 @@ namespace eth {
         iethres_analoginertial3 = 8,
         iethres_temperature     = 9,
         iethres_analogpsc       = 10,
-        iethres_analogpos       = 11
+        iethres_analogpos       = 11,
+        iethres_analogft        = 12
     } iethresType_t;
 
-    enum { iethresType_numberof = 12 };
+    enum { iethresType_numberof = 13 };
 
 
     class IethResource

src/libraries/icubmod/embObjLib/embObjGeneralDevPrivData.h

@@ -43,7 +43,7 @@ class yarp::dev::embObjDevPrivData
     inline  eth::TheEthManager* getEthManager()
     {return ethManager;}
 
-    inline bool isOpen() {return behFlags.opened;}
+    inline virtual bool isOpen() const {return behFlags.opened;}
     inline void setOpen(bool flag) {behFlags.opened=flag;}
     inline bool isVerbose() {return behFlags.verbosewhenok;}
 

src/libraries/icubmod/embObjLib/ethBoards.cpp

@@ -375,6 +375,8 @@ eth::IethResource* eth::EthBoards::get_interface(eOipv4addr_t ipv4, eOprotID32_t
                 type = iethres_analogpsc;
             else if(eoprot_entity_as_pos == en)
                 type = iethres_analogpos;
+            else if(eoprot_entity_as_ft == en)
+                type = iethres_analogft;                
             else
                 type = iethres_none;
         } break;

src/libraries/icubmod/embObjLib/ftInfo.cpp

@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ * Author: Luca Tricerri
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include "ftInfo.h"
+
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+
+bool FtInfo::toEomn(eOas_ft_sensordescriptor_t& out) const
+{
+    out.boardinfo.type = board;
+    out.canloc.addr = address;
+    out.canloc.insideindex = eobrd_caninsideindex_none;
+    out.boardinfo.firmware = {(uint8_t)majorFirmware, (uint8_t)minorFirmware, (uint8_t)buildFirmware};
+    out.boardinfo.protocol = {(uint8_t)majorProtocol, (uint8_t)minorProtocol};
+
+    try
+    {
+        if (port == 1)
+            out.canloc.port = eOcanport1;
+        else if (port == 2)
+            out.canloc.port = eOcanport2;
+        else
+        {
+            yError() << "FtInfo::toEomn() invalid can port";
+            out = eOas_ft_sensordescriptor_t();
+            return false;
+        }
+    }
+    catch (const std::exception& e)
+    {
+        yError() << "FtInfo::toEomn() invalid can port";
+        out = eOas_ft_sensordescriptor_t();
+        return false;
+    }
+
+    return true;
+}
+
+bool operator==(const FtInfo& right, const FtInfo& left)
+{
+    if (right.ftAcquisitionRate != left.ftAcquisitionRate)
+        return false;
+    if (right.temperatureAcquisitionRate != left.temperatureAcquisitionRate)
+        return false;
+    if (right.useCalibration != left.useCalibration)
+        return false;
+    if (right.board != left.board)
+        return false;
+    if (right.port != left.port)
+        return false;
+    if (right.address != left.address)
+        return false;
+    if (right.majorProtocol != left.majorProtocol)
+        return false;
+    if (right.minorProtocol != left.minorProtocol)
+        return false;
+    if (right.majorFirmware != left.majorFirmware)
+        return false;
+    if (right.minorFirmware != left.minorFirmware)
+        return false;
+    if (right.buildFirmware != left.buildFirmware)
+        return false;
+    return true;
+};
+
+bool operator!=(const FtInfo& right, const FtInfo& left)
+{
+    return !(right == left);
+}

src/libraries/icubmod/embObjLib/ftInfo.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ * Author: Luca Tricerri
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#ifndef _FTINFO_H_
+#define _FTINFO_H_
+
+#include <string>
+
+#include "EoAnalogSensors.h"
+#include "EoBoards.h"
+#include "EoManagement.h"
+#include "EoMotionControl.h"
+
+class FtInfo
+{
+   public:
+    uint8_t ftAcquisitionRate{0};
+    uint8_t temperatureAcquisitionRate{0};
+    eOas_ft_mode_t useCalibration{eoas_ft_mode_calibrated};
+    eObrd_type_t board{eobrd_unknown};
+    int port{0};
+    int address{0};
+    int majorProtocol{0};
+    int minorProtocol{0};
+    int majorFirmware{0};
+    int minorFirmware{0};
+    int buildFirmware{0};
+
+    bool toEomn(eOas_ft_sensordescriptor_t& out) const;
+};
+
+bool operator==(const FtInfo& right, const FtInfo& left);
+bool operator!=(const FtInfo& right, const FtInfo& left);
+
+#endif  // include-guard

src/libraries/icubmod/embObjLib/hostTransceiver.cpp

@@ -949,7 +949,14 @@ void HostTransceiver::eoprot_override_as(void)
             EO_INIT(.tag)           eoprot_tag_as_pos_status,
             EO_INIT(.init)          NULL,
             EO_INIT(.update)        eoprot_fun_UPDT_as_pos_status
-        }
+        },
+        {   // eoprot_tag_as_ft_status_timedvalue: ...
+            EO_INIT(.endpoint)      eoprot_endpoint_analogsensors,
+            EO_INIT(.entity)        eoprot_entity_as_ft,
+            EO_INIT(.tag)           eoprot_tag_as_ft_status_timedvalue,
+            EO_INIT(.init)          NULL,
+            EO_INIT(.update)        eoprot_fun_UPDT_as_ft_status_timedvalue
+        },
 
 #if 0   // marco.accame: i keep the code just for the debug phase.       
         ,{   // eoprot_tag_as_inertial_status_accelerometer

src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolAS_fun_userdef.c

@@ -65,6 +65,8 @@ static void handle_data_psc(const EOnv* nv, const eOropdescriptor_t* rd);
 
 static void handle_data_pos(const EOnv* nv, const eOropdescriptor_t* rd);
 
+static void handle_data_ft(const EOnv* nv, const eOropdescriptor_t* rd);
+
 // --------------------------------------------------------------------------------------------------------------------
 // - definition (and initialisation) of static variables
 // --------------------------------------------------------------------------------------------------------------------
@@ -151,6 +153,14 @@ extern void eoprot_fun_UPDT_as_pos_status(const EOnv* nv, const eOropdescriptor_
     }
 }
 
+extern void eoprot_fun_UPDT_as_ft_status_timedvalue(const EOnv* nv, const eOropdescriptor_t* rd)
+{ 
+    if(eo_ropcode_sig == rd->ropcode)
+    {
+        handle_data_ft(nv, rd);
+    }
+}
+
 
 
 // --------------------------------------------------------------------------------------------------------------------
@@ -199,6 +209,12 @@ static void handle_data_pos(const EOnv* nv, const eOropdescriptor_t* rd)
     feat_manage_analogsensors_data(eo_nv_GetIP(nv), rd->id32, (void *)posstatus);
 }
 
+static void handle_data_ft(const EOnv* nv, const eOropdescriptor_t* rd)
+{
+    eOas_ft_status_t *ftstatus  = (eOas_ft_status_t*)rd->data;
+    feat_manage_analogsensors_data(eo_nv_GetIP(nv), rd->id32, (void *)ftstatus);
+}
+
 // --------------------------------------------------------------------------------------------------------------------
 // - end-of-file (leave a blank line after)
 // --------------------------------------------------------------------------------------------------------------------

src/libraries/icubmod/embObjLib/serviceParser.cpp

@@ -30,7 +30,6 @@
 #include <yarp/os/LogStream.h>
 #include "EoAnalogSensors.h"
 
-
 #include "EoProtocol.h"
 #include "EoProtocolMN.h"
 #include "EoProtocolAS.h"
@@ -1440,8 +1439,8 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon
     ftconfig.ethservice.configuration.data.as.strain.canloc.port = thestrain_sensor.location.can.port;
     ftconfig.ethservice.configuration.data.as.strain.canloc.addr = thestrain_sensor.location.can.addr;
     ftconfig.ethservice.configuration.data.as.strain.canloc.insideindex = eobrd_caninsideindex_none;
-    
-    
+
+
 
     Bottle b_SERVICE(config.findGroup("SERVICE")); //b_SERVICE and b_SETTINGS could not be null, otherwise parseService function would have returned false
     Bottle b_SETTINGS = Bottle(b_SERVICE.findGroup("SETTINGS"));
@@ -1456,7 +1455,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon
         ftconfig.temperatureAcquisitionrate = b_SETTINGS_temp.get(1).asInt32();
         //TODO: chek that the acquisition rate is inside a reasonable range
     }
-
+        
     return true;
 }
 
@@ -4726,11 +4725,6 @@ bool ServiceParser::parseService2(Searchable &config, servConfigMC_t &mcconfig)
 
 */
 
-
-
-
-
-
     memcpy(&jc->joint2motor, &m1, sizeof(m1));
     memcpy(&jc->encoder2joint, &m2, sizeof(m2));
 

src/libraries/icubmod/embObjLib/serviceParser.h

@@ -41,7 +41,7 @@ typedef struct
     eOmn_serv_parameter_t   ethservice;
     int                     acquisitionrate;
     bool                    useCalibration;
-    std::string                  nameOfStrain;
+    std::string             nameOfStrain;
     eObrd_cantype_t         boardType;
     int                     temperatureAcquisitionrate;
 } servConfigFTsensor_t;
@@ -323,7 +323,8 @@ class ServiceParser
     servAScollector_t           as_service;
     servASstrainSettings_t      as_strain_settings;
 
-    servSKcollector_t           sk_service;
+    servSKcollector_t             sk_service;
+    eOmn_serv_config_data_sk_skin_t tmp;
 
 #if defined(SERVICE_PARSER_USE_MC)
     servMCcollector_t           mc_service;
@@ -345,7 +346,5 @@ class ServiceParser
     // suggestion: split check_motion() in sub-methods which parse the groups ...
 };
 
-
-
 #endif