Python MessageStoreProxy.update方法代码示例

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
 def add_edge(self, or_waypoint, de_waypoint, action) :
     #print 'removing edge: '+edge_name
     rospy.loginfo('Adding Edge from '+or_waypoint+' to '+de_waypoint+' using '+action)
     node_name = or_waypoint
     #nodeindx = self._get_node_index(edged[0])
     msg_store = MessageStoreProxy(collection='topological_maps')
     query = {"name" : node_name, "pointset": self.name}
     query_meta = {}
     query_meta["pointset"] = self.name
     query_meta["map"] = self.map
     available = msg_store.query(TopologicalNode._type, query, query_meta)
     if len(available) == 1 :
         found =False
         for i in available[0][0].edges :
             #print i.node
             if i.node == de_waypoint :
                 found=True
                 break
         if not found :
             edge = Edge()
             edge.node = de_waypoint
             edge.action = action
             edge.top_vel = 0.55
             available[0][0].edges.append(edge)
             msg_store.update(available[0][0], query_meta, query, upsert=True)
         else :
             rospy.logerr("Edge already exist: Try deleting it first")
     else :
         rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
         rospy.logerr("Available data: "+str(available))

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
 def update_node_waypoint(self, node_name, new_pose) :
     msg_store = MessageStoreProxy(collection='topological_maps')
     query = {"name" : node_name, "pointset": self.name}
     query_meta = {}
     query_meta["pointset"] = self.name
     query_meta["map"] = self.map
     available = msg_store.query(TopologicalNode._type, query, query_meta)
     if len(available) == 1 :
         positionZ=available[0][0].pose.position.z
         available[0][0].pose = new_pose
         available[0][0].pose.position.z = positionZ
         msg_store.update(available[0][0], query_meta, query, upsert=True)
     else :
         rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
         rospy.logerr("Available data: "+str(available))

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
 def update_node_vertex(self, node_name, vertex_index, vertex_pose) :
     msg_store = MessageStoreProxy(collection='topological_maps')
     query = {"name" : node_name, "pointset": self.name}
     query_meta = {}
     query_meta["pointset"] = self.name
     query_meta["map"] = self.map
     available = msg_store.query(TopologicalNode._type, query, query_meta)
     if len(available) == 1 :
         #vertex_to_edit=available[0][0].verts[vertex_index]
         new_x_pos = -(available[0][0].pose.position.x - vertex_pose.position.x)
         new_y_pos = -(available[0][0].pose.position.y - vertex_pose.position.y)
         available[0][0].verts[vertex_index].x = new_x_pos
         available[0][0].verts[vertex_index].y = new_y_pos
         msg_store.update(available[0][0], query_meta, query, upsert=True)
     else :
         rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
         rospy.logerr("Available data: "+str(available))

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
    def add_edge(self, or_waypoint, de_waypoint, action, edge_id) :

        rospy.loginfo('Adding Edge from '+or_waypoint+' to '+de_waypoint+' using '+action)
        node_name = or_waypoint

        #nodeindx = self._get_node_index(edged[0])
        msg_store = MessageStoreProxy(collection='topological_maps')
        query = {"name" : node_name, "pointset": self.nodes.name}
        query_meta = {}
        query_meta["pointset"] = self.nodes.name
        query_meta["map"] = self.nodes.map

        #print query, query_meta
        available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
        #print len(available)
        if len(available) == 1 :
            eids = []
            for i in available[0][0].edges :
                eids.append(i.edge_id)

            if not edge_id or edge_id in eids:
                test=0
                eid = '%s_%s' %(or_waypoint, de_waypoint)
                while eid in eids:
                    eid = '%s_%s_%3d' %(or_waypoint, de_waypoint, test)
                    test += 1
            else:
                eid=edge_id

            edge = strands_navigation_msgs.msg.Edge()
            edge.node = de_waypoint
            edge.action = action
            edge.top_vel = 0.55
            edge.edge_id = eid
            edge.map_2d = available[0][0].map

            available[0][0].edges.append(edge)
                     
            #print available[0][0]
            msg_store.update(available[0][0], query_meta, query, upsert=True)
            return True
        else :
            rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
            rospy.logerr("Available data: "+str(available))
            return False

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
 def remove_edge(self, edge_name) :
     #print 'removing edge: '+edge_name
     rospy.loginfo('Removing Edge: '+edge_name)
     msg_store = MessageStoreProxy(collection='topological_maps')
     query = {"edges.edge_id" : edge_name, "pointset": self.nodes.name}
     query_meta = {}
     query_meta["pointset"] = self.nodes.name
     query_meta["map"] = self.nodes.map
     available = msg_store.query(TopologicalNode._type, query, query_meta)
     
     if len(available) >= 1 :
         for i in available :
             print i[0]
             for j in i[0].edges:
                 if j.edge_id == edge_name :
                     i[0].edges.remove(j)
                     msg_store.update(i[0], query_meta, query, upsert=True)
     else :
         rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
         rospy.logerr("Available data: "+str(available)) 

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
def rename_node(name, new_name, top_map_name):
    """
    Renames a topological map node. All edges will be updated to reflect the change.

    @param name: the current name of the node to be changec
    @param new_name: the new name to give the node
    @param top_map_name: the name of the topological map to work with (pointset)

    @return (number of nodes changed, number of edges changed)
    """

    maps = queries.get_maps()
    if top_map_name not in maps:
        raise Exception("Unknown topological map.")

    msg_store = MessageStoreProxy(collection='topological_maps')
    
    nodes = msg_store.query(TopologicalNode._type, {}, {'pointset':top_map_name})
    node_names = [node.name for node,meta in nodes]
    node_renames = 0
    edge_changes = 0
    node_changes = 0

    if name not in node_names:
        raise Exception("No such node.")
    if new_name in node_names:
        raise Exception("New node name already in use.")

    old_metas = []
    for node, meta in nodes:
        old_metas.append(copy.deepcopy(meta))
        if meta["node"] == name:
            meta["node"] = new_name
        if node.name == name:
            node.name = new_name
            node_renames += 1
            if node_renames > 1:
                raise Exception("More than one node has the same name!")
        for edge in node.edges:
            if edge.node == name:
                edge.node = new_name
            if edge.edge_id.find(name) > -1:
                edge.edge_id = edge.edge_id.replace(name, new_name)
                edge_changes += 1
            
    # Save the changed nodes
    for ((node, meta), old_meta) in zip(nodes, old_metas):
        changed = msg_store.update(node, meta, {'name':old_meta['node'],
                                                'pointset':meta['pointset']})
        if changed.success:
            node_changes += 1

    return (node_changes, edge_changes)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
 def remove_edge(self, edge_name) :
     #print 'removing edge: '+edge_name
     rospy.loginfo('Removing Edge: '+edge_name)
     edged = edge_name.split('_')
     #print edged
     node_name = edged[0]
     #nodeindx = self._get_node_index(edged[0])
     msg_store = MessageStoreProxy(collection='topological_maps')
     query = {"name" : node_name, "pointset": self.name}
     query_meta = {}
     query_meta["pointset"] = self.name
     query_meta["map"] = self.map
     available = msg_store.query(TopologicalNode._type, query, query_meta)
     if len(available) == 1 :
         for i in available[0][0].edges :
             #print i.node
             if i.node == edged[1] :
                 available[0][0].edges.remove(i)
         msg_store.update(available[0][0], query_meta, query, upsert=True)
     else :
         rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
         rospy.logerr("Available data: "+str(available))

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]

