Fix drive script

[5yler]

Currently throws a lot of nan outputs for steering angle and wheel velocities. Also does not account for LIDAR orientation.

Tasks

• [x] fix nan outputs
• [x] remove laser scan parameter hardcoding
• [x] adapt drive to account for LIDAR yaw angle
  • [x] optionally with tf listener
  • [x] alternatively with parameter for constant offset angle

[5yler]

Working on this in a Jupyter notebook

The scan above is also a good example of the random noise encountered in sunlight, as described in issue #7.

[5yler]

This is a little late but I just realized the differential drive math in the script is incorrect:

omega = (v * math.tan(steer_angle)) / wheelbase_width
v_left = v - (omega / wheelbase_width)   # left wheel velocity
v_right = v + (omega / wheelbase_width)   # right wheel velocity

The above is wrong in both the calculation of omega and the individual wheel velocities. Here is the corrected version:

omega = (v * math.tan(steer_angle)) / wheelbase_length
# v = center velocity
# omega is the same for the entire car, but the outer wheel will have a higher
# velocity during the turn because the radius is higher

# v = r_center * omega
# v_left = (r_center - 0.5 * wheelbase_width) * omega
# r_center = v / omega
v_left = v - 0.5 * wheelbase_width * omega
v_right = v + 0.5 * wheelbase_width * omega

For more detail, see the Planning for car-like robots tutorial for teb_local_planner, section

2.2 Translational velocity and steering angle

The relevant variables including the translational velocity v and the steering angle phi are illustrated in the following figure. ICR denotes the instant centre of rotation.

For a vanishing angular velocity omega=0 the turning radius r tends to infinity which in turn leads to a zero steering angle phi=0. Otherwise the turning radius r might be computed by v/omega. Then, the steering angle is derived by phi=atan(wheelbase/r) (which indeed includes the first case). Note, the steering angle is not defined for v=0 using the equation introduced above. One could use the last known valid steering angle. However, in the following we assume that for a vanishing translational velocity the (desired) steering angle is set to zero.

Note: their somewhat ambiguous wheelbase is actually wheelbase_length

[5yler]

Seems to be working properly now!