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)