def test_double_loading(self):
world = self.ctx.worlds["test"]
ModelLoader().load(path.join("res", "tree.blend"), world="test")
ModelLoader().load(path.join("res", "tree.blend"), world="test")
time.sleep(PROPAGATION_TIME)
self.assertEquals(len(world.scene.nodes), 3) # one root and 2 trees
trees = world.scene.nodebyname("tree")
self.assertEquals(len(trees), 2) # should have 2 trees
self.assertEquals(trees[0].hires, trees[1].hires)
self.assertNotEquals(trees[0].id, trees[1].id)
def test_simple_descriptions(self):
worldName = "base"
world = self.ctx.worlds[worldName]
ModelLoader().load("res/spatialDescSimple.blend", world=worldName)
time.sleep(1) # leave some time for the loader to finish
inNode = world.scene.nodebyname("Inside")[0]
description = gen_spatial_desc(worldName, inNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "Inside is in Main")
onNode = world.scene.nodebyname("OnTop")[0]
description = gen_spatial_desc(worldName, onNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "OnTop is on top of Main")
mainNode = world.scene.nodebyname("Main")[0]
description = gen_spatial_desc(worldName, mainNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "Main is above Below")
belowNode = world.scene.nodebyname("Below")[0]
description = gen_spatial_desc(worldName, belowNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "Below is below Main" or description == "Below is below Inside" or description == "Below is below OnTop")
#Technically correct but unlikely and even confusing that someone would say the second two. Need to think about how this would prioritise for complex description.
southNode = world.scene.nodebyname("object")[0]
description = gen_spatial_desc(worldName, southNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "the object is to the south of Main" or description == "the object is to the south of Inside")
northNode = world.scene.nodebyname("ToNorth")[0]
northNode.name = "object"
world.scene.nodes.update(northNode)
description = gen_spatial_desc(worldName, northNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "an object is to the north of Main" or description == "an object is to the north of Inside")
eastNode = world.scene.nodebyname("cube")[0]
description = gen_spatial_desc(worldName, eastNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "the cube is to the east of Main" or description == "the cube is to the east of Inside")
westNode = world.scene.nodebyname("ToWest")[0]
westNode.name = "cube"
world.scene.nodes.update(westNode)
description = gen_spatial_desc(worldName, westNode.id, "en_GB", "Simple")
print(description)
self.assertTrue(description == "a cube is to the west of Main" or description == "a cube is to the west of Inside")
def load_mesh(world, model):
""" Loads meshes from a file and adds it to underworlds. Note
that the meshes loaded are stand alone and not attached to
a node.
"""
with underworlds.Context("edit-tool") as ctx:
target_world = ctx.worlds[world]
mesh_ids = []
meshes = ModelLoader().load_meshes(model, ctx=ctx)
for m in meshes.values():
mesh_ids = mesh_ids + m
logger.info("Loaded mesh %s with ids: %s" % (str(model), str(mesh_ids)))
return mesh_ids
def test_frames(self):
world = self.ctx.worlds["base"]
ModelLoader(world.name).load("testing/res/monkey_mat.dae")
time.sleep(PROPAGATION_TIME) # wait for propagation
objects = ['Scene', 'Camera', 'Lamp', 'Monkey_002', 'Monkey_001', 'Monkey']
self.assertItemsEqual(objects, [n.name for n in world.scene.nodes])
def test_anchoring(self):
world = self.ctx.worlds["test"]
nodes = ModelLoader().load(path.join("res","tree.blend"), world="test")
tree = world.scene.nodebyname("tree")[0]
self.assertEqual(tree.transformation[0,3], 0)
tree.transformation = compose_matrix(None, None, None, [2, 0, 0], None)
world.scene.nodes.update(tree)
time.sleep(PROPAGATION_TIME)
self.assertEqual(world.scene.nodes[tree.id].transformation[0,3], 2)
# ...loading another model reset the transformation of our original
# model
nodes = ModelLoader().load(path.join("res","cow.blend"), world="test")
time.sleep(PROPAGATION_TIME)
self.assertEqual(world.scene.nodes[tree.id].transformation[0,3], 2)
def _test_modelloading(self):
event = Event(Event.MODELLOAD)
with self.assertRaises(TimeoutError):
self.timeline.on(event).wait(timeout=0.1)
ModelLoader().load("res/base.dae", world=self.world)
self.timeline.on(event).wait(
timeout=0.1) # should not throw a timeouterror
ok = False
def onload(self, evt):
ok = True
self.timeline.on(event).call(onload)
ModelLoader(self.world).load("res/base.dae")
time.sleep(PROPAGATION_TIME)
self.assertTrue(ok)
def test_serialization(self):
world = self.ctx.worlds["base"]
ModelLoader(world.name).load("testing/res/base_visibility.dae")
time.sleep(PROPAGATION_TIME) # wait for propagation
for n in world.scene.nodes:
if n.type == MESH:
mesh = self.ctx.mesh(n.cad[0])
# all the meshes in this scene are cubes, ie, 6 faces, ie 12 triangles
self.assertEqual(len(mesh.faces), 12)
def test_basic_loading(self):
world = self.ctx.worlds["test"]
nodes = ModelLoader().load(path.join("res","tree.blend"), world="test")
time.sleep(PROPAGATION_TIME)
self.assertEqual(len(nodes), 2) # <BlenderRoot> and <tree>
self.assertEqual(len(world.scene.nodes), 2)
trees = world.scene.nodebyname("tree")
self.assertEqual(len(trees), 1) # only one tree
self.assertEqual(trees[0].type, MESH)
def load_urdf(self):
"""
This function read the URDF file given in constructor and save the robot structure
@return : None
"""
urdf_tree = etree.parse(self.urdf_file_path)
urdf_root = urdf_tree.getroot()
for link in urdf_root.iter("link"):
if link.find("visual") is not None:
if link.find("visual").find("geometry").find("mesh") is not None:
path = link.find("visual").find("geometry").find("mesh").get("filename").split("/")
if link.find("visual").find("geometry").find("mesh").get("scale"):
scale_str = link.find("visual").find("geometry").find("mesh").get("scale").split(" ")
scale = float(scale_str[0]) * float(scale_str[1]) * float(scale_str[2])
else:
scale = 0.1
count = 0
path_str = ""
element = path[len(path)-1]
while count < len(path):
if element == "meshes":
break
else:
path_str = "/" + element + path_str
count += 1
element = path[len(path)-1-count]
filename = self.model_dir_path + path_str
try:
nodes_loaded = ModelLoader().load(filename, self.ctx, world=self.target_world_name, root=None,
only_meshes=True, scale=scale)
for n in nodes_loaded:
if n.type == MESH:
self.model_map[link.get("name")] = n.properties["mesh_ids"]
self.aabb_map[link.get("name")] = n.properties["aabb"]
except Exception as e:
pass
else:
if link.find("visual").find("geometry").find("box") is not None:
mesh_ids = []
sizes = link.find("visual").find("geometry").find("box").get("size").split(" ")
box = Box.create(float(sizes[0]), float(sizes[1]), float(sizes[2]))
self.ctx.push_mesh(box)
mesh_ids.append([box.id])
self.model_map[link.get("name")] = mesh_ids
def test_complex_loading(self):
world = self.ctx.worlds["test"]
nodes = ModelLoader().load(path.join("res","visibility.blend"), world="test")
time.sleep(PROPAGATION_TIME)
self.assertEqual(len(nodes), 8)
self.assertEqual(len(world.scene.nodes), 8)
self.assertEqual(len(world.scene.nodebyname("Camera1")), 1)
cam1 = world.scene.nodebyname("Camera1")[0]
self.assertEqual(cam1.type, CAMERA)
self.assertFalse("mesh_ids" in cam1.properties)
self.assertEqual(len(world.scene.nodebyname("Cube1")), 1)
cube1 = world.scene.nodebyname("Cube1")[0]
self.assertEqual(cube1.type, MESH)
self.assertTrue("mesh_ids" in cube1.properties)
def test_visibility(self):
world = self.ctx.worlds["base"]
ModelLoader().load("res/visibility.blend", world="base")
cube1 = world.scene.nodebyname("Cube1")[0]
cube2 = world.scene.nodebyname("Cube2")[0]
visibility = VisibilityMonitor(self.ctx, world)
results = visibility.compute_all()
self.assertItemsEqual(["Camera1", "Camera2", "Camera3", "Camera4", "Camera5"], \
results.keys())
self.assertItemsEqual([cube1, cube2], results["Camera1"])
self.assertItemsEqual([cube1, cube2], results["Camera2"])
self.assertItemsEqual([cube2], results["Camera3"])
self.assertItemsEqual([cube1], results["Camera4"])
self.assertListEqual([], results["Camera5"])
def load(objName):
#Get Underworlds path and put together path for blender file.
#Using simplified models to speed up loading for Underworlds and increase stability
if objName == "giraffe":
objName = "giraffe2"
if objName == "Monkey":
objName = "Monkey2"
filename = "res\%s.blend" % (objName)
loadLogger.setLevel(logging.INFO)
logging.basicConfig(level=logging.INFO)
#Load model
nodes = ModelLoader().load(filename, world="test")
ids = [node.id for node in nodes if node.name == objName]
if ids:
myID = ids[0]
msg = "test_move load: ID = %s" % (myID)
logging.info(msg)
return myID
logging.info("test_move load: Could not get ID")
def create_object(self, frame):
if frame in self.ar_objects:
node = Mesh(name=self.ar_objects[frame]["name"])
try:
nodes_loaded = ModelLoader().load(
self.mesh_dir + self.ar_objects[frame]["mesh"],
self.ctx,
world=self.output_world_name,
root=None,
only_meshes=True,
scale=self.ar_objects[frame]["scale"])
for n in nodes_loaded:
if n.type == MESH:
node.properties["mesh_ids"] = n.properties["mesh_ids"]
node.properties["aabb"] = n.properties["aabb"]
return node
except Exception as e:
rospy.logwarn(
"[env_provider] Exception occurred with %s : %s" %
(self.ar_objects[frame]["name"], str(e)))
return None
def test_spatial_relations(self):
world = self.ctx.worlds["base"]
ModelLoader().load("res/spatial.blend", world="base")
mainbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("Main")[0])
belowbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("Below")[0])
insidebb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("Inside")[0])
onTopbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("OnTop")[0])
eastbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("ToEast")[0])
northbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("ToNorth")[0])
southbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("ToSouth")[0])
westbb = get_bounding_box_for_node(world.scene, world.scene.nodebyname("ToWest")[0])
self.assertTrue(isbelow(belowbb,mainbb))
self.assertTrue(isin(insidebb,mainbb))
self.assertTrue(isontop(onTopbb,mainbb))
self.assertTrue(istoeast(eastbb,mainbb))
self.assertTrue(istonorth(northbb,mainbb))
self.assertTrue(istosouth(southbb,mainbb))
self.assertTrue(istowest(westbb,mainbb))
def test_non_ambig_descriptions(self):
worldName = "base"
world = self.ctx.worlds[worldName]
ModelLoader().load("../map/map_wEmpty.blend", world=worldName)
time.sleep(2) # leave some time for the loader to finish
target_node = world.scene.nodebyname("empty_space-2")[0]
target_node2 = world.scene.nodebyname("police_department")[0]
for node in world.scene.nodes:
format_name(worldName, node.id)
time.sleep(2)
description = gen_spatial_desc(self.ctx, worldName, target_node.id,
"default", [], "en_GB", "NonAmbig")
print description
description = gen_spatial_desc(self.ctx, worldName, target_node2.id,
"default", [], "en_GB", "NonAmbig")
print description
def __init__(self, ctx, output_world, mesh_dir, reference_frame):
self.ros_subscriber = {"gaze": rospy.Subscriber("/wp2/gaze", GazeInfoArray, self.callbackGaze)}
self.human_cameras_ids = {}
self.ctx = ctx
self.human_bodies = {}
self.target = ctx.worlds[output_world]
self.target_world_name = output_world
self.reference_frame = reference_frame
self.mesh_dir = mesh_dir
self.human_meshes = {}
self.human_aabb = {}
self.nb_gaze_detected = {}
self.added_human_id = []
self.detection_time = None
self.reco_durations = []
self.record_time = False
self.robot_name = rospy.get_param("robot_name", "pepper")
self.already_removed_nodes = []
nodes_loaded = []
try:
nodes_loaded = ModelLoader().load(self.mesh_dir + "face.blend", self.ctx,
world=output_world, root=None, only_meshes=True,
scale=1.0)
except Exception as e:
rospy.logwarn("[multimodal_human_provider] Exception occurred with %s : %s" % (self.mesh_dir + "face.blend", str(e)))
for n in nodes_loaded:
if n.type == MESH:
self.human_meshes["face"] = n.properties["mesh_ids"]
self.human_aabb["face"] = n.properties["aabb"]
self.tfBuffer = tf2_ros.Buffer(rospy.Duration(TF_CACHE_TIME), debug=False)
self.listener = tf2_ros.TransformListener(self.tfBuffer)
translation_cam = transformations.translation_matrix((1, 0, 0.5))
# According to http://www.openni.ru/wp-content/uploads/2013/02/NITE-Algorithms.pdf
# the sensor is oriented as follow:
# " +X points to the right of the, +Y points up, and +Z
# points in the direction of increasing depth."
rotation_cam = transformations.euler_matrix(math.pi / 2, 0,
math.pi / 2)
camera.transformation = numpy.dot(translation_cam, rotation_cam)
camera.parent = world.scene.rootnode.id
nodes.append(camera)
# Load the mannequin mesh into underworlds and get back the list of
# underworlds nodes
bodypartslist = ModelLoader(args.world).load(
"../share/mannequin.blend", ctx=ctx, root=camera)
human_nodes = {node.name: node for node in bodypartslist}
try:
while True:
frame = userTracker.read_frame()
#logger.info("%s humans detected" % len(frame.users))
humanparts = process_frame(frame)
if humanparts is not None:
for name, transformation in humanparts.items():
node = human_nodes[name]
def test_simple_descriptions(self):
worldName = "base"
world = self.ctx.worlds[worldName]
ModelLoader().load("res/spatialDescSimple.blend", world=worldName)
time.sleep(1) # leave some time for the loader to finish
inNode = world.scene.nodebyname("Inside")[0]
description = gen_spatial_desc(self.ctx, worldName, inNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "Inside is in Main")
onNode = world.scene.nodebyname("OnTop")[0]
description = gen_spatial_desc(self.ctx, worldName, onNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "OnTop is on top of Main")
mainNode = world.scene.nodebyname("Main")[0]
description = gen_spatial_desc(self.ctx, worldName, mainNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "Main is above Below")
belowNode = world.scene.nodebyname("Below")[0]
description = gen_spatial_desc(self.ctx, worldName, belowNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "Below is below Main")
southNode = world.scene.nodebyname("object")[0]
description = gen_spatial_desc(self.ctx, worldName, southNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "the object is to the south of Main")
description = gen_spatial_desc(self.ctx, worldName, southNode.id,
"default", [], "en_GB", "Simple")
print(description)
self.assertTrue(description == "the object is to the front of Main")
northNode = world.scene.nodebyname("ToNorth")[0]
northNode.name = "object"
world.scene.nodes.update(northNode)
description = gen_spatial_desc(self.ctx, worldName, northNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "an object is to the north of Main")
description = gen_spatial_desc(self.ctx, worldName, northNode.id,
"default", [], "en_GB", "Simple")
print(description)
self.assertTrue(description == "an object is to the back of Main")
eastNode = world.scene.nodebyname("cube")[0]
description = gen_spatial_desc(self.ctx, worldName, eastNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "the cube is to the east of Main")
description = gen_spatial_desc(self.ctx, worldName, eastNode.id,
"default", [], "en_GB", "Simple")
print(description)
self.assertTrue(description == "the cube is to the right of Main")
westNode = world.scene.nodebyname("ToWest")[0]
westNode.name = "cube"
world.scene.nodes.update(westNode)
description = gen_spatial_desc(self.ctx, worldName, westNode.id, None,
[], "en_GB", "Simple")
print(description)
self.assertTrue(description == "a cube is to the west of Main")
description = gen_spatial_desc(self.ctx, worldName, westNode.id,
"default", [], "en_GB", "Simple")
print(description)
self.assertTrue(description == "a cube is to the left of Main")
def __init__(self, ctx, output_world, mesh_dir, reference_frame):
self.human_cameras_ids = {}
self.ctx = ctx
self.human_bodies = {}
self.target = ctx.worlds[output_world]
self.target_world_name = output_world
self.reference_frame = reference_frame
self.mesh_dir = mesh_dir
self.human_meshes = {}
self.human_aabb = {}
self.nb_gaze_detected = {}
self.human_perception_to_uwds_ids = {}
self.detection_time = None
self.reco_durations = []
self.record_time = False
self.robot_name = rospy.get_param("robot_name", "pepper")
self.is_robot_moving = rospy.get_param("is_robot_moving", False)
self.ros_pub = {"voice": rospy.Publisher("multimodal_human_monitor/voice_attention_point", PointStamped, queue_size=5),
"gaze": rospy.Publisher("multimodal_human_monitor/gaze_attention_point", PointStamped, queue_size=5),
"reactive": rospy.Publisher("multimodal_human_monitor/monitoring_attention_point", PointStamped, queue_size=5),
"monitoring_attention_point": rospy.Publisher("head_manager/head_monitoring_target", TargetWithPriority, queue_size=5),
"tf": rospy.Publisher("/tf", TFMessage, queue_size=10)}
self.log_pub = {"isLookingAt": rospy.Publisher("predicates_log/lookingat", String, queue_size=5),
"isPerceiving": rospy.Publisher("predicates_log/perceiving", String, queue_size=5),
"isSpeaking": rospy.Publisher("predicates_log/speak", String, queue_size=5),
"isSpeakingTo": rospy.Publisher("predicates_log/speakingto", String, queue_size=5),
"isNear": rospy.Publisher("predicates_log/near", String, queue_size=5),
"isClose": rospy.Publisher("predicates_log/close", String, queue_size=5),
"isMonitoring": rospy.Publisher("predicates_log/monitoring", String, queue_size=5)}
self.ros_sub = {"gaze": message_filters.Subscriber("wp2/gaze", GazeInfoArray),
"voice": message_filters.Subscriber("wp2/voice", VoiceActivityArray),
"person": message_filters.Subscriber("wp2/track", TrackedPersonArray)}
self.ros_services = {"monitor_humans": rospy.Service("multimodal_human_monitor/monitor_humans", MonitorHumans, self.handle_monitor_humans),
"find_alternate_id": rospy.Service("multimodal_human_monitor/find_alternate_id", FindAlternateId, self.handle_find_alternate_id),
"global_monitoring": rospy.Service("multimodal_human_monitor/global_monitoring", SetBool, self.handle_global_monitoring)}
self.ts = message_filters.TimeSynchronizer([self.ros_sub["gaze"], self.ros_sub["voice"], self.ros_sub["person"]], 50)
self.ts.registerCallback(self.callback)
self.head_signal_dq = deque()
self.already_removed_nodes = []
self.current_situations_map = {}
self.humans_to_monitor = []
self.reco_id_table = {}
self.inv_reco_id_table = {}
self.human_perceived = []
self.previous_human_perceived = []
self.human_speaking = []
self.previous_human_speaking = []
self.human_speakingto = {}
self.previous_human_speakingto = {}
self.human_lookat = {}
self.previous_human_lookat = {}
self.human_distances = {}
self.human_close = []
self.previous_human_close = []
self.human_near = []
self.previous_human_near = []
self.is_active = True
self.tf_buffer = tf2_ros.Buffer(rospy.Duration(TF_CACHE_TIME), debug=False)
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
try:
nodes_loaded = ModelLoader().load(self.mesh_dir + "face.blend", self.ctx,
world=output_world, root=None, only_meshes=True,
scale=1.0)
for n in nodes_loaded:
if n.type == MESH:
self.human_meshes["face"] = n.properties["mesh_ids"]
self.human_aabb["face"] = n.properties["aabb"]
except Exception as e:
rospy.logwarn("[multimodal_human_provider] Exception occurred with %s : %s" % (self.mesh_dir + "face.blend", str(e)))