This is the first step for the final project of this course, where we want to make a turtle.
So the turtle sim catch all the other turtles that appear on the screen.
And of course, we need to break this project into several steps.
We're not going to do everything at once.
And so what we're going to do in this first step is to first make the turtle.
So we're going to start turtle sim and make the turtle reach a certain point on the map, just that
okay.
And that's going to be a really a very good first step.
So then when we spawn more turtles, we can make the let's call it the Master turtle.
Go to each new turtle.
So first of all, let's start before we write any code.
Let's start the turtle sim node.
So turtle sim turtle sim node.
And let's see what topics, for example what services we need to use.
So I have my turtle sim here.
I'm going to put it on the side okay.
And this is the main turtle.
That's the first one.
It's called turtle one.
You see spawning turtle here?
Turtle one so we already have one that's going to be our main master turtle that we want to move.
And let's say we want to reach a point here so we can do Ros two topic list.
And we've already seen some of them before.
You see that we have a turtle one Cmdvel.
So command velocity.
And that is the topic we need to publish to to give a command.
So a command velocity to the turtle here.
And then the turtle is going to also to publish on a pause topic its current position.
Okay.
We can see that with Ros two topic eco and then turtle one pause.
And you can see it's publishing quite fast actually.
And this turtle here the coordinates you see x 5.5 and y 5.5 okay.
So x is this axis here.
And y is that axis?
So the zero is here, 5.5 here is the middle.
And it's going to be about 11 max on the x axis and then 11 max on the y axis.
All right.
So we know what's the minimum and maximum coordinates for the map.
And then theta is zero here.
So it's pointing forward.
And we can rotate like this up to 360 degrees okay.
So that's going to be 90 degrees 180 etc..
All right.
One thing we can also check for example you could start our graph and you make sure you de-select those
boxes.
And then maybe you have a name spaces or not.
But then you can see that topic here.
We need to publish to that topic because the total name is a subscriber.
And then this one we need to subscribe to this topic because the total theme is a publisher.
Okay.
So you can find all the information here with the command line.
And so we will need to use those two topics the command velocity topic and the post topic so that we
can create a control loop.
Okay.
We're going to get the current position and we're going to send a new command velocity depending on
that position.
All right.
So now that we have a better idea of what we need to do with those topics, let's create a new node.
And actually first I'm going to I'm going to stop that.
I'm going to create a new package.
So let's go to our let's go to our workspace.
You could create a new workspace if you want to, but let's use the same workspace.
And for now we have you see we have the bring up package the interfaces package and then a Python and
a C plus plus package.
But I'm not going to use those.
I'm just going to create a new package dedicated for that turtle sim project.
So Ros2 pkg create.
Let's name it turtle sim, catch them all.
And then I will provide a build type.
Build type.
This is going to be aimed at Python.
So I will do the solution in Python.
The detailed solution.
And of course you can find the cplusplus code for each step.
You can download that directly inside the course.
So I create a Python package.
And then I'm going to put some dependencies already.
Dependencies I will need rcl py because of course I use rcl pi in all the nodes.
And then as you can see we used the turtle sim package already.
We will need the turtle sim package because this controller depends actually on the existence of the
turtle sim.
So I'm going to add turtle sim.
Okay press enter.
I have those logs here with the warning for the license, but no worries about that.
And then inside this package.
So let's go to turtle SIM.
Catch them all.
You see we have another folder here called Turtle SIM.
Catch them all.
And this is going to be actually not empty.
We just have this init.py.
And I'm going to create a new file, let's say turtle control.py.
And let's make it executable.
Turtle control.py.
All right.
So now let's go back to our workspace.
And let's open VSCode here or whatever editor you want.
So let's find the file we have just created.
It's right here.
And well no surprise we are going to use the node template which I have put in this other package.
Okay.
So I'm just going to take that template here and put it in the node here in Turtle Controller.
Let's call it turtle control node.
Like that.
And then turtle Controller.
And then I will do two things.
First, I will create a subscriber to the post topic and I will create a publisher to the command velocity
topic.
Okay.
So we initialize that first.
So then we can use them.
And let's start with the subscriber for example.
So I'm going to do Ross two topic.
Actually I need to start turtle sim again if I want to introspect those topics.
So Ross to run turtle sim and turtle sim node okay.
And I can do so if I do topic list.
Yes.
If I do topic info with pose you can see the type is turtle sim message pose.
We have one publisher which is the turtle sim node and then Ross two interface show.
So we already know the type what's inside?
You see we have x y theta.
So that's the position.
And then you have linear velocity and angular velocity.
So basically the pose message here from the turtles in package contains the position and the velocity.
And what's going to be interesting for us is mostly actually this.
So the x y and theta.
So let's go ahead and let's create a subscriber for that.
You see turtle same message pose.
So I will do from turtle sim dot msg import pose.
And then because I use turtle sim you see we need in the package dot XML to add the turtle sim dependency,
which I have just provided when I created the package.
So let's go back here.
All right and let's create a subscriber.
So self dot let's say pose subscriber is equal to self.
Create subscription with Pose.
What is the topic name?
I'm going to use the exact topic name that one.
Okay, so you make sure you use the exact topic name.
If needed.
You copy paste from the terminal and then I need a callback.
So def that's going to be pause.
Actually let's name it callback pose.
We receive a message which is of type pose.
Actually I always name it message, but you could give a more meaningful name.
For example, we receive the pose which is a message and let's just pass for now.
I provide the callback here self dot callback pose like that.
Okay.
And what do we do here?
Well I'm just gonna say that I create a pose attribute here which is equal to none.
Okay.
So what I want to do here with this subscriber is simply that whenever we get a new post, we save it
inside the class as a private attribute so that then we will be able to use that everywhere in the class.
I'm just going to put the explicit type here, and then we can use the Autocompletion with that.
And so I will simply do self dot pose is equal to pose.
So the one we receive here on the callback.
And that's it for this callback.
Basically I get the pose and I save it to be used later.
Then let's create a publisher for the command velocity topic so that one.
If I do.
Ross two topic info with Cmdvel.
You see, we have a twist message that we have already seen before.
Ross two interface show twist from the Geometry messages package.
As you can see.
So different package.
So let's create a publisher I'm going to first import the interface from geometry messages dot msg import
twist and geometry messages.
I'm going to copy that and put that in the package dot XML with a new depend tag geometry msgs, and
I close the depend tag.
All right.
Now I have three dependencies here.
Okay, let's create a publisher for example here.
So self dot that's going to be a Cmdvel publisher is equal to self dot create publisher with the type
twist.
And then what is the topic name.
So here as you can see we are publishing to an already existing subscriber.
So we don't choose the topic name.
We need to use the one that already exists and we need to use the exact same name, and the name is
that one.
So I'm going to take that and copy it here.
And then we need a queue size.
Actually I forgot the queue size here on the subscriber as well.
Just put ten as it doesn't really matter here.
All right.
So we have initialized our subscriber and our publisher.
Now the question is so we're going to send commands.
We're going to move the turtle here by publishing on that topic.
But when do we do that.
One option that you could do and that would be valid, is to just check the pose here on the callback
when you receive a pose and then compute well the distance and the angle with the target pose.
And you could directly publish on this topic from the pose callback.
That would be a valid control loop.
But what we can also do and here I'm going to do things a tiny bit differently, is I'm going to create
my own control loop.
Because let's say you want to run a control loop Exactly.
At a certain frequency, then you're going to have more control with the timer.
Because here, if I just publish from the callback post, this is going to run as fast as we receive
messages.
So I think you could check the frequency, but I think the frequency is something like 60Hz.
So you receive 60 messages per second, which is quite a lot.
And that would be enough.
But let's say you don't know and maybe it would be 60 or maybe five or well, if you don't know then
it's not going to be that reliable okay.
Anyway, here it doesn't really matter.
You could do that.
It would be perfectly fine.
But I'm going to create an independent control loop.
And that's something you will also use quite often in your code.
And so I'm going to create a method here that I'm going to name control loop okay self pass.
And I'm going to simply create a timer self dot control loop timer is equal to Self create timer and
I'm going to put 0.01.
Let's say I want to run this control loop at 100Hz.
Exactly.
So I put self dot control loop.
All right.
And let's put it like this.
So basically what's going to happen is this is going to be run at 100 times per second.
And the main logic of the code is going to be in that okay.
So we kind of separate the communication with the outside.
For example with the subscribers we kind of separate that from the main control loop.
Because also let's say that in this control loop you need the information of the poles here, but you
need the information from several other subscribers.
Then if you just do your control loop or your algorithm in one of the subscriber callbacks, then you
might well, you might have some issues with Synchronizations and stuff like that.
Here we have an independent control loop.
All right.
And what we're going to do here is we're going to control where we're going to control that turtle.
And let's say we want to go here for example.
So we have a target destination.
I'm going to create a temporary target destination here in the constructor self dot target x let's say
8.0 and self dot target y.
Let's say 4.0 okay.
So this is 5.5.
The middle eight is going to be around here.
And then we go four on the y axis.
So it's going to be around there okay.
So our first test we want to make the control loop make the turtle go to that point.
So you see in the class we already have the target and the current post that's going to be updated.
So now what we need to do in this control loop, the first thing we need to do is to compute the distance
between the turtle and the target we want to reach and how to do that.
So here we have the turtle here the target point.
Now as you can see, we can have a rectangle.
Triangle.
Okay.
For any point you have a rectangle triangle we have you can see those two sides here.
That one is going to be the distance x okay.
So that's the difference between the target x and the pose x of the turtle.
And we have the same for the distance on the y axis.
And what's interesting for us is not those distances exactly but the main.
So the direct distance here between the turtle and the target point.
How do we compute this.
Well you can use the Pythagorean theorem, which is something that I think you should have seen in high
school.
I saw it in high school when I was 13 or 14.
So maybe you forgot.
Maybe you were sleeping in the class.
But just in case, here is the theorem.
It's not that complicated.
Okay, to get the distance, we need here to do the square root of the distance x.
So the square distance x plus the square distance y Why.
Okay.
So we're going to apply that directly into the code.
So back to our control loop I'm going to compute the distance.
So I will first compute the distance x okay which is the self dot target x minus self dot pose dot x
okay.
If you remember in the pose message we have a x field which corresponds to the x coordinate of the turtle.
Also, before I do this I will do an.
If self pose is equal to none, then I will return from the control loop.
I will not execute anything because you can see the first time it's none, it's not initialized.
And so between the initialization of the class and between the first time we receive a pose, it's going
to be none.
So if we do this we're going to get an error because it doesn't exist okay.
So we make sure that we have a pose to continue.
And so I have the distance x I have the distance y which is the same thing.
Self target y minus self pose y.
And then I'm going to name it also distance which is the direct distance.
And I'm going to use the math module.
So I'm going to include here maybe before or CLP.
It doesn't matter I'm going to import math which is a Python module.
And I'm going to do math dot.
And then how to do a square root.
You have s q r t.
And I'm going to multiply distance x with itself distance x plus distance y multiplied by itself okay.
And with this I can compute the direct distance.
And now that I have the distance well what I'm going to do is I'm going to first create a let's say
comment I could say message twist I'm going to create a twist message and then publish that message.
But also I could say cmd okay to make it a bit more explicit, then we're going to do stuff.
And then at the end I'm going to do self.com and then publisher publish the comment publish the message
okay.
And so now we need to say what command we are sending to the turtle to reach the target.
And I'm going to make two cases.
I'm going to say if distance is greater than 0.5 okay.
And else.
So we need to know when the turtle has reached the destination.
And usually when we do that we apply some sort of tolerance okay.
The turtle doesn't need to reach exactly the point because otherwise it's going to keep moving indefinitely.
We need to reach a point that is close enough to the target.
So in real life, you could say that your robot, if your robot is, let's say ten centimeters centimetres
from the target, then that's valid.
Or maybe it's going to be one centimetre or maybe two millimetres.
Depends on the application here.
I'm just saying we don't really have units for distances, but I'm just going to say that if the turtle
is at a distance that's more than 0.5, we are going to keep moving the turtle.
But if the turtle is less than 0.5 distance from the target, then I'm just going to say target reached
and I'm going to send a comment message.
So command dot linear dot x is equal to zero.
And command dot angular dot z is equal to zero.
So I'm going to stop the turtle when we reach the destination.
And y linear x and angular z.
So you can see on the twist message we have a linear x y z and an angular x y z.
Why I only use those two?
Because if you look at the turtle, well, we are working in 2D here.
Even actually when you use a mobile robot in 3D, you will mostly do navigation in 2D.
So that's going to be the same for a real robot.
And the turtle is going to move.
You see, on a linear speed here we can give a linear speed of x.
So positive or negative.
But the turtle is not going to drift on the y axis.
It's not going to happen and it's not going to go out of the screen.
So we don't have the y and z linear velocity only x.
And then for the angular well the turtle is not going to rotate.
Also kind of outside of the screen, it's going to only rotate like this, which if you take the right
hand rule is the z axis.
Okay.
So we only have those two comments.
Now what do we need to do to send a comment.
Well we will need position and orientation.
So we will need to provide a linear x velocity.
Let's start with that.
So comment dot linear dot x is equal to.
And this time we need to provide some value.
And I'm going to make it simple.
I'm just going to say that we provide the distance as a value.
All right.
So that's a very simple p controller.
We provide a value that will be proportional to the distance.
So the greater the distance the greater the velocity.
And as we get closer you will see that the turtle starts to go a bit slower.
Okay.
You could apply a fixed velocity.
You could apply anything you want here.
It's going to make it look a bit better.
All right so we have our position.
But then we need also the orientation.
So we need to provide a command.
Angular Z is equal to something.
But what would be the angle okay.
Because if we are here what angle do we need to apply to get to that point.
And so here we are back again on that screen.
We still want to reach that target okay.
We have already computed the distance.
But now what's the orientation?
So you see that this here, this arrow corresponds to the angle zero okay.
So zero means we are going forward like this.
And then as we increase the angle we are going to go you see on the positive side like this.
And on the negative side like that.
So that's the orientation for the turtle as it's facing forward here.
Now we want to compute that one.
So actually we want to compute the angle here.
What's the angle between the current orientation of the turtle and the target orientation which is named
theta in the in the code.
And once again well it's still the same configuration.
We still have a rectangle triangle okay.
So this time instead of computing the distance we can compute also the angle.
Here we have the distance x already we have the distance y.
And you can see that to get this angle here this angle alpha we need to compute the tangent.
So actually the tangent is the opposite divided by the adjacent distance.
So then we need to reverse the tangents.
We need to do arc tangent.
To get the angle and back to the code again.
I'm just going to comment that for now.
And I'm going to do goal theta is equal to.
And then how to compute the arc tangent I can use math a tan two.
Okay.
And you will use the distance y and distance x just like I showed you on the slide.
Then that is the angle for the target.
We need to compute the difference which is the goal minus the current one.
So self dot pose dot theta okay.
In the pose that we receive here from the subscriber we get the angle.
All right.
So that's a bit of mathematics here.
So if you've done that previously it might seem easy.
If you've never done that it might seem super hard.
But this is also something you can learn on the side.
And, well, there is a part of robotics where you need to learn a bit of mathematics.
Especially for control systems, which is what we're doing here.
All right.
So we have the difference which again.
We are going to use as the velocity here the common velocity for angular z.
So if the angle difference is a big number then the velocity is also going to be quite big.
As we get closer to the right angle to just reach the target point and go forward, then the velocity
and the angular z is going to decrease.
There is one more thing I'm going to do is to it's called normalize the angle.
Okay I'm going to do if diff is greater than math dot pi then I'm going to remove I'm going to do minus
two pi.
So two multiply math.pi.
And if the difference is Lower than minus math dot pi.
We do diff plus equal two times math dot pi.
Okay.
So we make sure that the angle is between minus pi and plus pi.
And this is going to be important especially if you reach the edge of the trigonometrical circle okay.
You could have some weird behaviors on the edges.
So with this we are safe.
All right.
So we should be able to test the code.
Actually I'm going to add one more thing here.
I'm going to add a multiplier here I'm going to add two and I'm going to add six.
And why is that.
Well simply because I have tested the program before okay.
And I have seen that multiplying by two here and by six here makes the turtle move better in a better
way and reaches the target in a better way.
Okay.
And the command is quite smooth.
And how did I get to this point?
Well, it's just about testing.
You know, you test stuff and then you see, you apply a bit more, a bit less.
That's how you tune a P controller.
Okay.
When you have a P or PID controller, well there's actually a lot of experimentation.
So there is a theory.
But in the end you're going to do a lot of experimentation.
So you start with something.
And then if it doesn't work or if it doesn't work, well then you tinker the values a bit and then you
end up with something that you like.
So let's do a very quick code recap before we start this in the constructor, what we have.
Well, we create a temporary target x and y.
We initialize a post to none.
And then we have a publisher and a subscriber for the topic.
We need to send the comment to and for the position of the turtle.
Then we create a control loop that's going to run at 100Hz.
In this control loop, what we first check that we have at least received one post from the subscriber.
And then we compute the distance between the turtle.
So this master turtle and the goal we want to reach.
So anywhere on the map, depending on that distance, we could say that if the distance is lower than
0.5, we just stop the turtle and that's it.
And then we send the command to the cmdvel topic.
But if we still have a distance before reaching the position of the target, then we compute the velocity.
So the linear velocity which is a multiplier of the distance.
And we also compute the orientation so that the turtle can correctly orientate itself to go to the target.
All right.
So this is going to run 100 times per second.
As you can see it's quite important that we don't block in this code.
We don't wait for stuff.
You see we just execute the code very fast because all of that is going to happen when we are spinning
the node.
Okay.
So this control loop is going to run at 100Hz per second.
And then we are also going to receive the pause every time we have a new message on that topic.
Okay.
So all of that is going to happen while the node is spinning.
Okay.
Let's save that.
And let's add a new executable here in setup.py.
So let's name it for example controller is equal to turtle sim.
Catch them all.
So that's the folder name here.
Dot turtle controller.
And that's going to be the main function.
As always we save everything.
And then let's go back here.
Let's go back to the roster workspace.
And let's do a call on build.
Packages select with my uh no actually it's turtle SIM.
Catch them all.
And I'm also going to add symlink install.
Great.
Then we have well we have the turtle sim here.
Still running.
We can leave it there.
I'm gonna.
I'm gonna use that terminal.
And let's do rush to run.
Turtle sim.
Catch them all, and we should find the controller executable.
So let's run that.
Let's make sure that we can see what's happening on the turtle.
Let's see if it's working.
So if it's working, the turtle should move to that point here somewhere.
Here.
Let's see.
And okay.
It's working.
Well it's working, but we have an issue in the code.
And well you can see in this case the error is not really explicit.
But actually yeah, it's something like with float float numbers.
And if we go back to the code I think that here, you see I have used the integer number because the
error seems to be when we reach the goal somewhere when we reach the goal.
So this is probably working fine, but that that is where the error comes from.
When we send an integer instead of a float number for the x property.
Okay.
Once again, if I do, for example Ros2 interface show geometry messages with twist.
You can see this is a float 64.
So the type is very important.
Let's put 0.0 and let's see again.
So I have used Simulink install.
Let's run that and let's run the node okay.
Oh and this time it's working all right.
So the total is moving to the target reaching the target and then stopping.
So the error was with this.
You see it can be quite tricky.
So double check that you use the correct types for all the messages you send on the topics and also
services.
Now if I want to test something else.
So let's say I want to go from here.
I want to go there.
So I'm going to give let's say a target x I'm going to put two and target y I'm going to put eight okay.
So that's going to be somewhere around here.
Now what I can do is I'm going to adjust.
So I'm going to put that here.
I'm going to leave the turtle theme alive the node alive.
And I'm going to stop and start the controller again.
And so it's going to start using the current pose of the turtle.
And you see it's starting from there and going there okay.
So now you can just change the target coordinates here.
Start the node again.
So it's working because we have used Simulink install.
And you could give different coordinates and move the turtle around.
All right.
And that's the end of this first step which was probably also the longest step.