Python MessageStoreProxy.insert方法代码示例

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
    def gen_blog_entry(self, roi_id, obj_id, objs):

        print 'Region: ' + self.get_roi_name(roi_id)
        time = dt.fromtimestamp(int(rospy.get_time()))
        body = '### CHANGE DETECTION REPORT\n\n'
        body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n'
        #body += '- **Object ID:** ' + str(obj_id)  + '\n\n'
        body += '- **Time:** ' + str(time)  + '\n\n'

        # Create some blog entries
        msg_store = MessageStoreProxy(collection=self.blog_collection)
        robblog_path = roslib.packages.get_pkg_dir('soma_utils') 

        for idx,obj in enumerate(objs):
            
            bridge = CvBridge()
            im = bridge.imgmsg_to_cv2(obj.image_mask, desired_encoding="bgr8")
            imgray = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)
            ret,thresh = cv2.threshold(imgray,1,1,1) #cv2.threshold(imgray,127,255,0)
            contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

            bridge = CvBridge()
            cv_image = bridge.imgmsg_to_cv2(obj.image_full, desired_encoding="bgr8")
            
            cv2.drawContours(cv_image,contours,-1,(0,0,255),2)
            full_scene_contour = bridge.cv2_to_imgmsg(cv_image)
            img_id = msg_store.insert(full_scene_contour)

            body += '<font color="red">Detected change (' + str(idx+1) + '/'+ str(len(objs)) + '):</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id
            
        e = RobblogEntry(title=str(time) + " Change Detection - " + self.roi_name[roi_id], body= body )
        msg_store.insert(e)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
    def gen_blog_entry(self, roi_id, uuid):

        print 'Region: ' + self.get_roi_name(roi_id)
        time = dt.fromtimestamp(int(rospy.get_time()))
        body = '### INTRUSION DETECTION REPORT\n\n'
        body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n'
        body += '- **Person UUID:** ' + str(uuid)  + '\n\n'
        body += '- **Time:** ' + str(time)  + '\n\n'
        #body += '- **Summary**: <font color="green">ALLOWED ITEMS (' + str(len(pos_objs)) + ')</font>, <font color="red">NOT-ALLOWED ITEMS (' + str(len(neg_objs)) + ')</font>\n\n'


        # # Create some blog entries
        msg_store = MessageStoreProxy(collection=self.blog_collection)
        robblog_path = roslib.packages.get_pkg_dir('soma_utils') 

        img = rospy.wait_for_message('/upper_body_detector/image', Image, 5)

        bridge = CvBridge()
        cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="bgr8")
        ros_img = bridge.cv2_to_imgmsg(cv_image)
        img_id = msg_store.insert(ros_img)
        body += '<font color="red">Detected person:</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id

        e = RobblogEntry(title=str(time) + " Intrusion Detection - " + self.get_roi_name(roi_id), body= body )
        msg_store.insert(e)

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


    def __init__(self, name):

        rospy.loginfo(" ...starting")

        self.msg_sub = rospy.Subscriber('/strands_tweets/tweet', strands_tweets.msg.Tweeted, self.tweet_callback, queue_size=1)

        rospy.loginfo(" ...done")


        rospy.spin()

    def tweet_callback(self, msg) :
        self.msg_store = MessageStoreProxy(collection='twitter_log')
        
        to_save = strands_tweets.msg.Tweeted()
        try:
            to_save.depth = rospy.wait_for_message('/head_xtion/depth/image_rect_meters', sensor_msgs.msg.Image, timeout=1.0)
        except rospy.ROSException :
           rospy.logwarn("Failed to get camera depth Image")


        try:
            to_save.photo = rospy.wait_for_message('/head_xtion/rgb/image_color', sensor_msgs.msg.Image, timeout=1.0)
        except rospy.ROSException :
            rospy.logwarn("Failed to get camera rgb Image")

        to_save.text = msg.text
        
        meta = {}
        meta["Description"] = "copy of tweeted images"
        self.msg_store.insert(msg, meta)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
    def add_topological_node(self, node_name, node_pose):
        dist = 8.0
        #Get New Node Name
        if node_name:
            name = node_name
        else:
            name = self.get_new_name()

        rospy.loginfo('Creating Node: '+name)        

        if name in self.names:
            rospy.logerr("Node already exists, try another name")
            return False
        
        #Create Message store
        msg_store = MessageStoreProxy(collection='topological_maps')

        meta = {}
        meta["map"] = self.nodes.map
        meta["pointset"] = self.nodes.name
        meta["node"] = name

        node = strands_navigation_msgs.msg.TopologicalNode()
        node.name = name
        node.map = self.nodes.map
        node.pointset = self.name
        node.pose = node_pose
        node.yaw_goal_tolerance = 0.1
        node.xy_goal_tolerance = 0.3
        node.localise_by_topic = ''
        vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)]
        for j in vertices :
            v = strands_navigation_msgs.msg.Vertex()
            v.x = float(j[0])
            v.y = float(j[1])
            node.verts.append(v)

        close_nodes = []
        for i in self.nodes.nodes :
            ndist = node_dist(node, i)
            if ndist < dist :
                if i.name != 'ChargingPoint' :
                    close_nodes.append(i.name)

        for i in close_nodes:
            e = strands_navigation_msgs.msg.Edge()
            e.node = i
            e.action = 'move_base'
            eid = '%s_%s' %(node.name, i)
            e.edge_id = eid
            e.top_vel =0.55
            e.map_2d = node.map
            node.edges.append(e)

        for i in close_nodes:
            self.add_edge(i, node.name, 'move_base', '')

        msg_store.insert(node,meta)
        return True

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
class PeriodicReplicatorClient(Thread):
    def __init__(self):
        Thread.__init__(self)
        self.dead = Event()
        self.interval = rospy.get_param("mongodb_replication_interval", 60 * 60 * 24) # default: 1 day
        self.delete_after_move = rospy.get_param("mongodb_replication_delete_after_move", False)
        self.replicate_interval = rospy.Duration(self.interval)
        self.database = rospy.get_param("robot/database")
        self.collections = sys.argv[2:]
        try:
            self.collections.append(rospy.get_param("robot/name"))
        except KeyError as e:
            rospy.logerr("specify param \"robot/name\" (e.g. pr1012, olive)")
            exit(1)
        self.periodic = rospy.get_param("~periodic", True)
        self.date_msg_store = MessageStoreProxy(database=self.database,
                                                collection="replication")

    def run(self):
        while not self.dead.wait(self.interval):
            move_before = self.time_after_last_replicate_date()
            self.move_entries(move_before)
            self.insert_replicate_date()

    def time_after_last_replicate_date(self):
        delta = 60 * 60 * 24 # 1 day
        try:
            last_replicated = self.date_msg_store.query(StringList._type, single=True, sort_query=[("$natural",-1)])
            date = last_replicated[1]["inserted_at"]
            rospy.loginfo("last replicated at %s", date)
            delta = (dt.datetime.now() - date).seconds + 60
        except Exception as e:
            rospy.logwarn("failed to search last replicated date from database: %s", e)
        finally:
            return rospy.Duration(delta)

    def insert_replicate_date(self):
        try:
            self.date_msg_store.insert(StringList(self.collections))
        except Exception as e:
            rospy.logwarn("failed to insert last replicate date to database: %s", e)

    def move_entries(self, move_before):
        client = actionlib.SimpleActionClient('move_mongodb_entries', MoveEntriesAction)
        client.wait_for_server()
        goal = MoveEntriesGoal(database=self.database,
                               collections=StringList(self.collections),
                               move_before=move_before,
                               delete_after_move=self.delete_after_move)
        client.send_goal(goal, feedback_cb=self.feedback_cb)
        client.wait_for_result()

    def feedback_cb(self, feedback):
        rospy.loginfo(feedback)

    def cancel(self):
        self.dead.set()

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
    def test_add_message(self):
        msg_store = MessageStoreProxy()
        POSE_NAME = "__test__pose__"
        p = Pose(Point(0, 1, 2), Quaternion(0, 0, 0, 1))
      
        # insert a pose object with a name
        msg_store.insert_named(POSE_NAME, p)
 
        # get it back with a name
        stored, meta = msg_store.query_named(POSE_NAME, Pose._type)

        self.assertIsInstance(stored, Pose)
        self.assertEqual(stored.position.x, p.position.x)
        self.assertEqual(stored.position.y, p.position.y)
        self.assertEqual(stored.position.z, p.position.z)

        self.assertEqual(stored.orientation.x, p.orientation.x)
        self.assertEqual(stored.orientation.y, p.orientation.y)
        self.assertEqual(stored.orientation.z, p.orientation.z)
        self.assertEqual(stored.orientation.w, p.orientation.w)

        p.position.x = 666

        msg_store.update_named(POSE_NAME, p)

        # get it back with a name
        updated = msg_store.query_named(POSE_NAME, Pose._type)[0]

        self.assertEqual(updated.position.x, p.position.x)

        # # try to get it back with an incorrect name
        wrong_name = "thid name does not exist in the datacentre"
        none_item = msg_store.query_named(wrong_name, Pose._type)[0]
        self.assertIsNone(none_item)

        # # get all non-existant typed objects, so get an empty list back
        none_query = msg_store.query( "not my type")
        self.assertEqual(len(none_query), 0)

        # add 100 query and sort by date inserted.
        for i in range(100):
            p = Pose(Point(0, 0, 0), Quaternion(i, 0, 100, 1))
            msg_store.insert(p)
            
        result = msg_store.query(Pose._type, message_query={ 'orientation.z': {'$gt': 10} }, sort_query=[("$natural", -1)])
        self.assertEqual(len(result), 100)
        self.assertEqual(result[0][0].orientation.x, 99)
        
        # must remove the item or unittest only really valid once
        print meta["_id"]
        print str(meta["_id"])
        deleted = msg_store.delete(str(meta["_id"]))
        self.assertTrue(deleted)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
    def add_node(self, name, dist, pos, std_action) :
        rospy.loginfo('Creating Node: '+name)
        msg_store = MessageStoreProxy(collection='topological_maps')

        found = False
        for i in self.nodes :
            if i.name == name :
                found = True

        if found :
            rospy.logerr("Node already exists, try another name")
        else :
            rospy.loginfo('Adding node: '+name)

            meta = {}
            meta["map"] = self.map
            meta["pointset"] = self.name
            meta["node"] = name

            node = TopologicalNode()
            node.name = name
            node.map = self.map
            node.pointset = self.name
            node.pose = pos
            vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)]
            for j in vertices :
                v = Vertex()
                v.x = float(j[0])
                v.y = float(j[1])
                node.verts.append(v)


            cx = node.pose.position.x
            cy = node.pose.position.y
            close_nodes = []
            for i in self.nodes :
                ndist = i._get_distance(cx, cy)
                if ndist < dist :
                    if i.name != 'ChargingPoint' :
                        close_nodes.append(i.name)

            for i in close_nodes :
                edge = Edge()
                edge.node = i
                edge.action = std_action
                node.edges.append(edge)

            msg_store.insert(node,meta)

            for i in close_nodes :
                self.add_edge(i, name, std_action)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
