imu.py

imu.py

from objc_util import *
import ctypes
import ui
from math import pi
from ImageColor import getrgb
import threading
from random import random
import multipeer, platform
import ast

load_framework('SceneKit')

SCNView, SCNScene, SCNBox, SCNPyramid, SCNCone, SCNCylinder, SCNSphere, SCNPlane, SCNNode, SCNMaterial, SCNCamera, SCNLight, SCNAction, SCNLookAtConstraint = map(ObjCClass, ['SCNView', 'SCNScene', 'SCNBox', 'SCNPyramid', 'SCNCone', 'SCNCylinder', 'SCNSphere', 'SCNPlane', 'SCNNode', 'SCNMaterial', 'SCNCamera', 'SCNLight', 'SCNAction',  'SCNLookAtConstraint' ])

class CMRotationRate (Structure):
	_fields_ = [('x', c_double), ('y', c_double), ('z', c_double)]
	
SCNTransaction = ObjCClass('SCNTransaction').alloc()
	
class ChatPeer(multipeer.MultipeerConnectivity):
	@on_main_thread
	def receive(self, message, from_peer):
		#global geometry_node
		#print('my Message from', from_peer.display_name, '-' , message)
		roll,pitch,yaw = ast.literal_eval(str(message))
		#print(roll,pitch,yaw)
		# change euler angles but by using animation
		SCNTransaction.begin()
		SCNTransaction.setAnimationDuration(0.3)
		self.geometry_node.setEulerAngles((pitch, yaw, roll))
		SCNTransaction.commit()

class my_thread_bt(threading.Thread):
	def __init__(self, geometry_node):
		threading.Thread.__init__(self)
		self.name = 'bt'
		self.stop = False
		self.geometry_node = geometry_node
	def run(self):
		pitch = 0
		yaw   = 0
		roll  = 0
		delta_ang = pi/10
		SCNTransaction = ObjCClass('SCNTransaction').alloc()
		# https://forum.omz-software.com/topic/3030
		CMMotionManager = ObjCClass('CMMotionManager').alloc().init()
		#print(CMMotionManager.isDeviceMotionAvailable())
		CMMotionManager.startGyroUpdates()
		while True:
			# EulerAngles is a SCNVector3
			# The order of components in this vector matches the axes of rotation:
			# Pitch (the x component) is the rotation about the node’s x-axis.
			# Yaw (the y component) is the rotation about the node’s y-axis.
			# Roll (the z component) is the rotation about the node’s z-axis.
			#pitch += (random() - 0.5) * delta_ang
			#yaw   += (random() - 0.5) * delta_ang
			#roll  += (random() - 0.5) * delta_ang
			gyro_data = CMMotionManager.gyroData()
			if not gyro_data:
				#print('data not available (yet?)')
				continue
			# Using the custom struct here:
			rate = gyro_data.rotationRate(argtypes=[], restype=CMRotationRate)
			# You can now access the struct's fields as x, y, z:
			roll  = rate.z
			pitch = rate.x
			yaw   = rate.y
			#print(rate.x, rate.y, rate.z)

			# change euler angles but by using animation
			SCNTransaction.begin()
			SCNTransaction.setAnimationDuration(0.3)
			self.geometry_node.setEulerAngles((pitch, yaw, roll))
			SCNTransaction.commit()
			if self.stop:
				break
		CMMotionManager.stopGyroUpdates()
		CMMotionManager.release()

