To define the behavior of all the functions of the Arduino board interface class that you have declared
in the previous lesson.
We need to create a new script, so a new Cplusplus file inside the source folder, this time of the
Arduino bot controller package.
And let's call this one Arduino bot interface dot cpp.
Since we are going to use this script to define the components of the Arduino bot interface class,
let's start by including the header file.
So the file Arduino bot interface dot HPP that we have created in the previous lesson.
So let's include from the Arduino bot controller the Arduino bot interface dot dot hpp.
Now let's use the same namespace that we've used in the previous lesson.
So the namespace is the Arduino bot controller As the name of the package and within this namespace.
Let's start by defining the functions of the Arduino board interface class, and let's start by its
constructor.
To define a function in a cpp file that we have declared in a header file, we need to specify the name
of the class that is the Arduino bot interface, and then the name of the function that is also in this
case the Arduino bot interface, as this is the constructor.
So let's define this function and let's leave it empty for the moment.
And the next let's define also the destructor.
So the Arduino bot interface and the function tilde Arduino bot interface.
So this one is the destructor.
This one.
So the destructor is called when the object of the Arduino bot interface class.
And thus the Arduino interface is destroyed.
In this function, we want to check whether the serial communication with the Arduino is still open
is still active.
So let's check if the variable that we declared in the previous lesson and that is called Arduino is
still open.
And in this case.
So if this is still open let's try to close it.
So let's try to call the function close.
And if something goes wrong.
So let's add a catch that is catching any exception.
So here we add the semicolon.
And here in the catch.
So if an exception is thrown by this function let's print an error message.
With fatal stream.
Let's get the logger.
For the Arduino bot interface.
So this is the scope and let's print the message.
Something went wrong while closing the connection.
And then let's also print with the port.
And then let's print the variable port.
So this variable here.
Next let's move on to define the behavior of the on init function.
So let's go back to the Arduino bot interface dot http.
And let's copy this one.
So the declaration of the on init function and let's copy it here in order to define it.
Then let's remove the column and let's add the name of the class.
So this one here and let's put it here.
Let's also remove the virtual statement and the override one.
And now within this function.
So within the UN init function let's first call the on init function of the base class.
So the base class is in the hardware interface and is the system interface.
And so let's call the on init function of this class here.
And this function receives as input the hardware info object.
So this variable here that the on init function of the Arduino board interface received as input.
And also it returns an object of type callback return which we store in a new variable.
So here let's create a new variable of type callback return that we call result, and that takes the
result of the init function on init function that we executed on the system interface.
And now let's check whether or not the initialization of the base class.
So with this one with the system interface was successful or not.
So if the result is not of type callback return success, then in this case let's already return the
result.
So if for example this one failed, then let's make also the on init function of the Arduino bot interface
fail otherwise.
So here, if the base class was successfully initialized, we can proceed to initialize also the serial
communication with the Arduino.
First we need to know which is the port to which the Arduino board is connected.
And so we do this by assigning a value of the port variable.
So let's try to assign a value to the variable port of the Arduino board interface class, and this
value is given from the info, and then from this one lets access to the hardware parameters and then
lets access to the element that is called port.
As you may have noticed, we are using this object here.
So the info object.
But if you remember we have not declared such an object here.
This means that basically this is an object that we are inheriting from the base class.
So it is defined in the system interface and contains the parameter with which the hardware interface
is launched is started.
And so here we are checking whether the parameter port exists.
If not.
So catch.
So if this doesn't exist it will throw an exception of type const out of range.
And so in this case, let's print an error message in the terminal.
Again with klccp fatal.
Then again let's get the logger with the scope.
Arduino bot interface.
And let's print the message.
No serial port provided.
And so in this case we are going to abort.
And so let's return an object of type callback return of type failure.
If everything went well.
So if we reached this point of the function, this point of the script, then it means that the port
parameter.
So this one exists.
And so basically it means that we started the hardware interface by setting also a parameter for the
port for the communication with the Arduino.
Now we can complete the on init function by also initializing the three decimal number vectors that
we have declared, and so these three vectors the position command, the previous position command,
and the position state.
Let's start with the position commands, and let's reserve a space for this one.
Since we already know its size and its size is equal to the size, so to the number of the joints of
the robot.
And so we can take this information still from the object info.
Then let's take the vector that is called joints.
And from this one let's use the size function.
Let's do the same also for the position states.
So let's reserve our space for this vector that has the same size.
And finally let's do the same also for the previous position command.
So let's reserve a space that is the same size.
So this one here.
And finally if we reached this point of this function let's return a success.
So a callback return of type success.
With this the on init function is completed.
And now let's define the function export state interface.
So once again let's go back to the declaration of this function.
And let's copy it.
So let's copy it declaration this one and let's paste it here.
Let's remove the override And also the virtual.
And let's add here the name of the class to which it belongs to.
So the Arduino bot interface.
The purpose of this function is to create a vector of objects of type state interface that are still
declared in the hardware interface library.
So let's create a vector.
And this is from the hardware interface library.
This is a vector of state interfaces.
And let's call this one state interfaces.
And we can use this list of interfaces to communicate the current status of the joints and so of the
motors of the robot to other nodes, and so to other ros2 applications.
As you might recall in the control section of this course, when we configured the Ros2 control library
for our robot, we mentioned that each joint communicates its state in the form of a position.
So using a position interface.
And so this is the same interface that we have to use.
Now in this case.
So let's loop through each joint.
So for size t equal to zero.
And then this goes to all the joints.
So the size of the joints is here in the info object.
And then joint size.
And then let's increment this index.
And in this loop let's add a new element to our state interface vector.
So let's use the function in place back to add a new element to construct a new element.
And so this one let's add a new object from the hardware interface.
And of type state interface.
The constructor of this class.
So the constructor of the state interface class requires to be passed as input to the joint name, and
so we can get this one from the info object still.
Then from the joints.
Then let's access to the joint at the index I.
And let's check the name.
So let's get the name of this joint.
And then to the constructor of the state interface class, we also need to pass the type of the interface
that we want to use.
So the type of the interfaces is defined in another library.
So let's include from the hardware.
Interface from the types module.
Let's import the hardware interface type values.
And now let's use this one to define that.
Here the type of the state interface is a position interface.
And so let's use the H w if position, and finally it stores the position of the joint into a new variable.
So let's say that we want to store it within the position states vector.
And so we are defining the position for the joint at the index I.
And finally once we have finished the loop through all the joints.
So once we have added all the joints that make up our robot into the state interface.
So the state interface that is a position interface, let's return the state interfaces vector.
Now let's define the other function that we have declared of type export command interface.
So if we go here let's define the behavior of this second function.
Here again let's remove the override, and let's remove the virtual.
And let's instead use the name of the class that is the Arduino bot interface.
So let's add it here before the name of the function.
If the purpose of this function so of the export state interface was to communicate the interface that
we need to use for knowing the state of each robot's joint.
The purpose of this one, so of the export command interface, is to communicate the interface that
we have to use in order to send movement commands for each joint.
So this one is used in order to receive the type of the interface used to know the state of each joint.
And this one is that is used to know the type of interface that we need to use to set.
So to send the commands to each of the joints.
Once again, as you might recall from the theoretical lesson of the control section, we decided to
use the position interface also to send comments, so also to write comments to the robot's joint.
So let's start by creating a new vector.
Of type hardware interface command interface.
And let's call this one command interfaces.
And as we did for the export state interface.
So let's loop through all the joints like this.
And this time let's add a new element to the command interface.
So let's do command interface in place back to create a new element.
And now the new element that we want to add to the vector is of type command interface.
So let's change this one to be command interface.
Still as input.
This class also requires the name of the joint so that still we take from the info object.
Then it requires the type of the interface that we want to use in order to send commands to this joint.
And still this is a position interface.
And finally, it takes the command which we want to send to our joints.
And so this is in the vector position commands.
And we want to send the item at the index I.
And finally also in this case let's return the command interfaces.
And now let's move on to define the on activate and on the activate functions of the Arduino bot interface
class, which basically implement the node lifecycle.
So let's go back here where we have declared those functions.
And let's copy their declaration and let's define them.
So let's define the first one that is the on activate.
And then let's define the second one.
So this one that is the on deactivate.
Okay.
And also let's add the name of the class.
So this one here to indicate that both the on activate and on deactivate functions belong to the Arduino
bot interface class.
In the activate function.
So here let's start by printing an informative message into the terminal.
And let's use the Arduino bot interface scope.
And let's print the message starting the robot hardware.
So like this.
Then let's initialize the three vectors of decimal numbers that we have declared.
So these three vectors here.
And let's start with the Position commands.
And let's set this 1 to 0.
So this has four elements.
And let's set all of them to zero.
Then let's also initialize the previous position commands.
And let's set also for this one the initial state.
So to be zero and then the position states to be as well a vector with four elements that we set to
zero.
With this we are assuming that the initial position.
And so the initial command of all the robots are zero.
At this point, let's try to open the serial communication with the Arduino using the lib serial.
So let's try to open.
So to use the Arduino object to open the variable port.
And so we are trying to open the communication with the Arduino in the requested port.
So in the port that we have indicated here with the parameter port.
And also let's try to open this communication with about rate that is from lib serial from baud rate.
We want to use the bout 115 200 and if something goes wrong.
So if anything actually goes wrong here in this initialization.
And so if something goes wrong while trying to opening the serial communication at this port with this
baud rate, let's print an error message.
As always, let's take the Arduino bot interface scope and let's print the message.
Something Went wrong while.
Interacting with the port.
And then let's print also the value of the port that we were unable to open.
And in this case, let's return a callback return message of type failure otherwise.
So if everything went well.
So here let's print an informative message so we can copy this one actually.
And let's print the message.
So here our order started.
And so ready to take commands.
And in this case let's return an object of type callback return success.
So to indicate that everything went well and that we were able to initialize to open the serial communication
and also to initialize all the variables that are needed for the on deactivate function.
So for this one, let's start by printing an informative message into the terminal.
So we can actually copy this one.
And let's paste it here.
And here.
The message that we are going to print is just stopping the robot hardware.
And now let's check if the serial communication is open.
So whether the serial communication is open.
And in this case let's try to close it.
So let's try to use the Arduino close function in order to close this communication.
And so to leave the serial port free for other application to use it in case something goes wrong.
So in case actually Anything goes wrong.
Let's print an error message in the terminal so we can copy this one.
And we can paste it here.
And this message says something went wrong while closing the connection with the port.
And then followed by the name of the port.
And if we were able here to correctly close the communication with the hardware, then let's print an
informative message and let's also return an object of type callback return success.
And here let's print the message.
So hardware stopped here there is a typo.
Is hardware started?
Next let's define the last two functions that we declared in the Arduino interface class.
So let's go back here.
And these are the read and the write function.
So let's copy the declaration.
and let's paste it here.
So let's start by defining the behavior of the read function and then the one here of the write function.
And let's use the name of the class.
So Arduino bot interface.
In order to define that both of these functions belong to the Arduino bot interface class.
The read function.
So this one, as the name suggests, is responsible for reading the current position of the robot's
joint and make it available in Ros2.
Anyway, since we are using very cheap servo motors for our simple robot, they don't provide any information
about their current position.
Therefore, let's just assume that every time we send a new position command to the robot, it is just
able to read it perfectly and thus we can simply set here that the current position.
So the current state of each servo motor is equal to the current command that we have sent to the robot.
So obviously this is an important assumption.
But given that the forces and the acceleration that are involved are very small, and also considering
that we are using a very cheap hardware and electronics, this is an assumption that we can accept.
And so now we can already return an object of type hardware interface, return type and return.
Okay.
So to indicate that there are no errors while the read function.
So this one as I said is responsible for reading the current position of the servo motors.
So the current position of the joints, the write function as the name says, has the responsibility
to write.
So to send position comments to the joints.
So to the Arduino so that it can activate it can move the servo motors accordingly.
So here within this function, let's first check that the new command that we received so that was received
by the robot is not equal to the last command that was received.
And so that would mean that the robot is already in the desired position.
So if for example, the position command that we have just received is equal to the previous position
command.
So if we received a command that is equal to the last one, let's just terminate the execution.
So let's just return another interface return type okay.
And this means that we are already there.
So this means that we are already aware of this command and we don't need to send it again otherwise.
So here if we received a new command.
We want to send it to the Arduino through the serial port.
To do this, we are going to use a very simple communication protocol, a very simple convention.
Let's say we will transmit a single string, a single message so that the communication is lightweight
and fast.
This message will contain four components separated by commas.
The first component is represented by the letter B and indicates the desired position for the motor
that controls the robot base.
The second component, indicated by the letter S, indicates that the desired position for the motor
that controls the robot shoulder.
The third, indicated by the letter E, indicates the desired position for the motor that controls the
elbow.
And finally, the last component that is indicated by the letter G indicates the desired position for
the motor that controls the gripper.
Back in the write function, so back here let's create this communication protocol.
So let's implement this communication protocol by creating a new string called message.
And so this will be the message that we are going to send through the serial port.
And considering the way that we will connect the servo motor to the robot link, and also because in
Ros2 the angles are expressed in radians, while in Arduino they are expressed in degree.
The value of the base joint of the robot.
So the one that we want to send to the base joint of the robot is equal to the value that we received
in the position command for the first joint plus pi that we can access with m pi divided by two.
And then we need to multiply all of these.
So let's wrap with some parentheses.
And let's multiply all of these by 180.
And let's also divide all of these.
So let's wrap with more parentheses.
Let's divide all of this again by pi.
So with the macro and pi this equation could return a decimal number.
But anyway let's cast it to an integer.
So let's perform a static cast to an integer for all this number here, since anyway, the command that
we are going to send to the motors through the Arduino can only be an integer, so can only be an angle
in degrees.
Let's append to our message.
So to the message, let's append the letter b.
And then as we always want to send a three digit number to the Arduino.
Let's also use a new function.
So here let's append the result of a new function that we are going to call compensate Zeros like this.
And this function here takes as input the base angle.
So this integer here that we have created.
So let's copy it and let's paste it within the compensate zeros.
And basically this function is going to add as many zeros as needed in order to make this number here
a three digit number.
And so this will eventually add some zeros at the beginning of this number here.
And also after this one here let's also append.
The actual number.
So the base so this angle here.
And first let's convert it into a string with the STD string.
And so let's convert this number here.
So the base number into a string.
So here we can use the function to string perfect.
And so this function here will we'll convert this integer into a string that we can append to this message
here.
And then in order to conclude.
So in order to finish the message that is set for the motor that actuates the base of the arm, let's
also append a final comma.
And so this concludes the message for the base.
And then we're also going to add the message for the other remaining joints of the robot.
But before doing so let's define this function here.
So the compensate zeros.
And actually we can do it here.
So within this same file.
And this is a very straightforward and simple function.
So let's define it the compensated zeros.
And this takes as input an integer that is the value that we want to check.
So for which we want to generate the zeros.
And this returns a string that will contain as many zeros as needed.
So let's define it.
And so here let's create a new string that is called compensate zero.
And by default let's initialize this one with an empty string.
And now let's check this value here.
So the command that we are going to send to each of the joints that actuate the motors of our robot.
So here let's check if the value is smaller than ten.
So this means that we have a value between 0 and 9.
And so in this case in order to make it a three digit number, we need to add two zeros in the front
of this number.
So let's set this string here.
So this is the compensate zero.
So this string here let's set it equal to zero zero.
So we are adding two zeros at the front.
Otherwise else if so if the value is smaller than 100.
So this means that we are dealing with a number between 10 and 99.
In this case we just want to add one zero.
So let's set the compensate zeros just to one zero.
Perfect and otherwise.
So this means that we are dealing with a number that already has three digits.
And so we can simply set the compensate zeros.
So this string here we can simply set it to an empty string.
So it is already a number with three digits.
And so we don't need to append anything at the beginning of this number.
And we simply can return this string here.
So the compensate zeros string perfect.
So now we can go back in the right function where we were using the compensated zeros before the actual
command.
In order to eventually add any other zero here.
Now let's continue also with the other joints.
Starting with the shoulder.
And this is equal to.
So the value that we want to send to the shoulder is equal to 180 minus.
Let's cast with the static cast.
So let's cast to an integer because the comment that we want to send to the Arduino and so to the servo
motor has to be an integer.
And so let's access again to the position command.
And this time we want to access to the position command at the position one.
And here we want to add the pi divided by two.
Perfect.
And so we want to multiply then all of these by 180.
So here we need a parentheses.
And also here.
So after this one we need also another parentheses in order to multiply all of these by 180.
Perfect.
And then we can add another parentheses.
So let's add another here at the beginning and another here after the 180.
In order to divide all of these by pie.
Perfect.
Now again let's append this message.
So this is for the shoulder.
So let's append to the message the letter s.
Then let's append this one here.
So the compensate zeros for the shoulder.
So this will take this value here and will eventually add any zeros in the front.
Then we can also append.
So this value here.
So the value again of the shoulder.
So this integer here converted into a string.
And finally in order to complete this message we want to append a final comma.
And so this means that we are closing the command for the joint that actuate.
So for the motor that actuates the joint of the shoulder.
Now let's also do the same for the elbow.
Perfect.
And so this is again let's access to the position commands.
And let's access to the position command at the position two.
And to this one we want to add pi divided by two.
And we want to multiply all of these by 180.
So let's use the parentheses here and let's multiply by 180.
Perfect.
And let's divide all of these by pi.
So again let's use the parentheses.
And let's divide by pi.
Perfect.
And also we want to cast all of these again using this function here.
So the static cast in order to cast all of these into an integer.
Now again we want to append this message to the string that we will send to the Arduino.
So let's append the letter e and then also let's compensate the zeros.
So this will check the elbow and will add any zeros in the front if needed.
Then let's also append.
So we can copy this line here.
And we can append it here.
So this will convert the elbow so this integer into a string and will append it to our message.
And finally let's also append a comma.
And so this will close the command that we are sending for the joint.
So for the motor that actuates the elbow of our robot.
Then let's also finally create the command for the gripper.
And so again we can access to the position commands vector this time at the position three.
And actually we want to use the minus sign.
And we want to multiply all of these by 180.
And we want to divide everything by pi divided by two.
So let's use the parentheses and let's divide all of these by pi divided by two.
Perfect.
And also in this case, we want to cast everything to an integer.
So let's use the static cast in order to cast the result of this operation into an integer.
Perfect.
Now, in order to append this comment here to the string that we are creating, let's append the letter
g.
Then let's append here the compensate zeros that will check this command here.
So the command for the gripper and that will eventually add any zeros in the front of this message.
And then let's also append the actual command that will be for the joint of the gripper.
So this command here that is converted into a string.
And finally let's append a final comma.
So this one here and let's paste it.
Perfect.
At this point the message is completed.
So this message here now contains a value for the joint.
So for the motor that actuates the base, the shoulder, the elbow and also the gripper.
And all these three commands are basically a three digit commands that goes from 0 to 180 degrees.
Now at this point the message is ready.
So we can try to send it to the Arduino.
So let's try to use the Arduino.
And on this one let's try to use the right function.
And let's try to write this message here that we have created.
Let's also catch any exception that may occur.
And in this case so if any of the exception of course, let's print an error message in the terminal
with our sleep error stream and this message.
So here let's use the sleep.
Get logger and let's get the Arduino bot interface logger.
And now after the get logger, let's print the message.
Something went wrong while.
Sending the message.
Here, let's print the message.
And also let's print to the port.
So let's print the name of the port.
So this variable here at which we are trying to send the message.
So in case for example we detect that this is the wrong port.
And in this case.
So if we are in this catch instruction here let's return an error.
So let's return an Arduino interface.
And the return type is an error message.
And then if everything went well.
So if we are here after this try catch statement, it means that we were able to actually send this
message to the Arduino.
And so here we can update the previous position commands variable with the current position commands.
So actually this is the variable that will be used in the following iteration of the write function.
And as everything went okay we can simply return the hardware interface.
And so the return type we can return okay perfect.
So with this we have completed the definition of the Arduino bot interface class.
So this class here that we have defined in the include folder.
So in this file here and now in the next lesson we are going to register it as a Ros2 control plugin
that will be used as the hardware interface of the real robot for the Ros2 control library.