def _request_qtc(self, qsr, world, parameters):
qrmsg = QSRlib_Request_Message(which_qsr=qsr,
input_data=world,
dynamic_args=parameters)
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qrmsg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
qtc = []
dis = []
for t in out.qsrs.get_sorted_timestamps():
for k, v in out.qsrs.trace[t].qsrs.items():
# print v.qsr.items()
# if len(v.qsr.items()) < 2:
# continue # Hacky but we only want dist when qtc is there too.
for l, w in v.qsr.items():
if l.startswith("qtc"):
# q = self._to_np_array(w)
# if l.startswith("qtcbcs"):
# q = q if len(q) == 4 else np.append(q, [np.nan, np.nan])
# qtc = np.array([q]) if not qtc.size else np.append(qtc, [q], axis=0)
qtc.append(w)
elif l == "argprobd":
dis.append(w)
return qtc, dis
def _request_qtc(self, qsr, world, include_missing_data=True):
"""reads all .qtc files from a given directory and resturns them as numpy arrays"""
qrmsg = QSRlib_Request_Message(
which_qsr=qsr,
input_data=world,
include_missing_data=include_missing_data)
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qrmsg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
rospy.logdebug("Request was made at " +
str(out.timestamp_request_made) + " and received at " +
str(out.timestamp_request_received) +
" and computed at " + str(out.timestamp_qsrs_computed))
ret = np.array([])
for t in out.qsrs.get_sorted_timestamps():
foo = str(t) + ": "
for k, v in zip(out.qsrs.trace[t].qsrs.keys(),
out.qsrs.trace[t].qsrs.values()):
foo += str(k) + ":" + str(v.qsr) + "; "
if qsr == self.qtc_types["qtcbc"]:
q = v.qsr if len(v.qsr) == 4 else np.append(
v.qsr, [np.nan, np.nan])
ret = np.array([q]) if not ret.size else np.append(
ret, [q], axis=0)
else:
ret = np.array([v.qsr]) if not ret.size else np.append(
ret, [v.qsr], axis=0)
rospy.logdebug(foo)
return ret
def __init__(self):
self.skeleton_data = {
} #keeps the published complete skeleton in a dictionary. key = uuid
self.rate = rospy.Rate(15.0)
## listeners:
rospy.Subscriber("skeleton_data/complete", skeleton_complete,
self.skeleton_callback)
## Logging params:
self._with_mongodb = rospy.get_param("~with_mongodb", "true")
self._logging = rospy.get_param("~logging", "false")
self._message_store = rospy.get_param("~message_store_prefix",
"people_skeleton")
if self._logging:
msg_store = self._message_store + "_world_state"
rospy.loginfo("Connecting to mongodb...%s" % msg_store)
self._store_client_world = MessageStoreProxy(collection=msg_store)
msg_store = self._message_store + "_qstag"
self._store_client_qstag = MessageStoreProxy(collection=msg_store)
## QSR INFO:
self._which_qsr = rospy.get_param("~which_qsr", "rcc3")
#self.which_qsr = ["qtcbs", "argd"]
self.set_params()
self.cln = QSRlib_ROS_Client()
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 _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 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 __init__(self):
self.skeleton_data = {} #keeps the published complete skeleton in a dictionary. key = uuid
self.rate = rospy.Rate(15.0)
## listeners:
rospy.Subscriber("skeleton_data/complete", skeleton_complete, self.skeleton_callback)
## Logging params:
self._with_mongodb = rospy.get_param("~with_mongodb", "true")
self._logging = rospy.get_param("~logging", "false")
self._message_store = rospy.get_param("~message_store_prefix", "people_skeleton")
if self._logging:
msg_store = self._message_store + "_world_state"
rospy.loginfo("Connecting to mongodb...%s" % msg_store)
self._store_client_world = MessageStoreProxy(collection=msg_store)
msg_store = self._message_store + "_qstag"
self._store_client_qstag = MessageStoreProxy(collection=msg_store)
## QSR INFO:
self._which_qsr = rospy.get_param("~which_qsr", "rcc3")
#self.which_qsr = ["qtcbs", "argd"]
self.set_params()
self.cln = QSRlib_ROS_Client()
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 __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 __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()
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)
input_data=world,
dynamic_args=dynamic_args)
print(which_qsr)
print(dynamic_args["tpcc"])
t1 = time()
if args.ros:
try:
import rospy
from qsrlib_ros.qsrlib_ros_client import QSRlib_ROS_Client
except ImportError:
raise ImportError("ROS not found")
client_node = rospy.init_node("qsr_lib_ros_client_example")
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qsrlib_request_message)
res = cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
else:
qsrlib = QSRlib()
qsrlib_response_message = qsrlib.request_qsrs(
req_msg=qsrlib_request_message)
pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message)
qstag = qsrlib_response_message.qstag
t2 = time()
print("Time: ", t2 - t1)
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)
# dynamic_args[which_qsr] = {"qsrs_for": ["o1"]}
# dynamic_args["for_all_qsrs"] = {"qsrs_for": [("o1", "o2"), "o2"]}
# try:
# print(dynamic_args[which_qsr]["qsrs_for"])
# except KeyError:
# print("qsrs_for not set in which_qsr namespace")
# print(dynamic_args["for_all_qsrs"]["qsrs_for"])
# # DBG: eof
# dynamic_args[which_qsr]["qsrs_for"] = [("o1", "o2", "o3"), ("o1", "o3")]
dynamic_args = {"tpcc": {"qsrs_for": [("o1", "o2", "o3"), ("o1", "o3")]}}
qsrlib_request_message = QSRlib_Request_Message(which_qsr=which_qsr, input_data=world, dynamic_args=dynamic_args)
if args.ros:
try:
import rospy
from qsrlib_ros.qsrlib_ros_client import QSRlib_ROS_Client
except ImportError:
raise ImportError("ROS not found")
client_node = rospy.init_node("qsr_lib_ros_client_example")
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qsrlib_request_message)
res = cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
else:
qsrlib = QSRlib()
qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)
pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message)
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)
class dataReader(object):
def __init__(self):
self.skeleton_data = {
} #keeps the published complete skeleton in a dictionary. key = uuid
self.rate = rospy.Rate(15.0)
## listeners:
rospy.Subscriber("skeleton_data/complete", skeleton_complete,
self.skeleton_callback)
## Logging params:
self._with_mongodb = rospy.get_param("~with_mongodb", "true")
self._logging = rospy.get_param("~logging", "false")
self._message_store = rospy.get_param("~message_store_prefix",
"people_skeleton")
if self._logging:
msg_store = self._message_store + "_world_state"
rospy.loginfo("Connecting to mongodb...%s" % msg_store)
self._store_client_world = MessageStoreProxy(collection=msg_store)
msg_store = self._message_store + "_qstag"
self._store_client_qstag = MessageStoreProxy(collection=msg_store)
## QSR INFO:
self._which_qsr = rospy.get_param("~which_qsr", "rcc3")
#self.which_qsr = ["qtcbs", "argd"]
self.set_params()
self.cln = QSRlib_ROS_Client()
def set_params(self):
quantisation_factor = 0.01
validate = False
no_collapse = True
argd_params = {"Touch": 0.5, "Near": 2, "Far": 3}
qstag_params = {"min_rows": 1, "max_rows": 1, "max_eps": 5}
qsrs_for = [("head", "right_hand"), ("head", "left_hand"),
("left_hand", "right_hand")]
object_types = {
"right_hand": "hand",
"left_hand": "hand",
"head": "head"
}
self.dynamic_args = {
"qtcbs": {
"quantisation_factor": quantisation_factor,
"validate": validate,
"no_collapse": no_collapse,
"qsrs_for": qsrs_for
},
"rcc3": {
"qsrs_for": qsrs_for
},
"argd": {
"qsr_relations_and_values": argd_params
},
"qstag": {
"object_types": object_types,
"params": qstag_params
},
"filters": {
"median_filter": {
"window": 3
}
}
}
def _create_qsrs(self):
while not rospy.is_shutdown():
for uuid, msg_data in self.skeleton_data.items():
if msg_data["flag"] != 1: continue
print ">> recieving worlds for:", uuid
qsrlib_response_message = self._call_qsrLib(uuid, msg_data)
if self._logging:
print msg_data.keys()
self.upload_qsrs_to_mongo(uuid,
qsrlib_response_message.qstag,
msg_data)
print ">>>", qsrlib_response_message.qstag.episodes
print ">", qsrlib_response_message.qstag.graphlets.histogram
#print ">>>", qsrlib_response_message.qstag.graph
del self.skeleton_data[uuid]
self.rate.sleep()
def skeleton_callback(self, msg):
self.skeleton_data[msg.uuid] = {
"flag": 1,
"detections": msg.number_of_detections,
"map": msg.map_name,
"current_node": msg.current_topo_node,
"start_time": msg.start_time,
"end_time": msg.end_time,
"world": self.convert_skeleton_to_world(msg.skeleton_data)
}
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 _call_qsrLib(self, uuid, msg_data):
qsrlib_request_message = QSRlib_Request_Message(which_qsr=self._which_qsr, input_data=msg_data["world"], \
dynamic_args=self.dynamic_args)
req = self.cln.make_ros_request_message(qsrlib_request_message)
if self._logging:
print "logging the world trace"
msg = QSRlibMongo(uuid=uuid,
data=pickle.dumps(msg_data["world"]),
start_time=msg_data["start_time"],
end_time=msg_data["end_time"])
query = {"uuid": uuid}
self._store_client_world.update(message=msg,
message_query=query,
upsert=True)
res = self.cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
return qsrlib_response_message
def upload_qsrs_to_mongo(self, uuid, qstag, msg_data):
print "logging the QSTAG"
eps = pickle.dumps(qstag.episodes)
graph = pickle.dumps(qstag.graph)
obs = [node['name'] for node in qstag.object_nodes]
print ">>", qstag.graphlets.graphlets
msg = QsrsToMongo(uuid=uuid,
which_qsr=self._which_qsr,
episodes=eps,
igraph=graph,
histogram=qstag.graphlets.histogram,
code_book=qstag.graphlets.code_book,
start_time=msg_data["start_time"],
map_name=msg_data["map"],
current_node=msg_data["current_node"],
number_of_detections=msg_data["detections"],
objects=obs,
end_time=msg_data["end_time"])
query = {"uuid": uuid}
self._store_client_qstag.update(message=msg,
message_query=query,
upsert=True)
class dataReader(object):
def __init__(self):
self.skeleton_data = {} #keeps the published complete skeleton in a dictionary. key = uuid
self.rate = rospy.Rate(15.0)
## listeners:
rospy.Subscriber("skeleton_data/complete", skeleton_complete, self.skeleton_callback)
## Logging params:
self._with_mongodb = rospy.get_param("~with_mongodb", "true")
self._logging = rospy.get_param("~logging", "false")
self._message_store = rospy.get_param("~message_store_prefix", "people_skeleton")
if self._logging:
msg_store = self._message_store + "_world_state"
rospy.loginfo("Connecting to mongodb...%s" % msg_store)
self._store_client_world = MessageStoreProxy(collection=msg_store)
msg_store = self._message_store + "_qstag"
self._store_client_qstag = MessageStoreProxy(collection=msg_store)
## QSR INFO:
self._which_qsr = rospy.get_param("~which_qsr", "rcc3")
#self.which_qsr = ["qtcbs", "argd"]
self.set_params()
self.cln = QSRlib_ROS_Client()
def set_params(self):
quantisation_factor = 0.01
validate = False
no_collapse = True
argd_params = {"Touch": 0.5, "Near": 2, "Far": 3}
qstag_params = {"min_rows" : 1, "max_rows" : 1, "max_eps" : 5}
qsrs_for = [("head", "right_hand"), ("head", "left_hand"), ("left_hand", "right_hand")]
object_types = {"right_hand": "hand", "left_hand": "hand", "head":"head"}
self.dynamic_args = {
"qtcbs": {"quantisation_factor": quantisation_factor,
"validate": validate,
"no_collapse": no_collapse,
"qsrs_for": qsrs_for},
"rcc3" : {"qsrs_for": qsrs_for},
"argd": {"qsr_relations_and_values" : argd_params},
"qstag": {"object_types" : object_types, "params" : qstag_params},
"filters": {"median_filter" : {"window": 3}}
}
def _create_qsrs(self):
while not rospy.is_shutdown():
for uuid, msg_data in self.skeleton_data.items():
if msg_data["flag"] != 1: continue
print ">> recieving worlds for:", uuid
qsrlib_response_message = self._call_qsrLib(uuid, msg_data)
if self._logging:
print msg_data.keys()
self.upload_qsrs_to_mongo(uuid, qsrlib_response_message.qstag, msg_data)
print ">>>", qsrlib_response_message.qstag.episodes
print ">", qsrlib_response_message.qstag.graphlets.histogram
#print ">>>", qsrlib_response_message.qstag.graph
del self.skeleton_data[uuid]
self.rate.sleep()
def skeleton_callback(self, msg):
self.skeleton_data[msg.uuid] = {
"flag": 1,
"detections" : msg.number_of_detections,
"map" : msg.map_name,
"current_node" : msg.current_topo_node,
"start_time": msg.start_time,
"end_time": msg.end_time,
"world": self.convert_skeleton_to_world(msg.skeleton_data)}
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 _call_qsrLib(self, uuid, msg_data):
qsrlib_request_message = QSRlib_Request_Message(which_qsr=self._which_qsr, input_data=msg_data["world"], \
dynamic_args=self.dynamic_args)
req = self.cln.make_ros_request_message(qsrlib_request_message)
if self._logging:
print "logging the world trace"
msg = QSRlibMongo(uuid = uuid, data = pickle.dumps(msg_data["world"]),
start_time = msg_data["start_time"], end_time = msg_data["end_time"])
query = {"uuid" : uuid}
self._store_client_world.update(message=msg, message_query=query, upsert=True)
res = self.cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
return qsrlib_response_message
def upload_qsrs_to_mongo(self, uuid, qstag, msg_data):
print "logging the QSTAG"
eps = pickle.dumps(qstag.episodes)
graph = pickle.dumps(qstag.graph)
obs = [node['name'] for node in qstag.object_nodes]
print ">>", qstag.graphlets.graphlets
msg = QsrsToMongo(uuid = uuid, which_qsr = self._which_qsr,
episodes=eps, igraph=graph, histogram=qstag.graphlets.histogram,
code_book = qstag.graphlets.code_book, start_time= msg_data["start_time"],
map_name = msg_data["map"], current_node = msg_data["current_node"],
number_of_detections = msg_data["detections"], objects = obs,
end_time = msg_data["end_time"]
)
query = {"uuid" : uuid}
self._store_client_qstag.update(message=msg, message_query=query, upsert=True)
