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 communication 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.

It contains a class, RobotController, that represents a real UGV, and contains functionality for publishing new speed values, UGVs attributes such as the robot_id or its speed values. It imports pathtracker, for calculating the robot navigation values.

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 desired 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 previous 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', timeout=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. baudrate (int) – communications speed between the XBee modules. stopbits (int) – Number of bits at the end of each message. parity (str) – Message parity. None by default 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 corresponds 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.
Return type:	np.array([V_Right, V_Left])

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. Depending 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)