一、场景设计
场景设计如下,一个线程每隔10s生成一个MoveBaseGoal并加入队列,子树Path依次取出MoveBaseGoal并发送给MoveBaseServer完成全部路径,需要实现的内容如下:
1、一个生成MoveBaseGoal的线程;
2、子树Path;
3、为了方便测试,需要一个模拟的MoveBaseServer;
二、程序实现
1、生成MoveBaseGoal:GenGoal.py

1 #!/usr/bin/env python 2 3 4 import threading 5 import time 6 7 from move_base_msgs.msg import * 8 9 10 _goal_list = [] 11 _gen_goal_thread = None 12 13 14 def get_goal(): 15 if len(_goal_list)>0: 16 return True, _goal_list.pop(0) 17 return False, None 18 19 20 def gen_goal(): 21 global _gen_goal_thread 22 _gen_goal_thread = threading.Thread(target=_gen_goal, name="GenGoal") 23 _gen_goal_thread.setDaemon(True) 24 _gen_goal_thread.start() 25 return 26 27 28 def _gen_goal(): 29 x, y = 0, 0 30 for i in range(1, 20): 31 x, y = x+i, y+i 32 goal = MoveBaseGoal() 33 goal.target_pose.pose.position.x, goal.target_pose.pose.position.y = x, y 34 _goal_list.append(goal) 35 time.sleep(10) 36 return 37 38 39 if __name__ == "__main__": 40 gen_goal() 41 while True: 42 flag, goal = get_goal() 43 if flag: 44 print(goal) 45 time.sleep(5)
为避免无休止地生成MoveBaseGoal,此处以20为上限。
2、子树Path:
1)visio视图:
2)代码
生成子树Path基本框架,命令如下:
$ cd <path_to_templates> $ python SubTree_Temlate_Generator.py Path
生成文件SubTree_Path.py和SubTree_Path_Test.py,存放于templates/SubTree_Gen/目录,修改SubTree_Path.py如下:

