robot looks at you
This commit is contained in:
parent
19bd8a2f6f
commit
482d05f252
|
|
@ -0,0 +1 @@
|
|||
export UNIFI_PASSWORD=""
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
.env
|
||||
__pycache__/
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
import time
|
||||
import math
|
||||
|
||||
|
||||
class BBoxProcessor:
|
||||
|
||||
def __init__(self):
|
||||
self.last_boxes = []
|
||||
self.last_timestamps = []
|
||||
|
||||
def get_joystick_position_from_new_set_of_bboxes(self, new_boxes: list) -> list:
|
||||
"""
|
||||
Given a set of bounding boxes, returns the joystick position that would move the robot to the closest one.
|
||||
"""
|
||||
new_boxes = self.process_next(new_boxes)
|
||||
closest_box = self.get_box_closest_to_center(new_boxes)
|
||||
if closest_box is None:
|
||||
return [0, 0]
|
||||
return self.get_normalized_box_position(closest_box)
|
||||
|
||||
def bbox_distance(self, box1: list, box2: list) -> float:
|
||||
"""
|
||||
Calculates the distance between two bounding boxes.
|
||||
"""
|
||||
return math.sqrt(
|
||||
((box1[0] + box1[2] / 2) - (box2[0] + box2[2] / 2)) ** 2
|
||||
+ ((box1[1] + box1[3] / 2) - (box2[1] + box2[3] / 2)) ** 2
|
||||
)
|
||||
|
||||
def process_next(self, new_boxes, dist_threshold=80) -> list:
|
||||
"""
|
||||
Receives a reading of bounding boxes and processes them.
|
||||
It compares previous bounding boxes with the new ones: If a previous box is close enough to an new one, it is considered the same object.
|
||||
It also checks if previous boxes have outlived their last appearance, and if so, they are not added to the new list.
|
||||
"""
|
||||
timestamp = time.time()
|
||||
time_to_live = 0.5
|
||||
|
||||
new_boxes_list = []
|
||||
new_timestamps_list = []
|
||||
for new_box in new_boxes:
|
||||
new_boxes_list.append(new_box)
|
||||
new_timestamps_list.append(timestamp)
|
||||
|
||||
min_distance = dist_threshold
|
||||
for index, box in enumerate(self.last_boxes):
|
||||
distance = 9999
|
||||
closest = None
|
||||
for new_box in new_boxes:
|
||||
new_distance = self.bbox_distance(box, new_box)
|
||||
if new_distance < min_distance and new_distance < distance:
|
||||
distance = new_distance
|
||||
closest = new_box
|
||||
|
||||
if (
|
||||
closest is None
|
||||
and timestamp < self.last_timestamps[index] + time_to_live
|
||||
):
|
||||
new_boxes_list.append(box)
|
||||
new_timestamps_list.append(timestamp)
|
||||
|
||||
self.last_boxes = new_boxes_list
|
||||
self.last_timestamps = new_timestamps_list
|
||||
return new_boxes_list
|
||||
|
||||
def get_box_closest_to_center(self, box_list: list) -> list:
|
||||
"""
|
||||
Returns the BBox closest to the center (500, 500)
|
||||
"""
|
||||
closest = []
|
||||
closest_distance = 9999
|
||||
for box in box_list:
|
||||
distance = math.sqrt(
|
||||
(box[0] + box[2] / 2 - 500) ** 2 + (box[1] + box[3] / 2 - 500) ** 2
|
||||
)
|
||||
if distance < closest_distance:
|
||||
closest = box
|
||||
closest_distance = distance
|
||||
return closest
|
||||
|
||||
def get_normalized_box_position(self, box: list) -> list:
|
||||
"""
|
||||
Returns the normalized position of a bounding box, shringking its range from 0 to 1000 to 0 to 1
|
||||
"""
|
||||
return [(box[0] + box[2] / 2) / 500.0 - 1, (box[1] + box[3] / 2) / 500 - 1]
|
||||
|
|
@ -0,0 +1,266 @@
|
|||
import struct
|
||||
import random
|
||||
|
||||
from Communication.SocketConnection import SocketConnection
|
||||
|
||||
# All MODBUS/TCP ADU are sent via TCP to registered port 502.
|
||||
# Remark : the different fields are encoded in Big-endian
|
||||
|
||||
# A Modbus frame is composed of an Application Data Unit (ADU), which encloses a Protocol Data Unit (PDU).
|
||||
# A dedicated header is used on TCP/IP to identify the MODBUS Application Data Unit.
|
||||
# It is called the MBAP header (MODBUS Application Protocol header)
|
||||
#
|
||||
# ADU = MBAP Header + PDU,
|
||||
# PDU = Function code + Data.
|
||||
|
||||
# Below the components of the Modbus TCP/IP are listed together with their size in bytes:
|
||||
# +---------------------------------+
|
||||
# | Application Data Unit (ADU) |
|
||||
# +---------------+-----------------+
|
||||
# | **Component** | **Size** (bytes)|
|
||||
# +---------------+-----------------+
|
||||
# | MBAP Header | 7 |
|
||||
# +---------------+-----------------+
|
||||
# | PDU | N |
|
||||
# +---------------+-----------------+
|
||||
|
||||
# The MBAP header is 7 bytes long and contains the following fields:
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Modbus Application Protocol header (MBAP) |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | **Field** | **Length** (bytes) | **Description** |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Transaction identifier | 2 | Identification of a |
|
||||
# | | | Modbus request/response transaction. |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Protocol identifier | 2 | Protocol ID, 0 = Modbus protocol |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Length | 2 | Number of following bytes |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Unit identifier | 1 | Identification of a remote slave |
|
||||
# | | | connected on a serial line or bus |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
|
||||
# The Protocol Data Unit has a variable length and consists of the following fields:
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Protocol Data Unit (PDU) |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | **Field** | **Length** (bytes) | **Description** |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Function code | 1 | Function codes as in other variants |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
# | Data | n | Data as response or commands |
|
||||
# +------------------------+--------------------+--------------------------------------+
|
||||
|
||||
# Response:
|
||||
# For a normal response, slave repeats the function code. Should a slave want to report an error,
|
||||
# it will reply with the requested function code plus 128 (hex 0x80) (3 becomes 131 = hex 0x83),
|
||||
# and will only include one byte of data, known as the exception code.
|
||||
|
||||
# A normal response frame would look like this:
|
||||
# +----------------------------------+
|
||||
# | Response (ADU) |
|
||||
# +----------------+-----------------+
|
||||
# | **Component** | **Size** (bytes)|
|
||||
# +----------------+-----------------+
|
||||
# | MBAP Header | 7 |
|
||||
# +----------------+-----------------+
|
||||
# | Function code | 1 |
|
||||
# +----------------+-----------------+
|
||||
# | Data | n |
|
||||
# +----------------+-----------------+
|
||||
|
||||
# An error response frame is 9 bytes long with the following fields
|
||||
# +----------------------------------+
|
||||
# | Response (ADU) (Error) |
|
||||
# +----------------+-----------------+
|
||||
# | **Component** | **Size** (bytes)|
|
||||
# +----------------+-----------------+
|
||||
# | MBAP Header | 7 |
|
||||
# +----------------+-----------------+
|
||||
# | Function code | 1 |
|
||||
# +----------------+-----------------+
|
||||
# | Exception code | 1 |
|
||||
# +----------------+-----------------+
|
||||
|
||||
# For more information on modbus:
|
||||
# http://www.modbus.org
|
||||
# http://www.modbus.org/docs/Modbus_Application_Protocol_V1_1b.pdf
|
||||
# http://www.modbus.org/docs/Modbus_Messaging_Implementation_Guide_V1_0b.pdf
|
||||
|
||||
|
||||
class ModbusTCP:
|
||||
"""
|
||||
A Modbus communication class designed for use with modbusTCP
|
||||
"""
|
||||
__version__ = '0.1'
|
||||
|
||||
# Modbus function code
|
||||
READ_COILS = 0x01
|
||||
READ_HOLDING_REGISTERS = 0x03
|
||||
|
||||
# Todo: Implement the following modbus functionality:
|
||||
# READ_DISCRETE_INPUTS = 0x02
|
||||
# READ_INPUT_REGISTERS = 0x04
|
||||
# WRITE_SINGLE_COIL = 0x05
|
||||
# WRITE_SINGLE_REGISTER = 0x06
|
||||
# WRITE_MULTIPLE_COILS = 0x0F
|
||||
# WRITE_MULTIPLE_REGISTERS = 0x10
|
||||
|
||||
# Todo: Implement error checking with the following exception codes
|
||||
# Modbus exception code
|
||||
# ILLEGAL_FUNCTION_CODE = 0x01
|
||||
# ILLEGAL_DATA_ACCESS = 0x02 # if the request address is illegal
|
||||
# ILLEGAL_DATA_VALUE = 0x03 # if the request data is invalid
|
||||
|
||||
def __init__(self, host, port=502):
|
||||
"""
|
||||
:param host: IP address to connect with
|
||||
:param port: Pot (standard 502) to connect with
|
||||
"""
|
||||
self.__transaction_id = 0 # For synchronization between messages of server and client
|
||||
self.__protocol_id = 0 # 0 for Modbus/TCP
|
||||
self.__unit_id = 0 # Slave address (255 if not used)
|
||||
|
||||
self.pretty_print_response = False # Check to print out response message in console
|
||||
|
||||
self.connection = SocketConnection(host, port)
|
||||
|
||||
def open(self):
|
||||
"""
|
||||
Open the socket for communication
|
||||
"""
|
||||
self.connection.connect()
|
||||
|
||||
def close(self):
|
||||
"""
|
||||
Close the socket
|
||||
"""
|
||||
self.connection.disconnect()
|
||||
|
||||
def read_coils(self, bit_address, quantity=1):
|
||||
""" Main function 1 of Modbus/TCP - 0x01
|
||||
|
||||
:param bit_address:
|
||||
:param quantity:
|
||||
:return:
|
||||
"""
|
||||
data_bytes = struct.pack(">HH", bit_address, quantity)
|
||||
message = self._create_message(self.READ_COILS, data_bytes)
|
||||
return self._send(message)
|
||||
|
||||
def read_holding_registers(self, reg_address, quantity=1):
|
||||
"""Main function 3 of Modbus/TCP - 0x03.
|
||||
|
||||
Reads the values stored in the registers at the specified addresses.
|
||||
:param reg_address: Address of first register to read (16-bit) specified in bytes.
|
||||
:param quantity: Number of registers to read (16-bit) specified in bytes
|
||||
:return: The values stored in the addresses specified in Bytes
|
||||
"""
|
||||
data_bytes = struct.pack(">HH", reg_address, quantity)
|
||||
message = self._create_message(self.READ_HOLDING_REGISTERS, data_bytes)
|
||||
return self._send(message)
|
||||
|
||||
def _create_message(self, function_code, data_bytes):
|
||||
"""
|
||||
Create packet in bytes format for sending.
|
||||
:param function_code: bytes
|
||||
:param data_bytes: bytes
|
||||
:return: Bytes modbus packet
|
||||
"""
|
||||
body = struct.pack('>B', function_code) + data_bytes # create PDU
|
||||
self.__transaction_id = random.randint(0, 65535)
|
||||
message_length = 1 + len(body)
|
||||
header = struct.pack(">HHHB", self.__transaction_id, self.__protocol_id, message_length, self.__unit_id)
|
||||
return header + body
|
||||
|
||||
def _send(self, adu):
|
||||
""" Send message over the socket
|
||||
|
||||
:param adu: The data to send over the socket
|
||||
:return: Bytes response from the other end of the socket
|
||||
"""
|
||||
self.open()
|
||||
self.connection.send(adu)
|
||||
response = self.connection.receive()
|
||||
self.close()
|
||||
|
||||
if self.pretty_print_response:
|
||||
self.pretty_print(response)
|
||||
|
||||
if self._error_check(response):
|
||||
return None
|
||||
return response
|
||||
|
||||
def _error_check(self, response):
|
||||
""" Check if the frame is void of errors
|
||||
|
||||
Raises an exception termination the program
|
||||
:param response: The ADU to check
|
||||
:return: None
|
||||
"""
|
||||
mbap = response[:7]
|
||||
function_code = response[7:8]
|
||||
mbap = struct.unpack(">HHHB", mbap)
|
||||
|
||||
if mbap[0] != self.__transaction_id:
|
||||
print("Modbus: Transaction ID mismatch"
|
||||
"\n - Send: {} \n - Response: {}".format(self.__transaction_id, mbap[0]))
|
||||
return True
|
||||
elif mbap[1] != self.__protocol_id:
|
||||
print("Modbus: Protocol ID mismatch"
|
||||
"\n - Send: {} \n - Response: {}".format(self.__protocol_id, mbap[1]))
|
||||
return True
|
||||
elif mbap[3] != self.__unit_id:
|
||||
print("Modbus: Unit ID mismatch"
|
||||
"\n - Send: {} \n - Response: {}".format(self.__unit_id, mbap[3]))
|
||||
return True
|
||||
elif mbap[2] != len(response[6:]):
|
||||
print("Modbus: Length mismatch"
|
||||
"\n - Length: {} \n - Remaining: {}".format(mbap[2], len(response[6:])))
|
||||
return True
|
||||
|
||||
function_code = struct.unpack(">B", function_code)
|
||||
if function_code[0] > 127:
|
||||
error_code = struct.unpack(">B", response[8:9])
|
||||
print("Modbus: Function error: {}".format(error_code))
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def set_pretty_print(self, value):
|
||||
"""
|
||||
Enable or disable printing of response message in console
|
||||
:param value: Boolean
|
||||
"""
|
||||
self.pretty_print_response = value
|
||||
|
||||
@staticmethod
|
||||
def pretty_print(response):
|
||||
""" Print Response in the console
|
||||
|
||||
Unpacks the MBAP and function code in readable format
|
||||
Data bytes
|
||||
:param response:
|
||||
"""
|
||||
mbap = response[:7]
|
||||
function_code = response[7:8]
|
||||
mbap = struct.unpack(">HHHB", mbap)
|
||||
function_code = struct.unpack(">B", function_code)
|
||||
|
||||
print("+--------------------------------------+")
|
||||
print("| ****Modbus TCP Response (ADU)**** |")
|
||||
print("+--------------------------------------+")
|
||||
print("| **Header information (MBAP)** |")
|
||||
print("+--------------------------------------+")
|
||||
print("| Transaction id: " + str(mbap[0]))
|
||||
print("| Protocol id: " + str(mbap[1]))
|
||||
print("| Length: " + str(mbap[2]))
|
||||
print("| Unit id: " + str(mbap[3]))
|
||||
print("+--------------------------------------+")
|
||||
print("| **Data information (PDU)** |")
|
||||
print("+--------------------------------------+")
|
||||
print("| Function code: " + str(function_code[0]))
|
||||
print("| Data: " + str(response[8:]))
|
||||
print("+--------------------------------------+")
|
||||
print("\n")
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
import socket
|
||||
|
||||
|
||||
class SocketConnection:
|
||||
"""
|
||||
Defines a simple interface for connecting to a socket
|
||||
"""
|
||||
def __init__(self, host, port):
|
||||
"""
|
||||
:param host: The IP to connect with
|
||||
:param port: Port to connect with
|
||||
"""
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.opened = False
|
||||
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
|
||||
def connect(self):
|
||||
"""
|
||||
Opens a socket connection with the robot for communication.
|
||||
:return:
|
||||
"""
|
||||
if self.opened:
|
||||
self.disconnect()
|
||||
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.s.settimeout(1)
|
||||
|
||||
try:
|
||||
self.s.connect((self.host, self.port))
|
||||
self.opened = True
|
||||
except OSError as error:
|
||||
print("Connecting OS error: {0}".format(error))
|
||||
return
|
||||
return self.s
|
||||
|
||||
def send(self, message):
|
||||
"""
|
||||
Send data over the socket connection
|
||||
:param message: The data to send
|
||||
:return:
|
||||
"""
|
||||
total_send = 0
|
||||
while total_send < len(message):
|
||||
send = self.s.send(message[total_send:])
|
||||
if send == 0:
|
||||
raise RuntimeError("socket connection broken")
|
||||
total_send = total_send + send
|
||||
self.s.send(message)
|
||||
|
||||
def receive(self):
|
||||
"""
|
||||
Recieve data over the socket connection
|
||||
:return:
|
||||
"""
|
||||
response = self.s.recv(1024)
|
||||
if len(response) == 0:
|
||||
raise RuntimeError("socket connection broken")
|
||||
return response
|
||||
|
||||
def disconnect(self):
|
||||
"""
|
||||
Closes the socket connection
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
self.s.close()
|
||||
except OSError as error:
|
||||
print("Disconnecting OS error: {0}".format(error))
|
||||
return
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
from Communication.ModbusTCP import ModbusTCP
|
||||
|
||||
import time, math, struct
|
||||
|
||||
# The robot controller acts as a Modbus TCP server (port 502),
|
||||
# clients can establish connections to it and send standard MODBUS requests to it.
|
||||
|
||||
# - Note that some Modbus device manufacturers use the terms Master (client) and Slave (server).
|
||||
# Typically the external IO device is going to be a server and the robot is going to behave as the client
|
||||
# (requesting and consuming messages from the server). Master=client and Slave=server.
|
||||
# However, note that the UR controller can be both a server and a client
|
||||
|
||||
# - Note that all values are unsigned, if you want to convert to signed integers,
|
||||
# program "if (val > 32768): val = val - 65535".
|
||||
#
|
||||
# - The MODBUS Server has 0-based addressing.
|
||||
# Be aware that some other devices are 1-based (e.g. Anybus X-gateways), then just add one to the address
|
||||
# on that device. (e.g. address 3 on the robot will be address 4 on the Anybus X-gateway)
|
||||
|
||||
|
||||
class URModbusServer:
|
||||
"""Give read and write access to data in the robot controller for other devices
|
||||
|
||||
An interface for communicating with the modbus TCP server (port 502) on the UR.
|
||||
Defines functions for retrieving information from the controller.
|
||||
Information will be re-requested if an error occurs.
|
||||
All information will be formatted to human readable information.
|
||||
"""
|
||||
|
||||
def __init__(self, host):
|
||||
"""
|
||||
:param host: IP address to connect with
|
||||
"""
|
||||
self.modbusTCP = ModbusTCP(host, 502)
|
||||
|
||||
def get_tcp_position(self):
|
||||
"""
|
||||
Connects with the Modbus server to requests Cartesian data of the TCP
|
||||
:return: Readable cartesian data of TCP, vector in mm, axis in radials
|
||||
"""
|
||||
packet = self.modbusTCP.read_holding_registers(400, quantity=6)
|
||||
|
||||
if packet is None:
|
||||
time.sleep(0.5)
|
||||
print("Modbus Error: retrying")
|
||||
return self.get_tcp_position()
|
||||
else:
|
||||
x = self._format(packet[9:11]) / 10
|
||||
y = self._format(packet[11:13]) / 10
|
||||
z = self._format(packet[13:15]) / 10
|
||||
rx = self._format(packet[15:17]) / 1000
|
||||
ry = self._format(packet[17:19]) / 1000
|
||||
rz = self._format(packet[19:21]) / 1000
|
||||
return x, y, z, rx, ry, rz
|
||||
|
||||
def get_joint_angles(self, tries=0) -> tuple:
|
||||
"""
|
||||
Connects with the Modbus server to requests the angles of each joint, in radians
|
||||
:return: Readable angle values of each joint in radials
|
||||
"""
|
||||
if tries>10:
|
||||
print("Modbus Error: Failed")
|
||||
return 0, 0, 0, 0, 0, 0
|
||||
|
||||
packet = self.modbusTCP.read_holding_registers(270, quantity=6)
|
||||
packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6)
|
||||
|
||||
if (packet is None) or (packet_signs is None):
|
||||
time.sleep(0.01)
|
||||
print(f"Modbus Error #{tries}: retrying")
|
||||
return self.get_joint_angles(tries+1)
|
||||
else:
|
||||
#print(f"Packet: {packet_signs.hex()}")
|
||||
base = self._format_sign(packet[9:11], packet_signs[9:11])
|
||||
shoulder = self._format_sign(packet[11:13], packet_signs[11:13])
|
||||
elbow = self._format_sign(packet[13:15], packet_signs[13:15])
|
||||
wrist_1 = self._format_sign(packet[15:17], packet_signs[15:17])
|
||||
wrist_2 = self._format_sign(packet[17:19], packet_signs[17:19])
|
||||
wrist_3 = self._format_sign(packet[19:21], packet_signs[19:21])
|
||||
|
||||
return base, shoulder, elbow, wrist_1, wrist_2, wrist_3
|
||||
|
||||
def get_joint_angles_degrees(self, tries=0):
|
||||
angles = self.get_joint_angles(tries)
|
||||
return tuple([round(math.degrees(angle), 3) for angle in angles])
|
||||
|
||||
def get_joint_speeds(self, tries=0):
|
||||
"""
|
||||
Connects with the Modbus server to requests the speed of each joint, in radians
|
||||
:return: Readable angle values of each joint in radials
|
||||
"""
|
||||
if tries>10:
|
||||
print("Modbus Error: Failed")
|
||||
return 0, 0, 0, 0, 0, 0
|
||||
|
||||
packet = self.modbusTCP.read_holding_registers(280, quantity=6)
|
||||
|
||||
if (packet is None):
|
||||
time.sleep(0.01)
|
||||
print(f"Modbus Error #{tries}: retrying")
|
||||
return self.get_joint_speeds(tries+1)
|
||||
else:
|
||||
#print(f"Packet: {packet_signs.hex()}")
|
||||
base = self._format(packet[9:11]) / 1000
|
||||
shoulder = self._format(packet[11:13]) / 1000
|
||||
elbow = self._format(packet[13:15]) / 1000
|
||||
wrist_1 = self._format(packet[15:17]) / 1000
|
||||
wrist_2 = self._format(packet[17:19]) / 1000
|
||||
wrist_3 = self._format(packet[19:21]) / 1000
|
||||
|
||||
return base, shoulder, elbow, wrist_1, wrist_2, wrist_3
|
||||
|
||||
@staticmethod
|
||||
def _format(d):
|
||||
"""Formats signed integers to unsigned float
|
||||
|
||||
:param d: signed integer to format
|
||||
:return: unsigned float
|
||||
"""
|
||||
d = d.hex()
|
||||
d_i = int(d, 16)
|
||||
d_f = 0
|
||||
|
||||
if d_i < 32768:
|
||||
d_f = float(d_i)
|
||||
if d_i > 32767:
|
||||
d_i = 65535 - d_i
|
||||
d_f = float(d_i) * -1
|
||||
|
||||
return d_f
|
||||
|
||||
@staticmethod
|
||||
def _format_2(d):
|
||||
"""Formats signed integers to signed float
|
||||
|
||||
:param d: signed integer to format
|
||||
:return: signed float
|
||||
"""
|
||||
d = d.hex()
|
||||
d_i = int(d, 16)
|
||||
d_f = float(d_i)
|
||||
return d_f
|
||||
|
||||
@staticmethod
|
||||
def _format_sign(d, sign):
|
||||
"""
|
||||
|
||||
:param d:
|
||||
:param sign:
|
||||
:return:
|
||||
"""
|
||||
sign = sign.hex()
|
||||
sign = int(sign, 16)
|
||||
|
||||
d = d.hex()
|
||||
d_i = int(d, 16)
|
||||
d_f = float(d_i) / 1000
|
||||
|
||||
if sign == 0:
|
||||
return d_f
|
||||
else:
|
||||
d_f = round(d_f - math.radians(360), 3)
|
||||
return d_f
|
||||
|
|
@ -0,0 +1,160 @@
|
|||
from Communication.SocketConnection import SocketConnection
|
||||
from Robot.UR.URModbusServer import URModbusServer
|
||||
from Robot.UR.URScript import URScript
|
||||
|
||||
|
||||
class URRobot:
|
||||
"""
|
||||
Interface for communicating with the UR Robot
|
||||
SecondaryPort used for sending commands
|
||||
ModbusServer used for retrieving info
|
||||
"""
|
||||
def __init__(self, host):
|
||||
self.secondaryPort = 30002
|
||||
self.secondaryInterface = SocketConnection(host, self.secondaryPort)
|
||||
self.secondaryInterface.connect()
|
||||
self.URModbusServer = URModbusServer(host)
|
||||
self.URScript = URScript()
|
||||
|
||||
# Max safe values of acceleration and velocity are 0.4
|
||||
# DO NOT USE THE FOLLOWING VALUES
|
||||
# MAX a=1.3962634015954636
|
||||
# MAX v=1.0471975511965976
|
||||
# These are the absolute max for 'safe' movement and should only be used
|
||||
# if you know what your are doing, otherwise you WILL damage the UR internally
|
||||
self.acceleration = 0.1
|
||||
self.velocity = 0.1
|
||||
|
||||
def movel(self, pose, a=0.1, v=0.1, joint_p=False):
|
||||
"""Move to position (linear in tool-space)
|
||||
|
||||
See :class:`URScript` for detailed information
|
||||
"""
|
||||
script = URScript.movel(pose, a, v, joint_p=joint_p).encode()
|
||||
return self._send_script(script)
|
||||
|
||||
def movej(self, q, a=0.1, v=0.1, joint_p=True):
|
||||
"""Move to position (linear in joint-space)
|
||||
|
||||
See :class:`URScript` for detailed information
|
||||
"""
|
||||
script = URScript.movej(q, a, v, joint_p=joint_p).encode()
|
||||
return self._send_script(script)
|
||||
|
||||
def speedj(self, qd, a=0.5, t=0):
|
||||
"""Accelerate linearly in joint space and continue with constant joint speed.
|
||||
|
||||
The time t is optional; if provided the function will return after time t, regardless of the target speed has been reached.
|
||||
If thetime t is not provided, the function will return when the target speed is reached.
|
||||
|
||||
See :class:`URScript` for detailed information
|
||||
"""
|
||||
script = URScript.speedj(qd, a, t).encode()
|
||||
return self._send_script(script)
|
||||
|
||||
def stopj(self, a=1.5):
|
||||
"""Stop (linear in joint space)
|
||||
|
||||
See :class:`URScript` for detailed information
|
||||
"""
|
||||
script = URScript.stopj(a).encode()
|
||||
return self._send_script(script)
|
||||
|
||||
def set_tcp(self, pose):
|
||||
"""Set the Tool Center Point
|
||||
|
||||
See :class:`URScript` for detailed information
|
||||
"""
|
||||
script = URScript.set_tcp(pose).encode()
|
||||
return self._send_script(script)
|
||||
|
||||
def get_tcp_position(self):
|
||||
""" Get TCP position
|
||||
|
||||
Will return values as seen on the teaching pendant (300.0mm)
|
||||
:return: 6 Floats - Position data of TCP (x, y, z) in mm (Rx, Ry, Rz) in radials
|
||||
"""
|
||||
position_data = self.URModbusServer.get_tcp_position()
|
||||
return position_data
|
||||
|
||||
def get_joint_angles(self):
|
||||
""" Get joint angles
|
||||
|
||||
:return: 6 Floats - Position joint angles (base, shoulder, elbow, wrist_1, wrist_2, wrist_3) in radians
|
||||
"""
|
||||
position_data = self.URModbusServer.get_joint_angles()
|
||||
return position_data
|
||||
|
||||
def get_joint_angles_degrees(self):
|
||||
""" Get joint angles in degrees
|
||||
|
||||
:return: 6 Floats - Position joint angles (base, shoulder, elbow, wrist_1, wrist_2, wrist_3) in degrees
|
||||
"""
|
||||
position_data = self.URModbusServer.get_joint_angles_degrees()
|
||||
return position_data
|
||||
|
||||
def get_joint_speeds(self):
|
||||
""" Get joint speeds in rads/s
|
||||
|
||||
:return: 6 Floats - Position joint speeds (base, shoulder, elbow, wrist_1, wrist_2, wrist_3) in rads/s
|
||||
"""
|
||||
speed_data = self.URModbusServer.get_joint_speeds()
|
||||
return speed_data
|
||||
|
||||
def set_io(self, io, value):
|
||||
"""
|
||||
Set the specified IO
|
||||
:param io: The IO to set as INT
|
||||
:param value: Boolean to enable or disable IO
|
||||
:return: Boolean to check if the command has been send
|
||||
"""
|
||||
script = "set_digital_out({}, {})".format(io, value) + "\n"
|
||||
script = script.encode()
|
||||
return self._send_script(script)
|
||||
|
||||
def translate(self, vector, a=0.1, v=0.1):
|
||||
""" Move TCP based on its current position
|
||||
|
||||
Example:
|
||||
If the TCP position was: 0.4, 0.5, 0.5 and the vector value passed is 0.1, 0.0, 0.0.
|
||||
It will attempt to translate the TCP to 0.5, 0.5, 0.5
|
||||
:param vector: the X, Y, Z to translate to
|
||||
:param a: tool acceleration [m/2^s]
|
||||
:param v: tool speed [m/s]
|
||||
:return: Boolean to check if the command has been send
|
||||
"""
|
||||
tcp_pos = list(self.get_tcp_position())
|
||||
tcp_pos[0] = tcp_pos[0] / 1000 + vector[0]
|
||||
tcp_pos[1] = tcp_pos[1] / 1000 + vector[1]
|
||||
tcp_pos[2] = tcp_pos[2] / 1000 + vector[2]
|
||||
return self.movel(tcp_pos, a, v)
|
||||
|
||||
def _send_script(self, _script):
|
||||
""" Send URScript to the UR controller
|
||||
|
||||
:param _script: formatted script to send
|
||||
:return: Boolean to check if the script has been send
|
||||
"""
|
||||
try:
|
||||
self.secondaryInterface.send(_script)
|
||||
except OSError as error:
|
||||
print("OS error: {0}".format(error))
|
||||
return False
|
||||
return True
|
||||
|
||||
@ staticmethod
|
||||
def format_cartesian_data(cartesian_data):
|
||||
"""
|
||||
Formats the vector from human readable mm to robot readable mm
|
||||
A position of 300mm becomes 0.3
|
||||
:param cartesian_data: data to format
|
||||
:return: formatted data
|
||||
"""
|
||||
i = 0
|
||||
for position in cartesian_data:
|
||||
cartesian_data[position] /= (1000 if i < 3 else 1)
|
||||
i += 1
|
||||
formatted_cartesian_data = cartesian_data
|
||||
|
||||
return formatted_cartesian_data
|
||||
|
||||
|
|
@ -0,0 +1,142 @@
|
|||
# SW Version 1.8 - URScript manual:
|
||||
# When connected URScript programs or commands are sent in clear text on the socket.
|
||||
# Each line is terminated by ’\n’
|
||||
# python 3 must use bytes (b"") to send the string
|
||||
|
||||
# When using URScript the robot moves according to the base plane.
|
||||
|
||||
# Joint positions (q) and joint speeds (qd) are represented directly as lists of 6 Floats, one for each robot joint.
|
||||
# Tool poses (x) are represented as poses also consisting of 6 Floats.
|
||||
# In a pose, the first 3 coordinates is a position vector and the last 3 an axis-angle
|
||||
|
||||
|
||||
class URScript:
|
||||
""" Defines functions for use with the URRobot.
|
||||
|
||||
Based on URScript manual version 1.8
|
||||
"""
|
||||
__version__ = 1.8
|
||||
|
||||
@staticmethod
|
||||
def movec(pose_via, pose_to, a=0.1, v=0.1, r=0, joint_p=False):
|
||||
"""Move Circular: Move to position (circular in tool-space)
|
||||
|
||||
TCP moves on the circular arc segment from current pose, through pose_via to pose_to.
|
||||
Accelerates to and moves with constant tool speed v
|
||||
:param pose_via: path point (note: only position is used).
|
||||
(pose_via can also be specified as joint positions, then forward kinematics is used to
|
||||
calculate the corresponding pose)
|
||||
:param pose_to: target pose (pose_to can also be specified as joint positions, then
|
||||
forward kinematics is used to calculate the corresponding pose
|
||||
:param a: tool acceleration [m/2^s]
|
||||
:param v: tool speed [m/s]
|
||||
:param r: blend radius (of target pose) [m]
|
||||
:param joint_p: if True, poses are specified as joint positions
|
||||
:return: string containing the movec script
|
||||
"""
|
||||
prefix = "" if joint_p else "p"
|
||||
return"movec({}[{}, {}, {}, {}, {}, {}], {}[{}, {}, {}, {}, {}, {}], " \
|
||||
"a={}, v={}, r={})".format(prefix, *pose_via, prefix, *pose_to, a, v, r) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def movej(q, a=0.1, v=0.1, t=0, r=0, joint_p=False):
|
||||
"""Move to position (linear in joint-space)
|
||||
|
||||
When using this command, the
|
||||
robot must be at standstill or come from a movej or movel with a blend.
|
||||
The speed and acceleration parameters controls the trapezoid speed
|
||||
profile of the move. The $t$ parameters can be used in stead to set the
|
||||
time for this move. Time setting has priority over speed and acceleration
|
||||
settings. The blend radius can be set with the $r$ parameters, to avoid
|
||||
the robot stopping at the point. However, if he blend region of this mover
|
||||
overlaps with previous or following regions, this move will be skipped, and
|
||||
an ’Overlapping Blends’ warning message will be generated.
|
||||
:param q: joint positions (q can also be specified as a pose, the inverse
|
||||
kinematics is used to calculate the corresponding joint positions)
|
||||
:param a: joint acceleration of leading axis [rad/sˆ2]
|
||||
:param v: joint speed of leading axis [rad/s]
|
||||
:param t: time [S]
|
||||
:param r: blend radius [m]
|
||||
:param joint_p: if True, poses are specified as joint positions
|
||||
:return: string containing the movej script
|
||||
"""
|
||||
prefix = "" if joint_p else "p"
|
||||
return "movej({}[{}, {}, {}, {}, {}, {}], a={}, v={}, t={}, r={})".format(prefix, *q, a, v, t, r) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def movel(pose, a=0.1, v=0.1, t=0, r=0, joint_p=False):
|
||||
"""Move to position (linear in tool-space)
|
||||
|
||||
:param pose: target pose (pose can also be specified as joint positions,
|
||||
then forward kinematics is used to calculate the corresponding pose)
|
||||
:param a: tool acceleration [m/2^s]
|
||||
:param v: tool speed [m/s]
|
||||
:param t: time [S]
|
||||
:param r: blend radius [m]
|
||||
:param joint_p: if True, poses are specified as joint positions
|
||||
:return: string containing the movel script
|
||||
"""
|
||||
prefix = "" if joint_p else "p"
|
||||
return "movel({}[{}, {}, {}, {}, {}, {}], a={}, v={}, t={}, r={})".format(prefix, *pose, a, v, t, r) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def movep(pose, a=0.1, v=0.1, t=0, r=0, joint_p=False):
|
||||
"""Move Process
|
||||
|
||||
Blend circular (in tool-space) and move linear (in tool-space) to position.
|
||||
Accelerates to and moves with constant tool speed v.
|
||||
:param pose: target pose (pose can also be specified as joint positions,
|
||||
then forward kinematics is used to calculate the corresponding pose
|
||||
:param a: tool acceleration [m/2^s]
|
||||
:param v: tool speed [m/s]
|
||||
:param t: time [S]
|
||||
:param r: blend radius [m]
|
||||
:param joint_p: if True, poses are specified as joint positions
|
||||
:return: string containing the movep script
|
||||
"""
|
||||
prefix = "" if joint_p else "p"
|
||||
return "movep({}[{}, {}, {}, {}, {}, {}], a={}, v={}, t={}, r={})" .format(prefix, *pose, a, v, t, r) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def speedj(qd, a=0.1, t=0):
|
||||
"""Accelerate linearly in joint space and continue with constant joint speed.
|
||||
|
||||
The time t is optional; if provided the function will return after time t, regardless of the target speed has been reached.
|
||||
If the time t is not provided, the function will return when the target speed is reached.
|
||||
:param qd: joint speeds [rad/s]
|
||||
:param a: joint acceleration [rad/sˆ2]
|
||||
:param t: time [S]. Optional. Will not consider if equals or bellow 0
|
||||
:return: string containing the speedj script
|
||||
"""
|
||||
speedtime = "" if t <= 0 else ", t=" + str(t)
|
||||
return "speedj([{}, {}, {}, {}, {}, {}], a={}{})".format(*qd, a, speedtime) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def set_tcp(pose):
|
||||
"""Set the Tool Center Point
|
||||
|
||||
Sets the transformation from the output flange coordinate system to the TCP as a pose.
|
||||
:param pose: A pose representing the offset in x, y z and the rotation of the tcp around x, y and z
|
||||
:return: A string containing the set tcp script
|
||||
"""
|
||||
return "set_tcp(p[{}, {}, {}, {}, {}, {}])".format(*pose) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def stopj(a=1.5):
|
||||
"""Stop (linear in joint space)
|
||||
|
||||
Decelerate joint speeds to zero
|
||||
:param a: joint acceleration [rad/sˆ2] (of leading axis)
|
||||
:return: String containing the stopj script
|
||||
"""
|
||||
return "stopj({})".format(a) + "\n"
|
||||
|
||||
@staticmethod
|
||||
def stopl(a=0.5):
|
||||
"""Stop (linear in tool space)
|
||||
|
||||
Decelerate tool speeds to zero
|
||||
:param a: joint acceleration [rad/sˆ2] (of leading axis)
|
||||
:return: String containing the stopj script
|
||||
"""
|
||||
return "stopl({})".format(a) + "\n"
|
||||
|
|
@ -0,0 +1,175 @@
|
|||
from Robot.UR.URRobot import URRobot
|
||||
from time import sleep
|
||||
import math
|
||||
import threading
|
||||
|
||||
|
||||
class URSentry:
|
||||
def __init__(self, host):
|
||||
self.robot = URRobot(host)
|
||||
self.sentry_pose = [1.571, -1.571, 0.785, -0.785, -1.571, 1.318]
|
||||
self.forward_pose = [1.571, -1.949, 1.974, -2.844, -1.571, 1.318]
|
||||
self.robot_speed = [0, 0, 0, 0, 0, 0]
|
||||
self.detections = []
|
||||
|
||||
# Flags
|
||||
self.await_stop = False
|
||||
self.await_stop_ticks = 0
|
||||
|
||||
self.send_to_zero_on_stop = False
|
||||
|
||||
# Smooth stopping
|
||||
self.smooth_stop_delayed_call = None
|
||||
|
||||
def get_joint_angles(self) -> "list[float]":
|
||||
"""
|
||||
Get the current joint angles of the robot in degrees
|
||||
"""
|
||||
return self.robot.get_joint_angles_degrees()
|
||||
|
||||
def sentry_position(self, a=0.5, v=1.5):
|
||||
self.robot.movej(self.sentry_pose, a=0.5, v=1.5)
|
||||
|
||||
def forward_position(self, a=0.5, v=1.5):
|
||||
self.robot.movej(self.forward_pose, a, v)
|
||||
|
||||
def update_detections(self, detections):
|
||||
self.detections = detections
|
||||
|
||||
def forward_position_to_base_angle_degrees(self, base_angle, a=0.5, v=1.5):
|
||||
pose = self.forward_pose
|
||||
pose[0] = math.radians(base_angle)
|
||||
self.robot.movej(pose, a, v)
|
||||
|
||||
def set_base_speed(self, speed):
|
||||
self.robot_speed[0] = speed
|
||||
|
||||
def set_neck_speed(self, speed):
|
||||
self.robot_speed[4] = speed
|
||||
|
||||
def smooth_stop(self):
|
||||
# print("Smooth stopping --------------------")
|
||||
self.robot.speedj([0, 0, 0, 0, 0, 0], 1.5)
|
||||
|
||||
def lerp(self, a, b, t):
|
||||
return a + t * (b - a)
|
||||
|
||||
def move_robot_base(self, joystick_pos_x: float):
|
||||
"""
|
||||
Move the robot based on a joystick input,
|
||||
where the the center is (0, 0) and the bottom right corner is (1, 1).
|
||||
|
||||
Horizontally, the speed of the base is adjusted on how far left or right the detection is.
|
||||
We take into account the maximum rotation limit, trying to slow down when nearing +-360 degrees,
|
||||
and trying to never reach that point. If needed, the robot should attempt to do a full rotation the other way to keep following
|
||||
the target.
|
||||
This can be changed so we first change the horizontal "neck" (wrist_2) until it reaches a limit, and then rotate the base,
|
||||
which would result in a more natural movement
|
||||
|
||||
Vertically, the speed of the vertical "neck" (wrist_1) is adjusted based on how far up or down the detection is.
|
||||
We limit it's rotation to +- 45 degrees, as we do not need more to follow the target.
|
||||
This can be changed in the future to acomplish more natural movements, by also adjusting the speed of the shoulder joint.
|
||||
|
||||
Movement is done entirely through speed control, which should result in a more fluid movement compared to pose control.
|
||||
|
||||
If flag await_stop is set True, we check if the current speed is 0 wait until all joints are stopped before moving again.
|
||||
|
||||
"""
|
||||
# We omit all processing until the robot has stopped moving for a certain amount of calls ('ticks')
|
||||
|
||||
# print("Base Speed: ", self.robot_speed[0])
|
||||
|
||||
ticks_to_wait = 10
|
||||
if self.await_stop:
|
||||
joint_speeds = self.robot.get_joint_speeds()
|
||||
if all([speed == 0 for speed in joint_speeds]):
|
||||
self.await_stop_ticks += 1
|
||||
if self.await_stop_ticks > ticks_to_wait:
|
||||
self.await_stop = False
|
||||
self.await_stop_ticks = 0
|
||||
return
|
||||
|
||||
if self.send_to_zero_on_stop:
|
||||
self.await_stop = True
|
||||
self.send_to_zero_on_stop = False
|
||||
self.forward_position_to_base_angle_degrees(0)
|
||||
|
||||
# Cancel smooth stopping due to no input
|
||||
if self.smooth_stop_delayed_call is not None:
|
||||
self.smooth_stop_delayed_call.cancel()
|
||||
self.smooth_stop_delayed_call = None
|
||||
|
||||
# # Center detections, so that the center of the image is (0, 0)
|
||||
# # We also invert the y axis, so that the upper part of the image is positive
|
||||
# # Thus: lower left corner is (-500, -500) and upper right corner is (500, 500)
|
||||
# joystick_pos_x -= 500
|
||||
# joystick_pos_y -= 500
|
||||
# joystick_pos_y = -joystick_pos_y
|
||||
|
||||
# Deadzones where we still consider the target in the middle, to avoid jittering
|
||||
horizontal_dead_zone_radius = 0.1
|
||||
vertical_dead_zone_radius = 0.1
|
||||
|
||||
# Speeds for the base and neck joints
|
||||
base_max_speed = 0.8
|
||||
base_min_speed = 0.3
|
||||
|
||||
base_acceleration = 1.5
|
||||
|
||||
# Maximum and minimum angles
|
||||
base_max_angle = 315
|
||||
base_danger_angle = 340
|
||||
|
||||
if abs(joystick_pos_x) < horizontal_dead_zone_radius:
|
||||
# We are in the deadzone, so we do not need to move the base, or stop moving it
|
||||
if self.robot_speed[0] == 0:
|
||||
return
|
||||
self.set_base_speed(0)
|
||||
|
||||
else:
|
||||
current_pose = self.get_joint_angles()
|
||||
base_angle = current_pose[0]
|
||||
# print("Base angle: ", base_angle)
|
||||
# Check if we are past the maximum angle
|
||||
if abs(base_angle) > base_max_angle:
|
||||
# We are past the maximum angle, so we undo a rotation by sending to 0
|
||||
self.send_to_zero_on_stop = True
|
||||
self.await_stop = True
|
||||
self.set_base_speed(0)
|
||||
self.robot.speedj(self.robot_speed, base_acceleration, 1)
|
||||
return
|
||||
|
||||
# We are not in the deadzone, so we need to move the base
|
||||
# Calculate the speed based on the distance from the center
|
||||
direction = math.copysign(1, joystick_pos_x)
|
||||
base_speed = (
|
||||
self.lerp(base_min_speed, base_max_speed, abs(joystick_pos_x))
|
||||
* direction
|
||||
)
|
||||
self.set_base_speed(base_speed)
|
||||
|
||||
# Schedule smooth stop if no input is given for a second
|
||||
self.smooth_stop_delayed_call = threading.Timer(
|
||||
0.4, self.move_robot_base, args=[0]
|
||||
)
|
||||
self.smooth_stop_delayed_call.start()
|
||||
|
||||
# print("Robot speed: ", self.robot_speed)
|
||||
self.robot.speedj(self.robot_speed, base_acceleration, 1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
host = "172.22.114.160"
|
||||
sentry = URSentry(host)
|
||||
# sentry.sentry_position()
|
||||
# sleep(3)
|
||||
sentry.forward_position()
|
||||
sleep(3)
|
||||
sentry.robot.speedj([0.8, 0, 0, 0, 0, 0], 0.5, 3)
|
||||
sleep(2)
|
||||
sentry.smooth_stop()
|
||||
sleep(2)
|
||||
sentry.forward_position_to_base_angle_degrees(45)
|
||||
|
||||
# while True:
|
||||
# print(sentry.robot.get_joint_speeds()[0])
|
||||
|
|
@ -0,0 +1,75 @@
|
|||
import asyncio
|
||||
import queue
|
||||
import json
|
||||
import requests
|
||||
import websockets
|
||||
import ssl
|
||||
import re
|
||||
import dotenv
|
||||
|
||||
# from UnifiWebsockets.decode import decode_packet
|
||||
|
||||
|
||||
# REST login script to get the CSRF token
|
||||
def get_token(base_url, username, password):
|
||||
login_url = f"{base_url}/api/auth/login"
|
||||
payload = {"username": username, "password": password}
|
||||
headers = {"Content-Type": "application/json"}
|
||||
try:
|
||||
session = requests.Session()
|
||||
response = session.post(login_url, json=payload, headers=headers, verify=False)
|
||||
response.raise_for_status() # Raise an exception for HTTP errors
|
||||
return session.cookies
|
||||
except requests.exceptions.RequestException as e:
|
||||
print(f"Failed to obtain token: {e}")
|
||||
return None, None
|
||||
|
||||
|
||||
# Function to connect to the websocket and print the event stream
|
||||
async def listen_to_event_stream(ws_url, cookies, queue):
|
||||
cookie_header = "".join([f"{key}={value}" for key, value in cookies.items()])
|
||||
headers = {"Cookie": cookie_header}
|
||||
|
||||
ssl_context = ssl._create_unverified_context() # Create an unverified SSL context
|
||||
try:
|
||||
async with websockets.connect(
|
||||
ws_url, extra_headers=headers, ssl=ssl_context
|
||||
) as websocket:
|
||||
while True:
|
||||
try:
|
||||
message = await websocket.recv()
|
||||
message = str(message)
|
||||
|
||||
queue.put(find_coords(message))
|
||||
# print(find_coords(message))
|
||||
|
||||
# coords_begin = message.find('coord') + 7
|
||||
# coords_end = message[coords_begin:].find(']')
|
||||
# print(message[coords_begin:])
|
||||
|
||||
# print(decode_packet(message))
|
||||
except websockets.ConnectionClosed:
|
||||
print("Connection closed")
|
||||
break
|
||||
except Exception as e:
|
||||
print(f"WebSocket connection failed: {e}")
|
||||
|
||||
|
||||
def find_coords(message: str) -> list[str]:
|
||||
coords_begin = [m.start() + 8 for m in re.finditer('"coord":', message)]
|
||||
coords_end = [m.start() + 1 for m in re.finditer('],"depth":', message)]
|
||||
coords = [
|
||||
json.loads(message[coords_begin[i] : coords_end[i]])
|
||||
for i in range(len(coords_begin))
|
||||
]
|
||||
return coords
|
||||
|
||||
|
||||
def run(q: queue.Queue) -> None:
|
||||
base_url = "https://172.22.114.176" # Replace with your UniFi Protect base URL
|
||||
username = "engr-ugaif" # Replace with your username
|
||||
password = dotenv.get_key(dotenv.find_dotenv(), "UNIFI_PASSWORD")
|
||||
ws_url = "wss://172.22.114.176/proxy/protect/ws/liveDetectTrack?camera=668daa1e019ce603e4002d31" # Replace with your WebSocket URL
|
||||
|
||||
cookies = get_token(base_url, username, password)
|
||||
asyncio.run(listen_to_event_stream(ws_url, cookies, q))
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
import zlib
|
||||
import struct
|
||||
import json
|
||||
from typing import Tuple, Union
|
||||
|
||||
# Constants
|
||||
EVENT_PACKET_HEADER_SIZE = 8
|
||||
|
||||
# Packet Types
|
||||
HEADER = 1
|
||||
PAYLOAD = 2
|
||||
|
||||
# Payload Types
|
||||
JSON_TYPE = 1
|
||||
STRING_TYPE = 2
|
||||
BUFFER_TYPE = 3
|
||||
|
||||
def decode_packet(packet: bytes) -> Union[Tuple[dict, Union[dict, str, bytes]], None]:
|
||||
try:
|
||||
data_offset = struct.unpack('>I', packet[ProtectEventPacketHeader.PAYLOAD_SIZE:ProtectEventPacketHeader.PAYLOAD_SIZE + 4])[0] + EVENT_PACKET_HEADER_SIZE
|
||||
|
||||
if len(packet) != (data_offset + EVENT_PACKET_HEADER_SIZE + struct.unpack('>I', packet[data_offset + ProtectEventPacketHeader.PAYLOAD_SIZE:data_offset + ProtectEventPacketHeader.PAYLOAD_SIZE + 4])[0]):
|
||||
raise ValueError("Packet length doesn't match header information.")
|
||||
except Exception as error:
|
||||
print(f"Error decoding update packet: {error}")
|
||||
return None
|
||||
|
||||
header_frame = decode_frame(packet[:data_offset], HEADER)
|
||||
payload_frame = decode_frame(packet[data_offset:], PAYLOAD)
|
||||
|
||||
if not header_frame or not payload_frame:
|
||||
return None
|
||||
|
||||
return header_frame, payload_frame
|
||||
|
||||
def decode_frame(packet: bytes, packet_type: int) -> Union[dict, str, bytes, None]:
|
||||
frame_type = packet[ProtectEventPacketHeader.TYPE]
|
||||
|
||||
if packet_type != frame_type:
|
||||
return None
|
||||
|
||||
payload_format = packet[ProtectEventPacketHeader.PAYLOAD_FORMAT]
|
||||
is_deflated = packet[ProtectEventPacketHeader.DEFLATED]
|
||||
|
||||
if is_deflated:
|
||||
payload = zlib.decompress(packet[EVENT_PACKET_HEADER_SIZE:])
|
||||
else:
|
||||
payload = packet[EVENT_PACKET_HEADER_SIZE:]
|
||||
|
||||
if frame_type == HEADER:
|
||||
if payload_format == JSON_TYPE:
|
||||
return json.loads(payload.decode('utf-8'))
|
||||
else:
|
||||
return None
|
||||
|
||||
if payload_format == JSON_TYPE:
|
||||
return json.loads(payload.decode('utf-8'))
|
||||
elif payload_format == STRING_TYPE:
|
||||
return payload.decode('utf-8')
|
||||
elif payload_format == BUFFER_TYPE:
|
||||
return payload
|
||||
else:
|
||||
print(f"Unknown payload packet type received in the realtime events API: {payload_format}")
|
||||
return None
|
||||
|
||||
def decode_packet_from_mac(packet: bytes, mac: str) -> Union[Tuple[dict, Union[dict, str, bytes]], None]:
|
||||
try:
|
||||
data_offset = struct.unpack('>I', packet[ProtectEventPacketHeader.PAYLOAD_SIZE:ProtectEventPacketHeader.PAYLOAD_SIZE + 4])[0] + EVENT_PACKET_HEADER_SIZE
|
||||
|
||||
if len(packet) != (data_offset + EVENT_PACKET_HEADER_SIZE + struct.unpack('>I', packet[data_offset + ProtectEventPacketHeader.PAYLOAD_SIZE:data_offset + ProtectEventPacketHeader.PAYLOAD_SIZE + 4])[0]):
|
||||
raise ValueError("Packet length doesn't match header information.")
|
||||
except Exception as error:
|
||||
print(f"Error decoding update packet: {error}")
|
||||
return None
|
||||
|
||||
header_frame = decode_frame(packet[:data_offset], HEADER)
|
||||
|
||||
if not header_frame:
|
||||
return None
|
||||
|
||||
# Check if modelKey is 'camera' and mac matches
|
||||
if header_frame.get('modelKey') != 'camera' or header_frame.get('mac') != mac:
|
||||
return None
|
||||
|
||||
payload_frame = decode_frame(packet[data_offset:], PAYLOAD)
|
||||
|
||||
if not payload_frame:
|
||||
return None
|
||||
|
||||
return header_frame, payload_frame
|
||||
|
||||
class ProtectEventPacketHeader:
|
||||
TYPE = 0
|
||||
PAYLOAD_FORMAT = 1
|
||||
DEFLATED = 2
|
||||
UNKNOWN = 3
|
||||
PAYLOAD_SIZE = 4
|
||||
|
||||
# Example usage:
|
||||
# packet = b'...' # binary string from websocket
|
||||
# mac_address = 'XX:XX:XX:XX:XX:XX' # Replace with the desired MAC address
|
||||
# result = decode_packet_from_mac(packet, mac_address)
|
||||
# if result:
|
||||
# action_frame, data_frame = result
|
||||
# print("Action Frame:", action_frame)
|
||||
# print("Data Frame:", data_frame)
|
||||
# else:
|
||||
# print("No matching packet found.")
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
from UnifiWebsockets import Unifi
|
||||
from URSentry import URSentry
|
||||
import queue
|
||||
import time
|
||||
import threading
|
||||
import BBoxProcessor
|
||||
|
||||
q = queue.Queue()
|
||||
|
||||
x = threading.Thread(target=Unifi.run, args=(q,))
|
||||
x.start()
|
||||
|
||||
b = BBoxProcessor.BBoxProcessor()
|
||||
|
||||
ur = URSentry("172.22.114.160")
|
||||
|
||||
joystick_queue = queue.Queue()
|
||||
|
||||
|
||||
class CameraJoystick(threading.Thread):
|
||||
def __init__(self, joystick_queue: queue.Queue[list[int]]):
|
||||
threading.Thread.__init__(self)
|
||||
self.joystick_queue = joystick_queue
|
||||
self.keep_running = True
|
||||
|
||||
def run(self):
|
||||
while self.keep_running:
|
||||
bb = b.get_joystick_position_from_new_set_of_bboxes(q.get())
|
||||
print(bb)
|
||||
self.joystick_queue.put(bb)
|
||||
|
||||
def stop(self):
|
||||
self.keep_running = False
|
||||
|
||||
|
||||
current_joystick = [0, 0]
|
||||
|
||||
|
||||
class JoystickScheduler(threading.Thread):
|
||||
def __init__(self, joystick_queue: queue.Queue[list[int]]):
|
||||
threading.Thread.__init__(self)
|
||||
self.joystick_queue = joystick_queue
|
||||
self.keep_running = True
|
||||
|
||||
def run(self):
|
||||
while self.keep_running:
|
||||
global current_joystick
|
||||
current_joystick = self.joystick_queue.get()
|
||||
|
||||
def stop(self):
|
||||
self.keep_running = False
|
||||
|
||||
|
||||
camera_joystick = CameraJoystick(joystick_queue)
|
||||
camera_joystick.start()
|
||||
joystick_scheduler = JoystickScheduler(joystick_queue)
|
||||
joystick_scheduler.start()
|
||||
|
||||
ur.await_stop = True
|
||||
ur.forward_position()
|
||||
|
||||
while True:
|
||||
ur.move_robot_base(-current_joystick[0])
|
||||
print("Setting robot base velocity to: ", current_joystick[0])
|
||||
time.sleep(0.2)
|
||||
Loading…
Reference in New Issue