Network Tutorial 1 : UDP Communication with Python

In this tutorial, you will learn how to exchange information with an external Python program using the Vortex UDP extensions.

Communication between multiple simulation software can be very useful in order to integrate models coming from different platforms. In Vortex®, this is made possible by using the UDP communication protocol to exchange information between the Vortex simulation and other software running on a local or remote network.

The example used in this tutorial is a simplified cruise control of a sports car. When activated, the cruise control automatically regulates the throttle of the engine in order to keep the speed of the car constant to the current value. The cruise control program is coded in Python and will be ran on the same machine as Vortex.

For more information about the UDP extensions, consult the Network Extensions documentation.

Configuring UDP Extensions in the Sports Car Mechanism

In this section, we will add "Receive" and "Send" UDP extensions to the car mechanism allowing information to be exchanged with the Python program.

1. Open the sports car vehicle workshop mechanism found in your Vortex Demo Scenes folder: (../Equipment/Vehicle_Sports_Car/Dynamic/Sports Car_Workshop.vxmechanism).

   This car has all of the parts, constraints, collision geometry, car model and driving controls already defined. Only the cruise control program needs to be added.
2. In the Network section of the Toolbox, insert a UDP Receive object in the Vehicles folder (by drag-and-drop) in the Explorer. Repeat this step with a UDP Send object.
3. In the Network section of the Toolbox, insert a UDP Receive in the Vehicles folder (by drag-and-drop) in the Explorer.
4. From the Explorer, right-click on the UDP Receive object and select Edit from the drop down menu.
5. Add an output and change its name to Throttle. Make sure it type is set to Double and Physical Dimension to None, then click on "Ok".
   

6. In the Parameters section of the UDP Receive Properties, change the Local port value to 20103.

   The port number 20103 was used for simplicity, since the default remote port used by Vortex is 20102. Any known unused port can be used. A list of known TCP/UDP ports can be found here

   

7. From the Explorer, right-click on the newly added UDP Send object and select Edit from the drop down menu.

8. Add three inputs with the following properties:

   Name	Type	Physical Dimension
   Current Speed	Double	None
   Current Throttle	Double	None
   Cruise Control Active	Double	None

   Following the order is important because it will have to match the order in which the data is packed in the cruise control program.
9. Save the mechanism in your working directory.

Establishing Connections

In this section, we will connect the inputs and output from the UDP communication to variables from the components in the mechanism.

1. From the Basics section in the Toolbox, insert a Connection Container. Rename it Cruise Control Connections.
2. Open the Connections Editor by double-clicking on it.

3. Locate the following objects in the Explorer and insert the listed input or output from their Properties in the connection container:

   Path > Object	Input/Output (Section)
   Vehicles > UDP Receive	Throttle (Outputs)
   Vehicles > UDP Send	Current Speed; Current Throttle; Cruise Control Active (Inputs)
   Roles > Joystick	ToggleButton4 (Outputs)
   Vehicles > Car FWD > Dynamic Component	Input Throttle (Inputs > Throttle Logic); Speed (Outputs > Chassis)
   Vehicles > Vehicle Controller	Throttle (Outputs)

4. Link the inputs and outputs together as shown in the picture:
   
5. Save the mechanism.

Coding the Python Cruise Control Program

In this section, we configure the UDP communication in the python cruise control program.

1. Open the cruise control program found in your Vortex Demo Scenes folder: (../Equipment/Vehicle_Sports_Car\Cruise Control\Python\Cruise Control_UDP_Workshop.py).

   This file already includes the cruise control logic. It is only missing the functions needed to establish UDP communication with Vortex Studio.

2. Two libraries need to be imported. socket, which establish the UDP connection and struct, which allows packing and unpacking the data sent via UDP. Add the following code:

   import socket
   import struct

3. Declare the IP address, the receive port and the send port. Add the following code:

   # Setting communication IP and Ports. Send and Receive ports should be different.
   UDP_IP = "127.0.0.1"
   UDP_RECEIVE_PORT = 20102
   UDP_SEND_PORT = 20103

4. Create an UDP socket named socket. The family is socket.AF_INET (IP) and the type is socket.SOCK_DGRAM (UDP). To make sure we get an error if the communication fail, add an error message if a socket.error exception is detected. Add the following code:

   # Create a socket
   try:
   	socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
   except socket.error:
   	print ("Failed to create socket")

5. Use the socket.bind function to bind the socket with the previously defined IP address and receive port. Add the following code:

   # Bind receiving port
   socket.bind((UDP_IP, UDP_RECEIVE_PORT))

6. In order to synchronize the Cruise Control program to the Vortex Simulation, use socket.setblocking and set the argument to True. This function halts the program until data is received from the socket. Add the following code:

   # Set the socket to blocking
   socket.setblocking(True)

The code from the following steps will be added inside the while loop where the cruise control logic is defined. This loop runs at about 60 Hertz, which is the default refresh rate of a Vortex simulation.

1. Use socket.recvfrom to receive data coming from Vortex via the communication port to receiveddata. Use 1024 bytes as buffer. Add the following code:

   # Receive Data from Vortex
   receiveddata = socket.recvfrom(1024)

