Skip to content

Instantly share code, notes, and snippets.

@cvpe cvpe/imu.py
Created Jan 25, 2020

Embed
What would you like to do?
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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.