[drones] Multiprocessing leftovers

exercises/static/exercises/drone_cat_mouse/web-template/hal.py

@@ -8,20 +8,45 @@
 class HAL:
     IMG_WIDTH = 320
     IMG_HEIGHT = 240
-
+    
     def __init__(self):
-        rospy.init_node("HAL_cat")
+        rospy.init_node("HAL")
+
+        self.shared_frontal_image = SharedImage("halfrontalimage")
+        self.shared_ventral_image = SharedImage("halventralimage")
+        self.shared_x = SharedValue("x")
+        self.shared_y = SharedValue("y")
+        self.shared_z = SharedValue("z")
+        self.shared_takeoff_z = SharedValue("sharedtakeoffz")
+        self.shared_az = SharedValue("az")
+        self.shared_azt = SharedValue("azt")
+        self.shared_vx = SharedValue("vx")
+        self.shared_vy = SharedValue("vy")
+        self.shared_vz = SharedValue("vz")
+        self.shared_landed_state = SharedValue("landedstate")
+        self.shared_position = SharedValue("position",3)
+        self.shared_velocity = SharedValue("velocity",3)
+        self.shared_orientation = SharedValue("orientation",3)
+        self.shared_yaw_rate = SharedValue("yawrate")
+
+        self.shared_CMD =  SharedValue("CMD")
 
         self.image = None
-        self.cat = DroneWrapper(name="rqt", ns="cat/")
+        self.cat = DroneWrapper(name="rqt", ns="/iris/")
+
+        # Update thread
+        self.thread = ThreadHAL(self.update_hal)
+
+
 
     # Explicit initialization functions
     # Class method, so user can call it without instantiation
-    @classmethod
-    def initRobot(cls):
-        new_instance = cls()
-        return new_instance
+
 
+    # Function to start the update thread
+    def start_thread(self):
+        self.thread.start()
+
     # Get Image from ROS Driver Camera
     def get_frontal_image(self):
         image = self.cat.get_frontal_image()
@@ -35,31 +60,19 @@ def get_ventral_image(self):
 
     def get_position(self):
         pos = self.cat.get_position()
-        return pos
+        self.shared_position.add(pos)
 
     def get_velocity(self):
         vel = self.cat.get_velocity()
-        return vel
+        self.shared_velocity.add(vel )
 
     def get_yaw_rate(self):
         yaw_rate = self.cat.get_yaw_rate()
         return yaw_rate
 
     def get_orientation(self):
         orientation = self.cat.get_orientation()
-        return orientation
-
-    def get_roll(self):
-        roll = self.cat.get_roll()
-        return roll
-
-    def get_pitch(self):
-        pitch = self.cat.get_pitch()
-        return pitch
-
-    def get_yaw(self):
-        yaw = self.cat.get_yaw()
-        return yaw
+        self.shared_orientation.add(orientation )
 
     def get_landed_state(self):
         state = self.cat.get_landed_state()
@@ -79,3 +92,64 @@ def takeoff(self, h=3):
 
     def land(self):
         self.cat.land()
+
+    def update_hal(self):
+        CMD = self.shared_CMD.get()
+
+        self.get_frontal_image()
+        self.get_ventral_image()
+        self.get_position()
+        self.get_velocity()
+        self.get_yaw_rate()
+        self.get_orientation()
+        self.get_landed_state()
+
+        if CMD == 0:  # POS
+            self.set_cmd_pos()
+        elif CMD == 1:  # VEL
+            self.set_cmd_vel()
+        elif CMD == 2:  # MIX
+            self.set_cmd_mix()
+        elif CMD == 3:  # TAKEOFF
+            self.takeoff()
+        elif CMD == 4:  # LAND
+            self.land()
+
+    # Destructor function to close all fds
+    def __del__(self):
+        self.shared_frontal_image.close()
+        self.shared_ventral_image.close()
+        self.shared_x.close()
+        self.shared_y.close()
+        self.shared_z.close()
+        self.shared_takeoff_z.close()
+        self.shared_az.close()
+        self.shared_azt.close()
+        self.shared_vx.close()
+        self.shared_vy.close()
+        self.shared_vz.close()
+        self.shared_landed_state.close()
+        self.shared_position.close()
+        self.shared_velocity.close()
+        self.shared_orientation.close()
+        self.shared_yaw_rate.close()
+
+class ThreadHAL(threading.Thread):
+    def __init__(self, update_function):
+        super(ThreadHAL, self).__init__()
+        self.time_cycle = 80
+        self.update_function = update_function
+
+    def run(self):
+        while(True):
+            start_time = datetime.now()
+
+            self.update_function()
+
+            finish_time = datetime.now()
+
+            dt = finish_time - start_time
+            ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
+
+            if(ms < self.time_cycle):
+                time.sleep((self.time_cycle - ms) / 1000.0)

