If you haven't followed me in the previous Python lesson to create the task server, which is the action
server that executes various actions on the robot, you need to create a new Ros2 package inside your
workspace.
So let's open a terminal and let's go to the workspace here.
Let's go to the source folder in which we are creating all the packages with all the functionalities
for our robot.
And here let's create a new package with the command Ros2 package create.
And let's use as a build type.
Amend CMake and let's call this new package Arduino Bot Remote.
So I'm not going to create this package as I have already created it in my workspace.
And then back to the Arduino bot workspace.
So back to the workspace.
Once you have created this new package, lets simply build the workspace in order to compile it.
So in order to compile the new package.
Arduino bot remote.
Perfect.
So back in Visual Studio Code here, as you can see we have the Arduino bot remote package.
And here let's start by creating a new C plus plus script.
So within the source folder let's add a new file called Task server dot cp that will actually contain
the implementation of the task server node.
So we're going to develop the task server as an action server.
So as an ros2 action server.
And as this can be quite complicated with a lot of instructions, instead of developing all the code
from scratch, we can simply copy paste the code that we have already developed for the action server
here in the Arduino bot CP examples.
So do one of the simple action server so we can copy all of this and we can paste it here within the
task server.
So now let's apply the changes that we need in order to adapt this simple action server to be the task
server.
So the first thing that we want to do is to rename here the class from Simple Action Server to Task
server.
So let's do a Ctrl F and let's rename the simple action server to Task Server.
And so let's replace all perfect.
Then also we can rename here.
So the simple action server node.
Instead of calling it Simple Action server we can simply call it Task Server.
And also here the name of the action server that we are starting is not anymore the Fibonacci Action
server, but we can call it task Server.
Also, another change that we can do in this script is to rename here the namespace.
So now we are not anymore in the Arduino bot.
CPP examples.
But we are in the Arduino bot remote package and so we need to change this one.
Also, if we go here below, here we are in the Arduino bot remote namespace and also in this case the
task server.
So that we are registering here is defined in the Arduino bot remote namespace.
Perfect.
So let's go back here.
And now let's continue initializing this one.
So the action server we have renamed it.
So now it is called task Server.
And in the previous laboratory lesson.
So when we developed the simple action server we have used this interface here.
So the Fibonacci interface that we have declared in the Arduino bot messages package.
Now for the purpose of this lesson.
So for developing the task server we are not going to use anymore this interface.
But we need to develop a new action interface.
Actually, we can develop this action interface still within the Arduino bot messages within the action
folder.
And here, if you haven't followed me in the previous Python lesson, you need to create the Arduino
bot task dot action.
So this will be the new interface that we are going to use to interact with the task server.
Then within this Arduino bot task action.
So the goal is simply an integer that indicates the task number.
And so this will be just a number that indicates which is the task that we want to execute on our robot.
So there will be a limited number of tasks that our robot can execute.
And so this one here indicates which of these tasks we want to execute.
Then we have the result.
So the result of this interface is just a boolean message that indicates whether the execution of the
action server.
So the task server was completed successfully or not.
And then a feedback message that actually we are not going to use in this lesson, but that you can
use for your own tests.
That is the percentage.
And so this, for example, can contain the percentage of completion of the action.
So of the task.
Then once you have created this Arduino task interface you also need to install it.
So within the Cmakelists.txt.
And so here after the Fibonacci action we also need to install the Arduino bot task action perfect.
So let's save also this file.
So this make list.
And then in order to start using it.
So to start using the Arduino bot task action here in the C plus plus file.
And to get also the auto completion of Ros2 in Visual Studio Code as we have declared a new interface.
So we need to go into the Arduino bot workspace and we need to build it.
So before using actually the interface we need to build.
And so to compile this action message interface, and this will be compiled then into the C plus plus
header that we can use here that we can use in our task server.
C plus plus.
Perfect.
So now once we have compiled it we can import.
So we can include here from the Arduino board messages.
We can import the Arduino bot task dot http.
And now we can start using it.
So we can start using this interface here.
So the Arduino bot task instead of the Fibonacci interface.
So also in this case whenever we use the Fibonacci we can do a Ctrl f and we can replace the Arduino
bot task.
So this is the new name of the interface.
And so let's replace all the occurrences.
Perfect.
So now we have created our action server that is called task Server.
And it implements this interface here.
Now we can implement the actual logic of the server.
So the one that will allow us to move the robot into a predefined position, depending on the goal that
we received.
And in order to implement all of this logic, we are going to use the C plus plus Moovit API.
So let's include them.
Let's do it here.
So let's include from the Moovit package and then from the move group interface.
Let's include the move group interface dot h.
And here we can use actually this class here.
So we can use this header file in order to create a new instance.
So here among the private variables we can create a new instance from the move it.
And then from the planning interface we can create a new instance of the move group interface class.
And let's call this one our move group.
And we can also create a second one that is for the second move group that we have defined in the move
it configuration for our robot.
That is the gripper move group.
Perfect.
And actually, let's declare a shared pointer to an object of this class with the STD shared pointer.
So a shared pointer to an object of type move group interface.
And now that we have declared these two objects.
So these are two pointers.
We can actually initialize them within the execute function.
So here actually we can remove all the code here that we developed in the execute function.
So up to this point and this was the code that we developed for the implementation of the Fibonacci
sequence.
And here let's check if the this variable here.
So this pointer here the arm move group is not initialized.
Let's initialize it.
So let's set the arm move group.
Let's initialize it with the make shared.
And here among the angular parentheses we need to indicate the type of the class that we want to initialize.
And so it's a move group interface object.
So let's copy it here.
And now within the constructor of this class.
So basically here among the parentheses we also need to indicate the implementation of the node that
it's going to be used in order to interact with the move group interface.
And actually as this class here that we are creating.
So the task server inherits from the Klccp node.
So implements a Ros2 node, we can simply provide the shared from this function.
And so basically this will create a node from the current class.
And also still the constructor of this class also takes the name of the move group that we want to initialize.
And this is the arm move group.
So we are initializing the arm.
Let's do the same also for the gripper.
So we can copy these instructions.
And we can paste it.
And we can check that the when the gripper move group is not initialized let's initialize it.
So let's set the gripper move group as a new shared pointer.
So an object of this type.
And this time the move group that we want to initialize is the gripper.
So now this one will have an instance of this class here that points to the gripper move group.
And this one here instead will have an instance of the move group interface that points to the arm move
group.
Perfect.
Now let's also continue with the other callback functions that are executed whenever.
For example, the action server receives a new goal or whenever it receives a new cancellation request.
And actually here in the goal callback function.
So we can leave it this function here as it is.
So basically we are automatically and always accepting here the message that we are receiving.
And here let's just change the message that we are printing in the terminal that says receive the goal
request with.
Task number.
And here for the goal let's print the task number.
So if you remember within the Arduino bot task action interface, the goal message just contains the
task number.
So here when we receive a new goal, we are just printing the task number that you are going to execute
and automatically we are going to accept and to execute it.
Then for the cancel callback function.
So for this one here we can also leave everything as it is.
The only thing that we need to change is here.
So after printing the message in the terminal we can check if the arm move group exists.
So if this one is initialized eventually we want to stop the robot.
So we want to execute the function stop.
So we want to call the function stop on the arm move group.
And so for example if the robot was moving with this function here we are stopping it.
And also let's do the same for the gripper.
So if the gripper is moving.
So if the gripper move group exists.
So it means that we have initialized it here.
And so it means that the function here execute is running.
Then we want to stop it.
So we want to stop the robot eventually if it was running perfect.
And so with this we have also completed the cancelled callback function.
Then let's move on to the accepted callback function that is executed right after the goal callback
function.
So this is the first one that is executed.
This is accepting the goal.
And then once this one accepts the goal.
So this one is executed.
And this function here, actually we can leave it as it is.
And so it simply returns immediately and starts this one here.
So it starts this function in a separate thread.
And so actually this function here, the execute function is the one that will contain the body, the
one that will contain all the logic that will execute the action server.
And so that will interact with the Moovit API in order to move the robot to the desired position.
So here, After printing the message in the terminal that we are executing the goal, we are initializing
the two move groups that we want to use in order to interact with the Moveit API.
Perfect.
And then let's also create the result message that we want to return to the action client.
So once the execution is finished, we need to return a result message.
So a message of this type here.
And so let's create it.
Let's call it result.
And so this is an instance with the make shared perfect.
So this is an instance from the Arduino bot messages and from the action.
This is an instance of the Arduino bot task interface.
And actually we just want that result message from this interface.
Perfect.
And so with this one we have initialized this message that we are going to return at the end of the
execute function.
Perfect.
And now, as we have already initialized the Cplusplus Moviethe interface, we can start using it to
send comments to the robot.
To do so.
So to set the comments, the position of the joints that we want to send to the robot.
Let's create two new vectors of doubles.
And let's call the first one our joint goal.
And let's also create a second one called gripper joint goal.
And so as the name of these vectors suggests, we're going to use this one in order to set the desired
position of the joints of the arm, and this one here instead to set the desired position of the joints
of the gripper.
Now here.
So again back in the execute callback function based on the task number that we have received.
So based on this number here that we have received in the goal message, we want to execute a different
movement.
So we want to bring the robot to a different position.
So let's check if the goal handle that we have received in this function.
And then let's get the goal.
And then let's access to the task number.
So let's check if this task number is equal to zero.
Perfect.
So we are executing the task number zero.
And for this task for example let's move the robot to the home position.
So let's set this vector here.
So the arm joint goal here let's set the arm joint goal equal to the vector zero zero and zero.
So as you might remember the r move group only contains three joints, so the one of the base of the
shoulder and the one of the elbow.
And so we are setting the position of the three of them to the position zero.
So this corresponds to the home position.
And then for the gripper.
So here this one is gripper joint goal.
This one only contains two joints.
And for example in the on position let's set the gripper opened.
So let's set the joint for to -0.7 and the joint 5 to 0.7.
So actually these two joints will have the same value.
So the two fingers will be open the same quantity.
And one is the opposite sign.
So one has the opposite sign as it will rotate in the opposite direction.
Otherwise if the task number that we have received.
So let's copy this one and let's paste it here.
So if we received the request to execute the task number one.
So in this case, we can bring the robot to an hypothetical picking position.
So we can copy these two instructions.
Let's paste it.
And let's say that our picking position is the angle -1.4 for the base.
Then -0.6 for the shoulder, and then -0.07 for the elbow and the gripper.
So in the picking position let's simulate that the gripper is actually grasping an object.
And so let's close it.
So let's close the gripper.
So let's bring it to the position zero zero.
Otherwise if we received a goal with the task number two perfect.
So in this case let's execute.
So let's bring the robot to a different position.
Again we can copy these two instructions here.
And let's say that in this case.
So when we receive the request.
So the task number that is the number two we want to bring the robot to a hypothetical rest position.
And let's say that this rest position is at -1.57 for the base.
Then we have zero for the shoulder.
And then we have -0.9.
And so I calculated already this position here that corresponds to the robot that is bent on itself.
So it's the rest position of the robot.
And in this rest position let's also set the gripper closed.
So let's set the value to be zero zero.
Finally here if you want you can continue by adding new tasks to your robot.
So for example what to do if you receive a task number three.
And so here you can add the code.
So the position of new tasks for your robot for the moment.
So for the purpose of this lesson I'm just going to stop here.
So we will just be able to send one of these three tasks to the robot.
So zero 1 or 2 and otherwise.
So if you set a task number that is different from these three here, let's just print an error message
in the terminal with RCL CP error.
Perfect.
Here.
Let's get the logger and let's print the message invalid task number perfect.
And here as we don't know.
So as we don't recognize the task number let's simply return so we cannot execute it.
Perfect.
So after this statement so after this control here that we are doing on the task number, basically
we have this variable here.
So the arm joint goal.
So this vector and also the gripper joint goal are fully initialized and contain the desired position
of the joints of the arm and the gripper.
So now, before setting this position here as the target goal, let's first set the start position.
So let's access to the R move group.
And here let's use the set start state.
Perfect.
And here we can simply use the R.
Move group.
And we can use the function get current state perfect.
So with this instruction basically we are setting the start state for the arm move group.
And we are setting it equal to the current state of the arm itself.
So we are simply setting the start position that corresponds to the current position of the arm.
Let's do the same also for the gripper.
So let's copy this line here and let's use the gripper move group.
So let's set the start state.
Also for this move group to be the gripper move group get current state.
Now after setting the start position let's also set the target position.
And so we can take again the arm move group.
And we can use the function set joint value target.
So here as the name of this function suggests we are setting a joint value.
So we are setting a value for all the joints that belong to this move group here.
And actually this value is here in this vector here.
So let's copy it and let's paste it.
Now basically with this we are setting one of these three values.
So one of these three vectors as the desired state.
So as the target state of the art.
Let's do the same also for the gripper.
So let's take the gripper move group.
And let's set the target value to be the gripper joint goal.
Now this function here.
So the set joint value target returns a boolean.
So let's store it in a new variable.
And let's call the first one r within.
Bounds.
And so this boolean basically is checking whether the joint value that we have set for this move group
here are within the boundaries.
So the mechanical boundaries that we set for our robot and also this one checks the same for the gripper.
And so let's call this variable gripper within bounds.
Perfect.
And so this stores the output of this function.
And now let's check If any of these two.
So if any of these two variables is equal to false.
So if this variable here is equal to false or this variable here is equal to false.
In this case it means that we are not able to set one of these two positions.
So we are out of the boundaries of the mechanical boundaries of our robot.
So in this case let's print an error message again with RCL CP error here.
Let's get the logger and let's print the message target.
Position out of boundaries.
Perfect.
And in this case, as we are not able to execute this position here.
So as we are not able to execute the desired target position, let's simply return like this.
So we are not going to execute anything.
Perfect.
So after this if statement, we are sure that the joint value target that we have set for our robot
are actually reachable.
And so we can start planning.
So we can start planning a path that brings the robot from its current position.
So from the start state to the desired joint state so to the desired target.
So in order to do so we can create two new variables of type plan.
So from the movie namespace and from the planning interface let's create an instance of the move group
interface plan class.
And let's call the first one our plan.
And let's also create another one called Gripper plan.
And so, as you can imagine, these two will contain the output of the planner.
Both for the arm and for the gripper.
Now in order to execute the planner.
So in order to ask the planner to calculate a trajectory from the current state.
So from the start state to the desired joint state.
So to this one here we can simply use the arm move group.
And on the arm move group we can simply plan and we can store the output of the plan for the arm within
this variable here.
So within the ARM plan let's do the same also for the gripper.
So we can copy this one.
And we can paste it for the gripper.
And let's store the result within the gripper plan.
Now as we already know here, this plan function returns actually an error code that defines whether
the plan was successfully or not.
So here let's check whether the arm.
So the plan for the arm was executed successfully.
So we were able to calculate a plan.
And in this case the error code that is returned by this function is of type.
Move it.
Code.
Move it.
Error code of type.
Success.
So if the result of this one is of type success.
And also let's check this a.
So let's check the result for the plan of the gripper.
Now let's store this one into a new boolean variable that we can call our plan success.
And so this takes the value of this comparison here.
So this one will be set to true if the result of this function here is equal to success.
Let's do the same also for the gripper.
So let's copy this one and let's paste it and let's call this one.
So this new variable gripper plan success here.
Let's close the parentheses and let's close the line.
Perfect.
And now here within the arm plan success.
We have the result of the planner for the arm move group.
And here we have the result of the planner for the gripper.
So let's check if the planner was successfully able to plan a trajectory for the arm.
And also it was successfully able to plan a trajectory for the gripper.
So in this case we can actually execute the trajectory.
And in order to execute the trajectory, we can access the arm move group.
And we can simply execute the move function.
And we can do the same also for the gripper.
So we can paste this one.
Let's change the name to be the gripper move group.
And so actually this instruction here will execute this plan here.
So the plan that was calculated at this step otherwise.
So if any of the two planners so the one of the arm or the one of the gripper fails, let's print an
error message in the terminal with rcl cpp error.
And this message says one or more planners failed.
Perfect.
And in this case let's simply return.
So we are terminating the execution of this script finally.
So after this if else statement.
So this means that we have successfully executed the movement of the robot.
So we have successfully moved the arm to the desired position and also the gripper to the desired position.
So we can finally set the Result variable.
So this one here that we have created.
And as you remember the result variable just contains a boolean that is called success.
And let's set this one equal to true.
Perfect.
Then we can take this variable here.
So the goal handle that is here.
And we can set this one as succeeded.
And this returns the result.
Perfect.
And so actually that's it.
So this will set the result.
So the success to true.
And this will return this result message to the actual client that actually requested the execution
of the action server.
So we can save this file.
And with this we have completed the implementation of the task server.
So now we are missing just to declare the dependencies that we have introduced.
So all of this that we have introduced for this node.
And so we need to declare them here within the Cmakelists.txt.
So here we need to add the dependency from the rcl cpp package.
Then if you haven't followed me in the Python laboratory lesson, you also need to add the dependency
from the Arduino bot messages.
Also, we need to add the dependency from the rcl cpp action and then the one from the cpp.
Components.
Perfect.
So here the one for the Arduino bot messages.
And also we need another dependency from the move it for us planning
interface.
Perfect.
And so actually these are all the dependencies that we have declared and that we need in order to correctly
compile the task server.
Now in order to install and to compile the task server, we can copy actually the same instructions
that we've used here within the Arduino bot CP examples.
So within the CMake list of this package.
So let's open it on one side.
And as we can see here, all of these are all the instructions that we used in order to compile and
install the simple action server.
So we can copy them and we can paste it here and here.
Actually the change that we need is to rename the name of the executable and the name of the library
that we are creating.
And so this is the task.
Server.
And then the name of the script is the task server CP.
Then let's also add the dependencies for the task server.
And also here let's also rename this one into task server.
Also this one here it's the task server.
Perfect.
Here, let's add the dependencies for the task server that are the Arduino bot messages, our Clcp or
Clcp actions components.
And also we need to add this dependency here.
So remove it Ros planning interface.
So let's copy it.
And then also let's register the component again for the task server.
And the plugin is the Arduino bot remote.
Again here we are.
So the name of the class is the task server and the executable.
So the node that actually we are going to execute let's call it.
Task server node.
Perfect.
And so with these instructions here we have simply compiled our task server.
And now we also need to install it.
So if we open this one here we can see that here with these instructions we have installed the simple
action server.
So let's copy it and let's paste it here.
So let's install the.
Task server.
We are just missing one thing that is to declare all these dependencies.
So all the ones that we have declared in this list also in the package dot XML.
So here let's add a new dependency from the RCL cpp, then another one from the clcp action and from
the Clcp.
Components.
Perfect.
Then we need the dependency from the Arduino bot messages.
And also we need the dependency from the move it Ros.
Planning.
Interface.
That's it.
Perfect.
So let's conclude this lesson by saving this file.
And let's build our workspace in order actually to check that we have not made any mistake.
So let's open a new terminal.
Let's go to the workspace and let's compile it.
So let's build it.
And so this is compiling the new node that we have created within the Arduino bot remote package.
Perfect.
So there are no errors.
This means that the compilation was successfully.
And so we can start already executing our task server node.
And in the following lesson we are going to launch it.
And so we are going to see how it works and how we can use this task server in order to provide an interface
that will allow us to move the robot to a predefined location, so to a predefined position depending
on the task number that we set.