2018SpringTeam5
Vehicle-to-Vehicle Communication
The aim of this project is to improve upon the existing DonkeyCar autonomous control framework by implementing situational awareness through inter-car communication of data collected by on-board sensors. To achieve this end, two cars were equipped with infrared and ultrasound sensors which were able to receive data from the track environment. The track environment was demarcated into two zones, and the entry point to a specific zone was fitted with an infrared signal emitter. This was done so that a car would know which zone it was in when its infrared sensor picked up a signal from an infrared emitter set up at a zone entry point. Zone and sensor information were then broadcasted to both cars in order to take action according to a given situation. For instance, if an obstacle were detected by a car on the track, this car would broadcast that it has been obstructed. The car behind it would then slow down and stop because it had received a signal to do so from the other car.
Sensor Setup
Infrared Sensors
Electronics setup, code
Purpose
As mentioned in the project overview, the infrared module is used for the cars to be aware of their position on the track. For simplicity, the position is really what zone the car is in. There are two possible zones - zone A and zone B. The track that we used was an elliptical track which was divided into two zones vertically. (see the diagram).
Setup
The IR emitter: Creating an emitter is simple. All you need is an IR LED, a 330Ω resistor and an Arduino board (we used micro). You connect the ground of the IR LED to the ground on the Arduino board and you connect the positive terminal of the IR LED with the 330Ω resistor, which is connected to one of the ports on the Arduino. This port will be the port that you code in. We used a library, which uses port 13 for an Arduino Micro and port 3 for and Arduino Uno. Details about the library are in given the the section below. Thats pretty much the setup.
Ultrasound Sensors
Electronics setup, code
Communications
Sockets were used to achieve communication of sensor data between the Raspberry Pi's fixed to each car. The documentation for sockets can be found here.
Creating the Server and Connecting a Client
The Server Socket
The following code shows how to create a server and tell it to listen for incoming connection requests from clients. It is a general overview of the logic we used in implementing a communication protocol between the two Raspberry Pi's that we used to control each respective car. The implementation of this logic within the DonkeyCar framework is covered here.
import socket serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) host = socket.gethostname() portNumber = 9031 serversocket.bind((host,portNumber)) # Binds socket to host IP address and an associated open port serversocket.listen(1) # Accepts client requests through this port. Argument specifies number of clients connection, client_address = serversocket.accept() # Awaits requests from clients.
When the script above is run, it will wait at the last line until a connection has been established with a client. gethostname() gets the name of the machine running this script as it is seen on its corresponding network; this name corresponds to the machine's IP address. portNumber should be set to a high number (we used somewhere in the 9000's) since lower-number ports are typically reserved for TCP/IP connections. The created serversocket is then tied to the specified host IP and port through the bind() function. From here, the server is told to listen(1) for an incoming request from a client; the argument passed to listen() tells the server to accept a request only from a single client. After the server accepts the request from the client, data can be transferred between the two sockets.
data = connection.recv(128) connection.close()
In this example, which is a continuation of the server socket setup, the argument passed to the function recv() specifies the buffer size (measured in bytes) of the message being received by the server from the client. Whatever information sent by the client is stored in the variable data. Most importantly, after the message is received, the connection is closed so that connection requests made by other clients to the server can also be processed.
The Client Socket
The following code shows how to create a client and tell it to send a request to a server. For communication to work, the client and server sockets must be connected to the same network and both must be hosted on separate machines.
import socket clientsocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) targetIP = 'x.x.x.x' portNumber = 9031 data = "Hello World" clientsocket.connect((targetIP,portNumber)) clientsocket.sendall(data) # send data to the server clientsocket.close()
Adjusting DonkeyCar Control
In order to adjust the control to react to specific situations, we needed to build off of the existing DonkeyCar framework to accept inputs from our own sensors and then map these inputs to specific outputs which could control the throttle of the car. We also needed to implement communication via sockets in order to broadcast sensor and situational information between each car. This section details the implementation of our control logic.
DonkeyCar Framework Overview
The DonkeyCar framework was built with the interfacing of custom sensors in mind. In the drive() loop of manage.py, a vehicle is created and parts are then added to this vehicle. The following code illustrates this:
V = dk.vehicle.Vehicle() th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
In this example, th_filter, a ThrottleFilter object, is added as a part to the Vehicle object V. It takes as an input 'user/throttle' and returns to an output 'throttle'. To better understand what is going on with these inputs and outputs, we look at another example:
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
Lambda, defined in transform.py in ~/donkeycar/parts/, is shown below:
class Lambda:
"""
Wraps a function into a donkey part.
"""
def __init__(self, f):
"""
Accepts the function to use.
"""
self.f = f
def run(self, *args, **kwargs):
return self.f(*args, **kwargs)
def shutdown(self):
return
There are two things to discuss here. First is the role of the Lambda. Lambda allows a function defined in the drive() loop of manage.py to share inputs and outputs with other parts added to the vehicle V. It also implements run() and shutdown() functions, which allows the function wrapped by Lambda to execute repeatedly with the drive() loop thread. In other words, Lambda allows any function to be turned into a part.
For the second point of discussion, we see that the part pilot_condition_part accepts as inputs 'user/mode' and returns to outputs 'run_pilot'. By now, it is evident that any number of inputs and outputs of any type can be attributed to any part. In this example, pilot_condition_part will turn the output 'run_pilot' to either True or False based on whether or not 'user/mode' is equal to 'user'. The function which characterizes pilot_condition_part will run repeatedly with every loop of drive(), so this part is constantly checking whether or not the autopilot should be run. Another important thing to note about inputs and outputs is that the inputs going into one part can very easily be the outputs of another. This allows for the seamless integration of a system made up of many complex parts which are able to readily share inputs and outputs.
An important and powerful feature of the add() function is the ability to allow parts to execute their run_threaded() functions in their own threads. This is done by setting the parameter threaded of the add() function to True. As a rule of thumb, any object with a defined run() or run_threaded() function can be added to a Vehicle object. update() or shutdown() may also be included.
We took advantage of this versatility in creating and interfacing parts to adjust values sent to the PWM board that control the throttle based on our logical framework.
Adding Custom Parts
The Arduino
In order to process our infrared and ultrasound sensor data within the DonkeyCar framework, we created a new part in ~/donkeycar/parts/ called arduino.py. The code for arduino.py is shown below:
import serial
class Arduino():
def __init__(self):
self.zone = 'B'
self.signal = 'GO'
self.first_or_last = 'first'
ser = serial.Serial('/dev/ttyACM0/',9600)
def run_threaded(self,other_zone):
"""
The pi will continually read output sent by the Arduino to the Serial Monitor.
A serial port is opened when the arduino part is created. The default zone is B
and the default go/stop condition is 'OK'. When the IR sensor is tripped, a 'STOP' signal
is sent to the serial monitor and read by the pi. The 'arduino' part will then set its
stop condition to True. Additionally, zone information is sent whenever a zone change
occurs. The part will then accordingly change its zone information. An output 'first_or_last' will
also be returned, which will show which car entered a zone first.
"""
read_serial = ser.readline()
zone_and_stop = read_serial.split()
if zone_and_stop[0] == 'STOP':
self.signal = 'STOP'
elif zone_and_stop[0] == 'OK':
self.signal = 'GO'
if zone_and_stop[1] == 'A' and other_zone == 'A':
self.zone, self.first_or_last = 'A','last'
elif zone_and_stop[1] == 'A' and other_zone != 'A':
self.zone, self.first_or_last = 'A','first'
elif zone_and_stop[1] == 'B' and other_zone == 'B':
self.zone, self.first_or_last = 'B','last'
else:
self.zone, self.first_or_last = 'B','first'
return self.signal,self.zone, self.first_or_last
We then created a part and added it to the Vehicle object V within the drive() loop of manage.py as follows:
arduino_part = Arduino() V.add(arduino_part, inputs=['other/zone'], outputs=['this_stop/go','this/zone', 'first/last'], threaded=True)
When arduino_part, an Arduino object, is created and added to the vehicle, it repeatedly executes the run_threaded() function defined in the Arduino class in its own thread; the threaded=True parameter of add() assures this. run_threaded() here will take an input 'other/zone' and places its returned values in outputs 'this_stop/go', 'this/zone', and 'first/last'.
arduino_part listens for any output the Arduino sends to its serial monitor which we connected to the Raspberry Pi's USB port /dev/ttyACM0/. With every execution of run_threaded(), a single line of the Arduino's output to the serial monitor is read and stored in the variable read_serial. To simplify the process, the Arduino was configured such that based on what data it received from its attached sensors, it printed a line to the serial monitor which was formatted as
[STOP/OK]\t[zone (A or B)]\n. For instance, if a car had crossed a checkpoint into zone A and no obstacles were detected by the ultrasound sensor, the Arduino would print to the serial monitor OK\tA\n. The Pi would then process read_serial into a list of two strings called zone_and_stop. The first entry of zone_and_stop contains either 'STOP' or 'OK', and the second entry contains either 'A' or 'B'. zone_and_stop is then processed to figure out what actions will need to be taken based on the current situation. The 'STOP'/'GO' signal is changed and returns to an output so that it may be used by other parts in the framework. Based on what zone the other car is in, the car running the code above will know whether or not it is the lead car. If car 1 enters a zone and it sees that the car 2 is still in that same zone, car 1 will know that it is following the lead car car 2. Otherwise, if car 1 enters a zone that the car 2 is not in, car 1 will know that it is the lead car. Getting each car to know whether or not it is the leader is important because in the event that both cars are in the same zone, the one behind the leader must slow down. This requires that both cars know at all times which one the leader is and which one the follower is.
Server and Client
To facilitate communication between the Raspberry Pi's of each car, two new parts were made in ~/donkeycar/parts/ called server.py and client.py. server.py is shown below:
import socket
class Server():
def __init__(self):
"""
Initializes the server and stored data. The server is hosted on the Pi that this
script is run, and the client is responsible for locating the server in order to send
its data.
"""
self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.data = ""
host = socket.gethostname()
portNumber = 9031
serversocket.bind((host,portNumber))
serversocket.listen(1)
def run_threaded(self):
"""
Accepts connections from clients and processes the data. The data sent will be formatted
as follows: 'stop/go condition from other car\tzone of other car'. This function
will return what zone the other car is in as well as whether a stop or go signal is
received from another car.
"""
connection, client_address = self.serversocket.accept()
try:
d = connection.recv(128)
finally:
connection.close()
self.data = d.decode()
stop_go_zone = self.data.split()
return stop_go_zone[0], stop_go_zone[1]
Very much like we did with the Arduino, we created a part with a Server object called server_part and added it to V:
server_part = Server() V.add(server_part, outputs=['other_stop/go', 'other/zone'], threaded=True)
When a Server object is created, a server socket is automatically created and hosted on the Raspberry Pi upon which the scripts above are run. A Server part will repeatedly execute its run_threaded() function in its own thread. server_part will continuously receive information about whether or not the other car has stopped and which zone the other car is in. This information is stored in the outputs 'other_stop/go' and 'other/zone'.
The code for the client is shown below:
import socket
class Client():
def __init__(self):
self.data = ""
self.target_IP = 'x.x.x.x' # change this later. This is the IP of the server we want to connect to
self.port_number = 9031
def run_threaded(self, this_stop_go, this_zone):
self.data = this_stop_go+'\t'+this_zone
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.connect((target_IP, port_number))
client_socket.sendall(self.data.encode())
client_socket.close()
Just like the server and the Arduino, the client part is added to the vehicle V:
client_part = Client() V.add(client_part, inputs=['this_stop/go','this_zone'], threaded=True)
The inputs that go into client_part are received by the server set up on the other car. In this way, we were able to send positional and situational information between the two Pis.
Override Control
In order to translate our situational information into actions taken by the car to slow down or stop, we needed to introduce a new mode of control called override control. This control mode was made as an addition to the existing modes such as the manual PS3 controller mode and the autopilot mode, and override control facilitates what values are sent to the PWM board in order to slow the car or stop it entirely. override_control.py is shown below:
import time
class Override_Control():
def __init__(self):
self.override_throttle = 0.0
self.throttle_scale = .17
def run_threaded(self,this_stop_go,other_stop_go,this_zone,other_zone,first_or_last):
"""
'STOP' has priority over slowdown, and resets the throttle scale which controls the slowdown.
If both cars are in the same zone and this car is the last to enter the zone, then this car will
slow down. Otherwise, the car will run as normal.
"""
if this_stop_go == 'STOP':
self.override_throttle = 0.0
self.throttle_scale = 0.17
elif this_zone == other_zone and first_or_last == 'last':
if self.override_throttle > 0.05:
self.throttle_scale -= self.throttle_scale*.05 #reduce throttle by 5%/iteration on slowdown
self.override_throttle = self.throttle_scale
time.sleep(0.1) # slows down exponentially every tenth of a second
return self.override_throttle
Like all other parts mentioned, it is added to the vehicle as follows:
override_controller = Override_Control() V.add(override_controller,inputs=['this_stop/go','other_stop/go','this/zone','other/zone','first/last'],outputs=['override/throttle'], threaded=True)
override_controller continuously checks whether or not an obstacle has been detected by the ultrasound sensor. If an obstacle is detected, the output 'override/throttle' is set to zero. As will be seen in the next section, this immediately stops the car. Otherwise, if there is no obstacle, override_controller will proceed to check if the car running this script is in the same zone as the other car as well as if the car running this script is following the lead car (shown above with the condition first_or_last=='last'.) If these conditions are satisfied, then the car will initiate an exponential slowdown with a throttle decay rate of 5% of the throttle value for every iteration, every tenth of a second. This continues until a minimum throttle threshold is met, at which point the car will maintain speed.
If none of the conditions discussed above are satisfied, run_threaded() will continue to execute and output to 'override/throttle'. It is not an issue to allow this function to keep running since control is returned to the autopilot anyways since these conditions are what cause override mode to be switched into in the first place.
Interfacing Custom Parts with Existing Parts
In order to get our custom parts to work within the DonkeyCar framework, we needed to make some adjustments to manage.py in order to get proper functionality. These changes are detailed further in this section.
Switching in and out of Override Mode
In order to switch in and out of override mode, certain conditions must be met; the car must either receive a 'STOP' signal from the Arduino or find that both cars are currently in the same zone. In these cases, the car switches into override mode. When neither of these cases occur (that is, when neither car is in the same zone and no obstacle is detected by either car) control is given to the autopilot by switching into local angle mode. The code below shows the implementation of this logic:
def override_switch(this_stop_go,this_zone,other_zone):
if this_zone == other_zone or this_stop_go == 'STOP':
return 'override'
else:
return 'local_angle'
override_switch_part = Lambda(override_switch)
V.add(override_switch_part,inputs=['this_stop/go','this/zone','other/zone'],outputs=['user/mode'])
Another function, shown below, selects which throttle values to send to the PWM board:
def drive_mode(mode, user_angle, user_throttle,
pilot_angle, pilot_throttle, override_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'override':
return pilot_angle, override_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle', 'override/throttle'], outputs=['angle', 'throttle'])
The outputs 'angle' and 'throttle' are the values that the PWM board uses to adjust the steering angle and speed of the car. This function has been adjusted so that 'override/throttle' is stored in 'angle'. This is the final piece of code which allows us to control the car with the logic discussed in the override control section.
For more detail on how exactly this works, see the DonkeyCar framework overview.