In this laboratory lesson, we are going to create a simple script, a simple C plus plus node that
uses the movie two API to send a goal to a robot and move it to the desired position.
So let's go back in Visual Studio Code, and since this is a simple script that you can use as a template
for implementing your ideas with the movie C Plus Plus API, we will place it inside the Arduino bot
C plus plus examples.
So within the source folder here, let's create a new file called simple move it interface dot cpp.
Here there is a typo interface.
Perfect here.
As always, let's start by include from the cl, cpp, the CL, cpp, Dehp that we are going to use
in order to interact with all the Ros two functionalities from this C plus plus script and then as the
node that we are going to create is very simple, we don't need to create a class, but we can directly
insert everything.
So all the code inside the main function.
So let's create the function main.
And here let's return zero.
Perfect.
And now in this function as usual let's start by initializing the Ros2 interface.
So here let's use the rcl cpp init function.
And let's provide the arguments of the main perfect.
And then also we can create a new shared pointer so we can create a new node.
And so first let's also include the memory package.
And then let's use it here in order to create A shared pointer.
To an object of type rcl.
CPP node.
Perfect.
So we are creating a new Ros2 node.
And let's simply call this one node.
And we can initialize it with the Clcp node Makeshared.
So with this function here we are initializing a new node and we need a function Makeshared.
So here we need to provide the name of the node.
So for example let's call it simple move it interface.
Perfect.
And now the only thing that actually we want to do in the main function is to execute another function
that we are going to define soon.
That is the move robot function.
So basically this function here will be in charge of executing the movement of the robot.
So of managing the Ros2 API for move it to and move the robot to the desired position.
And this function here that we're going to define soon will also take as input the node.
So this object here that we have created and after its execution.
So after having moved the robot we can just shut down.
So the clcp shut down.
Perfect.
So we can just shut down the Ros2 interface.
Perfect.
So the only thing that is missing now is to declare this function here.
So the move robot function, let's define it.
And this function will return void and will take as input a const shared pointer to an object of type
clcp node.
So it will take basically a node as input.
That is this object here.
Perfect.
Now within this function we want to interact with the move it C plus plus Cplusplus API, and to do
so we need to import another library.
So let's include from the movie package and from the move group interface.
Let's include the move group interface dot age perfect.
And so now from this file here.
So from this header file here we can create two new instances of the movie.
So from the movie namespace and from this namespace from the planning interface we can create an instance
of the move group interface class.
And actually we need to create two instances of the move group interface class.
And the first one.
So this one here let's call it our move.
Group.
And the second one is that.
So this one here, let's call it gripper.
Move.
Group.
Perfect.
So both of them are instances of the move group interface.
And here this class here.
So the move group interface class allows us to access and to send commands.
And also to view the status of a specific move group in move it.
If you remember, in fact when we configured move It for our robot, we defined two main move groups.
That is one for the R, which we called simply R, and the other one for the gripper, which is that
we called gripper.
So now with this interface here.
So with the move group interface class we can access.
So separately the arm move group and the gripper move group.
And so basically this means that we can send commands to the arm move group.
And also to execute them Independently from the gripper move group, so we can move the arm in a way
and then move the gripper in another way.
And so we can plan trajectories for the two move groups independently.
Now the constructor of this class.
So the constructor of the move group interface class takes as input the node.
So the instance that we have received here from the move robot function.
And also here let's provide this node here that we have initialized here.
Also the constructor of this class here takes a second input.
That is the name of the move group that we want to take.
And in this case so for the arm move group we want to access to the move group that we have configured
and that we have called R.
And for this second one.
So for this second instance instead we want to access to the gripper move group.
So to the move group that we have called gripper perfect.
Now that we have the two move group interface objects we can use them to actually send goals to the
robot.
And in this case, in order to send a goal, we want to set the desired position of each of the joints
of the robot.
So we want to specify the desired position that each of the joints of the arm and of the gripper needs
to reach.
So this is expressed with two vectors.
So let's create a vector of doubles.
And let's also create another one.
So we can copy this one.
And we can paste it here.
And let's call the first one.
So this one our joint goal.
So this will contain the desired position of all the joints of the arm.
And for the moment let's just set this one with an empty list.
And then let's also create the gripper joint goal.
And also in this case let's initialize it with an empty list.
Now for the arm.
So here as you will certainly remember, the arm is composed of three joints.
So the move group that we called arm is just composed of three joints that are the joint one, joint
two and joint three.
And for the goal for example, let's just set that we want the first joint.
So the one of the base to rotate 90 degrees.
And then we can just set all the other joints to zero.
So the desired goal for the arm will just rotate the base of the arm.
And for the gripper instead.
The gripper is only composed of two joints, the joint four and the joint five.
So the two fingers of the gripper.
And also, as you might remember, the two grippers were not moved separately, but they will move together.
So for example, let's open a little bit the gripper.
And let's set the joint 4 to 0.7 and the joint 5 to 0.7.
So as you can see they will move the same angle but in the opposite direction.
So the overall behavior of the gripper will be that the two fingers will open.
Perfect.
Now that we have created the desired goal, so the vector with the desired position of the joints of
the robot, we can move on.
And so we can set this one to the robot.
To do so, we can access to the R move group.
And we can use the function set joint value target.
And so we can also use the same function also on the gripper move group.
So let's copy this one and let's paste it for the gripper move group.
And so for the arm move group we want to set this one.
So this vector here as the joint value target.
So as the goal.
And this one here instead is the vector.
So is the values that we want to set for the gripper.
So for the two joints that actuates the gripper.
Perfect.
Now this function here.
So the set joint value target returns a boolean value indicating basically if the goal that we have
set.
So if this position here is actually reachable so is within the operational limits of the joint.
So for example if we set this one too big or too small, it means that it's out of the mechanical limits
of the robot.
And so this function will fail.
So let's store its result in a new Boolean variable that we can call r within bounds.
And also as you can imagine we can copy this same instruction.
And we can create a new variable called gripper within bounds.
And so basically these two boolean variables we can use now in order to check whether the goal that
we have set is actually reachable by the robot.
So if the arm is not within bounds or the gripper is not within bounds.
So if one of these two conditions is not verified, this means that we are not able to actually plan
a path for the joint target.
So for the goal that we have set.
And so in this case we just want to print a warning message.
So the goal is not achievable by the robot.
So we can use the Clcp warn function.
And then the Clcp get.
Logger function.
So let's get the Clcp logger.
And here the message that we want to print is target joint position.
Where outside of limits.
And so let's just inform the user that the joint position are actually out of the limits and that this
can incur in an error.
And so let's just return.
So let's just terminate the execution of this move robot as we are not able to reach this position at
this point once we have verified the feasibility of the goal.
So with this if statement, we can finally request move it to plan and execute the trajectory that brings
our robot from the start position.
So from the current position to the desired position that we have set here.
Perfect.
In order to do so, we need to create two instances of the plan class.
So from the move it namespace and from the planning interface, let's access to the move group.
Interface.
And let's create a new instance of the plan class.
Let's call the first one our plan perfect.
And then we can also create a second instance.
So we can copy this instruction here.
And we can paste it here.
And this one is for the gripper plan.
So gripper plan.
Perfect.
Now to plan a trajectory that brings the robot from the current position to the desired one.
It's very simple.
And we just need to take this one here.
So the arm move group object.
And we can just call the function plan.
So automatically this function here will plan a path from the current position of the robot to the desired
one that we have set here.
And also let's do the same for the gripper.
So let's plan the trajectory for the gripper move group.
And actually let's save the result of this plan.
So let's save the result of this function here within the ARM plan variable.
And for the gripper let's store it here within the gripper plan variable.
Now we want to check whether this function here.
So whether the plan function was executed successfully.
So we want to check whether actually move it was able to plan a trajectory from the current position
of the robot to the desired one.
So we want to check that the result of this function is of type.
Move it core.
Move it error code.
And this is of type success.
So we want to check that.
Actually the result of this plan is a success result.
So we have a success error code.
And now we want to save.
So this equality here.
So this control we want to save it into a new Boolean variable.
And let's call the first one.
Our plan success.
And so this is the result of checking whether the result of the plan was actually this error code.
So it was actually successful.
And let's also do the same for the gripper.
So let's create a new boolean variable called gripper plan success.
And this contains the result of the planner.
And so now we have these two variables.
So the arm plan success and the gripper plan success.
That will be set to true if the result of the planner is success.
Perfect.
So now we can use these two variables in order to check whether the planner was successfully executed.
So if the R plan success and the gripper plan success.
So if both of them succeeded, it means that actually we are ready to execute the plan that was calculated.
So the plan that brings the robot from its current position to the desired one.
And this is as simple as taking the arm move group and executing the move function.
And we can do the same also for the gripper move group.
So we can execute the move function.
So basically automatically this function here.
So the move function that we are calling on both the move groups will execute the plan that was calculated.
So this one here for both the arm and the gripper otherwise.
So if the planner so one of the two planners either the arm or the gripper was not successful.
It means that we cannot actually execute.
So we cannot actually move the robot in the desired position.
And so in this case let's print an error message with rcl cpp error.
And now let's use the rcl cpp get Logit function in order to access to the clcp logger.
And then here let's print the message.
One or more planners failed.
So in order to inform the user that we cannot execute the trajectory because one of the planners failed.
And so this is probably due to the fact that the start pose of the robot is invalid, or the target
pose of the robot.
So this one here is invalid.
And in this case, let's just terminate the execution of the script.
Perfect.
So we can save this file.
And with this we have completed the move robot function that interacts with the move it C plus plus
API.
In order to set a goal both for the arm and for the gripper, then plants a trajectory that brings the
robot to the desired goal and then also executes the planned trajectory.
Perfect.
So now we are missing to install this script here.
So to compile and install it.
So in the file cmakelists.txt.
Let's start by adding a new dependency that we've used and is the dependency from the move it Ros planning
interface package.
So this one here.
And then we can copy these two instructions here.
So the add executable instruction.
And let's paste it here in order to install the new executable that we are creating.
And this is the simple move it interface.
Perfect.
And this executable here is declared in the source folder.
And in the simple move it interface dot cpp.
Let's also add the dependencies for this script that are just the Clcp library and also this one here.
So the move it Ros planning interface.
So let's add it here.
Perfect.
And in fact as we can see from the C plus plus these are the only two dependencies that we have used
for our script.
Also, once we have compiled our node, let's also install it so we can add it here in the install instruction.
And so let's install the simple movie interface executable.
And then let's also add this executable here that we have compiled in the install instruction.
So here so that it is installed and we can actually start using it.
So let's save this file.
Don't forget to add the missing dependencies also to the file package dot XML.
So here we need to add a dependency from the package.
Move it Ros planning interface.
Perfect.
Since we already have the Clcp dependency, let's save also this file.
And now to conclude this lesson, let's just build the workspace in order to check that actually there
are no errors.
So let's go to the workspace and let's build it.
So this will compile the new node that we've created within the Arduino bot CP examples.
So now it is compiling it.
As we can see here there is an error.
And this is because there should be a typo somewhere.
So let's open Visual Studio Code.
And here we can see that actually there is a typo here.
So this is gripper.
So this is the name of this variable here.
So gripper plan success.
So let's save it and let's give it one more try.
Here we go.
Now the build was completed successfully so there are no more errors.
And in the following laboratory lesson we are going to start this node.
And we are going to see how it will plan a trajectory that moves the robot from the current state to
the desired state.