To create the task server, which is the action server that executes various actions on the robot.
We need to create a new Ros2 package inside our workspace.
So let's open a new terminal.
Let's go to the workspace and to the source folder in which we have already created several packages
that implements several functionalities for our robot.
And here let's create a new package with the command Ros2 package create.
Then let's select the build type.
So the build type that is ARM and CMake as we are going to use both Python and C plus plus code in this
package.
And then let's insert also the name of the package that is Arduino Robot Remote.
So we're going to call this new package Arduino Robot Remote.
And this will contain all the remote interface with the Amazon Alexa voice assistant.
So let's press enter.
And now if we take a look at the packages, we can see that there is the Arduino bot remote package.
So let's go back and let's build our workspace in order to compile also the new package.
So here the Arduino bot remote.
And now back in Visual Studio Code we can create the task server that we are going to use to interface
with the Moovit API, and also with the Alexa voice assistant.
So here in the Arduino bot remote package, let's create a new folder called Arduino Bot Remote.
And now within this folder let's create a new empty file that is called init dot pi.
And then let's create also another file, another Python file.
So the one that will actually contain the code for the task server.
And let's call it task server dot Pi.
Perfect.
And now in this file here.
So in the task server we want to develop an action server that actually offers several tasks to move
the robot.
So instead of implementing the action server from scratch, we can copy actually the implementation
of the action server that we did in the Arduino Pi examples.
So this one here, the simple action server.
So let's copy all of this and let's paste it here within the task server.
Now the first thing that we want to do here is to rename the class from Simple Action Server.
That is the one that we used here.
So let's do a Ctrl f.
And here let's replace the simple action server into task server.
And so let's replace all the occurrences.
Then let's also rename here the name of the node.
It's also task server perfect.
And also we can change here.
This is not anymore the simple action server but it's called task Server.
So let's also change the name of this variable here when we are creating the instance of the task server.
Perfect.
And now let's initialize the action server so we can still initialize it with the action server class.
So as an instance of the action server class and now the name.
So this one is the name of the task server.
And let's simply call this one task server.
Perfect.
And also we need to change here the communication interface that we want to use in order to contact.
So in order to send a goal to this action server.
So in the previous lesson we have used this one here.
So the Fibonacci interface that we declared in the Arduino bot messages.
And now for the purpose of this lesson we are going to need a new interface.
So the interface with the task server.
Again we can declare it here within the Arduino bot messages package.
And here within the action folder here let's create a new file called Arduino Bot task dot action.
Perfect.
And now here we need first to define the goal message.
So the goal interface that's the action server will receive.
And this will just contain a single number.
So the ID of the task that we want to execute.
So this is an integer.
And let's call it task number perfect.
And actually that's it for the goal message.
Then let's use three dashes in order to insert the result message.
And the result message just contains a boolean that is the success.
So this variable here will contain a boolean that indicates whether the action server completed the
action successfully or not.
And finally, let's insert a feedback message that actually we are not going to use, but that I leave
as an exercise for you to implement that just contains another integer and that contains the percentage.
So this is the completion percentage of the task that is executing the robot.
Now once we have declared this Arduino bot task action, we can install it.
So we can go to the Cmakelists.txt file.
And here along with the action Fibonacci that we used in the previous lesson, let's also declare the
Arduino bot task action.
So the one that we have just created and actually that's it.
So these are the only instructions that we need in order to define this interface.
So the task interface that will be used by the task server.
Now in order to be able to start using this action interface here in our node, we can simply open the
terminal.
We can go to the workspace and we can build it.
So basically this will install the Arduino bot task interface so that we will be able to use it here
in the task server dot pi node.
Perfect.
So once the compilation is completed we can go back to the task server.
And here from the Arduino bot messages and from the action folder.
Instead of importing the Fibonacci here we can import the Arduino bot task.
And so we can also see that we have the auto completion of Python, and if you don't.
So if you don't have automatically the auto completion of Python, you can use the key combination Ctrl
shift P and then look for Ros update Python path.
So this one here and just click on this one.
So this will refresh the dependencies.
And so then you will be able to see this one here.
So to have the auto completion for the Arduino bot task message now we can use it actually here.
So when we declare the action server now we can start using this one here.
So the Arduino bot task.
Now before moving on to the goal callback function, let's complete the constructor of the task server
by initializing also the Moveit interface.
So the Moveit API that will allow us to send a goal to the robot, and also to plan a trajectory so
we can import from the Moveit planning the.
Let's import the movie py class.
Perfect.
And now also let's import another class, this time from the Moveit core robot state.
So let's import the class that is called Robot State now in order to initialize the Moveit API.
So here within the constructor, let's create a new instance of the movie py class.
Let's call this one Arduino board.
So as the name of the robot and this is an instance of the movie Pi class.
And as you might remember when we developed the simple movie interface here, we need to provide the
node name to the constructor of this class that we can simply call move it Pi.
Perfect.
And then let's also access.
So still here in the constructor, let's also access the two planning groups that we have defined for
our robot.
So the planning group of the R that we can call Arduino robot arm.
And we can access this group from the Arduino board.
We can use the function get planning component.
So this one here and we want to access to the planning component that we simply called R.
Let's also do the same for the gripper.
So we can copy this instruction.
And we can create a new variable called Arduino Bot Gripper and steal from our robot.
So from the movie Pi instance, we can access to the planning component that we called gripper.
Perfect.
So with this now we have initialized the movie Pi class.
And also we have accessed the two planning components.
So the two move groups that are there and the gripper.
Now we can finally implement the actual logic of the server.
So the operation that the action server will execute whenever it receives a new goal.
So basically the logic of the goal callback function actually here.
So the first thing that we want to do in the goal callback function is to print an informative message
in the terminal in which we indicate so the received goal with request with task number.
So we indicate the task number that we received in the goal message.
In fact, as you might remember from the Arduino bot task interface, the goal message just contains
an integer that is the task number.
And so let's print it from the goal handle from the request.
Let's print the task number.
Here there is a typo.
It's number perfect.
And then actually we can remove all of these.
So all of these instructions that instead implemented the logic for the calculation of the Fibonacci
sequence.
So let's remove it.
And instead we are going to insert here the logic of our node that sends a goal to the robot.
So to move it to pi.
And that moves the robot to the desired position.
So the first thing that we want to do here is to get the current state of the robot.
So first the arm state.
And here this is an instance of the robot state class.
And with this we want to access to the Arduino, both variable and we want to get the robot model.
Perfect.
Let's do the same also for the gripper state.
So let's copy this one and let's paste it.
And here this is the gripper state.
Perfect.
And this one here.
So the arm state and the gripper state now contains the current state of the arm and the gripper.
And actually we are going to change this one.
So this instance of the robot state with the desired state.
So with the desired position of each of the joints that belong to the arm or to the gripper.
To do so we're going to need other two variables, other two vectors that we can call our joint goal.
So let's initialize this one with an empty vector and the other one that is gripper joint goal that
we can also initialize with an empty vector.
And we are going to use the numpy library now to fill these vectors.
So let's import it here.
Actually the same library we are not going to need it.
And instead we can import the numpy library as NP perfect.
And so now as I said we can start filling the value of the joints for the arm and for the gripper.
So now what we want actually to do is to set a different value.
So a different position of the arm joint and of the gripper joint based on the task number.
So based on the task ID that we received.
So we want to take the goal handle and the request and the task number.
And we want to check.
So we want to move the robot differently depending on the task number that we received.
So if we received the task with number zero.
So in this case, we want to move our robot simply to the home position.
So we want to set the arm joint goal to a numpy array.
And this numpy array simply moves the robot to the position zero, zero and zero.
So all the joints are back to the home position and for the gripper joint goal also this one.
Let's initialize it with a numpy array.
Perfect.
And in this case.
So in the on position we want the gripper to be slightly open.
So let's initialize the joint for to the position -0.7 and the joint five.
That reflects actually the joint for as the same angle.
So 0.7 but in the opposite side perfect.
And so this is the position in which the robot will move if we send the task number zero.
Otherwise if the task number.
So again this one here is equal to the task number one.
So if we ask the robot to perform the task number one, we can simulate that the robot moves to a hypothetical
picking position.
So in this picking position actually we can copy these two instructions.
And we want to send the robot to a joint position that I have already calculated.
And that for example for the base is -1.4 then -0.6.
And then for the elbow is -0.07.
So this is just values of the three joints that compose the arm and then the gripper.
So in this picking position we want to simulate that the gripper closes in order to grasp an object.
And so we simply set the value of the joints of the gripper to zero.
Then otherwise if we receive a gold handle.
Galindo.
So a task number with the number two.
So if we ask the robot to perform the task number two, for example, we can send it to an hypothetical
rest position.
So we can copy again these two variables.
And this time we can send the arm to a different position that I have calculated.
And that is -1.57 and then zero and then -0.9.
So basically this position of the arm will correspond to our rest position of the arm in which the arm
is bent on itself.
And also in this case let's keep the gripper closed finally.
Otherwise.
So we are here.
If we receive any other task number that is different from these three ones.
Here we simply want to print an error because we have not implemented any other action.
But here, for example, if you want, you can also create new task numbers.
So you can also create and set new tasks for the robot and for the moment.
For the purpose of this lesson, I'm just going to stop here.
And so in this case, I'm just going to print an error message with the get logger.
And let's print an error message that says invalid task number.
So we're going to accept only task numbers that are between 0 and 2.
Perfect.
And in this case, as we cannot execute this task let's simply return.
So let's simply terminate the execution of the action server.
So the execution of the goal callback function.
Perfect.
So now after this function here we have initialized these two vectors.
So the arm joint goal with one of these three values that corresponds to the position of the joints
of the arm.
And also we have initialized this second vector here.
So the gripper joint goal that corresponds to the desired position of the joints of the gripper.
Now it's time to set this target position.
So this is a target position of the arm and of the gripper.
So let's access to the arm state and let's use the function set joint group positions.
And so for the move group that is called arm we want to set the arm joint goal.
So this one here is the desired position of the arm.
Let's do the same also for the gripper state.
So let's set joint group positions.
And this one is for the gripper.
And we want to send the gripper joint goal.
So this value here.
So this vector here that we have created.
Perfect.
So now we have defined the desired state.
So this arm state and the gripper state are instances of the robot state class that contains the desired
position of each of the joints of the robot.
Now it's time to update also the state of the move groups.
So let's access to the move group that we called Arduino Bot R, and then let's first set the start
state with the function set start state to current state.
And so as the name of this function says this will simply initialize.
So we'll set the current state of the robot.
So the current position of the robot as its start state, so that the planner can use it in order to
calculate the trajectory.
So this will be the starting point.
Let's also do the same for the second move group.
So from the Arduino bot gripper.
Perfect.
Let's set the start state of the Arduino board to gripper.
Perfect.
And finally, once we have set the start state, we can also set the goal state.
So again, let's use the Arduino to move group.
And on this one let's use the function set goal state.
Let's do the same.
Also for the second move group that is the Arduino bot gripper.
Perfect.
And here let's set as a goal state an object of type robot state that is the arm state perfect.
And also for the gripper, let's set an object of type robot state that is the gripper state.
So this variable here in which you have set as a joint position one of these three here.
So one of these three positions here depending on the task that we have received.
Perfect.
So at this point our move groups.
So the Arduino bot arm and the Arduino bot gripper.
They have a start state and they also have a goal state.
So finally, we can plan a trajectory that brings the robot from the start state to the goal state.
So again we can use the Arduino bot R and we can simply execute the plan function.
And also we can do the same for the Arduino bot gripper.
And also on this one let's execute the plan function.
This function here.
So the plan returns an object and let's store it within the variable arm.
Plan result.
And also for the gripper, let's store the result of the planner into the gripper.
Plan.
Result.
Perfect.
And now at this point before executing this trajectory so this plan let's first verify that actually
the result is valid both for the R and for the gripper.
So let's check that the arm plan result is valid, and also that the gripper plan result is valid.
And so in this case we simply know that both.
So for the arm and for the gripper the planner was able to calculate a trajectory that brings it from
the start state to the goal state.
And so we can execute it.
So we can take the Arduino board and we can execute.
So we can call the execute function.
And we can execute the planner.
So from the arm plan result we can execute the trajectory that was calculated.
And let's set the controllers to an empty vector.
So like this.
And this will use the default controllers for this move group.
Here let's execute also the second plan.
showed one of the gripper perfect.
And again we're going to execute the trajectory that was here in the result message.
Otherwise if one of the two planners failed.
So if one of these two is not valid here, we just want to set a message in the terminal.
So let's use the get logger function.
And let's print an informative message that simply says one or more planners failed.
Perfect.
And here in this case we just want to print this informative message.
Perfect.
Now after this if statement.
So this means that the path was correctly executed.
And so we can already return.
So remember that here we are in a goal callback function for an action server.
So for this action server here.
So now at the end of the execution of this action Here we want to return a result for the action server.
So first let's set the goal handle as succeed.
So this will inform the action client that actually the execution of the goal succeeded.
And then we want also to return our result.
So let's return a result that is an instance of the Arduino bot task action.
And from the result message.
And in the result message.
As you might remember here we have just the success variable.
And so let's set it the success variable to true.
And so this will inform the client that the action was successfully executed.
And also let's return the result message.
So this one here this instance of the Arduino bot task result message is the one that we are going to
send to the action client.
Finally as this Python script here, we inserted it within a package.
So the Arduino bot remote that we created using as a build type amend CMake.
Here at the beginning of this file, we also need to define the directory of the Python interpreter.
And so this is the.
User bin and the environment is the Python three.
Perfect.
So we can save this script.
And with this our action server.
So the task server that implements this interface here.
So the Arduino bot task interface is completed.
And now we can start using it to send a task to our robot.
And so it will move the robot accordingly.
So based on the task number that we have asked to the robot, before doing so we need to install actually
this Python script here.
So we need to open the file.
Cmakelists.txt.
Here we can remove all the commented lines in order to have a file that we can read more easily.
Perfect.
And here let's add a new dependency from the CMake Python package.
Perfect.
Then let's also add a dependency from the rcl py package that we've used here in our node.
And also we need a dependency from the Arduino bot.
Messages package.
Perfect.
Then after these find packages instruction, we also need to inform the compiler that here within the
Arduino bot remote there will be Python nodes.
So let's use the command Python install package instruction.
And here we need to indicate that the Python script will be in the folder that is called as the package
itself.
So Arduino bot remote that we can also access with the variable project name.
And with this one we have informed the compiler about the existence of our Python node.
And now we have also to install it.
So we want to install.
And here we want to install a program.
And now we need to indicate the directory of the program that we want to install.
And so this is the in the Arduino bot remote folder.
So in the folder that has the same name of the package that we can access with the variable project
name.
And within this folder the file that we want to install is this one here.
So the task server dot pi.
Perfect.
And then we also need to set the destination.
And we want to install it in the lib folder and within the sub folder again that is called as the package
itself.
So again we can use the variable project name.
Perfect.
So we can save also this file.
And we are missing also to declare the dependencies here in the package dot XML file.
So here the first thing that we need to do is to add a build tool dependency from the amend cmake python.
And then we can also declare a dependency from the CLP library and another one from the Arduino bot
messages package.
And then also we need to declare an execution dependency from the move it by package.
And actually this dependency here is a conditional dependency.
So we're going to need this dependency only when this condition is satisfied.
And actually the condition is that you are using a Ros distribution that is bigger than or equal to
ion.
So you are using Ros distribution that is newer than ion, like for example Ros chassis.
You will have this package available.
Otherwise if you are on Ros you cannot actually implement this node here in Python.
But you will just have to implement it in C plus plus because this package here.
So the movie Pi is not available in Ros2 humble.
We can save also this file.
And that's it for this lesson.
In the following lesson, after developing the C plus plus implementation of the task server, we are
going to launch it and we are going to see how it works and how we can send any of these tasks.
So how we can execute any of these tasks in the robot.
So how we can use the movie Pi.
So this API here in order to move the robot to a predefined location.