#.........这里部分代码省略.........
                if self.users[subj]["message"] == "Out of Scene" and subj in self.accumulate_data:
                    self._publish_complete_data(subj)
                    self.data[subj]['flag'] = 0
                    
                #print "h ", self.data[subj]['flag'], self.users[subj]["message"]
                    
            
            #For all subjects, publish the incremental skeleton and accumulate into self.data also.
            list_of_subs = [subj for subj in self.data if self.data[subj]['flag'] == 1]
            #print ">>>", list_of_subs
            for subj in list_of_subs:
                if self.users[subj]["message"] != "New": 
                    continue  # this catches cases where a User leaves the scene but they still have /tf data
                
                #print ">", subj
                incr_msg = skeleton_message()
                incr_msg.userID = subj
                for i in self.joints:
                    joint = joint_message()
                    joint.name = i
                    joint.time = self.data[subj][i]['t_old']
                    
                    position = Point(x = self.data[subj][i]['value'][0], \
                               y = self.data[subj][i]['value'][1], z = self.data[subj][i]['value'][2]) 
                    rot = Quaternion(x = self.data[subj][i]['rot'][0], y = self.data[subj][i]['rot'][1],
                               z = self.data[subj][i]['rot'][2], w = self.data[subj][i]['rot'][3])

                    joint.pose.position = position
                    joint.pose.orientation = rot
                    incr_msg.joints.append(joint)
                self.data[subj]['flag'] = 0
                
                #publish the instant frame message on /incremental topic
                self.publish_incr.publish(incr_msg)
                
                #accumulate the messages
                if self.users[subj]["message"] == "New":
                    self._accumulate_data(subj, incr_msg)
                elif self.users[subj]["message"] == "No message":
                    print "Just published this user. They are not back yet, get over it."
                else:
                    raise RuntimeError("this should never have occured; why is message not `New` or `Out of Scene' ??? ")

            self.rate.sleep()


    def _accumulate_data(self, subj, current_msg):
        # accumulate the multiple skeleton messages until user goes out of scene
        if current_msg.userID in self.accumulate_data:
            self.accumulate_data[current_msg.userID].append(current_msg)
        else:
            self.accumulate_data[current_msg.userID] = [current_msg]


    def _publish_complete_data(self, subj):
        # when user goes "out of scene" publish their accumulated data
        st = self.accumulate_data[subj][0].joints[0].time
        en = self.accumulate_data[subj][-1].joints[-1].time
        
        msg = skeleton_complete(uuid = self.users[subj]["uuid"], \
                                skeleton_data = self.accumulate_data[subj], \
                                number_of_detections = len(self.accumulate_data[subj]), \
                                map_name = self.map_info, current_topo_node = self.current_node, \
                                start_time = st, end_time = en, robot_pose = self.robot_pose )

        self.publish_comp.publish(msg)
        rospy.loginfo("User #%s: published %s msgs as %s" % (subj, len(self.accumulate_data[subj]), msg.uuid))

        # remove the user from the users dictionary and the accumulated data dict.
        self.users[subj]["message"] = "No message"
        del self.accumulate_data[subj]
        del self.users[subj]["uuid"]
        
        if self._with_logging:
            query = {"uuid" : msg.uuid}
            #self._store_client.insert(traj_msg, meta)
            self._store_client.update(message=msg, message_query=query, upsert=True)


    def tracker_state_callback(self, msg):
        # get the tracker state message and UUID of tracker user
        self.users[msg.userID]["uuid"] = msg.uuid
        self.users[msg.userID]["message"] = msg.message
        self.users[msg.userID]["timepoint"] = msg.timepoint

    def robot_callback(self, msg):
        self.robot_pose = msg

    def node_callback(self, msg):
        self.current_node = msg.data

    def map_callback(self, msg):
        # get the topological map name
        self.map_info = msg.map
        self.topo_listerner.unregister()



    def publish_skeleton(self):
        self._get_tf_data()

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]

