Lab 4: Mapping Solution

The goals of this lab are to:




Implement a basic discrete map representation and map an environment.



Use coordinate transforms to map sensor readings into world coordinates



Understand discrete and algorithmic aspects of mapping






You need:




A functional Sparkiduino development environment



A Sparki robot and the box it came in



A working copy of the odometry line following code (provided in the Lab 4 base code)



The ArcBotics line following poster






Overview




Mapping and navigation are standard primitives in autonomous robots. Although the different map representations and planning algorithms vary drastically in complexity, the problems remain the same: localization and sensing is uncertain, maps are quickly corrupted, and both mapping and navigation have to deal with limited on-board resources. In this exercise you will use Sparki’s ultrasound sensor to map objects in the environment. You will initially display the objects' coordinates on the screen and then implement data structures and helper functions that (1) allow you to store a map in Sparki's memory and (2) are suitable for path planning.




Instructions




Each group must develop their own software implementation, turned in by each team member, and individual lab reports. Thus, each team member should turn in:




The group’s code (1 .ino file)



An individually produced lab report. The write-up should be done independently and answer each question. (Please put the number of each question next to your answers, rather than turning in your answers as an essay)



Additionally, at least one member of each group should turn in a video of the code running on the Sparki (1 file). If your video file is bigger than 50mb, please submit a link to google drive or some other cloud service of your choice.




You are encouraged to engage with your lab partners for collaborative problem-solving, but are expected to turn in your own write-ups. If your group does not finish the implementation by the end of the class, you may continue this lab on your own time as a complete group.




Part 1: Preliminaries




Place your obstacles throughout the course on the interior of the line, far enough inside such that Sparki won’t collide with it.



Verify that your odometry code is functioning properly and that you understand how it’s working: since you’re using the ultrasonic sensor every loop, your cycle time is now non-deterministic and CYCLE_TIME is now a minimum.



Write down the homogenous transform to convert coordinates from Sparki’s Ultrasonic Sensor’s frame of reference ( , , + ) into world frame coordinates ( , ).



If Sparki is at pose (0,0,0 ) and the ultrasonic sensor is facing forward and detects an object 5cm away, your homogenous transform should return (.05,0) for the object’s position.




Write a C function that will transform a point given in Sparki’s Ultrasonic Sensor frame of reference into world frame coordinates.



Part 2: Map Construction







Choose a resolution for your map (e.g., 3cm per cell). Create a data structure bool world_map[#y_cells][#x_cells] to store and maintain it.



The approximate size of the line-following course is ~60cm wide by ~42cm tall.




Create a function that takes a float (x, y) coordinate and returns the map/grid location that it corresponds to (i, j).



One way to have multiple return values in C is to include pointers to variables as function parameters, setting those variables within the function and only returning an integer error code (e.g., 0 for error, 1 for okay). In general, the standard practice for return values tends to be 0 for “ok” and integers for the various error conditions that may have happened, but you’re free to implement however you see fit. Ex:




int add_5_to_two_positive_ints(int *a, int *b) {




if (*a < 0) return 1; // 1 is the error code for “a < 0” if (*b < 0) return 2; // 2 is the error code for “b < 0”




*a += 5;




*b += 5;




return 0;




}




Create a function that takes a grid location ( , ) and returns the ( , ) world coordinate at its center.



Using the value 0 to indicate obstacles and 1 to indicate navigable space, use the functions from 1-3 and 2-2 to populate your map as Sparki traverses the line course.






Part 3: Visualization and Path Planning Prep







Write a function that will visualize your map on Sparki’s 128x64 display. You will have to pick a resolution for each ‘cell’ from your map, drawing navigable space as empty (or empty boxes) and occupied space as filled-in.



(See http://arcbotics.com/lessons/first-steps-with-the-lcd-display/#High_Level_Commands for help!)




Algorithms such as Dijkstra’s are not made for grid maps, but rather for graphs in general.



To compute the distances from a given start position to an end position, you will need three functions which you are to implement to complete the lab:

A function that assigns a single integer to each grid cell of your map : ( , ) →
(e.g., ( , ) = ∗ ℎ + )




A function that takes as input an integer (cell index) and returns the corresponding 2D coordinates : → ( , )



A function that takes as input two integers representing the indices of two different cells and returns the “cost” to move between them : ( , ) → . This function should return a low value (e.g., 1) if both cells are adjacent and unoccupied, and a high value (e.g., 99) otherwise.









Part 4: Lab Report




Create a report that answers each of these questions:







What is the drawback of a data structure that stores distances between every possible pair of nodes in the graph? How does the implementation in 3-2 address this problem?



What are the names of everyone in your lab group?



Roughly how much time did you spend programming this lab?



(Optional, confidential, not for credit) A brief description of any problems (either technical or collaborative) encountered while performing this lab (e.g., issues with the clarity of instructions, clarity of documentation, lab colleague’s behavior, etc.)