class LoggerBase(object):
    def __init__(self, db_name='jsk_robot_lifelog', col_name=None):
        self.db_name = rospy.get_param('robot/database','jsk_robot_lifelog')
        try:
            if col_name is None:
                self.col_name = rospy.get_param('robot/name')
            else:
                self.col_name = col_name
        except KeyError as e:
            rospy.logerr("please specify param \"robot/name\" (e.g. pr1012, olive)")
            exit(1)
        self.update_cycle = rospy.get_param("update_cycle", 1)

        self.task_id = None

        self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name)
        rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name))

    def insert(self, msg, meta={}, wait=False):
        if self.task_id is not None:
            meta.update({ "task_id": self.task_id })
        return self.msg_store.insert(msg, meta, wait=wait)

    def spinOnce(self):
        rospy.sleep(self.update_cycle)
        self.task_id = rospy.get_param("/task_id", None)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
def update_maps(to_update, client):
    db=client.message_store
    collection=db["topological_maps"]
#    available = collection.find().distinct("_meta.pointset")

    msg_store = MessageStoreProxy(collection='topological_maps')

    del_ids = []
    for pointset in to_update:
        #pointsetb='%s_b'%pointset
        #print pointsetb
        search = {"_meta.pointset": pointset}
        av =collection.find(search)
        #lnodes=[]
        eids=[] #list of known edge id's
        for a in av:
            #print a
            bc = update_node(a, pointset)
    
            nna = a['name']
            nma = a['map']
            vt = a['verts']
            if a['edges']:
                es = a['edges']
                for i in es:
                    ed, eids = update_edge(i, nna, nma, eids)
                    bc.edges.append(ed)
                
            for i in vt:
                v = update_vert(i)
                bc.verts.append(v)
            
            meta = update_meta(a['_meta'], pointset)
            
            #print bc
            #print meta
            del_ids.append(a['_id'])
            #lnodes.append(bc)
            msg_store.insert(bc,meta)
        #print lnodes
    
    print "Deleting updated nodes"
    for i in del_ids:
        msg_store.delete(str(i))
    print "done"    

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
class People_Tracker_Log():
    def __init__(self, people_tracker_topic, dataset_name):
        rospy.loginfo("Initialising Logging " + people_tracker_topic)
        self.pt = PeopleTracker()
        self.rp = Pose()
        self.dataset_name = dataset_name 
        self.topic = people_tracker_topic
        self.msg_store = MessageStoreProxy(collection="people_perception_morse")
        # Subscribing to global poses of human avatars
        rospy.Subscriber(people_tracker_topic, PeopleTracker,
                self.pt_callback)
        # Subscribing to robot pose
        rospy.Subscriber('/robot_pose', PoseStamped, self.rp_callback)
        # Publishing the log
        self.pub = rospy.Publisher('morse_people_tracker_log', Logging, queue_size=10)
        self.seq = 0

    def pt_callback(self,data):
        self.pt = data

    def rp_callback(self,data):
        self.rp = data.pose

    def log(self):
        if len(self.pt.distances) == 0:
            return
        meta = dict() 
        meta['perspective'] = self.dataset_name 
        rospy.logdebug("Person detected for " + self.topic)
        message = Logging()
        message.header.seq = self.seq
        message.header.stamp = rospy.Time.now()
        message.header.frame_id = '/map'
        message.uuids = self.pt.uuids
        message.people = self.pt.poses
        message.people_tracker = self.pt
        message.robot = self.rp
        self.msg_store.insert(message, meta)
        self.pub.publish(message)
        self.seq += 1 

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
    def publish_stats(self):
        pubst = NavStatistics()
        pubst.edge_id = self.stat.edge_id
        pubst.status = self.stat.status
        pubst.origin = self.stat.origin
        pubst.target = self.stat.target
        pubst.topological_map = self.stat.topological_map
        pubst.final_node = self.stat.final_node
        pubst.time_to_waypoint = self.stat.time_to_wp
        pubst.operation_time = self.stat.operation_time
        pubst.date_started = self.stat.get_start_time_str()
        pubst.date_at_node = self.stat.date_at_node.strftime('%A, %B %d %Y, at %H:%M:%S hours')
        pubst.date_finished = self.stat.get_finish_time_str()
        self.stats_pub.publish(pubst)

        meta = {}
        meta["type"] = "Topological Navigation Stat"
        meta["epoch"] = calendar.timegm(self.stat.date_at_node.timetuple())
        meta["date"] = self.stat.date_at_node.strftime('%A, %B %d %Y, at %H:%M:%S hours')
        meta["pointset"] = self.stat.topological_map

        msg_store = MessageStoreProxy(collection='nav_stats')
        msg_store.insert(pubst,meta)

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
def create_datacentre_task(to_replicate, delete_after_move=True):
    task = Task()
    # no idea, let's say 2 hours for now -- this action server can't be preempted though, so this is cheating
    task.max_duration = rospy.Duration(60 * 60 * 2)
    task.action = 'move_mongodb_entries'

    # add arg for collectionst o replication
    collections = StringList(to_replicate)
    msg_store = MessageStoreProxy()
    object_id = msg_store.insert(collections)
    task_utils.add_object_id_argument(task, object_id, StringList)
    # move stuff over 24 hours old
    task_utils.add_duration_argument(task, rospy.Duration(60 * 60 *24))
    # and delete afterwards
    task_utils.add_bool_argument(task, delete_after_move)
    return task

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
class PeriodicReplicatorClient(Thread):
    def __init__(self):
        Thread.__init__(self)
        self.dead = Event()
        self.replicate_interval = rospy.get_param("replication/interval", 60 * 60 * 24) # default: 1 day
        self.delete_after_move = rospy.get_param("replication/delete_after_move", False)
        self.database = rospy.get_param("robot/database")
        self.collections = rospy.myargv()[1:]
        try:
            self.collections.append(rospy.get_param("robot/name"))
        except KeyError as e:
            rospy.logerr("specify param \"robot/name\" (e.g. pr1012, olive)")
            exit(1)

        self.periodic = rospy.get_param("replication/periodic", True)
        if self.periodic:
            rospy.loginfo("periodic replication interval: %d [sec]", self.replicate_interval)
            self.disable_on_wireless_network = rospy.get_param("replication/disable_on_wireless_network", False)
            if self.disable_on_wireless_network:
                self.network_connected = False
                self.net_sub = rospy.Subscriber("/network/connected", Bool, self.network_connected_cb)
        else:
            self.replicate_interval = 1

        self.date_msg_store = MessageStoreProxy(database=self.database,
                                                collection="replication")
        self.replicate_ac = actionlib.SimpleActionClient('move_mongodb_entries', MoveEntriesAction)
        rospy.loginfo("waiting for service advertise /move_mongodb_entries ...")
        self.replicate_ac.wait_for_server()
        rospy.loginfo("replication enabled: db: %s, collection: %s, periodic: %s",
                      self.database, self.collections, self.periodic)

    def run(self):
        while not self.dead.wait(self.replicate_interval):
            if self.disable_on_wireless_network and not self.network_connected:
                rospy.loginfo("no wired network connection. skipping replication...")
            else:
                move_before = self.time_after_last_replicate_date()
                self.move_entries(move_before)
                self.insert_replicate_date()
                if not self.periodic:
                    self.exit()

    def time_after_last_replicate_date(self):
        delta = 0
        try:
            last_replicated = self.date_msg_store.query(StringList._type, single=True, sort_query=[("$natural",-1)])
            date = last_replicated[1]["inserted_at"]
            rospy.loginfo("last replicated at %s", date)
        except Exception as e:
            rospy.logwarn("failed to search last replicated date from database: %s", e)
        finally:
            return rospy.Duration(delta)

    def insert_replicate_date(self):
        try:
            self.date_msg_store.insert(StringList(self.collections))
        except Exception as e:
            rospy.logwarn("failed to insert last replicate date to database: %s", e)

    def move_entries(self, move_before):
        goal = MoveEntriesGoal(database=self.database,
                               collections=StringList(self.collections),
                               move_before=move_before,
                               delete_after_move=self.delete_after_move)
        self.replicate_ac.send_goal(goal,
                                    active_cb=self.active_cb,
                                    feedback_cb=self.feedback_cb)
        while not self.replicate_ac.wait_for_result(timeout=rospy.Duration(5.0)):
            if self.disable_on_wireless_network and not self.network_connected:
                rospy.loginfo("disconnected wired network connection. canceling replication...")
                self.cancel()


    def active_cb(self):
        if self.disable_on_wireless_network and not self.network_connected:
                rospy.loginfo("disconnected wired network connection. canceling replication...")
                self.cancel()

    def feedback_cb(self, feedback):
        rospy.loginfo(feedback)

    def cancel(self):
        self.replicate_ac.cancel_all_goals()

    def exit(self):
        self.cancel()
        self.dead.set()

    def network_connected_cb(self, msg):
        self.network_connected = msg.data

