This is the step number three.
And it's going to be the last step for this activity.
Number four in this step we are going to create a new node for the battery.
We're going to kind of simulate the behavior of the battery to say that it's full or empty.
And when it's full or empty we are going to change.
So we are going to set an LED with the set led service.
And that's why I've created first the LED panel node, so that we have the service that is already working
and tested.
So now when we create a client we know the service is already working.
Okay.
So it's going to be easier for us to test the client.
Great.
So let's go again in our Python package.
So my pi pkg and let's create here another file that's going to be battery dot pi.
Let's also make it executable with plus x.
Okay.
And let's edit it in the code.
So let's get the templates here in battery dot pi.
So that's going to be the battery node.
Okay.
Battery here.
Let's keep things simple.
And I'm going to do this step actually in two steps.
First we are going to kind of simulate.
We are going to fake the mechanism of the battery to say when it's full or when it's empty.
And when that is working we are going to call the service.
So let's get started.
And how can we monitor the battery state?
How can we say that the battery state is empty after four seconds and then full again after six seconds
repeatedly?
Well, first what I'm going to do is I'm going to create a timer and say that we check the battery state
ten times per second.
So usually if you had a real battery, you would probably create something like a timer and then check
the battery state every X amount of time.
Maybe you don't need that much.
Maybe every one minute, every 10s.
Well, here we're going to do it ten times per second.
So let's create a timer first here self dot.
Let's call it battery.
Timer is equal to self create create timer.
And I set 0.1.
So ten times per second self.
And let's create a function here check battery state.
Okay.
And let's provide the callback here.
Check battery state.
All right.
So here you would in a real application directly check the battery state and from that you can know
if maybe the battery is full or empty and call the service.
But now we don't have a real battery.
So I'm going to fake the behavior.
And like I gave you in the activity, we're going to say that let's say the battery is full and then
after four seconds is going to be empty.
And then six seconds later it's full again.
So we have a ten second cycle.
And how can I check this duration of four seconds and six seconds from the callback here so I could
save the time?
For example, I could save the time when the last time.
So the actual the last time that the battery state was changed, and then I could compare with the current
time and see if we have spent at least 4 seconds or 6 seconds.
So here I could start with creating a variable.
Time now is equal to the current time.
How to get the current time.
There is actually something you can use in Ros two, and I'm going to create a new function for that.
Diff get current time.
You will see why in a second.
I want the time in seconds to make it simple.
You can use the self.
So from the node you have get clock and then dot know and then dot sequence nanoseconds.
This will give you a tuple of seconds and nanoseconds two integers.
So for example seconds nano seconds is equal to self get clock now seconds nanoseconds.
All right.
And well we don't really want a tuple like this.
We want a float number.
So let's just do a return seconds plus nano seconds divided by.
And then we need nine zeros.
So Eight nine.
And let's put dot zero.
Okay.
So make sure you have nine zeros 123456789.
All right.
So now we have the current time with a comma with a decimal point.
And I've created a function so that we don't need to do that in the code.
We just get the current time sequence.
And that's going to be easier.
So self dot get current time sequence.
All right.
So every time we get to this callback timer here which is ten times per second, we get the current
time.
And we're going to check with the last time.
The last time what actually.
Well the last time that the battery state changed.
So I'm going to add here self last time battery state changed for example I can initialize it to the
current time get current time sequence.
Okay.
And so I will do if time now minus the last.
So last time the battery state change is greater than 4 seconds or 6 seconds.
Okay.
And how to know which one we want to check.
Well we could create here I'm just going to create a string self dot battery.
Battery state.
Here I'm just going to say full.
Okay.
You could create a number for example that goes between 0 and 100 something like that.
Well here we don't have a battery.
So I'm just going to make a very simple example.
So it's going to be full or it's going to be empty with a string.
And I can say if the self battery state is full.
Then inside this if I add this other if I check the time now minus the last time the battery state change.
So time minus time gives us a duration.
If this is greater than four seconds, then I can say that the battery state is empty.
And then maybe add a log get logger.
That's going to help us to do debug what's happening in the code.
Let's say battery is empty, um, charging.
And there's one more thing we need to do in this case is that, uh, well, we have changed the battery
state, so we need to update the last time the battery state changed.
So self dot last time battery state changed.
This is going to be time now.
So Now that's the time we have right now when we execute this specific callback.
Okay.
So this structure is quite common in programming and in robotics in general.
Okay.
You have a function that you call quite often here ten times per second.
Or it could be much, much faster.
You save the time of an event that you want to monitor.
And if you want to check a duration, then you get the current time and you just do current time minus
last time.
If this is greater than the duration you want, then you can well, you can do an action, change a
state, whatever.
And you keep basically the last time becomes the current time.
Okay.
And we have done half of the code here because the battery.
So it's full and then it's going to be empty.
But then we also need to check else.
So we could do else if uh self dot battery state is empty.
Actually I'm going to keep it like that.
We could just do an else because we will have only two states, but it's probably more explicit like
this.
So if it's empty, I do.
If time now minus self last time battery state change.
And this time we are going to wait for six seconds, then I can do self battery state is full and let's
add another log.
Get logger info.
Let's say the battery is now full and we also do self dot last time battery state change.
This is time now and that's how we're going to fake the battery state.
So this is just my implementation okay.
This is my solution.
Maybe you have come up with a different solution Okay.
I didn't give you so many information about using those functions here and this mechanism.
Okay.
That was to make you think more.
Okay.
That's a real challenge.
You're not going to have the solution to everything you do when you program with Ros2 and when you do
robotics.
So you might have come up with a complete different solution.
But if it still works the same, it's fine.
Okay.
So we have let's make sure we have the timer.
So that's going to get called ten times per second.
And just a quick recap of what's going to happen.
So the battery state is full.
The last time the state changed is right now.
So we're going to save it in the constructor.
Then we call that and then we check.
So we have if the battery is full that's the case.
We're going to check if the time to the current time minus the last time is greater than four seconds.
So we're not going to go inside this until four seconds have passed since basically the constructor
was executed.
And then we change the battery state.
We print a log and we save the current time as the last time.
So then the next time that we enter this check battery state method, we're going to also get the current
time and the battery state is going to be empty.
So we're going to go inside this Elif.
And then we can only enter this if after six seconds.
And when we enter this if we change the battery state to full, we also save the last time.
So basically the current time becomes the last time.
And then again we will go back to this first if okay etc. etc..
So let's try that.
Let's see if it works.
We have some logs that's going to be very useful.
I'm going to save and let's go to setup.py.
And let's add yet another executable here.
Battery is equal to my Python package dot battery because it's battery.py and the main function.
Okay let's build Built that Qualcomm build packages select let's build a Python package and let's also
add Simulink install.
Great.
And let's run the battery node here.
Source workspace and Ros to run my Pi PG with battery.
And let's see what we have.
I didn't have any log in the constructor.
Maybe I'm going to add one.
And you see battery is empty.
Let's wait a bit more.
Battery is now full and you can see the difference of time.
Here is six seconds.
And now we have four seconds okay so let's wait a bit more okay.
And you see we have two full cycle now.
So when we look at the logs you see we have a cycle here 10s.
And then 10s.
We have six seconds between empty and full and four seconds between full and empty.
All right.
So that was the probably the main challenge of this step is to come up with a solution to fake this
behavior.
And now what's left is quite straightforward.
We need to call the set led service that we have created in the LED panel node.
When the battery is empty, we want to turn on the third LED.
And when the battery is full we want to turn it off.
And so we're going to use the basically the same thing we did for the add to insert client.
Okay.
We're going to create a client inside the constructor.
So let's create a client here I'm first going to add a log actually self dot get logger.
Battery node has been started okay.
And here I'm going to create a client self dot set.
LED client is equal to Self-create client and I need to provide a service type.
So I need to do from my robot interfaces dot srv import set LED okay.
And I'm already using my robot interfaces in this package.
I've already set up the package dot XML with the dependency.
So let's put set LED and the name.
So let's make sure we use the same name here set LED.
All right.
So we have our client here.
Then what we can do is we can create a function to call the service.
So let's do that here.
For example the order doesn't matter call set LED.
And I'm going to put self and then led number state.
All right.
And what do I need to do?
Once again, if I come back to this example, I'm going to first wait for the service.
Then I'm going to create a request and send that request.
So let's do that while not self.
And let's set LED client dot wait for service.
With one second we could say self dot get logger one waiting for set LED service.
And you might want to handle that differently.
Maybe because here it's going to block here in this function.
But because this service is quite important for this node, you could also decide to do the while not
here directly in the constructor for example, to check that the service is up before you finish the
initialization of the node.
Okay.
Or here, instead of doing a while loop, you could just return an error if the service is not found.
So there are different ways of handling that.
It's going to depend on your application.
Here.
We just wait for the service to be up and that's it.
Now we can create our requests which is going to be set LED dot request.
And then we do request dot led number, which is the LED number we got from the parameter here.
And request dot state which is the state.
And then we can do so we're going to get a future out of that self set LED client dot call async with
the request.
All right.
So same thing what we did here.
and we can add a Dawn callback.
So let's create a callback actually.
So def callback set led.
So maybe callback call set led.
That's the callback for this method self.
And we get a future.
Okay.
So here let's do pass here.
So we don't have the error.
And we can do future dot add dawn callback self callback call set LED okay.
Just like we did here.
And note that I didn't provide the request.
You could pass the request as well, or you could pass any other parameter.
In this case you would have to use the partial keyword here with the Functools functionality.
I'm not going to do it here.
It's not necessarily useful for this application.
So I'm just going to provide the callback here with We've had done callback and we just receive a future.
So it's a bit simplified and then we can just do future dot result to get the response.
So response is equal to future dot result.
And let's just print this.
So actually this is going to be a boolean.
We can say if response dot okay.
And here maybe I can also provide the type I can say that's going to be set LED um response.
So then I have the auto completion response dot success.
If success self get logger and then info.
And we can say led turned on for example else self get logger info led not changed.
Okay, that's probably not going to happen because we're going to give something that's valid.
But well you can handle the response here if needed and monitor the different errors.
Great.
So let's do a quick recap here.
Once again we have so we have our client here that we have created with the interface and the name.
Then we have a function.
We wait for the service.
We create a request and we call asynchronously the service.
We register a callback that's going to be called when the server replies to the client.
And then we get the response and we just print a log here.
Now the only thing left is to actually call this method.
And where do we call this method.
Well, after we just change the state where we print a log.
And I'm going to do here self dot call set LED.
So what's the LED number?
I start to count at zero.
So that's zero one and two.
For the third LED.
And the state is going to be one.
Okay.
When the battery is empty I want to notify the user.
And then when the battery is full I'm going to do call set LED with also the same index.
And then it's going to be zero for the state zero.
All right.
And let's save.
And that's going to be it for the code.
So we've already tested the behavior here.
Now let's test if the client call is going to work successfully.
So let's go here.
And well I have run with Simulink install.
So it should work.
I'm just going to well I'm just going to run it here and let's do a source bash SC just in case.
Bash SC I'm going to run here the LED panel node which is needed first because, well, we need the
service for the battery.
Okay.
LED panel and I'm going to run my battery node.
So just run my Pi PG with battery.
And actually before that I'm just going to do a Ros two topic eco led panel state to see what's happening
on those LEDs.
So for now they are powered off.
Okay, let's run the battery.
And you see battery has been started.
We need to wait six seconds and you see battery is empty turned on.
And we see that LED is one still one.
And then LED turned uh, on what?
Actually, I probably need to change the log.
Let's go back here.
Well, you could pass the request and know what you have decided to do.
Let's say led state was changed.
And let's run it again.
So we start again.
But reload has been started.
Let's wait for six seconds.
You see, battery is empty.
Charging LED state was changed and then is no fool.
We go back to zero.
Let's wait another.
You see empty is one and then full.
It goes back to zero.
And again and again and again.
That's going to be infinitely repeating the same cycle.
All right.
And that's the end of this activity.
So you have practice here with everything.
You have created nodes.
You have created topics services.
You have run those communications between two nodes and all of that also using custom interfaces.