class MyView(ui.View):
 ###@on_main_thread
 def __init__(self,w,h):
  self.width = w
  self.height = h
  self.name = 'SceneKit IMU'
  self.background_color = 'white'
  
  main_view_objc = ObjCInstance(self)
  scene_view = SCNView.alloc().initWithFrame_options_(((0, 0),(self.width,self.height)), None).autorelease()
  scene_view.setAutoresizingMask_(18)
  scene_view.setAllowsCameraControl_(True)
  #scene_view.setDebugOptions_(0xFFFF)
  main_view_objc.addSubview_(scene_view)
  
  scene = SCNScene.scene()
  scene_view.setScene_(scene)
  
  root_node = scene.rootNode()
  
  camera = SCNCamera.camera()
  camera_node = SCNNode.node()
  camera_node.setCamera(camera)
  camera_node.setPosition((-35,35,35))
  root_node.addChildNode_(camera_node) 
  
  # build an image with text to use as material on wing: begin
  l = 40
  r = 2
  h = 3*15	# 3 x height of text
  sc = h / (2*pi*r)
  w = l*sc
  h = 2*pi*r*sc
  with ui.ImageContext(w,h) as ctx:
    path = ui.Path.rect(0,0,w,h)
    ui.set_color('blue')
    path.fill()
    x = w/10
    y = h/3
    t = 'Pytho'
    ui.draw_string(t, rect=(x, y, 8*w/10, 2+h/3), font=('<System-Bold>',15), color='white', alignment=ui.ALIGN_LEFT)
    x = 6*w/10
    t = 'nista'
    ui.draw_string(t, rect=(x, y, 8*w/10, 2+h/3), font=('<System-Bold>',15), color='white', alignment=ui.ALIGN_LEFT)
    ui_image = ctx.get_image()
    #ui_image.show()
  Material_pyth = SCNMaterial.material()
  Material_pyth.contents = ObjCInstance(ui_image)
  # build an image with text to use as material on wing: end
  
  # build an image with text to use as material on cockpit: begin
  l = 40
  r = 2
  h = 3*15	# 3 x height of text
  sc = h / (2*pi*r)
  w = l*sc
  h = 2*pi*r*sc
  with ui.ImageContext(w,h) as ctx:
    path = ui.Path.rect(0,0,w,h)
    ui.set_color('gray')
    path.fill()
    x = w/2 - 15/2 - 2
    y = h/3
    path1 = ui.Path.rect(x, y, 20, 2+h/3)
    ui.set_color('lightgray')
    path1.fill()
    t = '👨‍✈️'
    ui.draw_string(t, rect=(x, y, 20, 2+h/3), font=('<System-Bold>',15), color='white', alignment=ui.ALIGN_LEFT)
    ui_image = ctx.get_image()
    #ui_image.show()
  Material_nose = SCNMaterial.material()
  Material_nose.contents = ObjCInstance(ui_image)
  # build an image with text to use as material on cockpit: end
  
  # build an image with text to use as material on vertical stabilizer: begin
  w = 22
  h = 17
  with ui.ImageContext(w,h) as ctx:
    path = ui.Path.rect(0,0,w,h)
    ui.set_color((0.17, 0.6, 0.0))
    path.fill()
    x = 8
    y = 0
    t = 'V'
    ui.draw_string(t, rect=(x, y, w, h), font=('<System-Bold>',15), color='white', alignment=ui.ALIGN_LEFT)
    ui_image = ctx.get_image()
    #ui_image.show()
  Material_icon1 = SCNMaterial.material()
  Material_icon1.contents = ObjCInstance(ui_image)
  # build an image with text to use as material on vertical stabilizer: end
  
  # build an image with text to use as material on vertical stabilizer: begin
  w = 22
  h = 17
  with ui.ImageContext(w,h) as ctx:
    path = ui.Path.rect(0,0,w,h)
    ui.set_color((0.17, 0.6, 0.0))
    path.fill()
    x = 3
    y = 0
    t = 'Λ'
    ui.draw_string(t, rect=(x, y, w, h), font=('<System-Bold>',15), color='white', alignment=ui.ALIGN_LEFT)
    ui_image = ctx.get_image()
    #ui_image.show()
  Material_icon2 = SCNMaterial.material()
  Material_icon2.contents = ObjCInstance(ui_image)
  # build an image with text to use as material on vertical stabilizer: end
  
  geometry = SCNCylinder.cylinderWithRadius_height_(r,l)
  lc = l/10
  cone = SCNCone.coneWithTopRadius_bottomRadius_height_(r,r/2,lc)
  sphe = SCNSphere.sphereWithRadius_(r/2)
  wing = SCNBox.boxWithWidth_height_length_chamferRadius_(20*r, 2*r, 0.3, 0)
  vert = SCNBox.boxWithWidth_height_length_chamferRadius_(2*r, r, 0.3, 0)
  hori = SCNBox.boxWithWidth_height_length_chamferRadius_(3*r, r*0.6, 0.3, 0)
  mot1 = SCNCylinder.cylinderWithRadius_height_(r*0.3,r*2)
  mot2 = SCNCylinder.cylinderWithRadius_height_(r*0.3,r*2)
   
  cone_node = SCNNode.nodeWithGeometry_(cone)
  sphe_node = SCNNode.nodeWithGeometry_(sphe)
  wing_node = SCNNode.nodeWithGeometry_(wing)
  vert_node = SCNNode.nodeWithGeometry_(vert)
  hori_node = SCNNode.nodeWithGeometry_(hori)
  mot1_node = SCNNode.nodeWithGeometry_(mot1)
  mot2_node = SCNNode.nodeWithGeometry_(mot2)

  tx,ty,tz = (0,l/2+lc/2,0)
  x = (1,0,0,0, 0,1,0,0, 0,0,1,0, tx,ty,tz,1)
  cone_node.setPivot_(x)
  tx,ty,tz = (0,l/2+lc,0)
  x = (1,0,0,0, 0,1,0,0, 0,0,1,0, tx,ty,tz,1)
  sphe_node.setPivot_(x)
  geometry_node = SCNNode.nodeWithGeometry_(geometry)
  geometry_node.addChildNode_(cone_node)
  geometry_node.addChildNode_(sphe_node)
  geometry_node.addChildNode_(wing_node)
  
  # vertical stabilizer: rotation 90° around axe y
  tx,ty,tz = (2*r,-l/2+r/2,0)
  x = (0,0,1,0, 0,1,0,0, -1,0,0,0, tx,ty,tz,1)
  vert_node.setPivot_(x)
  geometry_node.addChildNode_(vert_node)
  
  tx,ty,tz = (0,-l/2+r*0.4,-r*1.5)
  x = (1,0,0,0, 0,1,0,0, 0,0,1,0, tx,ty,tz,1)
  hori_node.setPivot_(x)
  geometry_node.addChildNode_(hori_node)
  
  tx,ty,tz = (5*r,-0.2,0.5)
  x = (1,0,0,0, 0,1,0,0, 0,0,1,0, tx,ty,tz,1)
  mot1_node.setPivot_(x)
  geometry_node.addChildNode_(mot1_node)
  
  tx,ty,tz = (-5*r,-0.2,0.5)
  x = (1,0,0,0, 0,1,0,0, 0,0,1,0, tx,ty,tz,1)
  mot2_node.setPivot_(x)
  geometry_node.addChildNode_(mot2_node)
  
  root_node.addChildNode_(geometry_node)
  
  # cylinder horizontal: rotation 90° around axe x
  x = (1,0,0,0, 0,0,1,0, 0,-1,0,0, 0,0,0,1) 
  geometry_node.setPivot_(x)	
  			
  Materials = []
  colors = ['red','gray','gray','yellow','orange','gray','lightgray']
  for i in range(0,6):
    rgb = getrgb(colors[i])
    r,g,b = tuple(c/255.0 for c in rgb)
    Material = SCNMaterial.material()
    Material.contents = ObjCClass('UIColor').colorWithRed_green_blue_alpha_(r,g,b,1.0)
    Materials.append(Material)
  geometry.setMaterials_(Materials)
  cone.setMaterial_(Material_nose)
  #cone.setMaterials_(Materials[3:5])
  sphe.setMaterials_(Materials[4:5])
  hori.setMaterial_(Materials[1])
  mot1.setMaterials_(Materials)
  mot2.setMaterials_(Materials)
  wing_Materials = [Material_pyth,Materials[1],Material_pyth]+[Materials[1]]*3
  wing.setMaterials_(wing_Materials)
  vert_Materials = [Material_icon2,Materials[1],Material_icon1]+[Materials[1]]*3
  vert.setMaterials_(vert_Materials)

  # Add a constraint to the camera to keep it pointing to the target geometry
  constraint = SCNLookAtConstraint.lookAtConstraintWithTarget_(geometry_node)
  constraint.gimbalLockEnabled = True
  camera_node.constraints = [constraint]
  
  light_node = SCNNode.node()
  light_node.setPosition_((30, 0, -30))
  light = SCNLight.light()
  #light.setType_('spot')
  light.setType_('probe')
  #light.setType_('directional')
  light.setCastsShadow_(True)
  light.setColor_(UIColor.whiteColor().CGColor())
  light_node.setLight_(light)
  root_node.addChildNode_(light_node)
  
  mc = ChatPeer(display_name=platform.node(), service_type='chat')
  mc.geometry_node = geometry_node
  #thread_bt = my_thread_bt(geometry_node)
  #thread_bt.start()
  
 def will_close(self):
 	for t in threading.enumerate():
 		if t.name == 'bt':
 			t.stop = True
 			return

def main():	
	w, h = ui.get_screen_size()
	MainView = MyView(w, h)
	MainView.present('fullscreen', hide_title_bar=False)
	
# Protect against import	
if __name__ == '__main__':
  main()