2. Use struct.unpack to unpack the data contained in receiveddata into datastructure. Since the expected data contains two doubles(d) and one boolean(?), the first argument will be '<dd?'. Since receiveddata is a tuple with the format (<data>, <address>), the second argument is index 0 of receiveddata. datastructure is a tuple containing (<Current Speed>, <Current Throttle>, <Cruise Control Active>). Add the following code:

   # Unpack data according to Vortex UDP Send extension data structure.
   # See Format Characters section at this link : https://docs.python.org/2/library/struct.html
   datastructure = struct.unpack('<dd?', receiveddata[0])

3. Using socket.sendto, send outgoingmessagedate to the previously defined IP address and send port. Add the following code:

   # Send data
   socket.sendto(outgoingmessagedata,(UDP_IP, UDP_SEND_PORT))

Here's the full code to copy :

import socket
import struct


def PID(error, timeStep, P=1, I=0, D=0):
    global integralGain
    global lastError

    propGain = P + error
    integralGain = min(0.2, max(-0.2, integralGain + I * timeStep * error))
    derivativeGain = D * (error - lastError) / timeStep

    lastError = error
    return propGain + integralGain + derivativeGain

lastError = 0
integralGain = 0

# Setting communication IP and Ports. Send and Recieve ports should be different
UDP_IP = "127.0.0.1"
UDP_RECEIVE_PORT = 20102
UDP_SEND_PORT = 20103

# Create a socket
try:
   socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
except socket.error:
   print("Failed to create socket")

# Bind receiving port
socket.bind((UDP_IP, UDP_RECEIVE_PORT))

# Set the socket to blocking
socket.setblocking(True)

# Initialize toggle values (part of the Cruise control logic)
currentcruisespeed = 0.0
previouscruisestate = False
increasetoggle = False
decreasetoggle = False

# Infinite loop
while True:
    #Recieve Data from Vortex
    receiveddata = socket.recvfrom(1024)  # buffer size is 1024 bytes
    print(receiveddata)
    #Unpack data according to Vortex UDP Send extension data structure. See Format Characters section at this link : https://docs.python.org/2/library/struct.html
    #In this example, data structure is 4 Doubles and 3 Booleans
    datastructure = struct.unpack('<dddd???d', receiveddata[0])
    
    #Name variables
    Current_Speed = datastructure[0]
    Current_Throttle = datastructure[1]
    Speed_coefficient = datastructure[2]
    Cruise_increment = datastructure[3]
    Cruise_Control_Active = datastructure[4]
    Cruise_Control_Increase_Speed = datastructure[5]
    Cruise_Control_Decrease_Speed = datastructure[6]
    Time_Step = datastructure[7]
    
    #Cruise control logic
    if not increasetoggle and Cruise_Control_Increase_Speed:
        increasetoggle = True
        currentcruisespeed += Cruise_increment
        print ("Cruise speed set at", int(currentcruisespeed), "km/h")
    elif increasetoggle and not Cruise_Control_Increase_Speed:
        increasetoggle = False

    if not decreasetoggle and Cruise_Control_Decrease_Speed:
        decreasetoggle = True
        currentcruisespeed -= Cruise_increment
        print ("Cruise speed set at", int(currentcruisespeed), "km/h")
    elif decreasetoggle and not Cruise_Control_Decrease_Speed:
        decreasetoggle = False

    if not previouscruisestate and Cruise_Control_Active:
        previouscruisestate = True
        currentcruisespeed = int( Current_Speed )
        print ("Cruise control ON")
        print ("Cruise speed set at", int(currentcruisespeed), "km/h")
    elif previouscruisestate and not Cruise_Control_Active:
        previouscruisestate = False
        currentcruisespeed = 0.0
        lastdesiredthrottle = 0.0
        print ("Cruise control OFF")

    if Cruise_Control_Active:

        Throttle_Value = PID(currentcruisespeed - Current_Speed, Time_Step, 0.1, 0.05, 0.01)
        Current_Cruise_Control_Speed = currentcruisespeed

    else:
        Throttle_Value = Current_Throttle
        Current_Cruise_Control_Speed = 0.0
    
    #Pack data according to Vortex UDP Recieve extension data structure. See Format Characters section at this link : https://docs.python.org/2/library/struct.html
    #In this example, data structure is 2 Doubles
    outgoingmessagedata = struct.pack('<dd', Throttle_Value, Current_Cruise_Control_Speed)
    #Send data
    socket.sendto(outgoingmessagedata, (UDP_IP, UDP_SEND_PORT))

Testing the Cruise Control

In this section, we will add the sports car mechanism to an empty scene and test the newly created cruise control script.

1. Open the Sports Car Workshop scene found in your Vortex Demo Scenes folder: (../Scenario/SportsCar Scene/Sports Car_Workshop.vxscene).
2. From the Basics section of the Toolbox, add a Mechanisms From Files
3. If your Sports Car Mechanism is open on another tab, choose it from the list of Opened Resources. If not, click on Browse and locate it.
4. Move the mechanism on the road or in a location where it is going to be easy to maneuver.
5. Run Cruise_Control_UDP_Workshop.py using your Python interpreter. If using PyCharm, press Ctrl+Shift+F10.
6. Test (F7) the scene in Vortex.
7. Drive the car in Vortex. If using an Xbox controller, pressing the Y button will activate or deactivate the cruise control. Notice that when the cruise control is activated, the speed of the car is stable even if using the throttle joystick. Deactivating the cruise control lets you regain control of the car's acceleration.

Up Next : Network Tutorial 2 : UDP Communication with Matlab Simulink