We are now in step number five for this final project.
And while the project is actually working, you just saw that the turtle is actually catching all the
other turtles.
Now we are going to improve that project and in this step.
So it's not going to be that long.
We're going to choose to target the closest turtle instead of the first turtle in the list.
Okay.
And first I'm going to show you with the I can increase the frequency here in the turtle spawner.
Let's say instead of two seconds between each turtle I'm going to put 0.5 okay.
So I'm going to spawn a lot more turtles.
And let's see what's going to be the behavior of the master turtle.
So now I start the turtle sim node I start the controller again.
Let's put turtle here and I start the spawner.
So it's going to spawn turtle quite fast.
And well, as you can see, the turtle is just going through all of the other target turtles, but in
the order in which they appeared.
Which is the order of the list.
All right.
We could improve that a lot by saying that we just try to go to the nearest turtle instead of the first
turtle in the list.
All right.
So let's do that in the code.
And where do we do this.
Actually there is nothing to do in the turtle spawner.
Okay.
This node is just spawning turtles and killing turtles and also publishing the current list of alive
turtles.
So this is in the turtle controller.
When we receive the list of alive turtles here, you can see we decide to chase the first one.
But what we could do instead is to actually compute the distance for each turtle and find the nearest
one, and just go to the nearest one.
And actually here I'm going to give the choice.
I'm going to create a variable called self dot catch closest turtle first.
By default it's going to be true.
And here I'm going to say if self catch closes total first we do something I'm going to put pass and
then else we go to the first one okay.
And then we can maybe put that as a parameter so that the user, when we actually run that node we can
choose do we want to catch the closest total first or just follow the order of the list.
All right.
So I keep both options.
Now to find the closest total we're going to receive a list of turtles.
We just go through the list.
We compute the distance for each turtle compared to where we are right now using the current pose.
And we just say that the turtle to catch is the one with the smallest distance.
So here as a helper variables, I'm just going to create the closest turtle initialized to none.
And then I'm going to create closest turtle distance.
And then I do for turtle in.
So message turtles.
Okay, that's the list.
And we are in this.
If so, we know that we have at least one element.
So for each turtle what do I do.
I will compute the distance.
To compute the distance I need to first get the distance x which is well the turtle we get here x minus
self dot pose dot x.
Okay.
It's the same thing we do here basically.
And then distance y is going to be turtle dot y minus self pose y.
And we can do.
Actually I'm just going to take that line because it's going to be the exact same.
The distance is the square root of the distance x multiplied by itself plus the distance y multiplied
by itself.
So with this we have the distance between the turtle so that turtle in the array and our current pose.
So the pose of the master turtle.
Then I'm going to do.
If the closest turtle is well is none.
If it's not initialized or if the distance is lower than the closest turtle distance, I just say that
the closest turtle is the current one, so the turtle and the closest turtle distance is the current
distance we have computed.
All right.
So the first time we go to the if closest turtle is none.
So it's going to basically save that turtle as the closest turtle.
And we also save the distance.
You see we saved the distance as the closest turtle distance.
And then for the next ones we just compute the new distance.
And if it's shorter we save that turtle as the new closest turtle.
And we also keep the distance for the next time.
So basically after we are done with this for loop, we know that the closest total variable here contains
the closest total.
So we can do self dot total to catch is equal to closest total.
All right.
And that's it.
Because then we just decide to not follow the first one in the list.
We just decide to choose whatever is the closest.
Then we're going to chase it.
And at the end we just call this service catch total is going to remove that total and then send the
new list with just the turtles that are alive.
And one thing that's going to be interesting, and we're going to be able to see that when we test,
is that whenever you receive a new list of turtles, this is going to be computed again.
So if let's say the turtle is the master turtle is currently chasing one turtle, but then one other
turtle is found very close to the master turtle.
Then because we receive a new list, we compute the distances with all turtles.
If that new turtle is closest than the current one we are chasing, we might actually change the target.
Okay.
So let's actually see that with an example.
So now I can start I'm going to change actually the frequency.
Well instead of 0.5 I'm going to use 0.8.
Not to spawn turtles two too fast.
So let's start the turtle sim node.
Let's start the controller.
And let's start the spawner.
And let's see what happens.
So we chased that one.
Okay.
And let's wait until.
So maybe the case is going to be soon or not.
But maybe at some point you see okay now we were going to that turtle here.
But then this one spawned.
So the turtle just changed its trajectory to catch that one instead.
Okay.
That's what I was talking about.
And now you can see it's much more smooth.
Okay.
And it makes more sense.
We go to the nearest one.
Okay.
We don't go to the first one on the list.
We go to the nearest one.