WEBVTT 00:00.050 --> 00:05.120 This is the first step for the final project of this course, where we want to make a turtle. 00:05.120 --> 00:09.320 So the turtle sim catch all the other turtles that appear on the screen. 00:09.320 --> 00:12.590 And of course, we need to break this project into several steps. 00:12.590 --> 00:14.300 We're not going to do everything at once. 00:14.300 --> 00:18.560 And so what we're going to do in this first step is to first make the turtle. 00:18.560 --> 00:24.440 So we're going to start turtle sim and make the turtle reach a certain point on the map, just that 00:24.470 --> 00:24.740 okay. 00:24.770 --> 00:26.990 And that's going to be a really a very good first step. 00:26.990 --> 00:31.640 So then when we spawn more turtles, we can make the let's call it the Master turtle. 00:31.670 --> 00:34.370 Go to each new turtle. 00:34.580 --> 00:38.120 So first of all, let's start before we write any code. 00:38.120 --> 00:40.040 Let's start the turtle sim node. 00:41.090 --> 00:44.390 So turtle sim turtle sim node. 00:44.390 --> 00:48.320 And let's see what topics, for example what services we need to use. 00:48.320 --> 00:51.530 So I have my turtle sim here. 00:51.530 --> 00:57.080 I'm going to put it on the side okay. 00:57.080 --> 00:58.910 And this is the main turtle. 00:58.910 --> 00:59.900 That's the first one. 00:59.900 --> 01:01.340 It's called turtle one. 01:01.370 --> 01:03.140 You see spawning turtle here? 01:03.170 --> 01:08.960 Turtle one so we already have one that's going to be our main master turtle that we want to move. 01:08.960 --> 01:14.780 And let's say we want to reach a point here so we can do Ros two topic list. 01:14.780 --> 01:18.230 And we've already seen some of them before. 01:18.260 --> 01:21.620 You see that we have a turtle one Cmdvel. 01:21.650 --> 01:23.120 So command velocity. 01:23.120 --> 01:26.780 And that is the topic we need to publish to to give a command. 01:26.780 --> 01:29.540 So a command velocity to the turtle here. 01:29.540 --> 01:35.990 And then the turtle is going to also to publish on a pause topic its current position. 01:36.020 --> 01:36.500 Okay. 01:36.530 --> 01:43.460 We can see that with Ros two topic eco and then turtle one pause. 01:45.350 --> 01:47.960 And you can see it's publishing quite fast actually. 01:47.960 --> 01:55.220 And this turtle here the coordinates you see x 5.5 and y 5.5 okay. 01:55.250 --> 01:58.610 So x is this axis here. 01:58.610 --> 02:00.410 And y is that axis? 02:00.410 --> 02:04.400 So the zero is here, 5.5 here is the middle. 02:04.400 --> 02:12.020 And it's going to be about 11 max on the x axis and then 11 max on the y axis. 02:12.050 --> 02:12.380 All right. 02:12.410 --> 02:16.220 So we know what's the minimum and maximum coordinates for the map. 02:16.220 --> 02:17.900 And then theta is zero here. 02:17.900 --> 02:19.280 So it's pointing forward. 02:19.280 --> 02:24.710 And we can rotate like this up to 360 degrees okay. 02:24.740 --> 02:28.460 So that's going to be 90 degrees 180 etc.. 02:29.030 --> 02:29.870 All right. 02:29.900 --> 02:38.570 One thing we can also check for example you could start our graph and you make sure you de-select those 02:38.570 --> 02:39.380 boxes. 02:39.380 --> 02:43.370 And then maybe you have a name spaces or not. 02:43.370 --> 02:46.340 But then you can see that topic here. 02:46.340 --> 02:50.120 We need to publish to that topic because the total name is a subscriber. 02:50.120 --> 02:55.040 And then this one we need to subscribe to this topic because the total theme is a publisher. 02:55.070 --> 02:55.310 Okay. 02:55.340 --> 02:58.820 So you can find all the information here with the command line. 02:58.820 --> 03:06.440 And so we will need to use those two topics the command velocity topic and the post topic so that we 03:06.440 --> 03:08.090 can create a control loop. 03:08.120 --> 03:08.510 Okay. 03:08.540 --> 03:14.000 We're going to get the current position and we're going to send a new command velocity depending on 03:14.000 --> 03:14.960 that position. 03:14.990 --> 03:15.290 All right. 03:15.290 --> 03:20.660 So now that we have a better idea of what we need to do with those topics, let's create a new node. 03:20.660 --> 03:23.000 And actually first I'm going to I'm going to stop that. 03:23.030 --> 03:24.950 I'm going to create a new package. 03:24.950 --> 03:27.350 So let's go to our let's go to our workspace. 03:27.350 --> 03:31.670 You could create a new workspace if you want to, but let's use the same workspace. 03:31.670 --> 03:37.190 And for now we have you see we have the bring up package the interfaces package and then a Python and 03:37.190 --> 03:38.390 a C plus plus package. 03:38.390 --> 03:39.740 But I'm not going to use those. 03:39.740 --> 03:43.940 I'm just going to create a new package dedicated for that turtle sim project. 03:43.940 --> 03:47.660 So Ros2 pkg create. 03:47.690 --> 03:53.000 Let's name it turtle sim, catch them all. 03:53.000 --> 03:56.210 And then I will provide a build type. 03:56.690 --> 03:58.190 Build type. 03:58.220 --> 04:01.190 This is going to be aimed at Python. 04:01.190 --> 04:03.320 So I will do the solution in Python. 04:03.350 --> 04:05.120 The detailed solution. 04:05.120 --> 04:09.470 And of course you can find the cplusplus code for each step. 04:09.470 --> 04:12.170 You can download that directly inside the course. 04:12.170 --> 04:13.880 So I create a Python package. 04:13.880 --> 04:16.910 And then I'm going to put some dependencies already. 04:16.970 --> 04:23.420 Dependencies I will need rcl py because of course I use rcl pi in all the nodes. 04:23.420 --> 04:28.340 And then as you can see we used the turtle sim package already. 04:28.370 --> 04:34.400 We will need the turtle sim package because this controller depends actually on the existence of the 04:34.400 --> 04:35.180 turtle sim. 04:35.180 --> 04:37.670 So I'm going to add turtle sim. 04:38.360 --> 04:39.830 Okay press enter. 04:39.830 --> 04:44.240 I have those logs here with the warning for the license, but no worries about that. 04:44.240 --> 04:47.150 And then inside this package. 04:47.150 --> 04:49.160 So let's go to turtle SIM. 04:49.160 --> 04:50.270 Catch them all. 04:50.270 --> 04:55.430 You see we have another folder here called Turtle SIM. 04:55.430 --> 04:56.750 Catch them all. 04:56.750 --> 04:59.780 And this is going to be actually not empty. 04:59.810 --> 05:02.030 We just have this init.py. 05:02.060 --> 05:07.340 And I'm going to create a new file, let's say turtle control.py. 05:07.670 --> 05:09.980 And let's make it executable. 05:10.430 --> 05:13.010 Turtle control.py. 05:13.040 --> 05:14.060 All right. 05:14.060 --> 05:18.110 So now let's go back to our workspace. 05:18.110 --> 05:23.690 And let's open VSCode here or whatever editor you want. 05:24.920 --> 05:27.770 So let's find the file we have just created. 05:27.800 --> 05:29.660 It's right here. 05:29.690 --> 05:36.230 And well no surprise we are going to use the node template which I have put in this other package. 05:36.770 --> 05:37.370 Okay. 05:37.370 --> 05:45.770 So I'm just going to take that template here and put it in the node here in Turtle Controller. 05:46.760 --> 05:50.270 Let's call it turtle control node. 05:53.030 --> 05:53.900 Like that. 05:53.900 --> 05:57.680 And then turtle Controller. 05:59.990 --> 06:02.270 And then I will do two things. 06:02.270 --> 06:08.600 First, I will create a subscriber to the post topic and I will create a publisher to the command velocity 06:08.630 --> 06:09.110 topic. 06:09.140 --> 06:09.440 Okay. 06:09.470 --> 06:11.150 So we initialize that first. 06:11.150 --> 06:12.500 So then we can use them. 06:12.500 --> 06:15.410 And let's start with the subscriber for example. 06:15.410 --> 06:18.770 So I'm going to do Ross two topic. 06:18.800 --> 06:23.540 Actually I need to start turtle sim again if I want to introspect those topics. 06:23.540 --> 06:32.510 So Ross to run turtle sim and turtle sim node okay. 06:32.510 --> 06:35.330 And I can do so if I do topic list. 06:35.360 --> 06:36.260 Yes. 06:36.290 --> 06:45.470 If I do topic info with pose you can see the type is turtle sim message pose. 06:45.470 --> 06:51.650 We have one publisher which is the turtle sim node and then Ross two interface show. 06:51.650 --> 06:54.650 So we already know the type what's inside? 06:54.650 --> 06:57.380 You see we have x y theta. 06:57.380 --> 06:58.460 So that's the position. 06:58.460 --> 07:01.970 And then you have linear velocity and angular velocity. 07:01.970 --> 07:08.180 So basically the pose message here from the turtles in package contains the position and the velocity. 07:08.300 --> 07:11.780 And what's going to be interesting for us is mostly actually this. 07:11.780 --> 07:13.460 So the x y and theta. 07:13.460 --> 07:17.300 So let's go ahead and let's create a subscriber for that. 07:17.330 --> 07:19.700 You see turtle same message pose. 07:19.700 --> 07:29.000 So I will do from turtle sim dot msg import pose. 07:29.030 --> 07:34.820 And then because I use turtle sim you see we need in the package dot XML to add the turtle sim dependency, 07:34.820 --> 07:38.180 which I have just provided when I created the package. 07:38.180 --> 07:40.970 So let's go back here. 07:41.000 --> 07:42.860 All right and let's create a subscriber. 07:42.860 --> 07:51.530 So self dot let's say pose subscriber is equal to self. 07:51.530 --> 07:55.790 Create subscription with Pose. 07:56.270 --> 07:58.670 What is the topic name? 07:58.670 --> 08:02.990 I'm going to use the exact topic name that one. 08:03.560 --> 08:07.880 Okay, so you make sure you use the exact topic name. 08:07.880 --> 08:08.270 If needed. 08:08.270 --> 08:11.630 You copy paste from the terminal and then I need a callback. 08:11.630 --> 08:15.020 So def that's going to be pause. 08:15.050 --> 08:19.040 Actually let's name it callback pose. 08:20.360 --> 08:25.070 We receive a message which is of type pose. 08:25.100 --> 08:29.750 Actually I always name it message, but you could give a more meaningful name. 08:29.750 --> 08:35.090 For example, we receive the pose which is a message and let's just pass for now. 08:35.120 --> 08:42.140 I provide the callback here self dot callback pose like that. 08:42.170 --> 08:42.650 Okay. 08:42.650 --> 08:44.240 And what do we do here? 08:44.240 --> 08:53.570 Well I'm just gonna say that I create a pose attribute here which is equal to none. 08:53.600 --> 08:53.840 Okay. 08:53.870 --> 09:00.550 So what I want to do here with this subscriber is simply that whenever we get a new post, we save it 09:00.550 --> 09:06.490 inside the class as a private attribute so that then we will be able to use that everywhere in the class. 09:06.580 --> 09:11.920 I'm just going to put the explicit type here, and then we can use the Autocompletion with that. 09:11.920 --> 09:18.070 And so I will simply do self dot pose is equal to pose. 09:18.070 --> 09:20.740 So the one we receive here on the callback. 09:20.740 --> 09:22.900 And that's it for this callback. 09:22.900 --> 09:27.430 Basically I get the pose and I save it to be used later. 09:27.430 --> 09:33.820 Then let's create a publisher for the command velocity topic so that one. 09:33.850 --> 09:34.510 If I do. 09:34.540 --> 09:40.450 Ross two topic info with Cmdvel. 09:41.770 --> 09:45.580 You see, we have a twist message that we have already seen before. 09:45.940 --> 09:53.680 Ross two interface show twist from the Geometry messages package. 09:53.680 --> 09:54.430 As you can see. 09:54.430 --> 09:55.600 So different package. 09:55.600 --> 10:04.510 So let's create a publisher I'm going to first import the interface from geometry messages dot msg import 10:05.020 --> 10:08.680 twist and geometry messages. 10:08.680 --> 10:18.910 I'm going to copy that and put that in the package dot XML with a new depend tag geometry msgs, and 10:18.910 --> 10:20.350 I close the depend tag. 10:20.620 --> 10:20.920 All right. 10:20.950 --> 10:22.630 Now I have three dependencies here. 10:22.660 --> 10:25.990 Okay, let's create a publisher for example here. 10:25.990 --> 10:38.470 So self dot that's going to be a Cmdvel publisher is equal to self dot create publisher with the type 10:38.530 --> 10:39.580 twist. 10:39.820 --> 10:41.800 And then what is the topic name. 10:41.800 --> 10:46.930 So here as you can see we are publishing to an already existing subscriber. 10:46.930 --> 10:49.060 So we don't choose the topic name. 10:49.060 --> 10:55.400 We need to use the one that already exists and we need to use the exact same name, and the name is 10:55.400 --> 10:56.030 that one. 10:56.030 --> 10:58.640 So I'm going to take that and copy it here. 10:58.640 --> 11:01.850 And then we need a queue size. 11:01.880 --> 11:05.870 Actually I forgot the queue size here on the subscriber as well. 11:06.380 --> 11:09.920 Just put ten as it doesn't really matter here. 11:09.950 --> 11:10.520 All right. 11:10.520 --> 11:14.000 So we have initialized our subscriber and our publisher. 11:14.030 --> 11:17.480 Now the question is so we're going to send commands. 11:17.480 --> 11:21.590 We're going to move the turtle here by publishing on that topic. 11:21.590 --> 11:24.020 But when do we do that. 11:24.050 --> 11:31.820 One option that you could do and that would be valid, is to just check the pose here on the callback 11:31.880 --> 11:37.040 when you receive a pose and then compute well the distance and the angle with the target pose. 11:37.040 --> 11:41.930 And you could directly publish on this topic from the pose callback. 11:41.960 --> 11:43.820 That would be a valid control loop. 11:43.820 --> 11:48.410 But what we can also do and here I'm going to do things a tiny bit differently, is I'm going to create 11:48.410 --> 11:50.150 my own control loop. 11:50.150 --> 11:53.140 Because let's say you want to run a control loop Exactly. 11:53.170 --> 11:57.370 At a certain frequency, then you're going to have more control with the timer. 11:57.400 --> 12:03.760 Because here, if I just publish from the callback post, this is going to run as fast as we receive 12:03.760 --> 12:04.360 messages. 12:04.360 --> 12:09.460 So I think you could check the frequency, but I think the frequency is something like 60Hz. 12:09.460 --> 12:13.330 So you receive 60 messages per second, which is quite a lot. 12:13.330 --> 12:14.200 And that would be enough. 12:14.200 --> 12:19.900 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 12:19.900 --> 12:22.210 it's not going to be that reliable okay. 12:22.240 --> 12:23.680 Anyway, here it doesn't really matter. 12:23.680 --> 12:24.640 You could do that. 12:24.640 --> 12:25.930 It would be perfectly fine. 12:25.930 --> 12:28.270 But I'm going to create an independent control loop. 12:28.270 --> 12:31.690 And that's something you will also use quite often in your code. 12:31.690 --> 12:40.720 And so I'm going to create a method here that I'm going to name control loop okay self pass. 12:40.720 --> 12:55.120 And I'm going to simply create a timer self dot control loop timer is equal to Self create timer and 12:55.120 --> 12:57.250 I'm going to put 0.01. 12:57.250 --> 13:01.600 Let's say I want to run this control loop at 100Hz. 13:01.630 --> 13:02.320 Exactly. 13:02.320 --> 13:06.340 So I put self dot control loop. 13:06.730 --> 13:07.210 All right. 13:07.240 --> 13:08.860 And let's put it like this. 13:08.860 --> 13:14.950 So basically what's going to happen is this is going to be run at 100 times per second. 13:14.950 --> 13:20.290 And the main logic of the code is going to be in that okay. 13:20.320 --> 13:23.770 So we kind of separate the communication with the outside. 13:23.800 --> 13:29.260 For example with the subscribers we kind of separate that from the main control loop. 13:29.260 --> 13:35.080 Because also let's say that in this control loop you need the information of the poles here, but you 13:35.080 --> 13:38.020 need the information from several other subscribers. 13:38.020 --> 13:45.040 Then if you just do your control loop or your algorithm in one of the subscriber callbacks, then you 13:45.040 --> 13:48.820 might well, you might have some issues with Synchronizations and stuff like that. 13:48.820 --> 13:51.430 Here we have an independent control loop. 13:51.460 --> 13:51.790 All right. 13:51.790 --> 13:55.930 And what we're going to do here is we're going to control where we're going to control that turtle. 13:55.930 --> 13:58.030 And let's say we want to go here for example. 13:58.030 --> 14:00.100 So we have a target destination. 14:00.100 --> 14:10.420 I'm going to create a temporary target destination here in the constructor self dot target x let's say 14:10.450 --> 14:15.520 8.0 and self dot target y. 14:15.520 --> 14:17.800 Let's say 4.0 okay. 14:17.830 --> 14:19.840 So this is 5.5. 14:19.870 --> 14:22.210 The middle eight is going to be around here. 14:22.210 --> 14:24.880 And then we go four on the y axis. 14:24.880 --> 14:26.470 So it's going to be around there okay. 14:26.500 --> 14:31.570 So our first test we want to make the control loop make the turtle go to that point. 14:31.570 --> 14:36.940 So you see in the class we already have the target and the current post that's going to be updated. 14:36.940 --> 14:40.900 So now what we need to do in this control loop, the first thing we need to do is to compute the distance 14:40.900 --> 14:45.580 between the turtle and the target we want to reach and how to do that. 14:45.790 --> 14:49.450 So here we have the turtle here the target point. 14:49.480 --> 14:52.930 Now as you can see, we can have a rectangle. 14:52.960 --> 14:53.860 Triangle. 14:53.890 --> 14:54.070 Okay. 14:54.100 --> 14:58.930 For any point you have a rectangle triangle we have you can see those two sides here. 14:58.930 --> 15:01.750 That one is going to be the distance x okay. 15:01.780 --> 15:08.380 So that's the difference between the target x and the pose x of the turtle. 15:08.380 --> 15:11.470 And we have the same for the distance on the y axis. 15:11.470 --> 15:16.300 And what's interesting for us is not those distances exactly but the main. 15:16.330 --> 15:21.460 So the direct distance here between the turtle and the target point. 15:21.460 --> 15:22.750 How do we compute this. 15:22.750 --> 15:29.500 Well you can use the Pythagorean theorem, which is something that I think you should have seen in high 15:29.500 --> 15:29.830 school. 15:29.830 --> 15:32.200 I saw it in high school when I was 13 or 14. 15:32.200 --> 15:33.370 So maybe you forgot. 15:33.400 --> 15:34.990 Maybe you were sleeping in the class. 15:34.990 --> 15:37.600 But just in case, here is the theorem. 15:37.600 --> 15:39.130 It's not that complicated. 15:39.160 --> 15:45.610 Okay, to get the distance, we need here to do the square root of the distance x. 15:45.610 --> 15:49.870 So the square distance x plus the square distance y Why. 15:49.900 --> 15:50.200 Okay. 15:50.230 --> 15:53.740 So we're going to apply that directly into the code. 15:54.130 --> 15:58.990 So back to our control loop I'm going to compute the distance. 15:58.990 --> 16:12.610 So I will first compute the distance x okay which is the self dot target x minus self dot pose dot x 16:12.640 --> 16:13.060 okay. 16:13.090 --> 16:20.110 If you remember in the pose message we have a x field which corresponds to the x coordinate of the turtle. 16:20.140 --> 16:22.240 Also, before I do this I will do an. 16:22.270 --> 16:30.040 If self pose is equal to none, then I will return from the control loop. 16:30.040 --> 16:34.960 I will not execute anything because you can see the first time it's none, it's not initialized. 16:34.960 --> 16:40.660 And so between the initialization of the class and between the first time we receive a pose, it's going 16:40.660 --> 16:41.290 to be none. 16:41.290 --> 16:46.000 So if we do this we're going to get an error because it doesn't exist okay. 16:46.030 --> 16:49.210 So we make sure that we have a pose to continue. 16:49.270 --> 16:54.010 And so I have the distance x I have the distance y which is the same thing. 16:54.040 --> 17:00.460 Self target y minus self pose y. 17:00.460 --> 17:04.480 And then I'm going to name it also distance which is the direct distance. 17:04.480 --> 17:07.420 And I'm going to use the math module. 17:07.420 --> 17:11.620 So I'm going to include here maybe before or CLP. 17:11.650 --> 17:16.270 It doesn't matter I'm going to import math which is a Python module. 17:16.300 --> 17:19.570 And I'm going to do math dot. 17:19.600 --> 17:21.220 And then how to do a square root. 17:21.220 --> 17:23.140 You have s q r t. 17:23.620 --> 17:35.620 And I'm going to multiply distance x with itself distance x plus distance y multiplied by itself okay. 17:35.650 --> 17:39.100 And with this I can compute the direct distance. 17:39.100 --> 17:44.680 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 17:44.680 --> 17:50.860 comment I could say message twist I'm going to create a twist message and then publish that message. 17:50.860 --> 17:59.110 But also I could say cmd okay to make it a bit more explicit, then we're going to do stuff. 17:59.170 --> 18:08.110 And then at the end I'm going to do self.com and then publisher publish the comment publish the message 18:08.140 --> 18:08.680 okay. 18:08.710 --> 18:15.280 And so now we need to say what command we are sending to the turtle to reach the target. 18:15.280 --> 18:17.170 And I'm going to make two cases. 18:17.170 --> 18:22.540 I'm going to say if distance is greater than 0.5 okay. 18:22.570 --> 18:24.850 And else. 18:24.850 --> 18:29.500 So we need to know when the turtle has reached the destination. 18:29.500 --> 18:33.940 And usually when we do that we apply some sort of tolerance okay. 18:33.970 --> 18:38.830 The turtle doesn't need to reach exactly the point because otherwise it's going to keep moving indefinitely. 18:38.830 --> 18:42.580 We need to reach a point that is close enough to the target. 18:42.580 --> 18:47.800 So in real life, you could say that your robot, if your robot is, let's say ten centimeters centimetres 18:47.800 --> 18:49.810 from the target, then that's valid. 18:49.810 --> 18:53.200 Or maybe it's going to be one centimetre or maybe two millimetres. 18:53.200 --> 18:54.700 Depends on the application here. 18:54.700 --> 18:59.890 I'm just saying we don't really have units for distances, but I'm just going to say that if the turtle 18:59.890 --> 19:06.130 is at a distance that's more than 0.5, we are going to keep moving the turtle. 19:06.160 --> 19:15.940 But if the turtle is less than 0.5 distance from the target, then I'm just going to say target reached 19:15.940 --> 19:19.300 and I'm going to send a comment message. 19:19.360 --> 19:25.120 So command dot linear dot x is equal to zero. 19:25.300 --> 19:31.030 And command dot angular dot z is equal to zero. 19:31.030 --> 19:35.140 So I'm going to stop the turtle when we reach the destination. 19:35.140 --> 19:38.260 And y linear x and angular z. 19:38.260 --> 19:44.290 So you can see on the twist message we have a linear x y z and an angular x y z. 19:44.290 --> 19:46.450 Why I only use those two? 19:46.480 --> 19:50.500 Because if you look at the turtle, well, we are working in 2D here. 19:50.500 --> 19:55.990 Even actually when you use a mobile robot in 3D, you will mostly do navigation in 2D. 19:55.990 --> 19:58.000 So that's going to be the same for a real robot. 19:58.000 --> 19:59.860 And the turtle is going to move. 19:59.890 --> 20:03.700 You see, on a linear speed here we can give a linear speed of x. 20:03.700 --> 20:05.140 So positive or negative. 20:05.170 --> 20:08.110 But the turtle is not going to drift on the y axis. 20:08.110 --> 20:11.170 It's not going to happen and it's not going to go out of the screen. 20:11.170 --> 20:15.340 So we don't have the y and z linear velocity only x. 20:15.340 --> 20:18.490 And then for the angular well the turtle is not going to rotate. 20:18.490 --> 20:24.970 Also kind of outside of the screen, it's going to only rotate like this, which if you take the right 20:25.000 --> 20:27.730 hand rule is the z axis. 20:27.760 --> 20:27.970 Okay. 20:28.000 --> 20:29.950 So we only have those two comments. 20:29.980 --> 20:33.760 Now what do we need to do to send a comment. 20:33.790 --> 20:37.870 Well we will need position and orientation. 20:37.870 --> 20:40.840 So we will need to provide a linear x velocity. 20:40.870 --> 20:42.310 Let's start with that. 20:42.400 --> 20:46.780 So comment dot linear dot x is equal to. 20:46.810 --> 20:48.760 And this time we need to provide some value. 20:48.760 --> 20:50.290 And I'm going to make it simple. 20:50.290 --> 20:54.700 I'm just going to say that we provide the distance as a value. 20:54.730 --> 20:54.940 All right. 20:54.970 --> 20:57.370 So that's a very simple p controller. 20:57.400 --> 21:01.450 We provide a value that will be proportional to the distance. 21:01.450 --> 21:05.050 So the greater the distance the greater the velocity. 21:05.050 --> 21:09.880 And as we get closer you will see that the turtle starts to go a bit slower. 21:09.910 --> 21:10.150 Okay. 21:10.180 --> 21:12.490 You could apply a fixed velocity. 21:12.520 --> 21:14.410 You could apply anything you want here. 21:14.410 --> 21:15.850 It's going to make it look a bit better. 21:15.880 --> 21:17.770 All right so we have our position. 21:17.770 --> 21:20.020 But then we need also the orientation. 21:20.020 --> 21:22.690 So we need to provide a command. 21:22.720 --> 21:27.010 Angular Z is equal to something. 21:27.010 --> 21:30.370 But what would be the angle okay. 21:30.400 --> 21:36.580 Because if we are here what angle do we need to apply to get to that point. 21:37.270 --> 21:40.300 And so here we are back again on that screen. 21:40.300 --> 21:42.280 We still want to reach that target okay. 21:42.310 --> 21:45.220 We have already computed the distance. 21:45.220 --> 21:47.290 But now what's the orientation? 21:47.290 --> 21:53.080 So you see that this here, this arrow corresponds to the angle zero okay. 21:53.110 --> 21:56.770 So zero means we are going forward like this. 21:56.770 --> 22:01.960 And then as we increase the angle we are going to go you see on the positive side like this. 22:01.960 --> 22:04.060 And on the negative side like that. 22:04.060 --> 22:08.260 So that's the orientation for the turtle as it's facing forward here. 22:08.290 --> 22:11.200 Now we want to compute that one. 22:11.470 --> 22:14.140 So actually we want to compute the angle here. 22:14.140 --> 22:20.020 What's the angle between the current orientation of the turtle and the target orientation which is named 22:20.050 --> 22:21.790 theta in the in the code. 22:21.790 --> 22:24.340 And once again well it's still the same configuration. 22:24.340 --> 22:26.770 We still have a rectangle triangle okay. 22:26.800 --> 22:30.460 So this time instead of computing the distance we can compute also the angle. 22:30.460 --> 22:34.510 Here we have the distance x already we have the distance y. 22:34.510 --> 22:40.120 And you can see that to get this angle here this angle alpha we need to compute the tangent. 22:40.120 --> 22:45.610 So actually the tangent is the opposite divided by the adjacent distance. 22:45.610 --> 22:47.260 So then we need to reverse the tangents. 22:47.260 --> 22:48.520 We need to do arc tangent. 22:48.550 --> 22:52.300 To get the angle and back to the code again. 22:52.330 --> 22:54.520 I'm just going to comment that for now. 22:54.970 --> 22:59.800 And I'm going to do goal theta is equal to. 22:59.830 --> 23:06.130 And then how to compute the arc tangent I can use math a tan two. 23:06.310 --> 23:06.760 Okay. 23:06.790 --> 23:14.710 And you will use the distance y and distance x just like I showed you on the slide. 23:14.710 --> 23:17.440 Then that is the angle for the target. 23:17.470 --> 23:24.850 We need to compute the difference which is the goal minus the current one. 23:24.850 --> 23:29.170 So self dot pose dot theta okay. 23:29.200 --> 23:33.940 In the pose that we receive here from the subscriber we get the angle. 23:33.970 --> 23:34.270 All right. 23:34.300 --> 23:36.280 So that's a bit of mathematics here. 23:36.280 --> 23:38.890 So if you've done that previously it might seem easy. 23:38.890 --> 23:41.290 If you've never done that it might seem super hard. 23:41.290 --> 23:43.930 But this is also something you can learn on the side. 23:43.930 --> 23:48.310 And, well, there is a part of robotics where you need to learn a bit of mathematics. 23:48.340 --> 23:52.120 Especially for control systems, which is what we're doing here. 23:52.150 --> 23:52.480 All right. 23:52.510 --> 23:54.700 So we have the difference which again. 23:54.730 --> 24:01.660 We are going to use as the velocity here the common velocity for angular z. 24:01.660 --> 24:07.510 So if the angle difference is a big number then the velocity is also going to be quite big. 24:07.540 --> 24:14.590 As we get closer to the right angle to just reach the target point and go forward, then the velocity 24:14.590 --> 24:16.570 and the angular z is going to decrease. 24:16.600 --> 24:20.860 There is one more thing I'm going to do is to it's called normalize the angle. 24:20.890 --> 24:32.650 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 24:32.680 --> 24:33.880 two pi. 24:33.880 --> 24:38.470 So two multiply math.pi. 24:38.590 --> 24:47.820 And if the difference is Lower than minus math dot pi. 24:48.420 --> 24:57.090 We do diff plus equal two times math dot pi. 24:57.300 --> 24:57.750 Okay. 24:57.780 --> 25:03.030 So we make sure that the angle is between minus pi and plus pi. 25:03.030 --> 25:08.370 And this is going to be important especially if you reach the edge of the trigonometrical circle okay. 25:08.400 --> 25:10.800 You could have some weird behaviors on the edges. 25:10.800 --> 25:12.720 So with this we are safe. 25:12.750 --> 25:13.260 All right. 25:13.260 --> 25:15.900 So we should be able to test the code. 25:15.930 --> 25:18.570 Actually I'm going to add one more thing here. 25:18.570 --> 25:24.870 I'm going to add a multiplier here I'm going to add two and I'm going to add six. 25:25.530 --> 25:27.120 And why is that. 25:27.150 --> 25:30.390 Well simply because I have tested the program before okay. 25:30.420 --> 25:36.900 And I have seen that multiplying by two here and by six here makes the turtle move better in a better 25:36.900 --> 25:40.350 way and reaches the target in a better way. 25:40.380 --> 25:40.560 Okay. 25:40.590 --> 25:41.910 And the command is quite smooth. 25:41.910 --> 25:43.560 And how did I get to this point? 25:43.590 --> 25:45.150 Well, it's just about testing. 25:45.180 --> 25:48.720 You know, you test stuff and then you see, you apply a bit more, a bit less. 25:48.720 --> 25:51.390 That's how you tune a P controller. 25:51.420 --> 25:51.600 Okay. 25:51.630 --> 25:56.160 When you have a P or PID controller, well there's actually a lot of experimentation. 25:56.160 --> 25:57.330 So there is a theory. 25:57.330 --> 26:00.240 But in the end you're going to do a lot of experimentation. 26:00.240 --> 26:02.070 So you start with something. 26:02.070 --> 26:07.740 And then if it doesn't work or if it doesn't work, well then you tinker the values a bit and then you 26:07.740 --> 26:09.810 end up with something that you like. 26:09.810 --> 26:16.290 So let's do a very quick code recap before we start this in the constructor, what we have. 26:16.320 --> 26:19.620 Well, we create a temporary target x and y. 26:19.620 --> 26:21.750 We initialize a post to none. 26:21.750 --> 26:25.710 And then we have a publisher and a subscriber for the topic. 26:25.710 --> 26:29.550 We need to send the comment to and for the position of the turtle. 26:29.550 --> 26:33.180 Then we create a control loop that's going to run at 100Hz. 26:33.180 --> 26:38.970 In this control loop, what we first check that we have at least received one post from the subscriber. 26:38.970 --> 26:41.820 And then we compute the distance between the turtle. 26:41.820 --> 26:46.020 So this master turtle and the goal we want to reach. 26:46.020 --> 26:51.120 So anywhere on the map, depending on that distance, we could say that if the distance is lower than 26:51.150 --> 26:54.150 0.5, we just stop the turtle and that's it. 26:54.150 --> 26:58.980 And then we send the command to the cmdvel topic. 26:58.980 --> 27:05.910 But if we still have a distance before reaching the position of the target, then we compute the velocity. 27:05.910 --> 27:09.540 So the linear velocity which is a multiplier of the distance. 27:09.540 --> 27:17.340 And we also compute the orientation so that the turtle can correctly orientate itself to go to the target. 27:17.370 --> 27:17.730 All right. 27:17.760 --> 27:20.340 So this is going to run 100 times per second. 27:20.370 --> 27:24.420 As you can see it's quite important that we don't block in this code. 27:24.420 --> 27:25.740 We don't wait for stuff. 27:25.770 --> 27:31.260 You see we just execute the code very fast because all of that is going to happen when we are spinning 27:31.260 --> 27:31.980 the node. 27:32.010 --> 27:32.220 Okay. 27:32.250 --> 27:35.490 So this control loop is going to run at 100Hz per second. 27:35.490 --> 27:40.950 And then we are also going to receive the pause every time we have a new message on that topic. 27:41.070 --> 27:41.310 Okay. 27:41.340 --> 27:45.090 So all of that is going to happen while the node is spinning. 27:45.120 --> 27:45.360 Okay. 27:45.390 --> 27:46.440 Let's save that. 27:46.440 --> 27:50.820 And let's add a new executable here in setup.py. 27:50.850 --> 27:57.690 So let's name it for example controller is equal to turtle sim. 27:58.890 --> 28:01.530 Catch them all. 28:01.530 --> 28:04.260 So that's the folder name here. 28:04.290 --> 28:08.370 Dot turtle controller. 28:08.370 --> 28:10.560 And that's going to be the main function. 28:10.560 --> 28:12.750 As always we save everything. 28:12.750 --> 28:14.730 And then let's go back here. 28:14.820 --> 28:16.860 Let's go back to the roster workspace. 28:16.860 --> 28:18.420 And let's do a call on build. 28:20.550 --> 28:27.330 Packages select with my uh no actually it's turtle SIM. 28:28.230 --> 28:29.430 Catch them all. 28:29.430 --> 28:32.790 And I'm also going to add symlink install. 28:34.470 --> 28:35.190 Great. 28:35.190 --> 28:38.310 Then we have well we have the turtle sim here. 28:38.340 --> 28:39.390 Still running. 28:39.570 --> 28:41.490 We can leave it there. 28:41.790 --> 28:42.660 I'm gonna. 28:42.690 --> 28:46.380 I'm gonna use that terminal. 28:47.010 --> 28:49.050 And let's do rush to run. 28:49.350 --> 28:51.030 Turtle sim. 28:51.810 --> 28:56.880 Catch them all, and we should find the controller executable. 28:56.880 --> 28:59.790 So let's run that. 28:59.820 --> 29:03.600 Let's make sure that we can see what's happening on the turtle. 29:03.660 --> 29:04.920 Let's see if it's working. 29:04.920 --> 29:08.850 So if it's working, the turtle should move to that point here somewhere. 29:08.850 --> 29:09.270 Here. 29:09.300 --> 29:10.200 Let's see. 29:11.040 --> 29:11.970 And okay. 29:11.970 --> 29:12.870 It's working. 29:13.140 --> 29:16.920 Well it's working, but we have an issue in the code. 29:16.920 --> 29:22.320 And well you can see in this case the error is not really explicit. 29:22.320 --> 29:27.210 But actually yeah, it's something like with float float numbers. 29:27.210 --> 29:34.800 And if we go back to the code I think that here, you see I have used the integer number because the 29:34.800 --> 29:39.090 error seems to be when we reach the goal somewhere when we reach the goal. 29:39.090 --> 29:44.130 So this is probably working fine, but that that is where the error comes from. 29:44.130 --> 29:49.080 When we send an integer instead of a float number for the x property. 29:49.110 --> 29:49.410 Okay. 29:49.440 --> 29:58.650 Once again, if I do, for example Ros2 interface show geometry messages with twist. 29:59.820 --> 30:01.980 You can see this is a float 64. 30:01.980 --> 30:05.310 So the type is very important. 30:05.460 --> 30:09.360 Let's put 0.0 and let's see again. 30:09.360 --> 30:11.760 So I have used Simulink install. 30:12.480 --> 30:22.350 Let's run that and let's run the node okay. 30:22.380 --> 30:24.990 Oh and this time it's working all right. 30:25.020 --> 30:30.240 So the total is moving to the target reaching the target and then stopping. 30:30.240 --> 30:32.850 So the error was with this. 30:32.880 --> 30:34.410 You see it can be quite tricky. 30:34.410 --> 30:40.170 So double check that you use the correct types for all the messages you send on the topics and also 30:40.170 --> 30:40.890 services. 30:40.920 --> 30:43.890 Now if I want to test something else. 30:43.890 --> 30:45.750 So let's say I want to go from here. 30:45.750 --> 30:46.740 I want to go there. 30:46.770 --> 30:54.810 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. 30:54.840 --> 30:57.630 So that's going to be somewhere around here. 30:57.660 --> 31:01.230 Now what I can do is I'm going to adjust. 31:01.230 --> 31:02.790 So I'm going to put that here. 31:06.240 --> 31:10.710 I'm going to leave the turtle theme alive the node alive. 31:10.710 --> 31:13.560 And I'm going to stop and start the controller again. 31:13.560 --> 31:18.060 And so it's going to start using the current pose of the turtle. 31:19.200 --> 31:22.170 And you see it's starting from there and going there okay. 31:22.200 --> 31:25.710 So now you can just change the target coordinates here. 31:25.740 --> 31:27.000 Start the node again. 31:27.000 --> 31:30.180 So it's working because we have used Simulink install. 31:30.180 --> 31:35.040 And you could give different coordinates and move the turtle around. 31:35.070 --> 31:35.460 All right. 31:35.460 --> 31:39.780 And that's the end of this first step which was probably also the longest step.