Attempt at taking the knowledge I learned from my intership and improving my custom algorithm. Duvall Pinkney We discussed adding two items of state information to the Wander robot program. The first was to use a numpy array as a 'map' of the room.
The robot pose information - as generated in a pose callback
(see goto.py) - can be used to generate the x,y location of the
robot on a grid. This needs to be transformed to array indices. You can use a linear transformation: index = scale * coordinate + offset. In this the coordinate is either the x or the y coordinate of position. The scale magnifies the coordinates from meters to centimeters or millimeters - whatever resolution you want. The offset displaces the coordinates to the middle of the array, so that negative values will
appear on the left or top and positive values on the right or bottom.
And 0,0 will be right in the middle. Remember you must make the index
an integer as the last step, in order to use it to access the numpy array. The attached code (plotting maps.txt) shows an example of setting up the numpy array. The easiest way to use this map is to just add 1 to the elemnt at the index for every location that the robot occupies. That way you get a histogram in 2D of the robot's location - maybe it can avoid places it has been to frequently etc. The second idea we discussed was to use another numpy array to store the position of the walls etc as detected by the laser range sensor The laser range sensor produces a list of 360 values, the distances to the walls around the robot starting at angle 0 facing front, then angle 1 degree to the left, 2 degrees, etc. If the robot is at angle theta (that's gLoc[2] in the pose callback that gives the angle of
the robot), then the angle of the laser range ray is
theta +the index of the laser range list (msg.ranges in the laser callback) converted to radians. gLoc[2] is already in radians. If we look at one specific laser range ray, say the one with index i. Then the angle is alpha=theta+math.radians(i). The coordinates of the end point of the ray (where the wall is) can be gotten by adding the coordinates of the position of the robot (x,y) to (r cos alpha, r sin alpha) where r =msg.ranges[i], the range value at index i. Once you get the coordinates of the end point of the array, you can use the same linear transformation as used for the robot position to map it into the numpy array you make for walls. You can also use the histogram approach - adding one to the location every time you get a laser reflection from that location.
Thus, places with high values are more likely walls. You can use this information for example to determine if you are finished mapping a room, or if you can see doorways in the room and want to go into them. Note that the map generated by this
method is very much inferior to the one generated by gMapping! For next week: Modify Wander so that it includes the pose callback and pose subscriber, make your two numpy arrays, by adding the
code to make the position histogram at the end of the pose callback and the wall 'histogram' at the end of the laser callback. Use the plotting code to show the two maps. Put the plotting
code in the shutdown callback, so that it will display the maps when you stop the program.