exercises/static/exercises/drone_cat_mouse/web-template/hal_guest.py

@@ -12,16 +12,33 @@ class HAL:
     def __init__(self):
         rospy.init_node("HAL_mouse")
 
+        # Shared memory variables
+        self.shared_frontal_image = SharedImage("halfrontalimageguest")
+        self.shared_ventral_image = SharedImage("halventralimageguest")
+        self.shared_x = SharedValue("xguest")
+        self.shared_y = SharedValue("yguest")
+        self.shared_z = SharedValue("zguest")
+        self.shared_takeoff_z = SharedValue("sharedtakeoffzguest")
+        self.shared_az = SharedValue("azguest")
+        self.shared_azt = SharedValue("aztguest")
+        self.shared_vx = SharedValue("vxguest")
+        self.shared_vy = SharedValue("vyguest")
+        self.shared_vz = SharedValue("vzguest")
+        self.shared_landed_state = SharedValue("landedstateguest")
+        self.shared_position = SharedValue("positionguest",3)
+        self.shared_velocity = SharedValue("velocityguest",3)
+        self.shared_orientation = SharedValue("orientationguest",3)
+        self.shared_yaw_rate = SharedValue("yawrateguest")
+
+        self.shared_CMD =  SharedValue("CMDguest")
+
         self.image = None
         self.mouse = DroneWrapper(name="rqt", ns="mouse/")
 
-    # Explicit initialization functions
-    # Class method, so user can call it without instantiation
-    @classmethod
-    def initRobot(cls):
-        new_instance = cls()
-        return new_instance
-
+    # Function to start the update thread
+    def start_thread(self):
+        self.thread.start()
+
     # Get Image from ROS Driver Camera
     def get_frontal_image(self):
         image = self.mouse.get_frontal_image()
@@ -35,31 +52,19 @@ def get_ventral_image(self):
 
     def get_position(self):
         pos = self.mouse.get_position()
-        return pos
+        self.shared_position.add(pos)
 
     def get_velocity(self):
         vel = self.mouse.get_velocity()
-        return vel
+        self.shared_velocity.add(vel )
 
     def get_yaw_rate(self):
         yaw_rate = self.mouse.get_yaw_rate()
         return yaw_rate
 
     def get_orientation(self):
         orientation = self.mouse.get_orientation()
-        return orientation
-
-    def get_roll(self):
-        roll = self.mouse.get_roll()
-        return roll
-
-    def get_pitch(self):
-        pitch = self.mouse.get_pitch()
-        return pitch
-
-    def get_yaw(self):
-        yaw = self.mouse.get_yaw()
-        return yaw
+        self.shared_orientation.add(orientation )
 
     def get_landed_state(self):
         state = self.mouse.get_landed_state()
@@ -79,3 +84,64 @@ def takeoff(self, h=3):
 
     def land(self):
         self.mouse.land()