#.........这里部分代码省略.........
		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)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]

#.........这里部分代码省略.........
                f1 = open(d+'skeleton/skl_'+f_str+'.txt','w')
                # print self.inc_sk.joints[0]
                f1.write('time:'+str(self.inc_sk.joints[0].time.secs)+'.'+str(self.inc_sk.joints[0].time.nsecs)+'\n')
                for i in self.inc_sk.joints:
                    f1.write(i.name+'\n')
                    f1.write('position\n')
                    f1.write('x:'+str(i.pose.position.x)+'\n')
                    f1.write('y:'+str(i.pose.position.y)+'\n')
                    f1.write('z:'+str(i.pose.position.z)+'\n')
                    f1.write('orientation\n')
                    f1.write('x:'+str(i.pose.orientation.x)+'\n')
                    f1.write('y:'+str(i.pose.orientation.y)+'\n')
                    f1.write('z:'+str(i.pose.orientation.z)+'\n')
                    f1.write('w:'+str(i.pose.orientation.w)+'\n')
                f1.close()

                # update frame number
                if self.inc_sk.uuid in self.sk_mapping:
                    self.sk_mapping[self.inc_sk.uuid]['frame'] += 1

                #publish the gaze request of person on every detection:
                if self.inc_sk.joints[0].name == 'head':
                    head = Header(frame_id='head_xtion_depth_optical_frame')
                    look_at_pose = PoseStamped(header = head, pose=self.inc_sk.joints[0].pose)
                    self.publish_consent_pose.publish(look_at_pose)
                #self.gazeClient.send_goal(self.gazegoal)

                # all this should happen given a good number of detections:
                print "%s out of %d frames are obtained" % (self.sk_mapping[self.inc_sk.uuid]['frame'], self.after_a_number_of_frames)
                if self.sk_mapping[self.inc_sk.uuid]['frame'] == self.after_a_number_of_frames and self.request_sent_flag == 0:
                    print "storing the %sth image to mongo for the webserver..." % self.after_a_number_of_frames
                    # Skeleton on white background
                    query = {"_meta.image_type": "white_sk_image"}
                    white_sk_to_mongo =  self.msg_store.update(message=self.white_sk_msg, meta={'image_type':"white_sk_image"}, message_query=query, upsert=True)
                    # Skeleton on rgb background
                    query = {"_meta.image_type": "rgb_sk_image"}
                    rgb_sk_img_to_mongo = self.msg_store.update(message=self.rgb_sk_msg, meta={'image_type':"rgb_sk_image"}, message_query=query, upsert=True)
                    # Skeleton on depth background
                    query = {"_meta.image_type": "depth_image"}
                    depth_img_to_mongo = self.msg_store.update(message=self.depth_msg, meta={'image_type':"depth_image"}, message_query=query, upsert=True)

                    consent_msg = "Check_consent_%s" % (t)
                    print consent_msg
                    # I think this should be a service call - so it definetly returns a value.
                    self.request_sent_flag = 1
                    # move and speak: (if no target waypoint, go to original waypoint)
                    # self.reset_ptu()
                    try:
                        self.navgoal.target = self.config[waypoint]['target']
                    except:
                        self.navgoal.target = waypoint
                    if self.navgoal.target != waypoint:
                        self.nav_goal_waypoint = waypoint  #to return to after consent
                        self.navClient.send_goal(self.navgoal)
                        result = self.navClient.wait_for_result()
                        if not result:
                            self.go_back_to_where_I_came_from()

                    self.publish_consent_req.publish(consent_msg)
                    rospy.sleep(0.1)
                    if self.request_sent_flag:
                        self.speaker.send_goal(maryttsGoal(text=self.speech))
                    while self.consent_ret is None:
                        rospy.sleep(0.1)

                    # Move Eyes - look up and down to draw attension.

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
class TRAN(object):

    def __init__(self, name):
        self.stored_uuids = list()
        self._uncertain_uuids = list()
        self.have_stored_uuids = list()
        self.trajectories = OfflineTrajectories()
        self.upbods = MessageStoreProxy(collection='upper_bodies').query(
            LoggingUBD._type, sort_query=[("$natural", 1)]
        )
        self._store_client = MessageStoreProxy(collection="trajectory_types")
        self.bridge = CvBridge()
        self.window = None
        self._traj_type = -1

    # create window for the image to show, and the button to click
    def _create_window(self, img):
        # main window
        self.window = Tkinter.Tk()
        self.window.title("Trajectory Annotator")

        # image frame in the top part of the main window
        imgtk = ImageTk.PhotoImage(image=Image.fromarray(img))
        Tkinter.LabelFrame(self.window).pack()
        Tkinter.Label(self.window, image=imgtk).pack()

        # button frame in the bottom part of the main window
        buttons_frame = Tkinter.LabelFrame(self.window).pack(side=Tkinter.BOTTOM, expand="yes")
        # human
        human_button = Tkinter.Button(
            buttons_frame, text="Human", fg="black", command=self._human_button_cb
        )
        human_button.pack(side=Tkinter.LEFT)
        # non-human
        human_button = Tkinter.Button(
            buttons_frame, text="Non-Human", fg="black", command=self._nonhuman_button_cb
        )
        human_button.pack(side=Tkinter.RIGHT)

        # window main loop
        self.window.mainloop()

    # call back for human button
    def _human_button_cb(self):
        self.window.destroy()
        self._traj_type = 1

    # call back for non-human button
    def _nonhuman_button_cb(self):
        self.window.destroy()
        self._traj_type = 0

    # check in database whether the uuid exists
    def _check_mongo_for_uuid(self, uuids):
        for uuid in uuids:
            logs = self._store_client.query(
                TrajectoryType._type, message_query={"uuid": uuid}
            )
            if len(logs) and uuid not in self.have_stored_uuids:
                self.have_stored_uuids.append(uuid)

    # hidden annotation function for presenting image and get the vote
    def _annotate(self, ubd, index, uuids, annotation):
        stop = False
        self._check_mongo_for_uuid(uuids)

        if len(uuids) > 0 and not (set(uuids).issubset(self.stored_uuids) or set(uuids).issubset(self.have_stored_uuids)):
            if len(ubd[0].ubd_rgb) == len(ubd[0].ubd_pos):
                b, g, r = cv2.split(
                    self.bridge.imgmsg_to_cv2(ubd[0].ubd_rgb[index])
                )
                self._create_window(cv2.merge((r, g, b)))

                if int(self._traj_type) in [0, 1]:
                    for uuid in [i for i in uuids if i not in self.stored_uuids]:
                        rospy.loginfo("%s is stored..." % uuid)
                        annotation.update({uuid: int(self._traj_type)})
                        self.stored_uuids.append(uuid)
                        if len(uuids) > 1:
                            self._uncertain_uuids.append(uuid)

                    self._uncertain_uuids = [
                        i for i in self._uncertain_uuids if i not in uuids
                    ]
                else:
                    stop = True
            else:
                rospy.logwarn("UBD_RGB has %d data, but UBD_POS has %d data" % (len(ubd[0].ubd_rgb), len(ubd[0].ubd_pos)))
        # if all uuids have been classified before, then this removes
        # all doubts about those uuids
        elif set(uuids).issubset(self.stored_uuids):
            self._uncertain_uuids = [
                i for i in self._uncertain_uuids if i not in uuids
            ]

        return stop, annotation

    # annotation function
    def annotate(self):
        stop = False
