ROS行为树实现（Python）——模板应用

一、场景设计

　　场景设计如下，一个线程每隔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)

View Code

　　为避免无休止地生成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

View Code

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

View Code

 三、测试

　　测试命令如下：

$ 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

View Code

以上程序源码与记录地址：https://gitee.com/xjEzekiel/bt_tree/tree/master/templates/SubTree_Gen。