Because the robot is a non-holonomic, non-radially symmetric, non-zero turn radius type of vehicle, the path planning problem was fairly difficult.
I was struggling to find a readymade solution to the following problem: plot a curve between obstacles, with a constraint on the sharpest allowable curvature in the path.
For a setting with no obstacles, with a minimum turn radius specified, and where starting and ending state are specified as coordinate and angle in 2D space, I found Reeds-Shepp, which was already implemented in Python on GitHub. https://github.com/ghliu/pyReedsShepp, Reeds-Shepp Curves
I briefly considered using Rapidly Exploring Random Trees to search for solutions in this problem space, but decided on something different instead after reading some literature and testing some code. I think some of the variants I've seen published of RRTs would work nicely, but I didn't want to implement them in Python from scratch. Two repositories I looked at when making this decision:
https://github.com/iamprem/rmp, https://github.com/ArianJM/rapidly-exploring-random-trees
I decided to go with A* for search. To prepare my problem for that, I first discretized the map of obstacles (represented by polygons) using a visibility graph. I built the visibility graph using another Python repository pyvisgraph. I spent several days hunting down bugs in the code, coming up with one case of rounding error and one case of logical error that were causing problems in my setup. I opened up a few "Issues" through the GitHub portal and the author will be able to fix them. I considered doing a pull request but for just a few lines of code like that I figured the Issues would suffice. After fixing the bugs I provided support for interior polygons and adding obstacles to the visibility graph without recomputing, so I guess I should probably clean it all up and do a real pull request eventually.
I also had to discretize the robot angle at this point. I decided to go with 4 of what I called 'cones': North, East, South, West. The code has this parameter as K, so I call them K-cones (you can just as easily specify five cones, but then they can be harder to refer to casually when describing the solution). Since Reeds-Shepp takes in an exact angle, when computing the cost of a path from (start point, start cone), to (goal point, goal cone), I try all four combinations of clockwise and counterclockwise limits of the start cone and goal cone. Then I take the worst score and use that as the cost. Paths intersecting the obstacle map are ignored in this step, so they don't count towards the concept of the worst score.
Then I searched over this space with A*, and found solutions in the form of a list of (location, cone). But to resolve this into a navigable path still required resolving the cones (North, East, South, West) into angles. If there are n steps in the path, and we choose to sample d regularly spaced angles within a cone to find the best overall path length, then the size of the space is d raised to the power n. It isn't easy or necessarily possible to make n any smaller than it already is--so keep d small. I found that even for d=2 (checking just the most clockwise and counterclockwise limit of the cone) could find a solution. The difference between best total path length found were minimal anyhow.
I used Shapely to handle geometry related tasks like union-ing polygons together, checking for intersections, and so forth. I used matplotlib for drawing things to the screen.
You had to read all that, so here are some pictures!
An example solution after the angles have been resolved.
An easy solution, where the goal is immediately reached from the start point with a single Reeds-Shepp curve.
Here's a messy picture of all the edges in the graph for a particular setup. The pink things represent the cones.
This picture that I saved at some point in my work represents paths from points on the visibility graph that intersect the walls of this empty room. The intersecting sections are shown in red.
The strongest thing I can say for my hacked together system, "A* over Reeds-Shepp with K-Cones" is that is seems to work in all the cases I need it to work. I can't say anything about the optimality of the solution or even about whether a solution will be found if it exists. Computing the visibility graph and edge costs on the visibility graph when expanded for the 4 cones on each point takes about ten seconds on my laptop, for what that is worth. It can be saved and reloaded for future use, assuming the obstacle map hasn't changed.
I'm taking a break from this project to prepare for the Advanced Topics in CS class that I will be teaching this summer (high school grade levels, but college level materials). When I come back to this project, I will take the hardware from the last post, the path finding from this post, and a few bits of math I've worked out in my notes, to put the whole thing together into a robot that navigates from room to room using ArUco tags.
