Replace `embObjInertial` w/ `embObjIMU` – Stint 1 about robots-configuration

[Nicogene]

Alongside @marcoaccame, yesterday we traced back the development of embObjInertials and embObjIMU and we checked what are the differences between the two.

embObjInertials is the old device that was written some time ago originally for publishing acc and gyro data of imu of ems and mtb3 via AnalogServer; it has been adapted for publishing also via MultipleAnalogSensorsServer.
embObjIMU on the other hand is more recent and supports only publishing via MultipleAnalogSensorsServer, and it is used for publishing the data from the boards that basically mounts the bno055 imu:

• mtb4
• rfe

They rely on two different firmware services:

• eomn_serv_AS_inertials is used by embObjInertials
• eomn_serv_AS_inertials3 is used by embObjIMU

On FW-side eomn_serv_AS_inertials3 has been designed for covering all the functionalities of its predecessor, but for completely substitute it we need first some changes on icub-main.
At the end of this refactoring, we would have only eomn_serv_AS_inertials3, saving some memory in our boards.

In particular, we need:

• embObjIMU should handle also eth boards (gyro and acc of ems, see ref)
• Add in serviceParser the handle of the cases handled by embObjInertials (see ref we need these cases)
• Right now embObjIMU make the convertion raw->metric using one conversion factor (see ref). This was fine until the only imu used was the bno055 unit, but if it has to support also the one in ems and mtb3 this conversion factor has to change in function on the type of the board

cc @pattacini @valegagge @traversaro

[Nicogene]

Here is the PR on robots-configuration side:

• #540

Note that this is an important step in the analogServer removal.

cc @randaz81 @traversaro

[Nicogene]

Today w/ the help of @sgiraz and @davidetome I managed to build up a setup for testing the changes of these PRs.

I found and fixed a bug:

