His duty, a round of applause for everyone following other targets that we achieve in the three stages
that we have completed so far.
We extracted information from our environment.
The first two stages of localization and mapping.
Then we computed the path to go based on that information, in part planning.
Now, in the current coding session, we will be implementing the last bit of robot navigation, which
is motion planning here.
We will be planning about the robot motion or navigation which will take it to the music.
To implement the last stage of motion planning, we will be creating a new file inside the Ms. Vault
folder.
That we name bought.
Motion planning not by and incited.
We perform the necessary inputs.
These are the inputting of Siri to number it in math libraries.
Then we create the class.
I've bought a motion planner that didn't perform the actual tasks of motion planning inside of it.
Then we define the instance variable for this particular class, and that can be done by defining the
initialization function.
And inside it we define the first few sets of instance variable.
These are the first is basically a counter that counts the number of iterations that has elapsed since
the motion planner has begun.
Then we have the state variable or instance variable that basically keeps track of whether the initial
point has been taken or not.
This initial point is basically the car's initial location.
Once the motion planner has begun and then we have the actual container that will store the initial
location of the car.
Once we start the motion planner.
Then we define a new state variable that basically keeps track of whether the relationship of the end
goal for the image and the simulation has been computed or not.
So as this is the initialize and states, this is set to false.
Then we define the three containers that stores the most angle.
The first is the angle in image.
Then we have the what angle in simulation, and then we have the relationship between for the what angle
in image and simulation.
So once we have defined these instances variable for our motion planner class, we go back and create
the actual function that we call or perform the bot navigation for our robot and that will be named
as navigate forth.
So this takes in four input arguments.
The first is the location of the robot or the current location of the robot provided by the local date.
Then we have the bot controlled by the bot planning stage and then we have the velocity object that
stores the velocity components.
That includes the linear velocity and angular velocity.
And then we have the publisher that will publish the velocity once we have modified it.
So we proceed further and start by checking for the counter whether the count or self self-doubt column.
It's great and then 20 or not.
So obviously at the start we have initialize the counter to be zero.
So it is less than 20.
So this would not get true.
So if this goes through, we basically just set our velocity object linear component in extraction 2010
basically telling our car to stop.
So if it is less than 20, what would we do?
We want to move our car.
So we defined that as condition when the counter is less than 20 or or at the start of the motion planet
object.
So here's what we would do.
We will start with checking or extracting the initial location of our car.
So we first check if the self taught.
Initial point has been taken or accepted or not.
So if it has not been extracted, we extracted the initial location of the car.
So that can be done by simply storing initial location and setting this to the location that is provided
as an argument of this function.
Then once we have to get the initial location inside the initial location instance variable, we simply
set the stage variable of point initial taken to true.
And then we simply make our car move forward by setting the velocity object linear component.
In the extraction.
21.0, basically greater than zero, getting it to move forward.
And that can be done by simply calling the velocity publisher, calling its function or publish.
And passing in the velocity object.
This basically moves the car until the counter is is less than 20.
So we basically now see that increment the count on every iteration by one.
So once this counter reaches or 20 or greater than 20, this as would not get to, but this condition
would get to.
And we set the velocity linear component in its direction to 0.0, basically stopping the car.
So once this condition arrives, our counter is greater than 20.
What happens is that we basically start by stopping our car.
So we stop our car by using the velocity publisher.
Calling is a function of public.
And passing in the velocity object that we had just modified to 0.0 by the living component in the X
direction.
Now, once we have stopped our car, we can now compute the angle of our car in the image.
Now that can be done by creating a new function, but computes the angle and distance of the car.
Given the two points that are basically the initial location of the car that you extracted when we started
the motion planning class and then the current location.
So we create the new function.
We go right up here, we create the new function that computes the angle and distance between two points.
So this is just like the normal function that can be used to angular distance between two points.
But just the difference here is that for the normal convention or for the simulation, we have the X
increasing in the right direction and the Y axis increasing the positive or upward direction.
But for the image where we are computing the angular and distance, for this case, our expression is
like the original or normal convention, increasing the right.
But the y axis is in reverse.
It is increasing in the downward direction.
So what we need to do, we need to perform or compute the same trigonometric roles to compute the distance
and angle between two points for the image.
We need to reverse the error for the y axis.
And that can be done by simply subtracting the first point for the y axis with a second point of y axis
and not vice versa.
So basically for the error of by the point E is subtracted from or basically the point be subtracted
from point eight.
So point eight comes first and point we come after for the y axis.
And this is how we compute the same distance and angle for the image and for the distance.
We use the same Euclidean formula and for the angle we use the function of our time, two of the math
library and our time to basically turns the angle between two points and in the form of degree or in
the form of radians.
So we need to convert it into degrees to perform consistency and frame of reference.
So basically we get the angle in degrees.
And because this outputs in the value or in the range of negative pi to positive Y and converting it
to degrees, we get negative one, 80, one, 80, and we want it to the range of 0 to 360.
We convert it from -1 to 182 0 to 360 by simply adding 60 to the negative degree angle values.
So we do that right here.
And what we get as an output is basically the angle between points.
Not from negative one or 2 to 1, but from 0 to 3, 60 degrees, all positive.
And then we also have the distance computer.
So we have the distance computer and now we can call this function inside the navigate part function.
Right after we have stopped the car by saying self dot and going to distance for the point, a provided
the initial location of the car that is basically stored in the instance variable of init location and
then for the current location we pass in the bar location.
So once we provide this information, we have computed the two things, but we only require the issue
that is basically.
The car or the board angle.
The boat angle, which is an image and its distance.
But we only acquired the what angle.
Then once we have the board angle an image, we now require the board angle in simulation.
But first, let me see all this.
What angle?
In another instance variable.
That is the board and in it to print it out later on.
Now we need to extract the board angle in simulation because we have the board angle an image to compute
the board and the relation between image and simulation.
So let's create that particular functionality right about the angle and distance function.
And this is similar to what Mr. Luke wanted to compute at Angle in simulation in his go to go lot by
far.
So we use the ILO from Catalonian, we compute the the same get both and we get the yard in degrees
of converting the your computer from either from Catalonian to degrees.
And then I erred in a few line of code here.
What I did is I basically converted the -180 to 1 into degree range of the your degrees to 0 to 360
to maintain the consistency in frame of reference because the angle computed in image was in 0 to 60
degrees.
So we want the same transitory degree range for our angle in simulation computed from the post or the
get post function.
So basically what we do is take the negative your degrees and simply add in the 360, which really gives
us the total range of 0 to 360 as the final output for the angle in simulation.
So once we have the board angle simulation updated, we can now move to the navigate ball function.
After we have compute the board angle and image, you can now compute the board angle relation and that
is done by simply writing what angle relation right here, the board angle simulation, negative or
subtracting.
The bolt angle in image.
This gives us the boat angulation so now that the boat and the relation.
One thing I missed out earlier is that we create an instance are able to track the angle relation between
the image and the simulation has been computed or not.
So we need to check that before moving forward further.
So we right here, if not self-support or angle relation, computed or not.
So if it is not computed, then we compute this angle.
So once we have completed the board and coalition, then we can see that we have completed the board
and good relationship or the anger or the anger relationship computed to true.
This means once we have the anger relationship computed, we can now use the simulation angle to retrieve
the angle in image for the board.
So we do the same process by simply addressing the case.
Once we have the anger relationship computed, we now use that relationship along with the simulation
angle to compute the bot angle, and that is done very similarly.
We simply say the bot angle simulation and subtract the bot angle relationship.
We have just computed earlier to get the bot angle in image, so we don't need to compute the bot angle.
Now every time we could just use the bot angle in simulation, subtract this from our bot and religion
and we get the bot angle an image which required for consistency of frame of reference.
So once we have this information, we can simply print it out, print out the column location and print
out the initial what angle and this is has been completed.
So once we have completed this, we now have the initial task of computing the bot angle or bot orientation
in the image along with the bot location computed.
So we now have the complete robot post in image and now we can proceed further to create the go to go
function.
To perform the analysis of the first day or the motion black last.
We head on over.
The mist.
Over.
Where we perform the necessary inputs to integrate the motion class to our project.
The first import is basically importing the board motion planner class from the bought motion planning
file and then also importing the dormitory from the navigating message message.
Once we have these inputs, we head on over to the miss of the class where we are inside the installation
function.
We simply perform or create the object or the motion class.
And then we also create the object or the bus subscriber that basically uses the dormitory to compute
the pose of our car in simulation and also on the information of the pose to the function of the bot
motion class.
So once we have these two objects created, we can now head over to the main solving function where
after we perform the fourth planning stage or the third state of our robot navigation, we create or
perform the fourth and the final state of our robot navigation that the motion planner.
So we call them what motion planner function of navigate bot which takes in four input arguments.
The first input argument is basically a bot location or the location of our robot at this current moment.
So this is basically free from the bot localized localized locus location car.
So once we have this bot location passed to this navigate path function, we then head over to the second
input argument of this function that is the bot to go the port to gold was computed by the first stage
or the bot then a class.
So you head on to work with the bot partner.
Well we have the part to go.
At this moment we have not created an instance variable or part to go, so we need to create this instance
variable that basically still was the bot to go for our robot.
So we head on over to the bot planning part.
So here we are, the path planning.
We go to the initial license function where we create the instance variable of part to go and initialized
this to an empty list.
Then we head on over to the find path and display function that's just called from the Salvador part.
Go scroll down to where we have the bot to display and assign its value to the bot to go.
So now our third or part to goal instance where able for the more authentic class now has the path to
display.
So we now head on over back to the solver, assign this value to the bot variable and then pass this
as a second argument or navigate.
But then for the third input argument, we need to provide the velocity object as a third input argument.
And this is basically the velocity message that was created earlier that basically clears the object
of the quiz class.
And then we have the Velocity publisher to basically publish the velocity once it has been updated.
So this is basically initialized right at the top of the instruction function.
And then once we have completed this input arguments, our navigate bot function has been completed
and the integration of the full stage of the motion planner class has been completed.
Now, before testing this out, we need to perform a few more steps.
The first step is that we need to head over and simply close this with keys everywhere they present.
Because for the motion planner to work, we need the cart to move freely.
And this is simply stop the operation at wherever they are placed.
So we simply said turn them off.
By recommending them out.
We head over to Polyclinic where we also have the key.
So we turn this off to we head on over to the bot mapping, we turn this off to we head on over to the
localization.
We don't have the weight key.
So we're back to the MI solver and hopefully the stopping has gone.
So another thing that we need to do, we need to head over to the bot planning or mapping.
What is happening right here is because the graph has been created so it is not needed to be created
again.
So we need to set this self-talk graph from false to true.
So we head on over to down here, set the self taught graphic fight state variable for mapping state
to truth.
So what this will do is basically say that only do the mapping once and not anymore.
So once we have done this, we head on over to the Ms. solver and then proceed on the terminal and then
build the project.
So we build the project.
We run the simulation once the simulation starts.
We head on over to the second terminal and then run the solver.
So once we get the output of the license plate, you press spacebar and then we get the output for our
localization states.
Once more after one complete iteration.
So if you can see right here, we have landed on our error and the error see that we have a type error
that the object is not subscribed to.
So what is that object that is being called to be clear?
So this error is coming at the angry, distant function of the board motion lost.
And this is coming at line 76 where we were computing the error in the x axis.
So basically this particular function was taking in the location of our car in two different intervals
of time.
And the location of the car is basically in the format of tuple and that is subscript over, but it
is getting that into object.
So the error is coming from from somewhere in the localizing states.
So we head on over to the localized state and see whether we are computing the localization or the car
location correctly or not.
So if we head over to the localized state at the local spot function, what we observe is that when
once we have computed the background where we are computing it for for the first citation and then not
proceeding on in the first iteration.
So in the first iteration we are not retrieving the car location.
So what we have initialized our car location to the instance variable.
It is we have initialized this to an empty in in function or an integer.
So this is what is causing the error in the first iteration of our solver type.
So what we need to do need to simply say that once you have computed the background, you don't need
to wait for the next iteration to compute the car location.
You can simply compute the card location in the same migration.
So we.
Delete this else.
And simply take this back.
And then once we have the background extracted, we can now proceed to the second step, the third step,
and eventually extracting the car location.
So the location of the car is extracted on every iteration, even on the first.
So hopefully.
This fixes the error.
So we head on over to the terminal.
Close the simulation.
Will the project.
We rerun our simulation.
And then run the missile over on the next terminal.
And once the missile runs, we get the output of our nuke license state.
You press enter, then we get the output.
For our car.
Localized.
So at this moment.
What is happening is that we are seeing.
That we are seeing some output and this output seems to be wrong.
The problem is that the car hasn't moved from its initial location.
But to compute the car, location or the car, cause we want it to move from its initial location to
some final location.
After moving forward for a few frames, but it hasn't moved.
Where is the problem?
So somewhere in our code we have reset our velocity of our car back to zero.
So we close this down, close the simulation head on over to the Ms. Solver and voila, right at the
end of our Ms. solving function, we are simply resetting our linear component of our velocity message,
drop back down to zero and then publishing is using the velocity publisher.
So even though in the ocean planning it was setting it one for first 20 iteration, we are resetting
it to zero right over here.
So this is causing our car to not move and getting incorrect car orientation computed.
So we need to delete this line of code because we do not need it anymore.
So hopefully once we run this again, we get the correct output.
So we run the simulation again.
Once we have the simulation run up, we the solar.
And once we have the output of the locations, do you press spacebar?
And this time.
You can see that God was moving from this initial location to the final location.
And this time, if you look at the output, we get the output of the car angle initial.
So the initial angle that was computed from the initial location to the final location was to 70 degrees.
And the car angle in simulation was basically at this moment is to 70 degrees two.
So it hasn't changed because we're just moving in the forward direction with no change in angle.
So the car angle in simulation is also to 70 degrees as as of this moment.
And the car computer or the car angle for the image computed from relation is basically to 70.6 degrees.
And the relationship between the image and simulation for the car angle is basically just negative point
one, two degrees.
So using the relation, we're now able to compute the car angle and image from just the simulation using
the relation.
So car location at this moment is basically 300 pixels to 97 pixels in x and y axis.
And if we head on over to the simulation.
And.
Pick our terminal right on the top.
Set it to always on top.
We can now modify of a car, location or orientation and see whether it changes or not.
So if I changes.
Left facing to the wall.
He see that the angle computed for the car.
It's basically 16 degrees.
So at this moment, the car is 16 degrees.
And if I increase its angle.
To face to means entry.
It increases to 90 degrees and then I increase this other dimmer phase opposite to the wall.
It now goes to one or two degrees.
So basically it is increasing from the left facing the wall.
It is zero.
Then it increases to one or two degrees and then once it gets goes opposite and once it goes opposite
to the wall through the entry, it basically increases to 270 degrees.
And then going back to the wall, it increases to 360 degrees.
So 0 to 3 is 60 in this direction.
Now with this, we can confidently say that we have the complete bot angle computed from the bot angle
and simulation and its relation.
Combining this bot orientation with the bot location.
We have the complete robot post.
This means we can proceed onwards to defining the go to go function to make the robot reestimates exit
building.
Happy.
Good.
But.