-
Notifications
You must be signed in to change notification settings - Fork 102
/
patrol_tree.py
executable file
·135 lines (100 loc) · 4.9 KB
/
patrol_tree.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
#!/usr/bin/env python
"""
patrol_tree.py - Version 1.0 2013-03-18
Navigate a series of waypoints while monitoring battery levels.
Uses the pi_trees package to implement a behavior tree task manager.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2013 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import rospy
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist
from rbx2_msgs.srv import *
from pi_trees_ros.pi_trees_ros import *
from rbx2_tasks.task_setup import *
class Patrol():
def __init__(self):
rospy.init_node("patrol_tree")
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Create a list to hold the move_base tasks
MOVE_BASE_TASKS = list()
n_waypoints = len(self.waypoints)
# Create simple action navigation task for each waypoint
for i in range(n_waypoints + 1):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = self.waypoints[i % n_waypoints]
move_base_task = SimpleActionTask("MOVE_BASE_TASK_" + str(i), "move_base", MoveBaseAction, goal, reset_after=False)
MOVE_BASE_TASKS.append(move_base_task)
# Set the docking station pose
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = self.docking_station_pose
# Assign the docking station pose to a move_base action task
NAV_DOCK_TASK = SimpleActionTask("NAV_DOC_TASK", "move_base", MoveBaseAction, goal, reset_after=True)
# Create the root node
BEHAVE = Sequence("BEHAVE")
# Create the "stay healthy" selector
STAY_HEALTHY = Selector("STAY_HEALTHY")
# Create the patrol loop decorator
LOOP_PATROL = Loop("LOOP_PATROL", iterations=self.n_patrols)
# Add the two subtrees to the root node in order of priority
BEHAVE.add_child(STAY_HEALTHY)
BEHAVE.add_child(LOOP_PATROL)
# Create the patrol iterator
PATROL = Iterator("PATROL")
# Add the move_base tasks to the patrol task
for task in MOVE_BASE_TASKS:
PATROL.add_child(task)
# Add the patrol to the loop decorator
LOOP_PATROL.add_child(PATROL)
# Add the battery check and recharge tasks to the "stay healthy" task
with STAY_HEALTHY:
# The check battery condition (uses MonitorTask)
CHECK_BATTERY = MonitorTask("CHECK_BATTERY", "battery_level", Float32, self.check_battery)
# The charge robot task (uses ServiceTask)
CHARGE_ROBOT = ServiceTask("CHARGE_ROBOT", "battery_simulator/set_battery_level", SetBatteryLevel, 100, result_cb=self.recharge_cb)
# Build the recharge sequence using inline construction
RECHARGE = Sequence("RECHARGE", [NAV_DOCK_TASK, CHARGE_ROBOT])
# Add the check battery and recharge tasks to the stay healthy selector
STAY_HEALTHY.add_child(CHECK_BATTERY)
STAY_HEALTHY.add_child(RECHARGE)
# Display the tree before beginning execution
print "Patrol Behavior Tree"
print_tree(BEHAVE)
# Run the tree
while not rospy.is_shutdown():
BEHAVE.run()
rospy.sleep(0.1)
def check_battery(self, msg):
if msg.data is None:
return TaskStatus.RUNNING
else:
if msg.data < self.low_battery_threshold:
rospy.loginfo("LOW BATTERY - level: " + str(int(msg.data)))
return TaskStatus.FAILURE
else:
return TaskStatus.SUCCESS
def recharge_cb(self, result):
rospy.loginfo("BATTERY CHARGED!")
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_all_goals()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
tree = Patrol()