In this laboratory lesson, we will use some tools from the TF2 Library of Ros two to create a C plus
plus service server for the conversion of the orientation of an object from its representation in quaternion
to Euler angles and vice versa.
The service will be useful during the course whenever you want to know the corresponding Euler angles
for a given quaternion or vice versa.
If you haven't followed the previous Python laboratory lesson, then you need to create a new package
in our workspace.
So let's open a terminal and here let's go to the Arduino bot workspace and then to the source folder.
And here where we have already created all the packages that implement the functionalities of the robot
so far.
We need to create a new package.
So, so if you haven't already, we have to create the Arduino bot utils package and we can do so with
the command ros2 package create.
Then let's choose the build type.
And in this case, as we are going to include both Python and C plus plus node in this package, let's
use the Amend CMake as a build type and the package is called Arduino bot utils so you can press enter
to create this package.
I already have it so I will not create it.
And then you need to go back to the workspace and run the command.
Click on Build in order to rebuild the workspace.
And so in order for the new package, so for the Arduino bot utils package to be recognized in Ros two
now we can go back to Visual Studio code.
So let's open Visual Studio code and let's open the Arduino bot utils package that we have just created.
As usual, let's place all the C plus plus source code inside the SRC folder.
So here let's create a new file called Angle Conversion dot CP.
This file will contain the actual code for the conversion between the angular angles and quaternion
and vice versa.
In this script, we are going to create two service server inside our EC2 node, one for the conversion
to Quaternion to Euler and the other one for the opposite conversion.
So from Euler to quaternion.
However, instead of rewriting the code that we already know, we can copy the content of the simple
service server.
So the one that we developed in the Arduino bot CP examples in the source folder.
So let's open the simple service server and let's copy all the content and let's paste it here.
So let's paste it in the angle conversion.
And now let's start by changing the name of the class.
So let's call this one angles converter.
And still this one inherits from the CP node class.
Let's change also the name of the constructor and also within the bind function, Let's change the name
of the class.
Now let's change the name of the node.
So here, let's call the node angles Conversion.
Service.
And now let's start by creating two service servers.
So here with this instruction, we create one service server.
So let's copy this one and let's paste it below in order to create the second one.
Let's call the first service server.
User two Quaternion So.
And so let's also change the name of this variable here among the private variables.
So this is the Euler to quaternion.
Let's also create a new private variable.
So a new instance of the service class by a new shared pointer of the service class.
And let's call this one, as you might have imagine, quaternion to user.
And so let's store the content of the quaternion to user as the output of the create service function.
So let's call the first service.
So the one that we are storing in the user to quaternion variable, let's call it as well, user to
quaternion.
And the second one.
So the second service, let's call it quaternion to Euler, and these are the two names.
So Euler to quaternion and Quaternion to Euler are the two names of the service servers.
So the one that in Ros2 will be available for the client to interact with these two services.
Then let's also change the message that we are going to print in the terminal.
And so let's says Angle conversion services are ready.
So let's print this message here and now we need to change also the name of the callback function that
each of these two service server calls.
So for example, the first callback function, so the one executed by the user to quaternion service
is also called Euler to Quaternion callback, and the second one is called Quaternion to Euler callback.
So these are the two callback functions that are executed and still both of them receive two parameters
as input that are the request and response message.
So the interface message for the communication with the service server.
So let's declare also these two functions.
So the other two quaternion callback.
So let's for example, rename the service callback into Euler to Quaternion, then let's leave its content
empty.
Let's copy this function and let's paste it below in order to declare the second callback function.
So this one that is the quaternion to Euler callback before proceeding to define the logic of the two
callback functions, we need to remember to define the communication interface with the two services.
So as you can see now we are still using the old one that was the add to interface.
This means that we need to define how the request and the response message of the new service server
should look like for the interaction with the service.
If you haven't followed me in the Python laboratory lessons, we need to define these interfaces inside
the Arduino bot messages package.
So inside this package here, inside the SRB folder.
So we need to create two new more interfaces, two new more files, the Euler two quaternion XV and
the Quaternion two Euler SRB.
Let's start from the first one, the Euler two Quaternion interface.
In this case, the request message contains three decimal number three floating points that are the
three components, the three Euler angles.
So roll pitch and yaw.
And the response message instead contains four element, four decimal numbers that are the four components
of the quaternion that here we called X, y, z, and W.
The second interface that we need to declare is the quaternion to Euler interface.
And within this file, the request message this time is composed of four variables of four decimal numbers
that are X, Y, z and W, and the second one.
So the response message is composed of three components that are the three rotation of the Euler angles.
So the roll pitch and yaw.
Then in order to ensure that both of these interfaces are recognized and compiled, let's also add these
ones into the cmakelists.txt.
And so here, where we have already defined the Add interface, we need to add two more lines that are
the ones to declare the Euler to quaternion interface and the quaternion to user interface.
And now we can save this file.
And also before using these two interfaces within our script.
So within our angle conversion file, let's open a new terminal and let's build the workspace.
So let's go into the workspace and let's build it in order for the two new interfaces to be compiled
and so that we can use them in our script.
So once this is compiled, we can start importing the interface.
So still here we can reuse this line and still from the Arduino bot messages libraries from the SRB
folder, let's import the Ula to quaternion dot ZP and then let's also include still from the Arduino
board messages from the services of this package.
Let's include the quaternion to Euler dot ZP.
Now let's use the first interface.
So the interface for the communication with the Euler to quaternion is the Ula to quaternion and for
the second interface.
So for the communication with the service quaternion to Ula, let's use the quaternion to Ula interface.
And also we need to change.
So we need to set these ones here when we create the Ula to quaternion object, we need to use the Ula
to quaternion interface and the quaternion to ula interface when we create the quaternion to ula object.
So this one.
And also finally we need to change the type also of the interface within the callback function.
So the ula to quaternion callback function receives as input a request message that belongs to the Ula
to quaternion interface and returns a response message that still belongs to the ula to quaternion interface.
Then the quaternion to Euler callback function still receives a request message that this time belongs
to the quaternion to user interface and also returns a response message.
Also this one that belongs to the quaternion to Euler interface.
And now we can move on.
Defining the content of the two callback functions.
And so the logic for the conversion of the orientation between the two conventions, so between Euler
to Quaternion and vice.
Let's start with the Euler to quaternion callback function.
And so let's start by printing an informative message into the terminal with the Rql CPP info stream
function.
So let's use the CPP get log to get the logger of rql cpp and then let's print the message request to
convert user angles.
And now let's print the three Euler angles that were received in the request message.
So the role is in the variable request.
And here this is a pointer.
So let's access to the role.
Then let's also print the pitch.
Still from the request message.
Let's take the pitch.
And then finally, let's take the yo.
And so from the request message, let's take the yaw angle.
So let's print also the yaw angle.
And we want to convert all of these.
Into a quaternion.
So now let's insert the actual logic of the conversion.
And to perform the actual conversion, we can use the TF2 package that is available in Ros two.
So first, let's import this package.
So let's include from the TF2 package the utils modules and now we can use this module to create a new
instance of the quaternion class.
So here from the TF2 namespace, let's create a quaternion and let's call this variable Q.
And this class offers a very useful function that is called set Rpy.
So on the object Q Let's call this set Rpy function, and we can use this function to initialize a quaternion
object based on the three Euler angles.
So based on the three components of the Euler angles, so roll pitch and yaw, and so we can pass to
this function exactly the roll pitch that we received in the request message.
So from the request message, let's take the roll, then the pitch.
And finally also the EU.
Here there is an error, so we don't need a comma.
And here the name is Request.
Okay, now it's fine.
And now we have basically a quaternion object.
So the variable Q that has been constructed based on the three angles.
And so at this point, we can save these four components of the quaternion into the four coefficients.
So into the variable that we need to return into the response message.
So let's access to the response message and we need to assign a value to the variable X and we can use
the variable Q So the quaternion and let's use the get X function, we can do the same for the variable
Y, So let's use the get y function then let's do the same for the variable Z.
Again calling the get Z function and the variable W of the quaternion with the get W.
Finally, once the conversion is done, let's also print another informative message into the console.
So let's use the get logger function to get the logger and let's print the message corresponding.
Mueller quaternion.
And so let's print the four components of the quaternion that are X.
So let's print from the response message the X.
Then let's print the Y.
Then let's print the Z.
And then let's print also the W.
And so with this one, we have all the four components printed into the terminal.
Let's continue with the second callback function.
So the one that we called here quaternion to Euler callback function.
And again, let's start by printing an informative message into the terminal so we can copy this one
in order for avoiding rewriting this code and let's print the message requested to convert quaternion.
And so now let's print the four components of the quaternion that we received this time in the request
message.
So X is the one that we received in the request message in this called X, Then we have Y.
And this is the request.
Why?
Then we have the Z.
And finally, we have the W.
And do this.
We have the four components of the quaternion.
And then now we need to perform the conversion from the quaternion angles to Euler angles.
To do so, we can still use the quaternion class.
And so again, from the Tfcu library, let's create a new quaternion object and let's call this one
Q And this time we use it to create a quaternion from the four coefficients of the quaternion.
So from the four components of the quaternion that we received this time in the request message.
So let's create this quaternion object based on the request message.
So X, Y, z, and W, so these are the four components.
And then again, using the TF2 library.
So TF2 library, we can create a new object of type matrix three by three.
So this one and let's call this matrix M and this object can be initialized from a quaternion.
So from a quaternion object.
And so from the variable that we called Q this class.
So the matrix three by three class offers a very useful function called get r, P, which stands for
Get roll pitch and yaw.
So on the M object, let's call the get r, P.
And so this function here basically returns the roll pitch and the values.
So the Euler angles that we get after the conversion of the quaternion into a rotation matrix.
So basically this matrix is a rotation matrix.
This rotation matrix is constructed based on the value of the quaternion.
And now from the rotation matrix, we can get the roll pitch and so the Euler angles and let's store
these ones directly into the response message.
So into the roll pitch and also into the yaw at the end.
Let's also print another informative message.
So let's still copy this one.
Let's paste it here.
Let's close the parentheses.
And let's print the message.
Corresponding user angles.
And so let's print the URL that is in the response message.
The URL value.
Then let's also print the pitch.
So let's print the pitch variable.
And finally, let's print the yo.
That is still in the response message with this.
We have completed the implementation of our Angles Converter class, so all the class is completed.
We have defined its constructor and also two callback functions that are called when one of the two
services of the class gets called.
Now we only need to change the name of the class to instantiate.
So here in the main function, the class that we want to instantiate is the Angles Converter class.
And now we are ready to install this script and to execute it.
So let's save this file and let's start by modifying the file Cmakelists.txt.
So here, let's include the dependency from the Rql CPP package since we used it in our C plus plus
node.
And also if you haven't followed me in the python lessons, we need to include the Arduino board messages
package as we have used the definition of the Urartu quaternion and quaternion to user interfaces.
Also, as in our C plus plus script, we have used the TF2 library.
Let's also declare the usage of this library.
So let's add a new Findpackage instruction in which we declare the usage of the TF2 package.
Now to install actually a word node, let's add a new abstraction of type add executable.
So here before the python install package, let's add the add
executable instruction and let's name the new node angle
conversion.
And the source code of this file is in the source folder and in the angles conversion dot cplusplus
file.
Then let's also use the instruction arm and target
dependencies to pass to the angle conversion node.
So to this node all the dependencies it needs.
And so the one from the Rql CPP library, from the Arduino bot messages and from the TF2 library.
Finally, we can use an install instruction, so let's put it here in order to install this node.
And so we want to install the target is the angle conversion node.
So let's copy the name of the executable.
Let's paste it here within the install instruction.
And we need to install it.
So the destination.
Is the lib folder and the sub folder that has the same name of the project.
So Arduino but utils.
And so we can use the variable project name to indicate this path.
Let's save also this file.
And as a final step, we need to declare the dependencies of this node.
Also inside the package dot XML.
So here we need to declare the dependency.
So let's add a new dependency and let's declare this one from the CPP library.
Then we need to add a dependency from the Arduino bot messages library.
And also another dependency from the TF two library.
Let's save also this file.
And with this we have completed the creation of our node and also we have instructed the compiler on
how it should make it an executable.
Now we can finally move on to build our workspace and launch our node.
So let's open a new terminal and let's go into the workspace and let's build it with the command called
build.
And when the build is completed, we can open a new window of the terminal.
Here we can source the workspace.
So the file setup dot bash that is in the workspace.
And let's run our node.
So Ros to run.
And from the Arduino bot utils package, let's start the angle conversion node.
So let's press enter.
And upon the node startup we can see already that it informs us that the angle conversion services are
correctly running and are ready to process new conversion requests.
We can verify.
So by opening a new terminal and here we can execute the command Ros2 service list to get the list of
all the services that are currently available in Ros2.
And apart from the ones that were automatically created at the startup of the Node, we can see that
there are the two services that we created and so earlier to Quaternion and quaternion to Euler.
Now we can request the execution of our conversion so we can request so we can call one of these two
services.
And in order to do so, we first need to source the workspace also in this terminal in order for us
to to recognize the message interface that we are going to use for the communication with these two
services.
And so now we can use the command Ros2 service call.
Let's, for example, call the Euler to quaternion service, Ula to quaternion.
Now if we press tab twice, we can already see that Ros two automatically detects that this service
here is accepting ula to quaternion messages that are defined in the Arduino bot Messages library.
So let's type the interface, then let's use the double quotes, then let's press tab twice and let's
type role and then tab and we can see that the Autocompletion is inserting a default message.
So a prototype of the request message that we need to send to the user to quaternion service.
For example, here, let's ask to the service to convert the angles -0.5, then zero and then 1.5.
So this is in radians and we want to convert this one into a quaternion.
So let's press enter and as soon as we do so, we send this request to the server and we can see that
it responds with the corresponding quaternion.
So this is the quaternion that corresponds to these three Euler angles.
So these are the three Euler angles and this is the corresponding quaternion.
And you can also see that the client.
So here in the terminal, we can see that we received the response correctly from the service server.
Let's try to call the second service.
So, Ros, two service call this time.
Let's try to call the quaternion to alert.
Let's press tab twice.
And so we can see that this service is accepting the quaternion to Euler messages.
So the this interface.
So let's use this interface then.
Let's use double quotes.
Let's press tab twice, let's type X, let's press again tab and the Autocompletion of Ros is inserting
an empty prototype of the request message that belongs to this interface.
And now let's set the quaternion that we want to convert to the unit Quaternion.
So to the quaternion that has all the components set to zero except for the last one that we set to
one.
So let's press enter.
And actually we can see that the quaternion corresponds to here the corresponding angles.
So this is the quaternion that we wanted to convert and these are the corresponding Euler angles that
represent the same orientation as this one as this quaternion.