In this laboratory lesson, we will create a simple action server in C plus plus that calculates the
Fibonacci sequence.
By implementing these action server, we will have a practical example on how to implement and use Ros
to actions.
And so we will use this script as a template during the upcoming course lessons where we will implement
more complex applications.
In this lesson we want to use the Ros two actions to implement a node that calculates the Fibonacci
sequence.
In mathematics, the Fibonacci sequence, also known as Golden sequence, is a sequence of integers
where each number is the sum of the two preceding ones, except for the first two numbers, which are
by definition zero and one.
The length of the Fibonacci sequence is defined as its order.
For example, the first two numbers indicate the order one of the sequence.
So this is a sequence of order one.
Then the order two of this sequence is represented by another number one which is given by the sum of
the two previous numbers.
And then the order three of this sequence is represented by the number two.
That is still the sum of the two previous numbers and so on.
So the action server that we are going to implement in this lesson will receive as a goal the order
of the Fibonacci sequence that we want to calculate, and we will return the entire Fibonacci sequence
up to the requested order as a result.
So let's add the node in Visual Studio code in the Arduino board, CP example.
And so here in the source folder, let's create a new file and let's call this one simple action server
dot CP.
And as usual here, let's start by importing the CPP library.
So let's include the RCL, CPP.
And the RCL Cpjp.
Next, let's create a new node.
So a new class called Simple Action Server.
And this one inherits from the node class of the CPP library.
So this one inherits from the CPP and so from the node class.
And let's close also the parentheses.
Let's place this class inside a namespace and let's call this namespace as the package itself.
So Arduino bot CPP examples.
So let's paste the class definition here inside the Arduino board example namespace, and let's start
by creating the elements of the class so the attributes of the class and let's start with the public
ones.
And so here, let's start by declaring the explicit constructor of the Simple Action Server class.
And so this constructor takes as input an object of the node options class.
So from the CPP it takes a node options object.
And let's call this variable options and this contains some configuration parameters for the node itself.
By default, if no parameters are passed to the constructor of this class, let's initialize this one.
So let's give a default parameter with an empty.
So CPP with an empty node option object.
Then still with this constructor.
So let's create the body.
But here, when we initialize, so when we call the constructor, let's also call the constructor of
the base class.
So the constructor of the node class to which we pass a string as input that is the name of the node.
And let's call this one simple action server.
And also we pass the node options to this class.
So let's pass the variable that we called options.
And then in order to create an action server within this node, we need to import another library.
So let's include the Rql CPP action.
And from this one let's import the CPP action dot PHP library.
And now from this library we can use the server class to create a new action server.
So among the private variables of this class, let's create a new object of the CPP action.
And so let's create a new server.
This is a template class and as always, let's take a shared pointer to this class and let's name it
action server.
As we are creating a shared pointer, let's remember to include.
Also the memory module from the C plus plus standard library and then within the angular brackets of
the server template class.
So of this object here, we need to pass the communication interface that the action server has to use
for the communication with the client.
Specifically, we need to define an interface with the goal feedback and result message to exchange
messages with the action server.
If you haven't followed me in the Python lessons, we need to define this interface in the Arduino board
messages package.
So in this one in which we have already defined the message interface with the service already.
So as by convention, the message interface with the service was inserted in a folder called Serve the
message interface for the communication with an action.
Instead, we have to insert it within the folder called action.
So let's create this folder if you haven't done it yet.
And then within this folder let's create a new file called Fibonacci Dot Action, which will contain
the interface.
So the message interface for the communication with the action server.
In this file, we have to define the structure of the goal message.
And this is simply an integer that indicates the order of the Fibonacci sequence that we want the server
to calculate.
Then using the three dashes, we can also define how the result message is done, and this is given
by an array of integer numbers.
And so this represents the full Fibonacci sequence that is calculated by the server.
And finally, also still separated by three dashes.
We need to specify how the feedback message is done.
Also, this one is an array of integers, and this represents the partial Fibonacci sequence that is
calculated during the server execution.
Then once you have defined these Fibonacci action file.
So this interface we need also to declare it within the cmakelists.txt of the Arduino bot messages and
we can do so still in the Ros generate interfaces in which we have declared the interfaces.
With these services, we can also add a new line.
So we can declare also the existence of the Fibonacci action within the action folder.
And finally, we need also to change the package dot XML.
And here we need to add the dependency from the action messages.
Now if you have just created this interface.
So the Fibonacci dot action before being able to use this interface within our script.
So within our simple action server dot CPP, you need to compile again the workspace.
And so in a new terminal, let's go to the Arduino board workspace and here you need to compile it with
the command called build so that the new interface so the Fibonacci action is properly compiled and
we can start using it.
So in order to start using it, we can include from the Arduino board messages from the action folder.
This time let's include the Fibonacci dot, PHP and now we can start using this interface.
And so we can set that the action server that here we are implementing.
So from the PHP action, this action server has to use the interface that is in the Arduino board messages
in the action module and that is called Fibonacci.
So we are going to use this interface and now once we have this variable, so once we have created the
action server variable, we can start initializing it.
And so within the constructor let's initialize this shared pointer.
So the action server.
And to do that we can use the create server function.
So this time from the RCL, CP Action library, let's use the create server function.
This is as well a template function and within the angular brackets we need to specify the type.
So we need to specify the interface that this action server has to use.
And so basically this one here that we have already used to create the server object.
So let's say that this action will use the Fibonacci interface that is in the Arduino Bot Messages library
and then within the constructor.
So here within the parentheses we need to pass also the instance of the Ros two node that hosts the
action server.
And so this is the current node.
And so the current class, the simple action server class as this is implementing the node class.
So as this is inheriting from the node class and so we can use the pointer this.
Then we also need to set the name of the action server, namely the name by which the server will be
recognized and used in Ros two.
And we call this one Fibonacci.
And then it also receives three callback functions.
So we need to pass to this functions to the create server function three callback function that are
going to be executed in response to different events.
For the three of them, we can use the standard bind function.
So from the standard library, let's use the bind function to declare the three callback function.
So let's use the bind also here and also for the last one.
So now we have used three times the bind function from the standard library.
And now let's start setting which are these callback function that we are going to call.
The first one.
So the first callback function that we are going to use is the goal callback function that still we
are going to define within the simple action server class.
So let's say that this function so that the goal callback function belongs to the simple action server
class, let's format this a little bit better so that it's more readable this way.
And now let's also set the pointer this to say that we want to use the implementation of this function
that is in the simple action server.
So in the current object.
And then let's also use the namespace.
So using namespace placeholders from the standard library and from this namespace we can use the definition
of the placeholder one and the placeholder two.
And basically this says that this callback function will receive as input two parameters.
So one and two that here we set with the placeholders, then the second callback function.
So here in the second bind function is called the cancel callback.
So let's call this one cancel callback and once more we are going to define this function within the
simple action server.
Still, let's use the this pointer to indicate that we want to use the current implementation.
So the implementation of the cancel callback in the current object.
And then let's use just the placeholder one to indicate that this function is going to receive just
one variable as input.
And then let's declare also the third callback function.
So the last callback function that is the accepted callback.
So.
This is the accepted callback.
Here we need two dots.
And also let's copy this one to indicate that also the accepted callback will be defined within the
simple Action server class.
Then let's use the this pointer.
And also in this case, this function will receive just one parameter as input.
So in this case, let's use the placeholder one about these three callback functions.
So the goal callback, the cancel callback and the accepted callback function.
The first one.
So the goal callback function is called when the action server receives a new goal.
So a new goal from an action client.
And then when it receives a new goal, it can decide whether or not to accept it.
And so to start the execution of the action server or to reject it in case the goal is accepted.
So in case it satisfies certain requirements defined by the action server, then the accepted callback
function is executed and this will contain the actual logic of the action server.
So after the goal is actually accepted and then if during the execution of the server.
So if during the execution of the calculation of the Fibonacci sequence, the server receives a cancellation
request from the action client, then the cancel callback function is executed.
And so the goal is canceled and the calculation is stopped.
With this, we have instantiated and initialized a new action server called Fibonacci, which uses an
interface of messages called Fibonacci that is defined in the Arduino board messages package.
Now, after the initialization of this action server, let's print a new message in the terminal with
the Rql CPP info.
And let's use the get logger function from the RCL CPP
and let's take the RCL cpp logger and let's print the message starting the action server.
Now we can proceed to define the three callback function that we have used in the action server to handle
different phases and different requests that the action server may receive from the client.
So here, among the private attributes and the private methods of the simple Action server class, let's
start by defining the goal callback function, and this one returns an object of type RCL CP action
and it returns a goal response object.
And this one.
So this goal callback function receives as input the unique ID of the goal that was just received.
And so basically it receives as input an object of the CP action of type goal Uid.
And let's call also this one you u ID and also it receives as input a shared pointer to the goal message
of the action.
So to the goal message that was just received.
So let's take this one from the standard message.
Let's create a shared pointer and this is a shared pointer to an object of the interface Arduino bot
messages.
Then from the action folder, let's take the Fibonacci interface.
And from the Fibonacci interface, let's take the goal message.
And so let's call this variable goal.
So this shared pointer is called goal.
In this function.
Let's start by printing an informative message in the terminal with the rql CPP info.
And so here, let's get the logger.
Let's get the CPP logger and let's print the message received goal request
with order.
And then here, let's make this a stream.
And now let's print the order.
And so the order is in the goal object.
And then as this is a shared pointer, let's access to the order variable.
So here it goes, the Rql CPP namespace.
Now it's fine.
And now let's automatically accept the goal.
So for simplicity in this function, in the goal callback function, we don't perform any check either
on the goal or on the server, but let's simply return a goal response.
So let's automatically accept the goal.
So let's return an rql CPP action goal response and let's return an accept and execute.
So automatically this will accept the goal.
And so every time we receive a new goal, it will be automatically accepted.
And so after the goal callback function, as we are always accepting any goal that the server receives,
then the accept.
So the accepted callback function will be executed.
So let's move on and let's define this function.
This is a void function so it returns void and this function receives as input a shared pointer to an
object of the server goal and class.
So this is still from the standard library.
It receives a shared pointer to an object of the Rql CPP action namespace.
And from this one it takes a server goal and this is a template class and it needs as input the type
of interface.
So this is still in the Arduino board messages in the action folder and it receives a Fibonacci interface
and let's call this object goal and all.
And now let's define in this function the execution logic of the server.
However, we must remember not to keep the client blocked for too long while waiting for the server
to execute.
So while waiting for the server to finish the accepted callback function.
For this reason, let's place the actual code.
So the actual logic of the action server within a different thread in order to avoid to block the current
thread.
And so to allow to return the message to the client as fast as possible so that the client can continue
with its execution while the server is performing its functionalities to do so in C plus plus.
Let's include the thread module.
So from the standard library and now let's use it to create a new thread.
So from the standard library, let's create a new thread.
And inside this thread we want to execute a certain function.
So again, let's use the bind function from the standard library.
And also we want to detach the execution of this function from the current thread.
So we want this one to be executed in a different thread from the one in which the accepted callback
function is executed.
The function that we want to execute is called execute and we are going to define this function still
within the simple action server class.
So we can copy this one here and we can paste it here in order to indicate that the execute function
that we want to execute in this thread is declared in the Simple Action server class.
Then let's use the this pointer and the placeholder one in order to indicate that this function will
receive one parameter as input, and the parameter that it will receive as input is exactly the goal
angle object.
So this one here, this variable here that the accepted callback function just receives this input.
At this point, we can proceed to the definition of this execute function.
So let's define this function still within the simple action server.
So here we have also the definition of the execute function, and this one is the one that will actually
contain the logic of the server and that will be executed in a separate thread in order to keep the
client free.
So the client will not wait until the server execution, but will be able to perform other functionalities
while the server is calculating the Fibonacci sequence.
Again, this function receives a shared pointer to an object of the server goal and class, so this
receives the same input as the accepted callback.
So let's copy and paste the same input and the first thing that we can do in this execute function is
to print a new message in the terminal.
So with RCL, CP info and the message that we are going to print.
So first let's take the logger.
So rcl, CP, get logger and let's get the rcl cp logger.
And the message that we are going to print is executing goal.
So this informs basically the user that the action server is already in the execute function and that
is going to execute the goal soon.
Then let's create an object of the rate class.
So still from the RCL CP library, let's create an object rate and let's call this one loop rate.
And this object can be used to control the execution of a loop within our Ros two node so that two consecutive
execution of the loop are temporarily separated.
For example, let's say that they are separated by one second.
Next, let's create a goal variable.
So.
And this one contains the goal message that we can extract from the goal and.
So let's extract the goal message with the get goal function.
And so now instead of writing this, every time, we can just refer to the goal variable.
Now let's also create a feedback message.
And this is a shared pointer.
So STD.
To an object of the feedback message.
So from the Arduino bot messages from the action and from the Fibonacci interface, we want this feedback
variable to be an instance of the feedback message.
And now we can use this message.
So the feedback message that we have just created to send periodically messages to the action client
in order to inform it about the progress of the Fibonacci action server.
So let's create a new reference variable and let's call this one sequence.
And let's access.
So let's set this one equal to the feedback.
And let's access to the partial sequence.
So now every time, instead of accessing to the partial sequence of the feedback message like this,
we can just refer to the sequence variable.
And so now we can use this variable to initialize the Fibonacci sequence with the first two numbers
that always appear in the Fibonacci sequence.
So to this sequence, let's push back.
So let's add the first element that is zero.
And then let's add.
So let's push back the second element.
That is always one.
And now we can create the result variable.
And this is another shared pointer.
And this time this is a shared pointer to an object of the class result.
So of the interface result.
And so from the Arduino, both messages from the action and from the Fibonacci interface, we want the
result variable to be an instance of the result message.
Now at this point, all the components are ready and we can actually implement the logic.
So the algorithm for the calculation of the Fibonacci sequence.
So let's create a for loop that goes from the index one to the goal order.
So this is goal and let's access to the order variable.
And at each iteration let's increment the index.
And now let's also make this loop executed as long as the Ros two node is running.
So here let's also verify that RCL cpp is okay.
Let's wrap also all of this among the parentheses.
Okay.
And now this loop will be executed for the index that goes from one to the goal order and also until
Ros is running.
So the Ros two node is running first before performing any calculation, we need to check if the action
has received any cancellation requests.
And so if this happens, so if the goal ended as received the cancellation request, so is canceling.
So in this case, if the goal ended is canceling, then we need to update the result message with the
partial sequence that was calculated up to this iteration of the loop.
So let's update the result message and the sequence that is in the result message to the partial sequence
that we are calculating.
And then let's also set the status of the goal.
So let's set the goal handle to canceled and let's pass the result.
And also in this case, let's print a message in the terminal.
So let's take the logger.
And let's take the CP logger and let's print the message.
Goal cancelled.
And in this case, let's return.
So let's finish the execution of the execute function otherwise.
So if we go here, otherwise if no cancellation request was received.
So namely, if the action server is still in execution and the client didn't send any cancellation request,
then we can proceed with the calculation of the Fibonacci sequence.
So we can add a new element to the sequence.
So sequence.
And then let's push back a new element to this sequence.
And this one is given by the element at the last index, plus the element at the second last index.
So I minus one.
With this, we have added a new element to the Fibonacci sequence.
And so since we added this new element, let's also publish a feedback message.
So using the goal and let's publish a new feedback with the publish feedback function.
And to this we pass the feedback message.
So the feedback message in which we are filling the field, partial sequence and also let's print a
message in the terminal.
So rcl cp info.
And let's print the message, publish feedback, and then at this point, to simulate the effect that
the server is executing as low and costly operation.
So an operation that takes some time to complete.
Let's add a sleep.
So basically, let's interrupt the execution of this script.
So the execution of this for loop and let's wait it for a few seconds to do so.
We can use the loop rate object from the rate class.
So let's use this variable and here let's call the sleep function.
So basically this will stop the execution of the for loop for the time required in order to match the
frequency that is defined with the rate.
So in order to match the frequency of one hertz.
And so basically this will make the for loop publish a message each second at the end of this loop.
So here after the for loop, the action server will have already calculated all the Fibonacci sequence
so up to the desired order and also will have sent a feedback message to the client for each of the
elements that it added to the sequence.
Now let's check if the sleep is still running.
So if the node is still running, let's update the result message.
So let's update the result message and the variable sequence to the sequence variable that now already
contains the full Fibonacci sequence.
And then let's set the status of the goal with the goal and all.
So let's set this one to succeed and also let's pass the result that contains the full Fibonacci sequence.
Furthermore, let's also print a message in the terminal.
And this message says, let's take this one goal.
Succeeded now to complete the implementation of the Simple Action Server class.
We need to define also the cancel callback function so the execute function is completed and we can
move on to define the this one.
So the missing one, the cancel callback function that is executed when the server.
So the action server receives a cancellation requests from the client.
So let's define this function and this function returns an object.
So here of the CP action library of type cancel response and it receives an input another shared pointer.
To an object of the type CP action.
And so if the class server goal endl and also here this is a template class, so the server goal and
is a template class which needs as input the type of the interface that is Arduino board messages,
then action, and then the Fibonacci interface.
And let's call this variable again goal and all.
So let's move on to define the behavior of this function, the console callback function.
And let's start by printing a message in the terminal.
So let's get the logger.
And the message says.
Received request to cancel the.
And so let's return now an object of this type, so of cancel response.
And let's set this one to accept.
So this means that basically the action server accepted the gold cancellation.
So with this, the simple action server node is completed.
So our simple action server node is completed and now we can define the main function in which it will
actually be used.
So the functionalities of this class will actually be used.
Regarding the launch of a ros2 node that implements an action server in C plus plus, we can use a very
useful macro that is available in the components library.
So let's include from the RCL CPP components library.
So from this library, let's include the register node, macro CPP.
And from this one let's use at the end of the file.
So here the macro rcl cpp.
And then components and then register a node.
So let's use the macro to register the simple action server class that we created in the Arduino CP
examples.
And so that we called Simple Action Server.
Let's take care of adding this macro here.
So this function here after the namespace.
So here these are the parentheses that declare the namespace arduino bot CP example.
Take care of adding this macro here after the closing parentheses.
Next we can move on to declare the dependencies and also to install this node inside the file.
Cmakelists.txt.
Here, let's declare the usage of the library CPP action.
So let's use another find package instruction to declare the usage of the CP Action library and also
another one to declare the usage of the CP components.
And then we need to tell the compiler how it should build and install the simple action server CP script.
So the one that we have just created.
For an action server.
The instruction to install and compile the source code are slightly more complex and so we can simply
copy paste these instructions from the Ros two wiki, which I will also leave in the course description.
So here, after adding all the executables and before installing them, let's paste the instructions
that are attached to the description of this lesson.
In this case, basically what we are doing is adding a new library that is called Simple Action Server,
and then we are declaring the dependencies of this library that are basically defined here.
And so that are the Arduino board messages, the Lcvp.
Lcvp action and RCL CP components.
And finally here we are registering the node that implements the action server, which is the one that
is in the Arduino CP Examples namespace and is implemented in the class Simple Action Server.
Finally, let's also install this new node.
And so to do so, we need another install instruction.
So let's add it here.
And this time the targets are the simple action server.
And we want to install the archive destination into the lib folder, then also the library destination
into the lib folder and then let's set the runtime destination.
To the bin folder.
Let's save this file.
And now we need to declare these dependencies also in the package dot XML file of the Arduino CP examples
package.
So here, let's add a new dependency from the CP action package.
Then let's add a new dependency from the CP
components package and this is all we need in order for this simple action server to be executed and
correctly compiled.
So now after all these changes, of course we need to recompile our workspace.
So let's go to the workspace and let's build it with the Falcon build.
We can see that there are some warnings because of some unused variables, but none of these is an error.
So let's continue.
And so we can continue by executing our action server.
So in a new window of the terminal, let's source the workspace.
So the file setup dot bash.
And here to start our action server, let's run from the Arduino bot CP examples.
So if we press tab twice now we can see that we have a node that is called Simple Action Server Node.
So let's start the simple action server node.
Let's press enter and we get the confirmation that the action server node is running.
So we can see from the terminal output that the starting action server message is printed and also we
can double check it.
If we open a new terminal and we execute the command Ros2 action list and so we get the list of all
the currently available actions in Ros2.
And since we have started only one node, so the last node that we created, we can see that there is
only here one action server available.
That is the Fibonacci action.
Now we can also get more information about this action server using the Ros2 info command.
But before using so let's source our workspace also in this terminal in order for ros2 to recognize
the message interface.
So now we can use the Ros2 action info command and let's take more information about the Fibonacci and
let's also use the flag minus T in order to display also the type.
So the type of the interface of the message interface to communicate with this action server.
So let's press enter and currently we can see that for the action.
So for the server there is only one node that is interacting with this action server and this is the
simple action server.
And also we can see that this server is using the Fibonacci interface that is defined in the Arduino
bot messages.
At this point we can also send a new goal to this server and again from the terminal we can use the
command Ros2 action send goal.
And so let's send a goal to the Fibonacci.
And now if we press tab twice, we can see that Ros two automatically detects that this action server
accepts Fibonacci messages so that the Fibonacci is the interface that we have to use to communicate
with this server.
And so let's use this interface.
Now let's use the double quotes, then press tab twice, then type O and again tab.
And this will automatically insert an empty prototype of the goal message that we can send to this action
server.
So for example, let's request so let's send a goal of ten.
So basically we are asking the action server to calculate the Fibonacci sequence up to the order ten.
Let's also use the flag minus F to display the feedback.
So minus F stands for feedback in order to display the feedback in the same terminal.
So let's make this a little bit bigger and let's press enter.
And we can see that when we send the goal, there is an error.
So there is a segmentation fault in the action server and most probably this is due to some shared pointer
that we have not initialized.
So let's go back to the simple action server.
And actually we can see that here, both the feedback message and the result message.
We created a shared pointer, but we never initialized it.
So here, instead of using the shared pointer, let's directly use the make shared function.
So for both of these so that we are declaring to shared pointer still of type Fibonacci feedback and
Fibonacci result.
And also we are initializing them.
Now back in the terminal, let's build again our workspace.
So let's stop the client.
Let's start again the action server.
And now let's send again the goal.
So still with the order ten.
So let's press enter.
And now we can see that correctly.
As soon as we send this goal, the action server informs us that it has received a new goal.
So here we can see it and that is executing it and also that is constantly publishing feedback messages.
Also, we can see those feedback messages here in the terminal with the partial sequence that the action
server is calculating.
So at each step we can see that the server is adding a new element.
And also we can see that at the end of the service execution.
So here we have the confirmation that the action server executed correctly, the goal.
So it reached the order required.
And so here we can see also at the end the result message with the full sequence.