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 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
