WEBVTT 00:00.110 --> 00:06.320 If you haven't followed me in the previous Python lesson to create the task server, which is the action 00:06.320 --> 00:14.690 server that executes various actions on the robot, you need to create a new Ros2 package inside your 00:14.690 --> 00:15.560 workspace. 00:15.620 --> 00:20.180 So let's open a terminal and let's go to the workspace here. 00:20.180 --> 00:25.340 Let's go to the source folder in which we are creating all the packages with all the functionalities 00:25.340 --> 00:26.540 for our robot. 00:26.540 --> 00:34.910 And here let's create a new package with the command Ros2 package create. 00:34.910 --> 00:37.520 And let's use as a build type. 00:37.520 --> 00:45.620 Amend CMake and let's call this new package Arduino Bot Remote. 00:45.620 --> 00:51.140 So I'm not going to create this package as I have already created it in my workspace. 00:51.320 --> 00:55.340 And then back to the Arduino bot workspace. 00:55.340 --> 00:56.570 So back to the workspace. 00:56.570 --> 01:03.700 Once you have created this new package, lets simply build the workspace in order to compile it. 01:03.700 --> 01:06.640 So in order to compile the new package. 01:06.670 --> 01:08.260 Arduino bot remote. 01:08.260 --> 01:09.130 Perfect. 01:09.130 --> 01:16.180 So back in Visual Studio Code here, as you can see we have the Arduino bot remote package. 01:16.180 --> 01:20.650 And here let's start by creating a new C plus plus script. 01:20.650 --> 01:29.830 So within the source folder let's add a new file called Task server dot cp that will actually contain 01:29.830 --> 01:32.710 the implementation of the task server node. 01:32.860 --> 01:36.970 So we're going to develop the task server as an action server. 01:36.970 --> 01:38.770 So as an ros2 action server. 01:38.770 --> 01:44.530 And as this can be quite complicated with a lot of instructions, instead of developing all the code 01:44.530 --> 01:50.200 from scratch, we can simply copy paste the code that we have already developed for the action server 01:50.200 --> 01:53.080 here in the Arduino bot CP examples. 01:53.080 --> 02:00.580 So do one of the simple action server so we can copy all of this and we can paste it here within the 02:00.580 --> 02:01.960 task server. 02:02.230 --> 02:09.760 So now let's apply the changes that we need in order to adapt this simple action server to be the task 02:09.760 --> 02:10.540 server. 02:10.570 --> 02:16.600 So the first thing that we want to do is to rename here the class from Simple Action Server to Task 02:16.600 --> 02:17.500 server. 02:17.590 --> 02:23.380 So let's do a Ctrl F and let's rename the simple action server to Task Server. 02:23.410 --> 02:26.920 And so let's replace all perfect. 02:26.950 --> 02:29.380 Then also we can rename here. 02:29.380 --> 02:31.810 So the simple action server node. 02:31.810 --> 02:38.620 Instead of calling it Simple Action server we can simply call it Task Server. 02:38.620 --> 02:45.520 And also here the name of the action server that we are starting is not anymore the Fibonacci Action 02:45.520 --> 02:49.630 server, but we can call it task Server. 02:49.660 --> 02:55.530 Also, another change that we can do in this script is to rename here the namespace. 02:55.530 --> 02:58.080 So now we are not anymore in the Arduino bot. 02:58.110 --> 02:59.460 CPP examples. 02:59.610 --> 03:06.570 But we are in the Arduino bot remote package and so we need to change this one. 03:06.570 --> 03:13.770 Also, if we go here below, here we are in the Arduino bot remote namespace and also in this case the 03:13.770 --> 03:14.580 task server. 03:14.580 --> 03:20.010 So that we are registering here is defined in the Arduino bot remote namespace. 03:20.100 --> 03:20.940 Perfect. 03:20.940 --> 03:22.800 So let's go back here. 03:22.830 --> 03:25.710 And now let's continue initializing this one. 03:25.710 --> 03:28.380 So the action server we have renamed it. 03:28.380 --> 03:30.930 So now it is called task Server. 03:30.930 --> 03:32.880 And in the previous laboratory lesson. 03:32.880 --> 03:37.560 So when we developed the simple action server we have used this interface here. 03:37.560 --> 03:42.510 So the Fibonacci interface that we have declared in the Arduino bot messages package. 03:42.540 --> 03:44.970 Now for the purpose of this lesson. 03:44.970 --> 03:49.230 So for developing the task server we are not going to use anymore this interface. 03:49.230 --> 03:52.950 But we need to develop a new action interface. 03:53.150 --> 03:59.570 Actually, we can develop this action interface still within the Arduino bot messages within the action 03:59.570 --> 04:00.200 folder. 04:00.200 --> 04:06.470 And here, if you haven't followed me in the previous Python lesson, you need to create the Arduino 04:06.470 --> 04:08.420 bot task dot action. 04:08.420 --> 04:13.700 So this will be the new interface that we are going to use to interact with the task server. 04:14.210 --> 04:17.030 Then within this Arduino bot task action. 04:17.030 --> 04:22.550 So the goal is simply an integer that indicates the task number. 04:22.550 --> 04:28.550 And so this will be just a number that indicates which is the task that we want to execute on our robot. 04:28.550 --> 04:33.050 So there will be a limited number of tasks that our robot can execute. 04:33.050 --> 04:37.520 And so this one here indicates which of these tasks we want to execute. 04:37.700 --> 04:39.290 Then we have the result. 04:39.290 --> 04:45.140 So the result of this interface is just a boolean message that indicates whether the execution of the 04:45.140 --> 04:45.830 action server. 04:45.830 --> 04:49.880 So the task server was completed successfully or not. 04:49.880 --> 04:55.310 And then a feedback message that actually we are not going to use in this lesson, but that you can 04:55.310 --> 04:57.140 use for your own tests. 04:57.140 --> 04:58.640 That is the percentage. 04:58.640 --> 05:03.530 And so this, for example, can contain the percentage of completion of the action. 05:03.530 --> 05:04.790 So of the task. 05:04.940 --> 05:10.490 Then once you have created this Arduino task interface you also need to install it. 05:10.490 --> 05:12.980 So within the Cmakelists.txt. 05:13.370 --> 05:21.440 And so here after the Fibonacci action we also need to install the Arduino bot task action perfect. 05:21.470 --> 05:23.480 So let's save also this file. 05:23.480 --> 05:24.560 So this make list. 05:24.560 --> 05:26.840 And then in order to start using it. 05:26.840 --> 05:32.300 So to start using the Arduino bot task action here in the C plus plus file. 05:32.300 --> 05:39.470 And to get also the auto completion of Ros2 in Visual Studio Code as we have declared a new interface. 05:39.470 --> 05:44.810 So we need to go into the Arduino bot workspace and we need to build it. 05:44.810 --> 05:48.380 So before using actually the interface we need to build. 05:48.380 --> 05:54.850 And so to compile this action message interface, and this will be compiled then into the C plus plus 05:54.880 --> 05:58.810 header that we can use here that we can use in our task server. 05:59.050 --> 06:00.220 C plus plus. 06:00.400 --> 06:01.240 Perfect. 06:01.240 --> 06:05.890 So now once we have compiled it we can import. 06:05.890 --> 06:08.770 So we can include here from the Arduino board messages. 06:08.770 --> 06:12.670 We can import the Arduino bot task dot http. 06:12.940 --> 06:14.650 And now we can start using it. 06:14.650 --> 06:17.110 So we can start using this interface here. 06:17.110 --> 06:20.950 So the Arduino bot task instead of the Fibonacci interface. 06:20.950 --> 06:31.360 So also in this case whenever we use the Fibonacci we can do a Ctrl f and we can replace the Arduino 06:31.360 --> 06:32.590 bot task. 06:32.590 --> 06:35.170 So this is the new name of the interface. 06:35.170 --> 06:38.440 And so let's replace all the occurrences. 06:38.920 --> 06:39.700 Perfect. 06:39.700 --> 06:44.740 So now we have created our action server that is called task Server. 06:44.890 --> 06:47.470 And it implements this interface here. 06:47.520 --> 06:50.550 Now we can implement the actual logic of the server. 06:50.550 --> 06:57.030 So the one that will allow us to move the robot into a predefined position, depending on the goal that 06:57.030 --> 06:57.900 we received. 06:57.900 --> 07:03.930 And in order to implement all of this logic, we are going to use the C plus plus Moovit API. 07:03.930 --> 07:05.400 So let's include them. 07:06.030 --> 07:07.380 Let's do it here. 07:08.040 --> 07:17.100 So let's include from the Moovit package and then from the move group interface. 07:17.100 --> 07:20.760 Let's include the move group interface dot h. 07:20.790 --> 07:23.730 And here we can use actually this class here. 07:23.730 --> 07:28.140 So we can use this header file in order to create a new instance. 07:28.140 --> 07:34.410 So here among the private variables we can create a new instance from the move it. 07:35.580 --> 07:47.760 And then from the planning interface we can create a new instance of the move group interface class. 07:47.790 --> 07:54.240 And let's call this one our move group. 07:54.240 --> 07:59.250 And we can also create a second one that is for the second move group that we have defined in the move 07:59.250 --> 08:00.930 it configuration for our robot. 08:00.930 --> 08:06.450 That is the gripper move group. 08:06.450 --> 08:07.170 Perfect. 08:07.170 --> 08:16.920 And actually, let's declare a shared pointer to an object of this class with the STD shared pointer. 08:16.950 --> 08:21.510 So a shared pointer to an object of type move group interface. 08:22.170 --> 08:24.990 And now that we have declared these two objects. 08:24.990 --> 08:27.390 So these are two pointers. 08:27.390 --> 08:32.340 We can actually initialize them within the execute function. 08:32.340 --> 08:40.200 So here actually we can remove all the code here that we developed in the execute function. 08:40.200 --> 08:47.270 So up to this point and this was the code that we developed for the implementation of the Fibonacci 08:47.300 --> 08:48.140 sequence. 08:48.140 --> 08:52.700 And here let's check if the this variable here. 08:52.700 --> 08:57.200 So this pointer here the arm move group is not initialized. 08:57.230 --> 08:58.910 Let's initialize it. 08:59.030 --> 09:01.730 So let's set the arm move group. 09:01.850 --> 09:07.310 Let's initialize it with the make shared. 09:07.310 --> 09:13.610 And here among the angular parentheses we need to indicate the type of the class that we want to initialize. 09:13.610 --> 09:17.600 And so it's a move group interface object. 09:17.600 --> 09:19.760 So let's copy it here. 09:19.760 --> 09:22.520 And now within the constructor of this class. 09:22.520 --> 09:29.810 So basically here among the parentheses we also need to indicate the implementation of the node that 09:29.810 --> 09:33.890 it's going to be used in order to interact with the move group interface. 09:33.890 --> 09:37.040 And actually as this class here that we are creating. 09:37.040 --> 09:40.760 So the task server inherits from the Klccp node. 09:40.760 --> 09:50.380 So implements a Ros2 node, we can simply provide the shared from this function. 09:50.410 --> 09:54.250 And so basically this will create a node from the current class. 09:54.400 --> 10:01.450 And also still the constructor of this class also takes the name of the move group that we want to initialize. 10:01.780 --> 10:03.940 And this is the arm move group. 10:03.940 --> 10:05.740 So we are initializing the arm. 10:05.890 --> 10:07.930 Let's do the same also for the gripper. 10:07.930 --> 10:09.790 So we can copy these instructions. 10:09.790 --> 10:11.140 And we can paste it. 10:11.140 --> 10:17.890 And we can check that the when the gripper move group is not initialized let's initialize it. 10:17.890 --> 10:22.030 So let's set the gripper move group as a new shared pointer. 10:22.030 --> 10:23.830 So an object of this type. 10:23.830 --> 10:28.690 And this time the move group that we want to initialize is the gripper. 10:28.690 --> 10:34.600 So now this one will have an instance of this class here that points to the gripper move group. 10:34.600 --> 10:40.660 And this one here instead will have an instance of the move group interface that points to the arm move 10:40.690 --> 10:41.350 group. 10:41.410 --> 10:42.520 Perfect. 10:42.550 --> 10:48.160 Now let's also continue with the other callback functions that are executed whenever. 10:48.160 --> 10:54.700 For example, the action server receives a new goal or whenever it receives a new cancellation request. 10:54.700 --> 10:57.850 And actually here in the goal callback function. 10:57.850 --> 11:01.390 So we can leave it this function here as it is. 11:01.390 --> 11:08.530 So basically we are automatically and always accepting here the message that we are receiving. 11:08.530 --> 11:14.590 And here let's just change the message that we are printing in the terminal that says receive the goal 11:14.590 --> 11:15.970 request with. 11:18.160 --> 11:19.330 Task number. 11:19.330 --> 11:25.270 And here for the goal let's print the task number. 11:25.270 --> 11:31.690 So if you remember within the Arduino bot task action interface, the goal message just contains the 11:31.690 --> 11:32.590 task number. 11:32.590 --> 11:38.820 So here when we receive a new goal, we are just printing the task number that you are going to execute 11:38.820 --> 11:42.450 and automatically we are going to accept and to execute it. 11:42.960 --> 11:45.750 Then for the cancel callback function. 11:45.750 --> 11:50.700 So for this one here we can also leave everything as it is. 11:50.730 --> 11:53.970 The only thing that we need to change is here. 11:53.970 --> 12:01.680 So after printing the message in the terminal we can check if the arm move group exists. 12:01.680 --> 12:07.380 So if this one is initialized eventually we want to stop the robot. 12:07.380 --> 12:11.070 So we want to execute the function stop. 12:11.070 --> 12:14.460 So we want to call the function stop on the arm move group. 12:14.460 --> 12:19.470 And so for example if the robot was moving with this function here we are stopping it. 12:19.470 --> 12:21.570 And also let's do the same for the gripper. 12:21.570 --> 12:23.550 So if the gripper is moving. 12:24.090 --> 12:28.440 So if the gripper move group exists. 12:28.440 --> 12:31.470 So it means that we have initialized it here. 12:31.500 --> 12:35.460 And so it means that the function here execute is running. 12:35.490 --> 12:37.880 Then we want to stop it. 12:37.880 --> 12:43.040 So we want to stop the robot eventually if it was running perfect. 12:43.040 --> 12:46.610 And so with this we have also completed the cancelled callback function. 12:46.610 --> 12:52.790 Then let's move on to the accepted callback function that is executed right after the goal callback 12:52.790 --> 12:53.390 function. 12:53.390 --> 12:55.730 So this is the first one that is executed. 12:55.730 --> 12:57.890 This is accepting the goal. 12:57.890 --> 13:00.590 And then once this one accepts the goal. 13:00.950 --> 13:03.140 So this one is executed. 13:03.140 --> 13:06.680 And this function here, actually we can leave it as it is. 13:06.680 --> 13:12.170 And so it simply returns immediately and starts this one here. 13:12.170 --> 13:15.500 So it starts this function in a separate thread. 13:15.500 --> 13:21.470 And so actually this function here, the execute function is the one that will contain the body, the 13:21.470 --> 13:25.880 one that will contain all the logic that will execute the action server. 13:25.880 --> 13:32.240 And so that will interact with the Moovit API in order to move the robot to the desired position. 13:32.570 --> 13:39.920 So here, After printing the message in the terminal that we are executing the goal, we are initializing 13:39.920 --> 13:45.290 the two move groups that we want to use in order to interact with the Moveit API. 13:45.320 --> 13:46.040 Perfect. 13:46.040 --> 13:51.350 And then let's also create the result message that we want to return to the action client. 13:51.350 --> 13:56.150 So once the execution is finished, we need to return a result message. 13:56.150 --> 13:58.820 So a message of this type here. 13:59.480 --> 14:00.800 And so let's create it. 14:01.370 --> 14:04.100 Let's call it result. 14:04.220 --> 14:11.420 And so this is an instance with the make shared perfect. 14:11.420 --> 14:19.520 So this is an instance from the Arduino bot messages and from the action. 14:19.520 --> 14:24.500 This is an instance of the Arduino bot task interface. 14:24.500 --> 14:29.060 And actually we just want that result message from this interface. 14:29.060 --> 14:29.900 Perfect. 14:29.900 --> 14:35.800 And so with this one we have initialized this message that we are going to return at the end of the 14:35.800 --> 14:37.060 execute function. 14:37.090 --> 14:38.260 Perfect. 14:38.260 --> 14:44.800 And now, as we have already initialized the Cplusplus Moviethe interface, we can start using it to 14:44.830 --> 14:46.420 send comments to the robot. 14:46.450 --> 14:47.350 To do so. 14:47.350 --> 14:52.660 So to set the comments, the position of the joints that we want to send to the robot. 14:52.690 --> 14:57.760 Let's create two new vectors of doubles. 14:58.570 --> 15:04.780 And let's call the first one our joint goal. 15:04.780 --> 15:11.950 And let's also create a second one called gripper joint goal. 15:11.950 --> 15:18.460 And so as the name of these vectors suggests, we're going to use this one in order to set the desired 15:18.460 --> 15:24.880 position of the joints of the arm, and this one here instead to set the desired position of the joints 15:24.880 --> 15:26.050 of the gripper. 15:26.170 --> 15:27.430 Now here. 15:27.430 --> 15:33.270 So again back in the execute callback function based on the task number that we have received. 15:33.270 --> 15:39.660 So based on this number here that we have received in the goal message, we want to execute a different 15:39.660 --> 15:40.140 movement. 15:40.140 --> 15:43.200 So we want to bring the robot to a different position. 15:43.740 --> 15:49.560 So let's check if the goal handle that we have received in this function. 15:49.860 --> 15:54.060 And then let's get the goal. 15:54.060 --> 15:58.860 And then let's access to the task number. 15:58.860 --> 16:02.700 So let's check if this task number is equal to zero. 16:03.480 --> 16:04.050 Perfect. 16:04.050 --> 16:06.960 So we are executing the task number zero. 16:06.960 --> 16:11.820 And for this task for example let's move the robot to the home position. 16:11.970 --> 16:14.700 So let's set this vector here. 16:14.700 --> 16:25.650 So the arm joint goal here let's set the arm joint goal equal to the vector zero zero and zero. 16:25.650 --> 16:32.400 So as you might remember the r move group only contains three joints, so the one of the base of the 16:32.400 --> 16:34.260 shoulder and the one of the elbow. 16:34.290 --> 16:38.790 And so we are setting the position of the three of them to the position zero. 16:38.790 --> 16:41.310 So this corresponds to the home position. 16:41.310 --> 16:42.960 And then for the gripper. 16:43.530 --> 16:47.820 So here this one is gripper joint goal. 16:47.820 --> 16:49.950 This one only contains two joints. 16:49.950 --> 16:54.180 And for example in the on position let's set the gripper opened. 16:54.630 --> 17:01.320 So let's set the joint for to -0.7 and the joint 5 to 0.7. 17:01.320 --> 17:04.680 So actually these two joints will have the same value. 17:04.680 --> 17:07.770 So the two fingers will be open the same quantity. 17:07.770 --> 17:09.870 And one is the opposite sign. 17:09.870 --> 17:14.250 So one has the opposite sign as it will rotate in the opposite direction. 17:14.520 --> 17:18.780 Otherwise if the task number that we have received. 17:18.780 --> 17:22.380 So let's copy this one and let's paste it here. 17:22.380 --> 17:26.580 So if we received the request to execute the task number one. 17:26.580 --> 17:32.210 So in this case, we can bring the robot to an hypothetical picking position. 17:32.510 --> 17:34.460 So we can copy these two instructions. 17:34.460 --> 17:35.660 Let's paste it. 17:35.690 --> 17:45.140 And let's say that our picking position is the angle -1.4 for the base. 17:45.140 --> 17:54.500 Then -0.6 for the shoulder, and then -0.07 for the elbow and the gripper. 17:54.500 --> 18:00.260 So in the picking position let's simulate that the gripper is actually grasping an object. 18:00.290 --> 18:01.610 And so let's close it. 18:01.640 --> 18:03.770 So let's close the gripper. 18:03.770 --> 18:07.400 So let's bring it to the position zero zero. 18:07.430 --> 18:17.750 Otherwise if we received a goal with the task number two perfect. 18:17.750 --> 18:19.550 So in this case let's execute. 18:19.550 --> 18:21.950 So let's bring the robot to a different position. 18:21.980 --> 18:25.750 Again we can copy these two instructions here. 18:25.750 --> 18:27.670 And let's say that in this case. 18:27.670 --> 18:29.920 So when we receive the request. 18:29.920 --> 18:36.790 So the task number that is the number two we want to bring the robot to a hypothetical rest position. 18:36.820 --> 18:43.840 And let's say that this rest position is at -1.57 for the base. 18:43.840 --> 18:46.480 Then we have zero for the shoulder. 18:46.480 --> 18:48.820 And then we have -0.9. 18:48.820 --> 18:55.330 And so I calculated already this position here that corresponds to the robot that is bent on itself. 18:55.330 --> 18:57.640 So it's the rest position of the robot. 18:57.670 --> 19:01.420 And in this rest position let's also set the gripper closed. 19:01.420 --> 19:04.720 So let's set the value to be zero zero. 19:04.750 --> 19:10.900 Finally here if you want you can continue by adding new tasks to your robot. 19:10.900 --> 19:15.370 So for example what to do if you receive a task number three. 19:15.370 --> 19:17.260 And so here you can add the code. 19:17.260 --> 19:21.340 So the position of new tasks for your robot for the moment. 19:21.340 --> 19:24.610 So for the purpose of this lesson I'm just going to stop here. 19:24.610 --> 19:29.980 So we will just be able to send one of these three tasks to the robot. 19:29.980 --> 19:33.460 So zero 1 or 2 and otherwise. 19:33.460 --> 19:39.910 So if you set a task number that is different from these three here, let's just print an error message 19:39.910 --> 19:45.370 in the terminal with RCL CP error. 19:45.490 --> 19:46.210 Perfect. 19:46.210 --> 19:46.660 Here. 19:46.660 --> 19:59.530 Let's get the logger and let's print the message invalid task number perfect. 19:59.530 --> 20:01.240 And here as we don't know. 20:01.240 --> 20:07.240 So as we don't recognize the task number let's simply return so we cannot execute it. 20:07.630 --> 20:08.380 Perfect. 20:08.380 --> 20:14.620 So after this statement so after this control here that we are doing on the task number, basically 20:14.620 --> 20:16.390 we have this variable here. 20:16.390 --> 20:17.980 So the arm joint goal. 20:17.980 --> 20:24.540 So this vector and also the gripper joint goal are fully initialized and contain the desired position 20:24.540 --> 20:26.880 of the joints of the arm and the gripper. 20:27.150 --> 20:34.410 So now, before setting this position here as the target goal, let's first set the start position. 20:34.410 --> 20:39.660 So let's access to the R move group. 20:39.810 --> 20:45.330 And here let's use the set start state. 20:45.360 --> 20:46.290 Perfect. 20:46.290 --> 20:49.500 And here we can simply use the R. 20:51.510 --> 20:54.750 Move group. 20:54.780 --> 21:03.180 And we can use the function get current state perfect. 21:03.180 --> 21:09.510 So with this instruction basically we are setting the start state for the arm move group. 21:09.510 --> 21:14.550 And we are setting it equal to the current state of the arm itself. 21:14.550 --> 21:19.580 So we are simply setting the start position that corresponds to the current position of the arm. 21:19.790 --> 21:21.770 Let's do the same also for the gripper. 21:21.800 --> 21:27.050 So let's copy this line here and let's use the gripper move group. 21:27.050 --> 21:29.000 So let's set the start state. 21:29.000 --> 21:34.700 Also for this move group to be the gripper move group get current state. 21:34.790 --> 21:39.620 Now after setting the start position let's also set the target position. 21:39.620 --> 21:42.920 And so we can take again the arm move group. 21:42.980 --> 21:49.820 And we can use the function set joint value target. 21:49.910 --> 21:55.190 So here as the name of this function suggests we are setting a joint value. 21:55.190 --> 22:00.200 So we are setting a value for all the joints that belong to this move group here. 22:00.200 --> 22:03.830 And actually this value is here in this vector here. 22:04.340 --> 22:06.530 So let's copy it and let's paste it. 22:06.560 --> 22:10.940 Now basically with this we are setting one of these three values. 22:10.940 --> 22:14.210 So one of these three vectors as the desired state. 22:14.210 --> 22:16.640 So as the target state of the art. 22:16.790 --> 22:19.040 Let's do the same also for the gripper. 22:19.610 --> 22:23.810 So let's take the gripper move group. 22:23.810 --> 22:30.290 And let's set the target value to be the gripper joint goal. 22:30.350 --> 22:32.000 Now this function here. 22:32.000 --> 22:36.350 So the set joint value target returns a boolean. 22:36.350 --> 22:38.840 So let's store it in a new variable. 22:39.260 --> 22:43.700 And let's call the first one r within. 22:45.740 --> 22:46.760 Bounds. 22:46.880 --> 22:52.940 And so this boolean basically is checking whether the joint value that we have set for this move group 22:52.940 --> 22:54.890 here are within the boundaries. 22:54.920 --> 23:02.300 So the mechanical boundaries that we set for our robot and also this one checks the same for the gripper. 23:02.870 --> 23:09.050 And so let's call this variable gripper within bounds. 23:09.050 --> 23:09.590 Perfect. 23:09.590 --> 23:12.860 And so this stores the output of this function. 23:12.860 --> 23:16.630 And now let's check If any of these two. 23:16.660 --> 23:20.500 So if any of these two variables is equal to false. 23:21.070 --> 23:28.780 So if this variable here is equal to false or this variable here is equal to false. 23:30.160 --> 23:36.430 In this case it means that we are not able to set one of these two positions. 23:36.430 --> 23:40.810 So we are out of the boundaries of the mechanical boundaries of our robot. 23:40.810 --> 23:49.270 So in this case let's print an error message again with RCL CP error here. 23:49.270 --> 23:55.090 Let's get the logger and let's print the message target. 23:57.460 --> 24:03.700 Position out of boundaries. 24:03.730 --> 24:04.300 Perfect. 24:04.300 --> 24:09.040 And in this case, as we are not able to execute this position here. 24:09.040 --> 24:17.280 So as we are not able to execute the desired target position, let's simply return like this. 24:17.280 --> 24:20.250 So we are not going to execute anything. 24:20.430 --> 24:21.270 Perfect. 24:21.270 --> 24:27.840 So after this if statement, we are sure that the joint value target that we have set for our robot 24:27.840 --> 24:29.070 are actually reachable. 24:29.070 --> 24:30.960 And so we can start planning. 24:30.960 --> 24:35.790 So we can start planning a path that brings the robot from its current position. 24:35.790 --> 24:42.000 So from the start state to the desired joint state so to the desired target. 24:42.270 --> 24:47.160 So in order to do so we can create two new variables of type plan. 24:47.910 --> 24:59.700 So from the movie namespace and from the planning interface let's create an instance of the move group 25:00.300 --> 25:03.510 interface plan class. 25:03.510 --> 25:07.290 And let's call the first one our plan. 25:07.290 --> 25:12.030 And let's also create another one called Gripper plan. 25:12.030 --> 25:17.460 And so, as you can imagine, these two will contain the output of the planner. 25:17.490 --> 25:20.250 Both for the arm and for the gripper. 25:20.280 --> 25:22.770 Now in order to execute the planner. 25:22.770 --> 25:26.880 So in order to ask the planner to calculate a trajectory from the current state. 25:26.880 --> 25:31.050 So from the start state to the desired joint state. 25:31.050 --> 25:35.760 So to this one here we can simply use the arm move group. 25:36.030 --> 25:44.820 And on the arm move group we can simply plan and we can store the output of the plan for the arm within 25:44.820 --> 25:45.660 this variable here. 25:45.660 --> 25:50.310 So within the ARM plan let's do the same also for the gripper. 25:50.400 --> 25:52.080 So we can copy this one. 25:52.080 --> 25:56.190 And we can paste it for the gripper. 25:56.190 --> 26:00.060 And let's store the result within the gripper plan. 26:00.090 --> 26:07.590 Now as we already know here, this plan function returns actually an error code that defines whether 26:07.590 --> 26:10.190 the plan was successfully or not. 26:10.220 --> 26:13.910 So here let's check whether the arm. 26:13.910 --> 26:17.180 So the plan for the arm was executed successfully. 26:17.180 --> 26:19.190 So we were able to calculate a plan. 26:19.190 --> 26:23.840 And in this case the error code that is returned by this function is of type. 26:24.320 --> 26:25.340 Move it. 26:26.150 --> 26:27.080 Code. 26:28.910 --> 26:29.930 Move it. 26:30.530 --> 26:35.900 Error code of type. 26:35.900 --> 26:36.560 Success. 26:36.560 --> 26:40.310 So if the result of this one is of type success. 26:40.310 --> 26:42.050 And also let's check this a. 26:42.080 --> 26:45.380 So let's check the result for the plan of the gripper. 26:45.410 --> 26:54.530 Now let's store this one into a new boolean variable that we can call our plan success. 26:54.530 --> 27:00.350 And so this takes the value of this comparison here. 27:00.350 --> 27:07.100 So this one will be set to true if the result of this function here is equal to success. 27:07.130 --> 27:09.670 Let's do the same also for the gripper. 27:10.510 --> 27:14.560 So let's copy this one and let's paste it and let's call this one. 27:14.560 --> 27:18.730 So this new variable gripper plan success here. 27:18.760 --> 27:22.780 Let's close the parentheses and let's close the line. 27:22.780 --> 27:23.740 Perfect. 27:23.740 --> 27:26.620 And now here within the arm plan success. 27:26.620 --> 27:30.490 We have the result of the planner for the arm move group. 27:30.490 --> 27:34.060 And here we have the result of the planner for the gripper. 27:34.060 --> 27:42.250 So let's check if the planner was successfully able to plan a trajectory for the arm. 27:42.640 --> 27:46.810 And also it was successfully able to plan a trajectory for the gripper. 27:46.900 --> 27:50.830 So in this case we can actually execute the trajectory. 27:50.830 --> 27:55.540 And in order to execute the trajectory, we can access the arm move group. 27:55.540 --> 27:59.590 And we can simply execute the move function. 27:59.590 --> 28:02.080 And we can do the same also for the gripper. 28:02.170 --> 28:03.880 So we can paste this one. 28:04.120 --> 28:07.780 Let's change the name to be the gripper move group. 28:07.810 --> 28:12.340 And so actually this instruction here will execute this plan here. 28:12.340 --> 28:16.450 So the plan that was calculated at this step otherwise. 28:16.570 --> 28:22.630 So if any of the two planners so the one of the arm or the one of the gripper fails, let's print an 28:22.630 --> 28:28.210 error message in the terminal with rcl cpp error. 28:30.670 --> 28:38.020 And this message says one or more planners failed. 28:38.110 --> 28:38.800 Perfect. 28:38.830 --> 28:42.850 And in this case let's simply return. 28:42.850 --> 28:47.260 So we are terminating the execution of this script finally. 28:47.290 --> 28:50.110 So after this if else statement. 28:50.110 --> 28:53.980 So this means that we have successfully executed the movement of the robot. 28:53.980 --> 29:00.310 So we have successfully moved the arm to the desired position and also the gripper to the desired position. 29:00.310 --> 29:05.460 So we can finally set the Result variable. 29:06.270 --> 29:08.280 So this one here that we have created. 29:08.370 --> 29:14.700 And as you remember the result variable just contains a boolean that is called success. 29:16.290 --> 29:19.080 And let's set this one equal to true. 29:19.110 --> 29:19.770 Perfect. 29:19.800 --> 29:22.560 Then we can take this variable here. 29:22.560 --> 29:25.410 So the goal handle that is here. 29:25.410 --> 29:28.110 And we can set this one as succeeded. 29:30.810 --> 29:34.320 And this returns the result. 29:34.350 --> 29:35.460 Perfect. 29:35.580 --> 29:37.350 And so actually that's it. 29:37.350 --> 29:39.750 So this will set the result. 29:39.750 --> 29:41.100 So the success to true. 29:41.100 --> 29:47.220 And this will return this result message to the actual client that actually requested the execution 29:47.220 --> 29:48.510 of the action server. 29:48.810 --> 29:50.700 So we can save this file. 29:50.700 --> 29:54.750 And with this we have completed the implementation of the task server. 29:54.750 --> 29:59.100 So now we are missing just to declare the dependencies that we have introduced. 29:59.100 --> 30:02.160 So all of this that we have introduced for this node. 30:02.160 --> 30:06.920 And so we need to declare them here within the Cmakelists.txt. 30:07.400 --> 30:14.690 So here we need to add the dependency from the rcl cpp package. 30:14.690 --> 30:19.640 Then if you haven't followed me in the Python laboratory lesson, you also need to add the dependency 30:19.640 --> 30:21.560 from the Arduino bot messages. 30:21.650 --> 30:30.620 Also, we need to add the dependency from the rcl cpp action and then the one from the cpp. 30:32.870 --> 30:33.950 Components. 30:33.980 --> 30:34.610 Perfect. 30:34.610 --> 30:37.280 So here the one for the Arduino bot messages. 30:37.280 --> 30:45.380 And also we need another dependency from the move it for us planning 30:47.270 --> 30:48.440 interface. 30:48.440 --> 30:49.340 Perfect. 30:49.370 --> 30:55.490 And so actually these are all the dependencies that we have declared and that we need in order to correctly 30:55.490 --> 30:57.500 compile the task server. 30:57.860 --> 31:04.040 Now in order to install and to compile the task server, we can copy actually the same instructions 31:04.040 --> 31:07.640 that we've used here within the Arduino bot CP examples. 31:07.640 --> 31:10.160 So within the CMake list of this package. 31:10.160 --> 31:12.320 So let's open it on one side. 31:12.320 --> 31:18.980 And as we can see here, all of these are all the instructions that we used in order to compile and 31:18.980 --> 31:21.470 install the simple action server. 31:21.560 --> 31:26.690 So we can copy them and we can paste it here and here. 31:26.690 --> 31:32.960 Actually the change that we need is to rename the name of the executable and the name of the library 31:32.960 --> 31:34.100 that we are creating. 31:34.100 --> 31:37.520 And so this is the task. 31:39.590 --> 31:40.490 Server. 31:40.580 --> 31:44.570 And then the name of the script is the task server CP. 31:44.960 --> 31:48.800 Then let's also add the dependencies for the task server. 31:48.890 --> 31:53.450 And also here let's also rename this one into task server. 31:53.510 --> 31:58.630 Also this one here it's the task server. 31:58.660 --> 31:59.290 Perfect. 31:59.320 --> 32:05.860 Here, let's add the dependencies for the task server that are the Arduino bot messages, our Clcp or 32:05.860 --> 32:08.590 Clcp actions components. 32:08.650 --> 32:11.710 And also we need to add this dependency here. 32:11.710 --> 32:14.410 So remove it Ros planning interface. 32:14.410 --> 32:15.880 So let's copy it. 32:15.940 --> 32:21.250 And then also let's register the component again for the task server. 32:21.250 --> 32:26.950 And the plugin is the Arduino bot remote. 32:28.150 --> 32:29.740 Again here we are. 32:30.670 --> 32:34.990 So the name of the class is the task server and the executable. 32:34.990 --> 32:39.280 So the node that actually we are going to execute let's call it. 32:41.470 --> 32:43.150 Task server node. 32:43.150 --> 32:43.990 Perfect. 32:43.990 --> 32:50.140 And so with these instructions here we have simply compiled our task server. 32:50.140 --> 32:52.150 And now we also need to install it. 32:52.540 --> 33:00.180 So if we open this one here we can see that here with these instructions we have installed the simple 33:00.180 --> 33:01.080 action server. 33:01.080 --> 33:05.340 So let's copy it and let's paste it here. 33:05.340 --> 33:08.310 So let's install the. 33:11.490 --> 33:12.780 Task server. 33:12.810 --> 33:17.040 We are just missing one thing that is to declare all these dependencies. 33:17.040 --> 33:22.860 So all the ones that we have declared in this list also in the package dot XML. 33:22.920 --> 33:36.630 So here let's add a new dependency from the RCL cpp, then another one from the clcp action and from 33:36.630 --> 33:37.680 the Clcp. 33:40.170 --> 33:41.070 Components. 33:41.070 --> 33:41.730 Perfect. 33:41.730 --> 33:45.030 Then we need the dependency from the Arduino bot messages. 33:45.120 --> 33:50.550 And also we need the dependency from the move it Ros. 33:53.070 --> 33:54.390 Planning. 33:56.400 --> 33:57.510 Interface. 33:57.540 --> 33:58.620 That's it. 33:58.620 --> 33:59.430 Perfect. 33:59.430 --> 34:03.150 So let's conclude this lesson by saving this file. 34:03.150 --> 34:08.400 And let's build our workspace in order actually to check that we have not made any mistake. 34:08.730 --> 34:10.500 So let's open a new terminal. 34:10.530 --> 34:13.950 Let's go to the workspace and let's compile it. 34:13.950 --> 34:15.210 So let's build it. 34:15.210 --> 34:21.930 And so this is compiling the new node that we have created within the Arduino bot remote package. 34:26.130 --> 34:26.940 Perfect. 34:26.940 --> 34:28.320 So there are no errors. 34:28.320 --> 34:31.050 This means that the compilation was successfully. 34:31.050 --> 34:35.040 And so we can start already executing our task server node. 34:35.040 --> 34:37.740 And in the following lesson we are going to launch it. 34:37.740 --> 34:44.400 And so we are going to see how it works and how we can use this task server in order to provide an interface 34:44.400 --> 34:50.580 that will allow us to move the robot to a predefined location, so to a predefined position depending 34:50.580 --> 34:52.950 on the task number that we set.