• robotology/icub-main@ae34a68 (#894)

And I used these xml files for testing it

• Nicogene@7447c44

Unfortunately, it is not working, I get this log

[DEBUG] Reading file C:\Users\ngenesio\robotology\robots-configuration\experimentalSetups\ems_mtb\.\new_forearm-no_hand.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file C:\Users\ngenesio\robotology\robots-configuration\experimentalSetups\ems_mtb\.\new_forearm-no_hand.xml
[INFO] Yarprobotinterface was started using the following enable_tags:
[INFO] Yarprobotinterface was started using the following disable_tags:
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags:
[DEBUG] Preprocessor complete in:  0.0400258 s
[INFO] |yarp.os.Port|/nfa/yarprobotinterface| Port /nfa/yarprobotinterface active at tcp://10.0.1.104:10002/
[INFO] startup phase starting...
[INFO] Opening device left_arm-eb2-j4_15-inertials with parameters [("robotName" = "/nfa"), ("GENERAL" [group] = "(skipCalibration false) (useRawEncoderData false) (useLimitedPWM false) (verbose false)"), ("PC104" [group] = "(PC104IpAddress 10.0.1.104) (PC104IpPort 12345) (PC104TXrate 1) (PC104RXrate 5)"), ("ETH_BOARD" [group] = "(ETH_BOARD_PROPERTIES (IpAddress 10.0.1.1) (IpPort 12345) (Type ems4) (maxSizeRXpacket 768) (maxSizeROP 384)) (ETH_BOARD_SETTINGS (Name "wrist-eb2-j0_2") (RUNNINGMODE (period 1000) (maxTimeOfRXactivity 400) (maxTimeOfDOactivity 300) (maxTimeOfTXactivity 300) (TXrateOfRegularROPs 5))) (ETH_BOARD_ACTIONS (MONITOR_ITS_PRESENCE (enabled true) (timeout 0.020) (periodOfMissingReport 60.0)))"), ("SERVICE" [group] = "(type eomn_serv_AS_inertials3) (PROPERTIES (CANBOARDS (type eobrd_mtb) (PROTOCOL (major 0) (minor 0)) (FIRMWARE (major 0) (minor 0) (build 0))) (SENSORS (id on_eb2_gyros l_upper_arm_4) (type eoas_gyros_st_l3g4200d eoas_accel_mtb_int) (boardType ems4 mtb) (location ETH:0 CAN2:10))) (SETTINGS (acquisitionRate 50) (enabledSensors on_eb2_gyros l_upper_arm_4))")]
[DEBUG] |yarp.dev.PolyDriver|left_arm-eb2-j4_15-inertials| Parameters are (ETH_BOARD (ETH_BOARD_PROPERTIES (IpAddress "10.0.1.1") (IpPort 12345) (Type ems4) (maxSizeRXpacket 768) (maxSizeROP 384)) (ETH_BOARD_SETTINGS (Name wrist-eb2-j0_2) (RUNNINGMODE (period 1000) (maxTimeOfRXactivity 400) (maxTimeOfDOactivity 300) (maxTimeOfTXactivity 300) (TXrateOfRegularROPs 5))) (ETH_BOARD_ACTIONS (MONITOR_ITS_PRESENCE (enabled true) (timeout 0.02) (periodOfMissingReport 60.0)))) (GENERAL (skipCalibration false) (useRawEncoderData false) (useLimitedPWM false) (verbose false)) (PC104 (PC104IpAddress "10.0.1.104") (PC104IpPort 12345) (PC104TXrate 1) (PC104RXrate 5)) (SERVICE (type eomn_serv_AS_inertials3) (PROPERTIES (CANBOARDS (type eobrd_mtb) (PROTOCOL (major 0) (minor 0)) (FIRMWARE (major 0) (minor 0) (build 0))) (SENSORS (id on_eb2_gyros l_upper_arm_4) (type eoas_gyros_st_l3g4200d eoas_accel_mtb_int) (boardType ems4 mtb) (location "ETH:0" "CAN2:10"))) (SETTINGS (acquisitionRate 50) (enabledSensors on_eb2_gyros l_upper_arm_4))) (device embObjIMU) (id left_arm-eb2-j4_15-inertials) (robotName "/nfa")
[DEBUG] eth::parser::print(pc104Data) for PC104:
[DEBUG] PC104/PC104IpAddress:PC104IpPort =  10.0.1.104:12345
[DEBUG] PC104/PC104TXrate =  1
[DEBUG] PC104/PC104RXrate =  5
[DEBUG] EthSender is a PeriodicThread with txrate = 1 ms
[DEBUG] EthReceiver is a PeriodicThread with rxrate = 5 ms
[WARNING] in EthReceiver::config() the config socket has queue size =  1048576 ; you request ETHRECEIVER_BUFFER_SIZE=
[DEBUG] eth::parser::print(boardData) for BOARD wrist-eb2-j0_2
[DEBUG] ETH_BOARD/ETH_BOARD_PROPERTIES:
[DEBUG] ETH_BOARD/ETH_BOARD_PROPERTIES/IpAddress =  10.0.1.1
[DEBUG] ETH_BOARD/ETH_BOARD_PROPERTIES/IpPort =  12345
[DEBUG] ETH_BOARD/ETH_BOARD_PROPERTIES/Type =  ems4
[DEBUG] ETH_BOARD/ETH_BOARD_PROPERTIES/maxSizeRXpacket =  768
[DEBUG] ETH_BOARD/ETH_BOARD_PROPERTIES/maxSizeROP =  384
[DEBUG] ETH_BOARD/ETH_BOARD_SETTINGS:
[DEBUG] ETH_BOARD/ETH_BOARD_SETTINGS/Name =  wrist-eb2-j0_2
[DEBUG] ETH_BOARD/ETH_BOARD_SETTINGS/RUNNINGMODE/(period, maxTimeOfRXactivity, maxTimeOfDOactivity, maxTimeOfTXactivity, TXrateOfRegularROPs) =  1000 400 300 300 5
[DEBUG] ETH_BOARD/ETH_BOARD_ACTIONS/MONITOR_ITS_PRESENCE
[DEBUG] ETH_BOARD/ETH_BOARD_ACTIONS/MONITOR_ITS_PRESENCE/(enabled, timeout, periodOfMissingReport) =  true 0.02 60
[DEBUG] eth::EthMonitorPresence::config(): monitoring of presence is ON for BOARD 10.0.1.1 (wrist-eb2-j0_2) with timeout = 0.02 sec and period of missing report = 60 sec
[DEBUG] TheEthManager::requestResource2(): has just succesfully created a new EthResource for board of type ems4 with IP =  10.0.1.1
[WARNING] ServiceParser::check_analog() cannot find PROPERTIES.SENSORS.sensorName
[DEBUG] EthResource::verifyBoardPresence() found BOARD wrist-eb2-j0_2 with IP 10.0.1.1 after 0.0029291 seconds
[DEBUG] EthResource::verifyBoardTransceiver() has validated the transceiver of BOARD wrist-eb2-j0_2 with IP 10.0.1.1
[DEBUG] EthResource::cleanBoardBehaviour() has cleaned the application in BOARD wrist-eb2-j0_2 with IP 10.0.1.1 : config mode + cleared all its regulars
[DEBUG] EthResource::setTimingOfRunningCycle() for BOARD wrist-eb2-j0_2 with IP 10.0.1.1 has succesfully set: cycletime = 1000 usec, RX DO TX = ( 400 300 300 ) usec and TX rate = 5  every cycle
[INFO] EthResource::askBoardVersion() found BOARD wrist-eb2-j0_2 @ IP 10.0.1.1 of type ems4 with FW = ver 3.69 built on 2023 Jul 03 12:20
[INFO]  from BOARD 10.0.1.1 (0#∞NΘ☺) @ 18s 282m 698u: CAN discovery has started for 1 eobrd_mtb boards on (can1map, can2map) = (0x0000, 0x0400) with target can protocol ver 0.0 and application ver 0.0.0.
[WARNING]  from BOARD 10.0.1.1 (0#∞NΘ☺) @ 18s 282m 869u: CAN discovery is OK but FAKE (without any control on CAN w/ get-fw-version<> message) for 1 eobrd_mtb boards with target can protocol ver 0.0 and application ver 0.0.0. Search time was 0 ms
[DEBUG]  from BOARD 10.0.1.1 (0#∞NΘ☺), src LOCAL, adr 0, time 18s 282m 994u: (code 0x05000030, par16 0x0000 par64 0x0000000000000000) -> CFG: EOtheInertial3 can be correctly configured. tbd + .
[INFO] |yarp.dev.PolyDriver|left_arm-eb2-j4_15-inertials| Created device <embObjIMU>. See C++ class yarp::dev::embObjIMU for documentation.
[INFO] Opening device left_arm-inertials_wrapper with parameters [("robotName" = "/nfa"), ("period" = "10"), ("name" = "/icub/left_hand/inertialMTB")]
[DEBUG] |yarp.dev.PolyDriver|left_arm-inertials_wrapper| Parameters are (device multipleanalogsensorsserver) (id left_arm-inertials_wrapper) (name "/icub/left_hand/inertialMTB") (period 10) (robotName "/nfa")
[INFO] |yarp.dev.PolyDriver|left_arm-inertials_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device left_arm-inertials_wrapper with parameters [("networks" = "(SetOfInertials)"), ("SetOfInertials" = "left_arm-eb2-j4_15-inertials")]
[INFO] left_arm-inertials_wrapper is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/icub/left_hand/inertialMTB/measures:o| Port /icub/left_hand/inertialMTB/measures:o active at tcp://10.0.1.104:10003/
[INFO] |yarp.os.Port|/icub/left_hand/inertialMTB/rpc:o| Port /icub/left_hand/inertialMTB/rpc:o active at tcp://10.0.1.104:10004/
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
[INFO] run phase starting...
[DEBUG] yarprobotinterface running happily
[WARNING]  from BOARD 10.0.1.1 (0#∞NΘ☺), src LOCAL, adr 0, time 19s 301m 912u: (code 0x00000039, par16 0x0005 par64 0x0000000000000400) -> SYS: a service has detected that some CAN boards are not broacasting anymore. In par16 the type of service, in par64 LS 32 bits the bit mask of lost board (CAN1 in MS 16 bits and CAN2 in LS 16 bits) + .

to be investigated

cc @marcoaccame

[Nicogene]

Today I tested the only-ems case w/ this configuration the yarprobotinterface runs smoothly and I got this output:

(((-33.0 16.0 -142.0) 442349.63277989998)) () () () () () () () () ()

The firmware of the rfe, mtb3, mtb4 has not been changed, the only thing that could potentially introduce regression is the refactoring on icub-main layer for the conversion factor of the different sensors (acc, gyro, mag, eul etc).

For this reason I tried to acquire from the rfe using icub-main @ devel and icub-main w/ our refactoring and as you can see the result is ~ identical

icub-main@devel

(((0.125 0.0 -0.0625) 442008.68344559998)) (((-0.68999999999999995 1.6699999999999999 -9.6300000000000008) 442008.68344479997)) (((4.125e-05 3.2499999999999997e-05 7.5375000000000003e-05) 442008.68344539998)) (((170.0 3.5 -117.4375) 442008.68344579998)) () () () () () ()

icub-main@refactorEmbObjIMU

(((-0.0625 0.0 0.0) 442179.00701950002)) (((-0.68000000000000005 1.6899999999999999 -9.6400000000000006) 442179.00701900001)) (((4.125e-05 3.2874999999999999e-05 7.5500000000000006e-05) 442179.00701930001)) (((170.0 3.4375 -117.4375) 442179.00701970002)) () () () () () ()

This is the metadata:

((rfeimu_gyro rfeimu_gyro "")) ((rfeimu_acc rfeimu_acc "")) ((rfeimu_mag rfeimu_mag "")) ((rfeimu_eul rfeimu_eul "")) () () () () () ()

I am satisfied for the result we achieved, we can think do move the remaining steps in other stints

cc @marcoaccame @pattacini

[Nicogene]

The first PR has been opened:

• robotology/icub-main#894

[marcoaccame]

[WARNING] from BOARD 10.0.1.1 (0#∞NΘ☺) @ 18s 282m 869u: CAN discovery is OK but FAKE (without any control on CAN w/ get-fw-version<> message) for 1 eobrd_mtb boards with target can protocol ver 0.0 and application ver 0.0.0. Search time was 0 ms

you should avoid a fake discovery in testing. it may be that the mtb does not work and you don't know.

Luckily, there is

[WARNING] from BOARD 10.0.1.1 (0#∞NΘ☺), src LOCAL, adr 0, time 19s 301m 912u: (code 0x00000039, par16 0x0005 par64 0x0000000000000400) -> SYS: a service has detected that some CAN boards are not broacasting anymore. In par16 the type of service, in par64 LS 32 bits the bit mask of lost board (CAN1 in MS 16 bits and CAN2 in LS 16 bits) + .

telling you that. I suggest to add a proper application and protocol version in teh xml and try again. Also, using the CAN sniffer can help.

[Nicogene]

Let's revive this task. I have rebased and pushed all the dev branches, here are the PRs:

• #540
• robotology/icub-main#894
• robotology/icub-firmware#439

[Nicogene]

"Merge" of eo_inertials2_TxStart/Stop and eo_inertials_3_TxStart/Stop, now we are invoking the 2 for this test

Alongside @marcoaccame we managed to accomplish this task, and in the latest commit in icub-main I refactored embObjIMU in order to remove the class imuMeasureConverter, now the factor is handled as a simple double of sensorInfo_t class.

We managed to run a yarprobotinterface that published both rfe imu data, mtb accel and ems gyro all togheter just using the inertial3 service 🚀

[Nicogene]

Today I tried to make it possible to configurate and activate the EOInertials3 service without can boards attached and only the ems gyro, since the old service allowed to do it.

Here is the commit that should do it, but it fails:

• robotology/icub-firmware@a63cb5e

On icub-main side I get just this error that is not very informative:

[ERROR] embObjIMU: BOARD wrist-eb2-j0_2 (IP 10.0.1.1)  open() fails to start as service.... cannot continue
[ERROR] |yarp.dev.PolyDriver|left_arm-eb2-j4_15-inertials| Driver <embObjIMU> was found but could not open

Having just one can board enabled make yarprobotinterface run happily

[marcoaccame]

I can have a look at that

[marcoaccame]

Today I tried to make it possible to configurate and activate the EOInertials3 service without can boards attached and only the ems gyro, since the old service allowed to do it.

Here is the commit that should do it, but it fails:

• robotology/icub-firmware@a63cb5e

On icub-main side I get just this error that is not very informative:

[ERROR] embObjIMU: BOARD wrist-eb2-j0_2 (IP 10.0.1.1)  open() fails to start as service.... cannot continue
[ERROR] |yarp.dev.PolyDriver|left_arm-eb2-j4_15-inertials| Driver <embObjIMU> was found but could not open

Having just one can board enabled make yarprobotinterface run happily

Hi @Nicogene, this commit should have fixed. Will you pls test it?

[Nicogene]

The first 3 PR can be safely merged, I will fix the one on robots-configuration and mark it as ready asap

Everything should be now good to go 🚀

[Nicogene]

Alongside @marcoaccame, today we did an in-deep analysis of the situation of service support for the boards.
The reason of

WARNING] from BOARD 10.0.1.1 (0#∞NΘ☺), src LOCAL, adr 0, time 19s 301m 912u: (code 0x00000039, par16 0x0005 par64 0x0000000000000400) -> SYS: a service has detected that some CAN boards are not broacasting anymore. In par16 the type of service, in par64 LS 32 bits the bit mask of lost board (CAN1 in MS 16 bits and CAN2 in LS 16 bits) + .

is that eomn_serv_AS_inertials3 sends can messages for start/stop broadcasting different to the one of eomn_serv_AS_inertials.
Right now w/ the current version of the FW of the ems is not possible to use eomn_serv_AS_inertials3 w/ mtb gyro/acc and ems gyro.

We also tested if the boards we are using are functional and using eomn_serv_AS_inertials and embObjInertials everything works fine.

The next action point is to copy/paste in eomn_serv_AS_inertials3 the messages that mtb expect and test embObjIMU. If everything works fine we should merge eomn_serv_AS_inertials in eomn_serv_AS_inertials3 for handling the 2 different protocols in function of the type of the board. This latter step will be done/tracked in a separate issue since requires important changes

[Nicogene]

Today alongside @marcoaccame we did more tweaks/fixes for using embObjIMU w/ mtb:

• robotology/icub-main@8478238 (#894)
• robotology/icub-firmware@d50044c
• robotology/icub-firmware@55061a1

Now the yarprobotinterface starts fine but no data is published by the multipleanalogsensorsserver, this is due to the fact we are falling in this if:

• https://github.com/robotology/icub-main/blob/6cdeaf1b8ff245dcf5b03cd7cdbb59bc6d0a2633/src/libraries/icubmod/embObjIMU/embObjIMU.cpp#L307-L321

Probably there is something to be checked in the eo_inertials3_Tick function because we are receiving from the board "strange" id and board types:

[ERROR] NOT VALID value[0] is: seq = 0, timestamp = 2131344, type = eoas_gyros_mtb_ext, id = 26, v= ((0), -1, -1, -1), status = 0

From the board the data is coming:

🔴 gyro of mtb (disabled)
🟢 acc of mtb

We are configuring eoas_acc_mtb_int and id 26 is off.
To be further investigated

[Nicogene]

@marcoaccame kindly assisted me during a debug activity on the firmware and w/ this commit:

• robotology/icub-firmware@ad67dbc

We should be (almost) fine on FW side.
This problem was due to the fact that we were configuring the wrong devices since we missed some maps in the firmware.

Now we get the data both from mtb accel (only 0s but we verified that's correct, because the not valid/configured is all -1) and ems gyro

[INFO] VALID value[0] is: seq = 0, timestamp = 1275921, type = eoas_gyros_st_l3g4200d, id = 0, v= ((0), -67, 5, -108), status = 0
[INFO] VALID value[0] is: seq = 0, timestamp = 1275924, type = eoas_accel_mtb_int, id = 13, v= ((0), 0, 0, 0), status = 0

What we miss:

• "Merge" of eo_inertials2_TxStart/Stop and eo_inertials_3_TxStart/Stop, now we are invoking the 2 for this test
• On icub-main side make consider also eoas_accel_mtb_int, eoas_accel_mtb_ext, eoas_gyros_mtb_ext, eoas_gyros_st_l3g4200d as a type of data ok to be published by MASServer
• Manage the case of only ems gyro enabled
• Remove all inertials2 references in icub-firmware
• Remove embObjInertial from icub-main

[Nicogene]

On icub-main side make consider also eoas_accel_mtb_int, eoas_accel_mtb_ext, eoas_gyros_mtb_ext, eoas_gyros_st_l3g4200d as a type of data ok to be published by MASServer

It has been done in this commit:

• robotology/icub-main@e274118

On the low level I didn't change anything, I added a logic to remap the sens_index on the different types of gyros and acceleration (legacy and "new").

It works both w/ ems firmware modified + gyro ems + acc mtb and ems firmware @ devel + accel mtb4.

I will try to read both mtb and mtb4 gyro/acc after the merge of inertials2 and inertials3 is completed

[Nicogene]

Today I tried to make it possible to configurate and activate the EOInertials3 service without can boards attached and only the ems gyro, since the old service allowed to do it.
Here is the commit that should do it, but it fails:

• robotology/icub-firmware@a63cb5e

On icub-main side I get just this error that is not very informative:

[ERROR] embObjIMU: BOARD wrist-eb2-j0_2 (IP 10.0.1.1)  open() fails to start as service.... cannot continue
[ERROR] |yarp.dev.PolyDriver|left_arm-eb2-j4_15-inertials| Driver <embObjIMU> was found but could not open

Having just one can board enabled make yarprobotinterface run happily

Hi @Nicogene, this commit should have fixed. Will you pls test it?

Yes sure! I will test it on wednesday!

/remind Wednesday 22 November

[octo-reminder]

⏰ Reminder
Wednesday, November 22, 2023 10:00 AM (GMT+01:00)

[octo-reminder]

🔔 @Nicogene

[Nicogene]

All the PRs

• robotology/icub-main#894
• robotology/icub-firmware#439
• robotology/icub-firmware-build#117
• #540

Are ready to be merged except the one on robots-configuration because while working on this activity w/ @marcoaccame we noticed that eoas_accel_st_lis3x is not handled either in EoInertial2 and EoInertials3 but it is in several xml files, see:

• icub-firmware: https://github.com/search?q=repo%3Arobotology%2Ficub-firmware%20eoas_accel_st_lis3x%20%20%20%20%20&type=code
• robots-configuration: https://github.com/search?q=repo%3Arobotology%2Frobots-configuration%20eoas_accel_st_lis3x%20%20%20%20%20&type=code

The first 3 PR can be safely merged, I will fix the one on robots-configuration and mark it as ready asap

cc @pattacini @marcoaccame