From dda0c8546cb242265f0e29e546c2b61b1708a4bd Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Fri, 16 Sep 2022 16:33:08 +0530 Subject: [PATCH 1/3] pos control ex bug fix for beacon and value script of multiprocessing refactored --- .../web-template/shared/value.py | 95 +++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ .../follow_road/web-template/shared/value.py | 95 +++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ .../position_control/web-template/hal.py | 73 ++++++++-- .../web-template/shared/value.py | 107 ++++++++++++++ .../web-template/user_functions.py | 133 ++++++++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ .../web-template/shared/value.py | 95 +++++++++++++ 12 files changed, 1159 insertions(+), 9 deletions(-) create mode 100755 exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py create mode 100755 exercises/static/exercises/drone_gymkhana/web-template/shared/value.py create mode 100755 exercises/static/exercises/follow_road/web-template/shared/value.py create mode 100755 exercises/static/exercises/follow_turtlebot/web-template/shared/value.py create mode 100755 exercises/static/exercises/labyrinth_escape/web-template/shared/value.py create mode 100755 exercises/static/exercises/package_delivery/web-template/shared/value.py create mode 100755 exercises/static/exercises/position_control/web-template/shared/value.py create mode 100755 exercises/static/exercises/position_control/web-template/user_functions.py create mode 100755 exercises/static/exercises/power_tower_inspection/web-template/shared/value.py create mode 100755 exercises/static/exercises/rescue_people/web-template/shared/value.py create mode 100755 exercises/static/exercises/visual_lander/web-template/shared/value.py diff --git a/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py b/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py b/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/follow_road/web-template/shared/value.py b/exercises/static/exercises/follow_road/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/follow_road/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py b/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py b/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/package_delivery/web-template/shared/value.py b/exercises/static/exercises/package_delivery/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/package_delivery/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/position_control/web-template/hal.py b/exercises/static/exercises/position_control/web-template/hal.py index 4f11a1fbc..b457889a4 100644 --- a/exercises/static/exercises/position_control/web-template/hal.py +++ b/exercises/static/exercises/position_control/web-template/hal.py @@ -83,16 +83,71 @@ def land(self): self.drone.land() def init_beacons(self): - self.beacons = [] - self.beacons.append(Beacon('beacon1', np.array([0, 5, 0]), False, False)) - self.beacons.append(Beacon('beacon2', np.array([5, 0, 0]), False, False)) - self.beacons.append(Beacon('beacon3', np.array([0, -5, 0]), False, False)) - self.beacons.append(Beacon('beacon4', np.array([-5, 0, 0]), False, False)) - self.beacons.append(Beacon('beacon5', np.array([10, 0, 0]), False, False)) - self.beacons.append(Beacon('initial', np.array([0, 0, 0]), False, False)) + self.beacons = self.shared_beacons.get(type_name = "list", n_elem = 6) def get_next_beacon(self): for beacon in self.beacons: if beacon.is_reached() == False: - return beacon - return None \ No newline at end of file + self.shared_beacons.add(np.array([beacon]) ,type_name="list") + + + def update_hal(self): + self.get_frontal_image() + self.get_ventral_image() + self.get_position() + self.get_velocity() + self.get_yaw_rate() + self.get_orientation() + self.get_pitch() + self.get_roll() + self.get_yaw() + self.get_landed_state() + self.set_cmd_pos() + self.set_cmd_vel() + self.set_cmd_mix() + self.init_beacons() + self.get_next_beacon() + + + # 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_roll.close() + self.shared_pitch.close() + self.shared_yaw.close() + self.shared_yaw_rate.close() + self.shared_beacons.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) diff --git a/exercises/static/exercises/position_control/web-template/shared/value.py b/exercises/static/exercises/position_control/web-template/shared/value.py new file mode 100755 index 000000000..4b902fd10 --- /dev/null +++ b/exercises/static/exercises/position_control/web-template/shared/value.py @@ -0,0 +1,107 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value", n_elem = 3): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + if n_elem == 1 : + mock_val_arr = np.array([0.0]) + elif n_elem == 3: + mock_val_arr = np.array([0.0, 0.0, 0.0]) + elif n_elem == 6: + mock_val_arr = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + if n_elem == 1 : + array_val = np.ndarray(shape=(1,), + dtype = 'O', buffer=self.shm_buf) + elif n_elem == 3: + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + elif n_elem == 6: + array_val = np.ndarray(shape=(6,), + dtype = 'O', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/position_control/web-template/user_functions.py b/exercises/static/exercises/position_control/web-template/user_functions.py new file mode 100755 index 000000000..d0464d60e --- /dev/null +++ b/exercises/static/exercises/position_control/web-template/user_functions.py @@ -0,0 +1,133 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 +from shared.Beacon import Beacon + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + self.shared_velocity = SharedValue("velocity") + self.shared_orientation = SharedValue("orientation") + self.shared_roll = SharedValue("roll") + self.shared_pitch = SharedValue("pitch") + self.shared_yaw = SharedValue("yaw") + self.shared_yaw_rate = SharedValue("yawrate") + self.shared_beacons = SharedValue("beacons") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + def land(self): + pass + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(z) + self.shared_azt.add(az) + + + def get_position(self): + position = self.shared_position.get(type_name = "list") + return position + + def get_velocity(self): + velocity = self.shared_velocity.get(type_name = "list") + return velocity + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get(type_name = "value") + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get(type_name = "list") + return orientation + + def get_roll(self): + roll = self.shared_roll.get(type_name = "value") + return roll + + def get_pitch(self): + pitch = self.shared_pitch.get(type_name = "value") + return pitch + + def get_yaw(self): + yaw = self.shared_yaw.get(type_name = "value") + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get(type_name = "value") + return landed_state + + def init_beacons(self): + self.beacons = [] + self.beacons.append(Beacon('beacon1', np.array([0, 5, 0]), False, False)) + self.beacons.append(Beacon('beacon2', np.array([5, 0, 0]), False, False)) + self.beacons.append(Beacon('beacon3', np.array([0, -5, 0]), False, False)) + self.beacons.append(Beacon('beacon4', np.array([-5, 0, 0]), False, False)) + self.beacons.append(Beacon('beacon5', np.array([10, 0, 0]), False, False)) + self.beacons.append(Beacon('initial', np.array([0, 0, 0]), False, False)) + self.shared_beacons.add(self.beacons ,type_name="list") + + def get_next_beacon(self): + beacon = self.shared_beacons.get(type_name = "list", n_elem = 1) + return beacon[0] + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py b/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/rescue_people/web-template/shared/value.py b/exercises/static/exercises/rescue_people/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/rescue_people/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/visual_lander/web-template/shared/value.py b/exercises/static/exercises/visual_lander/web-template/shared/value.py new file mode 100755 index 000000000..9315148d4 --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/shared/value.py @@ -0,0 +1,95 @@ +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): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + mock_val_arr = np.array([0.0,0.0,0.0]) + byte_size = mock_val_arr.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() From f702db9386ddd405f02c21fdce56f6b02d02c901 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Mon, 19 Sep 2022 13:51:50 +0530 Subject: [PATCH 2/3] reduced shared variable objects for optimisation --- .../drone_cat_mouse/web-template/hal.py | 118 ++++++++++++--- .../drone_cat_mouse/web-template/hal_guest.py | 110 +++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ .../web-template/user_functions_guest.py | 124 ++++++++++++++++ .../drone_gymkhana/web-template/hal.py | 102 ++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ .../drone_hangar/web-template/hal.py | 102 ++++++++++--- .../drone_hangar/web-template/shared/value.py | 87 +++++++++++ .../web-template/user_functions.py | 124 ++++++++++++++++ .../exercises/follow_road/web-template/hal.py | 102 ++++++++++--- .../follow_road/web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ .../follow_turtlebot/web-template/hal.py | 102 ++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ .../labyrinth_escape/web-template/hal.py | 102 ++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ .../package_delivery/web-template/hal.py | 118 ++++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 136 ++++++++++++++++++ .../position_control/web-template/hal.py | 92 +++++++----- .../web-template/shared/value.py | 126 +++++++--------- .../web-template/user_functions.py | 58 ++++---- .../web-template/hal.py | 102 ++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ .../rescue_people/web-template/hal.py | 126 +++++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 130 +++++++++++++++++ .../visual_lander/web-template/hal.py | 102 ++++++++++--- .../web-template/shared/value.py | 114 +++++++-------- .../web-template/user_functions.py | 124 ++++++++++++++++ scripts/pylint_checker.py | 2 +- 36 files changed, 3085 insertions(+), 874 deletions(-) create mode 100755 exercises/static/exercises/drone_cat_mouse/web-template/user_functions.py create mode 100644 exercises/static/exercises/drone_cat_mouse/web-template/user_functions_guest.py create mode 100755 exercises/static/exercises/drone_gymkhana/web-template/user_functions.py create mode 100755 exercises/static/exercises/drone_hangar/web-template/shared/value.py create mode 100755 exercises/static/exercises/drone_hangar/web-template/user_functions.py create mode 100755 exercises/static/exercises/follow_road/web-template/user_functions.py create mode 100755 exercises/static/exercises/follow_turtlebot/web-template/user_functions.py create mode 100755 exercises/static/exercises/labyrinth_escape/web-template/user_functions.py create mode 100755 exercises/static/exercises/package_delivery/web-template/user_functions.py create mode 100755 exercises/static/exercises/power_tower_inspection/web-template/user_functions.py create mode 100755 exercises/static/exercises/rescue_people/web-template/user_functions.py create mode 100755 exercises/static/exercises/visual_lander/web-template/user_functions.py diff --git a/exercises/static/exercises/drone_cat_mouse/web-template/hal.py b/exercises/static/exercises/drone_cat_mouse/web-template/hal.py index de7d0570b..7a9cab46f 100644 --- a/exercises/static/exercises/drone_cat_mouse/web-template/hal.py +++ b/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,11 +60,11 @@ 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() @@ -47,19 +72,7 @@ def get_yaw_rate(self): 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) diff --git a/exercises/static/exercises/drone_cat_mouse/web-template/hal_guest.py b/exercises/static/exercises/drone_cat_mouse/web-template/hal_guest.py index ac9b969f2..0bd36a472 100644 --- a/exercises/static/exercises/drone_cat_mouse/web-template/hal_guest.py +++ b/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,11 +52,11 @@ 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() @@ -47,19 +64,7 @@ def get_yaw_rate(self): 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) diff --git a/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py b/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py +++ b/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/drone_cat_mouse/web-template/user_functions.py b/exercises/static/exercises/drone_cat_mouse/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/drone_cat_mouse/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/drone_cat_mouse/web-template/user_functions_guest.py b/exercises/static/exercises/drone_cat_mouse/web-template/user_functions_guest.py new file mode 100644 index 000000000..add1d5c08 --- /dev/null +++ b/exercises/static/exercises/drone_cat_mouse/web-template/user_functions_guest.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimageguest") + self.shared_left_image = SharedImage("guiventralimageguest") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/drone_gymkhana/web-template/hal.py b/exercises/static/exercises/drone_gymkhana/web-template/hal.py index 25635a119..5901aacd3 100644 --- a/exercises/static/exercises/drone_gymkhana/web-template/hal.py +++ b/exercises/static/exercises/drone_gymkhana/web-template/hal.py @@ -12,10 +12,31 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -36,11 +57,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -48,19 +69,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -80,3 +89,64 @@ def takeoff(self, h=3): def land(self): self.drone.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) diff --git a/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py b/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py +++ b/exercises/static/exercises/drone_gymkhana/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/drone_gymkhana/web-template/user_functions.py b/exercises/static/exercises/drone_gymkhana/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/drone_gymkhana/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/drone_hangar/web-template/hal.py b/exercises/static/exercises/drone_hangar/web-template/hal.py index 25635a119..5901aacd3 100644 --- a/exercises/static/exercises/drone_hangar/web-template/hal.py +++ b/exercises/static/exercises/drone_hangar/web-template/hal.py @@ -12,10 +12,31 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -36,11 +57,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -48,19 +69,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -80,3 +89,64 @@ def takeoff(self, h=3): def land(self): self.drone.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) diff --git a/exercises/static/exercises/drone_hangar/web-template/shared/value.py b/exercises/static/exercises/drone_hangar/web-template/shared/value.py new file mode 100755 index 000000000..0146309a2 --- /dev/null +++ b/exercises/static/exercises/drone_hangar/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() diff --git a/exercises/static/exercises/drone_hangar/web-template/user_functions.py b/exercises/static/exercises/drone_hangar/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/drone_hangar/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/follow_road/web-template/hal.py b/exercises/static/exercises/follow_road/web-template/hal.py index fd13904d6..234ff9e47 100755 --- a/exercises/static/exercises/follow_road/web-template/hal.py +++ b/exercises/static/exercises/follow_road/web-template/hal.py @@ -14,10 +14,31 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -38,11 +59,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -50,19 +71,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -82,3 +91,64 @@ def takeoff(self, h=3): def land(self): self.drone.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) diff --git a/exercises/static/exercises/follow_road/web-template/shared/value.py b/exercises/static/exercises/follow_road/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/follow_road/web-template/shared/value.py +++ b/exercises/static/exercises/follow_road/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/follow_road/web-template/user_functions.py b/exercises/static/exercises/follow_road/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/follow_road/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/follow_turtlebot/web-template/hal.py b/exercises/static/exercises/follow_turtlebot/web-template/hal.py index ee0fa1d02..5aa758bfa 100644 --- a/exercises/static/exercises/follow_turtlebot/web-template/hal.py +++ b/exercises/static/exercises/follow_turtlebot/web-template/hal.py @@ -14,10 +14,31 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -38,11 +59,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -50,19 +71,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -82,3 +91,64 @@ def takeoff(self, h=5): def land(self): self.drone.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) diff --git a/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py b/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py +++ b/exercises/static/exercises/follow_turtlebot/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/follow_turtlebot/web-template/user_functions.py b/exercises/static/exercises/follow_turtlebot/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/follow_turtlebot/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/labyrinth_escape/web-template/hal.py b/exercises/static/exercises/labyrinth_escape/web-template/hal.py index 25635a119..5901aacd3 100644 --- a/exercises/static/exercises/labyrinth_escape/web-template/hal.py +++ b/exercises/static/exercises/labyrinth_escape/web-template/hal.py @@ -12,10 +12,31 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -36,11 +57,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -48,19 +69,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -80,3 +89,64 @@ def takeoff(self, h=3): def land(self): self.drone.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) diff --git a/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py b/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py +++ b/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/labyrinth_escape/web-template/user_functions.py b/exercises/static/exercises/labyrinth_escape/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/labyrinth_escape/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/package_delivery/web-template/hal.py b/exercises/static/exercises/package_delivery/web-template/hal.py index f6a0957d0..732bddb48 100644 --- a/exercises/static/exercises/package_delivery/web-template/hal.py +++ b/exercises/static/exercises/package_delivery/web-template/hal.py @@ -3,9 +3,11 @@ import threading import time from datetime import datetime +from shared.magnet import Magnet from drone_wrapper import DroneWrapper -from magnet import Magnet +from shared.image import SharedImage +from shared.value import SharedValue # Hardware Abstraction Layer class HAL: @@ -14,11 +16,36 @@ class HAL: def __init__(self): 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_package_state = SharedValue("packagestate") + + self.shared_CMD = SharedValue("CMD") + self.image = None self.drone = DroneWrapper(name="rqt") self.magnet = Magnet() + # Update thread + self.thread = ThreadHAL(self.update_hal) + + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -39,11 +66,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -51,19 +78,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -92,4 +107,71 @@ def set_cmd_drop(self): def get_pkg_state(self): state = self.magnet.get_pkg_state() - return state + self.shared_package_state.add(state) + + 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() + self.get_pkg_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() + elif CMD == 5: # PKG PICK + self.set_cmd_pick() + elif CMD == 6: # PKG PICK + self.set_cmd_drop() + + # 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() + self.shared_package_state.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) diff --git a/exercises/static/exercises/package_delivery/web-template/shared/value.py b/exercises/static/exercises/package_delivery/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/package_delivery/web-template/shared/value.py +++ b/exercises/static/exercises/package_delivery/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/package_delivery/web-template/user_functions.py b/exercises/static/exercises/package_delivery/web-template/user_functions.py new file mode 100755 index 000000000..edb88628e --- /dev/null +++ b/exercises/static/exercises/package_delivery/web-template/user_functions.py @@ -0,0 +1,136 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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_package_state = SharedValue("packagestate") + + + self.shared_CMD = SharedValue("CMD") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + + def set_cmd_pick(self): + self.shared_CMD.add(5) #PKG PICK + + def set_cmd_drop(self): + self.shared_CMD.add(6) #PKG DROP + + def get_pkg_state(self): + package_state = self.shared_package_state.get() + return package_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/position_control/web-template/hal.py b/exercises/static/exercises/position_control/web-template/hal.py index b457889a4..c0bed0da1 100644 --- a/exercises/static/exercises/position_control/web-template/hal.py +++ b/exercises/static/exercises/position_control/web-template/hal.py @@ -1,10 +1,14 @@ import numpy as np import rospy import cv2 +import threading +import time +from datetime import datetime +from shared.Beacon import Beacon from drone_wrapper import DroneWrapper -from Beacon import Beacon - +from shared.image import SharedImage +from shared.value import SharedValue # Hardware Abstraction Layer class HAL: @@ -13,10 +17,34 @@ class HAL: def __init__(self): 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_beacons = SharedValue("beacons", 6) + self.shared_beacon = SharedValue("beacon") + + + self.shared_CMD = SharedValue("CMD") + self.image = None self.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -37,11 +65,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -49,19 +77,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -81,33 +97,43 @@ def takeoff(self, h=3): def land(self): self.drone.land() - + def init_beacons(self): - self.beacons = self.shared_beacons.get(type_name = "list", n_elem = 6) + self.beacons = self.shared_beacons.get() def get_next_beacon(self): for beacon in self.beacons: if beacon.is_reached() == False: - self.shared_beacons.add(np.array([beacon]) ,type_name="list") - + self.shared_beacon.add(beacon) 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_pitch() - self.get_roll() - self.get_yaw() self.get_landed_state() - self.set_cmd_pos() - self.set_cmd_vel() - self.set_cmd_mix() - self.init_beacons() - self.get_next_beacon() - + + + + 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() + elif CMD == 5: # INIT BEACON + self.init_beacons() + elif CMD == 6: # NEXT BEACON + self.get_next_beacon() + # Destructor function to close all fds def __del__(self): @@ -126,11 +152,9 @@ def __del__(self): self.shared_position.close() self.shared_velocity.close() self.shared_orientation.close() - self.shared_roll.close() - self.shared_pitch.close() - self.shared_yaw.close() self.shared_yaw_rate.close() self.shared_beacons.close() + self.shared_beacon.close() class ThreadHAL(threading.Thread): def __init__(self, update_function): diff --git a/exercises/static/exercises/position_control/web-template/shared/value.py b/exercises/static/exercises/position_control/web-template/shared/value.py index 4b902fd10..0146309a2 100755 --- a/exercises/static/exercises/position_control/web-template/shared/value.py +++ b/exercises/static/exercises/position_control/web-template/shared/value.py @@ -5,103 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value", n_elem = 3): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - if n_elem == 1 : - mock_val_arr = np.array([0.0]) - elif n_elem == 3: - mock_val_arr = np.array([0.0, 0.0, 0.0]) - elif n_elem == 6: - mock_val_arr = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - if n_elem == 1 : - array_val = np.ndarray(shape=(1,), - dtype = 'O', buffer=self.shm_buf) - elif n_elem == 3: - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - elif n_elem == 6: - array_val = np.ndarray(shape=(6,), - dtype = 'O', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/position_control/web-template/user_functions.py b/exercises/static/exercises/position_control/web-template/user_functions.py index d0464d60e..3b3ac58cd 100755 --- a/exercises/static/exercises/position_control/web-template/user_functions.py +++ b/exercises/static/exercises/position_control/web-template/user_functions.py @@ -20,14 +20,14 @@ def __init__(self): self.shared_vy = SharedValue("vy") self.shared_vz = SharedValue("vz") self.shared_landed_state = SharedValue("landedstate") - self.shared_position = SharedValue("position") - self.shared_velocity = SharedValue("velocity") - self.shared_orientation = SharedValue("orientation") - self.shared_roll = SharedValue("roll") - self.shared_pitch = SharedValue("pitch") - self.shared_yaw = SharedValue("yaw") + 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_beacons = SharedValue("beacons") + self.shared_beacons = SharedValue("beacons", 6) + self.shared_beacon = SharedValue("beacon") + + self.shared_CMD = SharedValue("CMD") # Get image function @@ -43,60 +43,68 @@ def get_ventral_image(self): def takeoff(self, height): self.shared_takeoff_z.add(height) + self.shared_CMD.add(3) #TAKEOFF + def land(self): - pass + self.shared_CMD.add(4) #LAND def set_cmd_pos(self, x, y , z, az): self.shared_x.add(x) self.shared_y.add(y) self.shared_z.add(z) self.shared_az.add(az) + + self.shared_CMD.add(0) #POS def set_cmd_vel(self, vx, vy, vz, az): self.shared_vx.add(vx) self.shared_vy.add(vy) self.shared_vz.add(vz) self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL def set_cmd_mix(self, vx, vy, z, az): self.shared_vx.add(vx) self.shared_vy.add(vy) - self.shared_vz.add(z) + self.shared_z.add(z) self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX def get_position(self): - position = self.shared_position.get(type_name = "list") - return position + position = self.shared_position.get() + return np.array(position) def get_velocity(self): - velocity = self.shared_velocity.get(type_name = "list") - return velocity + velocity = self.shared_velocity.get() + return np.array(velocity) def get_yaw_rate(self): - yaw_rate = self.shared_yaw_rate.get(type_name = "value") + yaw_rate = self.shared_yaw_rate.get() return yaw_rate def get_orientation(self): - orientation = self.shared_orientation.get(type_name = "list") - return orientation + orientation = self.shared_orientation.get() + return np.array(orientation) def get_roll(self): - roll = self.shared_roll.get(type_name = "value") + roll = self.shared_orientation.get()[0] return roll def get_pitch(self): - pitch = self.shared_pitch.get(type_name = "value") + pitch = self.shared_orientation.get()[1] return pitch def get_yaw(self): - yaw = self.shared_yaw.get(type_name = "value") + yaw = self.shared_orientation.get()[2] return yaw def get_landed_state(self): - landed_state = self.shared_landed_state.get(type_name = "value") + landed_state = self.shared_landed_state.get() return landed_state - + def init_beacons(self): self.beacons = [] self.beacons.append(Beacon('beacon1', np.array([0, 5, 0]), False, False)) @@ -105,11 +113,13 @@ def init_beacons(self): self.beacons.append(Beacon('beacon4', np.array([-5, 0, 0]), False, False)) self.beacons.append(Beacon('beacon5', np.array([10, 0, 0]), False, False)) self.beacons.append(Beacon('initial', np.array([0, 0, 0]), False, False)) - self.shared_beacons.add(self.beacons ,type_name="list") + self.shared_beacons.add(self.beacons) + self.shared_CMD.add(5) # INIT BEACON def get_next_beacon(self): - beacon = self.shared_beacons.get(type_name = "list", n_elem = 1) - return beacon[0] + beacon = self.shared_beacon.get() + self.shared_CMD.add(6) # NEXT BEACON + return beacon # Define GUI functions class GUIFunctions: diff --git a/exercises/static/exercises/power_tower_inspection/web-template/hal.py b/exercises/static/exercises/power_tower_inspection/web-template/hal.py index 4e68e2c52..4db5d6961 100644 --- a/exercises/static/exercises/power_tower_inspection/web-template/hal.py +++ b/exercises/static/exercises/power_tower_inspection/web-template/hal.py @@ -13,11 +13,32 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -38,11 +59,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -50,19 +71,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -83,4 +92,63 @@ def takeoff(self, h=5): def land(self): self.drone.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) diff --git a/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py b/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py +++ b/exercises/static/exercises/power_tower_inspection/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/power_tower_inspection/web-template/user_functions.py b/exercises/static/exercises/power_tower_inspection/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/power_tower_inspection/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/rescue_people/web-template/hal.py b/exercises/static/exercises/rescue_people/web-template/hal.py index 25635a119..b0411cfc0 100644 --- a/exercises/static/exercises/rescue_people/web-template/hal.py +++ b/exercises/static/exercises/rescue_people/web-template/hal.py @@ -1,9 +1,14 @@ import numpy as np import rospy import cv2 +import threading +import time +from datetime import datetime +from shared.light import Light from drone_wrapper import DroneWrapper - +from shared.image import SharedImage +from shared.value import SharedValue # Hardware Abstraction Layer class HAL: @@ -12,9 +17,35 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + self.drone = DroneWrapper(name="rqt",ns="/iris/") + self.light = Light() + + + # Update thread + self.thread = ThreadHAL(self.update_hal) + + # Explicit initialization functions # Class method, so user can call it without instantiation @@ -36,11 +67,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -48,19 +79,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -80,3 +99,74 @@ def takeoff(self, h=3): def land(self): self.drone.land() + + def set_cmd_on(self): + self.light.set_cmd_on() + + def set_cmd_off(self): + self.light.set_cmd_off() + + 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() + elif CMD == 5: # LIGHT ON + self.set_cmd_on() + elif CMD == 6: # LIGHT OFF + self.set_cmd_off() + + # 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) diff --git a/exercises/static/exercises/rescue_people/web-template/shared/value.py b/exercises/static/exercises/rescue_people/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/rescue_people/web-template/shared/value.py +++ b/exercises/static/exercises/rescue_people/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/rescue_people/web-template/user_functions.py b/exercises/static/exercises/rescue_people/web-template/user_functions.py new file mode 100755 index 000000000..69a47b344 --- /dev/null +++ b/exercises/static/exercises/rescue_people/web-template/user_functions.py @@ -0,0 +1,130 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + + def set_cmd_on(self): + self.shared_CMD.add(5) #LIGHT ON + + def set_cmd_off(self): + self.shared_CMD.add(6) #LIGHT OFF + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/hal.py b/exercises/static/exercises/visual_lander/web-template/hal.py index 25635a119..5901aacd3 100644 --- a/exercises/static/exercises/visual_lander/web-template/hal.py +++ b/exercises/static/exercises/visual_lander/web-template/hal.py @@ -12,10 +12,31 @@ class HAL: def __init__(self): 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.drone = DroneWrapper(name="rqt") + + # Explicit initialization functions # Class method, so user can call it without instantiation @classmethod @@ -36,11 +57,11 @@ def get_ventral_image(self): def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos) def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ) def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() @@ -48,19 +69,7 @@ def get_yaw_rate(self): def get_orientation(self): orientation = self.drone.get_orientation() - return orientation - - def get_roll(self): - roll = self.drone.get_roll() - return roll - - def get_pitch(self): - pitch = self.drone.get_pitch() - return pitch - - def get_yaw(self): - yaw = self.drone.get_yaw() - return yaw + self.shared_orientation.add(orientation ) def get_landed_state(self): state = self.drone.get_landed_state() @@ -80,3 +89,64 @@ def takeoff(self, h=3): def land(self): self.drone.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) diff --git a/exercises/static/exercises/visual_lander/web-template/shared/value.py b/exercises/static/exercises/visual_lander/web-template/shared/value.py index 9315148d4..0146309a2 100755 --- a/exercises/static/exercises/visual_lander/web-template/shared/value.py +++ b/exercises/static/exercises/visual_lander/web-template/shared/value.py @@ -5,91 +5,83 @@ import struct class SharedValue: - def __init__(self, name): + def __init__(self, name, n_elem = 1): # Initialize varaibles for memory regions and buffers and Semaphore - self.shm_buf = None; self.shm_region = None - self.value_lock = None + 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 - try: - self.value_lock = Semaphore(self.value_lock_name, O_CREX) - except ExistentialError: - value_lock = Semaphore(self.value_lock_name, O_CREAT) - value_lock.unlink() - self.value_lock = Semaphore(self.value_lock_name, O_CREX) + 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.release() + self.value_lock[i].release() # Get the shared value - def get(self, type_name= "value"): + def get(self): # Retreive the data from buffer - if type_name=="value": + + value = [None]*self.n_elem + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() - self.value_lock.acquire() - value = struct.unpack('f', self.shm_buf)[0] - self.value_lock.release() + 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 - elif type_name=="list": - mock_val_arr = np.array([0.0,0.0,0.0]) - byte_size = mock_val_arr.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - array_val = np.ndarray(shape=(3,), - dtype='float32', buffer=self.shm_buf) - self.value_lock.release() - return array_val - else: - print("missing argument for return type") # Add the shared value - def add(self, value, type_name= "value"): + def add(self, value): # Send the data to shared regions - if type_name=="value": + + for i in range(self.n_elem): try: - self.shm_region = SharedMemory(self.shm_name) - self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) - self.shm_region.close_fd() + 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 = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) - self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) - self.shm_region.close_fd() + 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.acquire() - self.shm_buf[:] = struct.pack('f', value) - self.value_lock.release() - elif type_name=="list": - byte_size = value.nbytes - self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) - self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) - self.shm_region.close_fd() - self.value_lock.acquire() - self.shm_buf[:] = value.tobytes() - self.value_lock.release() + 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): - self.value_lock.acquire() - self.shm_buf.close() + for i in range(self.n_elem): + self.value_lock[i].acquire() + self.shm_buf[i].close() - try: - unlink_shared_memory(self.shm_name) - except ExistentialError: - pass + try: + unlink_shared_memory(self.shm_name+str(i)) + except ExistentialError: + pass - self.value_lock.release() - self.value_lock.close() + self.value_lock[i].release() + self.value_lock[i].close() diff --git a/exercises/static/exercises/visual_lander/web-template/user_functions.py b/exercises/static/exercises/visual_lander/web-template/user_functions.py new file mode 100755 index 000000000..e595a5be1 --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/user_functions.py @@ -0,0 +1,124 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + 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") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + self.shared_CMD.add(3) #TAKEOFF + + def land(self): + self.shared_CMD.add(4) #LAND + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + self.shared_CMD.add(0) #POS + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + self.shared_CMD.add(1) #VEL + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_z.add(z) + self.shared_azt.add(az) + + self.shared_CMD.add(2) #MIX + + + def get_position(self): + position = self.shared_position.get() + return np.array(position) + + def get_velocity(self): + velocity = self.shared_velocity.get() + return np.array(velocity) + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get() + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get() + return np.array(orientation) + + def get_roll(self): + roll = self.shared_orientation.get()[0] + return roll + + def get_pitch(self): + pitch = self.shared_orientation.get()[1] + return pitch + + def get_yaw(self): + yaw = self.shared_orientation.get()[2] + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get() + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/scripts/pylint_checker.py b/scripts/pylint_checker.py index 4c3bd6264..41b6e1bf0 100644 --- a/scripts/pylint_checker.py +++ b/scripts/pylint_checker.py @@ -9,7 +9,7 @@ code_file = tempfile.NamedTemporaryFile(delete=False) code_file.write(python_code.encode()) code_file.seek(0) -options = code_file.name + ' --enable=similarities' + " --disable=C0114,C0116" +options = code_file.name + ' --enable=similarities' + " --disable=C0114,C0116,E1121" (stdout, stderr) = lint.py_run(options, return_std=True) code_file.seek(0) code_file.close() From 60434bf753a1dd16111d618e563fea637a61bb2e Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Mon, 19 Sep 2022 15:22:07 +0530 Subject: [PATCH 3/3] add rotors plugin in world file which initially got removed --- .../drone_gymkhana/web-template/drone_gymkhana.world | 4 ++++ .../exercises/drone_hangar/web-template/drone_hangar.world | 4 ++++ .../exercises/follow_road/web-template/follow_road.world | 4 ++++ .../labyrinth_escape/web-template/labyrinth_escape.world | 4 ++++ .../package_delivery/web-template/package_delivery.world | 4 ++++ .../position_control/web-template/position_control.world | 4 ++++ .../web-template/power_tower_inspection.world | 4 ++++ .../exercises/visual_lander/web-template/visual_lander.world | 4 ++++ 8 files changed, 32 insertions(+) diff --git a/exercises/static/exercises/drone_gymkhana/web-template/drone_gymkhana.world b/exercises/static/exercises/drone_gymkhana/web-template/drone_gymkhana.world index 30a4fea15..0a4c87cb0 100644 --- a/exercises/static/exercises/drone_gymkhana/web-template/drone_gymkhana.world +++ b/exercises/static/exercises/drone_gymkhana/web-template/drone_gymkhana.world @@ -131,6 +131,10 @@ model://sun + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/drone_hangar/web-template/drone_hangar.world b/exercises/static/exercises/drone_hangar/web-template/drone_hangar.world index 48ff09858..72ca428d9 100644 --- a/exercises/static/exercises/drone_hangar/web-template/drone_hangar.world +++ b/exercises/static/exercises/drone_hangar/web-template/drone_hangar.world @@ -166,6 +166,10 @@ + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/follow_road/web-template/follow_road.world b/exercises/static/exercises/follow_road/web-template/follow_road.world index d3d168a68..64565cb65 100755 --- a/exercises/static/exercises/follow_road/web-template/follow_road.world +++ b/exercises/static/exercises/follow_road/web-template/follow_road.world @@ -126,6 +126,10 @@ 15 0 0.01 + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/labyrinth_escape/web-template/labyrinth_escape.world b/exercises/static/exercises/labyrinth_escape/web-template/labyrinth_escape.world index c78d5586d..0b2d3480d 100644 --- a/exercises/static/exercises/labyrinth_escape/web-template/labyrinth_escape.world +++ b/exercises/static/exercises/labyrinth_escape/web-template/labyrinth_escape.world @@ -87,6 +87,10 @@ model://sun + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/package_delivery/web-template/package_delivery.world b/exercises/static/exercises/package_delivery/web-template/package_delivery.world index 6ec6bb43c..3e85abba3 100644 --- a/exercises/static/exercises/package_delivery/web-template/package_delivery.world +++ b/exercises/static/exercises/package_delivery/web-template/package_delivery.world @@ -213,6 +213,10 @@ + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/position_control/web-template/position_control.world b/exercises/static/exercises/position_control/web-template/position_control.world index f5e9a5955..74d915d57 100644 --- a/exercises/static/exercises/position_control/web-template/position_control.world +++ b/exercises/static/exercises/position_control/web-template/position_control.world @@ -63,6 +63,10 @@ model://sun + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/power_tower_inspection/web-template/power_tower_inspection.world b/exercises/static/exercises/power_tower_inspection/web-template/power_tower_inspection.world index 5697153c1..a4937b242 100644 --- a/exercises/static/exercises/power_tower_inspection/web-template/power_tower_inspection.world +++ b/exercises/static/exercises/power_tower_inspection/web-template/power_tower_inspection.world @@ -327,6 +327,10 @@ + + + 0 0 -9.8066 diff --git a/exercises/static/exercises/visual_lander/web-template/visual_lander.world b/exercises/static/exercises/visual_lander/web-template/visual_lander.world index f6b208469..002d28c8f 100644 --- a/exercises/static/exercises/visual_lander/web-template/visual_lander.world +++ b/exercises/static/exercises/visual_lander/web-template/visual_lander.world @@ -36,6 +36,10 @@ model://sun + + + 0 0 -9.8066