Publish `odom` data.

[AndyBlightLeeds]

This is purely derived from the encoders and knowledge about the wheel size and the robot dimensions.

[AndyBlightLeeds]

Sub tasks.

1. Add odom publisher with fake data.
2. Add encoder driver to raspi_robot.
3. Connect encoder output to odom publisher. This needs creation of a file that describes certain features of the robot, e.g. wheel circumference. The file is then used in the odometry calculations.

[AndyBlightLeeds]

Now publishes empty odom message.

[AndyBlightLeeds]

Encoder hardware is here: http://www.pridopia.co.uk/pi-motor-encoder.html.

[AndyBlightLeeds]

Encoders generate pulses but occasionally generate many spurious pulses, see #14.

[AndyBlightLeeds]

Encoder counts are now tied to motor direction. The odometry is still wrong.
Do a run by resetting ESP and then sending this command:

ros2 topic pub -1 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2}, angular: {z: 0.0}}"

This message requests 0.2ms. Each cmd_vel is only run for 1 second maximum so we should see the robot move forward about 20cm, measured 22.5cm with a slight twist, about 10 degrees) to the left when the battery was around 8V. The odometry being output is very wrong, see below:

(14144) app_messages: Received: forward 0.200000, rotate 0.000000
(14144) raspi_robot_motors: motors_drive, left 40, right -40, ticks 10
(14994) app_messages: Odom encoders: left 30, right 29
(14994) app_messages: Odom speed: left 36.000000, right 36.000000
(14994) app_messages: Odom velocity: x 35.400002, y 0.000000, theta 708.000000
(15004) app_messages: Odom pose: x -14.729658, y -32.190018, theta 708.000000
(15964) app_messages: Odom encoders: left 13, right 15
(15974) app_messages: Odom speed: left 15.600000, right 15.600000
(15974) app_messages: Odom velocity: x 16.799999, y 0.000000, theta 335.999969
(15984) app_messages: Odom pose: x -31.339893, y -29.672075, theta 1044.000000

The encoder ticks are about right, a total of 43 and 44, which also explains the slight twist to the left. Some sort of control on the motors would be really handy to make it go in a straight line!

The odom pose values are completely wrong. The x value should be +0.22m and the y value should be -0.01m (-ve is left). Looking at the previous stages, the speed values are roughly even, have a positive value but 36m/s is completely wrong! Going back to the code.

[AndyBlightLeeds]

After reviewing the constants used for odometry, I fixed many silly mistakes and used real values for wheel diameter and wheel base. Now I get this:

(123584) app_messages: Received: forward 0.200000, rotate 0.000000
...
(124534) app_messages: Odom encoders: left 42, right 42
(124534) app_messages: Odom speed: left 0.747677, right 0.747677
(124534) app_messages: Odom velocity: x 0.747677, y 0.000000, theta inf
(124544) app_messages: Odom pose: x nan, y nan, theta nan
...
(125514) app_messages: Odom encoders: left 12, right 13
(125514) app_messages: Odom speed: left 0.213622, right 0.213622
(125514) app_messages: Odom velocity: x 0.222523, y 0.000000, theta inf
(125524) app_messages: Odom pose: x nan, y nan, theta nan

One major improvement in the that y velocity is now zero but he velocity should be around 0.2m/s not 0.74m/s. I have no idea what happened to the pose values. Next job to do.