UviSpace Main Controller
Documentation
Release 1.0.0
Electronic Technology Department
Contents 1 Source Code 3 2 Documentation 5 2.1 Overview . . . 5 2.2 Getting Started . . . 8 2.3 Uvirobot package. . . 10 2.4 Uvisensor package . . . 16
2.5 Tutorials and manuals . . . 28
2.6 Release Notes . . . 29
Python Module Index 31
UviSpace Main Controller Documentation, Release 1.0.0
The UviSpace Main Controller is part of the UviSpace project, a system for remotely controlling Unmanned Ground Vehicles (UGVs) moving inside a delimited space and observed by a network of FPGA-based camera devices. This subsystem is in charge of obtaining camera data, processing it, and give the proper movement orders to the vehicles.
The project is developed by a team of researchers at the Electronic Technology Department in theUniversity of Vigo.
CHAPTER
1
Source Code
UviSpace Main Controller source code is available onGitHub. Anybody is welcome to clone it and work with it. If you wish to collaborate, just contact us.
CHAPTER
2
Documentation
Overview
General overview
UviSpace Main Controller is part of the UviSpace project, whose aim is to control an indoor intelligent space where several unmanned vehicles are simultaneously observed and controlled through a distributed vision system.
The Main Controller should be executed in a PC or embedded SoC (for example, ZedBoard) that has visibility of the whole system, so it can control the different parts.
The purpose of Main Controller is to decide the actions vehicles have to take in order to achieve their goals, based on the data obtained through the vision system. To accomplish this, the main tasks are:
• Communicate with the FPGA-based localization nodes, using the Ethernet LAN network. • Merge the data obtained from the localization nodes.
• Get the global coordinates of the UGVs.
• Given the destination of the UGVs, calculate the optimal paths. • Calculate the UGVs speed, using a navigation model.
• Communicate with the vehicles boards, using the XBee protocol, and send them the speed set points.
Software project
UviSpace Main Controller is written in Python2, and it is structured into 2 main packages, uvirobot and uvisensor. The uvirobot package deals with the classes and functions needed to implement the algorithms for moving an UGV and communicating with it, while the uvisensor package is used to communicate with FPGA devices using a TCP/IP connection and receive scene information captured through camera peripherals.
uvirobot
The uvirobot package consist in 2 callable scripts:
• messenger.py establishes the communication to the specified UGV, using the XBee protocol. Thus, prior run-ning it an XBee module has to be connected to the PC, and another one to the Arduino board serial port, and both configured accordingly. Once the communication is established, the module listens for speed set points and send them to the UGV. When the execution is canceled, a plot with the delay times is obtained and then the program ends.
• navigator.py listens for input UGV position values and destination coordinates, calculates the UGV optimal path, and finally outputs the speed set points needed to achieve the planned path.
Moreover, there are 5 importable libraries inside the uvirobot package:
• path_tracker.py contains a class whose methods calculate an UGV’s path points, once given a position and destination, and then stores them in an attribute.
• plotter.py contains functions used to construct a graph with a predefined format. It is designed to map the calculated vs. real path of a UGV, and the delay times.
• robot.py contains the RobotController() class, where each instantiated object represents a UGV, and establishes the upper interface for working with its speed values.
• serialcomm.py contains the SerMesProtocol() class, which is a child of the built-in python pyserial class. It defines a serial protocol, which will be used by the XBee modules for communicating the CPU with a UGV. • speedtransform.py contains the Speed() class, for dealing with operations related to the speed values, such as
transform between different scales or ensuring that the values are between valid boundaries. Finally, the package contains a couple of auxiliary modules into the resources folder:
• teleoperation.py is a callable module that allows to control the UGV with the keyboard, which is specially useful when the uvispace package has to be tested.
UviSpace Main Controller Documentation, Release 1.0.0
uvisensor
The uvisensor contains 1 callable module, multiplecamera.py. It is based on multithreading. The purpose of this module is to manage the information of N cameras, merge the vertices of every tracker and obtain the final number of UGVs(triangles) with their pose. There is a shared variable with all the vertices obtained from each camera.
The connection via TCP/IP to the cameras, as well as the processing of the images from each camera, is achieved in parallel.
Finally, another thread communicates with the user in order to get commands. At the moment, the only command is ‘Quit Program’ (‘Q’), but this functionality can be easily increased in the future.
Summing up, the list of threads is:
• 1 main thread that will merge the information from every camera (VideoSensor). • N threads for getting data from the N cameras connected to the system.
• 1 user oriented thread for getting commands from the user. The package has, as well, 4 importable modules:
• client.py contains the Client() class, which is a child of the Socket class from the socket.socket built-in Python module. This class contains methods for communicating specifically with the design FPGA hardware. Its methods allow to open and close the connection correctly, and to write and read from valid registers with the right format.
• geometry.py contains 2 classes. The Triangle() class is used for performing geometrical operations inherent to isosceles triangles, in order to get its base length, barycentre, position, angle... The Segment() class is used to determine a segment from its 2 points, and calculate afterwards the distance to another point.
• imgprocessing.py contains the Image() class, which has image-oriented methods, based on matrix operations, for getting useful information from image data.
• videosensor.py has the VideoSensor() class and several functions related to it. Each instantiated object represent an external FPGA device. Thus, this class has methods for setting up the TCP/IP connection, configuring the FPGA registers, and interact with it.
Getting Started
This section covers the first steps for downloading the main controller software and setting up a PC in order to be able to collaborate developing and testing the project.
System requirements
The software project has been tested correctly in the following OS: • Ubuntu 14
• Ubuntu 16
Besides, the following libraries and tools needs to be installed for running correctly the project:
• OpenCV: open-source library compatible with Python and C++. It offers lots of image processing functions with plenty of documentation. The employed version is OpenCV3.http://opencv.org/
• scikit-image: is an open-source SciPy third party library for dealing with image processing operations. In this project, it is used for image segmentation and shapes classification. It can be obtained more information at
http://scikit-image.org/.
• PySerial: Python library used for communicating through a serial port with external devices. An overview and download information can be obtained athttps://pythonhosted.org/pyserial/pyserial.html.
UviSpace Main Controller Documentation, Release 1.0.0
documentation when they are changed. It allows to create HMTL or PDF documents from plain text (ReStruc-tured text) using a variety of templates or customize a new one. There is a Getting Started tutorial at theofficial webpage. Moreover, it has been used ReadTheDocs for hosting the documentation. It offers tools for compiling the code and generating the HTML files in their own server.https://readthedocs.org/.
Aside from OpenCV, these libraries can be easily installed using pip, by using the requirements.txt file distributed with the source code.
Download the source
The source code is stored and maintained at an online repository that can be accessedhere. The project can be directly downloaded from the repository page. However, it is recommended toinstall gitand then clone the repository into a local repository. This way, you can use the git version control system in order to collaborate to the project and easily synchronize your work with the other developers.
Once you have git installed and configured, type the following instructions for cloning the UviSpace project to your PC:
$ cd /go/to/desired/directory # Set the
˓→terminal working directory
$ git clone https://github.com/UviDTE-UviSpace/uvispace-main-controller # Clone
˓→the project to your local machine
After that, the project will be cloned to a folder named UviSpace-main-controller into the directory you have specified.
Learning
Python
Python is the programming language used for the most of the Software project. It has a vast community, and there are lots of places to learn about it. Prior to developing code for the UviSpace project, it is highly recommended to learn basic programming ideas relating to Python. Think Python1is a great book for Python programming beginners. It is free to read online and download. Just follow the link provided in the Bibliography.
In order to produce quality code, much more reader-friendly, we encourage the readers and future developers of the UviSpaceproject to learn good ways of writing Python code, if they haven’t yet:
• The PEP 82 is a style guide proposed by the Python creator and its community. It is not very long, and the
benefits are really high, making it a highly recommended reading.
• Writing Idiomatic Python3is a book aimed to Python programmers with a prior knowledge, and is not
recom-mended to use it as a first approach to the Python programming. Anyway, it proposes several coding techniques that improve the code readability with little effort.
Bibliography
1Think Python, 1st edition (2012)http://greenteapress.com/wp/think-python/ 2PEP 8 – Style Guide for Python Codehttps://www.python.org/dev/peps/pep-0008/ 3Writing Idiomatic Pythonhttps://jeffknupp.com/writing-idiomatic-python-ebook
Uvirobot package
messenger.py
This module ‘listens’ to speed SPs and sends them through serial port. Usage: messenger.py [-r <robot_id>], [–robotid=<robot_id>]
To communicate with the external slaves, the data has to be packed using a prearranged protocol, in order to be unpacked and understood correctly by the slave.
If it is run as main script, it creates an instance of the class SerMesProtocol for managing the serial port. T When a new speed SP is received, it is sent to the target UGV using the instanced object.
Speed formatting:
The move_robot function has the default speed limit set to [89-165]. These limits are needed when the robot is connected to a DC source with a small intensity limit. If the program is run when the UGV is powered through a USB-B cable, it will move slowly, as the limit is too small to be able to move properly.
When the execution ends, the plotter module is called and the time delays values are plotted on a graph.
navigator.py
Routine for getting UGV poses and publishing speed set points.
The module instantiates a RobotController object and uses its methods for publishing new speed set points.
When calling the module, one argument must be passed, representing the id of the desired robot. It must be the same as the one passed to the messenger.py module.
plotter.py
This module provides utilities for easily drawing plots. It has 3 functions:
• format_plotting: sets the configuration parameters of the plots, in order to use a common format for all the plots. • path_plot: draws 2 plots in the same graph, representing the ideal path of an UGV and the real route it follows.
The X and Y axes represent the 2-D coordinates of the working real space.
• times_plot: Draws the time delays during execution. It is intended to draw 2 plots, one representing the com-munication delays with the slave, and the other representing the waiting times for other modules of the project to give required updates of the speed values.
robot.py
This module communicates with user and sensors for finding paths.
UviSpace Main Controller Documentation, Release 1.0.0
class uvirobot.robot.RobotController(robot_id=1, allowed_orientation_error=8, max_clear_goal_distance=25)
This class contains methods needed to control a robot’s behavior. Parameters
• robot_id (int) – identifier of the robot.
• init (bool) – indicates if the instance has been initialized or not.
• speed_status (dict) – dictionary with iteration values of the kalman filter, linear and angular speeds, and UGV setpoints.
• goal_points (numpy.array float64 (shape=Mx2).) – array that stores the following M goal points for the UGV.
• ideal_path (numpy.array float64 (shape=Nx2).) – array that stores the de-sired path for the robot, with N goal_points.
• real_path (numpy.array float64 (shape=Px2).) – array that stores the P points of the real_path made by the UGV.
• epsilon (float) – difference between angle beta and UGV angle (theta). Angle error to correct. (See ‘control_decision’ function)
• delta_epsilon (float) – difference between the epsilon angles of the current and previous iteration.
• allowed_orientation_error (float) – accepted value of angle error to consider that there is no error. The value entered in the method call is in degrees, and it is transformed into initialization to radians.
• distance (float) – distance between UGV and the next goal point.
• delta_distance (float) – difference between the distances of the current and previ-ous iteration.
• max_clear_goal_distance (float) – accepted value of distance between UGV and the next goal point to consider that there is no error.
NOTE: The units used in the script are: For linear speed, mm/s; for angular speed, rad/s; for distance mm. control_decision(pose)
Receive a new pose and obtain a speed value.
This method calls the ‘get_speed_forward’ or the ‘get_speed_onlyturn’ function to calculate the speed. After calculating the new speed value, call the ‘get_setpoints’ function to transform the speed value into setpoints.
Parameters pose – contains a 2-D position, with 2 cartesian values (x,y) and an angle value (theta). :type pose: dict
delete_goal()
Delete the current goal point of the array.
The current goal point is in the first row of the array. It is deleted when the destination is reached. The goal destination points are stored in the ideal_path array.
get_setpoints(linear, angular)
Receive speed value and transform it into setpoints. Parameters
• linear (float) – linear speed value.
• angular (float) – angular speed value. Returns setpoints values.
Return type (int, int) new_goal(goal)
Receives a new goal and stores it in goal points array.
Parameters goal – contains a 2-D position, with 2 cartesian values (x,y) and an angle value (theta). :type goal: dict
on_shutdown()
Shutdown method, called when execution is aborted. publish_message(step, linear, angular, sp_left, sp_right)
Receives speeds and setpoints and publish them. Parameters
• step (int) – kalman filter iterator counter. • linear (float) – linear speed value. • angular (float) – angular speed value. • sp_left (int) – setpoint left value. • sp_right (int) – setpoint right value.
serialcomm.py
This module contains the class SerMesProtocol().
The aim is to provide a set of methods for performing communication operations with an external device, using a pair of XBee modules (IEEE 802.15.4 protocol). Needless to say, the modules have to be previously configured for being able to communicate with each other, and that task is out of the scope of this module.
The aforementioned class’ methods build up messages following a common structure and send them through serial port to a slave with the same implemented protocol.
Moreover, it offers methods specific to the UGV operation, as the move method, that takes a speeds setpoints inputs and sends them correctly formatted to the slave.
class uvirobot.serialcomm.SerMesProtocol(port, baudrate, stopbits=1, parity=’N’, time-out=0.5)
This is a child of PySerial class and implements a comm. protocol.
This class implements a message-based protocol over the serial port in Master-slave mode: The master (PC) starts communication with the slave(peripheral) sending a message. The slave process the message and returns an answer.
The class uses the serial port to implement this protocol. Parameters
• port (str) – name identifier of the port path in the PC’s OS.
UviSpace Main Controller Documentation, Release 1.0.0
• timeout (float) – Time to wait to achieve the communication. get_soc()
Get the State of Charge (SoC) of the vehicle battery. move(setpoint)
Send a move order to the slave.
Parameters setpoint ([int, int]) – List with UGV speeds, whose elements range from 0 to 255. The first element corresponds to right wheels, and the second element to left wheels. Values are rounded if decimal.
Returns true or false condition which confirms that the message was received. Return type bool
read_message()
Read a message using the serial message protocol.
When the message is read, check the auxiliary bytes for assuring the consistence of the message. Returns
[Rx_OK, fun_code, data, length] • Rx_OK is 0 if an error ocurred.
• fun_code is the non decodified hex-data corresponding to the function code given by the slave.
• data is the non decodified hex-data corresponding to the data given by slave. • length is the size of the main data, in bytes
Return type [bool, str, str, int] ready(tries=10)
Check if the communication channel is ready.
The parameter tries specifies the number of attempts before exiting and raising an error message. Returns returns a true or false condition which confirms that the message was received. Return type bool
send_message(fun_code, data=’‘, send_delay=0.01)
Send a message to slaves formatted with the defined protocol. Parameters
• fun_code (str) – function code of the command that is going to be sent. • data (str) – DATA field of the message.
• send_delay (float) – Delay time to wait between sent bytes.
speedtransform.py
Module with classes to format speed and calculate setpoint.
An instance of the PolySpeedSolver class allows to obtain the speed reference in the range (0, 255) from the resolution of an equation whose coefficients are obtained from a configuration file, and from the values of linear and angular velocity .
An instance of the Speed class represents the speeds of 2WD (2-Wheel- Drive) UGVs, and the attributes and operations related to them. They convert linear-angular speeds into left-right speeds, which is the used format in the Arduino slaves.
They also allow to change the values scale. It is important to know that the Arduino manages speed values ranging from 0 to 255 for each wheel. The first 127 values represent reverse direction speeds, and the last 127 direct direction speeds (127 is null speed).
class uvirobot.speedtransform.Speed(speed=[0, 0], min_value=-0.3, max_value=0.3, spd_format=’linear_angular’, scale=’linear’)
This class manages the speed values compatible with a 2WD vehicle.
The default speed format is linear_angular. This means that the speed is a 2-values array, whose items corre-sponds to the linear and angular speed respectively.
Parameters
• speed ([float, float]) – speed values of the vehicle. If the format is linear_angular, it represents the linear and angular speeds of the vehicle. If the format is 2_wheel_drive, it represents the velocities of the right and left wheels of the vehicle, respectively.
• min_value (float) – Minimum value of the speed attribute • max_value (float) – Maximum value of the speed attribute
• spd_format (str) – Format of the speed. Possible values are stored in the tuple Speed.SPEEDFORMATS.
• scale (str) – Scaling of the speed. Possible values are stored in the tuple Speed.SPEEDSCALES.
check_bounds()
Check that the speed values are inside valid bounds.
If the value is out of bounds, it is rounded to the nearest limit. get_2WD_speeds(rho=0.065, L=0.15, wheels_modifiers=[1, 1])
Obtain two speeds components, one for each side of the vehicle.
It calculates the speed component for each side, when the linear and angular velocities of the vehicle are given. The method also changes the maximum and minimum values.
This calculus responds to the dynamics system proposed on: http://ieeexplore.ieee.org/stamp/stamp.jsp? arnumber=5674957
Parameters
• rho (float) – Parameter of the dynamic model, which represents the vehicle’s wheels diameter, in meters.
• L (float) – Parameter of the dynamic model, which represents the distance between the driving wheels of the vehicle.
• wheels_modifiers ([float, float]) – It is intended to adjust the error between the ideal model and the real system. Thus, it corrects the performance difference between the 2 wheels. It is recommended to tune this values by testing them on the real vehicle. Returns output value for the right and left wheels. Maximum and minimum limits are modified
proportionally to rho.
UviSpace Main Controller Documentation, Release 1.0.0
get_format()
Return the format value. get_max_value()
Return the maximum allowed value for a linear scale. get_min_value()
Return the minimum allowed value for a linear scale. get_scale()
Return the scale value. get_speed()
Return the value of the speed.
linear_transform(new_min, new_max) Change the range of values of the speed.
The method converts the actual speed values and the limits it can take. Parameters
• new_min (float) – absolute minimum that the new speed values may take. • new_max (float) – absolute maximum that the new speed values may take. nonlinear_transform(min_A=30, max_A=100, min_B=160, max_B=220, scale_zero=127)
Make a non-linear conversion of speed values.
Intended to avoid the useless values near to 0 speed on a real UGV, and extreme values that implies high power. It translates the input speeds to useful segments. A characterization of the UGV has been done before performing this rescalation.
This method can only be called if the class’ scale is linear i.e. can’t convert a nonlinear scale to another nonlinear scale.
Transformation
The values range is divided into 2 equal segments (segment A and segment B). If the value belongs to segment A, it will be rescalated between min_A and max_A values. On the contrary, if it belongs to segment B it is rescalated between min_B and max_B. The mid value is assigned to 0.
min_value zero_value max_value |---|---|
segment A segment B
min_A max_A min_B max_B
|---| | |---| scale_zero
Parameters
• [min_A, max_A, min_B, max_B] (int/float) – Limit values for the segments A and B. The transformed values will belong to one of the 2 intervals and 0.
min_A < max_A < scale_zero < min_B < max_B • scale_zero (int) – Null speed value in the new scale. Returns The speed value after being converted to the new scale. Return type int
set_speed(speed, speed_format, speed_scale=’linear’) Set a new speed value.
Input speed must be a 2-values list or tupple. If out of bounds, it will be rounded to the nearest limit. Parameters
• float/int] speed ([float/int,) – new values for the speed attribute. Depend-ing on the format, the values may refer to the linear and angular values, or to the left and right wheels speeds.
• speed_format (str) – The format of the new speed. It has to be a valid one (Check the attribute Speed.SPEEDFORMATS)
• speed_scale (str) – The scale of the new speed. It has to be a valid one (Check the attribute Speed.SPEEDSCALES)
Uvisensor package
UviSpace Main Controller Documentation, Release 1.0.0
client.py
This module contains the Client class, which inherits from socket.socket
• socket.socket class source code can be found in the following link: https://hg.python.org/cpython/file/2.7/Lib/ socket.py
• Previous class is a child of the _socket.socket class. Its source code can be found in the following link: https: //github.com/biosbits/bits/blob/master/python/_socket.py
class uvisensor.client.Client(buffer_size=2048, timeout=2.0) Child class of socket.socket which includes register operations.
It is intended to work with registers of an external FPGA. The register identifiers must correspond with the ones of the HDL circuit implemented in the FPGA.
Used methods from parent class:
•connect((host, port)): connects to specified remote address •close(): close the socket opened with the client
•send(data): send data to the client
•recv(buflen): read the specified number of bytes
•settimeout(timeout): time to wait for the client to respond Parameters
• buffer_size (int) – number of bytes of the incoming data.
• timeout (int or float) – value, in seconds, of the time that will be waited for reading incoming data. This will set the object to timeout mode (By default, it is set to blocking mode).
The class has 2 dictionaries, _REGISTERS and _COMMANDS, that contain all the valid commands that can be sent to the FPGA and the declared registers inside it. When interacting with the FPGA, can only be accessed the commands and registers in these dictionaries.
Registers
•RED/GREEN/BLUE_THRESHOLD: The pixel intensity thresholds for each of the 3 colors are stored in these registers, in the form of (low_threshold, high_threshold).
•IMAGE_SHAPE: •...
close_connection()
Send ‘CLOSE_CONNECTION’ command to FPGA and close TCP/IP socket. open_connection(ip, port)
Create a TCP/IP socket connection.
After connecting, the input buffer is read in order to remove its contents, as a ‘Welcome message’ is automatically generated.
Parameters
• ip (str) – IP address of the device. • port (int) – socket port.
read_data(size)
Perform the input buffer read operation several times.
Parameters size (int) – indicates number of bytes to be read. Returns A data concatenation of all packages read from the input buffer. read_register(regkey)
Read the value of a register and return it formatted. Parameters regkey (str) – register identifier Returns the content of the register
UviSpace Main Controller Documentation, Release 1.0.0
write_command(command, clean_buffer=False) Send a command to the TCP/IP client.
Parameters
• command (str) – FPGA command to be executed.
• clean_buffer (bool) – if True, a clean-up reading of the input buffer is done after writing.
Returns message returned from the FPGA after writing the command. If clean_buffer is False or there was no message, ‘EMPTY_BUFFER’ is returned.
write_register(regkey, value)
Write a value into a register and clean the input buffer. Parameters
• regkey (str) – register identifier that will be written.
• value – data that will be written to the register. It will be converted to a string before sending.
Returns message given back by the FPGA after writing the register
geometry.py
This module contain classes and methods with geometrical operations.
The operations are done in the 2-D space It works with 2-D shapes represented by arrays. Thus, the calculations are based on matrix operations and linear algebra.
class uvisensor.geometry.Triangle(vertices, isglobal=False, cartesian=False) Class for dealing with geometric operations referred to triangles.
An instance of the class represents an isosceles triangle in a 2-D space, with the 2 equal sides being bigger than the base one.
Parameters
• vertices (np.array(shape=3x2)) – vertices coordinates of the triangle object. • isglobal (bool) – Flag that indicates if the coordinate system refers to the 4-quadrant
system (global) or to a local quadrant system.
• cartesian (bool) – This flag indicates if the coordinates are referred to a cartesian system [x,y] instead of the images typical standard [row. column] = [y,x]
get_pose()
Return triangle’s angle and base midpoint, given its vertices.
The coordinates of 3 vertices defining the triangle are used, packed in a single 3x2 array. This method assumes that the triangle is isosceles and the 2 equal sides are bigger than the different one, called base. The following attributes are updated:
•self.sides : array containing the lengths of the 3 sides.
•self.base_index : array index of the minor side. This is also the index of the vertex between the 2 mayor sides, as vertices indexes are the indexes of their opposite sides.
Returns [X,Y] coordinate of the midpoint of the triangle’s base side and orientation angle of the triangle. It is the resulting angle between the horizontal axis and the segment that goes from the triangle’s midpoint to the frontal vertex. It is expressed in radians, in the range [-pi, pi]. Return type float32, float32, float32
get_window(min_value, max_value, k=1.25)
Get the coordinates of a rectangle window around the triangle.
At first, the barycenter of the triangle is calculated. Then, the window is calculated as a square, being its sides’ length k times the triangle’s longest side length and being its center the triangle’s barycenter. The output is stored in the self.window variable
Parameters
• min_value (int or np.array[int,int]) – value or values of the minimum al-lowed coordinates.
• max_value (int or np.array[int,int]) – value or values of the maximum al-lowed coordinates.
• k (int or float) – relative size between the window and the triangle base. It should be bigger than 1. As bigger as it gets, the bigger the window will be.
Returns array representing a square parallel to the horizontal coordinates axe. The first row contains the X and Y minimum values of the square, and the second row contains its X and Y maximum values.
Return type 2x2 np.array
global2local(offsets, K=None, cartesian2image=True) Convert Triangle coordinates to the local coordinates system.
Only absolute coordinates shall be transformed. Lengths and angles are invariant to the coordinate origin. Parameters
• offsets (list[int, int]) – column and row offsets between the local and the global systems.
• K (positive int or float) – Scale ratio to be applied to the points coordinates. • cartesian2image (bool) – If True, a conversion from cartesian coordinates system
to image system is performed. Thus, the output will be of the form of [row,column] instead of [x,y].
Raises ValueError – if the scale ratio is negative. homography(H)
Perform an homography operation to the Triangle vertices.
The homography is a geometrical transformation that obtains the projection of certain points from a plain to another. self.vertices variable is updated
Parameters H (np.array(shape=3x3)) – Homography matrix. Returns the new vertices coordinates values.
in_borders(limits, tolerance=150)
Evaluate if vertices are near a 4-sides polygon perimeter.
In the real world scenario, this method determines if the UGV, represented by the Triangle, is in the borders region of a given 2-D space, defined by an irregular 4-sides polygon.
UviSpace Main Controller Documentation, Release 1.0.0
• limits (iterable of length 4) – Array containing the coordinates of the 4 points defining the borders of the polygon.
• tolerance (int or float) – Maximum allowed distance (mm) to the limits to be considered within the borders region.
Returns flag set to True if the triangle is evaluated to be inside the given polygon Return type bool
inverse_homography(H)
Perform an inverse homography operation to the vertices. Get 𝑋𝑢from the equation (𝑤 · 𝑋) = 𝐻 · 𝑌 .
First of all, get 𝑤·𝑌1 using least squares method. Then, extracts 𝑤1 from the column matrix, with the hypothesis that 𝑌11= 1.
Parameters H (np.array(shape=3x3)) – Homography matrix. Returns the new vertices coordinates values.
local2global(offsets, K=None, image2cartesian=True) Convert Triangle coordinates to the global coordinates system. The function performs 2 transformations:
•Obtains the 4-quadrant coordinates. The input is a coordinate for a 1-quadrant system, and the output corresponds to the 4-quadrant system.
•Move y-axis origin. Initially, for a given image the origin is placed at its top. However, for the used system the origin is placed at the middle of the 4 quadrants.
If indicated, the coordinates system will be transformed to the cartesian one. This is recommended, as the image system does not make sense for a space with origin in the middle.
Only absolute coordinates shall be transformed. Lengths and angles are invariant to the coordinates origin. Finally, a scale ratio K will be applied to the coordinates. The coordinates will be directly multiplied by the ratio.
Parameters
• offsets (list[int, int]) – column and row offsets between the local and the global systems.
• K (positive int or float) – Scale ratio to be applied to the points coordinates. • image2cartesian (bool) – If True, a conversion from image coordinate system to
cartesian system is performed. Thus, the output will be of the form of [x,y] instead of [row,column].
Raises ValueError – if the scale ratio is negative.
class uvisensor.geometry.Segment(point_a, point_b)
This class contains methods for dealing with 2D segments operations. Parameters
• point_a (len-2 tuple or list) – X and Y coordinates of the initial points.
• point_b (len-2 tuple or list) – X and Y coordinates of the end points. distance2point(point)
Return the distance of a point to the nearest segment’s point.
The calculus is based on the dot (scalar) product. The perpendicular projection of a first vector on a second one is the dot product the 2 vectors divided by the modulus of the second:
𝐴.𝐵 = |𝐴| · |𝐵| · 𝑐𝑜𝑠(𝑎𝑙𝑝ℎ𝑎) being |𝐴| · 𝑐𝑜𝑠(𝑎𝑙𝑝ℎ𝑎) the projection of A on B
If the projection is less than 0, it implies that the point is left to the first point (as cos(alpha) is negative), being this first point the nearest one to the target point.
Moreover, if the projection is greater than the modulus of the segment, it implies that the target point is is right to the end point, being this the nearest one.
Finally, the the distance to the segment is obtained and returned using the Pythagoras’ theorem. Returns The distance of the point to the segment
imgprocessing.py
This module contains the Image class, for image processing operations.
The operations implemented in the class methods are focused to the images obtained from the external FPGAs in the UviSpace project. Thus, once obtained a grey scale image, this module provide functions for getting the shapes (triangles) in the image, and then their vertices. Prior to segment the image, a binarization has to be applied, as it eases the segmentation process.
Important note
A point array is written by convention in the form [row, column]. In a cartesian system, the points are expressed as (x,y). Finally, in an image representation (viewer), the typical is to display points coordinates as (x’, y’). They equivalences are the following:
𝑥 = 𝑥′= 𝑐𝑜𝑙𝑢𝑚𝑛 𝑦 = −𝑦′= −𝑟𝑜𝑤
Thus, special care has to be taken when dealing with operations in different scopes e.g. trigonometric operations will be handled with the cartesian values, while image operations are normally performed with the array convention. Finally, when sending the array values to a viewer or to an external device, the image representation mentioned above is the typical used system.
class uvisensor.imgprocessing.Image(image, contours=[]) Class with image processing methods oriented to UGV detection.
Parameters
• image (np.array) – original grey scale image.
UviSpace Main Controller Documentation, Release 1.0.0
binarize(thresholds)
Get a binarized image from a grey image given the thresholds.
The input image can only have one dimension. This method is intended to work with 3-component thresh-old values stored in a single 30-bit register:
•register[0 to 10] : red component thresholds •register[10 to 20] : green component thresholds •register[10 to 30] : blue component thresholds
The raw binary image contains a lot of noise. As it is very low around the triangles, masks around them are used to get rid of the noise in the rest of the image.
:param [int or float, int or float] thresholds [minimum and ] maximum values between whom the im-age intensity values will be accepted as 1 (rescaled to 255). Values greater than the maximum and smaller than the minimum will be truncated to 0.
Return bin_image Image of the same size as the input image with only 255 or 0 values (Equiv-alent to 1 and 0), according to the input threshold values.
Return type binary numpy.array(shape=MxN)
correct_distortion(kx=0.035, ky=0.035, only_contours=True) Correct barrel distortion on contours or on the whole image.
The distortion is corrected using a 2nd polynomial equation for every pixel with coordinates (𝑋𝑑, 𝑌𝑑). The resulting corrected coordinates (𝑋𝑢, 𝑌𝑢) are obtained with the following equations:
𝑋𝑢= (𝑋𝑑− 𝐶𝑥) * (1 + 𝑘𝑥* 𝑟2) + 𝐶𝑥 𝑌𝑢= (𝑌𝑑− 𝐶𝑦) * (1 + 𝑘𝑦* 𝑟2) + 𝐶𝑦 𝑟 = [(𝑋𝑑− 𝐶𝑥)2+ (𝑌𝑑− 𝐶𝑦)2]/[(𝐶𝑥2+ 𝐶 2 𝑦) * 2] Parameters
• kx (float) – X-Axe Distortion coefficient of the lens. • ky (float) – Y-Axe Distortion coefficient of the lens.
• only_contours (bool) – Specify if the correction is to be applied to the whole image or only to the contours.
get_shapes(tolerance=8, get_contours=True) Get the shapes’ vertices in the binarized image. Update the self.triangles attribute.
The shape is obtained using the Marching Cubes Algorithm. Once obtained, the vertices are calculated using the Ramer-Douglas-Peucker Algorithm. Both are implemented on the skimage library, and there is more information on its docs.
if the kwarg get_contours if False, it is assumed that the contours are already known (stored in variable self.contours). If this is the case, the marching cubes algorithm is omitted.
Parameters
• tolerance (float) – minimum distance between an observed pixel and the previous vertices pixels required to add the first one to the vertices list.
• get_contours (bool) – specify if the Marching Cubes Algorithm is applied to the binarized image. Specifically set to False when the binarization algorithm is implemented in the external device (i.e. the FPGA).
Returns vertices of the N shapes detected on the image. each element contains an Mx2 np.rray with the coordinates of the M vertices of the shape.
Return type list
videosensor.py
This module contains the VideoSensor class and related functions.
The functions are calls to the VideoSensor class methods in order to capture images and then work with them using the imgprrocessing.Image class and its methods.
class uvisensor.videosensor.VideoSensor(filename=’‘, scale=2.0) This class contains methods for dealing with FPGA-camera system.
Parameters
• filename (str) – Path to the configuration file of the camera. The path shall be passed relative to this script directory.
• scale (float) – scale ratio of the camera. relationship between the full resolution of the FPGA and the actual resolution that is being usedd. By, default, the FPGA has a 2:1 scale i.e. only half of the pixels are being used.
capture_frame(gray=True, tries=20, output_file=’‘) This method requests a frame to the FPGA.
Parameters
• gray (bool) – if true, a gray-scale image will be requested. If false, the requested image will be RGB.
• tries (int) – number of times that the system will try to obtain the requested image. After the last try, the system will exit.
• output_file (str) – URL name of the output file were the image will be stored. If left blank, the image won’t be saved.
Returns image contained in an array with dimensions specified in the configuration file. If gray color is True, the dim value will be 1, and 3 for False (representing color images). dim equals to the number of components per pixel.
Return type MxNxdim numpy.array
configure_tracker(tracker_id, min_x, min_y, width, height) Send to FPGA rectangle parameters for defining a tracker.
Parameters
• tracker_id (int) – identifier of the detected object.
• min_x (int) – value of the X cartesian coordinate of the tracker window. • min_y (int) – value of the Y cartesian coordinate of the tracker window. • width (int) – value of the width (X axis) of the tracker window
UviSpace Main Controller Documentation, Release 1.0.0
NOTE: It is mandatory that the coordinates and dimensions passed to the FPGA are integers. Other types like float are not valid and the FPGA will not recognize them.
connect_client()
Read TCP/IP parameters in config file and connect to the device. disconnect_client()
Close TCP/IP connection with the device.
If disconnect_client() function is not called, the socket won’t be able to be reopened. get_homography_array()
Get an homography array from the configuration file. get_limits_array()
Get the limits array from the configuration file. get_offsets()
Get the offset of the sensor respect to the iSpace center.
The row offset of the camera images corresponds to the images height and the column offset corresponds to the images width.
Returns row and column offsets i.e. [row_offset, col_offset] Return type list[float, float]
get_register(register)
Read the content of the specified register.
Parameters register (str) – key identifier of a valid register name of the FPGA. The full list of valid keys and their associated name can be found on the documentation of the client.Clientclass.
Returns the data stored in the indicated register. load_configuration(write2fpga=True)
Load the config file and send the configuration to the FPGA.
•Read camera and sensor parameters in self.filename. They are then stored in the self._params variable. •If write2fpga flag is True, write parameters in the FPGA registers by calling set_register() method.
Finally, send ‘CONFIGURE_CAMERA’ command to FPGA. read_conffile(filename)
Look for a configuration file on the given path and read it. set_register(register, value)
Write a value into an FPGA register. Parameters
• register (str) – key identifier of valid register name of the FPGA. The full list of valid keys and their associated name can be found on the documentation of the client.Client class.
• value (int or tuple/list) – the value that will be written to the register. It is mandatory to send it as string type. Thus, the value has to be converted. For tuples or lists, brackets or parenthesis are not allowed, so they have to be eliminated.
Returns message obtained back from the FPGA after writing into the register. Examples
• sent_value = ‘6’ —> OK
• sent_value = ‘(3.45, 2.21)’ —> No OK • sent_value = ‘3.45, 2.21’ —> OK
kalmanfilter.py
Module with a class for implementing a Kalman filter.
The Kalman filter is a well-known algorithm for improving the localization of an object given measures of its position and a model of its behaviour.
The algorithm is defined by two main stages, i.e.: the state prediction, that estimates the state (position) of the object given the previous one and the value of the input variables; and the fusion with the measurements, that filters the values obtained by the sensors and the prediction calculated on the previous stage.
Relevant documentation:
• http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
• http://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter. pdf
• http://biorobotics.ri.cmu.edu/papers/sbp_papers/integrated3/kleeman_kalman_basics.pdf
class uvisensor.kalmanfilter.Kalman(var_dim=3, input_dim=2) Class for implementing a linear Kalman Filter.
It contains two methods for implementing the two stages of the Kalman filter (predict and update), that should be run alternating each one; and two methods for updating the noise matrices.
The noise distributions can be changed before the update stage, and the Kalman gain will vary accordingly. predict(ext_input, delta_t)
Estimate the new position given the external input vector.
The matrix B, that maps input values to the corresponding state change, is directly proportional to the time step since the last iteration.
The function predicts the new position of the object, applying the input vector to the previous state. The function resolves the predicted position using 2 equations, one for the mean value and the other one for the covariance matrix:
𝑥(𝑡 + 1|𝑡) = 𝐹 · 𝑥(𝑡) + 𝐵 · 𝑢(𝑡) 𝑃 (𝑡 + 1|𝑡) = 𝐹 · 𝑃 (𝑡) · 𝐹′
Hypothesis: The angular speed between two iterations does not affect on the new position (x,y) of the object, as the time is considered small enough between iterations.
Parameters
• ext_input (np.array(shape = (input_dim x 1))) – values of the control variables.
UviSpace Main Controller Documentation, Release 1.0.0
Returns The predicted state means, and the predicted covariance matrix. set_measurement_noise(noise)
Set the measurement noise matrix to the given values
A bigger measurement noise will make the Kalman filter to diminish the weight of the state measurement given by the system sensors.
If the noise is uncorrelated, the input can be a tuple or list, and the output will be a diagonal matrix with the given values.
Note that the input dimensions must coincide with the dimensions of the system state variables. Parameters noise (’var_dim’ length list or tuple; or a
np.array(shape = (var_dim x var_dim))) – new values for the measure-ment noise.
Returns the new measurement error matrix. set_prediction_noise(noise)
Set the prediction noise matrix to the given values
A bigger estimation or prediction noise will make the Kalman filter to diminish the weight of the state prediction on the final result.
If the noise is uncorrelated, the input can be a tuple or list, and the output will be a diagonal matrix with the given values.
Note that the input dimensions must coincide with the dimensions of the system state variables.
NOTE: A more more accurate model would calculate the process (prediction) noise depending on the magnitude of the input variables. According to the propagation of errors theory, the error of a multiplication is bigger when each of the products is bigger i.e. in this case, the predicted position error is bigger when the speeds are bigger (also when the time difference is bigger).
Parameters noise (’var_dim’ length list or tuple; or a np.array(shape = (var_dim x var_dim))) – new values for the process noise.
Returns the new process error matrix. update(measurement)
Update the UGV position, merging the measurement and prediction
This is the second stage of the Kalman filter. Prior to applying it, the ‘predict’ stage has to be run, in order to obtain an estimated position and compare with the given measurement (‘z’).
𝑆(𝑡 + 1) = 𝐻 · 𝑃 (𝑡 + 1|𝑡) · 𝐻′+ 𝑅(𝑡 + 1) 𝐾(𝑡 + 1) = 𝑃 (𝑡 + 1|𝑡) · 𝐻′· 𝑆−1(𝑡 + 1)
𝑥(𝑡 + 1) = 𝑥(𝑡 + 1|𝑡) + 𝐾(𝑡 + 1) · (𝑧 − 𝐻 · 𝑥(𝑡 + 1|𝑡)) 𝑃 (𝑡 + 1) = 𝑃 (𝑡 + 1|𝑡) − 𝐾(𝑡 + 1) · 𝐻 · 𝑃 (𝑡 + 1|𝑡)
The final filtered position will be a compromise between both the prediction and the measurement, and the weight of each one will be given by the kalman gain (‘K’), which depends, among other variables, on the value of the process and measurement noises.
Parameters measurement (np.array(shape = (var_dim x 1))) – value of the sensors input.
Returns The filtered state means, and the filtered covariance matrix.
Tutorials and manuals
Python virtual environments
In order to ease the management of the project dependencies, it is recommended to make use of Python virtual envi-ronments. Virtual environments are used to provide isolation between projects, keeping all the required libraries in a directory available only to the virtual environment, and thus avoid polluting the global sites-packages directory. There is a utility called virtualenvwrapper that simplifies the management of virtual environments, and we will use this utility to obtain a clean virtual environment for our project.
To install this utility, run the following command (root permissions may be necessary): $ pip install virtualenvwrapper
Once the installation has finished, open up the .bashrc file and add the following lines:
export WORKON_HOME=$HOME/.virtualenvs # Virtual environments folder
source /usr/local/bin/virtualenvwrapper.sh # Enable virtualenvwrapper commands
To create a new virtual environment, run the following command:
$ mkvirtualenv uvispace # Syntax: mkvirtualenv (name for the virtual environment)
(uvispace) $ # Prompt gets updated to reflect the virtual environment is
˓→active
In case the virtual environment is already created, it is only necessary to activate it with the following command: $ workon uvispace # Syntax: workon (name for the virtual environment)
(uvispace) $ # Prompt gets updated to reflect the virtual environment is active
If we want to stop using a virtual environment, run the following command: (uvispace) $ deactivate
$ # Prompt gets updated to reflect the virtual environment is
˓→no longer active
By default, mkvirtualenv uses the system default Python interpreter. If a specific version of python is required, it can be specified with the -p parameter:
$ mkvirtualenv -p python2 (name) # Python2 virtual environment
$ mkvirtualenv -p python3.4 (name) # Python3.4 virtual environment
When entering a new virtual environment, there will be no access to the software libraries previously installed on the system, as the purpose of this tool is to have an isolated environment. To add a new library to the virtual environment, you only have to install it using pip (remember to previously activate the environment), and the same rule works for other pip commands like uninstall or freeze:
(uvispace) $ pip install <library-name>
Finally, certain projects, like uvispace, have predefined libraries and versions and they are specified under a require-ments.txtfile (The name does not have to be necessary the same). In this case, these libraries can be automatically installed on the virtual environment using the following command:
(uvispace) $ pip install -r requirements.txt
UviSpace Main Controller Documentation, Release 1.0.0
$ lsvirtualenv # List all available virtual environments
$ rmvirtualenv (name) # Deletes a virtual environment
A more extensive reference on the virtualenvwrapper utility can be found on itsdocumentation.
Release Notes
v1.0.0
Initial operative release.
• Python project for the Data Fusion Controller. Allows the control of a single UGV. Establishes the communi-cation with the UGV via XBee modules and with the Camera nodes via Ethernet.
Index
B
binarize() (uvisensor.imgprocessing.Image method),22
C
capture_frame() (uvisensor.videosensor.VideoSensor method),24
check_bounds() (uvirobot.speedtransform.Speed method),14
Client (class in uvisensor.client),17
close_connection() (uvisensor.client.Client method),18 configure_tracker() (uvisensor.videosensor.VideoSensor method),24 connect_client() (uvisensor.videosensor.VideoSensor method),25 control_decision() (uvirobot.robot.RobotController method),11 correct_distortion() (uvisensor.imgprocessing.Image method),23
D
delete_goal() (uvirobot.robot.RobotController method), 11
disconnect_client() (uvisensor.videosensor.VideoSensor method),25
distance2point() (uvisensor.geometry.Segment method), 22
G
get_2WD_speeds() (uvirobot.speedtransform.Speed method),14
get_format() (uvirobot.speedtransform.Speed method), 14 get_homography_array() (uvisen-sor.videosensor.VideoSensor method),25 get_limits_array() (uvisensor.videosensor.VideoSensor method),25 get_max_value() (uvirobot.speedtransform.Speed method),15 get_min_value() (uvirobot.speedtransform.Speed method),15 get_offsets() (uvisensor.videosensor.VideoSensor method),25
get_pose() (uvisensor.geometry.Triangle method),19 get_register() (uvisensor.videosensor.VideoSensor
method),25
get_scale() (uvirobot.speedtransform.Speed method),15 get_setpoints() (uvirobot.robot.RobotController method),
11
get_shapes() (uvisensor.imgprocessing.Image method), 23
get_soc() (uvirobot.serialcomm.SerMesProtocol method),13
get_speed() (uvirobot.speedtransform.Speed method),15 get_window() (uvisensor.geometry.Triangle method),20 global2local() (uvisensor.geometry.Triangle method),20
H
homography() (uvisensor.geometry.Triangle method),20
I
Image (class in uvisensor.imgprocessing),22
in_borders() (uvisensor.geometry.Triangle method),20 inverse_homography() (uvisensor.geometry.Triangle
method),21
K
Kalman (class in uvisensor.kalmanfilter),26
L
linear_transform() (uvirobot.speedtransform.Speed method),15
load_configuration() (uvisensor.videosensor.VideoSensor method),25
local2global() (uvisensor.geometry.Triangle method),21
M
move() (uvirobot.serialcomm.SerMesProtocol method), 13
N
new_goal() (uvirobot.robot.RobotController method),12
nonlinear_transform() (uvirobot.speedtransform.Speed method),15
O
on_shutdown() (uvirobot.robot.RobotController method), 12
open_connection() (uvisensor.client.Client method),18
P
predict() (uvisensor.kalmanfilter.Kalman method),26 publish_message() (uvirobot.robot.RobotController
method),12
R
read_conffile() (uvisensor.videosensor.VideoSensor method),25
read_data() (uvisensor.client.Client method),18
read_message() (uvirobot.serialcomm.SerMesProtocol method),13
read_register() (uvisensor.client.Client method),18 ready() (uvirobot.serialcomm.SerMesProtocol method),
13
RobotController (class in uvirobot.robot),10
S
Segment (class in uvisensor.geometry),21
send_message() (uvirobot.serialcomm.SerMesProtocol method),13
SerMesProtocol (class in uvirobot.serialcomm),12 set_measurement_noise() (uvisensor.kalmanfilter.Kalman method),27 set_prediction_noise() (uvisensor.kalmanfilter.Kalman method),27 set_register() (uvisensor.videosensor.VideoSensor method),25
set_speed() (uvirobot.speedtransform.Speed method),15 Speed (class in uvirobot.speedtransform),14
T
Triangle (class in uvisensor.geometry),19
U
update() (uvisensor.kalmanfilter.Kalman method),27 uvirobot.messenger (module),10 uvirobot.navigator (module),10 uvirobot.plotter (module),10 uvirobot.robot (module),10 uvirobot.serialcomm (module),12 uvirobot.speedtransform (module),13 uvisensor.client (module),17 uvisensor.geometry (module),19 uvisensor.imgprocessing (module),22 uvisensor.kalmanfilter (module),26 uvisensor.videosensor (module),24
V
VideoSensor (class in uvisensor.videosensor),24
