In this laboratory lesson, we will use some tools from the TF2 library in Ros2 to create a Python service
server that converts the orientation of an object from its quaternion representation to Euler angles,
and vice versa.
This service can be useful throughout the course.
Whenever you want to know the Euler angles corresponding to a given quaternion, or also vice versa.
Let's create this new tool within a new package in our workspace.
So let's navigate to the workspace and then to the source folder in which we have already created several
packages, and which contains the functionalities that we developed for the robot so far.
And here let's create a new package.
So with the command Ros2 package create let's use as a build type.
Amend CMake.
Since we will be including both Python and C plus plus code in this package.
So let's use CMake as build type and let's name this package Arduino bot utils.
So let's press enter to create this package.
And now back to the workspace.
Let's build the workspace so we can build in order for us to recognize also the newly created package.
So the Arduino bot utils package.
Now we can go back to Visual Studio Code.
And so here we can see that in our workspace there is the Arduino bot utils package.
And conventionally, as we are going to write Python code, this should be placed in a folder within
the package that has the same name of the package itself.
Normally in the previous laboratory lessons, when we developed some Python code, this folder was automatically
created by the build type amend Python.
And so here, for example, for the Arduino Pi examples, we can see that the Arduino bot Pi examples
sub folder was automatically created by the build type that we choose.
Anyway, as for the Arduino bot utils, we are going to include both C plus plus and Python script within
the same package.
So within the Arduino bot utils package we choose cmake as build type and so this is not created automatically.
The folders that we need in order to develop Python code.
So let's start by creating a new folder that is called Arduino bot utils.
And within this folder let's create a new Python file called init dot pi.
And for the moment we can leave this file empty.
And now let's create another Python file.
So the actual node.
So the script that will contain the actual logic of the node.
And so here let's name this one angle.
Conversion dot pi.
Here.
To start the script we need to declare the Python interpreter directory.
Since the CMake build type that we used cannot automatically locate it, so it can be found under the
user.
Been environment.
And the Python version that we are using is Python three.
So Python three.
Now we can proceed as usual in order to create a new two node in which we can instantiate two service
server one will be used for the conversion from quaternion to Euler angles, and the second one will
be used for the conversion between Euler angles to quaternion instead of rewriting the code that we
are already familiar with.
So, for example, the one of the service servers, we can copy directly the contents of the simple
service server that we implemented in the Arduino Pi examples.
And so this script is called the simple service server dot Pi.
So we can copy all the content of this file and we can paste it here.
Let's change the name of the class.
And so let's name this one angles converter.
And then let's also call the node.
So this one angle.
Conversion.
Service server.
And then we need to create two instances of a service server.
So we can copy this instruction here.
And we can paste it below in order to start this node and make it create a new service server.
Let's call the first one.
So this one Euler two.
Quaternion.
And the second one, as you might imagine, quaternion to Euler.
And so the first one.
So the variable Euler to quaternion contains an implementation of a service server that we can call
Euler to quaternion.
So let's copy this one.
And also the name of this server will be Euler to quaternion.
So all the other Ros to nodes and application that want to use this service have to contact it.
So I have to send a request to this service called Yola to quaternion.
And the second one as you might imagine, let's call it quaternion to Euler.
Let's also change the name of the callback function.
So in this case for the Euler to quaternion, the function is the Euler to quaternion callback, and
in the second case we want to execute the quaternion to Euler callback function.
So these are the names of the two callback functions that will be executed when we call the Euler to
quaternion or the quaternion to Euler.
So here let's also change the message that we are printing in the console.
And let's simply say angle.
Conversion services are ready so that we know that both of these services are ready.
Now let's start by defining the callback functions.
So here the one that was called service callback in the example.
Let's name it Yolov2 quaternion and it still receives a request message and our response message.
So for the moment let's leave the content empty.
And let's also define the second one.
So let's define the.
Quaternion to Euler callback.
So this one, and also in this case it receives a request and response message as.
These two are.
Callback function of a service server.
Before moving on to define the logic of the two callback functions.
As you might remember, we have to define the communication interface that the two services use.
That is how the request and response messages are structured for the interaction with these two servers.
We can define these interfaces as we did in the previous lessons in the Arduino bot messages package.
So this package in which we are inserting all the two message interfaces.
And so in the SRV folder in which we have already created the add two interface.
Let's create a new file and let's call this one angular two quaternion dot.
And here let's start by defining the request message of the server.
So the client will send this request message.
And this should contain three Euler angles.
So the three Euler angles that we want to convert into a quaternion.
And so all of these three are decimal numbers.
So let's use float 64.
And these are the roll.
The pitch.
And the yo angles.
And then let's also define the response message.
So let's separate with three dashes.
Let's define the response message.
And this one will contain the converted quaternion.
So this should contain the four elements of the quaternion that are the result of the conversion.
And also these four are decimal numbers.
So let's still use float 64.
And let's call this 1XY.
That.
N.W..
And so this is what we need for the service user to quaternion to convert the Euler angles that we receive
in input in the quaternion that we want to provide as output.
Let's create also the second interface.
So the one for the communication with the quaternion to Euler service.
So still.
Here let's create a new file called quaternion to user dot serve.
And here in the request message.
And this contains the four elements of the quaternion that we want to convert into Euler angles.
So we can, for example, copy the response message of the Euler to quaternion and paste it here.
Then let's also define the response message.
And this should contain the three Euler angles that are the result of the conversion.
So once again let's copy the three Euler angles.
So roll, pitch and yaw.
And with this we have finished with the definition of these two message interfaces with the two service
server.
Finally, we need to make these messages recognized and compiled in Ros two.
And so we need to add them to the cmake list dot.
TXT file of the Arduino bot messages package.
So here what we have already defined the add two service.
Let's define a new service that is the Euler.
To.
quaternion.sv and then another service that is the quaternion to euler.sv.
Let's save this file.
And now before using these two interfaces within our angle conversion service, let's open a new terminal.
And let's go to the workspace and let's build it so that the new interfaces are also built and compiled
and are recognized in Ros two.
And so we can start using them in our nodes.
So in our angle conversion node.
So once this is done we can go back to Visual Studio Code.
And here we can start using the interfaces.
So this time still from the Arduino but messages package from the SRB folder instead of the Arduino.
Let's import the Euler to quaternion and also the quaternion to Euler.
Let's use the first one.
So the Euler to quaternion interface.
In order to define the interface with the first service server.
So the one that we called Euler to quaternion.
And then let's use the second one to define the second interface.
So here the one with the service quaternion to Euler.
Now we can proceed to define the two callback functions.
So the one that will contain the logic for the orientation conversion.
Let's start with the logic for the conversion between Euler angles into a quaternion.
So in the Euler to quaternion callback function, let's first print a message in the terminal.
So with the get logger function, let's print an informative message that says requested to convert
to Euler angles, and then let's print the value of the Euler angle that was requested.
So overall this is a floating number.
Then pitch.
Then you.
And we want to convert this one into a.
Quaternion.
And so let's print then the value of the angles roll pitch and yaw that we want to convert.
And so we can access to these values within the request message.
So let's take the roll.
Then still from the request message, let's take the pitch.
And the you to perform the actual conversion.
We can use a package available in Ros two called TF transformations.
However, we first need to install it in our system and so to do so let's open a new terminal and let's
execute the command sudo apt get install ros followed by the Ros two version that you are using.
So in my case it is humble followed by the name of the package that is the TF.
Train transformations.
So let's press enter.
And this is installing the transformation module for Roscoe Ambel.
Then let's also install another Python library that is called transforms 3D.
And we can install this one with pip.
So the Python package manager with sudo pip three.
Since we are using Python three and let's install.
Transforms.
3D.
Now we can go back to the angles converter node.
So let's close the terminal.
And now we can start using these modules that we have just installed.
So from the TF transformations module let's import the quaternion from Euler function.
And let's use this function.
So the quaternion from Euler within the Euler to quaternion callback.
So here.
So let's call this function.
And this function takes as input the three Euler angles.
So the three variables that we received in the request message.
So request roll request pitch and request your.
This is your story, and it returns the four coefficient of the quaternion, which we can store directly
in the x, y, z and w variables of the response message.
So let's store this one within the response message.
So let's store the x variable then the y variable.
Then the z variable and then the w, so the four coefficients of the quaternion.
Let's finally print also another message in the console in the terminal.
So an informative message that says corresponding quaternion and then the four values so x.
Then why?
Then it.
And then we have W.
And let's finally return also the response message to the client that requested the execution of this
service.
So let's return the response.
Let's continue with the second callback function.
So the quaternion to Euler callbacks.
And here again let's start by printing an informative message in the terminal.
So get longer and let's print an informative message that says requested to convert and then quaternion
followed by the four coefficient of the quaternion.
And so let's print the x the y.
The z.
And the W of the quaternion.
Here we forgot also at the end of the string that we need to print.
All of these.
So we need to print all of these variables.
Okay.
And we need to do the same also here.
So here we need also to print the four coefficient.
But as this is the callback function for the quaternion to Euler service.
Now the four coefficient of the quaternion are in the request message.
So here let's print the four coefficients of the quaternion that are in the request message.
And so let's print the x component, then the y component, then the z component.
And finally also the w component.
And now in order to perform the conversion from a quaternion to Euler angles, we can use another function
still from the tf transformations library.
And this one is called Euler from quaternion.
So let's import this function and let's use it here within the quaternion to Euler callback function.
So let's use the Euler from quaternion function and this one receives as input a vector with the four
components of the quaternion.
And.
So let's pass a vector and the four component are defined here.
So those that are in the request message.
So we can paste it here also in the list that we are passing to the Euler from quaternion function.
And also this function returns the corresponding three Euler angles.
So we can store the three Euler angles that this function returns directly in the response message.
So let's store the role in the response role and then also the pitch and also the yo.
So we are storing the output of this function directly within the response message that the service
server will return.
And now let's print another informative message in the terminal.
So actually we can paste this one here in order to print the corresponding user angles.
And so we want to print the role.
The pitch and also the yo.
So the converted values.
And so now we can print the converted values that we have stored here in the response message.
So let's print those three values.
And finally with this we can return.
So let's return the result.
The response of the service server.
With this we have completed the implementation of the angle converter node.
And now we can proceed to use its functionalities within the main function.
So here that we have already defined.
Here we only need to change the name of the class that we want to instantiate.
That is called angles converter.
Let's also change the name here of the variable into angles converter.
Let's make it spin.
And also let's destroy the node whenever we press Ctrl c.
Now let's save this file and we need to inform the compiler about the existence of this Python node
so that it can be compiled and converted into an executable.
Since we are using CMake as a build type for the Arduino utils package, we need to follow a slightly
different approach for the installation of a node using the Cmakelists.txt file, where we can install
both Python and C plus plus node.
So let's open this file, and let's start by declaring the dependencies that we used in our Python node.
So here where there is the find package instruction.
Let's copy this instruction and let's paste it.
And we are using the RTL Pi library for our Python node.
Also let's paste it again.
And we are using also the Arduino but messages package from which we use the interfaces to quaternion
and quaternion to Euler to talk with the service servers, and then to declare that the Arduino utils
package contains some Python nodes.
We need to use.
So here after the find package we need to insert an amend python.
Install package instruction.
And so we use the name of the package which contains these Python nodes, which is also contained in
the variable.
Project name.
And now we can finally install our Python node.
So after the Python install package let's use the install instruction.
And so let's install a new program.
So programs and here are the script that we want to install is in the Arduino utils sub folder.
So is in this folder that has the same name as the package itself.
And so let's still using the variable project name.
And then we need to declare the name of the script that is the angle.
Conversion.
Dot pi and we want to install this one.
So in the destination of the executable will be the lib folder.
And inside the Arduino board utils sub folder.
So still as this has the same name of the package, let's use the variable project name.
So with this also the file cmake list is done.
Then as we are installing a Python node.
So this one.
Let's also insert the dependency from the build type.
Here I meant cmake python.
So let's add the cmake python.
In order to build Python nodes.
And with this our cmakelists.txt file is ready.
So we have defined all the dependencies.
And also we have defined how the build tool should build and install our Python node.
And so now we can move on and declare also the dependencies that we used in the package dot XML file.
So here let's add a dependency.
So another build tool dependency from the CMake Python that we are going to need to build our Python
node.
And then let's add a dependency from the Rql py library.
Then let's also add another dependency on the Arduino bot messages.
With this we have completed the creation of our node, and so we have instructed the compiler on how
it should make it an executable.
And also we have defined the dependencies that this node needs.
Now we can finally build our workspace and see how this node works.
So let's open a new terminal.
Let's go into the workspace and let's build it with the command called build.
So the build is successful.
This means that we have now an executable for our nodes.
And so let's start them.
So let's open a new window of the terminal here.
Let's search the workspace.
So the file setup dot bash that is in the install folder.
And let's run the node for the angle conversion.
So Ross to run and from the Arduino boot utils package let's start the angle conversion dot pi node.
So let's press enter.
And upon the node start up we can already see that it informs us that the angle conversion services
are correctly running, and so they are ready to process new conversion requests.
We can verify also this by opening a new terminal.
And so if we launch the Command Ross queue service list.
So we get a list of all the services that are currently available in Ross queue.
And apart from the one that were automatically created by the initialization of the Ross to node, we
can see that there are the two services that we called earlier to quaternion and also quaternion to
Euler.
Let's try to send a request to these services.
And to do so we need to source the workspace, which contains the definition of the message interface
that we need to use in order to contact the server.
So in order to send a request to this server.
And so let's use the command Ros2 service call.
And let's for example call the user to quaternion.
We can see that pressing tab twice we get the autocompletion of Ros two that says that this service.
So the user to quaternion requests an interface of this type.
So an interface of type user to quaternion as defined in the Arduino bot messages.
So let's use this interface.
Then let's press again tab twice.
Let's use the double quotes.
Then again let's press tab.
And let's type role and then tab again.
And now we can see that the message is autocompleted.
So now we can send our request.
So basically we can ask the service server to convert a certain Euler angles.
So roll pitch into a quaternion.
For example let's set the roll angle so this 1 to -0.5.
And then let's set the pitch to zero.
And for example let's set this 1 to 1.5.
Let's press enter.
And here we can see that there is an error in our Python script.
So if we go back to the angle conversion actually here we see that we are missing a percentage sign.
And we don't need the comb.
Actually.
Okay, so now we can go back and build again our workspace.
Let's launch again a word note.
And let's send again the request message.
And now the request is successful.
And so we can see that actually the service server response with the corresponding quaternion coefficients.
So these ones that are the result of the conversion between these angles.
So these roll pitch and the into a quaternion.
And also we can see that the client that is this one is receiving the correct response.
So is receiving the four coefficients of the quaternion.
Now let's try to call the second service.
So with the Ros two service call.
And now let's call the quaternion to Euler.
Again we can press tab twice to see that this service is using the quaternion two user interface as
defined in the Arduino board messages.
So let's use this interface.
Then let's use the double quotes.
Let's press tab twice.
Then let's type x and again tab.
And now we get an empty prototype of the request message that this quaternion to Euler service is accepting.
And so now we need to set the quaternion that we want to convert into Euler angles.
For example let's ask to convert the unit quaternion.
So basically the quaternion that has all the components set to zero except for the last one which corresponds
to one.
So let's press enter and we can see that here we have the corresponding Euler angles that corresponds
to this unitary quaternion rotation.
And so this basically indicates that both the service servers are correctly running.