WEBVTT 00:00.260 --> 00:07.160 In this laboratory lesson, we will create a simple action server in C plus plus that calculates the 00:07.160 --> 00:08.480 Fibonacci sequence. 00:09.230 --> 00:15.440 By implementing these action server, we will have a practical example on how to implement and use Ros 00:15.490 --> 00:16.760 to actions. 00:16.760 --> 00:23.330 And so we will use this script as a template during the upcoming course lessons where we will implement 00:23.330 --> 00:25.100 more complex applications. 00:25.610 --> 00:31.940 In this lesson we want to use the Ros two actions to implement a node that calculates the Fibonacci 00:31.940 --> 00:32.690 sequence. 00:33.020 --> 00:40.310 In mathematics, the Fibonacci sequence, also known as Golden sequence, is a sequence of integers 00:40.310 --> 00:46.280 where each number is the sum of the two preceding ones, except for the first two numbers, which are 00:46.280 --> 00:48.620 by definition zero and one. 00:48.920 --> 00:53.170 The length of the Fibonacci sequence is defined as its order. 00:53.180 --> 00:58.280 For example, the first two numbers indicate the order one of the sequence. 00:58.280 --> 01:00.470 So this is a sequence of order one. 01:00.770 --> 01:07.040 Then the order two of this sequence is represented by another number one which is given by the sum of 01:07.040 --> 01:08.600 the two previous numbers. 01:08.720 --> 01:13.020 And then the order three of this sequence is represented by the number two. 01:13.050 --> 01:17.250 That is still the sum of the two previous numbers and so on. 01:17.250 --> 01:23.040 So the action server that we are going to implement in this lesson will receive as a goal the order 01:23.040 --> 01:28.890 of the Fibonacci sequence that we want to calculate, and we will return the entire Fibonacci sequence 01:28.890 --> 01:31.830 up to the requested order as a result. 01:32.520 --> 01:38.700 So let's add the node in Visual Studio code in the Arduino board, CP example. 01:38.700 --> 01:49.080 And so here in the source folder, let's create a new file and let's call this one simple action server 01:49.320 --> 01:50.550 dot CP. 01:51.440 --> 01:56.060 And as usual here, let's start by importing the CPP library. 01:56.600 --> 01:59.990 So let's include the RCL, CPP. 02:00.650 --> 02:03.440 And the RCL Cpjp. 02:04.040 --> 02:06.380 Next, let's create a new node. 02:06.380 --> 02:13.370 So a new class called Simple Action Server. 02:13.370 --> 02:18.150 And this one inherits from the node class of the CPP library. 02:18.170 --> 02:25.670 So this one inherits from the CPP and so from the node class. 02:26.420 --> 02:28.910 And let's close also the parentheses. 02:29.300 --> 02:37.640 Let's place this class inside a namespace and let's call this namespace as the package itself. 02:38.180 --> 02:43.310 So Arduino bot CPP examples. 02:44.250 --> 02:52.800 So let's paste the class definition here inside the Arduino board example namespace, and let's start 02:52.800 --> 02:59.250 by creating the elements of the class so the attributes of the class and let's start with the public 02:59.250 --> 02:59.850 ones. 02:59.850 --> 03:09.420 And so here, let's start by declaring the explicit constructor of the Simple Action Server class. 03:09.420 --> 03:14.850 And so this constructor takes as input an object of the node options class. 03:15.150 --> 03:21.690 So from the CPP it takes a node options object. 03:21.990 --> 03:29.820 And let's call this variable options and this contains some configuration parameters for the node itself. 03:30.030 --> 03:36.480 By default, if no parameters are passed to the constructor of this class, let's initialize this one. 03:36.480 --> 03:39.660 So let's give a default parameter with an empty. 03:39.660 --> 03:44.980 So CPP with an empty node option object. 03:45.840 --> 03:47.830 Then still with this constructor. 03:47.840 --> 03:50.180 So let's create the body. 03:50.540 --> 03:56.690 But here, when we initialize, so when we call the constructor, let's also call the constructor of 03:56.690 --> 03:57.380 the base class. 03:57.380 --> 04:03.020 So the constructor of the node class to which we pass a string as input that is the name of the node. 04:03.020 --> 04:09.320 And let's call this one simple action server. 04:09.320 --> 04:13.220 And also we pass the node options to this class. 04:13.220 --> 04:16.490 So let's pass the variable that we called options. 04:17.180 --> 04:23.240 And then in order to create an action server within this node, we need to import another library. 04:23.270 --> 04:30.050 So let's include the Rql CPP action. 04:30.050 --> 04:35.780 And from this one let's import the CPP action dot PHP library. 04:35.990 --> 04:42.080 And now from this library we can use the server class to create a new action server. 04:42.230 --> 04:51.350 So among the private variables of this class, let's create a new object of the CPP action. 04:51.650 --> 04:54.050 And so let's create a new server. 04:54.080 --> 05:01.910 This is a template class and as always, let's take a shared pointer to this class and let's name it 05:02.300 --> 05:04.430 action server. 05:04.880 --> 05:09.500 As we are creating a shared pointer, let's remember to include. 05:11.880 --> 05:19.890 Also the memory module from the C plus plus standard library and then within the angular brackets of 05:19.890 --> 05:21.360 the server template class. 05:21.360 --> 05:28.200 So of this object here, we need to pass the communication interface that the action server has to use 05:28.200 --> 05:30.690 for the communication with the client. 05:30.720 --> 05:38.100 Specifically, we need to define an interface with the goal feedback and result message to exchange 05:38.100 --> 05:40.530 messages with the action server. 05:40.770 --> 05:47.100 If you haven't followed me in the Python lessons, we need to define this interface in the Arduino board 05:47.100 --> 05:48.240 messages package. 05:48.240 --> 05:54.120 So in this one in which we have already defined the message interface with the service already. 05:54.120 --> 06:02.190 So as by convention, the message interface with the service was inserted in a folder called Serve the 06:02.190 --> 06:04.710 message interface for the communication with an action. 06:04.710 --> 06:09.240 Instead, we have to insert it within the folder called action. 06:09.240 --> 06:12.940 So let's create this folder if you haven't done it yet. 06:13.120 --> 06:19.180 And then within this folder let's create a new file called Fibonacci Dot Action, which will contain 06:19.180 --> 06:20.020 the interface. 06:20.020 --> 06:23.830 So the message interface for the communication with the action server. 06:24.600 --> 06:29.160 In this file, we have to define the structure of the goal message. 06:29.160 --> 06:35.670 And this is simply an integer that indicates the order of the Fibonacci sequence that we want the server 06:35.670 --> 06:36.690 to calculate. 06:37.110 --> 06:43.530 Then using the three dashes, we can also define how the result message is done, and this is given 06:43.530 --> 06:46.230 by an array of integer numbers. 06:46.230 --> 06:51.150 And so this represents the full Fibonacci sequence that is calculated by the server. 06:51.270 --> 06:54.390 And finally, also still separated by three dashes. 06:54.420 --> 06:57.890 We need to specify how the feedback message is done. 06:57.900 --> 07:05.190 Also, this one is an array of integers, and this represents the partial Fibonacci sequence that is 07:05.190 --> 07:07.860 calculated during the server execution. 07:08.560 --> 07:13.000 Then once you have defined these Fibonacci action file. 07:13.000 --> 07:21.460 So this interface we need also to declare it within the cmakelists.txt of the Arduino bot messages and 07:21.460 --> 07:28.300 we can do so still in the Ros generate interfaces in which we have declared the interfaces. 07:28.300 --> 07:31.570 With these services, we can also add a new line. 07:31.570 --> 07:38.230 So we can declare also the existence of the Fibonacci action within the action folder. 07:38.620 --> 07:43.210 And finally, we need also to change the package dot XML. 07:43.210 --> 07:47.770 And here we need to add the dependency from the action messages. 07:48.510 --> 07:51.210 Now if you have just created this interface. 07:51.210 --> 07:56.910 So the Fibonacci dot action before being able to use this interface within our script. 07:56.910 --> 08:02.700 So within our simple action server dot CPP, you need to compile again the workspace. 08:02.700 --> 08:11.400 And so in a new terminal, let's go to the Arduino board workspace and here you need to compile it with 08:11.400 --> 08:17.730 the command called build so that the new interface so the Fibonacci action is properly compiled and 08:17.730 --> 08:18.960 we can start using it. 08:19.680 --> 08:28.680 So in order to start using it, we can include from the Arduino board messages from the action folder. 08:28.680 --> 08:35.730 This time let's include the Fibonacci dot, PHP and now we can start using this interface. 08:35.730 --> 08:40.940 And so we can set that the action server that here we are implementing. 08:40.950 --> 08:48.780 So from the PHP action, this action server has to use the interface that is in the Arduino board messages 08:49.140 --> 08:53.820 in the action module and that is called Fibonacci. 08:53.820 --> 09:00.540 So we are going to use this interface and now once we have this variable, so once we have created the 09:00.540 --> 09:04.560 action server variable, we can start initializing it. 09:04.560 --> 09:09.210 And so within the constructor let's initialize this shared pointer. 09:09.210 --> 09:10.890 So the action server. 09:11.100 --> 09:15.390 And to do that we can use the create server function. 09:15.390 --> 09:26.190 So this time from the RCL, CP Action library, let's use the create server function. 09:26.190 --> 09:32.790 This is as well a template function and within the angular brackets we need to specify the type. 09:32.790 --> 09:37.530 So we need to specify the interface that this action server has to use. 09:37.560 --> 09:44.460 And so basically this one here that we have already used to create the server object. 09:44.880 --> 09:52.350 So let's say that this action will use the Fibonacci interface that is in the Arduino Bot Messages library 09:52.650 --> 09:54.840 and then within the constructor. 09:54.840 --> 10:02.370 So here within the parentheses we need to pass also the instance of the Ros two node that hosts the 10:02.370 --> 10:03.390 action server. 10:03.390 --> 10:05.370 And so this is the current node. 10:05.520 --> 10:12.120 And so the current class, the simple action server class as this is implementing the node class. 10:12.120 --> 10:17.850 So as this is inheriting from the node class and so we can use the pointer this. 10:18.480 --> 10:25.080 Then we also need to set the name of the action server, namely the name by which the server will be 10:25.080 --> 10:27.360 recognized and used in Ros two. 10:27.630 --> 10:31.830 And we call this one Fibonacci. 10:32.400 --> 10:36.240 And then it also receives three callback functions. 10:36.240 --> 10:42.480 So we need to pass to this functions to the create server function three callback function that are 10:42.480 --> 10:46.380 going to be executed in response to different events. 10:47.060 --> 10:51.080 For the three of them, we can use the standard bind function. 10:51.080 --> 10:57.980 So from the standard library, let's use the bind function to declare the three callback function. 10:58.970 --> 11:05.210 So let's use the bind also here and also for the last one. 11:06.290 --> 11:11.690 So now we have used three times the bind function from the standard library. 11:12.050 --> 11:17.690 And now let's start setting which are these callback function that we are going to call. 11:18.580 --> 11:19.660 The first one. 11:19.660 --> 11:27.490 So the first callback function that we are going to use is the goal callback function that still we 11:27.490 --> 11:31.570 are going to define within the simple action server class. 11:31.780 --> 11:40.000 So let's say that this function so that the goal callback function belongs to the simple action server 11:40.000 --> 11:47.590 class, let's format this a little bit better so that it's more readable this way. 11:48.010 --> 11:55.420 And now let's also set the pointer this to say that we want to use the implementation of this function 11:55.420 --> 11:57.790 that is in the simple action server. 11:57.790 --> 11:59.500 So in the current object. 11:59.650 --> 12:02.530 And then let's also use the namespace. 12:02.920 --> 12:16.510 So using namespace placeholders from the standard library and from this namespace we can use the definition 12:16.510 --> 12:20.570 of the placeholder one and the placeholder two. 12:20.750 --> 12:26.990 And basically this says that this callback function will receive as input two parameters. 12:26.990 --> 12:33.920 So one and two that here we set with the placeholders, then the second callback function. 12:33.920 --> 12:40.430 So here in the second bind function is called the cancel callback. 12:42.950 --> 12:49.580 So let's call this one cancel callback and once more we are going to define this function within the 12:49.580 --> 12:51.530 simple action server. 12:51.890 --> 12:58.220 Still, let's use the this pointer to indicate that we want to use the current implementation. 12:58.220 --> 13:02.000 So the implementation of the cancel callback in the current object. 13:02.000 --> 13:08.240 And then let's use just the placeholder one to indicate that this function is going to receive just 13:08.240 --> 13:09.980 one variable as input. 13:10.310 --> 13:13.790 And then let's declare also the third callback function. 13:13.790 --> 13:17.870 So the last callback function that is the accepted callback. 13:17.870 --> 13:18.590 So. 13:21.290 --> 13:23.210 This is the accepted callback. 13:23.240 --> 13:25.120 Here we need two dots. 13:25.960 --> 13:33.100 And also let's copy this one to indicate that also the accepted callback will be defined within the 13:33.100 --> 13:34.990 simple Action server class. 13:35.290 --> 13:37.450 Then let's use the this pointer. 13:37.750 --> 13:41.960 And also in this case, this function will receive just one parameter as input. 13:41.980 --> 13:47.500 So in this case, let's use the placeholder one about these three callback functions. 13:47.500 --> 13:52.060 So the goal callback, the cancel callback and the accepted callback function. 13:52.090 --> 13:53.350 The first one. 13:53.350 --> 13:59.410 So the goal callback function is called when the action server receives a new goal. 13:59.410 --> 14:02.440 So a new goal from an action client. 14:02.440 --> 14:07.780 And then when it receives a new goal, it can decide whether or not to accept it. 14:07.780 --> 14:14.320 And so to start the execution of the action server or to reject it in case the goal is accepted. 14:14.320 --> 14:21.520 So in case it satisfies certain requirements defined by the action server, then the accepted callback 14:21.520 --> 14:26.840 function is executed and this will contain the actual logic of the action server. 14:26.840 --> 14:33.110 So after the goal is actually accepted and then if during the execution of the server. 14:33.110 --> 14:38.930 So if during the execution of the calculation of the Fibonacci sequence, the server receives a cancellation 14:38.930 --> 14:45.050 request from the action client, then the cancel callback function is executed. 14:45.050 --> 14:48.530 And so the goal is canceled and the calculation is stopped. 14:48.560 --> 14:55.850 With this, we have instantiated and initialized a new action server called Fibonacci, which uses an 14:55.850 --> 15:02.630 interface of messages called Fibonacci that is defined in the Arduino board messages package. 15:02.780 --> 15:08.870 Now, after the initialization of this action server, let's print a new message in the terminal with 15:08.870 --> 15:12.860 the Rql CPP info. 15:15.560 --> 15:19.820 And let's use the get logger function from the RCL CPP 15:22.520 --> 15:34.730 and let's take the RCL cpp logger and let's print the message starting the action server. 15:35.820 --> 15:42.560 Now we can proceed to define the three callback function that we have used in the action server to handle 15:42.560 --> 15:48.560 different phases and different requests that the action server may receive from the client. 15:48.890 --> 15:56.030 So here, among the private attributes and the private methods of the simple Action server class, let's 15:56.030 --> 16:09.140 start by defining the goal callback function, and this one returns an object of type RCL CP action 16:09.380 --> 16:14.450 and it returns a goal response object. 16:14.450 --> 16:15.530 And this one. 16:15.530 --> 16:22.250 So this goal callback function receives as input the unique ID of the goal that was just received. 16:22.250 --> 16:32.870 And so basically it receives as input an object of the CP action of type goal Uid. 16:33.740 --> 16:43.640 And let's call also this one you u ID and also it receives as input a shared pointer to the goal message 16:43.640 --> 16:44.630 of the action. 16:44.630 --> 16:48.200 So to the goal message that was just received. 16:48.200 --> 16:51.650 So let's take this one from the standard message. 16:52.010 --> 17:02.720 Let's create a shared pointer and this is a shared pointer to an object of the interface Arduino bot 17:02.750 --> 17:03.890 messages. 17:04.490 --> 17:10.850 Then from the action folder, let's take the Fibonacci interface. 17:10.940 --> 17:15.000 And from the Fibonacci interface, let's take the goal message. 17:15.000 --> 17:18.090 And so let's call this variable goal. 17:18.090 --> 17:20.790 So this shared pointer is called goal. 17:21.700 --> 17:22.600 In this function. 17:22.600 --> 17:30.280 Let's start by printing an informative message in the terminal with the rql CPP info. 17:30.670 --> 17:35.160 And so here, let's get the logger. 17:35.170 --> 17:46.810 Let's get the CPP logger and let's print the message received goal request 17:49.090 --> 17:50.140 with order. 17:50.470 --> 17:53.710 And then here, let's make this a stream. 17:54.850 --> 17:56.910 And now let's print the order. 17:56.920 --> 18:00.750 And so the order is in the goal object. 18:00.760 --> 18:06.280 And then as this is a shared pointer, let's access to the order variable. 18:06.760 --> 18:11.230 So here it goes, the Rql CPP namespace. 18:11.380 --> 18:12.730 Now it's fine. 18:13.060 --> 18:16.140 And now let's automatically accept the goal. 18:16.150 --> 18:22.850 So for simplicity in this function, in the goal callback function, we don't perform any check either 18:22.850 --> 18:27.860 on the goal or on the server, but let's simply return a goal response. 18:27.860 --> 18:30.980 So let's automatically accept the goal. 18:31.550 --> 18:44.510 So let's return an rql CPP action goal response and let's return an accept and execute. 18:44.510 --> 18:47.540 So automatically this will accept the goal. 18:47.540 --> 18:51.500 And so every time we receive a new goal, it will be automatically accepted. 18:51.500 --> 18:57.860 And so after the goal callback function, as we are always accepting any goal that the server receives, 18:57.860 --> 18:59.590 then the accept. 18:59.600 --> 19:03.310 So the accepted callback function will be executed. 19:03.320 --> 19:06.530 So let's move on and let's define this function. 19:06.830 --> 19:14.000 This is a void function so it returns void and this function receives as input a shared pointer to an 19:14.000 --> 19:17.570 object of the server goal and class. 19:17.900 --> 19:22.070 So this is still from the standard library. 19:22.550 --> 19:34.370 It receives a shared pointer to an object of the Rql CPP action namespace. 19:34.370 --> 19:43.580 And from this one it takes a server goal and this is a template class and it needs as input the type 19:43.580 --> 19:44.600 of interface. 19:44.930 --> 19:53.180 So this is still in the Arduino board messages in the action folder and it receives a Fibonacci interface 19:53.450 --> 19:58.280 and let's call this object goal and all. 19:58.940 --> 20:03.470 And now let's define in this function the execution logic of the server. 20:04.100 --> 20:10.640 However, we must remember not to keep the client blocked for too long while waiting for the server 20:10.640 --> 20:11.600 to execute. 20:11.600 --> 20:16.640 So while waiting for the server to finish the accepted callback function. 20:16.940 --> 20:19.520 For this reason, let's place the actual code. 20:19.520 --> 20:26.630 So the actual logic of the action server within a different thread in order to avoid to block the current 20:26.630 --> 20:27.050 thread. 20:27.050 --> 20:33.170 And so to allow to return the message to the client as fast as possible so that the client can continue 20:33.170 --> 20:40.220 with its execution while the server is performing its functionalities to do so in C plus plus. 20:40.220 --> 20:45.410 Let's include the thread module. 20:45.410 --> 20:49.670 So from the standard library and now let's use it to create a new thread. 20:49.670 --> 20:54.170 So from the standard library, let's create a new thread. 20:54.980 --> 20:58.880 And inside this thread we want to execute a certain function. 20:58.880 --> 21:04.520 So again, let's use the bind function from the standard library. 21:04.790 --> 21:11.300 And also we want to detach the execution of this function from the current thread. 21:11.300 --> 21:17.120 So we want this one to be executed in a different thread from the one in which the accepted callback 21:17.120 --> 21:18.830 function is executed. 21:19.460 --> 21:26.930 The function that we want to execute is called execute and we are going to define this function still 21:26.930 --> 21:29.630 within the simple action server class. 21:29.990 --> 21:38.270 So we can copy this one here and we can paste it here in order to indicate that the execute function 21:38.270 --> 21:43.700 that we want to execute in this thread is declared in the Simple Action server class. 21:44.400 --> 21:51.990 Then let's use the this pointer and the placeholder one in order to indicate that this function will 21:51.990 --> 21:59.460 receive one parameter as input, and the parameter that it will receive as input is exactly the goal 21:59.460 --> 22:00.960 angle object. 22:00.990 --> 22:07.650 So this one here, this variable here that the accepted callback function just receives this input. 22:08.630 --> 22:13.160 At this point, we can proceed to the definition of this execute function. 22:13.490 --> 22:20.000 So let's define this function still within the simple action server. 22:20.330 --> 22:26.300 So here we have also the definition of the execute function, and this one is the one that will actually 22:26.300 --> 22:32.780 contain the logic of the server and that will be executed in a separate thread in order to keep the 22:32.780 --> 22:33.710 client free. 22:33.710 --> 22:39.830 So the client will not wait until the server execution, but will be able to perform other functionalities 22:39.830 --> 22:42.980 while the server is calculating the Fibonacci sequence. 22:43.520 --> 22:51.200 Again, this function receives a shared pointer to an object of the server goal and class, so this 22:51.200 --> 22:54.330 receives the same input as the accepted callback. 22:54.350 --> 23:01.760 So let's copy and paste the same input and the first thing that we can do in this execute function is 23:01.760 --> 23:03.560 to print a new message in the terminal. 23:03.560 --> 23:10.440 So with RCL, CP info and the message that we are going to print. 23:10.440 --> 23:12.420 So first let's take the logger. 23:12.660 --> 23:20.460 So rcl, CP, get logger and let's get the rcl cp logger. 23:20.880 --> 23:26.370 And the message that we are going to print is executing goal. 23:26.370 --> 23:32.100 So this informs basically the user that the action server is already in the execute function and that 23:32.100 --> 23:34.410 is going to execute the goal soon. 23:35.310 --> 23:38.910 Then let's create an object of the rate class. 23:38.910 --> 23:47.100 So still from the RCL CP library, let's create an object rate and let's call this one loop rate. 23:47.310 --> 23:54.810 And this object can be used to control the execution of a loop within our Ros two node so that two consecutive 23:54.810 --> 23:58.130 execution of the loop are temporarily separated. 23:58.140 --> 24:01.710 For example, let's say that they are separated by one second. 24:02.730 --> 24:05.280 Next, let's create a goal variable. 24:05.280 --> 24:05.880 So. 24:08.450 --> 24:14.260 And this one contains the goal message that we can extract from the goal and. 24:15.860 --> 24:22.340 So let's extract the goal message with the get goal function. 24:22.340 --> 24:28.130 And so now instead of writing this, every time, we can just refer to the goal variable. 24:28.550 --> 24:31.010 Now let's also create a feedback message. 24:33.230 --> 24:35.270 And this is a shared pointer. 24:38.720 --> 24:39.740 So STD. 24:44.580 --> 24:48.180 To an object of the feedback message. 24:48.360 --> 24:58.830 So from the Arduino bot messages from the action and from the Fibonacci interface, we want this feedback 24:58.830 --> 25:02.640 variable to be an instance of the feedback message. 25:03.030 --> 25:04.980 And now we can use this message. 25:04.980 --> 25:11.220 So the feedback message that we have just created to send periodically messages to the action client 25:11.220 --> 25:16.230 in order to inform it about the progress of the Fibonacci action server. 25:16.440 --> 25:22.860 So let's create a new reference variable and let's call this one sequence. 25:24.150 --> 25:25.620 And let's access. 25:25.620 --> 25:28.920 So let's set this one equal to the feedback. 25:29.640 --> 25:32.850 And let's access to the partial sequence. 25:32.850 --> 25:38.640 So now every time, instead of accessing to the partial sequence of the feedback message like this, 25:38.670 --> 25:41.490 we can just refer to the sequence variable. 25:41.580 --> 25:47.320 And so now we can use this variable to initialize the Fibonacci sequence with the first two numbers 25:47.320 --> 25:50.560 that always appear in the Fibonacci sequence. 25:50.680 --> 25:55.480 So to this sequence, let's push back. 25:55.510 --> 25:58.210 So let's add the first element that is zero. 25:58.780 --> 26:00.440 And then let's add. 26:00.460 --> 26:03.760 So let's push back the second element. 26:03.760 --> 26:05.020 That is always one. 26:05.500 --> 26:08.710 And now we can create the result variable. 26:11.050 --> 26:13.600 And this is another shared pointer. 26:19.050 --> 26:23.490 And this time this is a shared pointer to an object of the class result. 26:23.490 --> 26:25.680 So of the interface result. 26:26.310 --> 26:33.660 And so from the Arduino, both messages from the action and from the Fibonacci interface, we want the 26:33.660 --> 26:37.560 result variable to be an instance of the result message. 26:37.830 --> 26:43.650 Now at this point, all the components are ready and we can actually implement the logic. 26:43.650 --> 26:47.570 So the algorithm for the calculation of the Fibonacci sequence. 26:47.580 --> 26:55.180 So let's create a for loop that goes from the index one to the goal order. 26:55.200 --> 27:02.430 So this is goal and let's access to the order variable. 27:02.970 --> 27:06.210 And at each iteration let's increment the index. 27:06.210 --> 27:12.060 And now let's also make this loop executed as long as the Ros two node is running. 27:12.060 --> 27:19.130 So here let's also verify that RCL cpp is okay. 27:19.140 --> 27:23.190 Let's wrap also all of this among the parentheses. 27:25.400 --> 27:26.060 Okay. 27:26.060 --> 27:32.870 And now this loop will be executed for the index that goes from one to the goal order and also until 27:32.870 --> 27:34.130 Ros is running. 27:34.130 --> 27:41.570 So the Ros two node is running first before performing any calculation, we need to check if the action 27:41.570 --> 27:44.330 has received any cancellation requests. 27:44.330 --> 27:54.500 And so if this happens, so if the goal ended as received the cancellation request, so is canceling. 27:54.980 --> 28:01.490 So in this case, if the goal ended is canceling, then we need to update the result message with the 28:01.490 --> 28:05.690 partial sequence that was calculated up to this iteration of the loop. 28:05.930 --> 28:14.330 So let's update the result message and the sequence that is in the result message to the partial sequence 28:14.330 --> 28:15.950 that we are calculating. 28:15.950 --> 28:18.550 And then let's also set the status of the goal. 28:18.560 --> 28:27.510 So let's set the goal handle to canceled and let's pass the result. 28:27.720 --> 28:30.750 And also in this case, let's print a message in the terminal. 28:34.120 --> 28:35.800 So let's take the logger. 28:39.470 --> 28:44.840 And let's take the CP logger and let's print the message. 28:45.080 --> 28:47.570 Goal cancelled. 28:47.930 --> 28:50.000 And in this case, let's return. 28:50.390 --> 28:56.260 So let's finish the execution of the execute function otherwise. 28:56.270 --> 29:02.140 So if we go here, otherwise if no cancellation request was received. 29:02.150 --> 29:08.900 So namely, if the action server is still in execution and the client didn't send any cancellation request, 29:08.900 --> 29:12.680 then we can proceed with the calculation of the Fibonacci sequence. 29:13.160 --> 29:15.680 So we can add a new element to the sequence. 29:16.070 --> 29:17.390 So sequence. 29:17.510 --> 29:23.120 And then let's push back a new element to this sequence. 29:23.120 --> 29:34.910 And this one is given by the element at the last index, plus the element at the second last index. 29:34.910 --> 29:37.410 So I minus one. 29:37.830 --> 29:41.640 With this, we have added a new element to the Fibonacci sequence. 29:41.640 --> 29:46.570 And so since we added this new element, let's also publish a feedback message. 29:46.590 --> 29:56.670 So using the goal and let's publish a new feedback with the publish feedback function. 29:57.300 --> 30:01.680 And to this we pass the feedback message. 30:01.680 --> 30:07.830 So the feedback message in which we are filling the field, partial sequence and also let's print a 30:07.830 --> 30:09.600 message in the terminal. 30:09.600 --> 30:12.720 So rcl cp info. 30:19.400 --> 30:28.070 And let's print the message, publish feedback, and then at this point, to simulate the effect that 30:28.070 --> 30:32.330 the server is executing as low and costly operation. 30:32.330 --> 30:36.110 So an operation that takes some time to complete. 30:36.140 --> 30:37.880 Let's add a sleep. 30:37.880 --> 30:41.000 So basically, let's interrupt the execution of this script. 30:41.000 --> 30:47.870 So the execution of this for loop and let's wait it for a few seconds to do so. 30:47.870 --> 30:52.580 We can use the loop rate object from the rate class. 30:52.700 --> 30:58.640 So let's use this variable and here let's call the sleep function. 30:58.640 --> 31:05.750 So basically this will stop the execution of the for loop for the time required in order to match the 31:05.750 --> 31:08.090 frequency that is defined with the rate. 31:08.090 --> 31:11.180 So in order to match the frequency of one hertz. 31:11.180 --> 31:18.300 And so basically this will make the for loop publish a message each second at the end of this loop. 31:18.300 --> 31:24.930 So here after the for loop, the action server will have already calculated all the Fibonacci sequence 31:24.930 --> 31:31.350 so up to the desired order and also will have sent a feedback message to the client for each of the 31:31.350 --> 31:34.020 elements that it added to the sequence. 31:34.620 --> 31:40.460 Now let's check if the sleep is still running. 31:40.470 --> 31:45.000 So if the node is still running, let's update the result message. 31:45.180 --> 31:52.860 So let's update the result message and the variable sequence to the sequence variable that now already 31:52.860 --> 31:55.440 contains the full Fibonacci sequence. 31:55.560 --> 31:59.940 And then let's set the status of the goal with the goal and all. 32:00.150 --> 32:08.850 So let's set this one to succeed and also let's pass the result that contains the full Fibonacci sequence. 32:08.970 --> 32:11.640 Furthermore, let's also print a message in the terminal. 32:16.460 --> 32:20.780 And this message says, let's take this one goal. 32:23.160 --> 32:28.860 Succeeded now to complete the implementation of the Simple Action Server class. 32:28.890 --> 32:35.490 We need to define also the cancel callback function so the execute function is completed and we can 32:35.490 --> 32:38.190 move on to define the this one. 32:38.190 --> 32:43.050 So the missing one, the cancel callback function that is executed when the server. 32:43.050 --> 32:47.820 So the action server receives a cancellation requests from the client. 32:48.150 --> 32:53.970 So let's define this function and this function returns an object. 32:55.050 --> 33:07.740 So here of the CP action library of type cancel response and it receives an input another shared pointer. 33:10.830 --> 33:16.530 To an object of the type CP action. 33:17.070 --> 33:25.710 And so if the class server goal endl and also here this is a template class, so the server goal and 33:25.830 --> 33:32.010 is a template class which needs as input the type of the interface that is Arduino board messages, 33:32.010 --> 33:36.870 then action, and then the Fibonacci interface. 33:36.870 --> 33:41.490 And let's call this variable again goal and all. 33:41.700 --> 33:47.910 So let's move on to define the behavior of this function, the console callback function. 33:47.910 --> 33:51.240 And let's start by printing a message in the terminal. 33:55.050 --> 33:56.850 So let's get the logger. 33:58.140 --> 34:00.000 And the message says. 34:01.210 --> 34:07.930 Received request to cancel the. 34:09.760 --> 34:16.990 And so let's return now an object of this type, so of cancel response. 34:18.910 --> 34:20.920 And let's set this one to accept. 34:20.920 --> 34:26.440 So this means that basically the action server accepted the gold cancellation. 34:27.380 --> 34:31.530 So with this, the simple action server node is completed. 34:31.550 --> 34:37.760 So our simple action server node is completed and now we can define the main function in which it will 34:37.760 --> 34:38.840 actually be used. 34:38.840 --> 34:41.780 So the functionalities of this class will actually be used. 34:42.570 --> 34:48.930 Regarding the launch of a ros2 node that implements an action server in C plus plus, we can use a very 34:48.930 --> 34:54.060 useful macro that is available in the components library. 34:54.210 --> 35:02.730 So let's include from the RCL CPP components library. 35:02.730 --> 35:09.600 So from this library, let's include the register node, macro CPP. 35:09.870 --> 35:13.100 And from this one let's use at the end of the file. 35:13.110 --> 35:17.100 So here the macro rcl cpp. 35:18.070 --> 35:24.390 And then components and then register a node. 35:24.820 --> 35:32.310 So let's use the macro to register the simple action server class that we created in the Arduino CP 35:32.320 --> 35:33.250 examples. 35:33.580 --> 35:36.670 And so that we called Simple Action Server. 35:37.840 --> 35:41.520 Let's take care of adding this macro here. 35:41.530 --> 35:45.670 So this function here after the namespace. 35:46.000 --> 35:51.670 So here these are the parentheses that declare the namespace arduino bot CP example. 35:51.700 --> 35:56.410 Take care of adding this macro here after the closing parentheses. 35:57.440 --> 36:03.140 Next we can move on to declare the dependencies and also to install this node inside the file. 36:03.170 --> 36:04.940 Cmakelists.txt. 36:05.630 --> 36:10.220 Here, let's declare the usage of the library CPP action. 36:10.790 --> 36:19.730 So let's use another find package instruction to declare the usage of the CP Action library and also 36:19.730 --> 36:27.110 another one to declare the usage of the CP components. 36:27.530 --> 36:34.100 And then we need to tell the compiler how it should build and install the simple action server CP script. 36:34.100 --> 36:36.050 So the one that we have just created. 36:36.750 --> 36:37.830 For an action server. 36:37.830 --> 36:44.670 The instruction to install and compile the source code are slightly more complex and so we can simply 36:44.670 --> 36:51.720 copy paste these instructions from the Ros two wiki, which I will also leave in the course description. 36:51.720 --> 36:58.410 So here, after adding all the executables and before installing them, let's paste the instructions 36:58.410 --> 37:01.650 that are attached to the description of this lesson. 37:01.770 --> 37:08.880 In this case, basically what we are doing is adding a new library that is called Simple Action Server, 37:08.880 --> 37:14.190 and then we are declaring the dependencies of this library that are basically defined here. 37:14.190 --> 37:17.580 And so that are the Arduino board messages, the Lcvp. 37:17.940 --> 37:21.330 Lcvp action and RCL CP components. 37:21.330 --> 37:27.030 And finally here we are registering the node that implements the action server, which is the one that 37:27.030 --> 37:34.500 is in the Arduino CP Examples namespace and is implemented in the class Simple Action Server. 37:35.240 --> 37:37.760 Finally, let's also install this new node. 37:37.850 --> 37:41.270 And so to do so, we need another install instruction. 37:41.270 --> 37:42.800 So let's add it here. 37:44.780 --> 37:54.920 And this time the targets are the simple action server. 37:55.760 --> 38:07.520 And we want to install the archive destination into the lib folder, then also the library destination 38:10.430 --> 38:15.350 into the lib folder and then let's set the runtime destination. 38:18.610 --> 38:20.710 To the bin folder. 38:21.220 --> 38:23.140 Let's save this file. 38:23.170 --> 38:31.180 And now we need to declare these dependencies also in the package dot XML file of the Arduino CP examples 38:31.180 --> 38:31.870 package. 38:32.020 --> 38:40.060 So here, let's add a new dependency from the CP action package. 38:40.690 --> 38:45.580 Then let's add a new dependency from the CP 38:47.920 --> 38:54.970 components package and this is all we need in order for this simple action server to be executed and 38:54.970 --> 38:56.260 correctly compiled. 38:57.260 --> 39:02.180 So now after all these changes, of course we need to recompile our workspace. 39:02.360 --> 39:08.960 So let's go to the workspace and let's build it with the Falcon build. 39:11.420 --> 39:18.670 We can see that there are some warnings because of some unused variables, but none of these is an error. 39:18.680 --> 39:20.390 So let's continue. 39:21.170 --> 39:25.550 And so we can continue by executing our action server. 39:25.820 --> 39:29.450 So in a new window of the terminal, let's source the workspace. 39:29.630 --> 39:31.460 So the file setup dot bash. 39:31.460 --> 39:39.220 And here to start our action server, let's run from the Arduino bot CP examples. 39:39.230 --> 39:45.830 So if we press tab twice now we can see that we have a node that is called Simple Action Server Node. 39:46.130 --> 39:49.730 So let's start the simple action server node. 39:49.760 --> 39:55.190 Let's press enter and we get the confirmation that the action server node is running. 39:55.220 --> 40:01.970 So we can see from the terminal output that the starting action server message is printed and also we 40:01.970 --> 40:03.200 can double check it. 40:03.230 --> 40:11.580 If we open a new terminal and we execute the command Ros2 action list and so we get the list of all 40:11.580 --> 40:14.660 the currently available actions in Ros2. 40:14.880 --> 40:19.800 And since we have started only one node, so the last node that we created, we can see that there is 40:19.800 --> 40:23.130 only here one action server available. 40:23.130 --> 40:25.140 That is the Fibonacci action. 40:25.560 --> 40:31.860 Now we can also get more information about this action server using the Ros2 info command. 40:31.890 --> 40:40.590 But before using so let's source our workspace also in this terminal in order for ros2 to recognize 40:40.590 --> 40:42.270 the message interface. 40:42.360 --> 40:51.900 So now we can use the Ros2 action info command and let's take more information about the Fibonacci and 40:51.900 --> 40:56.730 let's also use the flag minus T in order to display also the type. 40:56.730 --> 41:02.340 So the type of the interface of the message interface to communicate with this action server. 41:02.580 --> 41:06.750 So let's press enter and currently we can see that for the action. 41:06.750 --> 41:12.780 So for the server there is only one node that is interacting with this action server and this is the 41:12.780 --> 41:14.730 simple action server. 41:14.730 --> 41:21.180 And also we can see that this server is using the Fibonacci interface that is defined in the Arduino 41:21.180 --> 41:22.350 bot messages. 41:22.950 --> 41:28.470 At this point we can also send a new goal to this server and again from the terminal we can use the 41:28.470 --> 41:33.540 command Ros2 action send goal. 41:33.660 --> 41:37.680 And so let's send a goal to the Fibonacci. 41:38.040 --> 41:44.460 And now if we press tab twice, we can see that Ros two automatically detects that this action server 41:44.460 --> 41:50.760 accepts Fibonacci messages so that the Fibonacci is the interface that we have to use to communicate 41:50.760 --> 41:51.870 with this server. 41:52.740 --> 41:54.720 And so let's use this interface. 41:54.840 --> 42:01.620 Now let's use the double quotes, then press tab twice, then type O and again tab. 42:01.620 --> 42:08.370 And this will automatically insert an empty prototype of the goal message that we can send to this action 42:08.370 --> 42:09.000 server. 42:09.210 --> 42:13.560 So for example, let's request so let's send a goal of ten. 42:13.560 --> 42:19.860 So basically we are asking the action server to calculate the Fibonacci sequence up to the order ten. 42:20.100 --> 42:24.510 Let's also use the flag minus F to display the feedback. 42:24.510 --> 42:30.030 So minus F stands for feedback in order to display the feedback in the same terminal. 42:30.330 --> 42:35.040 So let's make this a little bit bigger and let's press enter. 42:35.850 --> 42:39.350 And we can see that when we send the goal, there is an error. 42:39.360 --> 42:46.740 So there is a segmentation fault in the action server and most probably this is due to some shared pointer 42:46.740 --> 42:48.600 that we have not initialized. 42:48.720 --> 42:51.480 So let's go back to the simple action server. 42:52.100 --> 42:57.650 And actually we can see that here, both the feedback message and the result message. 42:57.650 --> 43:01.160 We created a shared pointer, but we never initialized it. 43:01.190 --> 43:08.120 So here, instead of using the shared pointer, let's directly use the make shared function. 43:08.270 --> 43:14.810 So for both of these so that we are declaring to shared pointer still of type Fibonacci feedback and 43:14.810 --> 43:15.860 Fibonacci result. 43:15.860 --> 43:17.990 And also we are initializing them. 43:18.230 --> 43:22.580 Now back in the terminal, let's build again our workspace. 43:23.280 --> 43:25.430 So let's stop the client. 43:25.910 --> 43:28.370 Let's start again the action server. 43:28.430 --> 43:30.850 And now let's send again the goal. 43:30.860 --> 43:32.780 So still with the order ten. 43:32.900 --> 43:34.490 So let's press enter. 43:34.490 --> 43:36.380 And now we can see that correctly. 43:36.380 --> 43:41.670 As soon as we send this goal, the action server informs us that it has received a new goal. 43:41.690 --> 43:49.500 So here we can see it and that is executing it and also that is constantly publishing feedback messages. 43:49.520 --> 43:55.800 Also, we can see those feedback messages here in the terminal with the partial sequence that the action 43:55.800 --> 43:56.970 server is calculating. 43:56.970 --> 44:02.310 So at each step we can see that the server is adding a new element. 44:02.550 --> 44:05.760 And also we can see that at the end of the service execution. 44:05.760 --> 44:11.250 So here we have the confirmation that the action server executed correctly, the goal. 44:11.250 --> 44:13.890 So it reached the order required. 44:14.010 --> 44:19.230 And so here we can see also at the end the result message with the full sequence.