Created
September 24, 2022 16:26
-
-
Save JasonRBowling/c7162b93d58c10ff88ec1d414e76a434 to your computer and use it in GitHub Desktop.
Odometry position calculations
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
def l_tick_cb(msg): | |
global wheel_diameter | |
global l_distance | |
global l_vel | |
ticks = msg.data | |
l_distance = ((ticks / 32.0) / 56.0) * (math.pi * wheel_diameter) | |
l_vel = l_distance * 20.0 | |
l_rpm_actual = (ticks / 32.0) * 20.0 * 60.0 | |
#print("l_vel: " + str(l_vel)) | |
# we have both numbers of ticks - update odometry position | |
#if (r_distance != 0.0) or (l_distance != 0.0): | |
updateOdom() | |
def updateOdom(): | |
# motor controller publishes right ticks, then left, so call this after left tick | |
global theta | |
global l_distance | |
global r_distance | |
global wheel_base | |
global x_position | |
global y_position | |
global odom_pub | |
global odom_broadcaster | |
dCenter = (l_distance + r_distance) / 2.0 | |
# calculate change in angle | |
phi = (r_distance - l_distance) / wheel_base | |
theta += phi | |
# constrain _theta to the range 0 to 2 pi | |
if (theta > 2.0 * math.pi): | |
theta -= 2.0 * math.pi | |
if (theta < 0.0): | |
theta += 2.0 * math.pi | |
x_position += dCenter * math.cos(theta) | |
y_position += dCenter * math.sin(theta) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment