- Exercise 1: Vacuum Cleaner
- Exercise 2: F1 Follow Line
- Exercise 3: Obstacle Avoidance
- Exercise 4: Global Navigation
- Exercise 5: Monte Carlo Laser Localization
Our first practise consists on designing a vacuum cleaner. We will make a FSM(Finite State Machine) to split the code into different states.
This is our FSM:
(Made with: draw.io )
- from HAL import HAL - to import the HAL(Hardware Abstraction Layer) library class. This class contains the functions that sends and receives information to and from the Hardware(Gazebo).
- from GUI import GUI - to import the GUI(Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.
- HAL.getBumperData().state - To establish if the robot has crashed or not. Returns a 1 if the robot collides and a 0 if it has not crashed.
- HAL.getBumperData().bumper - If the robot has crashed, it turns to 1 when the crash occurs at the center of the robot, 0 when it occurs at its left and 2 if the collision is at its right.
- HAL.setV() - to set the linear speed
- HAL.setW() - to set the angular velocity
- HAL.getLaserData() - It allows to obtain the data of the laser sensor, which consists of 180 pairs of values (0-180º, distance in millimeters).
First of all we have solved the problem with those four states using a while loop, so that the code works at a certain frecuency. Every time the while loop iterates, it evaluates whether the state of the code is forward, backward, spiral or turn, and then effectuates as so. Those states are constantly changing and can interact with each other in order to achieve the behavior we want for our vacuum cleaner.
from GUI import GUI
from HAL import HAL
import time
import random
SPIRAL=1
BACKWARDS=2
TURN=3
FORWARD=4
state= 1
radius=0.3
#check if the vacuum cleaner crashed a wall
def bumper_hit():
bumper_state=HAL.getBumperData().state
if bumper_state == 1:
bumper_value=HAL.getBumperData().bumper
HAL.setV(0)
if bumper_value == 0:
print("izq")
HAL.setW(1)
return "hit"
if bumper_value == 1:
print("centro")
HAL.setW(2)
return "hit"
if bumper_value == 2:
print("dcha")
HAL.setW(-1)
return "hit"
#spiral state
def spiral(radius):
var=bumper_hit()
if var=="hit":
HAL.setV(0)
HAL.setW(0)
return "back"
time.sleep(1)
HAL.setV(radius)
HAL.setW(1.5)
time.sleep(1)
#backwards state
def backwards():
HAL.setV(-1)
time.sleep(0.5)
return "turn"
#turn state
def turn():
HAL.setV(0)
HAL.setW(2)
time_spin= random.uniform(0.8,1.5)
time.sleep(time_spin)
return "forward"
#forward state
def forward():
print("straight and forward as it should be!")
HAL.setV(1)
HAL.setW(0)
var=bumper_hit()
if var=="hit":
HAL.setV(0)
HAL.setW(0)
return "back"
else:
time.sleep(7)
return "spiral"
while True:
if state == SPIRAL:
radius=radius+0.1
if spiral(radius) == "turn":
state=TURN
if radius >= 1.4:
state=4
if state == BACKWARDS:
if backwards() == "turn":
state=TURN
if state == TURN:
if turn() == "forward":
state=FORWARD
if state == FORWARD:
if forward() == "spiral":
radius=0.5
state=SPIRAL
elif forward() == "back":
state=BACKWARDS
There are several things that we need to point out in our code, the first thing is the variables assignment:
SPIRAL=1
BACKWARDS=2
TURN=3
FORWARD=4In this way, we will be able to call any state by its name instead of the number that represents it. For instance, we can see the way it is implemented in this snippet:
if state == FORWARD:
if forward() == "spiral":
radius=0.5
state=SPIRAL
elif forward() == "back":
state=BACKWARDSHere, it is clear that the state we are in is the "FORWARD" state, and while iterating in that state, if the function returns "spiral", the state changes to SPIRAL and so does it to BACKWARDS when returnning "back".
The function "bumper_hit()" simply checks if the vacuum cleaner bumped into something and where was it.
When the code is running and we take an example map, after a few minutes, we obtain this percentage of the whole house floor covered:
- On our second step we needed to get rid of the "time.sleep()" function calls, due to the fact that can cause several problems to our code. As it is a non-stopping loop, when time.sleep() is done, we loose the whole control of the system and no matter what happens to our robot, that we cannot respond. That is why the only thing could use is "time.time()" to get to know the actual time and be able to compare it with the same function call but at any other time.
We could change the spiral by deleting the time.sleep(), but this time we needed to calculate again the spiral with brand-new measurements.
from GUI import GUI
from HAL import HAL
import time
import random
#States:
SPIRAL=1
BACKWARDS=2
TURN=3
FORWARD=4
#initial state
state= SPIRAL
radius=0.01
#check if the vacuum cleaner bumped
def bumper_hit():
bumper_state=HAL.getBumperData().state
if bumper_state == 1:
return "hit"
def spiral(radius):
#if the robot hitted a wall, then stop and turn back
if bumper_hit()=="hit":
HAL.setV(0)
HAL.setW(0)
return "back"
HAL.setV(radius)
HAL.setW(1)
def backwards():
HAL.setV(-1)
time_end=time.time()
if time_end- time_var >= 0.5:
return "turn"
else:
return "backwards"
def turn():
print("turn up the music!")
HAL.setV(0)
HAL.setW(2)
#random numbre between 0.8 and 1.5
time_spin= random.uniform(0.8,1.5)
time_end=time.time()
if time_end- time_var >= time_spin:
return "forward"
else:
return "turn"
def forward():
print("straight and forward as it should be!")
HAL.setV(1)
HAL.setW(0)
if bumper_hit()=="hit":
HAL.setV(0)
HAL.setW(0)
return "back"
#at a random moment, do a spiral
random_num=random.randint(1,300)
if random_num== 5:
return "spiral"
while True:
#SPIRAL STATE
if state == SPIRAL:
if spiral(radius) == "back":
state=BACKWARDS
#update time variable
time_var=time.time()
#if the vel. is too high, reset and go forward
if radius >= 2.5:
state=FORWARD
#wider spiral while iterating
radius=radius+0.01
#BACKWARDS STATE
if state == BACKWARDS:
if backwards() == "turn":
state=TURN
#update time variable
time_var=time.time()
#TURN STATE
if state == TURN:
if turn() == "forward":
state=FORWARD
#FORWARD STATE
if state == FORWARD:
if forward() == "back":
state=BACKWARDS
#update time variable
time_var=time.time()
#random spiral event
if forward() == "spiral":
radius=0.1
state=SPIRAL
Here, we measure the time just before entering into a function so that then when we enter and measure it again, we can take that amount of time that passed between one and another time.time() and use it for several things. Example:
We are going forward and suddenly bump into a wall, our function returns "back" and we change our state so that next time iterating, the new state will be "BACKWARDS". Just before that, we measure the time in the variable "time_var".
#FORWARD STATE
if state == FORWARD:
if forward() == "back":
state=BACKWARDS
#update time variable
time_var=time.time()Then we access the function backwards(), in which we find another measuring of time but this time in the variable time_end().
def backwards():
HAL.setV(-1)
time_end=time.time()
if time_end- time_var >= 0.5:
return "turn"
else:
return "backwards"By comparing those 2 variables, we can predict the time that passed between them and then operate like in this case, where we want to wait for half a second.
- When running the code several times, we realize that even though the vacuum cleaner covers almost the whole map for not so long, it passes through some places over and over. That is why the function bumper_hit() has been expanded so that when the bumper of the robot crashes, the performance is quite different. When it crashes on the left side, the robot stops, gets back and turns right. Same thing for the right side but to the right.
We add global variables for the left and right:
#global variables for bumper
left = 0
right = 0In the bumper_hit() function, we update them when necessary
#bumper funtion, returns run if bumper got hit
#updates global variables for position of the bump
def bumper_hit():
bumper_state=HAL.getBumperData().state
if bumper_state == 1:
bumper_location=HAL.getBumperData().bumper
if bumper_location==0:
global left
left=left+1
if bumper_location==2:
global right
right=right+1
return "hit"So that then we can make the vacuum cleaner act different in the 'turn' function() depending on those variables. After that, we need to reinitialize them to iterate again:
def turn(left_,right_):
#turn right if bumped on left
if left_ == 1:
HAL.setW(2)
#turn left if bumped on right
if right_ == 1:
HAL.setW(-2)
#if got hit in the centre, make a random turn
if (right_ == 0) and (left_ == 0):
HAL.setW(2)
HAL.setV(0)
#random number
time_spin= random.uniform(0.8,1.5)
time_end=time.time()
#reset global variables
global left
global right
left=0
right=0
if time_end- time_var >= time_spin:
#reset of bumper
return "forward"
else:
return "turn"- In the end, as the practise characteristics have been modified throughout the days, there has been made a major change related to laser. Reading the API of the robot, there is an example of the laser performance, which is this one:
#introduce laser data in an array
def parse_laser_data(laser_data):
laser = []
for i in range(180):
dist = laser_data.values[i]
angle = math.radians(i)
laser += [(dist, angle)]
return laserThe way it works is pretty simple ,it estimates values and keeps them in the 'laser' array. Those values are angles and distances. That is why we have implemented an update to our code which consists on a different behavior when going on a spiral. The way it worked before, was that whenever the spiral velocity was too high, the vacuum cleaner changed to the FORWARD state. Now it is still working that way, but we have included another behavior. Whenever the robot is doing a spiral, if de distance between the robot and a wall is less than 0.5 meters, it will automatically change to FORWARD state too.
#introduce laser data and compare distance
def laser_detect(laser):
for i in range(0, 180, 30):
if i == 180:
i = 179
if laser[i][0] < 0.5:
return 0The condition is written in this part of the code (spiral state):
#get laser data
laser_data = HAL.getLaserData()
#if it is to close to a wall, reduce vel, and go straight
if (laser_detect(parse_laser_data(laser_data)) == 0):
state =4- Overall, here is the final code:
from GUI import GUI
from HAL import HAL
import time
import random
import math
#States:
SPIRAL=1
BACKWARDS=2
TURN=3
FORWARD=4
#initial state
state= SPIRAL
#initial radius to increase
radius=0.01
#global variables for bumper
left = 0
right = 0
#introduce laser data in an array
def parse_laser_data(laser_data):
laser = []
for i in range(180):
dist = laser_data.values[i]
angle = math.radians(i)
laser += [(dist, angle)]
return laser
#introduce laser data and compare distance
def laser_detect(laser):
for i in range(0, 180, 30):
if i == 180:
i = 179
if laser[i][0] < 0.5:
return 0
#bumper funtion, returns run if bumper got hit
#updates global variables for position of the bump
def bumper_hit():
bumper_state=HAL.getBumperData().state
if bumper_state == 1:
bumper_location=HAL.getBumperData().bumper
if bumper_location==0:
global left
left=left+1
if bumper_location==2:
global right
right=right+1
return "hit"
#create a spiral
def spiral(radius):
HAL.setV(radius)
HAL.setW(1)
#if the robot hitted a wall, then stop and turn back
if bumper_hit()=="hit":
HAL.setW(0)
HAL.setV(0)
return "back"
#go back for half a second
def backwards():
HAL.setV(-1)
time_end=time.time()
if time_end- time_var >= 0.5:
return "turn"
else:
return "backwards"
#turn depending on where
#the bumper got hit a random amount of time
def turn(left_,right_):
#turn right if bumped on left
if left_ == 1:
HAL.setW(2)
#turn left if bumped on right
if right_ == 1:
HAL.setW(-2)
#if got hit in the centre, make a random turn
if (right_ == 0) and (left_ == 0):
HAL.setW(2)
HAL.setV(0)
#random number
time_spin= random.uniform(0.8,1.5)
time_end=time.time()
#reset global variables
global left
global right
left=0
right=0
if time_end- time_var >= time_spin:
#reset of bumper
return "forward"
else:
return "turn"
#move forward
def forward():
HAL.setV(1)
HAL.setW(0)
#if the robot hitted a wall, then stop and turn back
if bumper_hit()=="hit":
HAL.setW(0)
HAL.setV(0)
return "back"
#at a random moment, do a spiral
random_num=random.randint(1,300)
if random_num== 5:
return "spiral"
while True:
#SPIRAL STATE
if state == SPIRAL:
if spiral(radius) == "back":
state=BACKWARDS
#update global time variable
time_var=time.time()
#if the vel. is too high, reset and go forward
if radius >= 2.5:
state=4
#----------
#get laser data
laser_data = HAL.getLaserData()
#if it is to close to a wall, reduce vel, and go straight
if (laser_detect(parse_laser_data(laser_data)) == 0):
state =4
#----------
#wider spiral while iterating
radius=radius+0.01
#BACKWARDS STATE
if state == BACKWARDS:
if backwards() == "turn":
state=TURN
#update global time variable
time_var=time.time()
#TURN STATE
if state == TURN:
if turn(left,right) == "forward":
state=FORWARD
#FORWARD STATE
if state == FORWARD:
if forward() == "back":
state=BACKWARDS
#update global time variable
time_var=time.time()
#random spiral event
if forward() == "spiral":
radius=0.1
state=SPIRALHere is a little test of the vacuum cleaner after a couple of minutes performing the last code:
Here is a video showing the behavior:
Here is a little demo video at x2.00 speed(that can be seen online):
TEST.Hecho.con.Clipchamp.4.mp4
The goal of this exercise is to perform a PID reactive control capable of following the line painted on the racing circuit.
- from HAL import HAL - to import the HAL(Hardware Abstraction Layer) library class. This class contains the functions that sends and receives information to and from the Hardware(Gazebo).
- from GUI import GUI - to import the GUI(Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.
- HAL.getImage() - to get the image
- HAL.setV() - to set the linear speed
- HAL.setW() - to set the angular velocity
- GUI.showImage() - allows you to view a debug image or with relevant information
First of all, we need to implement OpenCV and Python Color Detection, so that we can constantly get the POV of the car. After that we need to apply a mask to the image to get the red color of the line needed to be followed. Our first code implementation was this one:
from GUI import GUI
from HAL import HAL
import numpy as np
import cv2
while True:
#get raw image
image=HAL.getImage()
hsv=cv2.cvtColor(image,cv2.COLOR_BGR2HSV)
#define color thresholds
lower_tresh = np.array([0,100,100])
upper_tresh =np.array([10,255,255])
#a mask is created which identifies pixels in this color range
mask =cv2.inRange(hsv, lower_tresh,upper_tresh)
cv2.imshow("mask",mask)
The main purpose of this code is to segment an image based on a color range in the HSV (Hue, Saturation, Value) color space. 'lower_tresh and upper_tresh are a NumPy array that define the lower limit of the color range you want to detect. In this case, it's looking for red. But we can detect any colour we want with this method.
In this line:
mask =cv2.inRange(hsv, lower_tresh,upper_tresh)a mask is created that identifies the pixels in the original image that fall within the color range defined by lower_thresh and upper_thresh. The resulting mask will be a binary image in black and white, where pixels falling within the color range will be white (255), and those outside the range will be black (0).
After that we wanted an indicator so that we can know whether the car is following the line or not and how is it going to do that. That is why, we added this part of the program:
M = cv2.moments(mask)
if M['m00'] > 0:
#centroids of X and Y
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)The 'moments' function from OpenCV is used to calculate the moments of the binary image (the mask we added). Moments are statistical descriptors used to characterize a distribution of pixels in an image. In this case, the moments of the mask 'mask' are being calculated.
For 'cx' and 'cy', which are the coordinates of the center of mass (centroid) of the object in the binary image. The moments m['m10'] and m['m01'] represent the sums of the X and Y coordinates of all pixels in the object, respectively, and m['m00'] represents the area of the object. Dividing these sums by the area of the object gives the centroid coordinates (cx, cy) as integers. After that, we 'paint' the circle in the centroid of the line, that we calculated before. Here is how the final image can be seen on screen:
We included another update for the image, so that in this way, it is clearer and much harder for the program to mistake.
#process the image and reduce noise and small pixel variations.
hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
blur = cv.GaussianBlur(hsv, (5,5), 0)
# detect the color of line
res = cv.inRange(blur, lowest_thresh, top_thresh)
#dilate and erode the mask
d = cv.dilate(res, kernel=keneral, iterations=D_iter)
e = cv.erode(d, kernel=keneral, iterations=E_iter)We will use the cv2.dilate() and cv2.erode() functions to process the image. In this way, we can let the F1 know more about farther road. As a result, the F1 can handle sharp turns better.
Before applying the PID in our code we need to create several functions, so that it will be much easier to undesrtand and work with.
- get_red_mask(img) It receives the image, applies a mask so that we only get the red colour, and then 'depurates' it and returns it. Returns the mask
- get_centroid(mask) It calculates the centroid of the line from the mask that is introduced. Returns the centroid (cx,cy)
- calculate_angular_velocity(error) It calculates the angular speed by applying a PID inside, which is already explained.
Now it is time to use the PID to control the F1. In this situation, we have the: 'HAL.setW(X)' function, so that we can directly set the angular velocity of the F1 and keep adjusting it at every iteration. The general equation of the PID is the following:
We need to implement this in our code, watching carefully the error every time it iterates and adjusting the constants Kp,Kd and Ki to minimize the error and oscilate less.
We included the PID by calling our function 'calculate_angular_velocity()'. In our while loop, we did this:
#PID control
cur_error = -(centroid[0] - (width/2))/300
angular=calculate_angular_velocity(cur_error)
HAL.setW(angular)Here, as it can be seen, the loop handles the error and deals with it by calling a function that will have the PID and will return the angular speed after the PID is implemented. We implement the PID this way:
#proportional error
P = Kp * error
D = Kd * ( error - prev_error)
prev_error = error
integral_e += error
I = Ki * integral_e
angular= P + I + D
This is our code right now:
This code was taking less than 3 minutes. Which actually works pretty accurate, but the car needs to be faster, we are not in a Fiat 500 :)
After implementing this PID, we realized that although the code is working perfectly and the car stays on the line the whole time with smooth movements. We needed to do something else so that the car could go faster and complete one entire lap in less than one or two minutes. That is when we came up with an idea, a new PID controller for the linear speed so that the car itself can regulate the way it behaves, depending on the track. So that if it is following the line in a straight line, the speed will work different than when the car is turning.
After trials and errors, we came up with a brand-new idea, which is the following: We need to have 2 PID controllers. Both of them will be handling the angular speed, but this time we will be measuring a threshold to determinate whether the car is in a straight line or in a curve. Errors for both controllers:
#angular error handling
prev_error = 0
integral_e = 0
#linear error handling
prev_error_lin=0
integral_e_lin =0PIDs:
#curves
Kp = 1
Ki = 0.0001
Kd = 2.5
#straight lines
Kp_l=1
Ki_l=0.0001
Kd_l= 2.5The way we are going to implement this is by measuring where the centroid of the line is at every time ( after getting the mask) and comparing it to the location of the center of the line, but a bit higher. In this way we can get the difference between coordinates in the X axis and stablish a threshold. This threshold is there to decide whether the car is taking a straight line at a high-speed or just turning in a curve. Here is the implementation in the 'get_top_centroid' function which returns that point mentioned before:
def get_top_centroid(mask):
contours, _ = cv.findContours(mask, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
highest_point = None
for contour in contours:
if len(contour) > 0:
top_point = tuple(contour[contour[:, :, 1].argmin()][0])
if highest_point is None or top_point[1] < highest_point[1]:
highest_point = top_point
if highest_point is not None:
cv.circle(img, highest_point, 5, (0, 0, 255), -1)
centroid = (highest_point)
return centroid
Now we can get the difference in X axis coordinates and depending on that, effectuate our code:
#get threshold
curve_threshold=abs(centroid[0]- top_centroid[0])
if (curve_threshold < 15):
counter +=1
if counter >= 5:
print("straight")
linear=calculate_linear_velocity(linear_error)
HAL.setW(linear)
HAL.setV(15)
if (curve_threshold > 20):
print("Curve")
angular=calculate_angular_velocity(cur_error)
HAL.setW(angular)
HAL.setV(4)
counter = 0As we can notice, we left a gap in the conditionals for the threshold. We did this on purpose to apply 'histeresis' so that instead of an abrupt change, we make it smooth.
The counter that can be seen is perfectly studied and used so that when the car recognises more than 5 times in a row that it is in a straight line, the car speeds up. This is because sometimes when the car is turning, a few iterations recognized the line as straight. By using this we can ensure that the car is really in a straight line, and not just turning but heading the front.
The code works but there is still something missing. This is the time when we realize that it is much better for the car to keep the reference of the central point (cx, cy) instead of the 'line' point. This is why the same code as before was implemented but using this new point, as we can see here:
We changed the function that originated the other point with this new one:
def get_mid_centroid():
cx=320
cy=225
cv.circle(img, (cx, cy), 20, (0, 0, 255), -1)
centroid=(cx,cy)
return centroidThe main issue with this code is that it takes much longer to proccess itself, that is why the PIDs are not working the same way as before and the code is really unestable, allowing the car to get out of the track and hit himself. Apart from that we could complete a lap in approximately 30 seconds less than before, but this is not good enough, comparing it with all the issues we got.
This is the code with the implementation
Overall, we decided to take the first code as the final one. Even though it is the easiest one to implement, it is incredibly reliable, and that is what we are looking for in this exercise. The other implementations worked well and were well designed but the objective is to follow the line, and they made the car speed up much more, however, the line was not followed as expected.
We even realized that once you run the code more than once, the car works different at the very beginning, this may be because the page saves previous data and once you run the code, it will start with that data saved. This can be seen when you stop running the code and leave the car turning. When re-running the code again, it works for a couple of seconds as if it was still turning, and then it behaves slightly different as the previous time.
Now it is time to adjust the PID even more. After trials and errors adjusting the PID values, we came up with the final ones, which are: Kp = 1 Ki = 0.001 Kd = 2.5
Now you can run the code and the car will keep on the track forever!
This code can be seen in here: Accurate Implementation
Video.sin.titulo.Hecho.con.Clipchamp.1.mp4
The goal of this exercise is to control a car so that it keeps on track as well as it is avoiding other cars which are displayed on different parts of the racing circuit.
- from HAL import HAL - to import the HAL(Hardware Abstraction Layer) library class. This class contains the functions that sends and receives information to and from the Hardware(Gazebo).
- from GUI import GUI - to import the GUI(Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.
- HAL.getPose3d().x - to get the position of the robot (x coordinate)
- HAL.getPose3d().y - to obtain the position of the robot (y coordinate)
- HAL.getPose3d().yaw - to get the orientation of the robot with regarding the map
- HAL.getLaserData() - to obtain laser sensor data It is composed of 180 pairs of values: (0-180º distance in millimeters)
- HAL.getImage() - to get the image
- HAL.setV() - to set the linear speed
- HAL.setW() - to set the angular velocity
- GUI.showImage() - allows you to view a debug image or with relevant information
To simplify, the implementation of control points is offered. To use it, only two actions must be carried out:
- currentTarget = GUI.map.getNextTarget() - Obtain the following point:
- currentTarget.setReached(True) - Mark it as visited when necessary:
The graphical interface (GUI) allows to visualize each of the vectors of calculated forces. There is a function for this purpose:
# Car direction (green line in the image below)
carForce = [2.0, 0.0]
# Obstacles direction (red line in the image below)
obsForce = [0.0, 2.0]
# Average direction (black line in the image below)
avgForce = [-2.0, 0.0]
GUI.showForces(carForce, obsForce, avgForce)As well as the destination that we have assigned:
Current target
target = [1.0, 1.0]
GUI.showLocalTarget(target)More can be seen at Unibotics webpage: Webpage Link
Taking all of this, now it is time for the implementation:
First of all we will be implementing the laser. We will be doing it with this function:
def parse_laser_data (laser_data):
laser = []
i = 0
while (i < 180):
dist = laser_data.values[i]
if dist > 10:
dist = 10
angle = math.radians(i-90) # because the front of the robot is -90 degrees
laser += [(dist, angle)]
i+=1
return laserThe function parses laser data taking into account that the laser only has 180º of coverage and that the measure read at 90º corresponds to the ‘front’ of the robot.
Navigation using VFF (Virtual Force Field), is going to be the navigation algorithm that we will be using this time. The way it is done is the following:
Each object in the environment generates a repulsive force towards the robot. Destiny generates an attractive force in the robot. This makes it possible for the robot to go towards the target, distancing itself of the obstacles, so that their address is the vector sum of all the forces.
In this pic we can see the three vectors all together:
Car direction (green line in the image) Obstacles direction (red line in the image) Average direction (black line in the image)
Now it is time to implement the coordinate system that we will be using to create the VFF mentioned:
We have 2 different coordinate systems in this exercise.
- Absolute coordinate system: Its origin (0,0) is located in the finish line of the circuit (exactly where the F1 starts the lap).
- Relative coordinate system: It is the coordinate system solidary to the robot (F1). Positive values of X means ‘forward’, and positive values of Y means ‘left’.
We will be using the following function to convert absolute coordinates to relative ones:
def absolute2relative (x_abs, y_abs, robotx, roboty, robott):
# robotx, roboty are the absolute coordinates of the robot
# robott is its absolute orientation
# Convert to relatives
dx = x_abs - robotx
dy = y_abs - roboty
# Rotate with current angle
x_rel = dx * math.cos (-robott) - dy * math.sin (-robott)
y_rel = dx * math.sin (-robott) + dy * math.cos (-robott)
return x_rel and y_relNow the first part of our 'while' loop in the code, will work like this:
#get image
image=HAL.getImage()
#get laser
laser_data = HAL.getLaserData()
laser = parse_laser_data(laser_data)
#----------------------------------------------
# Get absolute position of current target
currentTarget = GUI.map.getNextTarget()
target_abs_x = currentTarget.getPose().x
target_abs_y = currentTarget.getPose().y
#----------------------------------------------
absolute_target = target_abs_x, target_abs_y
#----------------------------------------------
# Get robot position and orientation
robot_x = HAL.getPose3d().x
robot_y = HAL.getPose3d().y
robot_yaw = HAL.getPose3d().yaw
#----------------------------------------------
#get relative coordinates of target
target_rel_x,target_rel_y = absolute2relative(target_abs_x,target_abs_y,robot_x,robot_y,robot_yaw)
relative_target = target_rel_x, target_rel_yWe will also implement the 'get_repulsive_force()' function, which can be defined as:
def get_repulsive_force(parse_laser):
laser = parse_laser
laser_vectorized = []
for dist, angle in laser:
x = 1/dist * math.cos(angle) * -1
y = 1/dist * math.sin(angle) * -1
v = (x,y)
laser_vectorized += [v]
laser_mean = np.mean(laser_vectorized, axis=0)
return laser_meanThis function usage is pretty simple, the parameter is the laser data (already parsed) and it returns a vector pointing to the opposite way of where the 'mean of obstacles detected' are. We take both distance and angle (polar coordinates) from the laser data of the parameter and we convert them into Cartesian coordinates (x and y). Then we create the vector 'v' with both coordinates and add it to our list. In the end we can create a mean so that we only return one vector with the direction of the repulsive force. As the code is iterating the whole time, whenever a car is spotted, it instantly evades it as the mean of vectors changes instantly.
After that, it is time to define the three vectors that we already mentioned at the beginning. The first one is the car vector. We need it to keep constantly pointing at targets, that is why we use the target relative coordinates to define it. However, as we need it for the total/average force vector, and later for the speed, we will need to reduce and control it. When we display the vectors(will be seen later) there is kind of a little 'bounding box' that can be used as a reference. Taking this into account, we can define the values that are defined on the code below, defining coordinates for the lenght of x and the lenght of y maximum so that in this way the vector is always 'locked' in that 3.5 x 3.2 box.
The second one is the obstacle vector, which will be using the function we mentioned before to get the repulsive force vector and we can multiply it by a number so that in this way we can reduce or enlarge it. It is important to add that these values are arbitrary and will be changed later.
Same is done with the third vector, the average vector (or mean vector). Which calculates the total force taking into account both vectors repuslive and car vector. We will also use some numbers to modify results by multiplying them by both coordinates.
# Car direction defined in a green vector
car_vect = [max(min(target_rel_x, 3.5), -3.5), max(min(target_rel_y, 3.2), -3.2)]
# obstacle direction defined in a red vector
obs_vect = [get_repulsive_force(laser)[0]*5, get_repulsive_force(laser)[1]*15]
# average direction defined in a black line
avg_vector = [(car_vect[0]+obs_vect[0])*1.5, (car_vect[1] + obs_vect[1]) *0.6]Check if the robot is close to the objective(2 units) and if so, set the current objective as 'reached'. It will later try to reach the next one.
if (target_rel_x < 2 and target_rel_y < 2):
currentTarget.setReached(True)Now we define the linear and angular speed. We set the both speeds depending on the total/average vector.
HAL.setW(tan * 1.5)
HAL.setV(avg_vector[0])Now we show the target, as well as the vectors of the three forces and the current image on the screen.
GUI.showLocalTarget(relative_target)
GUI.showForces(car_vect, obs_vect, avg_vector)
GUI.showImage(image)After all of this, the code will work. However, it took a very long time to check different values to multiply the vectors so that the robot evaded other cars perfectly. At first, most of the cars were evaded perfectly but there were 3 of them which our robot could not. That is why I came up with a brand-new solution, which was to decrease speed, allowing us to react with more time and precission. How did we manage to do that? By reducing the total vector or average vector (the sum of other both). In this way we can reduce the speed(as it is used for setting the robot speed).
Then I came up with the idea of increasing the repulsion vector so that the sooner the car detects an 'anomaly' (and by anomaly, I mean an obstacle that pops out of nowhere), the better reaction it can have, allowing the car to take a sharp turn when necessary.
We also decided to change the distance that the robot needs to be from the objective so that it will be able to change it when it is closer. First time it was 2 units, but we decided to make it even more precise, changing it into 1.5.
Another little change was to declare the global values of the AngularValue= 1.5 that will be used to set the angular speed and the DistanceToObjective = 1.5 that we mentioned before.
In the end, after lots of changes, improvements and by trials and errors, these are the values that make the code work perfectly:
# Car direction defined in a green vector
car_vect = [max(min(target_rel_x, 3.5), -3.5), max(min(target_rel_y, 3.2), -3.2)]
# obstacle direction defined in a red vector
obs_vect = [get_repulsive_force(laser)[0]*3, get_repulsive_force(laser)[1]*8]
# average direction defined in a black line
avg_vector = [(car_vect[0]+obs_vect[0])*1, (car_vect[1] + obs_vect[1]) *0.3]Global Navigation using TeleTaxi TOC Global Navigation
The objective of this practice is to implement the logic of a Gradient Path Planning (GPP) algorithm. Global navigation through GPP, consists of: Selected a destination, the GPP algorithm is responsible for finding the shortest path to it, avoiding, in the case of this practice, everything that is not road.
Once the path has been selected, the logic necessary to follow this path and reach the objective must be implemented in the robot.
- from HAL import HAL - to import the HAL(Hardware Abstraction Layer) library class. This class contains the functions that sends and receives information to and from the Hardware(Gazebo).
- from GUI import GUI - to import the GUI(Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.
- from MAP import MAP - This class contains functions that interact with data related to the map and Gazebo world.
- HAL.setV() - to set the linear speed
- HAL.setW() - to set the angular velocity
- HAL.getPose3d() - returns x,y and theta components of the robot in world coordinates
- GUI.showNumpy(numpy) - shows Gradient Path Planning field on the user interface. It represents the values of the field that have been assigned to the array passed as a parameter. Accepts as input a two-dimensional uint8 numpy array whose values can range from 0 to 255 (grayscale). In order to have a grid with the same resolution as the map, the array should be 400x400
- GUI.showPath(array) - shows a path on the map. The parameter should be a 2D array containing each of the points of the path
- GUI.getTargetPose() - returns x,y coordinates of chosen destionation in the world. Destination is set by clicking on the map image
- MAP.getMap(url) - - Returns a numpy array with the image data in grayscale as a 2 dimensional array. The URL of the Global Navigation map is ‘/RoboticsAcademy/exercises/static/exercises/global_navigation_newmanager/resources/images/cityLargenBin.png’, so the instruction to get the map is
array = MAP.getMap('/RoboticsAcademy/exercises/static/exercises/global_navigation_newmanager/resources/images/cityLargenBin.png')
- MAP.rowColumn(vector) - returns the index in map coordinates corresponding to the vector in world coordinates passed as parameter
To start this practice, the first thing needed is to get the map from the Unibotics files:
map_url = '/RoboticsAcademy/exercises/static/exercises/global_navigation_newmanager/resources/images/cityLargenBin.png'
map_data = MAP.getMap(map_url)Once we have the map, we can get the position where the car is placed at the very beginning and get the target that has to be reached.
The way this is implemented is:
# start pose
start_pose =HAL.getPose3d()
pos=[start_pose.x, start_pose.y]
# get the clicked target
goal_pose = GUI.getTargetPose()
# get coordinates for actual location in map
new_target_map = tuple(MAP.rowColumn(goal_pose))As so, we get the starting position and the goal position with the actual map coordinates.
After that we need to define the grid. We will be using a BFS search. The algorithm works by assigning weights to a grid of cells. Given the source and target, the algorithm starts from the target node and moves outwards like a ripple, while progressively assigning weights to the neighboring cells.
#Get grid and show it
grid = get_grid(map_data, start_cell,new_target_map)
grid_normalized = normalize_grid(grid)
GUI.showNumpy(grid_normalized)We get the grid from get_grid() and then normalize it before showing it. In this function we need to create a priority queue. The way we define the grid is by expanding neighbors near the target.
It is important to mention that as soon as the grid finds the start position, it will stop searching. For the later navigation, we need to define cells located nearby the car at the very beginning, this is why we are expanding a bit more the grid like this:
if target_map[0] > 200 and target_map[1] > 200:
#print("DOWN RIGHt")
values2expand = (-10,-10)
if target_map[0] <= 200 and target_map[1] > 200:
#print("DOWN LEFT")
values2expand = (10,-10)
if target_map[0] > 200 and target_map[1] <= 200:
#print("UP RIGHT")
values2expand = (10,-10)
if target_map[0] <= 200 and target_map[1] <= 200:
#print("UP LEFT")
values2expand = (10,10)By doing this we are able to expand a bit more the grid into the opposite direction of the clicked zone (where we are doing the grid). Now the grid will stop increasing when that new position is reached, and not the starting point.
For the algorythm itself:
First of all we assign no cost for the target:
# Create the grid to keep costs
grid = np.full(map_array.shape, 255)
# No cost for target node
grid[target_map[1], target_map[0]] = 0And from there we use several costs for different directions as so:
# Directions for the grid (N, E, S, W, NE, NW, SE, SW)
directions = [(-1, 0), (0, 1), (1, 0), (0, -1), (-1, 1), (-1, -1), (1, 1), (1, -1)]
# Corresponding movement costs for the directions
HOR_AND_VERT_COST = 1
DIAGONAL_COST = math.sqrt(2)
When an obstacle is reached, this is the way it is handled so that the path we are creating later does not go through obstacles:
# Increase weights for obstacle cells
if map_array[neighbor[1], neighbor[0]] == 0:
new_cost += 300
obstacle_positions.add(neighbor)In the end we keep all obstacles so that later we can update the grid and increase the obstacles. By doing this, we assure that the car does not bump into any wall:
for obstacle in obstacle_positions:
for i in range(-2, 3):
for j in range(-2, 3):
neighbor = (obstacle[0] + i, obstacle[1] + j)
if 0 <= neighbor[1] < grid.shape[0] and 0 <= neighbor[0] < grid.shape[1]:
grid[neighbor[1], neighbor[0]] = 300 # Treat as obstacleOnce we have the whole grid, it is time to get the path and display it on screen. We define path as an array to keep the coordinates by comparing costs and keeping the lowest ones iterativelly.
Now that whe have all of this done, it is time to navigate until we reach the target. For navigating effectively, we constantly keep checking cells around the position of the robot. By doing this we can get to know the lowest cell, keep where it is, and reach it:
def get_next_position(current_pos, cost_grid):
next_pos = None
less_cost = float("inf")
for direction in directions:
neighbor = (current_pos[0] + direction[0], current_pos[1] + direction[1])
if 0 <= neighbor[0] < cost_grid.shape[0] and 0 <= neighbor[1] < cost_grid.shape[1]:
cost = cost_grid[neighbor[1], neighbor[0]]
if cost < less_cost:
less_cost = cost
next_pos = neighborIf we keep doing this we can reach the target, no matter where the robot is placed at the beginning.
while current_pos != goal_cell:
next_pos = get_next_position(current_pos, cost_grid)
target_world = gridToWorld(next_pos)
target_x, target_y = target_world
navigate_to_point(target_x, target_y)
current_pos = next_posTo move and reach each cell (with a lower cost that the previous one), we are using this logic:
We get x,y and theta:
current_pos = HAL.getPose3d()
current_x, current_y, current_yaw = current_pos.x, current_pos.y, current_pos.yawThen we get the distance to that point:
distance = math.sqrt((target_x - current_x) ** 2 + (target_y - current_y) ** 2)
To make the car spin and face the point we are using this logic:
# Get real angle to target
angle_to_target = math.atan2(target_y - current_y, target_x - current_x)
As we keep iterating, we want to keep decreasing the difference between where the car is facing and the target:
# Get angles needed for the car to turn and face target
angle_diff = angle_to_target - current_yawIf we normalize the angle, it will always be between -pi and pi. With this logic, we avoid making the car spin with non sense:
# Normalize the angle between -pi and pi
angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.piWe have also implemented a function that stops the car once the final goal is reached:
def stop_car():
while True:
HAL.setV(0)
HAL.setW(0)Taking all of this into account, our while loop will look like this:
while True:
navigate_to_goal(start_cell, new_target_map, grid)
print("Goal Reached")
# stop car when goal is reached
stop_car()This is how the grid and the path will look like:
prueba1.mp4
prueba2.mp4
prueba3.mp4
The aim of this practice is to develop a visual localisation algorithm based on the particle filter.
- from HAL import HAL - to import the HAL library class. This class contains the - - functions that receives information from the webcam.
- from GUI import GUI - to import the GUI (Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.
- HAL.getImage() - to get the image
- HAL.setV() - to set the linear speed
- HAL.setW() - to set the angular velocity
- HAL.getPose3d().x - to get the position of the robot (x coordinate)
- HAL.getPose3d().y - to obtain the position of the robot (y coordinate)
- HAL.getPose3d().yaw - to get the orientation of the robot with regarding the map
- GUI.showImage() - allows you to view a debug image or with relevant information
- GUI.showParticles(particles) - shows the particles on the map. It is necessary to pass a list of particles as an argument. Each particle must be a list with [positionx, positiony, angle].
Taking into account that at the moment the exercise was being done, unibotics webpage did not work properly, we were given the 'HAL.py','GUI.py' and 'MAP.py' libraries to work with. As so, we were given some example codes related to the robot movement, the way particles are shown, the way the laser is shown as well as the propagation of the particles.
Knowing all of this, now it is time to build up our code!
From the unibotics page we can get this diagram which will help us with the code:
Here is the structure of the main() funtion:
First of all, we need to randomly display particles throughout the map and that is what we will be doing in this function:
# Initialize random particles
particles = initialize_particles()It does not only display particles randomly, but also takes into account that the cannot be displayed on the black spaces, which represent obstacles such as tables or walls.
After calling that function, once the map is shown, it should look more or less like this:
Next step is to stablish robot movement, which has been done this way:
robot = HAL()
# Set a custom initial pose
robot.pose[0] = 1.1
# Create a GUI object and link it with the robot
gui = GUI(robot=robot)
# Set a small velocity
robot.setV(0.3)
robot.setW(0.8)Here we create the robot and set a spawnpoint, so that after that, with the linear and angular speeds that have been selected, it can keep doing circles until it discovers its position.
The rest of the code is done in a while loop so that we can work with the particles.
while True:
# Get some laser data and show it in the GUI
robot_laser_data = robot.getLaserData()
gui.showLaser(robot_laser_data)Get robot laser info and display it.
#----------Particles movement---------------------------------
particles = propagate_particles(particles)Particles keep moving throughout the map the same way as the robot
#------------Particle weight-----------------------------------
# Compute particle weights based on similarity to robot's laser data
weights = compute_particle_weights(particles, robot_laser_data)Depending on the similarity between laser data captured by the robot and laser data captured as 'virual laser' on each particle, we can give weights to that relation. This is done in the compute_particle_weights() function, which afterwards calls two other functions: get_laser_data() and virtual_laser_beam() that can also be found in the HAL.py file, but this ones have been changed so that we can 'simulate' each particle having lasers.
The way compute_particle_weights() works is by assigning weights to the particles, once the have all been compared. To do so, we need to get distances for both lasers ( robot and a particle) and if we get the difference, we can conclude that the less difference, the higher weight it is assigned ( as it means that laser data is much more similar or not). This is why we are doing the mean of all lasers and then get the inverse.
def compute_particle_weights(particles,robot_laser_data):
weights = []
for particle in particles:
# Current laser particle:
particle_laser_data = get_laser_data(particle)
#print(F"particle_laser_data: {particle_laser_data}")
# Difference between robot laser and particle
diff = abs(robot_laser_data - particle_laser_data)
mean= np.mean(diff)
weights.append(1/mean)
return weightsWith those weights we can then resample particles so that we can update them taking into account those weigts:
# Resample particles based on their weights
particles = resample_particles(particles, weights)
#-------------LAST THING TO DO IS JUST PRINT-------------------
# Show the particles in the GUI
gui.showParticles(particles [:,:3])
# Update the GUI image and wait
gui.updateGUI(block=True)Now this, is our first version, but taking into account the amount of time the computer needs to run the code, (as it has to simulate taking 180 values for the virtual laser of each particle and compare them to the robot laser) we will be measuring less distances from the laser. This is why we decreased from 180 to 18 measurements. This is the way we are implementing it in our code:
Every iteration in the while loop, gets laser data from the robot. However, we will just be taking 18 measurements (one of each ten ):
# Get some laser data and show it in the GUI
robot_laser_data = robot.getLaserData()
rob_laser_dat_red = robot_laser_data[::10]
rob_laser_dat_red_18 = rob_laser_dat_red[:18]We are doing the exact same thing in get_laser_data():
N_LASERS_PARTICLE = 18
.
.
.
get_laser_data():
for beam_angle in range(N_LASERS_PARTICLE):Now we have arrays that can be compared as they have the same measurements.
By doing this we can spawn more and more particles at the very beginning.
The whole final code, once launched should look like this:
