1import logging; logger = logging.getLogger("morse." + __name__) 2import morse.core.actuator 3from morse.core.services import service, async_service, interruptible 4from morse.core import status 5from morse.helpers.components import add_data, add_property 6from morse.core import mathutils 7import math 8 9class Arucomarker(morse.core.actuator.Actuator): 10 """ 11 The ArUco marker is an AR-Marker that allows to compute the camera 12 pose from images in the 'real world'. 13 14 See: http://www.uco.es/investiga/grupos/ava/node/26 15 16 The purpose of this actuator is to provide a virtual instance of 17 such a marker in MORSE. By adding an ArUco marker to a MORSE 18 simulation you can subsequently stream/export a (virtual) camera 19 image and eventually use an AR Marker without a physical camera 20 setup or, i.e, test algorithms or simulate visual servoring. 21 22 .. example:: 23 24 from morse.builder import * 25 26 ### Add a virtual ArUco marker to the scene 27 robot = Morsy() 28 29 aruco = Arucomarker() 30 aruco.add_stream('ros', topic="/aruco_pose") 31 aruco.properties(zoffset=0.3, xoffset=-0.09, yoffset=.0) 32 33 robot.append(aruco) 34 35 env = Environment('empty') 36 37 38 :noautoexample: 39 """ 40 41 _name = "ArUco Marker" 42 _short_desc = "A virtual representation of an ArUco Marker" 43 44 add_data('x', 0.0, 'float', 'X axis translation metres') 45 add_data('y', 0.0, 'float', 'Y axis translation metres') 46 add_data('z', 0.0, 'float', 'Z axis translation metres') 47 add_data('roll', 0.0, 'float', 'X axis rotation in rad') 48 add_data('pitch', 0.0, 'float', 'Y axis rotation in rad') 49 add_data('yaw', 0.0, 'float', 'Z axis rotation in rad') 50 51 """ 52 Initialises translation properties, they can be accessed via builder script 53 These properties add a static offset to the marker. You may want to use this 54 if you plan on aligning the marker to a virtual camera, which would allow you 55 to test your tracking algorithms based on the image of a virtual camera for 56 instance 57 """ 58 add_property('_xoffset', 0.0, 'xoffset', 'float', "X axis translation offset in metres") 59 add_property('_yoffset', 0.0, 'yoffset', 'float', "Y axis translation offset in metres") 60 add_property('_zoffset', 0.0, 'zoffset', 'float', "Z axis translation offset in metres") 61 62 def __init__(self, obj, parent=None): 63 logger.info("%s initialization" % obj.name) 64 morse.core.actuator.Actuator.__init__(self, obj, parent) 65 66 """ 67 Save the ArUco object and its parent to compute the relative 68 position later on in default_action() 69 """ 70 self.aruco = {} 71 self.aruco['aruco'] = self.bge_object 72 self.aruco['parent'] = self.robot_parent 73 74 """ 75 Spawn the marker as specified in the builder script via translate 76 The rotation is zeroed initially 77 """ 78 self.local_data['x'] = self.aruco['aruco'].worldPosition[0] 79 self.local_data['y'] = self.aruco['aruco'].worldPosition[1] 80 self.local_data['z'] = self.aruco['aruco'].worldPosition[2] 81 self.local_data['roll'] = 0.0 82 self.local_data['pitch'] = 0.0 83 self.local_data['yaw'] = 0.0 84 85 logger.info('Component initialized') 86 87 @service 88 def get_local_position(self): 89 return self.aruco['aruco'].localPosition 90 91 @service 92 def get_world_position(self): 93 return self.aruco['aruco'].worldPosition 94 95 @service 96 def get_local_orientation(self): 97 return self.aruco['aruco'].localOrientation 98 99 @service 100 def get_world_orientation(self): 101 return self.aruco['aruco'].worldOrientation 102 103 """ 104 Apply roation and translation, function 105 lifted from teleport actuator class + 106 applying parents position in case it moves 107 """ 108 def force_pose(self, position, orientation): 109 me = self.aruco['aruco'] 110 parent = self.aruco['parent'] 111 pose3d = parent.position_3d 112 parent_pose = mathutils.Vector((pose3d.x, pose3d.y, pose3d.z)) 113 if position: 114 me.worldPosition = position + parent_pose 115 if orientation: 116 me.worldOrientation = orientation 117 118 """ 119 The default action which is executed every LOGIC TICK 120 Compute current location, i.e., provided via middleware (in metres) 121 NOTE: By default, the ArUco translation (AT), as defined in the 122 original ArUco library, corresponds to the MORSE translation 123 (MT) as follows: 124 125 MTx = ATz 126 MTy = ATx 127 MTz = -ATy 128 129 See example below 130 """ 131 def default_action(self): 132 position = mathutils.Vector( (self.local_data['z']+self._xoffset, 133 self.local_data['x']+self._yoffset, 134 (-1.0*self.local_data['y']+self._zoffset)) ) 135 136 orientation = mathutils.Euler([ self.local_data['roll' ], 137 self.local_data['pitch'], 138 self.local_data['yaw' ] ]) 139 140 """ Convert Euler to Matrix, worldOrientation accepts Quat, Mat """ 141 orientation.order = "YZX" 142 orientation_mat = orientation.to_matrix() 143 self.force_pose(position, orientation_mat) 144