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)