def setUp(self):
self.server = underworlds.server.start()
time.sleep(0.1) # leave some time to the server to start
self.ctx = underworlds.Context("unittest - nodes")
self.ctx2 = underworlds.Context("unittest - nodes 2")
self.previous_object_near = self.object_near
self.previous_object_in = self.object_in
def runOnce(self):
"""
"""
self.compute_relations(self.source.scene)
def run(self):
rate = rospy.Rate(30)
while not rospy.is_shutdown():
self.runOnce()
rate.sleep()
if __name__ == "__main__":
sys.argv = [
arg for arg in sys.argv if "__name" not in arg and "__log" not in arg
]
sys.argc = len(sys.argv)
import argparse
parser = argparse.ArgumentParser(
description="Monitor allocentric spatial relations")
parser.add_argument("source_world", help="Underworlds world to monitor")
args = parser.parse_args()
rospy.init_node("allocentric_relations_manager", anonymous=True)
with underworlds.Context("Allocentric Relations Manager") as ctx:
AllocentricMonitor(ctx, args.source_world).run()
def setUp(self):
self.server = underworlds.server.start()
time.sleep(0.1) # leave some time to the server to start
self.ctx = underworlds.Context("unittest - visibility")
while not rospy.is_shutdown():
self.filter()
self.monitor_robot()
if __name__ == "__main__":
sys.argv = [arg for arg in sys.argv if "__name" not in arg and "__log" not in arg]
sys.argc = len(sys.argv)
parser = argparse.ArgumentParser(description="Add in the given output world, the nodes from input "
"world and the robot agent from ROS")
parser.add_argument("input_world", help="Underworlds input world")
parser.add_argument("output_world", help="Underworlds output world")
parser.add_argument("urdf_file_path", help="The path of the urdf file")
parser.add_argument("model_dir_path", help="The path of the robot mesh directory")
parser.add_argument("robot_name", help="The robot name")
parser.add_argument("perspective_frame", help="The name of the robot head gaze frame")
parser.add_argument("--cam_rot", default="0.0_0.0_0.0",
help="The camera rotation offset :\"<rx>_<ry>_<rz>\" in [°] ")
parser.add_argument("--reference", default="map", help="The reference frame")
args = parser.parse_args()
rospy.init_node("robot_filter", anonymous=False)
with underworlds.Context("Robot filter") as ctx:
rx, rz, ry = [math.radians(float(i)) for i in args.cam_rot.split("_")]
rot = transformations.euler_matrix(rx, rz, ry, 'rxyz')
RobotMonitor(ctx, args.input_world, args.output_world, args.urdf_file_path, args.model_dir_path,
args.robot_name, args.perspective_frame, rot, args.reference).run()
import underworlds
with underworlds.Context("tablet_interface") as ctx:
world = ctx.worlds["base"]
# create a new node
childnode = underwords.Node("child", type=underwords.types.CAMERA)
# parent it to the rootnode (or any other node if you prefer)
childnode.parent = world.scene.rootnode
# set its pose
childnode.transformation([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z],
[0, 0, 0, 1]])
# if you do not provide any mesh, you might want to provide manually the boundingbox
childnode.aabb([[x1, y1, z1], [x2, y2, z2]])
# then, propagate the change:
world.scene.nodes.update(childnode)
#### Accessing existing nodes
node = None
nodes = world.scene.nodebyname("name")
if nodes:
node = nodes[0]
node.transformation[0, 3] = x
node.transformation[1, 3] = y
node.transformation[2, 3] = z
# or...
#! /usr/bin/env python
import time
import logging
logger = logging.getLogger("underworlds.testing")
logging.basicConfig(level=logging.DEBUG)
import underworlds
from underworlds.types import Node
# Add a PROVIDER client
with underworlds.Context("provider") as provider_ctx:
world = provider_ctx.worlds["base"]
world.scene.nodes.update(Node()) # create and add a random node
# Add a READER client
with underworlds.Context("reader") as reader_ctx:
world = reader_ctx.worlds["base"]
world2 = reader_ctx.worlds["brave new world"]
for n in world.scene.nodes:
world2.scene.nodes.update(n)
def setUp(self):
self.server = underworlds.server.start()
self.ctx = underworlds.Context("unittest - parenting")
def load(self,
filename,
ctx=None,
world=DEFAULT_WORLD,
root=None,
only_meshes=False):
"""Loads a Collada (or any Assimp compatible model) file in the world.
The kinematic chains are added to the world's geometric state.
The meshes are added to the meshes repository.
A new 'load' event is also added in the world timeline.
:param string path: the path (relative or absolute) to the Collada resource
:param Context ctx: an existing underworlds context. If not provided, a
new one is created (named 'model loader')
:param string world: the target world for the creation of nodes
:param Node root: if given, the loaded nodes will be parented to this
node instead of the scene's root.
:param bool only_meshes: if true, no node is created. Only the
meshes are pushed to the server.
:returns: the list of loaded underworlds nodes.
"""
self.meshes = {}
self.node_map = {}
if not only_meshes and world is None:
raise RuntimeError("Can not create nodes if the world is None")
close_ctx_at_end = False
# Create a context if needed:
if ctx is None:
close_ctx_at_end = True
ctx = underworlds.Context("model loader")
logger.info("Loading model <%s> with ASSIMP..." % filename)
model = pyassimp.load(filename,
aiProcessPreset_TargetRealtime_MaxQuality)
logger.info("...done")
if not only_meshes:
if not root:
logger.info("Merging the root nodes")
self.node_map[model.rootnode.name] = (
model.rootnode, ctx.worlds[world].scene.rootnode)
else:
self.node_map[model.rootnode.name] = (model.rootnode, Node())
logger.info("Nodes found:")
self.recur_node(model.rootnode, model)
logger.info("%d nodes in the model" % len(self.node_map))
logger.info("Loading the nodes...")
for n, pair in list(self.node_map.items()):
self.fill_node_details(*pair, assimp_model=model, custom_root=root)
logger.info("...done")
# Send first the meshes to make sure they are available on the server
# when needed by clients. (but only if they do not already exist on
# the server)
logger.info("Sending meshes to the server...")
count_sent = 0
count_notsent = 0
for id, mesh in list(self.meshes.items()):
if ctx.has_mesh(id):
logger.debug(
"Not resending mesh <%s>: already available on the server"
% id)
count_notsent += 1
else:
logger.debug("Sending mesh <%s>" % id)
ctx.push_mesh(mesh)
count_sent += 1
logger.info(
"Sent %d meshes (%d were already available on the server)" %
(count_sent, count_notsent))
if not only_meshes:
nodes = ctx.worlds[world].scene.nodes
# Send the nodes to the server (only the nodes)
logger.info("Sending the nodes to the server...")
for name, pair in list(self.node_map.items()):
nodes.update(pair[1])
pyassimp.release(model)
if close_ctx_at_end:
ctx.close()
return [nodes[1] for _, nodes in self.node_map.items()]
self.output.scene.nodes.update(nodes_to_update)
def run(self):
while not rospy.is_shutdown():
self.update_objects()
if __name__ == '__main__':
sys.argv = [
arg for arg in sys.argv if "__name" not in arg and "__log" not in arg
]
sys.argc = len(sys.argv)
import argparse
parser = argparse.ArgumentParser(description="Monitor AR Tag objects")
parser.add_argument("input_world", help="Underworlds world to listen")
parser.add_argument("output_world", help="Underworlds world to produce")
parser.add_argument("ar_objects_file_path",
help="The path to localise the AR objects description")
parser.add_argument("mesh_dir",
help="The path to localize the object meshes")
parser.add_argument("--reference",
default="map",
help="The reference frame of the system (default map)")
args = parser.parse_args()
rospy.init_node("ar_object_provider", anonymous=True)
with underworlds.Context("AR objects provider") as ctx:
ARObjectProvider(ctx, args.output_world, args.ar_objects_file_path,
args.mesh_dir, args.reference).run()
self.ros_pub["speaking_attention_point"].publish(self.attention_point)
self.ros_pub["vizu"].publish(self.attention_point.target)
def run(self):
rate = rospy.Rate(30)
while not rospy.is_shutdown():
if self.attention_point:
self.publish_attention_point()
rate.sleep()
if __name__ == "__main__":
sys.argv = [
arg for arg in sys.argv if "__name" not in arg and "__log" not in arg
]
sys.argc = len(sys.argv)
# Manage command line options
import argparse
parser = argparse.ArgumentParser(description="Handle speech synthesis")
parser.add_argument(
"world",
help="The world where to write the situation associated to speech")
args = parser.parse_args()
rospy.init_node("speech_wrapper", anonymous=True)
with underworlds.Context(
"speech_wrapper") as ctx: # Here we connect to the server
SpeechWrapperNode(ctx, args.world).run()
def setUp(self):
self.server = underworlds.server.start()
self.ctx = underworlds.Context("unittest - visibility")
def run(self):
rate = rospy.Rate(30)
while not rospy.is_shutdown():
self.clean_humans()
self.publish_human_tf_frames()
if __name__ == "__main__":
sys.argv = [
arg for arg in sys.argv if "__name" not in arg and "__log" not in arg
]
sys.argc = len(sys.argv)
parser = argparse.ArgumentParser(
description="Add in the given output world, the nodes from input "
"world and the robot agent from ROS")
parser.add_argument("output_world", help="Underworlds output world")
parser.add_argument("mesh_dir",
help="The path used to localize the human meshes")
parser.add_argument("--reference",
default="map",
help="The reference frame")
args = parser.parse_args()
rospy.init_node('multimodal_human_provider', anonymous=False)
with underworlds.Context("Multimodal human monitor") as ctx:
MultimodalHumanMonitor(ctx, args.output_world, args.mesh_dir,
args.reference).run()
with open("/home/ysallami/duration_stat.csv", "w") as csvfile2:
fieldnames = ["reco_durations"]
writer = csv.DictWriter(csvfile2, fieldnames=fieldnames)
writer.writeheader()
for duration in self.reco_durations:
writer.writerow({"reco_durations": duration})
csvfile2.close()
if __name__ == "__main__":
sys.argv = [arg for arg in sys.argv if "__name" not in arg and "__log" not in arg]
sys.argc = len(sys.argv)
parser = argparse.ArgumentParser(description="Add in the given output world, the nodes from input "
"world and the robot agent from ROS")
parser.add_argument("output_world", help="Underworlds output world")
parser.add_argument("mesh_dir", help="The path used to localize the human meshes")
parser.add_argument("--reference", default="map", help="The reference frame")
args = parser.parse_args()
rospy.init_node('multimodal_human_provider', anonymous=False)
with underworlds.Context("Multimodal human provider") as ctx:
MultimodalHumanProvider(ctx, args.output_world, args.mesh_dir, args.reference).run()
def setUp(self):
self.server = underworlds.server.start()
time.sleep(0.1) # leave some time to the server to start
self.ctx = underworlds.Context("unittest - basic server interaction")