Odometry position calculations

gistfile1.txt

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)