WEBVTT 00:00.080 --> 00:03.110 This is now step three for this final project. 00:03.110 --> 00:08.630 And for now, what we have is we have one node, as you can see here, that can spawn turtles on the 00:08.630 --> 00:09.800 turtle SIM window. 00:09.800 --> 00:16.580 And we have another node that can control the main turtle turtle one here and move it to some coordinates. 00:16.580 --> 00:20.000 So now we need to find a way to link those two nodes. 00:20.000 --> 00:21.650 So to make them communicate. 00:21.650 --> 00:30.440 And the first thing we can do is to ask the turtle spawner node to actually send the live turtles. 00:30.440 --> 00:32.030 So we're going to call a live turtles. 00:32.030 --> 00:37.850 The turtles that are still present here on the screen is to send that to the turtle controller, so 00:37.850 --> 00:44.690 the turtle controller can get the list of turtles with the coordinates and starts to reach one turtle. 00:44.690 --> 00:47.240 And that's what we are going to do here first. 00:47.240 --> 00:54.290 So we are going to go to our turtle spawner node first and create a publisher so that for example, 00:54.290 --> 01:00.140 whenever we spawn a new turtle, we can publish the list of a live turtles. 01:00.140 --> 01:05.700 And then when this publisher is working, we can add the subscriber side on the turtle controller node. 01:05.700 --> 01:07.110 And so what do we want to send. 01:07.110 --> 01:08.790 Well we want to send first. 01:08.820 --> 01:10.230 That's going to be a list. 01:10.260 --> 01:16.590 A list of what a list of turtles and what information we have inside the turtle. 01:16.590 --> 01:17.970 What do we care about. 01:17.970 --> 01:25.980 Well, we care about the well basically the things, the name and then the three coordinates x, y and 01:25.980 --> 01:26.520 theta. 01:26.550 --> 01:32.490 Actually, the orientation for the target turtle is not really important, but we can send all of that. 01:32.520 --> 01:36.450 And so what I'm going to do here is I'm going to have to create a new interface. 01:36.450 --> 01:38.880 I'm going to go to terminal here. 01:38.910 --> 01:43.170 Let's go to the workspace in the source directory. 01:43.170 --> 01:49.110 And well we already have my robot interfaces package that's already fully configured. 01:49.110 --> 01:51.180 And it's inside this workspace. 01:51.180 --> 01:54.720 So you could create a new interfaces package for that application if you want. 01:54.750 --> 01:57.750 But I'm just going to use that package okay. 01:57.780 --> 02:05.680 So I go in my robot Robots interfaces, and in the MSG folder where I already have two and I'm going 02:05.710 --> 02:07.060 to create a new one touch. 02:07.090 --> 02:10.870 I'm going to name it turtle dot msg. 02:11.680 --> 02:11.920 Okay. 02:11.950 --> 02:15.130 To create one kind of object for a turtle. 02:15.130 --> 02:21.130 And then I'm going to create another one turtle array dot msg. 02:21.130 --> 02:25.090 So we can create a list of turtle objects. 02:25.270 --> 02:27.190 All right let's go back to the code. 02:27.190 --> 02:34.780 And here in my robot interfaces I have my two new interfaces here. 02:34.780 --> 02:37.270 And so what are the informations we need. 02:37.300 --> 02:39.100 String name. 02:39.610 --> 02:42.370 And then float64. 02:42.400 --> 02:44.530 I'm going to use float 64 x. 02:45.130 --> 02:47.410 Float 64 y. 02:47.710 --> 02:50.740 Float 64 theta. 02:51.040 --> 02:51.550 Okay. 02:51.580 --> 02:55.810 That's going to define one turtle that I send over the topic. 02:55.810 --> 02:58.930 So before you create an interface like this you might check. 02:58.960 --> 03:02.490 Do you already have an already existing interface for that. 03:02.520 --> 03:04.620 If not, you create a new interface. 03:04.650 --> 03:04.950 Okay. 03:04.980 --> 03:10.650 If you find an existing interface, but you have to use it in a way that's not designed for, it's best 03:10.650 --> 03:17.490 to create a new interface that also has a meaningful name and just the fields that you need. 03:17.520 --> 03:18.060 All right. 03:18.090 --> 03:20.700 And as you can see, it's quite easy to create an interface. 03:20.700 --> 03:22.740 So I have my turtle message. 03:22.740 --> 03:24.180 That's for one turtle. 03:24.210 --> 03:28.230 Now in my turtle array I'm going to add a list of. 03:28.230 --> 03:32.190 So I'm going to put turtle with brackets. 03:32.190 --> 03:36.900 So you see turtle here is the type of the message of the interface. 03:36.900 --> 03:41.160 And I don't put any package in front because it's from the same package. 03:41.790 --> 03:44.700 And I'm going to name that turtles. 03:44.850 --> 03:52.350 So this turtle array message contains a list of the turtle message here. 03:52.410 --> 03:56.550 Now what we need to do is to build those interfaces. 03:56.550 --> 04:03.040 So I go to the cmakelists.txt of my robot interfaces and I'm going to add the messages here. 04:03.040 --> 04:04.540 So one per line. 04:04.630 --> 04:06.460 Turtle dot msg. 04:06.460 --> 04:12.190 And here msg turtle array dot msg. 04:12.580 --> 04:13.510 And that's it. 04:13.510 --> 04:15.310 Everything else is correctly configured. 04:15.310 --> 04:17.740 So make sure you save all the files. 04:17.770 --> 04:19.750 Let's go back to the terminal. 04:19.750 --> 04:23.380 And let's also stop that. 04:23.800 --> 04:25.960 Let's build the interfaces. 04:25.960 --> 04:28.600 Click on Build packages. 04:28.600 --> 04:34.030 Select with my robot interfaces. 04:34.030 --> 04:38.110 We might have a warning like this is not a problem. 04:40.330 --> 04:40.810 Okay. 04:40.840 --> 04:42.550 And now the interfaces are built. 04:42.550 --> 04:44.080 Let's just test. 04:44.080 --> 04:50.380 So let's source the workspace and then let's do Ros2 interface. 04:50.410 --> 04:57.640 Show my robot interfaces in MSG slash turtle. 04:57.670 --> 04:58.810 Do we find it? 04:58.840 --> 05:02.600 Yes, we find turtle and we find total array. 05:03.320 --> 05:04.790 Yes, we find total array. 05:04.790 --> 05:10.940 And as you can see, total array description is the list of total with the name x, y and theta. 05:11.090 --> 05:15.230 So if you see this the interfaces are built and you can use them in your code. 05:15.260 --> 05:18.230 But now well you will need to source all your terminals. 05:18.230 --> 05:24.650 And if you remember, if you want to have the autocompletion for the the interfaces in your VSCode environment, 05:24.680 --> 05:28.280 you also need to open it from a source environment. 05:28.280 --> 05:34.820 So what I would recommend is just close everything and open everything again. 05:35.000 --> 05:39.290 At this point it's quite quick and easy. 05:39.950 --> 05:47.420 You see, in a few seconds everything is done and I can well, I can close the simplest the txt. 05:47.510 --> 05:49.610 I don't need that anymore. 05:50.000 --> 05:50.300 Okay. 05:50.330 --> 05:54.350 And I can come back to my turtle spawner.py. 05:54.350 --> 05:59.390 And now I can include my new interfaces I can do from. 05:59.450 --> 06:11.130 That's going to be my robot interfaces dot msg import turtle and from my robot interfaces dot msg import 06:11.940 --> 06:13.500 turtle array. 06:13.770 --> 06:19.350 Just to make things a bit cleaner with all those imports it doesn't matter, but I could group for example 06:19.350 --> 06:22.470 the interfaces here just like that. 06:22.500 --> 06:25.500 Okay, you could put the non Ros stuff first. 06:25.530 --> 06:28.800 Well you do as you want as long as it's clean. 06:28.800 --> 06:30.390 And then what do I want to do. 06:30.390 --> 06:33.000 I want to publish on a topic. 06:33.000 --> 06:36.450 I want to publish a turtle array message. 06:36.450 --> 06:38.310 So I'm going to create a publisher. 06:38.310 --> 06:42.240 I'm going to create it here self dot. 06:43.500 --> 06:47.820 Let's name it alive turtles. 06:48.240 --> 06:54.750 Publisher is equal to self dot create publisher. 06:54.780 --> 06:57.720 The type is turtle array. 06:58.440 --> 07:00.300 The topic name is. 07:00.300 --> 07:02.980 Well we don't have any subscribers yet. 07:02.980 --> 07:09.730 We are kind of creating the topic here, so let's name it alive turtles. 07:10.000 --> 07:10.360 Okay? 07:10.390 --> 07:12.910 I don't put any slash here. 07:12.940 --> 07:16.600 It's going to be added automatically and I need the queue size. 07:16.630 --> 07:17.050 All right. 07:17.050 --> 07:18.280 So we have a publisher. 07:18.280 --> 07:23.230 Then what I'm going to create is a function to publish on the topic. 07:23.230 --> 07:35.170 So publish alive turtles itself I'm going to create a message of type turtle array. 07:35.200 --> 07:41.590 And then message dot turtles need to be an array of turtles. 07:41.590 --> 07:50.920 And for now what I'm going to do is I'm just going to create an array here self dot live turtles, which 07:50.920 --> 07:53.500 is going to be an empty array okay. 07:53.530 --> 08:00.100 So I initialize an empty array in the constructor because we have created zero turtle and I just put 08:00.130 --> 08:07.800 self Alive turtles and I can do self dot alive turtles. 08:07.800 --> 08:11.850 Publisher dot publish the message. 08:12.120 --> 08:12.570 All right. 08:12.570 --> 08:19.140 So I already have the structure for the publisher and the method to send a message on the topic. 08:19.170 --> 08:21.270 Now we need to do two things. 08:21.270 --> 08:24.570 We need to save the alive turtles. 08:24.570 --> 08:29.010 So basically when we spawn a new turtle we need to save it inside the list. 08:29.010 --> 08:31.830 And we also need to publish that list. 08:31.830 --> 08:33.090 So how to do that? 08:33.090 --> 08:39.540 Well, when we get the confirmation in this callback called spawn service method, when we get the confirmation 08:39.540 --> 08:41.790 that we have spawned a new turtle. 08:41.820 --> 08:42.030 Okay. 08:42.060 --> 08:46.380 And the name is not empty, then here we know we have a new turtle. 08:46.380 --> 08:48.090 We can add it to the list. 08:48.090 --> 08:50.340 So I'm going to keep the structure. 08:50.340 --> 08:53.940 And just after this log I'm going to keep the log I can create. 08:53.970 --> 08:59.010 Let's say new turtle is equal to turtle okay. 08:59.040 --> 09:07.030 That's why I also imported the turtle Still message here is that we can create, you see a turtle object, 09:07.030 --> 09:11.710 and then we can fill this with the information and add it to the list. 09:11.740 --> 09:11.980 Okay. 09:11.980 --> 09:15.880 So then we can just publish the list as a turtle array. 09:17.920 --> 09:26.560 So we create our new turtle and then we can do new turtle dot name is equal to. 09:26.590 --> 09:29.890 Well we have response dot name. 09:29.890 --> 09:32.740 That's the name we get here as a response. 09:32.980 --> 09:41.710 And then new turtle dot x is equal to request dot x. 09:41.710 --> 09:43.330 And we don't have any autocompletion. 09:43.330 --> 09:49.540 We could also provide the type here spawn dot request. 09:49.960 --> 09:57.040 So then I do new turtle dot y is equal to request dot y. 09:57.070 --> 10:05.150 And finally new turtle dot theta is equal to request dot theta. 10:05.150 --> 10:09.860 So you can see here that passing the request in the callback is quite important. 10:09.860 --> 10:15.260 So that we can get the x and y for the for the turtle that we create. 10:15.260 --> 10:19.310 And now that we have this object, we can simply add it to the list. 10:19.700 --> 10:27.260 Self dot live turtles append new turtle. 10:27.890 --> 10:32.180 Okay, so every time we spawn a new turtle we add it to the list. 10:32.180 --> 10:33.890 So that's done. 10:33.920 --> 10:36.770 Now when to publish this list? 10:36.770 --> 10:42.020 Well we could publish just after we add it to the list because that's when we update the list. 10:42.500 --> 10:47.090 So self dot publish a live turtles. 10:47.090 --> 10:52.640 We don't need to pass anything here because the array is already a class attribute. 10:52.880 --> 10:53.300 Okay. 10:53.330 --> 10:55.910 So for this publisher, as you can see we don't need to create a timer. 10:55.910 --> 10:57.200 We don't need to create anything. 10:57.200 --> 11:02.850 We just publish from a client service callback. 11:02.850 --> 11:08.400 So every time we spawn a new turtle, we add it to the list and then we publish the list. 11:08.430 --> 11:12.570 Let's save that and let's test that functionality first. 11:12.630 --> 11:15.810 I don't need to build because I have used Simulink install. 11:15.810 --> 11:19.200 So let's just run turtle sim here to run. 11:19.440 --> 11:23.310 Turtle sim and turtle sim node. 11:23.940 --> 11:26.310 Then I can run my spawner. 11:26.790 --> 11:34.260 Turtle sim catch and spawner. 11:35.100 --> 11:37.290 So, is it working? 11:37.290 --> 11:39.000 Let's see that it's working. 11:39.030 --> 11:39.510 Yes. 11:39.510 --> 11:40.170 The first one. 11:40.170 --> 11:40.380 No. 11:40.380 --> 11:43.650 But then from turtle two you see turtle three, etc.. 11:43.650 --> 11:49.470 So now if I do Ros two topic list I have. 11:49.500 --> 11:53.220 Okay, I have lots of topics because for each turtle it's going to create a few topics. 11:53.220 --> 12:05.060 But then I have my alive turtles topic Ros two topic equal alive turtles And you can see we have a list. 12:05.060 --> 12:08.150 So I'm gonna press Ctrl C. 12:08.180 --> 12:13.700 Now I'm going to stop everything and you can see what we received here. 12:13.700 --> 12:15.140 So one message. 12:15.470 --> 12:22.400 He had a list of turtles with the name x, the Y and the theta. 12:22.430 --> 12:28.040 The sea turtle two, turtle three, turtle four until turtle 16. 12:28.130 --> 12:28.520 All right. 12:28.550 --> 12:33.050 So we have the list of all the turtles that we have spawned so far on the turtle sim window. 12:33.050 --> 12:33.530 Great. 12:33.530 --> 12:38.210 And now that you see this, we can do the subscriber side because we know the publisher is publishing 12:38.210 --> 12:39.770 the correct data. 12:39.770 --> 12:41.210 So let's go back to the code. 12:41.210 --> 12:51.650 And this time let's open Turtle Control dot Pi and let's add the interface from my robot interfaces 12:51.800 --> 12:54.290 dot msg imports. 12:54.320 --> 13:02.210 I'm also going to import turtle and from my robot interfaces message import. 13:02.940 --> 13:04.710 turtle array. 13:04.740 --> 13:06.480 Then we can add a subscriber. 13:06.480 --> 13:19.200 So for example here before that one or after that one let's say a live turtles subscriber self create 13:19.230 --> 13:24.330 subscription with the type turtle array. 13:24.750 --> 13:26.640 What is the topic name? 13:26.640 --> 13:30.060 The topic name must be exactly a live turtle. 13:31.620 --> 13:32.910 Live turtles with an S. 13:32.910 --> 13:34.020 I don't put a slash here. 13:34.020 --> 13:36.030 It's going to be added automatically. 13:36.360 --> 13:38.730 And then I need a callback. 13:38.760 --> 13:40.740 Let's create a callback here. 13:40.740 --> 13:47.790 For example def callback a live turtles. 13:49.020 --> 13:59.160 We receive a turtle list turtle list as a message which is of type turtle array. 13:59.190 --> 14:05.320 Okay, let's put pass for now and let's finish this, so we provide the callback. 14:06.520 --> 14:08.380 Callback alive turtles. 14:08.380 --> 14:10.780 And we also put a queue size. 14:10.810 --> 14:12.310 Go back to a new line here. 14:12.340 --> 14:13.210 All right. 14:14.260 --> 14:15.700 So we have our subscriber. 14:15.700 --> 14:17.080 We have our callback. 14:17.080 --> 14:18.730 What do we do in this callback. 14:18.730 --> 14:23.890 Well here we receive the list of all turtles including the coordinates. 14:23.890 --> 14:27.520 So what I'm going to do is I'm going to say that as a first step. 14:27.520 --> 14:30.190 And then we're going to improve that in the following step. 14:30.190 --> 14:39.340 I'm going to say that we are going to catch the first turtle okay I'm going to add a here self turtle 14:39.370 --> 14:41.800 to catch. 14:42.370 --> 14:44.920 I'm going to initialize that to none. 14:45.400 --> 14:47.950 And then I'm going to say turtle. 14:47.950 --> 14:53.560 So self dot turtle to catch is equal to the turtle list. 14:53.560 --> 14:58.840 Dot turtles okay so that's the turtles field inside the message. 14:58.840 --> 15:02.500 So if you confused with this you could we could also just say MSG. 15:03.350 --> 15:04.550 Say it's the message. 15:04.550 --> 15:06.560 And from the message, we get the total list. 15:06.590 --> 15:09.740 Well, just make sure that you write something that makes sense. 15:10.070 --> 15:14.030 And that's going to be the index zero. 15:14.060 --> 15:14.450 Okay. 15:14.480 --> 15:15.320 As simple as that. 15:15.320 --> 15:25.040 And of course what I'm going to do is I'm also going to check if the length of the turtles array is 15:25.040 --> 15:26.570 greater than zero. 15:26.990 --> 15:27.230 Okay. 15:27.230 --> 15:33.020 So we only do that if we receive a non-empty list, because if not we're going to get an error. 15:33.050 --> 15:33.410 Great. 15:33.410 --> 15:41.240 So whenever I get the list which is published by the spawner node, I save the first turtle of the list 15:41.240 --> 15:42.740 as the turtle to catch. 15:42.740 --> 15:51.230 And this here, this is a turtle, so it contains the x and y coordinate of the target. 15:51.230 --> 15:54.290 So you see I don't need those x and y coordinates. 15:54.290 --> 15:56.510 I don't need those temporary x and y. 15:56.540 --> 15:58.160 I can just remove them. 15:58.160 --> 16:01.550 And I can use those of the turtle to catch instead. 16:01.550 --> 16:08.400 So here instead of target x I can say total to catch dot x. 16:08.400 --> 16:09.930 We don't have the auto completion. 16:09.930 --> 16:16.320 So I can say here I can put the type for example. 16:17.370 --> 16:19.650 And now it should be working. 16:19.680 --> 16:20.610 Yes. 16:21.300 --> 16:24.300 And total to catch dot y. 16:24.330 --> 16:24.720 Okay. 16:24.750 --> 16:31.920 You see that since we just used the x target and the y target from the total we have received from this 16:31.920 --> 16:35.280 subscriber, I'm also going to add something here. 16:35.280 --> 16:43.680 If self pose is known or if also self dot total to catch is none. 16:43.680 --> 16:47.910 So if we haven't received a total yet okay. 16:47.940 --> 16:52.320 If we don't have any target, we're not going to do anything in this control loop. 16:52.320 --> 16:53.670 So we're going to return. 16:53.670 --> 16:56.880 Because if this is none then you're going to get an error here. 16:56.940 --> 16:57.480 All right. 16:57.480 --> 17:01.650 And there is nothing more to change in this control loop okay. 17:01.680 --> 17:05.790 Because it was already working for target X and target Y. 17:05.820 --> 17:12.870 Now we have just changed that to say that the target X and Y is from the turtle that we receive over 17:12.870 --> 17:19.260 the topic, and for now, we just get the first turtle and we're going to improve that later in this 17:19.260 --> 17:20.070 project. 17:20.100 --> 17:22.950 Let's save and let's test that. 17:23.250 --> 17:31.470 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 17:31.470 --> 17:32.220 going on. 17:34.770 --> 17:37.800 I put it right here and then I can start the. 17:37.830 --> 17:43.830 Well I can start the controller actually because as you can see the controller is going to run this 17:43.830 --> 17:44.850 control loop. 17:44.970 --> 17:48.930 But if we don't have any turtle to catch it's not going to do anything. 17:49.200 --> 18:01.080 So I can start the controller here was to run turtle SIM catch them all with controller. 18:01.740 --> 18:03.210 And is it working? 18:03.210 --> 18:04.110 Yes. 18:04.560 --> 18:05.490 Nothing is happening. 18:05.490 --> 18:06.540 But that's normal. 18:07.020 --> 18:08.490 And then let's start. 18:09.120 --> 18:10.770 Turtle SIM. 18:11.460 --> 18:12.300 Catch them all. 18:12.300 --> 18:13.590 Let's start the spawner. 18:13.590 --> 18:16.770 And let's pay attention to what's going to happen. 18:19.350 --> 18:22.800 So first one it's not spawned. 18:22.890 --> 18:27.210 Then second one, you see the turtle is going to that first turtle. 18:27.210 --> 18:29.490 Then we continue to spawn the turtles. 18:29.490 --> 18:35.340 But of course we have just asked the controller to go to the first turtle in the array. 18:35.400 --> 18:37.890 So I'm going to stop the spawner here. 18:37.890 --> 18:40.950 And what you can see is that it's correctly working okay. 18:40.980 --> 18:41.700 The turtle. 18:41.700 --> 18:46.680 So the main turtle went to the target turtle. 18:46.680 --> 18:48.720 So that's a very good result already. 18:48.720 --> 18:52.890 Now of course in the next step we need to actually remove that turtle. 18:52.890 --> 18:58.920 And we need to say to the master turtle to go to the next one so that we can catch one turtle and then 18:58.920 --> 19:03.120 catch all the other turtles as soon as they spawn.