+
+    def update_hal(self):
+        CMD = self.shared_CMD.get()
+
+        self.get_frontal_image()
+        self.get_ventral_image()
+        self.get_position()
+        self.get_velocity()
+        self.get_yaw_rate()
+        self.get_orientation()
+        self.get_landed_state()
+
+        if CMD == 0:  # POS
+            self.set_cmd_pos()
+        elif CMD == 1:  # VEL
+            self.set_cmd_vel()
+        elif CMD == 2:  # MIX
+            self.set_cmd_mix()
+        elif CMD == 3:  # TAKEOFF
+            self.takeoff()
+        elif CMD == 4:  # LAND
+            self.land()
+
+    # Destructor function to close all fds
+    def __del__(self):
+        self.shared_frontal_image.close()
+        self.shared_ventral_image.close()
+        self.shared_x.close()
+        self.shared_y.close()
+        self.shared_z.close()
+        self.shared_takeoff_z.close()
+        self.shared_az.close()
+        self.shared_azt.close()
+        self.shared_vx.close()
+        self.shared_vy.close()
+        self.shared_vz.close()
+        self.shared_landed_state.close()
+        self.shared_position.close()
+        self.shared_velocity.close()
+        self.shared_orientation.close()
+        self.shared_yaw_rate.close()
+
+class ThreadHAL(threading.Thread):
+    def __init__(self, update_function):
+        super(ThreadHAL, self).__init__()
+        self.time_cycle = 80
+        self.update_function = update_function
+
+    def run(self):
+        while(True):
+            start_time = datetime.now()
+
+            self.update_function()
+
+            finish_time = datetime.now()
+
+            dt = finish_time - start_time
+            ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
+
+            if(ms < self.time_cycle):
+                time.sleep((self.time_cycle - ms) / 1000.0)

exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py

@@ -0,0 +1,87 @@
+import numpy as np
+import mmap
+from posix_ipc import Semaphore, O_CREX, ExistentialError, O_CREAT, SharedMemory, unlink_shared_memory
+from ctypes import sizeof, memmove, addressof, create_string_buffer, c_float
+import struct
+
+class SharedValue:
+    def __init__(self, name, n_elem = 1):
+        # Initialize varaibles for memory regions and buffers and Semaphore
+        self.n_elem = n_elem
+        self.shm_buf = [None]*self.n_elem; self.shm_region = [None]*self.n_elem
+        self.value_lock = [None]*self.n_elem
+
+        self.shm_name = name; self.value_lock_name = name
+
+        # Initialize or retreive Semaphore
+        for i in range(self.n_elem):
+            try:
+                self.value_lock[i] = Semaphore(self.value_lock_name+str(i), O_CREX)
+            except ExistentialError:
+                value_lock = Semaphore(self.value_lock_name+str(i), O_CREAT)
+                value_lock.unlink()
+                self.value_lock[i] = Semaphore(self.value_lock_name+str(i), O_CREX)
+
+            self.value_lock[i].release()
+
+    # Get the shared value
+    def get(self):
+        # Retreive the data from buffer
+
+        value = [None]*self.n_elem
+        for i in range(self.n_elem):
+            try:
+                self.shm_region[i] = SharedMemory(self.shm_name+str(i))
+                self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, sizeof(c_float))
+                self.shm_region[i].close_fd()
+            except ExistentialError:
+                self.shm_region[i] = SharedMemory(self.shm_name+str(i), O_CREAT, size=sizeof(c_float))
+                self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, self.shm_region[i].size)
+                self.shm_region[i].close_fd()
+            self.value_lock[i].acquire()
+            value[i] = struct.unpack('f', self.shm_buf[i])[0]
+            self.value_lock[i].release()
+
+        if self.n_elem <=1:
+            return value[0]
+        else: 
+            return value
+
+
+
+
+    # Add the shared value
+    def add(self, value):
+        # Send the data to shared regions
+
+        for i in range(self.n_elem):
+            try:
+                self.shm_region[i] = SharedMemory(self.shm_name+str(i))
+                self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, sizeof(c_float))
+                self.shm_region[i].close_fd()
+            except ExistentialError:
+                self.shm_region[i] = SharedMemory(self.shm_name+str(i), O_CREAT, size=sizeof(c_float))
+                self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, self.shm_region[i].size)
+                self.shm_region[i].close_fd()
+
+            self.value_lock[i].acquire()
+            if self.n_elem <=1:
+                self.shm_buf[i][:] = struct.pack('f', value)
+            else:
+                self.shm_buf[i][:] = struct.pack('f', value[i])
+            self.value_lock[i].release()
+
+
+    # Destructor function to unlink and disconnect
+    def close(self):
+        for i in range(self.n_elem):
+            self.value_lock[i].acquire()
+            self.shm_buf[i].close()
+
+            try:
+                unlink_shared_memory(self.shm_name+str(i))
+            except ExistentialError:
+                pass
+
+            self.value_lock[i].release()
+            self.value_lock[i].close()