Wrong pose: how to debug ?

[rgrunbla]

Hello,

I'm exploring how to connect our turtlebot2 to ROS2 ( foxy ). I tried to use kobuki simple keyop, as provided by this repository, and it works well to control the robot but the current pose functionality seems to be broken, as the values never change:

$ kobuki-simple-keyop

Simple Keyop : Utility for driving kobuki by keyboard.

Parameters
----------
 - linear_vel_max   [1]
 - linear_vel_step  [0.05]
 - angular_vel_max  [6.6]
 - angular_vel_step [0.33]

Reading from keyboard
---------------------
Forward/back arrows : linear velocity incr/decr.
Right/left arrows : angular velocity incr/decr.
Spacebar : reset linear/angular velocities.
q : quit.

current pose: [x: 5.61871e-310, y: 6.9339e-310, heading: 6.95299e-310]
current pose: [x: 5.61871e-310, y: 6.9339e-310, heading: 6.95299e-310]   <- The kobuki base actually moves but the values do not change
[…]
current pose: [x: 5.61871e-310, y: 6.9339e-310, heading: 6.95299e-310]
current pose: [x: 5.61871e-310, y: 6.9339e-310, heading: 6.95299e-310]

Is there a way to debug this ? Is that normal given the current state of the drivers ?

On a side note: is this possible to control the robot from ROS2 right now ?

Thanks,
Rémy

Additional information:

$ kobuki-version-info

Version Info

 * Hardware Version: 1.0.4
 * Firmware Version: 1.2.0
 * Software Version: 1.3.1
 * Unique Device ID: 98107186-859195458-1125344564

[muratayd]

modify:
ecl::concatenate_poses(pose, pose_update);
with this:
pose = ecl::concatenate_poses(pose, pose_update);
Then, you will have pose updated. However, the heading (pose[2]) is inaccurate. This is still a bug to fix.