WEBVTT 00:00.080 --> 00:05.990 To define the behavior of all the functions of the Arduino board interface class that you have declared 00:05.990 --> 00:07.310 in the previous lesson. 00:07.310 --> 00:14.720 We need to create a new script, so a new Cplusplus file inside the source folder, this time of the 00:14.720 --> 00:16.850 Arduino bot controller package. 00:16.850 --> 00:24.800 And let's call this one Arduino bot interface dot cpp. 00:25.100 --> 00:30.770 Since we are going to use this script to define the components of the Arduino bot interface class, 00:30.980 --> 00:33.680 let's start by including the header file. 00:33.680 --> 00:39.890 So the file Arduino bot interface dot HPP that we have created in the previous lesson. 00:40.580 --> 00:48.710 So let's include from the Arduino bot controller the Arduino bot interface dot dot hpp. 00:49.550 --> 00:53.570 Now let's use the same namespace that we've used in the previous lesson. 00:54.260 --> 01:03.270 So the namespace is the Arduino bot controller As the name of the package and within this namespace. 01:03.270 --> 01:09.870 Let's start by defining the functions of the Arduino board interface class, and let's start by its 01:09.870 --> 01:10.830 constructor. 01:11.670 --> 01:19.080 To define a function in a cpp file that we have declared in a header file, we need to specify the name 01:19.080 --> 01:25.560 of the class that is the Arduino bot interface, and then the name of the function that is also in this 01:25.560 --> 01:28.740 case the Arduino bot interface, as this is the constructor. 01:28.740 --> 01:33.420 So let's define this function and let's leave it empty for the moment. 01:33.420 --> 01:36.540 And the next let's define also the destructor. 01:37.290 --> 01:44.010 So the Arduino bot interface and the function tilde Arduino bot interface. 01:44.010 --> 01:46.110 So this one is the destructor. 01:46.740 --> 01:47.460 This one. 01:47.460 --> 01:53.040 So the destructor is called when the object of the Arduino bot interface class. 01:53.040 --> 01:55.950 And thus the Arduino interface is destroyed. 01:56.610 --> 02:01.890 In this function, we want to check whether the serial communication with the Arduino is still open 02:01.890 --> 02:03.060 is still active. 02:03.090 --> 02:10.380 So let's check if the variable that we declared in the previous lesson and that is called Arduino is 02:10.380 --> 02:11.610 still open. 02:11.760 --> 02:13.020 And in this case. 02:13.020 --> 02:16.380 So if this is still open let's try to close it. 02:16.410 --> 02:22.170 So let's try to call the function close. 02:22.380 --> 02:24.480 And if something goes wrong. 02:24.510 --> 02:30.930 So let's add a catch that is catching any exception. 02:31.020 --> 02:33.600 So here we add the semicolon. 02:33.600 --> 02:35.400 And here in the catch. 02:35.400 --> 02:41.010 So if an exception is thrown by this function let's print an error message. 02:43.800 --> 02:47.130 With fatal stream. 02:47.550 --> 02:49.320 Let's get the logger. 02:55.710 --> 03:00.490 For the Arduino bot interface. 03:00.490 --> 03:04.210 So this is the scope and let's print the message. 03:05.290 --> 03:14.500 Something went wrong while closing the connection. 03:14.890 --> 03:19.450 And then let's also print with the port. 03:19.780 --> 03:22.720 And then let's print the variable port. 03:23.230 --> 03:24.610 So this variable here. 03:24.640 --> 03:30.520 Next let's move on to define the behavior of the on init function. 03:30.520 --> 03:34.840 So let's go back to the Arduino bot interface dot http. 03:35.380 --> 03:37.660 And let's copy this one. 03:37.660 --> 03:44.500 So the declaration of the on init function and let's copy it here in order to define it. 03:44.500 --> 03:48.820 Then let's remove the column and let's add the name of the class. 03:48.820 --> 03:53.200 So this one here and let's put it here. 03:53.230 --> 03:58.030 Let's also remove the virtual statement and the override one. 03:58.090 --> 04:00.130 And now within this function. 04:00.130 --> 04:06.820 So within the UN init function let's first call the on init function of the base class. 04:06.820 --> 04:13.330 So the base class is in the hardware interface and is the system interface. 04:13.330 --> 04:19.630 And so let's call the on init function of this class here. 04:19.630 --> 04:24.250 And this function receives as input the hardware info object. 04:24.250 --> 04:30.940 So this variable here that the on init function of the Arduino board interface received as input. 04:31.570 --> 04:37.570 And also it returns an object of type callback return which we store in a new variable. 04:37.570 --> 04:46.270 So here let's create a new variable of type callback return that we call result, and that takes the 04:46.270 --> 04:52.330 result of the init function on init function that we executed on the system interface. 04:53.080 --> 04:57.440 And now let's check whether or not the initialization of the base class. 04:57.440 --> 05:02.030 So with this one with the system interface was successful or not. 05:02.030 --> 05:16.700 So if the result is not of type callback return success, then in this case let's already return the 05:16.700 --> 05:17.270 result. 05:17.270 --> 05:23.900 So if for example this one failed, then let's make also the on init function of the Arduino bot interface 05:23.900 --> 05:25.670 fail otherwise. 05:25.670 --> 05:32.810 So here, if the base class was successfully initialized, we can proceed to initialize also the serial 05:32.810 --> 05:34.640 communication with the Arduino. 05:35.180 --> 05:40.400 First we need to know which is the port to which the Arduino board is connected. 05:40.430 --> 05:45.320 And so we do this by assigning a value of the port variable. 05:45.710 --> 05:56.130 So let's try to assign a value to the variable port of the Arduino board interface class, and this 05:56.130 --> 06:06.660 value is given from the info, and then from this one lets access to the hardware parameters and then 06:06.660 --> 06:10.890 lets access to the element that is called port. 06:11.550 --> 06:14.610 As you may have noticed, we are using this object here. 06:14.610 --> 06:16.110 So the info object. 06:16.110 --> 06:20.280 But if you remember we have not declared such an object here. 06:20.310 --> 06:25.470 This means that basically this is an object that we are inheriting from the base class. 06:25.470 --> 06:32.070 So it is defined in the system interface and contains the parameter with which the hardware interface 06:32.100 --> 06:33.840 is launched is started. 06:33.840 --> 06:38.880 And so here we are checking whether the parameter port exists. 06:39.840 --> 06:40.710 If not. 06:40.740 --> 06:43.770 So catch. 06:43.890 --> 06:55.080 So if this doesn't exist it will throw an exception of type const out of range. 06:55.500 --> 07:01.200 And so in this case, let's print an error message in the terminal. 07:02.280 --> 07:06.150 Again with klccp fatal. 07:06.870 --> 07:14.640 Then again let's get the logger with the scope. 07:16.290 --> 07:18.870 Arduino bot interface. 07:18.990 --> 07:20.910 And let's print the message. 07:20.940 --> 07:26.370 No serial port provided. 07:27.180 --> 07:30.870 And so in this case we are going to abort. 07:31.140 --> 07:40.350 And so let's return an object of type callback return of type failure. 07:40.590 --> 07:42.510 If everything went well. 07:42.600 --> 07:48.600 So if we reached this point of the function, this point of the script, then it means that the port 07:48.600 --> 07:49.440 parameter. 07:49.440 --> 07:51.120 So this one exists. 07:51.120 --> 07:57.270 And so basically it means that we started the hardware interface by setting also a parameter for the 07:57.270 --> 08:00.270 port for the communication with the Arduino. 08:01.290 --> 08:07.650 Now we can complete the on init function by also initializing the three decimal number vectors that 08:07.650 --> 08:13.050 we have declared, and so these three vectors the position command, the previous position command, 08:13.050 --> 08:14.670 and the position state. 08:15.300 --> 08:23.940 Let's start with the position commands, and let's reserve a space for this one. 08:23.940 --> 08:30.360 Since we already know its size and its size is equal to the size, so to the number of the joints of 08:30.360 --> 08:31.200 the robot. 08:31.200 --> 08:35.130 And so we can take this information still from the object info. 08:35.280 --> 08:38.760 Then let's take the vector that is called joints. 08:39.690 --> 08:43.800 And from this one let's use the size function. 08:44.070 --> 08:52.080 Let's do the same also for the position states. 08:52.390 --> 08:57.220 So let's reserve our space for this vector that has the same size. 08:58.030 --> 09:03.160 And finally let's do the same also for the previous position command. 09:05.290 --> 09:10.990 So let's reserve a space that is the same size. 09:11.860 --> 09:13.180 So this one here. 09:13.180 --> 09:19.360 And finally if we reached this point of this function let's return a success. 09:19.510 --> 09:24.700 So a callback return of type success. 09:25.990 --> 09:29.020 With this the on init function is completed. 09:29.020 --> 09:34.150 And now let's define the function export state interface. 09:34.240 --> 09:38.560 So once again let's go back to the declaration of this function. 09:38.560 --> 09:40.450 And let's copy it. 09:40.450 --> 09:46.720 So let's copy it declaration this one and let's paste it here. 09:47.980 --> 09:54.170 Let's remove the override And also the virtual. 09:54.230 --> 09:59.030 And let's add here the name of the class to which it belongs to. 09:59.630 --> 10:01.460 So the Arduino bot interface. 10:01.460 --> 10:09.140 The purpose of this function is to create a vector of objects of type state interface that are still 10:09.140 --> 10:11.690 declared in the hardware interface library. 10:11.930 --> 10:13.760 So let's create a vector. 10:15.440 --> 10:19.160 And this is from the hardware interface library. 10:19.160 --> 10:22.310 This is a vector of state interfaces. 10:22.310 --> 10:27.470 And let's call this one state interfaces. 10:27.860 --> 10:34.040 And we can use this list of interfaces to communicate the current status of the joints and so of the 10:34.040 --> 10:38.750 motors of the robot to other nodes, and so to other ros2 applications. 10:39.410 --> 10:45.440 As you might recall in the control section of this course, when we configured the Ros2 control library 10:45.440 --> 10:51.770 for our robot, we mentioned that each joint communicates its state in the form of a position. 10:51.770 --> 10:54.200 So using a position interface. 10:54.200 --> 10:56.750 And so this is the same interface that we have to use. 10:56.780 --> 10:58.580 Now in this case. 10:58.910 --> 11:01.250 So let's loop through each joint. 11:01.490 --> 11:07.640 So for size t equal to zero. 11:07.640 --> 11:10.460 And then this goes to all the joints. 11:10.850 --> 11:16.190 So the size of the joints is here in the info object. 11:16.400 --> 11:18.920 And then joint size. 11:19.130 --> 11:22.100 And then let's increment this index. 11:22.760 --> 11:27.200 And in this loop let's add a new element to our state interface vector. 11:27.350 --> 11:34.880 So let's use the function in place back to add a new element to construct a new element. 11:34.940 --> 11:41.750 And so this one let's add a new object from the hardware interface. 11:42.350 --> 11:45.200 And of type state interface. 11:45.320 --> 11:46.940 The constructor of this class. 11:46.940 --> 11:53.440 So the constructor of the state interface class requires to be passed as input to the joint name, and 11:53.440 --> 11:58.660 so we can get this one from the info object still. 11:58.690 --> 12:00.580 Then from the joints. 12:00.820 --> 12:04.360 Then let's access to the joint at the index I. 12:04.660 --> 12:06.910 And let's check the name. 12:06.910 --> 12:09.550 So let's get the name of this joint. 12:09.610 --> 12:16.720 And then to the constructor of the state interface class, we also need to pass the type of the interface 12:16.720 --> 12:18.100 that we want to use. 12:18.850 --> 12:22.930 So the type of the interfaces is defined in another library. 12:23.980 --> 12:26.980 So let's include from the hardware. 12:29.710 --> 12:33.040 Interface from the types module. 12:33.070 --> 12:38.260 Let's import the hardware interface type values. 12:38.560 --> 12:41.260 And now let's use this one to define that. 12:41.260 --> 12:46.690 Here the type of the state interface is a position interface. 12:46.690 --> 12:58.530 And so let's use the H w if position, and finally it stores the position of the joint into a new variable. 12:58.530 --> 13:05.430 So let's say that we want to store it within the position states vector. 13:05.430 --> 13:10.560 And so we are defining the position for the joint at the index I. 13:11.010 --> 13:16.380 And finally once we have finished the loop through all the joints. 13:16.380 --> 13:21.600 So once we have added all the joints that make up our robot into the state interface. 13:21.600 --> 13:30.090 So the state interface that is a position interface, let's return the state interfaces vector. 13:30.210 --> 13:38.130 Now let's define the other function that we have declared of type export command interface. 13:38.550 --> 13:43.710 So if we go here let's define the behavior of this second function. 13:43.710 --> 13:50.500 Here again let's remove the override, and let's remove the virtual. 13:51.250 --> 13:56.530 And let's instead use the name of the class that is the Arduino bot interface. 13:56.710 --> 13:59.920 So let's add it here before the name of the function. 14:00.070 --> 14:07.300 If the purpose of this function so of the export state interface was to communicate the interface that 14:07.300 --> 14:10.960 we need to use for knowing the state of each robot's joint. 14:11.290 --> 14:17.110 The purpose of this one, so of the export command interface, is to communicate the interface that 14:17.110 --> 14:21.940 we have to use in order to send movement commands for each joint. 14:21.940 --> 14:28.660 So this one is used in order to receive the type of the interface used to know the state of each joint. 14:28.660 --> 14:34.570 And this one is that is used to know the type of interface that we need to use to set. 14:34.570 --> 14:37.330 So to send the commands to each of the joints. 14:37.480 --> 14:42.940 Once again, as you might recall from the theoretical lesson of the control section, we decided to 14:42.970 --> 14:50.140 use the position interface also to send comments, so also to write comments to the robot's joint. 14:50.770 --> 14:52.960 So let's start by creating a new vector. 14:55.000 --> 15:03.550 Of type hardware interface command interface. 15:03.550 --> 15:08.650 And let's call this one command interfaces. 15:08.650 --> 15:12.010 And as we did for the export state interface. 15:12.010 --> 15:16.300 So let's loop through all the joints like this. 15:16.300 --> 15:21.370 And this time let's add a new element to the command interface. 15:21.610 --> 15:25.840 So let's do command interface in place back to create a new element. 15:25.840 --> 15:32.950 And now the new element that we want to add to the vector is of type command interface. 15:32.950 --> 15:35.740 So let's change this one to be command interface. 15:35.920 --> 15:36.880 Still as input. 15:36.910 --> 15:43.180 This class also requires the name of the joint so that still we take from the info object. 15:43.180 --> 15:49.580 Then it requires the type of the interface that we want to use in order to send commands to this joint. 15:49.580 --> 15:52.280 And still this is a position interface. 15:52.280 --> 15:57.290 And finally, it takes the command which we want to send to our joints. 15:57.440 --> 16:02.690 And so this is in the vector position commands. 16:02.690 --> 16:06.200 And we want to send the item at the index I. 16:07.220 --> 16:14.450 And finally also in this case let's return the command interfaces. 16:15.560 --> 16:22.370 And now let's move on to define the on activate and on the activate functions of the Arduino bot interface 16:22.370 --> 16:26.060 class, which basically implement the node lifecycle. 16:26.090 --> 16:29.570 So let's go back here where we have declared those functions. 16:29.750 --> 16:34.850 And let's copy their declaration and let's define them. 16:35.090 --> 16:39.800 So let's define the first one that is the on activate. 16:41.300 --> 16:43.580 And then let's define the second one. 16:43.880 --> 16:48.630 So this one that is the on deactivate. 16:51.300 --> 16:51.780 Okay. 16:51.780 --> 16:53.910 And also let's add the name of the class. 16:53.940 --> 17:03.330 So this one here to indicate that both the on activate and on deactivate functions belong to the Arduino 17:03.330 --> 17:04.800 bot interface class. 17:05.700 --> 17:07.260 In the activate function. 17:07.260 --> 17:11.760 So here let's start by printing an informative message into the terminal. 17:15.930 --> 17:22.680 And let's use the Arduino bot interface scope. 17:22.980 --> 17:30.720 And let's print the message starting the robot hardware. 17:30.900 --> 17:32.340 So like this. 17:32.580 --> 17:37.950 Then let's initialize the three vectors of decimal numbers that we have declared. 17:37.950 --> 17:40.350 So these three vectors here. 17:40.350 --> 17:44.940 And let's start with the Position commands. 17:45.360 --> 17:47.880 And let's set this 1 to 0. 17:48.090 --> 17:50.460 So this has four elements. 17:50.460 --> 17:52.500 And let's set all of them to zero. 17:57.090 --> 18:02.010 Then let's also initialize the previous position commands. 18:02.010 --> 18:05.340 And let's set also for this one the initial state. 18:05.970 --> 18:15.780 So to be zero and then the position states to be as well a vector with four elements that we set to 18:15.810 --> 18:16.410 zero. 18:16.500 --> 18:20.010 With this we are assuming that the initial position. 18:20.010 --> 18:23.400 And so the initial command of all the robots are zero. 18:23.430 --> 18:30.060 At this point, let's try to open the serial communication with the Arduino using the lib serial. 18:30.090 --> 18:33.390 So let's try to open. 18:33.390 --> 18:38.760 So to use the Arduino object to open the variable port. 18:39.450 --> 18:44.710 And so we are trying to open the communication with the Arduino in the requested port. 18:44.710 --> 18:49.150 So in the port that we have indicated here with the parameter port. 18:50.050 --> 19:01.960 And also let's try to open this communication with about rate that is from lib serial from baud rate. 19:01.960 --> 19:09.100 We want to use the bout 115 200 and if something goes wrong. 19:10.840 --> 19:15.700 So if anything actually goes wrong here in this initialization. 19:16.090 --> 19:22.150 And so if something goes wrong while trying to opening the serial communication at this port with this 19:22.150 --> 19:25.090 baud rate, let's print an error message. 19:29.800 --> 19:38.440 As always, let's take the Arduino bot interface scope and let's print the message. 19:39.550 --> 19:44.950 Something Went wrong while. 19:47.140 --> 19:51.040 Interacting with the port. 19:51.040 --> 19:56.020 And then let's print also the value of the port that we were unable to open. 19:56.920 --> 20:05.830 And in this case, let's return a callback return message of type failure otherwise. 20:05.830 --> 20:08.110 So if everything went well. 20:08.110 --> 20:13.810 So here let's print an informative message so we can copy this one actually. 20:15.670 --> 20:17.530 And let's print the message. 20:18.400 --> 20:23.500 So here our order started. 20:23.530 --> 20:29.170 And so ready to take commands. 20:29.170 --> 20:38.800 And in this case let's return an object of type callback return success. 20:38.800 --> 20:45.590 So to indicate that everything went well and that we were able to initialize to open the serial communication 20:45.590 --> 20:51.950 and also to initialize all the variables that are needed for the on deactivate function. 20:51.950 --> 20:57.260 So for this one, let's start by printing an informative message into the terminal. 20:57.320 --> 20:59.720 So we can actually copy this one. 20:59.960 --> 21:01.850 And let's paste it here. 21:02.090 --> 21:03.050 And here. 21:03.050 --> 21:09.380 The message that we are going to print is just stopping the robot hardware. 21:09.530 --> 21:15.140 And now let's check if the serial communication is open. 21:15.710 --> 21:18.650 So whether the serial communication is open. 21:18.650 --> 21:21.170 And in this case let's try to close it. 21:21.170 --> 21:29.210 So let's try to use the Arduino close function in order to close this communication. 21:29.210 --> 21:35.780 And so to leave the serial port free for other application to use it in case something goes wrong. 21:37.250 --> 21:41.090 So in case actually Anything goes wrong. 21:41.090 --> 21:45.680 Let's print an error message in the terminal so we can copy this one. 21:46.520 --> 21:48.380 And we can paste it here. 21:50.540 --> 21:59.240 And this message says something went wrong while closing the connection with the port. 21:59.270 --> 22:01.850 And then followed by the name of the port. 22:01.850 --> 22:09.440 And if we were able here to correctly close the communication with the hardware, then let's print an 22:09.470 --> 22:14.840 informative message and let's also return an object of type callback return success. 22:14.840 --> 22:16.670 And here let's print the message. 22:16.670 --> 22:22.460 So hardware stopped here there is a typo. 22:22.670 --> 22:24.380 Is hardware started? 22:25.160 --> 22:31.340 Next let's define the last two functions that we declared in the Arduino interface class. 22:31.790 --> 22:33.260 So let's go back here. 22:33.260 --> 22:36.830 And these are the read and the write function. 22:36.950 --> 22:39.050 So let's copy the declaration. 22:39.960 --> 22:42.300 and let's paste it here. 22:43.710 --> 22:52.770 So let's start by defining the behavior of the read function and then the one here of the write function. 22:54.900 --> 22:57.000 And let's use the name of the class. 22:57.000 --> 22:59.370 So Arduino bot interface. 23:00.540 --> 23:07.410 In order to define that both of these functions belong to the Arduino bot interface class. 23:07.980 --> 23:09.600 The read function. 23:09.600 --> 23:15.540 So this one, as the name suggests, is responsible for reading the current position of the robot's 23:15.540 --> 23:18.750 joint and make it available in Ros2. 23:19.290 --> 23:26.280 Anyway, since we are using very cheap servo motors for our simple robot, they don't provide any information 23:26.280 --> 23:28.170 about their current position. 23:28.380 --> 23:34.740 Therefore, let's just assume that every time we send a new position command to the robot, it is just 23:34.770 --> 23:42.600 able to read it perfectly and thus we can simply set here that the current position. 23:42.600 --> 23:49.710 So the current state of each servo motor is equal to the current command that we have sent to the robot. 23:49.740 --> 23:53.100 So obviously this is an important assumption. 23:53.100 --> 23:59.640 But given that the forces and the acceleration that are involved are very small, and also considering 23:59.640 --> 24:05.520 that we are using a very cheap hardware and electronics, this is an assumption that we can accept. 24:06.210 --> 24:18.780 And so now we can already return an object of type hardware interface, return type and return. 24:18.810 --> 24:19.440 Okay. 24:19.440 --> 24:24.870 So to indicate that there are no errors while the read function. 24:24.870 --> 24:30.780 So this one as I said is responsible for reading the current position of the servo motors. 24:30.780 --> 24:37.420 So the current position of the joints, the write function as the name says, has the responsibility 24:37.420 --> 24:38.350 to write. 24:38.350 --> 24:41.740 So to send position comments to the joints. 24:41.740 --> 24:46.780 So to the Arduino so that it can activate it can move the servo motors accordingly. 24:47.410 --> 24:53.470 So here within this function, let's first check that the new command that we received so that was received 24:53.470 --> 24:57.370 by the robot is not equal to the last command that was received. 24:57.370 --> 25:01.840 And so that would mean that the robot is already in the desired position. 25:01.960 --> 25:09.580 So if for example, the position command that we have just received is equal to the previous position 25:09.580 --> 25:10.000 command. 25:10.000 --> 25:15.700 So if we received a command that is equal to the last one, let's just terminate the execution. 25:15.700 --> 25:24.010 So let's just return another interface return type okay. 25:24.130 --> 25:25.960 And this means that we are already there. 25:25.960 --> 25:32.890 So this means that we are already aware of this command and we don't need to send it again otherwise. 25:33.160 --> 25:36.380 So here if we received a new command. 25:36.380 --> 25:39.800 We want to send it to the Arduino through the serial port. 25:40.580 --> 25:45.440 To do this, we are going to use a very simple communication protocol, a very simple convention. 25:45.440 --> 25:52.700 Let's say we will transmit a single string, a single message so that the communication is lightweight 25:52.700 --> 25:53.660 and fast. 25:54.260 --> 25:58.370 This message will contain four components separated by commas. 25:59.000 --> 26:04.670 The first component is represented by the letter B and indicates the desired position for the motor 26:04.670 --> 26:06.530 that controls the robot base. 26:07.280 --> 26:13.730 The second component, indicated by the letter S, indicates that the desired position for the motor 26:13.730 --> 26:15.680 that controls the robot shoulder. 26:16.370 --> 26:22.310 The third, indicated by the letter E, indicates the desired position for the motor that controls the 26:22.310 --> 26:23.030 elbow. 26:23.180 --> 26:29.630 And finally, the last component that is indicated by the letter G indicates the desired position for 26:29.630 --> 26:32.270 the motor that controls the gripper. 26:33.170 --> 26:38.540 Back in the write function, so back here let's create this communication protocol. 26:38.570 --> 26:45.290 So let's implement this communication protocol by creating a new string called message. 26:45.290 --> 26:49.310 And so this will be the message that we are going to send through the serial port. 26:49.730 --> 26:55.190 And considering the way that we will connect the servo motor to the robot link, and also because in 26:55.190 --> 27:00.620 Ros2 the angles are expressed in radians, while in Arduino they are expressed in degree. 27:00.650 --> 27:03.590 The value of the base joint of the robot. 27:03.620 --> 27:10.130 So the one that we want to send to the base joint of the robot is equal to the value that we received 27:10.130 --> 27:21.710 in the position command for the first joint plus pi that we can access with m pi divided by two. 27:21.770 --> 27:24.020 And then we need to multiply all of these. 27:24.020 --> 27:27.500 So let's wrap with some parentheses. 27:27.620 --> 27:31.730 And let's multiply all of these by 180. 27:31.760 --> 27:33.920 And let's also divide all of these. 27:33.920 --> 27:36.300 So let's wrap with more parentheses. 27:36.480 --> 27:39.840 Let's divide all of this again by pi. 27:39.870 --> 27:45.960 So with the macro and pi this equation could return a decimal number. 27:45.960 --> 27:48.930 But anyway let's cast it to an integer. 27:49.110 --> 27:58.890 So let's perform a static cast to an integer for all this number here, since anyway, the command that 27:58.890 --> 28:04.920 we are going to send to the motors through the Arduino can only be an integer, so can only be an angle 28:04.920 --> 28:05.910 in degrees. 28:06.480 --> 28:08.910 Let's append to our message. 28:08.940 --> 28:14.580 So to the message, let's append the letter b. 28:15.060 --> 28:20.190 And then as we always want to send a three digit number to the Arduino. 28:20.220 --> 28:23.070 Let's also use a new function. 28:23.190 --> 28:35.970 So here let's append the result of a new function that we are going to call compensate Zeros like this. 28:35.970 --> 28:40.320 And this function here takes as input the base angle. 28:40.320 --> 28:43.050 So this integer here that we have created. 28:43.110 --> 28:48.510 So let's copy it and let's paste it within the compensate zeros. 28:48.510 --> 28:55.380 And basically this function is going to add as many zeros as needed in order to make this number here 28:55.410 --> 28:57.780 a three digit number. 28:57.780 --> 29:03.060 And so this will eventually add some zeros at the beginning of this number here. 29:03.060 --> 29:06.360 And also after this one here let's also append. 29:09.270 --> 29:10.590 The actual number. 29:10.590 --> 29:13.590 So the base so this angle here. 29:13.590 --> 29:20.730 And first let's convert it into a string with the STD string. 29:20.760 --> 29:23.880 And so let's convert this number here. 29:23.880 --> 29:26.580 So the base number into a string. 29:26.580 --> 29:30.360 So here we can use the function to string perfect. 29:30.360 --> 29:37.090 And so this function here will we'll convert this integer into a string that we can append to this message 29:37.090 --> 29:37.690 here. 29:37.690 --> 29:40.060 And then in order to conclude. 29:40.060 --> 29:47.200 So in order to finish the message that is set for the motor that actuates the base of the arm, let's 29:47.200 --> 29:49.930 also append a final comma. 29:50.800 --> 29:54.100 And so this concludes the message for the base. 29:54.100 --> 29:59.260 And then we're also going to add the message for the other remaining joints of the robot. 29:59.260 --> 30:02.260 But before doing so let's define this function here. 30:02.260 --> 30:03.820 So the compensate zeros. 30:03.820 --> 30:05.680 And actually we can do it here. 30:05.680 --> 30:07.300 So within this same file. 30:07.300 --> 30:11.200 And this is a very straightforward and simple function. 30:11.200 --> 30:15.100 So let's define it the compensated zeros. 30:15.100 --> 30:20.800 And this takes as input an integer that is the value that we want to check. 30:20.800 --> 30:23.920 So for which we want to generate the zeros. 30:23.920 --> 30:30.310 And this returns a string that will contain as many zeros as needed. 30:30.310 --> 30:31.870 So let's define it. 30:31.870 --> 30:40.750 And so here let's create a new string that is called compensate zero. 30:41.020 --> 30:44.740 And by default let's initialize this one with an empty string. 30:44.740 --> 30:47.260 And now let's check this value here. 30:47.260 --> 30:53.830 So the command that we are going to send to each of the joints that actuate the motors of our robot. 30:53.830 --> 30:59.170 So here let's check if the value is smaller than ten. 30:59.170 --> 31:04.030 So this means that we have a value between 0 and 9. 31:04.030 --> 31:10.180 And so in this case in order to make it a three digit number, we need to add two zeros in the front 31:10.180 --> 31:11.350 of this number. 31:11.440 --> 31:14.680 So let's set this string here. 31:14.680 --> 31:18.700 So this is the compensate zero. 31:18.700 --> 31:23.560 So this string here let's set it equal to zero zero. 31:23.560 --> 31:26.260 So we are adding two zeros at the front. 31:26.380 --> 31:34.430 Otherwise else if so if the value is smaller than 100. 31:34.460 --> 31:39.170 So this means that we are dealing with a number between 10 and 99. 31:39.170 --> 31:42.110 In this case we just want to add one zero. 31:42.110 --> 31:47.420 So let's set the compensate zeros just to one zero. 31:47.420 --> 31:49.190 Perfect and otherwise. 31:49.820 --> 31:54.260 So this means that we are dealing with a number that already has three digits. 31:54.260 --> 31:57.830 And so we can simply set the compensate zeros. 31:57.830 --> 32:01.940 So this string here we can simply set it to an empty string. 32:01.940 --> 32:04.430 So it is already a number with three digits. 32:04.430 --> 32:08.690 And so we don't need to append anything at the beginning of this number. 32:08.690 --> 32:12.560 And we simply can return this string here. 32:12.560 --> 32:16.160 So the compensate zeros string perfect. 32:16.160 --> 32:25.100 So now we can go back in the right function where we were using the compensated zeros before the actual 32:25.100 --> 32:25.580 command. 32:25.580 --> 32:30.000 In order to eventually add any other zero here. 32:30.060 --> 32:33.180 Now let's continue also with the other joints. 32:33.210 --> 32:34.890 Starting with the shoulder. 32:36.540 --> 32:38.040 And this is equal to. 32:38.070 --> 32:43.590 So the value that we want to send to the shoulder is equal to 180 minus. 32:43.710 --> 32:47.430 Let's cast with the static cast. 32:47.460 --> 32:53.310 So let's cast to an integer because the comment that we want to send to the Arduino and so to the servo 32:53.340 --> 32:55.080 motor has to be an integer. 32:55.080 --> 32:58.380 And so let's access again to the position command. 32:58.380 --> 33:03.510 And this time we want to access to the position command at the position one. 33:03.600 --> 33:12.420 And here we want to add the pi divided by two. 33:12.510 --> 33:13.440 Perfect. 33:13.440 --> 33:17.970 And so we want to multiply then all of these by 180. 33:17.970 --> 33:20.460 So here we need a parentheses. 33:20.730 --> 33:22.890 And also here. 33:22.890 --> 33:29.670 So after this one we need also another parentheses in order to multiply all of these by 180. 33:29.700 --> 33:30.510 Perfect. 33:30.510 --> 33:33.150 And then we can add another parentheses. 33:33.150 --> 33:38.310 So let's add another here at the beginning and another here after the 180. 33:38.340 --> 33:41.550 In order to divide all of these by pie. 33:42.450 --> 33:43.350 Perfect. 33:43.380 --> 33:45.870 Now again let's append this message. 33:45.870 --> 33:47.700 So this is for the shoulder. 33:47.700 --> 33:51.330 So let's append to the message the letter s. 33:51.360 --> 33:54.270 Then let's append this one here. 33:54.270 --> 33:57.930 So the compensate zeros for the shoulder. 33:57.930 --> 34:02.730 So this will take this value here and will eventually add any zeros in the front. 34:02.760 --> 34:04.920 Then we can also append. 34:05.550 --> 34:07.110 So this value here. 34:07.110 --> 34:10.170 So the value again of the shoulder. 34:10.170 --> 34:13.260 So this integer here converted into a string. 34:13.260 --> 34:19.440 And finally in order to complete this message we want to append a final comma. 34:19.440 --> 34:24.210 And so this means that we are closing the command for the joint that actuate. 34:24.210 --> 34:27.880 So for the motor that actuates the joint of the shoulder. 34:27.970 --> 34:30.370 Now let's also do the same for the elbow. 34:32.020 --> 34:32.920 Perfect. 34:32.920 --> 34:37.630 And so this is again let's access to the position commands. 34:38.170 --> 34:41.890 And let's access to the position command at the position two. 34:41.920 --> 34:47.140 And to this one we want to add pi divided by two. 34:47.170 --> 34:50.380 And we want to multiply all of these by 180. 34:50.770 --> 34:56.920 So let's use the parentheses here and let's multiply by 180. 34:56.950 --> 34:57.760 Perfect. 34:57.760 --> 35:00.250 And let's divide all of these by pi. 35:00.760 --> 35:02.770 So again let's use the parentheses. 35:02.770 --> 35:05.830 And let's divide by pi. 35:05.860 --> 35:06.670 Perfect. 35:06.670 --> 35:10.930 And also we want to cast all of these again using this function here. 35:10.930 --> 35:16.270 So the static cast in order to cast all of these into an integer. 35:16.300 --> 35:22.510 Now again we want to append this message to the string that we will send to the Arduino. 35:22.510 --> 35:28.630 So let's append the letter e and then also let's compensate the zeros. 35:28.630 --> 35:36.040 So this will check the elbow and will add any zeros in the front if needed. 35:36.040 --> 35:37.510 Then let's also append. 35:37.540 --> 35:39.670 So we can copy this line here. 35:39.670 --> 35:41.320 And we can append it here. 35:41.320 --> 35:47.860 So this will convert the elbow so this integer into a string and will append it to our message. 35:47.860 --> 35:50.260 And finally let's also append a comma. 35:50.290 --> 35:54.250 And so this will close the command that we are sending for the joint. 35:54.250 --> 35:57.760 So for the motor that actuates the elbow of our robot. 35:57.820 --> 36:02.200 Then let's also finally create the command for the gripper. 36:02.890 --> 36:10.450 And so again we can access to the position commands vector this time at the position three. 36:10.450 --> 36:14.440 And actually we want to use the minus sign. 36:14.440 --> 36:19.330 And we want to multiply all of these by 180. 36:19.330 --> 36:23.170 And we want to divide everything by pi divided by two. 36:23.200 --> 36:31.670 So let's use the parentheses and let's divide all of these by pi divided by two. 36:31.790 --> 36:32.600 Perfect. 36:32.600 --> 36:36.380 And also in this case, we want to cast everything to an integer. 36:36.470 --> 36:42.860 So let's use the static cast in order to cast the result of this operation into an integer. 36:42.890 --> 36:43.610 Perfect. 36:43.640 --> 36:50.090 Now, in order to append this comment here to the string that we are creating, let's append the letter 36:50.120 --> 36:50.630 g. 36:51.020 --> 36:57.440 Then let's append here the compensate zeros that will check this command here. 36:57.440 --> 37:03.260 So the command for the gripper and that will eventually add any zeros in the front of this message. 37:03.350 --> 37:08.840 And then let's also append the actual command that will be for the joint of the gripper. 37:08.840 --> 37:12.920 So this command here that is converted into a string. 37:12.920 --> 37:16.130 And finally let's append a final comma. 37:16.160 --> 37:19.160 So this one here and let's paste it. 37:19.160 --> 37:20.120 Perfect. 37:20.210 --> 37:22.790 At this point the message is completed. 37:22.790 --> 37:27.650 So this message here now contains a value for the joint. 37:27.650 --> 37:33.800 So for the motor that actuates the base, the shoulder, the elbow and also the gripper. 37:33.800 --> 37:42.230 And all these three commands are basically a three digit commands that goes from 0 to 180 degrees. 37:42.260 --> 37:44.750 Now at this point the message is ready. 37:44.750 --> 37:47.720 So we can try to send it to the Arduino. 37:47.960 --> 37:52.310 So let's try to use the Arduino. 37:52.310 --> 37:56.240 And on this one let's try to use the right function. 37:56.420 --> 38:00.560 And let's try to write this message here that we have created. 38:00.590 --> 38:05.180 Let's also catch any exception that may occur. 38:05.750 --> 38:11.870 And in this case so if any of the exception of course, let's print an error message in the terminal 38:11.900 --> 38:18.710 with our sleep error stream and this message. 38:18.710 --> 38:21.380 So here let's use the sleep. 38:23.760 --> 38:32.460 Get logger and let's get the Arduino bot interface logger. 38:32.520 --> 38:35.910 And now after the get logger, let's print the message. 38:36.870 --> 38:42.540 Something went wrong while. 38:44.550 --> 38:45.900 Sending the message. 38:45.930 --> 38:47.850 Here, let's print the message. 38:49.080 --> 38:53.340 And also let's print to the port. 38:53.370 --> 38:56.160 So let's print the name of the port. 38:56.280 --> 39:00.600 So this variable here at which we are trying to send the message. 39:00.600 --> 39:04.680 So in case for example we detect that this is the wrong port. 39:04.710 --> 39:05.910 And in this case. 39:05.910 --> 39:11.430 So if we are in this catch instruction here let's return an error. 39:11.430 --> 39:16.710 So let's return an Arduino interface. 39:17.340 --> 39:21.600 And the return type is an error message. 39:21.600 --> 39:24.150 And then if everything went well. 39:24.150 --> 39:30.930 So if we are here after this try catch statement, it means that we were able to actually send this 39:30.930 --> 39:32.190 message to the Arduino. 39:32.220 --> 39:40.170 And so here we can update the previous position commands variable with the current position commands. 39:40.170 --> 39:45.870 So actually this is the variable that will be used in the following iteration of the write function. 39:45.870 --> 39:52.680 And as everything went okay we can simply return the hardware interface. 39:52.680 --> 39:57.990 And so the return type we can return okay perfect. 39:57.990 --> 40:03.180 So with this we have completed the definition of the Arduino bot interface class. 40:03.180 --> 40:07.530 So this class here that we have defined in the include folder. 40:07.530 --> 40:15.090 So in this file here and now in the next lesson we are going to register it as a Ros2 control plugin 40:15.090 --> 40:21.930 that will be used as the hardware interface of the real robot for the Ros2 control library.