#.........这里部分代码省略.........

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
class RegionObservationTimeManager(object):

    def __init__(self, soma_map, soma_config):
        self.soma_map = soma_map
        self.soma_config = soma_config
        self.ms = MessageStoreProxy(collection="region_observation_time")
        self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
        self.roslog = pymongo.MongoClient(
            rospy.get_param("mongodb_host", "localhost"),
            rospy.get_param("mongodb_port", 62345)
        ).roslog.robot_pose
        self.reinit()

    def reinit(self):
        self.region_observation_duration = dict()
        self._month_year_observation_taken = dict()

    def load_from_mongo(self, days, minute_interval=60):
        """
            Load robot-region observation time from database (mongodb) and store them in
            self.region_observation_duration.
            Returning (region observation time, total duration of observation)
        """
        roi_region_daily = dict()
        total_duration = 0
        self.minute_interval = minute_interval
        for i in days:
            end_date = i + datetime.timedelta(hours=24)
            rospy.loginfo(
                "Load region observation time from %s to %s..." % (str(i), str(end_date))
            )
            query = {
                "soma": self.soma_map, "soma_config": self.soma_config,
                "start_from.secs": {
                    "$gte": time.mktime(i.timetuple()),
                    "$lt": time.mktime(end_date.timetuple())
                }
            }
            roi_reg_hourly = dict()
            for log in self.ms.query(RegionObservationTime._type, message_query=query):
                end = datetime.datetime.fromtimestamp(log[0].until.secs)
                if log[0].region_id not in roi_reg_hourly:
                    temp = list()
                    start = datetime.datetime.fromtimestamp(log[0].start_from.secs)
                    interval = (end.minute + 1) - start.minute
                    if interval != minute_interval:
                        continue
                    for j in range(24):
                        group_mins = {k*interval: 0 for k in range(1, (60/interval)+1)}
                        temp.append(group_mins)
                        roi_reg_hourly.update({log[0].region_id: temp})
                roi_reg_hourly[log[0].region_id][end.hour][end.minute+1] = log[0].duration.secs
                roi_reg_hourly[log[0].region_id][end.hour][end.minute+1] += 0.000000001 * log[0].duration.nsecs
                total_duration += log[0].duration.secs
            roi_region_daily.update({i.day: roi_reg_hourly})
        self.region_observation_duration = roi_region_daily
        return roi_region_daily, total_duration

    def store_to_mongo(self):
        """
            Store region observation time from self.region_observation_duration to mongodb.
            It will store soma map, soma configuration, region_id, the starting and end time where
            robot sees a region in some interval, and the duration of how long the robot during
            the interval.
        """
        rospy.loginfo("Storing region observation time to region_observation_time collection...")
        data = self.region_observation_duration
        try:
            minute_interval = self.minute_interval
        except:
            rospy.logwarn("Minute interval is not defined, please call either load_from_mongo or calculate_region_observation_duration first")
            return

        for day, daily_data in data.iteritems():
            for reg, reg_data in daily_data.iteritems():
                for hour, hourly_data in enumerate(reg_data):
                    for minute, duration in hourly_data.iteritems():
                        date_until = datetime.datetime(
                            self._month_year_observation_taken[day][1],
                            self._month_year_observation_taken[day][0],
                            day, hour, minute-1,
                            59
                        )
                        until = time.mktime(date_until.timetuple())
                        start_from = until - (60 * minute_interval) + 1
                        msg = RegionObservationTime(
                            self.soma_map, self.soma_config, reg,
                            rospy.Time(start_from), rospy.Time(until),
                            rospy.Duration(duration)
                        )
                        self._store_to_mongo(msg)

    def _store_to_mongo(self, msg):
        query = {
            "soma": msg.soma, "soma_config": msg.soma_config,
            "region_id": msg.region_id, "start_from.secs": msg.start_from.secs,
            "until.secs": msg.until.secs
        }
        if msg.duration.nsecs > 0:
            if len(self.ms.query(RegionObservationTime._type, message_query=query)) > 0:
