Fixed delay error on queue
This commit is contained in:
parent
0445a7354e
commit
91493bba22
|
@ -8,7 +8,7 @@ class BBoxProcessor:
|
||||||
self.last_boxes = []
|
self.last_boxes = []
|
||||||
self.last_timestamps = []
|
self.last_timestamps = []
|
||||||
|
|
||||||
def get_joystick_position_from_new_set_of_bboxes(self, new_boxes: list) -> list:
|
def get_joystick_position_from_new_set_of_bboxes(self, new_boxes: list) -> list | None:
|
||||||
"""
|
"""
|
||||||
Given a set of bounding boxes, returns the joystick position that would move the robot to the closest one.
|
Given a set of bounding boxes, returns the joystick position that would move the robot to the closest one.
|
||||||
"""
|
"""
|
||||||
|
@ -16,7 +16,7 @@ class BBoxProcessor:
|
||||||
closest_box = self.get_box_closest_to_center(new_boxes)
|
closest_box = self.get_box_closest_to_center(new_boxes)
|
||||||
if closest_box == []:
|
if closest_box == []:
|
||||||
return None
|
return None
|
||||||
print("Closest box: ", closest_box, "All: ", new_boxes)
|
#print("Closest box: ", closest_box, "All: ", new_boxes)
|
||||||
return self.get_normalized_box_position(closest_box)
|
return self.get_normalized_box_position(closest_box)
|
||||||
|
|
||||||
def bbox_distance(self, box1: list, box2: list) -> float:
|
def bbox_distance(self, box1: list, box2: list) -> float:
|
||||||
|
@ -42,7 +42,7 @@ class BBoxProcessor:
|
||||||
for new_box in new_boxes:
|
for new_box in new_boxes:
|
||||||
new_boxes_list.append(new_box)
|
new_boxes_list.append(new_box)
|
||||||
new_timestamps_list.append(timestamp)
|
new_timestamps_list.append(timestamp)
|
||||||
|
# print("New boxes: ", new_boxes_list, end="")
|
||||||
min_distance = dist_threshold
|
min_distance = dist_threshold
|
||||||
for index, box in enumerate(self.last_boxes):
|
for index, box in enumerate(self.last_boxes):
|
||||||
distance = 9999
|
distance = 9999
|
||||||
|
@ -57,9 +57,10 @@ class BBoxProcessor:
|
||||||
closest is None
|
closest is None
|
||||||
and timestamp < self.last_timestamps[index] + time_to_live
|
and timestamp < self.last_timestamps[index] + time_to_live
|
||||||
):
|
):
|
||||||
|
# print(" | Survivor: ", box, end="")
|
||||||
new_boxes_list.append(box)
|
new_boxes_list.append(box)
|
||||||
new_timestamps_list.append(self.last_timestamps[index])
|
new_timestamps_list.append(self.last_timestamps[index])
|
||||||
|
# print("")
|
||||||
self.last_boxes = new_boxes_list
|
self.last_boxes = new_boxes_list
|
||||||
self.last_timestamps = new_timestamps_list
|
self.last_timestamps = new_timestamps_list
|
||||||
return new_boxes_list
|
return new_boxes_list
|
||||||
|
|
|
@ -42,7 +42,7 @@ class URModbusServer:
|
||||||
|
|
||||||
if packet is None:
|
if packet is None:
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
print("Modbus Error: retrying")
|
print("[TCP] Modbus Error: retrying")
|
||||||
return self.get_tcp_position()
|
return self.get_tcp_position()
|
||||||
else:
|
else:
|
||||||
x = self._format(packet[9:11]) / 10
|
x = self._format(packet[9:11]) / 10
|
||||||
|
@ -59,15 +59,16 @@ class URModbusServer:
|
||||||
:return: Readable angle values of each joint in radials
|
:return: Readable angle values of each joint in radials
|
||||||
"""
|
"""
|
||||||
if tries>10:
|
if tries>10:
|
||||||
print("Modbus Error: Failed")
|
print("[Angles] Modbus Error: Failed")
|
||||||
return 0, 0, 0, 0, 0, 0
|
return 0, 0, 0, 0, 0, 0
|
||||||
|
|
||||||
packet = self.modbusTCP.read_holding_registers(270, quantity=6)
|
packet = self.modbusTCP.read_holding_registers(270, quantity=6)
|
||||||
|
time.sleep(0.001)
|
||||||
packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6)
|
packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6)
|
||||||
|
|
||||||
if (packet is None) or (packet_signs is None):
|
if (packet is None) or (packet_signs is None):
|
||||||
time.sleep(0.01)
|
time.sleep(0.01)
|
||||||
print(f"Modbus Error #{tries}: retrying")
|
print(f"[Angles] Modbus Error #{tries}: retrying")
|
||||||
return self.get_joint_angles(tries+1)
|
return self.get_joint_angles(tries+1)
|
||||||
else:
|
else:
|
||||||
base = self._format_sign(packet[9:11], packet_signs[9:11])
|
base = self._format_sign(packet[9:11], packet_signs[9:11])
|
||||||
|
@ -89,14 +90,14 @@ class URModbusServer:
|
||||||
:return: Readable angle speeds of each joint in radians per second
|
:return: Readable angle speeds of each joint in radians per second
|
||||||
"""
|
"""
|
||||||
if tries>10:
|
if tries>10:
|
||||||
print("Modbus Error: Failed")
|
print("[Speeds] Modbus Error: Failed")
|
||||||
return 0, 0, 0, 0, 0, 0
|
return 0, 0, 0, 0, 0, 0
|
||||||
|
|
||||||
packet = self.modbusTCP.read_holding_registers(280, quantity=6)
|
packet = self.modbusTCP.read_holding_registers(280, quantity=6)
|
||||||
|
|
||||||
if (packet is None):
|
if (packet is None):
|
||||||
time.sleep(0.01)
|
time.sleep(0.01)
|
||||||
print(f"Modbus Error #{tries}: retrying")
|
print(f"[Speeds] Modbus Error #{tries}: retrying")
|
||||||
return self.get_joint_speeds(tries+1)
|
return self.get_joint_speeds(tries+1)
|
||||||
else:
|
else:
|
||||||
base = self._format(packet[9:11]) / 1000
|
base = self._format(packet[9:11]) / 1000
|
||||||
|
|
|
@ -73,7 +73,7 @@ class URSentry:
|
||||||
"""
|
"""
|
||||||
Control the robot based on a joystick input.
|
Control the robot based on a joystick input.
|
||||||
"""
|
"""
|
||||||
|
print("Joystick pos: ", joystick_pos)
|
||||||
# Check for flags that would block control due to things happening
|
# Check for flags that would block control due to things happening
|
||||||
|
|
||||||
# Initialize robot if it hasn't already
|
# Initialize robot if it hasn't already
|
||||||
|
@ -189,7 +189,7 @@ class URSentry:
|
||||||
|
|
||||||
# Speeds for the base and neck joints
|
# Speeds for the base and neck joints
|
||||||
base_max_speed = 1.5
|
base_max_speed = 1.5
|
||||||
base_min_speed = 0.1
|
base_min_speed = 0.0
|
||||||
|
|
||||||
base_acceleration = 1.5
|
base_acceleration = 1.5
|
||||||
|
|
||||||
|
|
12
main.py
12
main.py
|
@ -24,15 +24,17 @@ class CameraJoystick(threading.Thread):
|
||||||
self.keep_running = True
|
self.keep_running = True
|
||||||
|
|
||||||
def run(self):
|
def run(self):
|
||||||
|
global current_joystick
|
||||||
while self.keep_running:
|
while self.keep_running:
|
||||||
|
time.sleep(0.0001)
|
||||||
q_val = []
|
q_val = []
|
||||||
try:
|
try:
|
||||||
q_val = q.get_nowait()
|
q_val = q.get_nowait()
|
||||||
except queue.Empty:
|
except queue.Empty:
|
||||||
pass
|
pass
|
||||||
#print(bb)
|
#print(bb)
|
||||||
bb = b.get_joystick_position_from_new_set_of_bboxes(q_val)
|
current_joystick = b.get_joystick_position_from_new_set_of_bboxes(q_val)
|
||||||
self.joystick_queue.put(bb)
|
#self.joystick_queue.put(bb)
|
||||||
|
|
||||||
def stop(self):
|
def stop(self):
|
||||||
self.keep_running = False
|
self.keep_running = False
|
||||||
|
@ -58,8 +60,8 @@ class JoystickScheduler(threading.Thread):
|
||||||
|
|
||||||
camera_joystick = CameraJoystick(joystick_queue)
|
camera_joystick = CameraJoystick(joystick_queue)
|
||||||
camera_joystick.start()
|
camera_joystick.start()
|
||||||
joystick_scheduler = JoystickScheduler(joystick_queue)
|
# joystick_scheduler = JoystickScheduler(joystick_queue)
|
||||||
joystick_scheduler.start()
|
# joystick_scheduler.start()
|
||||||
|
|
||||||
ur.initialize_pose()
|
ur.initialize_pose()
|
||||||
|
|
||||||
|
@ -70,5 +72,5 @@ while True:
|
||||||
time.sleep(0.2)
|
time.sleep(0.2)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
camera_joystick.stop()
|
camera_joystick.stop()
|
||||||
joystick_scheduler.stop()
|
# joystick_scheduler.stop()
|
||||||
break
|
break
|
||||||
|
|
Loading…
Reference in New Issue