This is now step three for this final project.
And for now, what we have is we have one node, as you can see here, that can spawn turtles on the
turtle SIM window.
And we have another node that can control the main turtle turtle one here and move it to some coordinates.
So now we need to find a way to link those two nodes.
So to make them communicate.
And the first thing we can do is to ask the turtle spawner node to actually send the live turtles.
So we're going to call a live turtles.
The turtles that are still present here on the screen is to send that to the turtle controller, so
the turtle controller can get the list of turtles with the coordinates and starts to reach one turtle.
And that's what we are going to do here first.
So we are going to go to our turtle spawner node first and create a publisher so that for example,
whenever we spawn a new turtle, we can publish the list of a live turtles.
And then when this publisher is working, we can add the subscriber side on the turtle controller node.
And so what do we want to send.
Well we want to send first.
That's going to be a list.
A list of what a list of turtles and what information we have inside the turtle.
What do we care about.
Well, we care about the well basically the things, the name and then the three coordinates x, y and
theta.
Actually, the orientation for the target turtle is not really important, but we can send all of that.
And so what I'm going to do here is I'm going to have to create a new interface.
I'm going to go to terminal here.
Let's go to the workspace in the source directory.
And well we already have my robot interfaces package that's already fully configured.
And it's inside this workspace.
So you could create a new interfaces package for that application if you want.
But I'm just going to use that package okay.
So I go in my robot Robots interfaces, and in the MSG folder where I already have two and I'm going
to create a new one touch.
I'm going to name it turtle dot msg.
Okay.
To create one kind of object for a turtle.
And then I'm going to create another one turtle array dot msg.
So we can create a list of turtle objects.
All right let's go back to the code.
And here in my robot interfaces I have my two new interfaces here.
And so what are the informations we need.
String name.
And then float64.
I'm going to use float 64 x.
Float 64 y.
Float 64 theta.
Okay.
That's going to define one turtle that I send over the topic.
So before you create an interface like this you might check.
Do you already have an already existing interface for that.
If not, you create a new interface.
Okay.
If you find an existing interface, but you have to use it in a way that's not designed for, it's best
to create a new interface that also has a meaningful name and just the fields that you need.
All right.
And as you can see, it's quite easy to create an interface.
So I have my turtle message.
That's for one turtle.
Now in my turtle array I'm going to add a list of.
So I'm going to put turtle with brackets.
So you see turtle here is the type of the message of the interface.
And I don't put any package in front because it's from the same package.
And I'm going to name that turtles.
So this turtle array message contains a list of the turtle message here.
Now what we need to do is to build those interfaces.
So I go to the cmakelists.txt of my robot interfaces and I'm going to add the messages here.
So one per line.
Turtle dot msg.
And here msg turtle array dot msg.
And that's it.
Everything else is correctly configured.
So make sure you save all the files.
Let's go back to the terminal.
And let's also stop that.
Let's build the interfaces.
Click on Build packages.
Select with my robot interfaces.
We might have a warning like this is not a problem.
Okay.
And now the interfaces are built.
Let's just test.
So let's source the workspace and then let's do Ros2 interface.
Show my robot interfaces in MSG slash turtle.
Do we find it?
Yes, we find turtle and we find total array.
Yes, we find total array.
And as you can see, total array description is the list of total with the name x, y and theta.
So if you see this the interfaces are built and you can use them in your code.
But now well you will need to source all your terminals.
And if you remember, if you want to have the autocompletion for the the interfaces in your VSCode environment,
you also need to open it from a source environment.
So what I would recommend is just close everything and open everything again.
At this point it's quite quick and easy.
You see, in a few seconds everything is done and I can well, I can close the simplest the txt.
I don't need that anymore.
Okay.
And I can come back to my turtle spawner.py.
And now I can include my new interfaces I can do from.
That's going to be my robot interfaces dot msg import turtle and from my robot interfaces dot msg import
turtle array.
Just to make things a bit cleaner with all those imports it doesn't matter, but I could group for example
the interfaces here just like that.
Okay, you could put the non Ros stuff first.
Well you do as you want as long as it's clean.
And then what do I want to do.
I want to publish on a topic.
I want to publish a turtle array message.
So I'm going to create a publisher.
I'm going to create it here self dot.
Let's name it alive turtles.
Publisher is equal to self dot create publisher.
The type is turtle array.
The topic name is.
Well we don't have any subscribers yet.
We are kind of creating the topic here, so let's name it alive turtles.
Okay?
I don't put any slash here.
It's going to be added automatically and I need the queue size.
All right.
So we have a publisher.
Then what I'm going to create is a function to publish on the topic.
So publish alive turtles itself I'm going to create a message of type turtle array.
And then message dot turtles need to be an array of turtles.
And for now what I'm going to do is I'm just going to create an array here self dot live turtles, which
is going to be an empty array okay.
So I initialize an empty array in the constructor because we have created zero turtle and I just put
self Alive turtles and I can do self dot alive turtles.
Publisher dot publish the message.
All right.
So I already have the structure for the publisher and the method to send a message on the topic.
Now we need to do two things.
We need to save the alive turtles.
So basically when we spawn a new turtle we need to save it inside the list.
And we also need to publish that list.
So how to do that?
Well, when we get the confirmation in this callback called spawn service method, when we get the confirmation
that we have spawned a new turtle.
Okay.
And the name is not empty, then here we know we have a new turtle.
We can add it to the list.
So I'm going to keep the structure.
And just after this log I'm going to keep the log I can create.
Let's say new turtle is equal to turtle okay.
That's why I also imported the turtle Still message here is that we can create, you see a turtle object,
and then we can fill this with the information and add it to the list.
Okay.
So then we can just publish the list as a turtle array.
So we create our new turtle and then we can do new turtle dot name is equal to.
Well we have response dot name.
That's the name we get here as a response.
And then new turtle dot x is equal to request dot x.
And we don't have any autocompletion.
We could also provide the type here spawn dot request.
So then I do new turtle dot y is equal to request dot y.
And finally new turtle dot theta is equal to request dot theta.
So you can see here that passing the request in the callback is quite important.
So that we can get the x and y for the for the turtle that we create.
And now that we have this object, we can simply add it to the list.
Self dot live turtles append new turtle.
Okay, so every time we spawn a new turtle we add it to the list.
So that's done.
Now when to publish this list?
Well we could publish just after we add it to the list because that's when we update the list.
So self dot publish a live turtles.
We don't need to pass anything here because the array is already a class attribute.
Okay.
So for this publisher, as you can see we don't need to create a timer.
We don't need to create anything.
We just publish from a client service callback.
So every time we spawn a new turtle, we add it to the list and then we publish the list.
Let's save that and let's test that functionality first.
I don't need to build because I have used Simulink install.
So let's just run turtle sim here to run.
Turtle sim and turtle sim node.
Then I can run my spawner.
Turtle sim catch and spawner.
So, is it working?
Let's see that it's working.
Yes.
The first one.
No.
But then from turtle two you see turtle three, etc..
So now if I do Ros two topic list I have.
Okay, I have lots of topics because for each turtle it's going to create a few topics.
But then I have my alive turtles topic Ros two topic equal alive turtles And you can see we have a list.
So I'm gonna press Ctrl C.
Now I'm going to stop everything and you can see what we received here.
So one message.
He had a list of turtles with the name x, the Y and the theta.
The sea turtle two, turtle three, turtle four until turtle 16.
All right.
So we have the list of all the turtles that we have spawned so far on the turtle sim window.
Great.
And now that you see this, we can do the subscriber side because we know the publisher is publishing
the correct data.
So let's go back to the code.
And this time let's open Turtle Control dot Pi and let's add the interface from my robot interfaces
dot msg imports.
I'm also going to import turtle and from my robot interfaces message import.
turtle array.
Then we can add a subscriber.
So for example here before that one or after that one let's say a live turtles subscriber self create
subscription with the type turtle array.
What is the topic name?
The topic name must be exactly a live turtle.
Live turtles with an S.
I don't put a slash here.
It's going to be added automatically.
And then I need a callback.
Let's create a callback here.
For example def callback a live turtles.
We receive a turtle list turtle list as a message which is of type turtle array.
Okay, let's put pass for now and let's finish this, so we provide the callback.
Callback alive turtles.
And we also put a queue size.
Go back to a new line here.
All right.
So we have our subscriber.
We have our callback.
What do we do in this callback.
Well here we receive the list of all turtles including the coordinates.
So what I'm going to do is I'm going to say that as a first step.
And then we're going to improve that in the following step.
I'm going to say that we are going to catch the first turtle okay I'm going to add a here self turtle
to catch.
I'm going to initialize that to none.
And then I'm going to say turtle.
So self dot turtle to catch is equal to the turtle list.
Dot turtles okay so that's the turtles field inside the message.
So if you confused with this you could we could also just say MSG.
Say it's the message.
And from the message, we get the total list.
Well, just make sure that you write something that makes sense.
And that's going to be the index zero.
Okay.
As simple as that.
And of course what I'm going to do is I'm also going to check if the length of the turtles array is
greater than zero.
Okay.
So we only do that if we receive a non-empty list, because if not we're going to get an error.
Great.
So whenever I get the list which is published by the spawner node, I save the first turtle of the list
as the turtle to catch.
And this here, this is a turtle, so it contains the x and y coordinate of the target.
So you see I don't need those x and y coordinates.
I don't need those temporary x and y.
I can just remove them.
And I can use those of the turtle to catch instead.
So here instead of target x I can say total to catch dot x.
We don't have the auto completion.
So I can say here I can put the type for example.
And now it should be working.
Yes.
And total to catch dot y.
Okay.
You see that since we just used the x target and the y target from the total we have received from this
subscriber, I'm also going to add something here.
If self pose is known or if also self dot total to catch is none.
So if we haven't received a total yet okay.
If we don't have any target, we're not going to do anything in this control loop.
So we're going to return.
Because if this is none then you're going to get an error here.
All right.
And there is nothing more to change in this control loop okay.
Because it was already working for target X and target Y.
Now we have just changed that to say that the target X and Y is from the turtle that we receive over
the topic, and for now, we just get the first turtle and we're going to improve that later in this
project.
Let's save and let's test that.
So to test I will need to start first the turtle SIM node which I'm going to put here so I can see what's
going on.
I put it right here and then I can start the.
Well I can start the controller actually because as you can see the controller is going to run this
control loop.
But if we don't have any turtle to catch it's not going to do anything.
So I can start the controller here was to run turtle SIM catch them all with controller.
And is it working?
Yes.
Nothing is happening.
But that's normal.
And then let's start.
Turtle SIM.
Catch them all.
Let's start the spawner.
And let's pay attention to what's going to happen.
So first one it's not spawned.
Then second one, you see the turtle is going to that first turtle.
Then we continue to spawn the turtles.
But of course we have just asked the controller to go to the first turtle in the array.
So I'm going to stop the spawner here.
And what you can see is that it's correctly working okay.
The turtle.
So the main turtle went to the target turtle.
So that's a very good result already.
Now of course in the next step we need to actually remove that turtle.
And we need to say to the master turtle to go to the next one so that we can catch one turtle and then
catch all the other turtles as soon as they spawn.