# 需要导入模块: from mongodb_store.message_store import MessageStoreProxy [as 别名]
# 或者: from mongodb_store.message_store.MessageStoreProxy import insert [as 别名]
class YamlMapLoader(object):
    def __init__(self):
        self.msg_store = MessageStoreProxy(collection='topological_maps')

    def get_maps(self):
        """
        Queries the database and returns details of the available topological maps.
        :return: A dictionary where each key is the name of a topological map and each
        item is a dictionary of details. Details are:
           "number_nodes" ; integer
            "edge_actions" : list of action server names used for traversal
            "last_modified" : datetime.datetime object for the last time a node was inserted
        """
        maps = dict()

        nodes = self.msg_store.query(TopologicalNode._type)

        for node in nodes:
            pointset = node[1]["pointset"]
            if not maps.has_key(pointset):
                maps[pointset] = {"number_nodes": 0, "edge_actions": set(), "last_modified": ""}
            maps[pointset]["number_nodes"] += 1
            if (maps[pointset]["last_modified"] == "" or
                        node[1]["inserted_at"] > maps[pointset]["last_modified"]):
                maps[pointset]["last_modified"] = node[1]["inserted_at"]
            for edge in node[0].edges:
                maps[pointset]["edge_actions"].add(edge.action)

        return maps

    def _load_yaml(self, filename):
        rospy.loginfo("loading %s"%filename)
        with open(filename, 'r') as f:
            return yaml.load(f)

    def read_maps(self, p):
        data = []
        if os.path.isdir(p):
            for f in os.listdir(p):
                if f.endswith(".yaml"):
                    data.append(self._load_yaml(p+'/'+f))
        else:
            data.append(self._load_yaml(p))
        return data

    def insert_maps(self, data, new_pointset=None, force=False):
        current_maps = self.get_maps()
        for idx, tmap in enumerate(data):
            pointset = None
            if new_pointset != None: # If there are more than one map, it takes the custom pointset and appends an index
                pointset = new_pointset+str(idx+1) if len(data) > 1 else new_pointset
            first_node = True
            for i in tmap:
                try:
                    meta = i['meta']
                    meta['pointset'] = pointset if pointset != None else meta['pointset']
                    if meta['pointset'] in current_maps and first_node:
                        first_node = False
                        if not force:
                            rospy.logwarn("Map '%s' already in datacentre, skipping! Use -f to force override or change pointset name with --pointset" % meta['pointset'])
                            break
                        else:
                            topo_map = topological_map(meta['pointset'])
                            topo_map.delete_map()
                    elif first_node:
                        first_node = False
                        rospy.loginfo("Inserting map: %s" % meta['pointset'])
                    msgv = dc_util.dictionary_to_message(i['node'], TopologicalNode)
                    self.msg_store.insert(msgv,meta)
                except TypeError:
                    pass # Not a topo map

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

    def __init__(self, soma_map, soma_conf, config_file=None):

        self.soma_map = soma_map
        self.soma_conf = soma_conf
        if config_file:
            self._config_file = config_file
        else:
            # default file
            rp = RosPack()
            path = rp.get_path('soma_roi_manager') + '/config/'
            filename = 'default.json'
            self._config_file=path+filename
        self._soma_obj_ids = dict()
        self._soma_obj_msg = dict()
        self._soma_obj_roi_ids = dict()
        self._soma_obj_roi = dict()
        self._soma_obj_type = dict()
        self._soma_obj_pose = dict()

        self._interactive = True

        self._msg_store=MessageStoreProxy(collection="soma_roi")

        self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
        
        self._server = InteractiveMarkerServer("soma_roi")

        self._init_types()

        self._init_menu()
        
        self.load_objects()

        rospy.spin()


    def _init_types(self):
        # read from config in soma_objects 
        
        with open(self._config_file) as config_file:
            config = json.load(config_file)

            self.mesh = dict()
            for k, v in config['roi'].iteritems():
                self.mesh[k] = v

    def _init_menu(self):

        self.menu_handler = MenuHandler()

        add_point_entry = self.menu_handler.insert( "Add Point", callback=self._add_point_cb)
        del_point_entry = self.menu_handler.insert( "Delete Point", callback=self._del_point_cb)


        add_entry = self.menu_handler.insert( "Add ROI" )

        self.menu_item = dict()
        for k in sorted(self.mesh.keys()):
            entry =  self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
            self.menu_item[entry] = k
            
        del_entry =  self.menu_handler.insert( "Delete ROI", callback=self._del_cb)

        
        enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
        self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )

    def _add_cb(self, feedback):
        rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
        self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)

    def _del_cb(self, feedback):
        rospy.loginfo("Delete ROI: %s", feedback.marker_name)
        roi = self._soma_obj_roi[feedback.marker_name]
        for r in self._soma_obj_roi_ids[roi]:
            self.delete_object(r)
        self.undraw_roi(roi)

    def _add_point_cb(self, feedback):
        rospy.loginfo("Add point: %s", feedback.marker_name)
        roi = self._soma_obj_roi[feedback.marker_name]
        t   = self._soma_obj_type[feedback.marker_name]
        self.add_object(t, feedback.pose, roi)
        #self.draw_roi(roi)

    def _del_point_cb(self, feedback):
        rospy.loginfo("Delete point: %s", feedback.marker_name)
        self.delete_object(feedback.marker_name)
        roi = self._soma_obj_roi[feedback.marker_name]
        self.draw_roi(roi)

    def _update_poly(self, feedback):
        return
    
    def _update_cb(self, feedback):
        p = feedback.pose.position
        #print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
        self._soma_obj_pose[feedback.marker_name] = feedback.pose
#.........这里部分代码省略.........