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