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