perception_base.py 文件源码

def __init__(self, node_name):
        rospy.init_node(node_name, anonymous=False)

        try:
            conf_file = rospy.get_param('~config_file')
        except KeyError as e:
            rospy.logerr('You should set the parameter for perception config file...')
            quit()

        with open(os.path.abspath(conf_file)) as f:
            self.conf_data = yaml.load(f.read())
            rospy.loginfo('loading perception config file... %d perception(s) exists...'%len(self.conf_data.keys()))
            for item in self.conf_data.keys():
                rospy.loginfo('\033[92m  - %s: %d event(s) and %d data(s).\033[0m'%(item, len(self.conf_data[item]['events']), len(self.conf_data[item]['data'])))

        self.dict_srv_wr = {}
        self.dict_srv_rd = {}

        for item in self.conf_data.keys():
            if self.conf_data[item].has_key('target_memory'):
                memory_name = self.conf_data[item]['target_memory']
                rospy.loginfo('\033[94m[%s]\033[0m wait for bringup %s...'%(rospy.get_name(), memory_name))

                rospy.wait_for_service('/%s/write_data'%memory_name)
                self.dict_srv_wr[memory_name] = rospy.ServiceProxy('/%s/write_data'%memory_name, WriteData)
                self.dict_srv_rd[memory_name] = rospy.ServiceProxy('/%s/read_data'%memory_name, ReadData)

                self.register_data_to_memory(memory_name, item, self.conf_data[item]['data'])

        self.is_enable_perception = True
        rospy.Subscriber('%s/start'%rospy.get_name(), Empty, self.handle_start_perception)
        rospy.Subscriber('%s/stop'%rospy.get_name(), Empty, self.handle_stop_perception)

        self.pub_event = rospy.Publisher('forwarding_event', ForwardingEvent, queue_size=10)
        rospy.loginfo('\033[94m[%s]\033[0m initialize perception_base done...'%rospy.get_name())