def get_plan(self): ''' Calculate the plan from current image to the goal. ''' print('Received a request for plan to goal') if self.y_current is None or self.y_goal is None: print('Unable to plan because missing images.') return [-1] # Plan instance = namedtuple('instance', ['y_stary', 'y_goal']) size = self.size instance.y_start = get_uncertain_image(self.y_current, size) instance.y_goal = get_uncertain_image(self.y_goal, size) # Run planning result = self.algo.plan(instance.y_start, instance.y_goal) plan = result.plan print('Planning done, found plan: ' + str(plan)) # Fix a bug temporarily TODO: Relearn diffeos orbit = OrbitModule(size) return orbit.inverse_plan(plan)
def __init__(self, id_discdds, diffeo_structure_threshold, id_algo, plan_length, num_tests, get_planning_thresholds, plans): config = get_current_config() self.config = config # Load objects from configuration manager self.discdds = config.discdds.instance(id_discdds) self.algo = init_algorithm(self.config, id_algo, id_discdds, self.discdds) self.cmdlist = [ self.discdds.actions[i].original_cmd for i in range(len(self.discdds.actions)) ] # pdb.set_trace() self.metric_goal = self.algo.metric_goal # Save input arguments self.id_discdds = id_discdds self.id_algo = id_algo self.diffeo_structure_threshold = diffeo_structure_threshold self.plan_length = plan_length self.num_tests = num_tests # Load orbit camera module self.orbit_module = OrbitModule(self.discdds.get_shape()) # Set get_planning_thresholds function if get_planning_thresholds.__class__ == 'str': try: self.get_planning_thresholds = eval(get_planning_thresholds) except: logger.info() else: self.get_planning_thresholds = get_planning_thresholds self.get_planning_thresholds_name = str(get_planning_thresholds) # Initiate diffeo_structure self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold) if plans == 'random': self.plans = self.gennerate_random_plans() elif plans.__class__ in [list, tuple]: self.plans = tuple(plans) else: self.plans = eval(plans) logger.info('Initialized with plans: ' + str(self.plans))
def __init__(self, id_discdds, diffeo_structure_threshold, id_algo, plan_length, num_tests, get_planning_thresholds, plans): config = get_current_config() self.config = config # Load objects from configuration manager self.discdds = config.discdds.instance(id_discdds) self.algo = init_algorithm(self.config, id_algo, id_discdds, self.discdds) self.cmdlist = [self.discdds.actions[i].original_cmd for i in range(len(self.discdds.actions))] # pdb.set_trace() self.metric_goal = self.algo.metric_goal # Save input arguments self.id_discdds = id_discdds self.id_algo = id_algo self.diffeo_structure_threshold = diffeo_structure_threshold self.plan_length = plan_length self.num_tests = num_tests # Load orbit camera module self.orbit_module = OrbitModule(self.discdds.get_shape()) # Set get_planning_thresholds function if get_planning_thresholds.__class__ == 'str': try: self.get_planning_thresholds = eval(get_planning_thresholds) except: logger.info() else: self.get_planning_thresholds = get_planning_thresholds self.get_planning_thresholds_name = str(get_planning_thresholds) # Initiate diffeo_structure self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold) if plans == 'random': self.plans = self.gennerate_random_plans() elif plans.__class__ in [list, tuple]: self.plans = tuple(plans) else: self.plans = eval(plans) logger.info('Initialized with plans: ' + str(self.plans))
class OnlinePlanning(): def __init__(self, id_discdds, diffeo_structure_threshold, id_algo, plan_length, num_tests, get_planning_thresholds, plans): config = get_current_config() self.config = config # Load objects from configuration manager self.discdds = config.discdds.instance(id_discdds) self.algo = init_algorithm(self.config, id_algo, id_discdds, self.discdds) self.cmdlist = [self.discdds.actions[i].original_cmd for i in range(len(self.discdds.actions))] # pdb.set_trace() self.metric_goal = self.algo.metric_goal # Save input arguments self.id_discdds = id_discdds self.id_algo = id_algo self.diffeo_structure_threshold = diffeo_structure_threshold self.plan_length = plan_length self.num_tests = num_tests # Load orbit camera module self.orbit_module = OrbitModule(self.discdds.get_shape()) # Set get_planning_thresholds function if get_planning_thresholds.__class__ == 'str': try: self.get_planning_thresholds = eval(get_planning_thresholds) except: logger.info() else: self.get_planning_thresholds = get_planning_thresholds self.get_planning_thresholds_name = str(get_planning_thresholds) # Initiate diffeo_structure self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold) if plans == 'random': self.plans = self.gennerate_random_plans() elif plans.__class__ in [list, tuple]: self.plans = tuple(plans) else: self.plans = eval(plans) logger.info('Initialized with plans: ' + str(self.plans)) def gennerate_random_plans(self): n_cmd = len(self.discdds.actions) plans = [] for _ in range(self.num_tests): plan = () ie = 0 while len(plan) < int(self.plan_length): # Add new command plan += (np.random.randint(0, n_cmd),) # remove redundant command plan = self.diffeo_struct.get_canonical(plan) ie += 1 if ie > 50: print('WARNING, a lot of iterations to generate a non redundant plan.') plans.append(plan) break plans.append(plan) return plans def run_all_tests(self, env='default'): all_stats = [] for plan in self.plans: stat = self.run_test(plan, env) all_stats.append(stat) return all_stats def run_test(self, plan, env='default'): # Initiate stats object labels = {} labels['id_discdds'] = self.id_discdds labels['diffeo_structure_threshold'] = self.diffeo_structure_threshold labels['id_algo'] = self.id_algo labels['plan_length'] = self.plan_length labels['get_planning_thresholds'] = self.get_planning_thresholds_name labels['env'] = env active_instance = OnlineStats(labels, self.metric_goal) # Setup the planning problem self.create_testcase(plan_true=plan, active_instance=active_instance) # Run the planning problem self.run_planning(active_instance=active_instance) self.prediction_images(active_instance) # return info about test return active_instance def create_testcase(self, plan_true, active_instance): # The goal image is where we start the demo # self.y_goal = self.orbit_module.get_image() active_instance.y_goal = self.orbit_module.get_image() active_instance.labels['plan_true'] = plan_true active_instance.labels['plan_true_reduced'] = self.diffeo_struct.get_canonical(plan_true) active_instance.plan_true = plan_true active_instance.plan_true_reduced = self.diffeo_struct.get_canonical(plan_true) # Move the camera to the start position plan_inverse = self.orbit_module.inverse_plan(plan_true) self.orbit_module.execute_plan(plan_inverse) # self.y_start = self.orbit_module.get_image() def run_planning(self, active_instance): # Capture the initial image active_instance.y_start = self.orbit_module.get_image() # pdb.set_trace() if not self.get_planning_thresholds.__class__ in [list, tuple]: precision, min_visibility = self.get_planning_thresholds(algo=self.algo, discdds=self.discdds, active_instance=active_instance) else: precision, min_visibility = self.get_planning_thresholds active_instance.labels['precision'] = precision active_instance.labels['min_visibility'] = min_visibility active_instance.precision = precision active_instance.min_visibility = min_visibility planning_result = self.algo.plan(active_instance.y_start, active_instance.y_goal, precision=precision, min_visibility=min_visibility) if not planning_result.success: return None logger.info('Plan found: ' + str(planning_result.plan)) plan_found = self.orbit_module.inverse_plan(planning_result.plan) self.orbit_module.execute_plan(plan_found) # Update active_instance with results active_instance.plan_found = plan_found active_instance.plan_found_reduced = self.diffeo_struct.get_canonical(plan_found) active_instance.y_result = self.orbit_module.get_image() active_instance.labels['plan_found'] = plan_found active_instance.labels['plan_found_reduced'] = self.diffeo_struct.get_canonical(plan_found) def prediction_images(self, active_instance): ''' predicts the resulting images from active_instance.y_start and plan :param active_instance: ''' if active_instance.plan_found is not None: inv_plan = self.orbit_module.inverse_plan(active_instance.plan_found) active_instance.y_found_pred = self.discdds.predict(active_instance.y_start, inv_plan) if active_instance.plan_true is not None: inv_plan = self.orbit_module.inverse_plan(active_instance.plan_true) active_instance.y_goal_pred = self.discdds.predict(active_instance.y_start, inv_plan)
class OnlinePlanning(): def __init__(self, id_discdds, diffeo_structure_threshold, id_algo, plan_length, num_tests, get_planning_thresholds, plans): config = get_current_config() self.config = config # Load objects from configuration manager self.discdds = config.discdds.instance(id_discdds) self.algo = init_algorithm(self.config, id_algo, id_discdds, self.discdds) self.cmdlist = [ self.discdds.actions[i].original_cmd for i in range(len(self.discdds.actions)) ] # pdb.set_trace() self.metric_goal = self.algo.metric_goal # Save input arguments self.id_discdds = id_discdds self.id_algo = id_algo self.diffeo_structure_threshold = diffeo_structure_threshold self.plan_length = plan_length self.num_tests = num_tests # Load orbit camera module self.orbit_module = OrbitModule(self.discdds.get_shape()) # Set get_planning_thresholds function if get_planning_thresholds.__class__ == 'str': try: self.get_planning_thresholds = eval(get_planning_thresholds) except: logger.info() else: self.get_planning_thresholds = get_planning_thresholds self.get_planning_thresholds_name = str(get_planning_thresholds) # Initiate diffeo_structure self.diffeo_struct = DiffeoStructure(self.discdds, diffeo_structure_threshold) if plans == 'random': self.plans = self.gennerate_random_plans() elif plans.__class__ in [list, tuple]: self.plans = tuple(plans) else: self.plans = eval(plans) logger.info('Initialized with plans: ' + str(self.plans)) def gennerate_random_plans(self): n_cmd = len(self.discdds.actions) plans = [] for _ in range(self.num_tests): plan = () ie = 0 while len(plan) < int(self.plan_length): # Add new command plan += (np.random.randint(0, n_cmd), ) # remove redundant command plan = self.diffeo_struct.get_canonical(plan) ie += 1 if ie > 50: print( 'WARNING, a lot of iterations to generate a non redundant plan.' ) plans.append(plan) break plans.append(plan) return plans def run_all_tests(self, env='default'): all_stats = [] for plan in self.plans: stat = self.run_test(plan, env) all_stats.append(stat) return all_stats def run_test(self, plan, env='default'): # Initiate stats object labels = {} labels['id_discdds'] = self.id_discdds labels['diffeo_structure_threshold'] = self.diffeo_structure_threshold labels['id_algo'] = self.id_algo labels['plan_length'] = self.plan_length labels['get_planning_thresholds'] = self.get_planning_thresholds_name labels['env'] = env active_instance = OnlineStats(labels, self.metric_goal) # Setup the planning problem self.create_testcase(plan_true=plan, active_instance=active_instance) # Run the planning problem self.run_planning(active_instance=active_instance) self.prediction_images(active_instance) # return info about test return active_instance def create_testcase(self, plan_true, active_instance): # The goal image is where we start the demo # self.y_goal = self.orbit_module.get_image() active_instance.y_goal = self.orbit_module.get_image() active_instance.labels['plan_true'] = plan_true active_instance.labels[ 'plan_true_reduced'] = self.diffeo_struct.get_canonical(plan_true) active_instance.plan_true = plan_true active_instance.plan_true_reduced = self.diffeo_struct.get_canonical( plan_true) # Move the camera to the start position plan_inverse = self.orbit_module.inverse_plan(plan_true) self.orbit_module.execute_plan(plan_inverse) # self.y_start = self.orbit_module.get_image() def run_planning(self, active_instance): # Capture the initial image active_instance.y_start = self.orbit_module.get_image() # pdb.set_trace() if not self.get_planning_thresholds.__class__ in [list, tuple]: precision, min_visibility = self.get_planning_thresholds( algo=self.algo, discdds=self.discdds, active_instance=active_instance) else: precision, min_visibility = self.get_planning_thresholds active_instance.labels['precision'] = precision active_instance.labels['min_visibility'] = min_visibility active_instance.precision = precision active_instance.min_visibility = min_visibility planning_result = self.algo.plan(active_instance.y_start, active_instance.y_goal, precision=precision, min_visibility=min_visibility) if not planning_result.success: return None logger.info('Plan found: ' + str(planning_result.plan)) plan_found = self.orbit_module.inverse_plan(planning_result.plan) self.orbit_module.execute_plan(plan_found) # Update active_instance with results active_instance.plan_found = plan_found active_instance.plan_found_reduced = self.diffeo_struct.get_canonical( plan_found) active_instance.y_result = self.orbit_module.get_image() active_instance.labels['plan_found'] = plan_found active_instance.labels[ 'plan_found_reduced'] = self.diffeo_struct.get_canonical( plan_found) def prediction_images(self, active_instance): ''' predicts the resulting images from active_instance.y_start and plan :param active_instance: ''' if active_instance.plan_found is not None: inv_plan = self.orbit_module.inverse_plan( active_instance.plan_found) active_instance.y_found_pred = self.discdds.predict( active_instance.y_start, inv_plan) if active_instance.plan_true is not None: inv_plan = self.orbit_module.inverse_plan( active_instance.plan_true) active_instance.y_goal_pred = self.discdds.predict( active_instance.y_start, inv_plan)
def get_plan(self, req): ''' Calculate the plan from current image to the goal. Called from ROS srv. ''' self.info(str(req)) self.info('Received a request for plan to goal') # Initiate response object res = camera_actuator.srv.planServiceResponse() # @UndefinedVariable if self.state in [WAIT_GOAL, WAIT_CURRENT]: self.info('Module is in state: ' + self.state + 'Unable to plan because of unspecified images.') res.plan = [-1] return res # Assert the module is idle state, otherwise refuse to plan. if not self.state == IDLE: self.info('Module is in state: ' + self.state + ' and will not accept new requests for plans right now') res.plan = [-1] return res self.info('Passed state check, OK') self.state = PLANNING # indicate that the module is busy with planning # Plan instance = namedtuple('instance', ['y_stary', 'y_goal']) size = self.size y_start = self.get_y_current(size) y_goal = self.get_y_goal(size) instance.y_start = y_start instance.y_goal = y_goal # Run planning algo = self.online_planner.algo discdds = self.online_planner.discdds thresholds = rospy.get_param(self.node_name + '/planning_thresholds') if thresholds.__class__ == str: get_planning_thresholds = eval('online_planning.' + thresholds) thresh = get_planning_thresholds(algo=algo, discdds=discdds, active_instance=instance) else: thresh = thresholds precision, min_visibility = thresh self.info('Ready to plan with precision %g' % precision) result = self.online_planner.algo.plan(y_start, y_goal, precision, min_visibility) plan = result.plan self.info('Planning done, found plan: ' + str(plan)) orbit = OrbitModule(size) res.plan = orbit.inverse_plan(plan) self.state = IDLE return res