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