def testconfig():
    if Storage.config is None:
        config = DiffeoplanConfigMaster()
        config.load()
        set_current_config(config)
        
    return get_current_config()
    def init_plan(self):
        '''
        load parameters from ROS parameter server and activate a online planner 
        from the given parameters.
        '''
        print('init_plan...')
        config_path = rospy.get_param(self.node_name + '/config_path')
        
        config = DiffeoplanConfigMaster()
        config.load(config_path)
        set_current_config(config)
        print('...')
        
        id_discdds = rospy.get_param(self.node_name + '/id_discdds')
        id_algo = rospy.get_param(self.node_name + '/id_algo')
        print('...')
#        pdb.set_trace()
        self.discdds = config.discdds.instance(id_discdds)
        self.algo = config.algos.instance(id_algo)
        self.algo.plan_time = 0.5 / self.system['frequency']
        self.algo.init(id_discdds, self.discdds)
        print('...') 
        
        size = self.discdds.actions[0].diffeo.get_shape()
        self.size = (size[1], size[0]) 
        print('planner initiated')
Exemple #3
0
    def init_plan(self):
        '''
        load parameters from ROS parameter server and activate a online planner 
        from the given parameters.
        '''
        print('init_plan...')
        config_path = rospy.get_param(self.node_name + '/config_path')

        config = DiffeoplanConfigMaster()
        config.load(config_path)
        set_current_config(config)
        print('...')

        id_discdds = rospy.get_param(self.node_name + '/id_discdds')
        id_algo = rospy.get_param(self.node_name + '/id_algo')
        print('...')
        #        pdb.set_trace()
        self.discdds = config.discdds.instance(id_discdds)
        self.algo = config.algos.instance(id_algo)
        self.algo.plan_time = 0.5 / self.system['frequency']
        self.algo.init(id_discdds, self.discdds)
        print('...')

        size = self.discdds.actions[0].diffeo.get_shape()
        self.size = (size[1], size[0])
        print('planner initiated')
Exemple #4
0
    def load_diffeoplan(self):
        '''
        load parameters from ROS parameter server and activate a online planner 
        from the given parameters.
        '''
        config_path = rospy.get_param(self.node_name + '/config_path')
        
        config = DiffeoplanConfigMaster()
        config.load(config_path)
        set_current_config(config)
        
        id_discdds = rospy.get_param(self.node_name + '/id_discdds')
        id_algo = rospy.get_param(self.node_name + '/id_algo')

        diffeo_structure_threshold = 0.2
        
        # Check for threshold params, if evaluated to a list of tuple, then 
        # give the list or tuple as thresholds.
        thresholds = rospy.get_param(self.node_name + '/planning_thresholds')
        try:
            if eval(thresholds).__class__ in [list, tuple]:
                get_planning_thresholds = eval(thresholds)
            else:
                get_planning_thresholds = thresholds
        except:
            get_planning_thresholds = thresholds
        
        # dummy variables
        plan_length = 0
        num_tests = 0
        plans = [] 

        self.online_planner = OnlinePlanning(id_discdds,
                 diffeo_structure_threshold,
                 id_algo,
                 plan_length,
                 num_tests,
                 get_planning_thresholds,
                 plans)
        
        self.state = WAIT_GOAL