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