Dockerfile and slight robot adjustments

URSentry.py

@@ -34,6 +34,7 @@ class URSentry:
     def initialize_pose(self):
         self.await_stop = True
         self.sentry_position()
+        self.has_initialized = True
 
     def get_joint_angles(self) -> "list[float]":
         """
@@ -82,11 +83,10 @@ class URSentry:
         # Initialize robot if it hasn't already
         if not self.has_initialized:
             self.initialize_pose()
-            self.has_initialized = True
             return
 
         # If we are awaiting a stop, we do not control the robot until it remains at a standstill for a certain amount of ticks
-        ticks_to_wait = 3
+        ticks_to_wait = 6
         if self.await_stop:
             joint_speeds = self.robot.get_joint_speeds()
             print("Awaiting stop -------------------- speeds: ", joint_speeds)
@@ -108,7 +108,7 @@ class URSentry:
 
         # If the joystick is None, we return to sentry mode after a certain amount of ticks
         # We only do this if we have detected something at least once, as the camera gets buggy after fast movements
-        ticks_for_return_to_sentry = 150
+        ticks_for_return_to_sentry = 600
         if joystick_pos is None:
             if self.has_detected_once and not self.is_on_sentry_mode:
                 self.none_input_ticks += 1
@@ -187,7 +187,7 @@ class URSentry:
         # 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
+        horizontal_dead_zone_radius = 0.05
         vertical_dead_zone_radius = 0.01
 
         # Speeds for the base and neck joints
@@ -252,7 +252,7 @@ class URSentry:
                 difference_from_limit = (joint_limit[i] - current_pose[i])
                 # difference_from_limit = difference_from_limit if difference_from_limit * joint_direction > 0 else 0
                 # self.robot_speed[i] = round(difference_from_limit * vertical_speed * abs(joystick_pos_y), 5) 
-                self.robot_speed[i] = round(joint_difference * vertical_speed * joystick_pos_y * 0.3, 5) if difference_from_limit * joint_direction > 0 else 0
+                self.robot_speed[i] = round(joint_difference * vertical_speed * joystick_pos_y, 5) if difference_from_limit * joint_direction > 0 else 0
                 print("Joint: ", i, " limit:", joint_limit[i], " current:" , current_pose[i] , " reachedlimit: ", not (difference_from_limit * joint_direction > 0), " Speed: ", self.robot_speed[i])
             print("")
             #self.set_neck_speed(neck_speed)

UnifiWebsockets/Unifi.py

@@ -65,10 +65,10 @@ def find_coords(message: str) -> list[str]:
     return coords
 
 
-def run(q: queue.Queue) -> None:
+def run(q: queue.Queue, passwrd : str | None = None) -> 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")
+    password = dotenv.get_key(dotenv.find_dotenv(), "UNIFI_PASSWORD") if passwrd is None else passwrd
     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)

dockerfile

@@ -0,0 +1,22 @@
+FROM python:3.12
+
+LABEL maintainer="Tomas Letelier"
+
+WORKDIR /usr/src/app
+
+COPY requirements.txt ./
+
+RUN pip install --no-cache-dir --upgrade pip \
+  && pip install --no-cache-dir -r requirements.txt
+
+COPY Communication/ ./Communication/
+COPY Robot/ ./Robot/
+COPY UnifiWebsockets/ ./UnifiWebsockets/
+
+COPY BBoxProcessor.py .
+COPY URSentry.py .
+COPY dockermain.py .
+
+ENV UNIFI_PASSWORD = ''
+
+CMD [ "python", "./dockermain.py" ]

dockermain.py

@@ -0,0 +1,82 @@
+from UnifiWebsockets import Unifi
+from URSentry import URSentry
+import queue
+import time
+import threading
+import BBoxProcessor
+import os
+
+unifi_password = os.getenv('UNIFI_PASSWORD')
+if unifi_password == '':
+    print("Please set the 'UNIFI_PASSWORD' environment variable.")
+    exit(1)
+
+q = queue.Queue()
+
+x = threading.Thread(target=Unifi.run, args=(q,unifi_password))
+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):
+        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)
+            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
+
+
+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.initialize_pose()
+
+while True:
+    try:
+        ur.control_robot(current_joystick)
+        #print("Setting robot base velocity to: ", current_joystick[0])
+        time.sleep(0.1)
+    except KeyboardInterrupt:
+        camera_joystick.stop()
+        # joystick_scheduler.stop()
+        break

main.py

@@ -69,7 +69,7 @@ while True:
     try:
         ur.control_robot(current_joystick)
         #print("Setting robot base velocity to: ", current_joystick[0])
-        time.sleep(0.2)
+        time.sleep(0.1)
     except KeyboardInterrupt:
         camera_joystick.stop()
         # joystick_scheduler.stop()