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
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/drone_cat_mouse/web-template/shared/value.py
@@ -0,0 +1,87 @@
+import numpy as np
+import mmap
+from posix_ipc import Semaphore, O_CREX, ExistentialError, O_CREAT, SharedMemory, unlink_shared_memory
+from ctypes import sizeof, memmove, addressof, create_string_buffer, c_float
+import struct
+
+class SharedValue:
+ def __init__(self, name, n_elem = 1):
+ # Initialize varaibles for memory regions and buffers and Semaphore
+ self.n_elem = n_elem
+ self.shm_buf = [None]*self.n_elem; self.shm_region = [None]*self.n_elem
+ self.value_lock = [None]*self.n_elem
+
+ self.shm_name = name; self.value_lock_name = name
+
+ # Initialize or retreive Semaphore
+ for i in range(self.n_elem):
+ try:
+ self.value_lock[i] = Semaphore(self.value_lock_name+str(i), O_CREX)
+ except ExistentialError:
+ value_lock = Semaphore(self.value_lock_name+str(i), O_CREAT)
+ value_lock.unlink()
+ self.value_lock[i] = Semaphore(self.value_lock_name+str(i), O_CREX)
+
+ self.value_lock[i].release()
+
+ # Get the shared value
+ def get(self):
+ # Retreive the data from buffer
+
+ value = [None]*self.n_elem
+ for i in range(self.n_elem):
+ try:
+ self.shm_region[i] = SharedMemory(self.shm_name+str(i))
+ self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, sizeof(c_float))
+ self.shm_region[i].close_fd()
+ except ExistentialError:
+ self.shm_region[i] = SharedMemory(self.shm_name+str(i), O_CREAT, size=sizeof(c_float))
+ self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, self.shm_region[i].size)
+ self.shm_region[i].close_fd()
+ self.value_lock[i].acquire()
+ value[i] = struct.unpack('f', self.shm_buf[i])[0]
+ self.value_lock[i].release()
+
+ if self.n_elem <=1:
+ return value[0]
+ else:
+ return value
+
+
+
+
+ # Add the shared value
+ def add(self, value):
+ # Send the data to shared regions
+
+ for i in range(self.n_elem):
+ try:
+ self.shm_region[i] = SharedMemory(self.shm_name+str(i))
+ self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, sizeof(c_float))
+ self.shm_region[i].close_fd()
+ except ExistentialError:
+ self.shm_region[i] = SharedMemory(self.shm_name+str(i), O_CREAT, size=sizeof(c_float))
+ self.shm_buf[i] = mmap.mmap(self.shm_region[i].fd, self.shm_region[i].size)
+ self.shm_region[i].close_fd()
+
+ self.value_lock[i].acquire()
+ if self.n_elem <=1:
+ self.shm_buf[i][:] = struct.pack('f', value)
+ else:
+ self.shm_buf[i][:] = struct.pack('f', value[i])
+ self.value_lock[i].release()
+
+
+ # Destructor function to unlink and disconnect
+ def close(self):
+ for i in range(self.n_elem):
+ self.value_lock[i].acquire()
+ self.shm_buf[i].close()
+
+ try:
+ unlink_shared_memory(self.shm_name+str(i))
+ except ExistentialError:
+ pass
+
+ self.value_lock[i].release()
+ self.value_lock[i].close()
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/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_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
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/drone_gymkhana/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_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/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/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/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/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
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/follow_road/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/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
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/follow_turtlebot/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/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/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/labyrinth_escape/web-template/shared/value.py b/exercises/static/exercises/labyrinth_escape/web-template/shared/value.py
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/labyrinth_escape/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/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/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/package_delivery/web-template/shared/value.py b/exercises/static/exercises/package_delivery/web-template/shared/value.py
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/package_delivery/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/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 4f11a1fbc..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,18 +97,81 @@ def takeoff(self, h=3):
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()
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_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_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: # INIT BEACON
+ self.init_beacons()
+ elif CMD == 6: # NEXT BEACON
+ 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_yaw_rate.close()
+ self.shared_beacons.close()
+ self.shared_beacon.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/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/position_control/web-template/shared/value.py b/exercises/static/exercises/position_control/web-template/shared/value.py
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/position_control/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/position_control/web-template/user_functions.py b/exercises/static/exercises/position_control/web-template/user_functions.py
new file mode 100755
index 000000000..3b3ac58cd
--- /dev/null
+++ b/exercises/static/exercises/position_control/web-template/user_functions.py
@@ -0,0 +1,143 @@
+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",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")
+
+
+ # 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 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)
+ self.shared_CMD.add(5) # INIT BEACON
+
+ def get_next_beacon(self):
+ beacon = self.shared_beacon.get()
+ self.shared_CMD.add(6) # NEXT BEACON
+ return beacon
+
+# 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/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/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/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..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/power_tower_inspection/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/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
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/rescue_people/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/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
new file mode 100755
index 000000000..0146309a2
--- /dev/null
+++ b/exercises/static/exercises/visual_lander/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/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/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
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()