search

robotology / yarp-devices-ros2

Plugins, devices and network wrappers for Yarp-ROS2 interoperation
http://www.yarp.it
10 stars 8 forks source link

`wrenchStamped_nws_ros2` providing wrong header on ros2 messages #69

Open SimoneMic opened 3 months ago

SimoneMic commented 3 months ago

As per title, the wrenchStamped_nws_ros2 on ergocub configuraton (on both feet) provides a wrong header, with negative stamp and no frame_id. On the YARP side everything seems fine, so I think that is a device issue.

This is a sample of the message from a read on the cmd: yarp read ... /ergocub/right_leg/FT/measures:o

() () () () (((40.4000000000000056843) 1717078148.42488503456) ((28.3000000000000007105) 1717078148.43069005013) 
((27.8000000000000007105) 1717078148.42868113518)) 
(((-91.827392578125 -61.1572265625 -232.91015625 -7.34710693359375 5.9326171875 1.13983154296875) 7476983358.0) 
((-102.447509765625 -25.360107421875 -215.88134765625 -2.67791748046875 10.22186279296875 -1.13067626953125) 7476991856.0) 
((148.223876953125 87.799072265625 -496.58203125 -10.19439697265625 0.90179443359375 -1.93634033203125) 7476989847.0)) 
() () () ()

Meanwhile, if I echo the topic from ros2 topic echo /left_foot_heel_tiptoe_ft I have:

header:
  stamp:
    sec: -2147483648
    nanosec: 0
  frame_id: ''
wrench:
  force:
    x: -92.83447265625
    y: -25.360107421875
    z: -130.37109375
  torque:
    x: -2.80609130859375
    y: 0.25634765625
    z: 0.0

Meanwhile, another topic (like /tf) has the following header:

transforms:
- header:
    stamp:
      sec: 1717079483
      nanosec: 308000087
    frame_id: odom
  child_frame_id: root_link
  transform:
    translation:
      x: 1.5290903526151591
      y: 0.035388356263611205
      z: 0.7515624213150165
    rotation:
      x: -0.0018447978502054816
      y: -0.03913718579379549
      z: -0.04541550995990248
      w: 0.9981995335924154
- header:
    stamp:
      sec: 1717079483
      nanosec: 308000087
    frame_id: l_sole
  child_frame_id: geometric_unicycle
  transform:
    translation:
      x: 0.00022424499583761914
      y: -0.09999400839366368
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 3.95930088464666e-05
      w: 0.9999999992161969

This is the config file of the device (link):

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
        <param name="topic_name">      /left_foot_heel_tiptoe_ft </param>
        <param name="node_name">      ergocub_left_foot_heel_tiptoe_ft          </param>
        <param name="period">      0.002                          </param>

        <action phase="startup" level="10" type="attach">
            <paramlist name="networks">
        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
                <elem name="FirstStrain">  left_leg-eb9-j4_5-strain  </elem>
            </paramlist>
        </action>

        <action phase="shutdown" level="15" type="detach" />
    </device>

And the subdevice:

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_leg-eb9-j4_5-strain" type="embObjMultipleFTsensors">

        <xi:include href="../../general.xml"/>

        <xi:include href="../../hardware/electronics/left_leg-eb9-j4_5-eln.xml" />

        <group name="SERVICE">

            <param name="type"> eomn_serv_AS_ft </param>

            <group name="PROPERTIES">

                <group name="CANBOARDS">
                    <param name="type">                 strain2            </param>

                    <group name="PROTOCOL">
                        <param name="major">            2                       </param>    
                        <param name="minor">            0                   </param>     
                    </group>                    
                    <group name="FIRMWARE">
                        <param name="major">            2                       </param>    
                        <param name="minor">            3                   </param>
                        <param name="build">            0              </param>
                    </group>
                </group>

                <group name="SENSORS">
                    <param name="id">                   l_foot_rear_ft  l_foot_front_ft   </param>
                    <param name="board">                 strain2        strain2             </param>
                    <param name="location">             CAN2:13     CAN2:14                 </param>
                </group>                

            </group>

            <group name="SETTINGS">        
                <param name="enabledSensors">       l_foot_rear_ft          l_foot_front_ft      </param>
                <param name="ftPeriod">             2                10                     </param>
                <param name="temperaturePeriod">    1000             1000                    </param>
                <param name="useCalibration">       true             true                 </param>           
            </group>       

            <group name="CANMONITOR">        
                <param name="checkPeriod">      100     </param>
                <param name="reportMode">       ALL     </param>
                <param name="ratePeriod">       20000 </param>
            </group>              

        </group>

  </device>
randaz81 commented 3 months ago

It looks like that: In yarp the timestamp is read from the subdevice but not used, instead it is assigned a generated timestamp: https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp#L565

In ros2 the timestamp is the one obtained by the subdevice. https://github.com/robotology/yarp-devices-ros2/blob/3a1f4de56e6ff39c8d7feb50dcec6c07627d5905/src/devices/multipleAnalogSensors_nws_ros2/WrenchStamped_nws_ros2.cpp#L24

I thus think that we can consider two different issues here:

traversaro commented 2 months ago

In yarp the timestamp is read from the subdevice but not used, instead it is assigned a generated timestamp: https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp#L565

Good point, we indeed have the timestamp attribute in the message so we should just use it: https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift#L15 .

traversaro commented 2 months ago

In yarp the timestamp is read from the subdevice but not used, instead it is assigned a generated timestamp: https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp#L565

Good point, we indeed have the timestamp attribute in the message so we should just use it: https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift#L15 .

Looking a bit more, I think the timestamps are properly handled on YARP, on the server side in https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp#L475-L482 and on the client side in https://github.com/robotology/yarp/blob/93ac24db717e847304e200b0be807671cad63e89/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp#L227 . The stamp added to the YARP message is never used by the client, but if anyone is interested in modifying its logic I would be happy to point on the right modifications to do.