Assignment 5: Two Tiered Navigation

Potential fields navigation doesn't have much, ahem, potential in complex environments. So let's try a more robust approach to navigation. Files

Global Path Planning

Given a map, we want to be able to find the shortest path from one point to another. This section lays out one potential way to accomplish this. Please implement the following three methods in assignment05/src/assignment05/path_planning.py:

def neighbors(x, y, width, height):
Given integer coordinates that lie in a map of size (width, height), return valid neighbors to the coordinates. Only return points where 0 <= x < width and 0 <= y < height. Examples:
```
>>> neighbors(5,5,10,10)
[(5, 6), (5, 4), (6, 5), (4, 5)]
>>> neighbors(0,5,10,10)
[(0, 6), (0, 4), (1, 5)]
>>> neighbors(0,9,10,10)
[(0, 8), (1, 9)]
```
def distance_to_points(the_map, points):Given an OccupancyGrid object (the_map) and a list of integer coordinates on that map, return a dictionary that maps integer coordinates to their Manhattan distance to the closest point in the set of points. However, the path to tha point must not cross through any cells in the map that have a value > 90.
def plan_path(start_x, start_y, end_x, end_y, the_map, ds=None): Plan a path from start_x, start_y to end_x, end_y that does not go through any cell with score greater than 90. Coordinates are integers in grid coordinates. Return a list of grid coordinate pairs. One way to accomplish this is to get the distance_to_points dictionary using the goal point as the points list, and then starting at the start, move to the neighbor with the lowest distance to the goal. If ds is None, you should create the distance dictionary, otherwise just use the parameter ds.

Testing Global Path Planning

The following commands will visualize the path planning process.

roslaunch navigator distance.launch Once you set a goal using the rviz tool, this script will display the distance to that cell using text and colors. This uses the easy map from last assignment.
roslaunch navigator distance.launch easy:=false Same thing, but uses the CSE550 map. Only displays the colors. The visualization may use a lot of memory.
roslaunch navigator path.launch Set a goal and initial pose using the rviz tools and this script will display the path. Uses easy map.
roslaunch navigator path.launch easy:=false Same thing but CSE550 map.

Local Path Planning

Now we need to translate our plans into actual commands. Implement the following methods in assignment05/src/assignment05/controller.py:

def forward(p, vx, vz, t) Back to forward kinematics! The code for this should look very familiar, just with slightly different variable names. Given a pose p (x,y,theta), a forward velocity vx and angular velocity vz and a time t, return the new pose for where the robot will be after the given command.
def score_trajectory(p, goal, vx, vz, ds, path, the_map): This is the real meat of the assignment. Given a pose p, a goal (x,y,theta), return a score for the given trajectory (vx,vz), with lower scores equating to better trajectories. You can use the distance dict, shortest path and occupancy grid in your scoring function. More details below.
def find_best_trajectory(p, goal, ds, path, the_map, min_vx=0.0, max_vx=1.0, x_res=3, min_vz=-1.0, max_vz=1.0, z_res=5, debug=True):
Given a pose and goal, the distance dict, shortest path and map, find the best trajectory by scoring all of the trajectories in the space, and returning the best one. You should try all combinations of velocities with the specified resolutions. For instance, for vx using the default parameters, you should try all combinations with vx equal to 0, 0.5 and 1.0. (x_res=3 means try three velocities in the specified range). This should clearly use the score_trajectory function. It should return a tuple (vx, vz).

For your initial scoring function, simply calculate the resultant pose from the given velocities using your forward kinematics method. Then, translate the resulting pose into map coordinates and return the value for that location in the distance dictionary (or some arbitrarily high value if it doesn't exist).

Once you have gotten that working, you may also want to consider changing your scoring function for when the robot is with an arbitrary distance to the goal so that the goal orientation is also considered.

Testing the Local Path Planner

You are provided with an initial script assignment05/src/drive.py which subscribes to the goal pose and robot odometry, and publishes the best trajectory according to your above code. You can run roslaunch navigator empty.launch to try it out in a blank world using the simulator.

Obstacle Avoidance

Now you should have a robot that can effective drive around an empty map. However, it doesn't take obstacles into account. This portion of the assignment has less structure, and you should make changes to the code as you see fit. This may mean changing the structure/API of the code, which is fine.

Update drive.py to subscribe to the '/base_scan' topic and keep track of where obstacles are. You may reuse any code from assignment02 if you wish.

Next, you'll need to modify your code to take these obstacles into account. First, be sure that your distance dict is updated periodically to reflect new obstacles that you may have found. Second, you may need to modify your trajectory scoring function to not drive too close to the obstacles. You can conveniently use your distance_to_points function with the points equal to all occupied cells in the map to figure out how close to obstacles you are.

Lastly, feel free to make any other changes to the code that you wish to get your robot to move quicker. Change the trajectory resolution? Modify your scoring function to take into account distance to the global path? Consider Euclidean distance as opposed to Manhattan Distance. The choices are yours.

Due Date: May 1, 2015, 6pm CST.

CSE550