def convert_skeleton_to_world(self, data, use_every=1):
world = World_Trace()
joint_states = {
'head': [],
'neck': [],
'torso': [],
'left_shoulder': [],
'left_elbow': [],
'left_hand': [],
'left_hip': [],
'left_knee': [],
'left_foot': [],
'right_shoulder': [],
'right_elbow': [],
'right_hand': [],
'right_hip': [],
'right_knee': [],
'right_foot': []
}
for tp, msg in enumerate(data):
for i in msg.joints:
o = Object_State(name=i.name, timestamp=tp, x=i.pose.position.x,\
y = i.pose.position.y, z=i.pose.position.z, xsize=0.1, ysize=0.1, zsize=0.1)
joint_states[i.name].append(o)
for joint_data in joint_states.values():
world.add_object_state_series(joint_data)
return world
def __compute_qsr(self, bb1, bb2):
if not self.qsr:
return ""
ax, ay, bx, by = self.bb1
cx, cy, dx, dy = self.bb2
qsrlib = QSRlib()
world = World_Trace()
world.add_object_state_series([
Object_State(name="red",
timestamp=0,
x=((ax + bx) / 2.0),
y=((ay + by) / 2.0),
xsize=abs(bx - ax),
ysize=abs(by - ay)),
Object_State(name="yellow",
timestamp=0,
x=((cx + dx) / 2.0),
y=((cy + dy) / 2.0),
xsize=abs(dx - cx),
ysize=abs(dy - cy))
])
dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}}
qsrlib_request_message = QSRlib_Request_Message(
which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args)
qsrlib_response_message = qsrlib.request_qsrs(
req_msg=qsrlib_request_message)
for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
foo = ""
for k, v in zip(
qsrlib_response_message.qsrs.trace[t].qsrs.keys(),
qsrlib_response_message.qsrs.trace[t].qsrs.values()):
foo += str(k) + ":" + str(v.qsr) + "; \n"
return foo
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace
Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
't' is used in the World Trace."""
ob_states = {}
world = World_Trace()
for t in self.sorted_timestamps:
# Joints:
for joint_id, (x, y, z) in self.map_frame_data[t].items():
if joint_id not in ob_states.keys():
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)]
else:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z))
# SOMA objects
for object, (x, y, z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [Object_State(name=str(object), timestamp=t, x=x, y=y, z=z)]
else:
ob_states[object].append(Object_State(name=str(object), timestamp=t, x=x, y=y, z=z))
# Robot's position
(x, y, z) = self.robot_data[t][0]
if "robot" not in ob_states.keys():
ob_states["robot"] = [Object_State(name="robot", timestamp=t, x=x, y=y, z=z)]
else:
ob_states["robot"].append(Object_State(name="robot", timestamp=t, x=x, y=y, z=z))
for object_state in ob_states.values():
world.add_object_state_series(object_state)
self.map_world = world
def __init__(self):
self.world = World_Trace()
self.options = sorted(QSRlib().qsrs_registry.keys())
self.calculi = self.options[0]
# if a value lies between the thresholds set for 'near' and 'far' then the relation is immediately reported as 'far'
# the values are bound by a 'lesser than, < ' relationship
self.distance_args = {"Near": 3.0, "Medium": 3.5, "Far": 4.0}
self.dynamic_args = {
"argd": {
"qsr_relations_and_values": self.distance_args
},
"for_all_qsrs": {
'qsrs_for': [('object_1', 'robot'), ('object_2', 'robot')]
}
}
self.relationship_pub = rospy.Publisher("argd_realtionship",
QsrMsg,
queue_size=10)
self.pose_sub1 = message_filters.Subscriber("extracted_pose1", PoseMsg)
self.pose_sub2 = message_filters.Subscriber("extracted_pose2", PoseMsg)
self.cln = QSRlib_ROS_Client()
# the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.pose_sub1, self.pose_sub2], 3, 0.1)
self.ts.registerCallback(self.relations_callback)
def __init__(self, *args):
super(TestQTC, self).__init__(*args)
rospy.init_node(NAME)
self.world = World_Trace()
self.world_duplicate = World_Trace()
self._load_file()
def getQSRState(self, xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1, td):
isValid = False
state = None
human_os = [
Object_State(name="human_os", timestamp=0, x=xh0, y=yh0),
Object_State(name="human_os", timestamp=td, x=xh1, y=yh1)
]
robot_os = [
Object_State(name="robot_os", timestamp=0, x=xr0, y=yr0),
Object_State(name="robot_os", timestamp=td, x=xr1, y=yr1)
]
# make some input data
world = World_Trace()
world.add_object_state_series(human_os)
world.add_object_state_series(robot_os)
# make a QSRlib request message
qsrlib_request_message = QSRlib_Request_Message(
self.which_qsr, world, self.dynammic_args)
# request your QSRs
qsrlib_response_message = self.qsrlib.request_qsrs(
req_msg=qsrlib_request_message)
# should have 1 timestamp
t = qsrlib_response_message.qsrs.get_sorted_timestamps()
if len(t) != 1:
rospy.logerr(
"Node [" + rospy.get_name() +
"@getQSRState] : response timestamp message lenght is not 1.")
return (isValid, state)
t = t[0]
# should have one value: human to robot
v = qsrlib_response_message.qsrs.trace[t].qsrs.values()
if len(v) != 1:
rospy.logerr(
"Node [" + rospy.get_name() +
"@getQSRState] : response values message lenght is not 1.")
return (isValid, state)
v = v[0]
state = v.qsr.values()
if len(state) != 1:
rospy.logerr(
"Node [" + rospy.get_name() +
"@getQSRState] : response state message lenght is not 1.")
return (isValid, state)
state = state[0]
# Ok, maybe is valid ...
isValid = True
return (isValid, state)
def apply_mean_filter(self, window_length=3):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
joints, ob_states = {}, {}
world = World_Trace()
window = {}
filtered_cnt = 0
for t in self.sorted_timestamps:
joints[t] = {}
for joint_id, (x, y, z) in self.skeleton_data[t].items():
# print "jointID=", joint_id, (x,y,z)
try:
window[joint_id].pop(0)
except (IndexError, KeyError):
window[joint_id] = []
window[joint_id].append((float(x), float(y), float(z)))
avg_x, avg_y, avg_z = 0, 0, 0
for l, point in enumerate(window[joint_id]):
avg_x += point[0]
avg_y += point[1]
avg_z += point[2]
x, y, z = avg_x / float(l + 1), avg_y / float(
l + 1), avg_z / float(l + 1)
joints[t][joint_id] = (x, y, z)
#Create a QSRLib format list of Object States (for each joint id)
if joint_id not in ob_states.keys():
ob_states[joint_id] = [
Object_State(name=joint_id,
timestamp=filtered_cnt,
x=x,
y=y,
z=z)
]
else:
ob_states[joint_id].append(
Object_State(name=joint_id,
timestamp=filtered_cnt,
x=x,
y=y,
z=z))
filtered_cnt += 1
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
world.add_object_state_series(obj)
self.filtered_skeleton_data = joints
self.camera_world = world
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace
Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
't' is used in the World Trace."""
ob_states = {}
world = World_Trace()
# print ">>", self.map_frame_data.keys()
for t, frame in enumerate(self.sorted_timestamps):
#Joints:
for joint_id, (x, y, z) in self.map_frame_data[frame].items():
if joint_id not in ob_states.keys():
ob_states[joint_id] = [
Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)
]
else:
ob_states[joint_id].append(
Object_State(name=joint_id, timestamp=t, x=x, y=y,
z=z))
# SOMA objects
for object, (x, y, z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [
Object_State(name=str(object),
timestamp=t,
x=x,
y=y,
z=z)
]
else:
ob_states[object].append(
Object_State(name=str(object),
timestamp=t,
x=x,
y=y,
z=z))
# Robot's position
(x, y, z) = self.robot_data[frame][0]
if 'robot' not in ob_states.keys():
ob_states['robot'] = [
Object_State(name='robot', timestamp=t, x=x, y=y, z=z)
]
else:
ob_states['robot'].append(
Object_State(name='robot', timestamp=t, x=x, y=y, z=z))
for object_state in ob_states.values():
world.add_object_state_series(object_state)
self.map_world = world
def isValidState(xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1, td, which_qsr,
dynammic_args, forbidden_states):
ans = 1
human_os = [
Object_State(name="human_os", timestamp=0, x=xh0, y=yh0),
Object_State(name="human_os", timestamp=td, x=xh1, y=yh1)
]
robot_os = [
Object_State(name="robot_os", timestamp=0, x=xr0, y=yr0),
Object_State(name="robot_os", timestamp=td, x=xr1, y=yr1)
]
# make some input data
world = World_Trace()
world.add_object_state_series(human_os)
world.add_object_state_series(robot_os)
# make a QSRlib request message
qsrlib_request_message = QSRlib_Request_Message(which_qsr, world,
dynammic_args)
# request your QSRs
qsrlib_response_message = qsrlib.request_qsrs(
req_msg=qsrlib_request_message)
# should have 1 timestamp
t = qsrlib_response_message.qsrs.get_sorted_timestamps()
if len(t) != 1:
print("something is wrong!!!!!!!!!!!!! with t")
sys.exit(1)
t = t[0]
# should have one value: human to robot
v = qsrlib_response_message.qsrs.trace[t].qsrs.values()
if len(v) != 1:
print("something is wrong!!!!!!!!!!!!! with v")
sys.exit(1)
v = v[0]
state = v.qsr.values()
if len(state) != 1:
print("something is wrong!!!!!!!!!!!!! with state")
sys.exit(1)
state = state[0]
if state in forbidden_states:
ans = 0
#print(state)
return (ans, state)
def qsr_relation_between(obj1_name, obj2_name, obj1, obj2):
global frame
qsrlib = QSRlib()
options = sorted(qsrlib.qsrs_registry.keys())
if init_qsr.qsr not in options:
raise ValueError("qsr not found, keywords: %s" % options)
world = World_Trace()
object_types = {obj1_name: obj1_name, obj2_name: obj2_name}
dynamic_args = {
"qstag": {
"object_types": object_types,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 3
}
},
"tpcc": {
"qsrs_for": [(obj1_name, obj2_name)]
},
"rcc2": {
"qsrs_for": [(obj1_name, obj2_name)]
},
"rcc4": {
"qsrs_for": [(obj1_name, obj2_name)]
},
"rcc8": {
"qsrs_for": [(obj1_name, obj2_name)]
}
}
o1 = [Object_State(name=obj1_name, timestamp=frame, x=obj1[0], y=obj1[1], \
xsize=obj1[2], ysize=obj1[3])]
o2 = [Object_State(name=obj2_name, timestamp=frame, x=obj2[0], y=obj2[1], \
xsize=obj2[2], ysize=obj2[3])]
world.add_object_state_series(o1)
world.add_object_state_series(o2)
qsrlib_request_message = QSRlib_Request_Message(which_qsr=init_qsr.qsr, \
input_data=world, dynamic_args=dynamic_args)
qsrlib_response_message = qsrlib.request_qsrs(
req_msg=qsrlib_request_message)
pretty_print_world_qsr_trace(init_qsr.qsr,
qsrlib_response_message) #, vis = True)
qsr_value = find_qsr_value(init_qsr.qsr, qsrlib_response_message)
return qsr_value
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace"""
ob_states = {}
world = World_Trace()
for t in self.sorted_timestamps:
#Joints:
for joint_id, (x, y, z) in self.map_frame_data[t].items():
if joint_id not in ob_states.keys():
ob_states[joint_id] = [
Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)
]
else:
ob_states[joint_id].append(
Object_State(name=joint_id, timestamp=t, x=x, y=y,
z=z))
# SOMA objects
for object, (x, y, z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [
Object_State(name=str(object),
timestamp=t,
x=x,
y=y,
z=z)
]
else:
ob_states[object].append(
Object_State(name=str(object),
timestamp=t,
x=x,
y=y,
z=z))
# Robot's position
(x, y, z) = self.robot_data[t][0]
if 'robot' not in ob_states.keys():
ob_states['robot'] = [
Object_State(name='robot', timestamp=t, x=x, y=y, z=z)
]
else:
ob_states['robot'].append(
Object_State(name='robot', timestamp=t, x=x, y=y, z=z))
for object_state in ob_states.values():
world.add_object_state_series(object_state)
self.map_world = world
class QSRlib_Request_Message(object):
def __init__(self, which_qsr="", input_data=None, qsrs_for=[], timestamp_request_made=None,
start=0, finish=-1, objects_names=[], include_missing_data=True, qsr_relations_and_values={},
future=False, config=None, dynamic_args=None):
self.future = future
self.which_qsr = which_qsr
self.input_data = None
self.set_input_data(input_data=input_data, start=start, finish=finish, objects_names=objects_names)
self.qsrs_for = qsrs_for
self.timestamp_request_made = datetime.now() if timestamp_request_made is None else timestamp_request_made
self.include_missing_data = include_missing_data
self.qsr_relations_and_values = qsr_relations_and_values # should be more dynamic
self.config = config
self.dynamic_args = dynamic_args
def make(self, which_qsr, input_data, qsrs_for=[], timestamp_request_made=None, future=None, ini=None,
dynamic_args=None):
if future:
self.future = future
self.which_qsr = which_qsr
self.input_data = self.set_input_data(input_data)
self.qsrs_for = qsrs_for
self.timestamp_request_made = datetime.now() if timestamp_request_made is None else timestamp_request_made
self.ini = None
self.dynamic_args = None
def set_input_data(self, input_data, start=0, finish=-1, objects_names=[]):
error = False
if input_data is None:
input_data = World_Trace()
if isinstance(input_data, World_Trace):
if len(input_data.trace) > 0:
self.input_data = input_data
else:
if self.input_data is not None and len(self.input_data.trace) > 0:
print("Reusing previous input data")
else:
print("Warning (QSRlib_Request_Message.set_input_data): It seems you are trying to reuse previous data, but previous data is empty")
self.input_data = World_Trace(description="error")
error = True
else:
print("ERROR (QSRLib_.set_input_data): input data has incorrect type, must be of type 'World_Trace'")
self.input_data = World_Trace(description="error")
error = True
if not error:
if finish >= 0:
self.input_data = self.input_data.get_at_timestamp_range(start=start, finish=finish)
if len(objects_names) > 0:
self.input_data = self.input_data.get_for_objects(objects_names=objects_names)
def ppl_cb(self, msg):
self.__buffer["human"].append(msg.poses[0])
self.__buffer["robot"].append(self.robot_pose)
# QTC needs at least two instances in time to work
ob = []
if len(self.__buffer["human"]) > 1:
# Creating the world trace for both agents over all timestamps
world = World_Trace()
for idx, (human, robot) in enumerate(zip(self.__buffer["human"], self.__buffer["robot"])):
ob.append(Object_State(
name="human",
timestamp=idx,
x=human.position.x,
y=human.position.y
))
ob.append(Object_State(
name="robot",
timestamp=idx,
x=robot.position.x,
y=robot.position.y
))
world.add_object_state_series(ob)
# Creating the qsr_lib request message
qrmsg = QSRlib_Request_Message(
which_qsr="qtccs",
input_data=world,
dynamic_args=self.__parameters
)
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qrmsg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
# Printing the result, publishing might be more useful though ;)
qsr_array = []
for t in out.qsrs.get_sorted_timestamps():
for k, v in out.qsrs.trace[t].qsrs.items():
qsr_array.append(v.qsr.values()[0])
#test
self.send_dict(qsr_array)
#print out
rospy.sleep(0.3) # Worst smoothing ever, I'm sure you can do better
def convert_skeleton_to_world(self, data, use_every=1):
world = World_Trace()
joint_states = {'head' : [], 'neck' : [], 'torso' : [], 'left_shoulder' : [],'left_elbow' : [],
'left_hand' : [],'left_hip' : [], 'left_knee' : [],'left_foot' : [],'right_shoulder' : [],
'right_elbow' : [],'right_hand' : [],'right_hip' : [],'right_knee' : [],'right_foot' : []}
for tp, msg in enumerate(data):
for i in msg.joints:
o = Object_State(name=i.name, timestamp=tp, x=i.pose.position.x,\
y = i.pose.position.y, z=i.pose.position.z, xsize=0.1, ysize=0.1, zsize=0.1)
joint_states[i.name].append(o)
for joint_data in joint_states.values():
world.add_object_state_series(joint_data)
return world
def apply_median_filter(self, window_size=11, vis=False):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
fx = 525.0
fy = 525.0
cx = 319.5
cy = 239.5
data, f_data, ob_states = {}, {}, {}
self.camera_world = World_Trace()
self.filtered_skeleton_data = {}
for t in self.sorted_timestamps:
self.filtered_skeleton_data[t] = {} # initialise with all timepoints
for joint_id, (x, y, z) in self.skeleton_data[t].items():
try:
data[joint_id]["x"].append(x)
data[joint_id]["y"].append(y)
data[joint_id]["z"].append(z)
except:
data[joint_id] = {"x": [x], "y": [y], "z": [z]}
for joint_id, joint_dic in data.items():
f_data[joint_id] = {}
for dim, values in joint_dic.items():
filtered_values = sp.signal.medfilt(values, window_size) # filter
f_data[joint_id][dim] = filtered_values
if dim is "z" and 0 in filtered_values:
print "Z should never be 0. (distance to camera)."
vis = True
if vis and "hand" in joint_id:
print joint_id
title1 = "input %s position: %s" % (joint_id, dim)
title2 = "filtered %s position: %s" % (joint_id, dim)
plot_the_results(values, filtered_values, title1, title2)
# Create a QSRLib format list of Object States (for each joint id)
for cnt, t in enumerate(self.sorted_timestamps):
x = f_data[joint_id]["x"][cnt]
y = f_data[joint_id]["y"][cnt]
z = f_data[joint_id]["z"][cnt]
# add the x2d and y2d (using filtered x,y,z data)
# print self.uuid, t, joint_id, (x,y,z)
x2d = int(x * fx / z * 1 + cx)
y2d = int(y * fy / z * -1 + cy)
self.filtered_skeleton_data[t][joint_id] = (x, y, z, x2d, y2d) # Kept for Legacy
try:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z))
except:
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z)]
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
self.camera_world.add_object_state_series(obj)
def ppl_cb(self, msg):
self.__buffer["human"].append(msg.poses[0])
self.__buffer["robot"].append(self.robot_pose)
# QTC needs at least two instances in time to work
ob = []
if len(self.__buffer["human"]) > 1:
# Creating the world trace for both agents over all timestamps
world = World_Trace()
for idx, (human, robot) in enumerate(
zip(self.__buffer["human"], self.__buffer["robot"])):
ob.append(
Object_State(name="human",
timestamp=idx,
x=human.position.x,
y=human.position.y))
ob.append(
Object_State(name="robot",
timestamp=idx,
x=robot.position.x,
y=robot.position.y))
world.add_object_state_series(ob)
# Creating the qsr_lib request message
qrmsg = QSRlib_Request_Message(which_qsr="qtccs",
input_data=world,
dynamic_args=self.__parameters)
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qrmsg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
# Printing the result, publishing might be more useful though ;)
qsr_array = []
for t in out.qsrs.get_sorted_timestamps():
for k, v in out.qsrs.trace[t].qsrs.items():
qsr_array.append(v.qsr.values()[0])
#test
self.send_dict(qsr_array)
#print out
rospy.sleep(0.3) # Worst smoothing ever, I'm sure you can do better
def _convert_to_world(self,
data_dict,
quantisation_factor=0,
validate=True,
no_collapse=False,
distance_threshold=np.Inf):
world = World_Trace()
agent1 = data_dict["agent1"]
agent2 = data_dict["agent2"]
name1 = agent1["name"]
name2 = agent2["name"]
x1 = np.array(agent1["x"], dtype=float)
y1 = np.array(agent1["y"], dtype=float)
x2 = np.array(agent2["x"], dtype=float)
y2 = np.array(agent2["y"], dtype=float)
ob = []
for idx, (e_x1, e_y1, e_x2, e_y2) in enumerate(zip(x1, y1, x2, y2)):
ob.append(
Object_State(name=name1,
timestamp=idx,
x=e_x1,
y=e_y1,
quantisation_factor=quantisation_factor,
validate=validate,
no_collapse=no_collapse,
distance_threshold=distance_threshold))
ob.append(
Object_State(name=name2,
timestamp=idx,
x=e_x2,
y=e_y2,
quantisation_factor=quantisation_factor,
validate=validate,
no_collapse=no_collapse,
distance_threshold=distance_threshold))
world.add_object_state_series(ob)
return world
def __init__(self):
self.world = World_Trace()
self.options = sorted(QSRlib().qsrs_registry.keys())
self.calculi = self.options[6]
# quantisation_factor represents the minimum change that should be considered to assume that the object has moved
self.dynamic_args = {"qtcbs": {"no_collapse":True,"quantisation_factor": 0.01}, "qtcbs": {"qsrs_for": [("object_1","robot"), ("object_2","robot")]}}
self.prev_timestamp = 0
self.prev_pose_x1 = self.prev_pose_y1 = 0
self.prev_pose_x2 = self.prev_pose_y2 = 0
self.prev_robot_pose_x = self.prev_robot_pose_y = 0
self.relationship_pub = rospy.Publisher("qtcb_realtionship", QsrMsg, queue_size = 10)
self.pose_sub1 = message_filters.Subscriber("extracted_pose1",PoseMsg)
self.pose_sub2 = message_filters.Subscriber("extracted_pose2",PoseMsg)
self.cln = QSRlib_ROS_Client()
# the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
self.ts = message_filters.ApproximateTimeSynchronizer([self.pose_sub1, self.pose_sub2], 10, 0.1)
self.ts.registerCallback(self.relations_callback)
def apply_mean_filter_old(self, window_length=3):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
joints, ob_states = {}, {}
world = World_Trace()
window = {}
filtered_cnt = 0
for t in self.sorted_timestamps:
joints[t] = {}
for joint_id, (x, y, z) in self.skeleton_data[t].items():
# print "jointID=", joint_id, (x,y,z)
try:
window[joint_id].pop(0)
except (IndexError, KeyError):
window[joint_id] = []
window[joint_id].append((float(x), float(y), float(z)))
avg_x, avg_y, avg_z = 0, 0, 0
for l, point in enumerate(window[joint_id]):
avg_x += point[0]
avg_y += point[1]
avg_z += point[2]
x, y, z = avg_x / float(l + 1), avg_y / float(l + 1), avg_z / float(l + 1)
joints[t][joint_id] = (x, y, z)
# Create a QSRLib format list of Object States (for each joint id)
if joint_id not in ob_states.keys():
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z)]
else:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z))
filtered_cnt += 1
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
world.add_object_state_series(obj)
self.filtered_skeleton_data = joints
self.camera_world = world
def __compute_qsr(self, bb1, bb2):
if not self.qsr:
return ""
ax, ay, bx, by = self.bb1
cx, cy, dx, dy = self.bb2
qsrlib = QSRlib()
world = World_Trace()
world.add_object_state_series([Object_State(name="red", timestamp=0, x=((ax+bx)/2.0), y=((ay+by)/2.0), xsize=abs(bx-ax), ysize=abs(by-ay)),
Object_State(name="yellow", timestamp=0, x=((cx+dx)/2.0), y=((cy+dy)/2.0), xsize=abs(dx-cx), ysize=abs(dy-cy))])
dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}}
qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args)
qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)
for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
foo = ""
for k, v in zip(qsrlib_response_message.qsrs.trace[t].qsrs.keys(),
qsrlib_response_message.qsrs.trace[t].qsrs.values()):
foo += str(k) + ":" + str(v.qsr) + "; \n"
return foo
def qsr_message(state_series, objects):
qsrlib = QSRlib()
options = sorted(qsrlib.qsrs_registry.keys())
if init_qsr.qsr not in options:
raise ValueError("qsr not found, keywords: %s" % options)
world = World_Trace()
# Create dynamic arguments for every qsr type
dynamic_args = dynamic_qsr_arguments(objects)
# Add all object states in the World.
for o in state_series:
world.add_object_state_series(o)
# Create request message
qsrlib_request_message = QSRlib_Request_Message(which_qsr=init_qsr.qsr, input_data=world, dynamic_args=dynamic_args)
qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)
pretty_print_world_qsr_trace(init_qsr.qsr, qsrlib_response_message, vis = True)
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace"""
self.subj_world_trace = {}
for subj in self.skeleton_map:
ob_states={}
world = World_Trace()
map_frame_data = self.skeleton_map[subj]
for joint_id in map_frame_data.keys():
#Joints:
for t in xrange(self.frames):
x = map_frame_data[joint_id][t][0]
y = map_frame_data[joint_id][t][1]
z = map_frame_data[joint_id][t][2]
if joint_id not in ob_states.keys():
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=t+1, x=x, y=y, z=z)]
else:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=t+1, x=x, y=y, z=z))
# SOMA objects
for t in xrange(self.frames):
for object, (x,y,z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [Object_State(name=str(object), timestamp=t+1, x=x, y=y, z=z)]
else:
ob_states[object].append(Object_State(name=str(object), timestamp=t+1, x=x, y=y, z=z))
# # Robot's position
# (x,y,z) = self.robot_data[t][0]
# if 'robot' not in ob_states.keys():
# ob_states['robot'] = [Object_State(name='robot', timestamp=t, x=x, y=y, z=z)]
# else:
# ob_states['robot'].append(Object_State(name='robot', timestamp=t, x=x, y=y, z=z))
for obj, object_state in ob_states.items():
world.add_object_state_series(object_state)
# get world trace for each person
region = self.soma_roi_config[self.waypoint]
self.subj_world_trace[subj] = self.get_object_frame_qsrs(world, self.objects[region])
def _convert_to_world(self, data_dict):
world = World_Trace()
agent1 = data_dict["agent1"]
agent2 = data_dict["agent2"]
name1 = agent1["name"]
name2 = agent2["name"]
x1 = np.array(agent1["x"], dtype=float)
y1 = np.array(agent1["y"], dtype=float)
x2 = np.array(agent2["x"], dtype=float)
y2 = np.array(agent2["y"], dtype=float)
ob = []
for idx, (e_x1, e_y1, e_x2, e_y2) in enumerate(zip(x1, y1, x2, y2)):
ob.append(Object_State(name=name1, timestamp=idx, x=e_x1, y=e_y1))
ob.append(Object_State(name=name2, timestamp=idx, x=e_x2, y=e_y2))
world.add_object_state_series(ob)
return world
def __init__(self):
self.world = World_Trace()
self.options = sorted(QSRlib().qsrs_registry.keys())
self.calculi = self.options[0]
# any value in between goes to the upper bound
self.dynamic_args = {
"argd": {
"qsr_relations_and_values": {
"Near": 0.36,
"Medium": .40,
"Far": .45
}
}
}
self.relationship_pub = rospy.Publisher("argd_realtionship",
QsrMsg,
queue_size=2)
self.pose_sub1 = rospy.Subscriber("extracted_pose1", PoseMsg,
self.relations_callback)
self.cln = QSRlib_ROS_Client()
def set_input_data(self, input_data, start=0, finish=-1, objects_names=[]):
error = False
if input_data is None:
input_data = World_Trace()
if isinstance(input_data, World_Trace):
if len(input_data.trace) > 0:
self.input_data = input_data
else:
if self.input_data is not None and len(self.input_data.trace) > 0:
print("Reusing previous input data")
else:
print("Warning (QSRlib_Request_Message.set_input_data): It seems you are trying to reuse previous data, but previous data is empty")
self.input_data = World_Trace(description="error")
error = True
else:
print("ERROR (QSRLib_.set_input_data): input data has incorrect type, must be of type 'World_Trace'")
self.input_data = World_Trace(description="error")
error = True
if not error:
if finish >= 0:
self.input_data = self.input_data.get_at_timestamp_range(start=start, finish=finish)
if len(objects_names) > 0:
self.input_data = self.input_data.get_for_objects(objects_names=objects_names)
from __future__ import print_function, division
from qsrlib_io.world_trace import Object_State, World_Trace
def print_world_trace(world_trace):
for t in world_trace.get_sorted_timestamps():
print("-t:", t)
for oname, o in world_trace.trace[t].objects.items():
print("%s\t%f\t%f\t%f\t%f\t%f\t%f" % (oname, o.x, o.y, o.z, o.xsize, o.ysize, o.xsize))
def print_world_state(world_state):
for oname, o in world_state.objects.items():
print("%s\t%f\t%f\t%f\t%f\t%f\t%f" % (oname, o.x, o.y, o.z, o.xsize, o.ysize, o.xsize))
if __name__ == "__main__":
world = World_Trace()
o1 = [Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.),
Object_State(name="o1", timestamp=1, x=1., y=2., xsize=5., ysize=8.),
Object_State(name="o1", timestamp=2, x=1., y=3., xsize=5., ysize=8.),
Object_State(name="o1", timestamp=3, x=1., y=4., xsize=5., ysize=8.),
Object_State(name="o1", timestamp=4, x=1., y=5., xsize=5., ysize=8.)]
o2 = [Object_State(name="o2", timestamp=0, x=11., y=1., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=1, x=11., y=2., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=2, x=11., y=3., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=3, x=11., y=4., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=4, x=11., y=5., xsize=5., ysize=8.)]
world.add_object_state_series(o1)
world.add_object_state_series(o2)
# world_new = world.get_at_timestamp_range(2, 3, include_finish=False)
class qtc_interpreter:
def __init__(self):
self.world = World_Trace()
self.options = sorted(QSRlib().qsrs_registry.keys())
self.calculi = self.options[6]
# quantisation_factor represents the minimum change that should be considered to assume that the object has moved
self.dynamic_args = {"qtcbs": {"no_collapse":True,"quantisation_factor": 0.01}, "qtcbs": {"qsrs_for": [("object_1","robot"), ("object_2","robot")]}}
self.prev_timestamp = 0
self.prev_pose_x1 = self.prev_pose_y1 = 0
self.prev_pose_x2 = self.prev_pose_y2 = 0
self.prev_robot_pose_x = self.prev_robot_pose_y = 0
self.relationship_pub = rospy.Publisher("qtcb_realtionship", QsrMsg, queue_size = 10)
self.pose_sub1 = message_filters.Subscriber("extracted_pose1",PoseMsg)
self.pose_sub2 = message_filters.Subscriber("extracted_pose2",PoseMsg)
self.cln = QSRlib_ROS_Client()
# the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
self.ts = message_filters.ApproximateTimeSynchronizer([self.pose_sub1, self.pose_sub2], 10, 0.1)
self.ts.registerCallback(self.relations_callback)
def relations_callback(self,extracted_pose1,extracted_pose2):
qtc_relation_msg = QsrMsg()
print("extracted_pose1",extracted_pose1.pose_vec.position)
print("marker1", extracted_pose1.marker_id)
print('==========================================')
print("extracted_pose2",extracted_pose2.pose_vec.position)
print("marker2", extracted_pose2.marker_id)
print("##########################################")
o1 = [Object_State(name="object_1", timestamp=self.prev_timestamp, x=self.prev_pose_x1, y=self.prev_pose_y1),
Object_State(name="object_1", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose1.pose_vec.position.x, y=extracted_pose1.pose_vec.position.z)]
o2 = [Object_State(name="robot", timestamp=self.prev_timestamp, x=self.prev_robot_pose_x, y=self.prev_robot_pose_y),
Object_State(name="robot", timestamp=extracted_pose1.header.stamp.secs, x=0, y=0)]
o3 = [Object_State(name="object_2", timestamp=self.prev_timestamp, x=self.prev_pose_x2, y=self.prev_pose_y2),
Object_State(name="object_2", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose2.pose_vec.position.x, y=extracted_pose2.pose_vec.position.z)]
self.world.add_object_state_series(o1)
self.world.add_object_state_series(o2)
self.world.add_object_state_series(o3)
self.prev_timestamp = extracted_pose1.header.stamp.secs
self.prev_pose_x1 = extracted_pose1.pose_vec.position.x
self.prev_pose_y1 = extracted_pose1.pose_vec.position.z
self.prev_pose_x2 = extracted_pose2.pose_vec.position.x
self.prev_pose_y2 = extracted_pose2.pose_vec.position.z
self.prev_robot_pose_x = 0
self.prev_robot_pose_y = 0
qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.calculi, input_data=self.world, dynamic_args=self.dynamic_args)
req = self.cln.make_ros_request_message(qsrlib_request_message)
res = self.cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
qtc_relation_msg.selected_qsr = str(self.calculi)
qtc_relation_msg.marker_id1 = extracted_pose1.marker_id
qtc_relation_msg.marker_id2 = extracted_pose2.marker_id
for time in qsrlib_response_message.qsrs.get_sorted_timestamps():
qtc_relation_msg.message_time = extracted_pose1.header.stamp.secs
object_key = str(extracted_pose1.header.stamp.secs) + " = "
value_key = str(extracted_pose1.header.stamp.secs) + " = "
for key, value in zip(qsrlib_response_message.qsrs.trace[time].qsrs.keys(),
qsrlib_response_message.qsrs.trace[time].qsrs.values()):
object_key += str(key) + ';'
qtc_relation_msg.objects = object_key
value_key += str(value.qsr)
qtc_relation_msg.relationship = value_key
self.relationship_pub.publish(qtc_relation_msg)
print(qtc_relation_msg)
def __compute_qsr(self, bb1, bb2):
if not self.qsr:
return ""
ax, ay, bx, by = bb1
cx, cy, dx, dy = bb2
qsrlib = QSRlib()
world = World_Trace()
if self.args.distribution[0] == "weibull":
weibull_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/weibull_units.npy")
weibull = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/weibull.npy")
scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")
o1 = []
o2 = []
o3 = []
for ii,unit in tqdm(enumerate(weibull_units)):
o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
for ii,weib in tqdm(enumerate(weibull)):
o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(weib*100), object_type="Weibull"))
for ii,score in tqdm(enumerate(scores)):
o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
elif self.args.distribution[0] == "beta":
beta_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta_units.npy")
beta = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta.npy")
scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")
o1 = []
o2 = []
o3 = []
for ii,unit in tqdm(enumerate(beta_units)):
o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
for ii,beta in tqdm(enumerate(beta)):
o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(beta*100), object_type="Beta"))
for ii,score in tqdm(enumerate(scores)):
o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
elif self.args.distribution[0] == "cauchy":
cauchy_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/cauchy_units.npy")
cauchy = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/cauchy.npy")
scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")
o1 = []
o2 = []
o3 = []
for ii,unit in tqdm(enumerate(cauchy_units)):
o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
for ii,cauc in tqdm(enumerate(cauchy)):
o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(cauc*100), object_type="Cauchy"))
for ii,score in tqdm(enumerate(scores)):
o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
elif self.args.distribution[0] == "rayleigh":
rayleigh_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/rayleigh_units.npy")
rayleigh = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta.npy")
scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")
o1 = []
o2 = []
o3 = []
for ii,unit in tqdm(enumerate(rayleigh_units)):
o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
for ii,rayl in tqdm(enumerate(rayleigh)):
o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(rayl*100), object_type="Rayleigh"))
for ii,score in tqdm(enumerate(scores)):
o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
elif self.args.distribution[0] == "gamma":
gamma_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/gamma_units.npy")
gamma = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/gamma.npy")
scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")
o1 = []
o2 = []
o3 = []
for ii,unit in tqdm(enumerate(gamma_units)):
o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
for ii,gam in tqdm(enumerate(gamma)):
o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(gam*100), object_type="Gamma"))
for ii,score in tqdm(enumerate(scores)):
o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
world.add_object_state_series(o1)
world.add_object_state_series(o2)
world.add_object_state_series(o3)
# world.add_object_state_series([Object_State(name="red", timestamp=0, x=((ax+bx)/2.0), y=((ay+by)/2.0), xsize=abs(bx-ax), ysize=abs(by-ay)),
# Object_State(name="yellow", timestamp=0, x=((cx+dx)/2.0), y=((cy+dy)/2.0), xsize=abs(dx-cx), ysize=abs(dy-cy))])
dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}}
qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args)
qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)
for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
foo = ""
for k, v in zip(qsrlib_response_message.qsrs.trace[t].qsrs.keys(),
qsrlib_response_message.qsrs.trace[t].qsrs.values()):
foo += str(k) + ":" + str(v.qsr) + "; \n"
return foo
class event(object):
"""Event object class"""
def __init__(self, uuid=None, dir_=None, waypoint=None):
self.uuid = uuid
# self.start_frame, self.end_frame = self.get_last_frame()
# self.waypoint = waypoint
self.dir = dir_
self.sorted_timestamps = []
self.sorted_ros_timestamps = []
self.bad_timepoints = {} # use for filtering
self.skeleton_data = {} # type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
self.map_frame_data = {} # type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
self.robot_data = {} # type: dict[timepoint][joint_id]= ((x, y, z), (roll, pitch, yaw))
# self.world = World_Trace()
def apply_median_filter(self, window_size=11, vis=False):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
fx = 525.0
fy = 525.0
cx = 319.5
cy = 239.5
data, f_data, ob_states = {}, {}, {}
self.camera_world = World_Trace()
self.filtered_skeleton_data = {}
for t in self.sorted_timestamps:
self.filtered_skeleton_data[t] = {} # initialise with all timepoints
for joint_id, (x, y, z) in self.skeleton_data[t].items():
try:
data[joint_id]["x"].append(x)
data[joint_id]["y"].append(y)
data[joint_id]["z"].append(z)
except:
data[joint_id] = {"x": [x], "y": [y], "z": [z]}
for joint_id, joint_dic in data.items():
f_data[joint_id] = {}
for dim, values in joint_dic.items():
filtered_values = sp.signal.medfilt(values, window_size) # filter
f_data[joint_id][dim] = filtered_values
if dim is "z" and 0 in filtered_values:
print "Z should never be 0. (distance to camera)."
vis = True
if vis and "hand" in joint_id:
print joint_id
title1 = "input %s position: %s" % (joint_id, dim)
title2 = "filtered %s position: %s" % (joint_id, dim)
plot_the_results(values, filtered_values, title1, title2)
# Create a QSRLib format list of Object States (for each joint id)
for cnt, t in enumerate(self.sorted_timestamps):
x = f_data[joint_id]["x"][cnt]
y = f_data[joint_id]["y"][cnt]
z = f_data[joint_id]["z"][cnt]
# add the x2d and y2d (using filtered x,y,z data)
# print self.uuid, t, joint_id, (x,y,z)
x2d = int(x * fx / z * 1 + cx)
y2d = int(y * fy / z * -1 + cy)
self.filtered_skeleton_data[t][joint_id] = (x, y, z, x2d, y2d) # Kept for Legacy
try:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z))
except:
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z)]
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
self.camera_world.add_object_state_series(obj)
def apply_mean_filter_old(self, window_length=3):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
joints, ob_states = {}, {}
world = World_Trace()
window = {}
filtered_cnt = 0
for t in self.sorted_timestamps:
joints[t] = {}
for joint_id, (x, y, z) in self.skeleton_data[t].items():
# print "jointID=", joint_id, (x,y,z)
try:
window[joint_id].pop(0)
except (IndexError, KeyError):
window[joint_id] = []
window[joint_id].append((float(x), float(y), float(z)))
avg_x, avg_y, avg_z = 0, 0, 0
for l, point in enumerate(window[joint_id]):
avg_x += point[0]
avg_y += point[1]
avg_z += point[2]
x, y, z = avg_x / float(l + 1), avg_y / float(l + 1), avg_z / float(l + 1)
joints[t][joint_id] = (x, y, z)
# Create a QSRLib format list of Object States (for each joint id)
if joint_id not in ob_states.keys():
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z)]
else:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z))
filtered_cnt += 1
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
world.add_object_state_series(obj)
self.filtered_skeleton_data = joints
self.camera_world = world
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace
Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
't' is used in the World Trace."""
ob_states = {}
world = World_Trace()
for t in self.sorted_timestamps:
# Joints:
for joint_id, (x, y, z) in self.map_frame_data[t].items():
if joint_id not in ob_states.keys():
ob_states[joint_id] = [Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)]
else:
ob_states[joint_id].append(Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z))
# SOMA objects
for object, (x, y, z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [Object_State(name=str(object), timestamp=t, x=x, y=y, z=z)]
else:
ob_states[object].append(Object_State(name=str(object), timestamp=t, x=x, y=y, z=z))
# Robot's position
(x, y, z) = self.robot_data[t][0]
if "robot" not in ob_states.keys():
ob_states["robot"] = [Object_State(name="robot", timestamp=t, x=x, y=y, z=z)]
else:
ob_states["robot"].append(Object_State(name="robot", timestamp=t, x=x, y=y, z=z))
for object_state in ob_states.values():
world.add_object_state_series(object_state)
self.map_world = world
parser.add_argument("qsr", help="choose qsr: %s" % options, type=str)
parser.add_argument("-i", "--input", help="file from which to read object states", type=str)
parser.add_argument("--validate", help="validate state chain. Only QTC", action="store_true")
parser.add_argument("--quantisation_factor", help="quantisation factor for 0-states in qtc, or 's'-states in mos", type=float)
parser.add_argument("--no_collapse", help="does not collapse similar adjacent states. Only QTC", action="store_true")
parser.add_argument("--distance_threshold", help="distance threshold for qtcb <-> qtcc transition. Only QTCBC", type=float)
parser.add_argument("-c", "--config", help="config file", type=str)
parser.add_argument("--ros", action="store_true", default=False, help="Use ROS eco-system")
args = parser.parse_args()
if args.qsr in options:
which_qsr = args.qsr
else:
raise ValueError("qsr not found, keywords: %s" % options)
world = World_Trace()
dynamic_args = {}
if which_qsr in ["rcc2", "rcc3", "ra"]:
dynamic_args = {which_qsr: {"quantisation_factor": args.quantisation_factor}}
o1 = [Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.),
Object_State(name="o1", timestamp=1, x=1., y=2., xsize=5., ysize=8.),
Object_State(name="o1", timestamp=2, x=1., y=3., xsize=5., ysize=8.)]
o2 = [Object_State(name="o2", timestamp=0, x=11., y=1., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=1, x=11., y=2., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=2, x=11., y=3., xsize=5., ysize=8.),
Object_State(name="o2", timestamp=3, x=11., y=4., xsize=5., ysize=8.)]
o3 = [Object_State(name="o3", timestamp=0, x=1., y=11., xsize=5., ysize=8.),
except:
pass
h_state_seqs.append(h_state_seq)
bag.close()
r_positions.append([r_xs, r_ys])
h_positions.append([h_xs, h_ys])
# In[61]:
# Test getting QTC_C sequence
bag_no = 0
quantisation_factor = 0.05
world = World_Trace()
h_x = [h_state_seqs[bag_no][i].x for i in range(len(h_state_seqs[bag_no]))]
h_y = [h_state_seqs[bag_no][i].y for i in range(len(h_state_seqs[bag_no]))]
r_x = [r_state_seqs[bag_no][i].x for i in range(len(r_state_seqs[bag_no]))]
r_y = [r_state_seqs[bag_no][i].y for i in range(len(r_state_seqs[bag_no]))]
# In[62]:
# Downsample state series' to 200kHz frequency
h_state_series = pd.DataFrame({
"x": h_x,
"y": h_y
},
index=[
class argd_interpreter:
def __init__(self):
self.world = World_Trace()
self.options = sorted(QSRlib().qsrs_registry.keys())
self.calculi = self.options[0]
# if a value lies between the thresholds set for 'near' and 'far' then the relation is immediately reported as 'far'
# the values are bound by a 'lesser than, < ' relationship
self.distance_args = {"Near": 3.0, "Medium": 3.5, "Far": 4.0}
self.dynamic_args = {
"argd": {
"qsr_relations_and_values": self.distance_args
},
"for_all_qsrs": {
'qsrs_for': [('object_1', 'robot'), ('object_2', 'robot')]
}
}
self.relationship_pub = rospy.Publisher("argd_realtionship",
QsrMsg,
queue_size=10)
self.pose_sub1 = message_filters.Subscriber("extracted_pose1", PoseMsg)
self.pose_sub2 = message_filters.Subscriber("extracted_pose2", PoseMsg)
self.cln = QSRlib_ROS_Client()
# the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.pose_sub1, self.pose_sub2], 3, 0.1)
self.ts.registerCallback(self.relations_callback)
def relations_callback(self, extracted_pose1, extracted_pose2):
o1 = [
Object_State(name="object_1",
timestamp=extracted_pose1.header.stamp.secs,
x=extracted_pose1.pose_vec.position.x,
y=extracted_pose1.pose_vec.position.z)
]
o2 = [
Object_State(name="robot",
timestamp=extracted_pose1.header.stamp.secs,
x=0,
y=0)
]
o3 = [
Object_State(name="object_2",
timestamp=extracted_pose1.header.stamp.secs,
x=extracted_pose2.pose_vec.position.x,
y=extracted_pose2.pose_vec.position.z)
]
self.world.add_object_state_series(o1)
self.world.add_object_state_series(o2)
self.world.add_object_state_series(o3)
qsrlib_request_message = QSRlib_Request_Message(
which_qsr=self.calculi,
input_data=self.world,
dynamic_args=self.dynamic_args)
req = self.cln.make_ros_request_message(qsrlib_request_message)
res = self.cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
argd_relation_msg = QsrMsg()
argd_relation_msg.selected_qsr = str(self.calculi)
argd_relation_msg.marker_id1 = extracted_pose1.marker_id
argd_relation_msg.marker_id2 = extracted_pose2.marker_id
for time in qsrlib_response_message.qsrs.get_sorted_timestamps():
argd_relation_msg.message_time = extracted_pose1.header.stamp.secs
object_key = str(time) + " = "
value_key = str(time) + " = "
for key, value in zip(
qsrlib_response_message.qsrs.trace[time].qsrs.keys(),
qsrlib_response_message.qsrs.trace[time].qsrs.values()):
object_key += str(key) + ';'
argd_relation_msg.objects = object_key
value_key += str(value.qsr)
argd_relation_msg.relationship = value_key
self.relationship_pub.publish(argd_relation_msg)
print(argd_relation_msg)
class event(object):
"""Event object class"""
def __init__(self, uuid, dir_, start, end, label, waypoint=None):
self.uuid = uuid
# self.start_frame, self.end_frame = self.get_last_frame()
# self.waypoint = waypoint
self.dir = dir_
self.start_frame = start
self.end_frame = end
self.label = label
self.sorted_timestamps = []
# self.sorted_ros_timestamps = []
self.bad_timepoints = {} #use for filtering
self.skeleton_data = {
} #type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
self.map_frame_data = {
} #type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
self.robot_data = {
} #type: dict[timepoint][joint_id]= ((x, y, z), (roll, pitch, yaw))
# self.world = World_Trace()
def apply_median_filter(self, window_size=11, vis=False):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
fx = 525.0
fy = 525.0
cx = 319.5
cy = 239.5
data, f_data, ob_states = {}, {}, {}
self.camera_world = World_Trace()
self.filtered_skeleton_data = {}
for t in self.sorted_timestamps:
self.filtered_skeleton_data[t] = {
} # initialise with all timepoints
for joint_id, (x, y, z) in self.skeleton_data[t].items():
try:
data[joint_id]["x"].append(x)
data[joint_id]["y"].append(y)
data[joint_id]["z"].append(z)
except KeyError:
data[joint_id] = {"x": [x], "y": [y], "z": [z]}
for joint_id, joint_dic in data.items():
f_data[joint_id] = {}
for dim, values in joint_dic.items():
filtered_values = sp.signal.medfilt(values,
window_size) # filter
f_data[joint_id][dim] = filtered_values
if dim is "z" and 0 in filtered_values:
print ">> Z should never be 0. (distance to camera)."
return False
#vis = True
if vis and "hand" in joint_id:
print joint_id
title1 = 'input %s position: %s' % (joint_id, dim)
title2 = 'filtered %s position: %s' % (joint_id, dim)
#plot_the_results(values, filtered_values, title1, title2)
# Create a QSRLib format list of Object States (for each joint id)
for cnt, t in enumerate(self.sorted_timestamps):
x = f_data[joint_id]["x"][cnt]
y = f_data[joint_id]["y"][cnt]
z = f_data[joint_id]["z"][cnt]
# add the x2d and y2d (using filtered x,y,z data)
# print self.uuid, t, joint_id, (x,y,z)
try:
x2d = int(x * fx / z * 1 + cx)
y2d = int(y * fy / z * -1 + cy)
except ValueError:
print "ValueError for %s at frame: %s. t:%s" % (self.uuid,
cnt, t)
x2d = 0
y2d = 0
self.filtered_skeleton_data[t][joint_id] = (
x, y, z, x2d, y2d) # Kept for Legacy
try:
ob_states[joint_id].append(
Object_State(name=joint_id,
timestamp=cnt,
x=x,
y=y,
z=z))
except KeyError:
ob_states[joint_id] = [
Object_State(name=joint_id,
timestamp=cnt,
x=x,
y=y,
z=z)
]
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
self.camera_world.add_object_state_series(obj)
return True
def apply_mean_filter_old(self, window_length=3):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
joints, ob_states = {}, {}
world = World_Trace()
window = {}
filtered_cnt = 0
for t in self.sorted_timestamps:
joints[t] = {}
for joint_id, (x, y, z) in self.skeleton_data[t].items():
# print "jointID=", joint_id, (x,y,z)
try:
window[joint_id].pop(0)
except (IndexError, KeyError):
window[joint_id] = []
window[joint_id].append((float(x), float(y), float(z)))
avg_x, avg_y, avg_z = 0, 0, 0
for l, point in enumerate(window[joint_id]):
avg_x += point[0]
avg_y += point[1]
avg_z += point[2]
x, y, z = avg_x / float(l + 1), avg_y / float(
l + 1), avg_z / float(l + 1)
joints[t][joint_id] = (x, y, z)
#Create a QSRLib format list of Object States (for each joint id)
if joint_id not in ob_states.keys():
ob_states[joint_id] = [
Object_State(name=joint_id,
timestamp=filtered_cnt,
x=x,
y=y,
z=z)
]
else:
ob_states[joint_id].append(
Object_State(name=joint_id,
timestamp=filtered_cnt,
x=x,
y=y,
z=z))
filtered_cnt += 1
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
world.add_object_state_series(obj)
self.filtered_skeleton_data = joints
self.camera_world = world
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace
Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
't' is used in the World Trace."""
ob_states = {}
world = World_Trace()
# print ">>", self.map_frame_data.keys()
for t, frame in enumerate(self.sorted_timestamps):
#Joints:
for joint_id, (x, y, z) in self.map_frame_data[frame].items():
if joint_id not in ob_states.keys():
ob_states[joint_id] = [
Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)
]
else:
ob_states[joint_id].append(
Object_State(name=joint_id, timestamp=t, x=x, y=y,
z=z))
# SOMA objects
for object, (x, y, z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [
Object_State(name=str(object),
timestamp=t,
x=x,
y=y,
z=z)
]
else:
ob_states[object].append(
Object_State(name=str(object),
timestamp=t,
x=x,
y=y,
z=z))
# Robot's position
(x, y, z) = self.robot_data[frame][0]
if 'robot' not in ob_states.keys():
ob_states['robot'] = [
Object_State(name='robot', timestamp=t, x=x, y=y, z=z)
]
else:
ob_states['robot'].append(
Object_State(name='robot', timestamp=t, x=x, y=y, z=z))
for object_state in ob_states.values():
world.add_object_state_series(object_state)
self.map_world = world
def apply_median_filter(self, window_size=11, vis=False):
"""Once obtained the joint x,y,z coords.
Apply a median filter over a temporal window to smooth the joint positions.
Whilst doing this, create a world Trace object"""
fx = 525.0
fy = 525.0
cx = 319.5
cy = 239.5
data, f_data, ob_states = {}, {}, {}
self.camera_world = World_Trace()
self.filtered_skeleton_data = {}
for t in self.sorted_timestamps:
self.filtered_skeleton_data[t] = {
} # initialise with all timepoints
for joint_id, (x, y, z) in self.skeleton_data[t].items():
try:
data[joint_id]["x"].append(x)
data[joint_id]["y"].append(y)
data[joint_id]["z"].append(z)
except KeyError:
data[joint_id] = {"x": [x], "y": [y], "z": [z]}
for joint_id, joint_dic in data.items():
f_data[joint_id] = {}
for dim, values in joint_dic.items():
filtered_values = sp.signal.medfilt(values,
window_size) # filter
f_data[joint_id][dim] = filtered_values
if dim is "z" and 0 in filtered_values:
print ">> Z should never be 0. (distance to camera)."
return False
#vis = True
if vis and "hand" in joint_id:
print joint_id
title1 = 'input %s position: %s' % (joint_id, dim)
title2 = 'filtered %s position: %s' % (joint_id, dim)
#plot_the_results(values, filtered_values, title1, title2)
# Create a QSRLib format list of Object States (for each joint id)
for cnt, t in enumerate(self.sorted_timestamps):
x = f_data[joint_id]["x"][cnt]
y = f_data[joint_id]["y"][cnt]
z = f_data[joint_id]["z"][cnt]
# add the x2d and y2d (using filtered x,y,z data)
# print self.uuid, t, joint_id, (x,y,z)
try:
x2d = int(x * fx / z * 1 + cx)
y2d = int(y * fy / z * -1 + cy)
except ValueError:
print "ValueError for %s at frame: %s. t:%s" % (self.uuid,
cnt, t)
x2d = 0
y2d = 0
self.filtered_skeleton_data[t][joint_id] = (
x, y, z, x2d, y2d) # Kept for Legacy
try:
ob_states[joint_id].append(
Object_State(name=joint_id,
timestamp=cnt,
x=x,
y=y,
z=z))
except KeyError:
ob_states[joint_id] = [
Object_State(name=joint_id,
timestamp=cnt,
x=x,
y=y,
z=z)
]
# #Add all the joint IDs into the World Trace
for joint_id, obj in ob_states.items():
self.camera_world.add_object_state_series(obj)
return True
def get_world_frame_trace(self, world_objects):
"""Accepts a dictionary of world (soma) objects.
Adds the position of the object at each timepoint into the World Trace"""
self.subj_world_trace = {}
for subj in self.skeleton_map:
ob_states = {}
world = World_Trace()
map_frame_data = self.skeleton_map[subj]
for joint_id in map_frame_data.keys():
#Joints:
for t in xrange(self.frames):
x = map_frame_data[joint_id][t][0]
y = map_frame_data[joint_id][t][1]
z = map_frame_data[joint_id][t][2]
if joint_id not in ob_states.keys():
ob_states[joint_id] = [
Object_State(name=joint_id,
timestamp=t + 1,
x=x,
y=y,
z=z)
]
else:
ob_states[joint_id].append(
Object_State(name=joint_id,
timestamp=t + 1,
x=x,
y=y,
z=z))
# SOMA objects
for t in xrange(self.frames):
for object, (x, y, z) in world_objects.items():
if object not in ob_states.keys():
ob_states[object] = [
Object_State(name=str(object),
timestamp=t + 1,
x=x,
y=y,
z=z)
]
else:
ob_states[object].append(
Object_State(name=str(object),
timestamp=t + 1,
x=x,
y=y,
z=z))
# # Robot's position
# (x,y,z) = self.robot_data[t][0]
# if 'robot' not in ob_states.keys():
# ob_states['robot'] = [Object_State(name='robot', timestamp=t, x=x, y=y, z=z)]
# else:
# ob_states['robot'].append(Object_State(name='robot', timestamp=t, x=x, y=y, z=z))
for obj, object_state in ob_states.items():
world.add_object_state_series(object_state)
# get world trace for each person
region = self.soma_roi_config[self.waypoint]
self.subj_world_trace[subj] = self.get_object_frame_qsrs(
world, self.objects[region])
mos_qsrs_for = ["o1", "o2"]
tpcc_qsrs_for = [("o1", "o2", "o3")]
object_types = {"o1": "Unit", "o2": "Beta", "o1-o2": "Score"}
if args.qsr in options:
if args.qsr != "multiple":
which_qsr = args.qsr
else:
which_qsr = multiple
elif args.qsr == "hardcoded":
which_qsr = ["qtcbs", "argd", "mos"]
else:
raise ValueError("qsr not found, keywords: %s" % options)
world = World_Trace()
dynamic_args = {
"qtcbs": {
"quantisation_factor": args.quantisation_factor,
"validate": args.validate,
"no_collapse": args.no_collapse,
"qsrs_for": qtcbs_qsrs_for
},
"argd": {
"qsr_relations_and_values": args.distance_threshold,
"qsrs_for": argd_qsrs_for
},
"mos": {
"qsrs_for": mos_qsrs_for
},
class argd_interpreter:
def __init__(self):
self.world = World_Trace()
self.options = sorted(QSRlib().qsrs_registry.keys())
self.calculi = self.options[0]
# any value in between goes to the upper bound
self.dynamic_args = {
"argd": {
"qsr_relations_and_values": {
"Near": 0.36,
"Medium": .40,
"Far": .45
}
}
}
self.relationship_pub = rospy.Publisher("argd_realtionship",
QsrMsg,
queue_size=2)
self.pose_sub1 = rospy.Subscriber("extracted_pose1", PoseMsg,
self.relations_callback)
self.cln = QSRlib_ROS_Client()
# the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
def relations_callback(self, extracted_pose1):
print('------------------------')
print(extracted_pose1)
print("========================")
o1 = [
Object_State(name="object_1",
timestamp=extracted_pose1.header.stamp.secs,
x=extracted_pose1.pose_vec.position.x,
y=extracted_pose1.pose_vec.position.z)
]
o2 = [
Object_State(name="robot",
timestamp=extracted_pose1.header.stamp.secs,
x=0,
y=0)
]
self.world.add_object_state_series(o1)
self.world.add_object_state_series(o2)
qsrlib_request_message = QSRlib_Request_Message(
which_qsr=self.calculi,
input_data=self.world,
dynamic_args=self.dynamic_args)
req = self.cln.make_ros_request_message(qsrlib_request_message)
res = self.cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
argd_relation_msg = QsrMsg()
argd_relation_msg.selected_qsr = str(self.calculi)
argd_relation_msg.marker_id1 = extracted_pose1.marker_id
for time in qsrlib_response_message.qsrs.get_sorted_timestamps():
argd_relation_msg.message_time = extracted_pose1.header.stamp.nsecs
for key, value in zip(
qsrlib_response_message.qsrs.trace[time].qsrs.keys(),
qsrlib_response_message.qsrs.trace[time].qsrs.values()):
argd_relation_msg.objects = key
argd_relation_msg.relationship = str(value.qsr)
self.relationship_pub.publish(argd_relation_msg)
print(argd_relation_msg)
class TestQTC(unittest.TestCase):
TEST_FILE = find_resource(PKG, 'qtc.csv')[0]
__duplicate = "duplicate"
__human_robot = ("human", "robot")
__robot_human = ("robot", "human")
__human_duplicate = ("human", __duplicate)
dynamic_args = {
"qtcs": {
"quantisation_factor": 0.01,
"validate": True,
"no_collapse": False,
"distance_threshold": 1.2
},
"for_all_qsrs": {
"qsrs_for": [__human_robot]
}
}
correct = {
"qtcbs": [{'qtcbs': '-,-'}, {'qtcbs': '-,0'}, {'qtcbs': '0,0'}, {'qtcbs': '0,+'}, {'qtcbs': '+,+'}],
"qtcbs_nocollapse": lambda: json.load(open(find_resource(PKG, 'qtcbs_nocollapse.txt')[0],'r')),
"qtccs": [{'qtccs': '-,-,0,0'}, {'qtccs': '-,-,+,0'}, {'qtccs': '-,0,+,0'}, {'qtccs': '-,0,+,+'}, {'qtccs': '0,0,+,+'}, {'qtccs': '0,0,+,0'}, {'qtccs': '0,+,+,0'}, {'qtccs': '+,+,+,0'}, {'qtccs': '+,+,0,0'}],
"qtccs_nocollapse": lambda: json.load(open(find_resource(PKG, 'qtccs_nocollapse.txt')[0],'r')),
"qtcbcs": [{'qtcbcs': '-,-'}, {'qtcbcs': '-,0'}, {'qtcbcs': '-,0,+,0'}, {'qtcbcs': '-,0,+,+'}, {'qtcbcs': '0,0,+,+'}, {'qtcbcs': '0,0,+,0'}, {'qtcbcs': '0,+,+,0'}, {'qtcbcs': '+,+,+,0'}, {'qtcbcs': '+,+'}],
"qtcbcs_nocollapse": lambda: json.load(open(find_resource(PKG, 'qtcbcs_nocollapse.txt')[0],'r')),
}
def __init__(self, *args):
super(TestQTC, self).__init__(*args)
rospy.init_node(NAME)
self.world = World_Trace()
self.world_duplicate = World_Trace()
self._load_file()
def _inverse_correct(self, correct):
ret = []
for e in correct:
k,v = e.items()[0]
try:
ret.append({k: ','.join(np.array(v.split(','))[[1,0,3,2]])})
except IndexError:
ret.append({k: ','.join(np.array(v.split(','))[[1,0]])})
return ret
def _load_file(self):
ob = []
with open(self.TEST_FILE) as csvfile:
reader = csv.DictReader(csvfile)
for idx,row in enumerate(reader):
ob.append(Object_State(
name=row['agent1'],
timestamp=idx+1,
x=float(row['x1']),
y=float(row['y1'])
))
ob.append(Object_State(
name=row['agent2'],
timestamp=idx+1,
x=float(row['x2']),
y=float(row['y2'])
))
ob.append(Object_State(
name=self.__duplicate,
timestamp=idx+1,
x=float(row['x2']),
y=float(row['y2'])
))
self.world.add_object_state_series(ob)
def _request(self, msg):
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(msg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
foo = {}
for t in out.qsrs.get_sorted_timestamps():
for k, v in zip(out.qsrs.trace[t].qsrs.keys(), out.qsrs.trace[t].qsrs.values()):
try:
foo[k].append(v.qsr)
except KeyError:
foo[k] = [v.qsr]
return foo
def _create_qsr(self, qsr, dynamic_args=None):
dynamic_args = self.dynamic_args if not dynamic_args else dynamic_args
qsrlib_request_message = QSRlib_Request_Message(
which_qsr=qsr,
input_data=self.world,
dynamic_args=dynamic_args
)
return self._request(qsrlib_request_message)
def _to_strings(self, array):
return [x.values()[0] for x in array]
def test_qtcb(self, between=__human_robot, dynamic_args=dynamic_args):
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(between)], self.correct["qtcbs"])
def test_qtcb_multiple_objects(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
self.test_qtcb(self.__human_duplicate, dynamic_args=dynamic_args)
def test_qtcb_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbs"]))
def test_qtcb_nocollapse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtcbs_nocollapse"]())
def test_qtcb_nocollapse_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbs_nocollapse"]()))
def test_qtcc(self, between=__human_robot, dynamic_args=dynamic_args):
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtccs"])
def test_qtcc_multiple_objects(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
self.test_qtcc(self.__human_duplicate, dynamic_args=dynamic_args)
def test_qtcc_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtccs"]))
def test_qtcc_nocollapse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtccs_nocollapse"]())
def test_qtcc_nocollapse_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtccs_nocollapse"]()))
def test_qtcbc(self, between=__human_robot, dynamic_args=dynamic_args):
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtcbcs"])
def test_qtcbc_multiple_objects(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
self.test_qtcbc(self.__human_duplicate, dynamic_args=dynamic_args)
def test_qtcbc_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbcs"]))
def test_qtcbc_nocollapse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtcbcs_nocollapse"]())
def test_qtcbc_nocollapse_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbcs_nocollapse"]()))