1 #!/usr/bin/env python 2 3 4 _prefix = "Path" 5 6 7 import rospy 8 from move_base_msgs.msg import * 9 10 from bt_tree_lib.bt_tree_ros import * 11 from GenGoal import * 12 13 14 Path_SubTree_Dict = {} 15 16 17 class Path_SubTreeParamStatus(object): 18 PREPARING = 0 19 EXECUTING = 1 20 21 22 class Path_SubTreeParam(object): 23 def __init__(self, *args, **kwargs): 24 super(Path_SubTreeParam, self).__init__(*args, **kwargs) 25 self.status = Path_SubTreeParamStatus.PREPARING 26 27 self.goal = MoveBaseGoal() 28 return 29 30 def get_status(self): 31 return self.status 32 33 def set_status(self, status): 34 self.status = status 35 return 36 37 def reset(self): 38 self.status = Path_SubTreeParamStatus.PREPARING 39 40 self.__set_goal(self.goal, MoveBaseGoal()) 41 return 42 43 def set(self, param): 44 self.__set_goal(self.goal, param) 45 return 46 47 def __set_goal(self, goal, param): 48 goal.target_pose = param.target_pose 49 return 50 51 52 def Path_init(): 53 gen_goal() 54 return 55 56 57 def Path_cancel(): 58 for key, value in Path_SubTree_Dict.items(): 59 value['SubTreeParam'].reset() 60 value['SubTree'].reset() 61 return 62 63 64 def Path_get_SubTree(name): 65 # Input check. 66 if not isinstance(name, str): 67 return False, None, None, "Parameter passed is not a string." 68 if Path_SubTree_Dict.has_key(name): 69 return False, None, None, "The Path_SubTree %s has already existed." % name 70 71 SubTreeParam = Path_SubTreeParam() 72 SubTree = _Path_get_SubTree(SubTreeParam) 73 74 # Save the SubTree, and return result. 75 Path_SubTree_Dict[name] = {'SubTreeParam': SubTreeParam, 'SubTree': SubTree} 76 return True, SubTreeParam, SubTree, "" 77 78 79 def _Path_get_SubTree(param): 80 HAS_PATH_EXECUTING = PathNode_has_Path_EXECUTING("HAS_PATH_EXECUTING", param) 81 GET_A_NEW_GOAL = Get_A_New_Goal("GET_A_NEW_GOAL", param) 82 PATH_LOOP_HEADER = Selector("PATH_LOOP_HEADER", [HAS_PATH_EXECUTING, GET_A_NEW_GOAL]) 83 PATH_LOOP_HEADER_WRAPPER = DecoratorNode_NOT("PATH_LOOP_HEADER_WRAPPER", PATH_LOOP_HEADER) 84 85 SET_PATH_EXECUTING = PathNode_set_Path_EXECUTING("SET_PATH_EXECUTING", param) 86 NAVIGATE = SimpleActionNode("NAVIGATE", "move_base", MoveBaseAction, param.goal, 10) 87 SET_PATH_PREPARING = PathNode_set_Path_PREPARING("SET_PATH_PREPARING", param) 88 PATH_LOOP_BODY = Sequence_withMemory("PATH_LOOP_BODY", [SET_PATH_EXECUTING, NAVIGATE, SET_PATH_PREPARING]) 89 90 91 return Selector("PATH_LOOP", [PATH_LOOP_HEADER_WRAPPER, PATH_LOOP_BODY]) 92 93 94 class PathNode_has_Path_EXECUTING(LeafNode): 95 def __init__(self, name, param, *args, **kwargs): 96 super(PathNode_has_Path_EXECUTING, self).__init__(name, *args, **kwargs) 97 self.param = param 98 return 99 100 def run(self): 101 status = self.param.get_status() 102 if status == Path_SubTreeParamStatus.EXECUTING: 103 return NodeStatus.SUCCESS 104 return NodeStatus.FAILURE 105 106 107 class PathNode_set_Path_EXECUTING(LeafNode): 108 def __init__(self, name, param, *args, **kwargs): 109 super(PathNode_set_Path_EXECUTING, self).__init__(name, *args, **kwargs) 110 self.param = param 111 return 112 113 def run(self): 114 self.param.set_status(Path_SubTreeParamStatus.EXECUTING) 115 return NodeStatus.SUCCESS 116 117 118 class PathNode_set_Path_PREPARING(LeafNode): 119 def __init__(self, name, param, *args, **kwargs): 120 super(PathNode_set_Path_PREPARING, self).__init__(name, *args, **kwargs) 121 self.param = param 122 return 123 124 def run(self): 125 self.param.set_status(Path_SubTreeParamStatus.PREPARING) 126 return NodeStatus.SUCCESS 127 128 129 class Get_A_New_Goal(LeafNode): 130 def __init__(self, name, param, *args, **kwargs): 131 super(Get_A_New_Goal, self).__init__(name, *args, **kwargs) 132 self.param = param 133 return 134 135 def run(self): 136 flag, goal = get_goal() 137 if flag: 138 self.param.set(goal) 139 return NodeStatus.SUCCESS 140 return NodeStatus.FAILURE
3、模拟MoveBaseServer:SimServer.py

1 #!/usr/bin/env python 2 3 4 import math 5 import time 6 7 import rospy 8 import actionlib 9 from move_base_msgs.msg import * 10 11 12 class SimServer(object): 13 def __init__(self, *args, **kwargs): 14 super(SimServer, self).__init__(*args, **kwargs) 15 self.position = {'x': 0, 'y': 0} 16 17 self.server = actionlib.SimpleActionServer("move_base", MoveBaseAction, self.execute, False) 18 self.server.start() 19 return 20 21 def execute(self, goal): 22 rospy.loginfo("%s accept." % str(self.server.current_goal)) 23 rospy.loginfo("%s" % str(goal)) 24 25 position, orientation = goal.target_pose.pose.position, goal.target_pose.pose.orientation 26 position = {'x': position.x, 'y': position.y} 27 28 count = int(math.sqrt((position['x']-self.position['x'])**2 + (position['y']-self.position['y'])**2)) + 1 29 30 self.position.update(position) 31 32 for index in range(0, count): 33 if self.server.is_preempt_requested(): 34 rospy.loginfo("%s preempt occurred." % str(self.server.current_goal)) 35 self.server.set_preempted() 36 return 37 time.sleep(1) 38 self.server.set_succeeded() 39 return 40 41 42 if __name__ == "__main__": 43 rospy.init_node("sim_server") 44 server = SimServer() 45 rospy.spin()
三、测试
测试命令如下:
$ rosrun bt_tree SimServer.py $ rosrun bt_tree SubTree_Path_Test.py
输出记录如下:

