Source code for morse.middleware.pocolibs.sensors.viman
import logging; logger = logging.getLogger("morse." + __name__)
import time
import re
from morse.core import blenderapi
from morse.helpers.transformation import Transformation3d
from morse.helpers import passive_objects
from morse.middleware.pocolibs_datastream import *
from viman.struct import *
object_config_file = "objectList_cfg"
def _fill_world_matrix(obj, t3d):
""" Fill the world matix part of the structure
This function receives the Blender rotation matrix and position of an object
It calls a module function to fill out data structure of type VimanThetaMat
"""
obj.thetaMatOrigin.nx = t3d.matrix[0][0]
obj.thetaMatOrigin.ox = t3d.matrix[1][0]
obj.thetaMatOrigin.ax = t3d.matrix[2][0]
obj.thetaMatOrigin.px = t3d.x
obj.thetaMatOrigin.ny = t3d.matrix[0][1]
obj.thetaMatOrigin.oy = t3d.matrix[1][1]
obj.thetaMatOrigin.ay = t3d.matrix[2][1]
obj.thetaMatOrigin.py = t3d.y
obj.thetaMatOrigin.nz = t3d.matrix[0][2]
obj.thetaMatOrigin.oz = t3d.matrix[1][2]
obj.thetaMatOrigin.az = t3d.matrix[2][2]
obj.thetaMatOrigin.pz = t3d.z
def _fill_robot_matrix(obj, robot, obj_3dpose):
""" Fill the world matix part of the structure
This function receives the Blender rotation matrix and position of an object
It calls a module function to fill out data structure of type VimanThetaMat
"""
robot3d = robot.position_3d
t3d = robot3d.transformation3d_with(obj_3dpose)
obj.thetaMatRobot.nx = t3d.matrix[0][0]
obj.thetaMatRobot.ox = t3d.matrix[1][0]
obj.thetaMatRobot.ax = t3d.matrix[2][0]
obj.thetaMatRobot.px = t3d.x
obj.thetaMatRobot.ny = t3d.matrix[0][1]
obj.thetaMatRobot.oy = t3d.matrix[1][1]
obj.thetaMatRobot.ay = t3d.matrix[2][1]
obj.thetaMatRobot.py = t3d.y
obj.thetaMatRobot.nz = t3d.matrix[0][2]
obj.thetaMatRobot.oz = t3d.matrix[1][2]
obj.thetaMatRobot.az = t3d.matrix[2][2]
obj.thetaMatRobot.pz = t3d.z
[docs]class VimanPoster(PocolibsDataStreamOutput):
_type_name = "VimanObjectPublicArray"
_type_url = "http://trac.laas.fr/git/viman-genom/tree/vimanStruct.h#n139"
[docs] def initialize(self):
PocolibsDataStreamOutput.initialize(self, VimanObjectPublicArray)
# Initialise the object
self.obj = VimanObjectPublicArray()
self.scene_object_list = []
#If provided, read the list of ARToolkit tags to fill the list of objects
#to export.
self.scene_object_list += self._read_object_list()
#Complete the list with the objects already tracked by the semantic cam.
if 'passiveObjectsDict' in blenderapi.persistantstorage():
self.scene_object_list += [obj['label'] for obj in blenderapi.persistantstorage().passiveObjectsDict.values()]
if not self.scene_object_list:
logger.error("No VIMAN object to track. Make sure some objects have " +\
"the game property 'Object' defined. Disabling poster for now.")
return None
# Init the data structures used by this poster
self.obj.nbObjects = len(self.scene_object_list)
i = 0
for object in self.scene_object_list:
logger.info("Adding " + object + " to the objects tracked by VIMAN")
self.obj.objects[i].name = bytes(str(object), 'utf-8')
i += 1
def _read_object_list(self):
""" Open the file specified in the object_config_file variable
Read the list of component names"""
scene_object_list = []
logger.info("Reading '%s' config file for VIMAN poster" % object_config_file)
try:
fp = open(object_config_file, "r")
for line in fp:
match = re.search('object (\w+)', line)
if match is not None:
scene_object_list.append(match.group(1))
logger.debug("\t- %s" % match.group(1))
fp.close()
except IOError as detail:
logger.debug(detail)
logger.info("ARToolkit tag library not found. Skipping it.")
return []
return scene_object_list
[docs] def default(self, ci):
seen_objects = [obj['name'] for obj in self.data['visible_objects']]
parent = self.component_instance.robot_parent
i = 0
for object_id in self.scene_object_list:
try:
t = time.time()
tacq_sec = int(t)
tacq_usec = int((t - tacq_sec) * 1000)
self.obj.objects[i].tacq_sec = tacq_sec
self.obj.objects[i].tacq_usec = tacq_usec
if object_id in seen_objects:
obj = passive_objects.obj_from_label(object_id)
position_3d = Transformation3d(obj)
logger.debug("VIMAN " + object_id + "(" + obj.name +
") is visible at " + str(position_3d))
self.obj.objects[i].found_Stereo = 1
_fill_world_matrix(self.obj.objects[i], position_3d)
_fill_robot_matrix(self.obj.objects[i], parent, position_3d)
else:
self.obj.objects[i].found_Stereo = 0
except KeyError as detail:
logger.debug("WARNING: Object %s not found in the scene" % detail)
i += 1
self.write(self.obj)