I spent a couple hours porting all the code over into this new format,
and I want to spare you all this editing work so in the final code
that is a little bit incomplete still, we have a grid.
We've got a function called "main."
Main then runs a path planner, A-star, smooths it, and then runs the controller, as in "run."
Then the controller even implemented for you our particle filter that you're familiar with.
There's nothing new here.
You're going to get the exact same code from class that you programmed yourself.
Then I go through a loop where I compute a crosstrack error,
apply my only PD controller--here is no I term here--
and I run my particle filter as before to estimate where the robot is.
What I would like you to do is to implement the crosstrack error function,
and I want you to use as an input the estimate, not the actual robot position,
but the best estimate, which you can get by running filter.get_position.
Now here is the difficulty, and I can tell you confidently it took me more than an hour
to solve this problem myself just for this class.
Our path now is a sequence of linear pieces.
When our robot drives along, it has a certain crosstrack error,
but as the robot state project beyond the end of a line segment, as is happening here,
we have to change the corresponding line segments to be the next one.
I addition, to calculating the assigned error relative to an arbitrary line segment,
not just the y-axis, we also have to detect when the robot steps beyond
the end of a line segment and switch over to the next one.
Now, suppose this is our line segment.
The path is given by the coordinates of the beginning point, p1, and the end point, p2,
both of which are (x, y) coordinates, which you get straight in the path.
Suppose our robot's position is something like this
where it has its own (x, y) estimate that comes out of the particle filter in your case,
and it has it's own orientation, theta.
Then both the cross track error as well as how far it has progressed
along the line segment--call this "U"--can be calculated using a dot product.
Specifically, let's call this vector over here delta x and delta y
as defined in x2 minus x1, and y2 minus y1--this vector over here.
Let's call this vector over here our Rx, which is x minus x1, and Ry.
Then U, the ratio of how far we've progressed along this segment is given
by the dot product Rx times delta x plus Ry times delta y
divided over the sum of squares delta x times delta x plus delta y times delta y. Why?
Well, this normalizes the vector length to 1, and this is the dot product of this vector over here
and the green vector, which happens to define the distance.
If this is larger than 1, we know we've left the segment and it's time to move onto the next one.
Finally, the crosstrack error--the red one over here--is given by a similar
but not identical dot product of Ry times delta--
notice we are now multiplying a y with an x--
minus--instead of plus--our x times delta y
with the exact same normalizer as down here.
You can see the normalizer over here.
What I want you to implement are these pieces of math over here.
When you run your controller, you will find that I setup for you a variable called "index"
that's the index into your path.
When U exceeds 1, we should increment this index
to make sure it never goes beyond what's legal in path length.
The crosstrack error should be computed relative to the current index
and is, of course, the assigned error using the exact same dot product I've shown you.
The last thing I want to tell you is what the path is.
I want you to use the following path.
The path is called "S" path.
It is given the run function as one of the parameters over here.
You can see it up here.
S path index is the indexth element of this path and 0 stands for x and 1 stands for y.
Please fill in the missing code over here.
I should tell you, when you run our controller with the missing code included,
you get actually a valid, nice path that mostly doesn't collide.
Occasionally it does, because of randomness in the system,
but it should be mostly collision free.
For this example, it will require about 130 or so robot steps.
Just so that you see a typical answer, here is a random run.
You read this as follows--true means the robot actually found the goal,
zero means zero collisions, and it took 137 steps.
Let me run it again, and here is another outcome.
The robot didn't collide and reached the goal in 145 steps.
I should warn you that sometimes I do get collisions here,
and it's because our obstacle surfaces are relatively large.
The noise in the system makes it hard to navigate.
But most of the time we should be able to get to the goal without difficulties
if we implement this piece of code correctly.
Just to warn you, it took me quite a while to work this out.