[INFO] [1541560447.702743]: NAVIGATE connected to move_base. [INFO] [1541560447.703035]: NAVIGATE sending goal... [INFO] [1541560447.703393]: RUNNING [INFO] [1541560448.704274]: RUNNING [INFO] [1541560449.705932]: RUNNING [INFO] [1541560449.737800]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560450.707340]: SUCCESS [INFO] [1541560451.708958]: SUCCESS [INFO] [1541560452.709779]: SUCCESS [INFO] [1541560453.710609]: SUCCESS [INFO] [1541560454.711420]: SUCCESS [INFO] [1541560455.712190]: SUCCESS [INFO] [1541560456.713061]: SUCCESS [INFO] [1541560457.714181]: NAVIGATE sending goal... [INFO] [1541560457.715852]: RUNNING [INFO] [1541560458.717042]: RUNNING [INFO] [1541560459.718073]: RUNNING [INFO] [1541560460.718957]: RUNNING [INFO] [1541560460.725067]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560461.719963]: SUCCESS [INFO] [1541560462.720858]: SUCCESS [INFO] [1541560463.721659]: SUCCESS [INFO] [1541560464.722459]: SUCCESS [INFO] [1541560465.723285]: SUCCESS [INFO] [1541560466.724095]: SUCCESS [INFO] [1541560467.724926]: NAVIGATE sending goal... [INFO] [1541560467.725485]: RUNNING [INFO] [1541560468.726893]: RUNNING [INFO] [1541560469.727682]: RUNNING [INFO] [1541560470.728476]: RUNNING [INFO] [1541560471.729293]: RUNNING [INFO] [1541560472.730082]: RUNNING [INFO] [1541560472.746182]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560473.731095]: SUCCESS [INFO] [1541560474.731949]: SUCCESS [INFO] [1541560475.732760]: SUCCESS [INFO] [1541560476.733493]: SUCCESS [INFO] [1541560477.734197]: NAVIGATE sending goal... [INFO] [1541560477.734792]: RUNNING [INFO] [1541560478.735802]: RUNNING [INFO] [1541560479.736804]: RUNNING [INFO] [1541560480.737945]: RUNNING [INFO] [1541560481.738746]: RUNNING [INFO] [1541560482.739576]: RUNNING [INFO] [1541560483.740307]: RUNNING [INFO] [1541560483.766805]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560484.741266]: SUCCESS [INFO] [1541560485.742125]: SUCCESS [INFO] [1541560486.742954]: SUCCESS [INFO] [1541560487.743846]: NAVIGATE sending goal... [INFO] [1541560487.744455]: RUNNING [INFO] [1541560488.745434]: RUNNING [INFO] [1541560489.746273]: RUNNING [INFO] [1541560490.747088]: RUNNING [INFO] [1541560491.747969]: RUNNING [INFO] [1541560492.749143]: RUNNING [INFO] [1541560493.750795]: RUNNING [INFO] [1541560494.751711]: RUNNING [INFO] [1541560495.752792]: RUNNING [INFO] [1541560495.786118]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560496.754624]: SUCCESS [INFO] [1541560497.755636]: NAVIGATE sending goal... [INFO] [1541560497.756814]: RUNNING [INFO] [1541560498.758104]: RUNNING [INFO] [1541560499.758901]: RUNNING [INFO] [1541560500.760289]: RUNNING [INFO] [1541560501.761584]: RUNNING [INFO] [1541560502.762700]: RUNNING [INFO] [1541560503.763545]: RUNNING [INFO] [1541560504.764808]: RUNNING [INFO] [1541560505.765920]: RUNNING [INFO] [1541560506.766838]: RUNNING [INFO] [1541560506.802577]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560507.768125]: SUCCESS [INFO] [1541560508.769323]: NAVIGATE sending goal... [INFO] [1541560508.770256]: RUNNING [INFO] [1541560509.770947]: RUNNING [INFO] [1541560510.771866]: RUNNING [INFO] [1541560511.772928]: RUNNING [INFO] [1541560512.773718]: RUNNING [INFO] [1541560513.775912]: RUNNING [INFO] [1541560514.777313]: RUNNING [INFO] [1541560515.778188]: RUNNING [INFO] [1541560516.779282]: RUNNING [INFO] [1541560517.780541]: RUNNING [INFO] [1541560518.781994]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560518.782480]: RUNNING [INFO] [1541560519.783807]: SUCCESS [INFO] [1541560520.785016]: NAVIGATE sending goal... [INFO] [1541560520.785971]: RUNNING [INFO] [1541560521.787210]: RUNNING [INFO] [1541560522.788060]: RUNNING [INFO] [1541560523.788994]: RUNNING [INFO] [1541560524.789782]: RUNNING [INFO] [1541560525.790649]: RUNNING [INFO] [1541560526.791904]: RUNNING [INFO] [1541560527.792762]: RUNNING [INFO] [1541560528.793818]: RUNNING [INFO] [1541560529.794881]: RUNNING [INFO] [1541560530.795884]: RUNNING [INFO] [1541560531.796667]: RUNNING [INFO] [1541560532.797410]: RUNNING [INFO] [1541560532.802936]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560533.798837]: SUCCESS [INFO] [1541560534.799909]: NAVIGATE sending goal... [INFO] [1541560534.800486]: RUNNING [INFO] [1541560535.801326]: RUNNING [INFO] [1541560536.802758]: RUNNING [INFO] [1541560537.803675]: RUNNING [INFO] [1541560538.804825]: RUNNING [INFO] [1541560539.805888]: RUNNING [INFO] [1541560540.806976]: RUNNING [INFO] [1541560541.807769]: RUNNING [INFO] [1541560542.808796]: RUNNING [INFO] [1541560543.809726]: RUNNING [INFO] [1541560544.811051]: RUNNING [INFO] [1541560545.811856]: RUNNING [INFO] [1541560546.812706]: RUNNING [INFO] [1541560547.813476]: RUNNING [INFO] [1541560547.822089]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560548.814559]: SUCCESS [INFO] [1541560549.815447]: NAVIGATE sending goal... [INFO] [1541560549.815964]: RUNNING [INFO] [1541560550.816830]: RUNNING [INFO] [1541560551.817850]: RUNNING [INFO] [1541560552.818725]: RUNNING [INFO] [1541560553.819542]: RUNNING [INFO] [1541560554.820323]: RUNNING [INFO] [1541560555.821087]: RUNNING [INFO] [1541560556.821917]: RUNNING [INFO] [1541560557.822646]: RUNNING [INFO] [1541560558.823466]: RUNNING [INFO] [1541560559.824285]: RUNNING [INFO] [1541560560.825054]: RUNNING [INFO] [1541560561.825865]: RUNNING [INFO] [1541560562.826708]: RUNNING [INFO] [1541560563.827547]: RUNNING [INFO] [1541560564.828410]: RUNNING [INFO] [1541560564.843506]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560565.829362]: SUCCESS [INFO] [1541560566.830326]: NAVIGATE sending goal... [INFO] [1541560566.830829]: RUNNING [INFO] [1541560567.831581]: RUNNING [INFO] [1541560568.832745]: RUNNING [INFO] [1541560569.833754]: RUNNING [INFO] [1541560570.835110]: RUNNING [INFO] [1541560571.835948]: RUNNING [INFO] [1541560572.836760]: RUNNING [INFO] [1541560573.837681]: RUNNING [INFO] [1541560574.838756]: RUNNING [INFO] [1541560575.839555]: RUNNING [INFO] [1541560576.840217]: RUNNING [INFO] [1541560577.841033]: RUNNING [INFO] [1541560578.842089]: RUNNING [INFO] [1541560579.843409]: RUNNING [INFO] [1541560580.844266]: RUNNING [INFO] [1541560581.845363]: RUNNING [INFO] [1541560582.846974]: RUNNING [INFO] [1541560582.865583]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560583.848569]: SUCCESS [INFO] [1541560584.849456]: NAVIGATE sending goal... [INFO] [1541560584.850073]: RUNNING [INFO] [1541560585.851023]: RUNNING [INFO] [1541560586.852383]: RUNNING [INFO] [1541560587.853386]: RUNNING [INFO] [1541560588.854944]: RUNNING [INFO] [1541560589.856223]: RUNNING [INFO] [1541560590.857803]: RUNNING [INFO] [1541560591.859368]: RUNNING [INFO] [1541560592.860830]: RUNNING [INFO] [1541560593.862199]: RUNNING [INFO] [1541560594.863528]: RUNNING [INFO] [1541560595.864540]: RUNNING [INFO] [1541560596.865796]: RUNNING [INFO] [1541560597.867013]: RUNNING [INFO] [1541560598.867874]: RUNNING [INFO] [1541560599.868872]: RUNNING [INFO] [1541560600.870281]: RUNNING [INFO] [1541560601.871244]: RUNNING [INFO] [1541560601.889680]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560602.873287]: SUCCESS [INFO] [1541560603.874769]: NAVIGATE sending goal... [INFO] [1541560603.875334]: RUNNING [INFO] [1541560604.876476]: RUNNING [INFO] [1541560605.878231]: RUNNING [INFO] [1541560606.879812]: RUNNING [INFO] [1541560607.881448]: RUNNING [INFO] [1541560608.883839]: RUNNING [INFO] [1541560609.884760]: RUNNING [INFO] [1541560610.885711]: RUNNING [INFO] [1541560611.886467]: RUNNING [INFO] [1541560612.887311]: RUNNING [INFO] [1541560613.888394]: RUNNING [INFO] [1541560614.889322]: RUNNING [INFO] [1541560615.890350]: RUNNING [INFO] [1541560616.891179]: RUNNING [INFO] [1541560617.892096]: RUNNING [INFO] [1541560618.893425]: RUNNING [INFO] [1541560619.895120]: RUNNING [INFO] [1541560620.895891]: RUNNING [INFO] [1541560621.896937]: RUNNING [INFO] [1541560622.898355]: RUNNING [INFO] [1541560622.913970]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560623.899703]: SUCCESS [INFO] [1541560624.900819]: NAVIGATE sending goal... [INFO] [1541560624.901553]: RUNNING [INFO] [1541560625.902503]: RUNNING [INFO] [1541560626.903458]: RUNNING [INFO] [1541560627.904306]: RUNNING [INFO] [1541560628.905327]: RUNNING [INFO] [1541560629.906738]: RUNNING [INFO] [1541560630.907539]: RUNNING [INFO] [1541560631.908556]: RUNNING [INFO] [1541560632.910306]: RUNNING [INFO] [1541560633.911389]: RUNNING [INFO] [1541560634.912648]: RUNNING [INFO] [1541560635.913522]: RUNNING [INFO] [1541560636.914891]: RUNNING [INFO] [1541560637.916136]: RUNNING [INFO] [1541560638.917593]: RUNNING [INFO] [1541560639.919202]: RUNNING [INFO] [1541560640.920039]: RUNNING [INFO] [1541560641.921071]: RUNNING [INFO] [1541560642.922524]: RUNNING [INFO] [1541560643.923342]: RUNNING [INFO] [1541560644.925056]: RUNNING [INFO] [1541560644.938527]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560645.926185]: SUCCESS [INFO] [1541560646.926938]: NAVIGATE sending goal... [INFO] [1541560646.927623]: RUNNING [INFO] [1541560647.928346]: RUNNING [INFO] [1541560648.929134]: RUNNING [INFO] [1541560649.930028]: RUNNING [INFO] [1541560650.930872]: RUNNING [INFO] [1541560651.931892]: RUNNING [INFO] [1541560652.932960]: RUNNING [INFO] [1541560653.934059]: RUNNING [INFO] [1541560654.935275]: RUNNING [INFO] [1541560655.936324]: RUNNING [INFO] [1541560656.937948]: RUNNING [INFO] [1541560657.938827]: RUNNING [INFO] [1541560658.939709]: RUNNING [INFO] [1541560659.940599]: RUNNING [INFO] [1541560660.941635]: RUNNING [INFO] [1541560661.942555]: RUNNING [INFO] [1541560662.943758]: RUNNING [INFO] [1541560663.944492]: RUNNING [INFO] [1541560664.946063]: RUNNING [INFO] [1541560665.946868]: RUNNING [INFO] [1541560666.947807]: RUNNING [INFO] [1541560667.949321]: RUNNING [INFO] [1541560668.950315]: RUNNING [INFO] [1541560668.966195]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560669.952162]: SUCCESS [INFO] [1541560670.953490]: NAVIGATE sending goal... [INFO] [1541560670.954870]: RUNNING [INFO] [1541560671.955926]: RUNNING [INFO] [1541560672.957027]: RUNNING [INFO] [1541560673.958255]: RUNNING [INFO] [1541560674.959171]: RUNNING [INFO] [1541560675.960550]: RUNNING [INFO] [1541560676.961435]: RUNNING [INFO] [1541560677.962480]: RUNNING [INFO] [1541560678.963447]: RUNNING [INFO] [1541560679.964363]: RUNNING [INFO] [1541560680.965341]: RUNNING [INFO] [1541560681.966236]: RUNNING [INFO] [1541560682.967149]: RUNNING [INFO] [1541560683.968278]: RUNNING [INFO] [1541560684.969539]: RUNNING [INFO] [1541560685.970686]: RUNNING [INFO] [1541560686.971705]: RUNNING [INFO] [1541560687.973022]: RUNNING [INFO] [1541560688.973959]: RUNNING [INFO] [1541560689.975022]: RUNNING [INFO] [1541560690.976353]: RUNNING [INFO] [1541560691.978459]: RUNNING [INFO] [1541560692.979429]: RUNNING [INFO] [1541560693.980904]: RUNNING [INFO] [1541560693.994909]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560694.982230]: SUCCESS [INFO] [1541560695.983242]: NAVIGATE sending goal... [INFO] [1541560695.983816]: RUNNING [INFO] [1541560696.984756]: RUNNING [INFO] [1541560697.985735]: RUNNING [INFO] [1541560698.987147]: RUNNING [INFO] [1541560699.988000]: RUNNING [INFO] [1541560700.989018]: RUNNING [INFO] [1541560701.991038]: RUNNING [INFO] [1541560702.992001]: RUNNING [INFO] [1541560703.993195]: RUNNING [INFO] [1541560704.994245]: RUNNING [INFO] [1541560705.995128]: RUNNING [INFO] [1541560706.996982]: RUNNING [INFO] [1541560707.998182]: RUNNING [INFO] [1541560708.999681]: RUNNING [INFO] [1541560710.000630]: RUNNING [INFO] [1541560711.001684]: RUNNING [INFO] [1541560712.002878]: RUNNING [INFO] [1541560713.003736]: RUNNING [INFO] [1541560714.004745]: RUNNING [INFO] [1541560715.006159]: RUNNING [INFO] [1541560716.007213]: RUNNING [INFO] [1541560717.008511]: RUNNING [INFO] [1541560718.009309]: RUNNING [INFO] [1541560719.010173]: RUNNING [INFO] [1541560720.010987]: RUNNING [INFO] [1541560721.011881]: RUNNING [INFO] [1541560721.026078]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560722.013222]: SUCCESS [INFO] [1541560723.013995]: NAVIGATE sending goal... [INFO] [1541560723.014580]: RUNNING [INFO] [1541560724.015516]: RUNNING [INFO] [1541560725.016354]: RUNNING [INFO] [1541560726.017177]: RUNNING [INFO] [1541560727.017993]: RUNNING [INFO] [1541560728.018758]: RUNNING [INFO] [1541560729.019771]: RUNNING [INFO] [1541560730.021116]: RUNNING [INFO] [1541560731.021871]: RUNNING [INFO] [1541560732.022678]: RUNNING [INFO] [1541560733.023453]: RUNNING [INFO] [1541560734.024274]: RUNNING [INFO] [1541560735.025005]: RUNNING [INFO] [1541560736.025840]: RUNNING [INFO] [1541560737.026638]: RUNNING [INFO] [1541560738.027406]: RUNNING [INFO] [1541560739.028205]: RUNNING [INFO] [1541560740.029068]: RUNNING [INFO] [1541560741.029825]: RUNNING [INFO] [1541560742.030762]: RUNNING [INFO] [1541560743.031824]: RUNNING [INFO] [1541560744.032633]: RUNNING [INFO] [1541560745.033415]: RUNNING [INFO] [1541560746.034315]: RUNNING [INFO] [1541560747.035212]: RUNNING [INFO] [1541560748.036356]: RUNNING [INFO] [1541560749.037514]: RUNNING [INFO] [1541560749.057109]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560750.038495]: SUCCESS [INFO] [1541560751.039415]: NAVIGATE sending goal... [INFO] [1541560751.040289]: RUNNING [INFO] [1541560752.041508]: RUNNING [INFO] [1541560753.042424]: RUNNING [INFO] [1541560754.043541]: RUNNING [INFO] [1541560755.044282]: RUNNING [INFO] [1541560756.045108]: RUNNING [INFO] [1541560757.046168]: RUNNING [INFO] [1541560758.047579]: RUNNING [INFO] [1541560759.048404]: RUNNING [INFO] [1541560760.049376]: RUNNING [INFO] [1541560761.050640]: RUNNING [INFO] [1541560762.051534]: RUNNING [INFO] [1541560763.052906]: RUNNING [INFO] [1541560764.054658]: RUNNING [INFO] [1541560765.056044]: RUNNING [INFO] [1541560766.056993]: RUNNING [INFO] [1541560767.058226]: RUNNING [INFO] [1541560768.059606]: RUNNING [INFO] [1541560769.060395]: RUNNING [INFO] [1541560770.061201]: RUNNING [INFO] [1541560771.061956]: RUNNING [INFO] [1541560772.062785]: RUNNING [INFO] [1541560773.063610]: RUNNING [INFO] [1541560774.064394]: RUNNING [INFO] [1541560775.065190]: RUNNING [INFO] [1541560776.065931]: RUNNING [INFO] [1541560777.066840]: RUNNING [INFO] [1541560778.067864]: RUNNING [INFO] [1541560778.092443]: NAVIGATE terminated with status SUCCEEDED. [INFO] [1541560779.068807]: SUCCESS [INFO] [1541560780.069715]: SUCCESS [INFO] [1541560781.070590]: SUCCESS [INFO] [1541560782.071392]: SUCCESS [INFO] [1541560783.072234]: SUCCESS [INFO] [1541560784.073319]: SUCCESS [INFO] [1541560785.074117]: SUCCESS [INFO] [1541560786.074911]: SUCCESS [INFO] [1541560787.075697]: SUCCESS [INFO] [1541560788.076473]: SUCCESS [INFO] [1541560789.077278]: SUCCESS [INFO] [1541560790.078096]: SUCCESS [INFO] [1541560791.078868]: SUCCESS [INFO] [1541560792.079680]: SUCCESS [INFO] [1541560793.080445]: SUCCESS [INFO] [1541560794.081240]: SUCCESS [INFO] [1541560795.082158]: SUCCESS [INFO] [1541560796.083226]: SUCCESS [INFO] [1541560797.084045]: SUCCESS [INFO] [1541560798.085260]: SUCCESS [INFO] [1541560799.086055]: SUCCESS [INFO] [1541560800.086841]: SUCCESS [INFO] [1541560801.088042]: SUCCESS [INFO] [1541560802.089177]: SUCCESS [INFO] [1541560803.090343]: SUCCESS [INFO] [1541560804.091446]: SUCCESS [INFO] [1541560805.092943]: SUCCESS [INFO] [1541560806.094411]: SUCCESS [INFO] [1541560807.095080]: SUCCESS [INFO] [1541560808.095923]: SUCCESS [INFO] [1541560809.096645]: SUCCESS [INFO] [1541560810.097542]: SUCCESS
以上程序源码与记录地址:https://gitee.com/xjEzekiel/bt_tree/tree/master/templates/SubTree_Gen。