#.........这里部分代码省略.........

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]
class TrajectoryOccurrenceFrequencies(object):

    def __init__(
        self, soma_map, soma_config, minute_interval=1,
        window_interval=10, periodic_type="weekly"
    ):
        """
            Initialize a set of trajectory occurrence frequency classified by regions, days, hours, minute intervals respectively.
            Hours will be set by default to [0-23]. Days are set as days of a week [0-6] where Monday is 0.
            Argument periodic_type can be set either 'weekly' or 'monthly'.
        """
        self.soma = soma_map
        self.soma_config = soma_config
        self.periodic_type = periodic_type
        if self.periodic_type == "weekly":
            self.periodic_days = [i for i in range(7)]
        else:
            self.periodic_days = [i for i in range(31)]
        self.minute_interval = minute_interval
        self.window_interval = window_interval
        self.ms = MessageStoreProxy(collection="occurrence_rates")
        self.reinit()

    def reinit(self):
        self.tof = dict()

    def load_tof(self):
        """
            Load trajectory occurrence frequency from mongodb occurrence_rates collection.
        """
        rospy.loginfo("Retrieving trajectory occurrence frequencies from database...")
        query = {
            "soma": self.soma, "soma_config": self.soma_config,
            "periodic_type": self.periodic_type
        }
        logs = self.ms.query(OccurrenceRate._type, message_query=query)
        if len(logs) == 0:
            rospy.logwarn(
                "No data for %s with config %s and periodicity type %s" % (
                    self.soma, self.soma_config, self.periodic_type
                )
            )
            return

        for i in logs:
            same_hour = (i[0].end_hour == i[0].start_hour)
            within_interval = (i[0].end_hour == (i[0].start_hour+1) % 24) and (i[0].end_minute - i[0].start_minute) % 60 == self.window_interval
            if same_hour or within_interval:
                if i[0].region_id not in self.tof:
                    self.init_region_tof(i[0].region_id)
                key = "%s-%s" % (
                    datetime.time(i[0].start_hour, i[0].start_minute).isoformat()[:-3],
                    datetime.time(i[0].end_hour, i[0].end_minute).isoformat()[:-3]
                )
                if key in self.tof[i[0].region_id][i[0].day]:
                    self.tof[i[0].region_id][i[0].day][key].occurrence_shape = i[0].occurrence_shape
                    self.tof[i[0].region_id][i[0].day][key].occurrence_scale = i[0].occurrence_scale
                    self.tof[i[0].region_id][i[0].day][key].set_occurrence_rate(i[0].occurrence_rate)
        rospy.loginfo("Retrieving is complete...")

    def update_tof_daily(self, curr_day_data, prev_day_data, curr_date):
        """
            Update trajectory occurrence frequency for one day. Updating the current day,
            tof needs information regarding the number of trajectory from the previous day as well.
            The form for curr_day_data and prev_day_data is {reg{date[hour{minute}]}}.
        """
        rospy.loginfo("Daily update for trajectory occurrence frequency...")
        for reg, hourly_traj in curr_day_data.iteritems():
            date = curr_date.day
            if self.periodic_type == "weekly":
                date = curr_date.weekday()
            prev_day_n_min_traj = previous_n_minutes_trajs(
                prev_day_data[reg], self.window_interval, self.minute_interval
            )
            self._update_tof(reg, date, hourly_traj, prev_day_n_min_traj)
        rospy.loginfo("Daily update is complete...")

    def _update_tof(self, reg, date, hourly_traj, prev_n_min_traj):
        length = (self.window_interval / self.minute_interval)
        temp_data = prev_n_min_traj + [-1]
        pointer = length - 1
        for hour, mins_traj in enumerate(hourly_traj):
            minutes = sorted(mins_traj)
            for mins in minutes:
                traj = mins_traj[mins]
                temp_data[pointer % length] = traj
                pointer += 1
                if reg not in self.tof:
                    self.init_region_tof(reg)
                if sum(temp_data) == (-1 * length):
                    continue
                else:
                    total_traj = length / float(length + sum([i for i in temp_data if i == -1]))
                    total_traj = math.ceil(total_traj * sum([i for i in temp_data if i != -1]))
                temp = [
                    hour + (mins - self.window_interval) / 60,
                    (mins - self.window_interval) % 60
                ]
                hour = (hour + (mins/60)) % 24
                key = "%s-%s" % (
#.........这里部分代码省略.........

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import update [as 别名]

#.........这里部分代码省略.........
        if vis:
            print ">>>"
            print "storing: ", uuid, type(uuid)
            print "date: ", self.date  # , type(self.date)
            print "number of detectons: ", len(self.accumulate_data[uuid])  # , type(len(self.accumulate_data[uuid]))
            print "map info: %s. (x,y) Position: (%s,%s)" % (
                self.map_info,
                first_map_point.x,
                first_map_point.y,
            )  # , type(self.map_info)
            print "current node: ", self.current_node, type(self.current_node)
            print "start/end rostime:", st, en  # type(st), en, type(en)

        msg = SkeletonComplete(
            uuid=uuid,
            date=self.date,
            time=self.sk_mapping[uuid]["time"],
            skeleton_data=self.accumulate_data[uuid],
            number_of_detections=len(self.accumulate_data[uuid]),
            map_name=self.map_info,
            current_topo_node=self.current_node,
            start_time=st,
            end_time=en,
            robot_data=self.accumulate_robot[uuid],
            human_map_point=first_map_point,
        )

        self.publish_comp.publish(msg)
        rospy.loginfo("User #%s: published %s msgs as %s" % (subj, len(self.accumulate_data[uuid]), msg.uuid))

        if self._with_logging:
            query = {"uuid": msg.uuid}
            # self._store_client.insert(traj_msg, meta)
            self._store_client.update(message=msg, message_query=query, upsert=True)

        # remove the user from the users dictionary and the accumulated data dict.
        del self.accumulate_data[uuid]
        del self.sk_mapping[uuid]

    def _dump_images(self, msg):
        """For each incremental skeleton - dump an rgb/depth image to file"""

        self.inc_sk = msg
        if str(datetime.datetime.now().date()) != self.date:
            print "new day!"
            self.date = str(datetime.datetime.now().date())
            self.dir1 = "/home/" + getpass.getuser() + "/SkeletonDataset/no_consent/" + self.date + "/"
            print "checking if folder exists:", self.dir1
            if not os.path.exists(self.dir1):
                print "  -create folder:", self.dir1
                os.makedirs(self.dir1)

        if self.sk_mapping[msg.uuid]["state"] is "Tracking" and self.sk_mapping[msg.uuid]["frame"] is 1:

            self.sk_mapping[msg.uuid]["time"] = str(datetime.datetime.now().time()).split(".")[0]
            t = self.sk_mapping[msg.uuid]["time"] + "_"
            print "  -new skeleton detected with id:", msg.uuid
            new_dir = self.dir1 + self.date + "_" + t + msg.uuid  # +'_'+waypoint
            # print "new", new_dir

            if not os.path.exists(new_dir):
                os.makedirs(new_dir)
                os.makedirs(new_dir + "/depth")
                os.makedirs(new_dir + "/robot")
                os.makedirs(new_dir + "/skeleton")
                if self.record_rgb: