wrenchStamped_nws_ros2 providing wrong header on ros2 messages #69
Description
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>