Homework of FRA638 (Mobile Robotics)

slam_filter_motor.py

from math import sin, cos, pi
from lego_robot import *
from pylab import *

# This function takes the old (x, y, heading) pose and the motor ticks
# (ticks_left, ticks_right) and returns the new (x, y, heading).
def filter_step(old_pose, motor_ticks, ticks_to_mm, robot_width,
                scanner_displacement):

    r = motor_ticks[1] * ticks_to_mm
    l = motor_ticks[0] * ticks_to_mm
    w = robot_width
    x = old_pose[0]
    y = old_pose[1]
    theta = old_pose[2]
    # Find out if there is a turn at all.
    #  Left == Right
    if motor_ticks[0] == motor_ticks[1]:
        # No turn. Just drive straight.
        xp = x + (r * cos(theta))
        yp = y + (l * sin(theta))
        thetap = theta
        return (xp, yp, thetap)

    else:
        # Turn. Compute alpha, R, etc.
        alpha = (r - l) / w
        R = l / alpha
        cx = x - (R+ (w/2)) * sin(theta)
        cy = y - (R+ (w/2)) * -cos(theta)
        thetap = (theta+alpha) % (2*pi)
        xp = cx + (R+(w/2)) * sin(thetap)
        yp = cy + (R+(w/2)) * -cos(thetap)
        return (xp, yp, thetap)

if __name__ == '__main__':
    # Empirically derived distance between scanner and assumed
    # center of robot.
    scanner_displacement = 30.0

    # Empirically derived conversion from ticks to mm.
    ticks_to_mm = 0.349

    # Measured width of the robot (wheel gauge), in mm.
    robot_width = 150.0

    # Measured start position.
    # X , Y , Theta
    pose = (0.0, 0.0, 0.0)

    # Read data.
    logfile = LegoLogfile()
    logfile.read("/Users/Svl2Nuk3/Desktop/Unit_A/robot4_motors.txt")

    # Loop over all motor tick records generate filtered position list.
    filtered = []
    for ticks in logfile.motor_ticks:
        pose = filter_step(pose, ticks, ticks_to_mm, robot_width,
                           scanner_displacement)
        filtered.append(pose)

    # Write all filtered positions to file.
    f = open("/Users/Svl2Nuk3/Desktop/Unit_A/poses_from_ticks.txt", "w")
    for pose in filtered:
        print >> f, "F %f %f %f" % pose
        print pose
        plot([p[0] for p in filtered],[p[1] for p in filtered],'bo')
    f.close()
    show()