def optimal(self, beta=10, style='static'): self.beta = beta if style == 'static': # full graph construction self.product.graph['ts'].build_full() self.product.build_full() self.run, plantime = dijkstra_plan_networkX(self.product, self.beta) elif style == 'ready': self.product.build_full() self.run, plantime = dijkstra_plan_networkX(self.product, self.beta) elif style == 'on-the-fly': # on-the-fly construction self.product.build_initial() self.product.build_accept() self.run, plantime = dijkstra_plan_optimal(self.product, self.beta) print 'the plan prefix:\n' print [n for n in self.run.pre_plan] #print '\n' print 'the plan suffix:\n' print [n for n in self.run.suf_plan] #print '\n' self.opt_log.append((self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost, self.run.sufcost, self.run.totalcost)) self.last_time = self.Time self.acc_change = 0 self.index = 1 self.segment = 'line' self.next_move = self.run.pre_plan[self.index] return plantime
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run == None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
if isinstance(self.run.pre_plan[1], str):
init_pose = (rospy.get_param('amcl_initial_pose_x'),
rospy.get_param('amcl_initial_pose_y'),
rospy.get_param('amcl_initial_pose_a'))
self.run.line = [(init_pose, 'None')] + self.run.line
print[n for n in self.run.line]
self.run.pre_plan = [init_pose] + self.run.pre_plan
print[n for n in self.run.pre_plan]
rospy.logwarn("Prefix plan corrected")
#print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print[n for n in self.run.line]
print 'the suffix of plan **states**:'
print[n for n in self.run.loop]
#print '\n'
print '------------------------------'
print 'the prefix of plan **actions**:'
print[n for n in self.run.pre_plan]
print 'the suffix of plan **actions**:'
print[n for n in self.run.suf_plan]
self.opt_log.append(
(self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost,
self.run.sufcost, self.run.totalcost))
self.last_time = self.Time
self.acc_change = 0
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
return plantime
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
# write_dot(self.product.graph['ts'], "./ts.dot")
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run is None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
# print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print[n for n in self.run.line]
print 'the suffix of plan **states**:'
print[n for n in self.run.loop]
print '------------------------------'
print 'the prefix of plan **aps**:'
print[self.product.graph['ts'].node[n]['label'] for n in self.run.line]
print 'the suffix of plan **aps**:'
print[self.product.graph['ts'].node[n]['label'] for n in self.run.loop]
# print '\n'
print '------------------------------'
# print 'the prefix of plan **actions**:'
# print [n for n in self.run.pre_plan]
# print 'the suffix of plan **actions**:'
# print [n for n in self.run.suf_plan]
self.opt_log.append(
(self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost,
self.run.sufcost, self.run.totalcost))
self.last_time = self.Time
self.acc_change = 0
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
return plantime
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run == None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
#print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print [n for n in self.run.line]
def margin_opt_path(self, opt_path, beta):
self.reset_beta(beta)
self.product.build_full_margin(opt_path)
#marg_path = dijkstra_path_networkX(self.product, opt_path[0], opt_path[-1])
self.run, plantime = dijkstra_plan_networkX(self.product, self.gamma)
return self.run.suffix
def margin_optimal(self, opt_path, style='ready'):
self.product.build_full_margin(opt_path)
self.run, plantime = dijkstra_plan_networkX(self.product, self.beta)
