How does the teb_local_planner calculate path points for ackermann steering vehicles?

edit

Hello all,

I am trying to understand whether the steering angle and vehicle velocity commands make sense from the teb_local_planner given the local path poses and orientation. Trying to get an intuitive understanding of what's going on with conventional bicycle model equations. I can't seem to get the calculations right or are the results just an approximation of the kinematic model from teb_local_planner? I am not quite certain which is the case. Appreciate any help you guys can offer!

pose0 from the local_path output should be the initial position of the vehicle. (x,y,yaw) = (-21.87,11.4,-0.157), t_stamp=0.866s

pose1 from the local_path output should be the next position the vehicle is trying to reach. (x,y,yaw) = (-21.78,11.39,-0.102),t_stamp=0.866s according to my understanding positive yaw is CCW

The command is applied at t_stamp=0.866s is (cmd_vel,tire_wheel_angle) = (0.222,0.148) according to my understanding wheel_angle positive is CW

The bicycle model I am referring to is from pages 23 - 24. I would post the pictures here but my account is too new. "Chapter 2, Lateral Vehicle Dynamics", R. Rajamani, Vehicle Dynamics and Control, Mechanical Engineering Series, https://www.springer.com/cda/content/.... (2012)https://www.springer.com/cda/con.... (2012)

I am calculating the center of rotation from the rear axle lf = 2.1685 lr = 0 delta_f = tire_wheel_angle from teb_local_planner psi = yaw from teb_local_planner

From the calculations, I get the following (d_x, d_y, d_psi) = (1.1e-2,-3.3e-3,-7.5e-4). These values are after multiplying dt =0.05

Adding these values to pose0, these don't seem to be close to pose1. If anyone could help offer any insight, I would greatly appreciate your help.

Thank you!