Introduction

The purpose of this programming assignment is to work through all of the steps of creating a ROS package using Python. This project will not require the use of the Turtlebots, so you should be able to complete it outside of the robotics lab.

The goal of your project will be to create a Python node that is able to navigate a simulated 2-d turtle to a designated goal location.

Setting Up Your Environment

Follow the steps from the Subversion and Launch Files Lab to checkout your personal svn repository, create a workspace, and create a package. Your repository name is the same as your eid. Your package should be named cs354_pa1 and it should depend on geometry_msgs, turtlesim, rospy and std_msgs

Creating A Python Node

Create a Python file named "turtle_nav.py" that moves the turtle in a smooth trajectory to a designated goal location. Ultimately, you will need to read the goal location from the parameter server, but to get started you can just hard-code a goal location in your Python code. You should be able to test your node by starting the turtlesim node, then running turtle_nav.py.

If your code is working correctly, you should be able to type:

rosrun cs354_pa1 turtle_nav.py

and watch your turtle merrily navigate to the goal.

Warning! The logic involved in smoothly moving the turtle to a target location is trickier than you might expect. It is helpful if you first transform the goal location into turtle-centric coordinates. The following snippet of Python code accomplishes this:

(Updated 2/2.)

import numpy as np

def trans(x, y, z):
    """ Create a translation matrix. """ 
    matrix = np.eye(4)
    matrix[:,3] = [x, y, z, 1.0]
    return matrix

def rot_z(theta):
    """ Create a rotation matrix around the z-axis. Theta is in radians. """
    matrix = np.eye(4)
    matrix[0,0] = np.cos(theta)
    matrix[0,1] = -np.sin(theta)
    matrix[1,0] = np.sin(theta)
    matrix[1,1] = np.cos(theta)
    return matrix

def world_to_turtle(point_x, point_y, turtle_x, turtle_y, turtle_theta):
    """ Transform a point from world to turtle coordinates. 
    
    The provided point will be transformed into the turtle's
    coordinate frame.  In that coordinate frame, (0, 0) corresponds to
    the location of the turtle, (1.0, 0) is the spot 1 meter ahead of
    the turtle and (0, 1.0) is the spot 1 meter to the left of the
    turtle.

    Parameters: 
       point_x, point_y   - the point to transfrom (in world coordinates).
       turtle_x, turtle_y - the location of the turtle.
       turtle_theta       - the rotation of the turtle

    Returns:
       (x, y) - coordinates of the provided point in 
                the turtle's coordinate frame.  

    Example:
    >>> x, y = world_to_turtle(0, 0, 0, 1, 0)
    >>> print x, y
    0.0 -1.0

    """
    point = [point_x, point_y, 0.0, 1.0]
    R1 = rot_z(-turtle_theta)
    T1= trans(-turtle_x, -turtle_y, 0)
    T = np.dot(R1, T1)
    result = np.dot(T, point)
    return result[0], result[1]

Reading Parameters and Creating a Launch File

Modify your turtle_nav.py file so that it obtains the goal location from the parameter server. The goal parameters should be stored in /goalx and /goaly.

Once this is working, you should be able to move your turtle to a particular location by executing commands like the following:

$ rosparam set /goalx 2.0
$ rosparam set /goaly 3.0
$ rosrun cs354_pa1 turtle_nav.py

Reading Parameters and Creating a Launch File

Create a launch file named "pa1.launch" that handles all of the steps of starting the simulation, setting the navigation goal, and starting the navigation node. You should be able to run the entire simulation by typing one instruction into the terminal:

roslaunch cs354_pa1 pa1.launch

It should also be possible to change the default goal location by using command line launch arguments:

roslaunch cs354_pa1 pa1.launch goalx:=3.0 goaly:=4.0

Submitting

Submit by checking your project into your personal subversion repository with the commit message "PA1 FINAL SUBMISSION".