├── README.md
├── pi_trees
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    └── package.xml
├── pi_trees_lib
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    ├── examples
    │   ├── composite_tasks.py
    │   ├── launch
    │   │   └── patrol_tree.launch
    │   ├── parallel_example.py
    │   ├── patrol_tree.py
    │   └── weighted_random_example.py
    ├── package.xml
    ├── setup.py
    └── src
    │   └── pi_trees_lib
    │       ├── __init__.py
    │       ├── pi_trees_lib.py
    │       └── task_setup.py
└── pi_trees_ros
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    ├── package.xml
    ├── setup.py
    └── src
        └── pi_trees_ros
            ├── __init__.py
            └── pi_trees_ros.py


/README.md:
--------------------------------------------------------------------------------
1 | **PLEASE NOTE: This code is depreciated and will not be maintained or developed further.**
2 | 
3 | A Python/ROS package for implementing Behavior Trees.
4 | 


--------------------------------------------------------------------------------
/pi_trees/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package pi_trees
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.1.2 (2016-09-25)
 6 | ------------------
 7 | * Updated dependencies in package.xml files
 8 | * Contributors: Patrick Goebel
 9 | 
10 | 0.1.1 (2014-08-01)
11 | ------------------
12 | * Added blank line to end of CMakeLists.txt
13 | * Added initial CHANGELOG.rst files
14 | * Cleanup
15 | * Initial commit
16 | * Contributors: Patrick Goebel
17 | 


--------------------------------------------------------------------------------
/pi_trees/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(pi_trees)
3 | find_package(catkin REQUIRED)
4 | catkin_metapackage()
5 | 


--------------------------------------------------------------------------------
/pi_trees/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>pi_trees</name>
 3 |   <version>0.1.1</version>
 4 | 
 5 |   <description>Behavior Trees for ROS</description>
 6 | 
 7 |   <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
 8 |   <license>BSD</license>
 9 |   <url type="website">http://ros.org/wiki/pi_trees</url>
10 |   <url type="bugtracker">https://github.com/pirobot/pi_trees/issues</url>
11 |   <author email="patrick@pirobot.org">Patrick Goebel</author>
12 | 
13 |   <!-- Dependencies which this package needs to build itself. -->
14 |   <buildtool_depend>catkin</buildtool_depend>
15 | 
16 |   <!-- Dependencies needed after this package is compiled. -->
17 |   <run_depend>pi_trees_lib</run_depend>
18 |   <run_depend>pi_trees_ros</run_depend>
19 |   
20 |   <export>
21 |     <metapackage/>
22 |   </export>
23 | 
24 | </package>
25 | 


--------------------------------------------------------------------------------
/pi_trees_lib/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package pi_trees_lib
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.1.2 (2016-09-25)
 6 | ------------------
 7 | * Added error message when number of random weights do not match number of children
 8 | * Added weighted_random_example.py
 9 | * Added weighted versions of random selector, sequence and iterator
10 | * Added composite_task.py example
11 | * Added try-except to print_tree function
12 | * Replaced print_tree function to fix some pathological cases
13 | * Fixed syntax errors in WaitTask
14 | * Merge branch 'indigo-devel' of https://github.com/pirobot/pi_trees into indigo-devel
15 |   Fixed typo in WaitTask
16 | * Fixed reset_after behavior for Sequence and Selector tasks
17 | * typo for waittask
18 | * Added root level status (BEHAVE.status) to the main loop so that root node status would be colored appropriately
19 | * Removed old parallel tasks (commented out earlier)
20 | * Fixed ParallelOne and ParallelAll tasks to really execute each child task on every iteration
21 | * Fixed reset_after status for Sequence
22 | * Added status colors to root node for DOT graph
23 | * Added parallelogram shape for ParallelOne and ParallelAll tasks
24 | * Added task_setup.py for use with patrol_tree.py example
25 | * Added root node status color to display
26 | * Updated the patrol_tree.py sample script to check battery level only when not recharging
27 | * Changed rate from 10 to 5 for patrol_tree.launch example
28 | * Added patrol example to pi_trees_lib examples
29 | * Modified print_dot_tree() function to write dotfile only if tree has changed since last write
30 | * Fixed initial value of last_dot_tree from None to the empty string
31 | * Added pi_trees_viewer suppot to pi_trees_lib
32 | * Added reset_after option to other composite tasks
33 | * Added a few more task decorators and the reset_after option (defaults to False) to reset any task after execution if set True
34 | * Added ASCII symbols option to graph representation and improved the print_dot_tree() output
35 | * Added UntilFail and until_fail() decorators
36 | * Fixed bug in task_not() decorator and added invert alias
37 | * Fixed bug in TaskNot decorator and add Invert alias
38 | * Added limit() decorator
39 | * Added 1 to the displayed loop count for the loop() decorator to be consistent with the Limit decorator
40 | * Added Limit decorator
41 | * Added random versions of Sequence, Selector and Iterator
42 | * Added new CallbackTask to pi_trees_lib
43 | * Added new CallbackTask to pi_trees_lib
44 | * Added catkin_install_python line to CMakeLists.txt
45 | * Renamed scripts directory back to examples and parallel_tasks.py to parallel_example.py
46 | * Small reformatting of comment
47 | * Renamed examples directory to scripts and renamed counting.py example to parallel_tasks.py
48 | * Removed dependency on pygraph and pygraphviz for now
49 | * Updated dependencies in package.xml files
50 | * Contributors: Jack000, Patrick Goebel
51 | 
52 | 0.1.1 (2014-08-01)
53 | ------------------
54 | * Added initial CHANGELOG.rst files
55 | * Removed print_dot_tree statement from counting.py example
56 | * Updates some comments
57 | * Updated counting.py example
58 | * Did some house cleaning
59 | * Cleanup
60 | * Initial commit
61 | * Contributors: Patrick Goebel
62 | 


--------------------------------------------------------------------------------
/pi_trees_lib/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | 
 3 | project(pi_trees_lib)
 4 | 
 5 | find_package(catkin REQUIRED)
 6 | 
 7 | catkin_package()
 8 | 
 9 | catkin_python_setup()
10 | 
11 | catkin_install_python(PROGRAMS examples/parallel_example.py
12 |                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
13 | 


--------------------------------------------------------------------------------
/pi_trees_lib/examples/composite_tasks.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from pi_trees_lib.pi_trees_lib import *
 4 | import time
 5 | 
 6 | class CompositeTasks():
 7 |     def __init__(self):
 8 |         # The root node
 9 |         BEHAVE = Sequence("behave")
10 |         
11 |         # Create a Selector composite task (returns SUCCESS as soon as any subtask returns SUCCESS)
12 |         SELECTOR_TASK = Selector("Selector Count")
13 | 
14 |         # Create a Sequence composite task (returns FAULURE as soon as any subtask returns FAILURE)
15 |         SEQUENCE_TASK = Sequence("Sequence Count")
16 |         
17 |         WAIT_TASK = WaitTask("Wait Task", 5)
18 |         
19 |         # Create three counting tasks
20 |         COUNT2 = Count("Count+2", 1, 2, 1)
21 |         COUNT5 = Count("Count-5", 5, 1, -1)
22 |         COUNT16 = Count("Count+16", 1, 16, 1)
23 | 
24 |         # Add the tasks to the sequence composite task
25 |         SEQUENCE_TASK.add_child(COUNT2)
26 |         SEQUENCE_TASK.add_child(WAIT_TASK)
27 |         SEQUENCE_TASK.add_child(COUNT5)
28 |         SEQUENCE_TASK.add_child(COUNT16)
29 |         
30 |         # Add the tasks to the selector composite task
31 |         SELECTOR_TASK.add_child(COUNT5)
32 |         SELECTOR_TASK.add_child(COUNT2)
33 |         SELECTOR_TASK.add_child(COUNT16)
34 |         
35 |         # Add the composite task to the root task
36 |         BEHAVE.add_child(SEQUENCE_TASK)
37 |         BEHAVE.add_child(SELECTOR_TASK)
38 | 
39 |         # Print a simple representation of the tree
40 |         print "Behavior Tree Structure"
41 |         print_tree(BEHAVE, use_symbols=True)
42 |             
43 |         # Run the tree
44 |         while True:
45 |             status = BEHAVE.run()
46 |             if status == TaskStatus.SUCCESS:
47 |                 print "Finished running tree."
48 |                 break
49 | 
50 | # A counting task that extends the base Task task
51 | class Count(Task):
52 |     def __init__(self, name, start, stop, step, *args, **kwargs):
53 |         super(Count, self).__init__(name, *args, **kwargs)
54 |         
55 |         self.name = name
56 |         self.start = start
57 |         self.stop = stop
58 |         self.step = step
59 |         self.count = self.start
60 |         print "Creating task Count", self.start, self.stop, self.step
61 |  
62 |     def run(self):
63 |         if abs(self.count - self.stop - self.step) <= 0:
64 |             return TaskStatus.SUCCESS
65 |         else:
66 |             print self.name, self.count
67 |             time.sleep(0.5)
68 |             self.count += self.step
69 |             if abs(self.count - self.stop - self.step) <= 0:
70 |                 return TaskStatus.SUCCESS
71 |             else:
72 |                 return TaskStatus.RUNNING
73 | 
74 |     
75 |     def reset(self):
76 |         self.count = self.start
77 | 
78 | if __name__ == '__main__':
79 |     tree = CompositeTasks()
80 | 
81 | 


--------------------------------------------------------------------------------
/pi_trees_lib/examples/launch/patrol_tree.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |  <arg name="dotfilepath" default="$(env HOME)/.ros/tree.dot" />
 4 |   
 5 |   <node pkg="pi_trees_lib" name="patrol_tree" type="patrol_tree.py" output="screen">
 6 |     <param name="rate" value="5" />
 7 |     <param name="square_size" value="1.0" />
 8 |     <param name="n_patrols" value="-1" />
 9 |     <param name="low_battery_threshold" value="50" />
10 |     <param name="dotfilepath" value="$(arg dotfilepath)" />
11 |   </node>
12 |   
13 | </launch>
14 | 


--------------------------------------------------------------------------------
/pi_trees_lib/examples/parallel_example.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from pi_trees_lib.pi_trees_lib import *
 4 | import time
 5 | 
 6 | class ParallelExample():
 7 |     def __init__(self):
 8 |         # The root node
 9 |         BEHAVE = Sequence("behave")
10 |         
11 |         # Create a ParallelOne composite task (returns SUCCESS as soon as any subtask returns SUCCESS)
12 |         PARALLEL_TASKS = ParallelOne("Counting in Parallel")
13 |         
14 |         # Create three counting tasks
15 |         COUNT2 = Count("Count+2", 1, 2, 1)
16 |         COUNT5 = Count("Count-5", 5, 1, -1)
17 |         COUNT16 = Count("Count+16", 1, 16, 1)
18 | 
19 |         # Add the tasks to the parallel composite task
20 |         PARALLEL_TASKS.add_child(COUNT5)
21 |         PARALLEL_TASKS.add_child(COUNT2)
22 |         PARALLEL_TASKS.add_child(COUNT16)
23 |         
24 |         # Add the composite task to the root task
25 |         BEHAVE.add_child(PARALLEL_TASKS)
26 |         
27 |         # Print a simple representation of the tree
28 |         print "Behavior Tree Structure"
29 |         print_tree(BEHAVE)
30 |             
31 |         # Run the tree
32 |         while True:
33 |             status = BEHAVE.run()
34 |             if status == TaskStatus.SUCCESS:
35 |                 print "Finished running tree."
36 |                 break
37 | 
38 | # A counting task that extends the base Task task
39 | class Count(Task):
40 |     def __init__(self, name, start, stop, step, *args, **kwargs):
41 |         super(Count, self).__init__(name, *args, **kwargs)
42 |         
43 |         self.name = name
44 |         self.start = start
45 |         self.stop = stop
46 |         self.step = step
47 |         self.count = self.start
48 |         print "Creating task Count", self.start, self.stop, self.step
49 |  
50 |     def run(self):
51 |         if abs(self.count - self.stop - self.step) <= 0:
52 |             return TaskStatus.SUCCESS
53 |         else:
54 |             print self.name, self.count
55 |             time.sleep(0.5)
56 |             self.count += self.step
57 |             if abs(self.count - self.stop - self.step) <= 0:
58 |                 return TaskStatus.SUCCESS
59 |             else:
60 |                 return TaskStatus.RUNNING
61 | 
62 |     
63 |     def reset(self):
64 |         self.count = self.start
65 | 
66 | if __name__ == '__main__':
67 |     tree = ParallelExample()
68 | 
69 | 


--------------------------------------------------------------------------------
/pi_trees_lib/examples/patrol_tree.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | import rospy
  4 | from std_msgs.msg import Float32
  5 | from geometry_msgs.msg import Twist
  6 | from rbx2_msgs.srv import *
  7 | from pi_trees_ros.pi_trees_ros import *
  8 | from pi_trees_lib.task_setup import *
  9 | 
 10 | class BlackBoard():
 11 |     def __init__(self):
 12 |         self.battery_level = None
 13 |         self.charging = None
 14 | 
 15 | class Patrol():
 16 |     def __init__(self):
 17 |         rospy.init_node("patrol_tree")
 18 | 
 19 |         # Set the shutdown function (stop the robot)
 20 |         rospy.on_shutdown(self.shutdown)
 21 |         
 22 |         # Initialize a number of parameters and variables
 23 |         setup_task_environment(self)
 24 |         
 25 |         # Initialize the black board
 26 |         self.blackboard = BlackBoard()
 27 |         
 28 |         # How frequently do we "tic" the tree?
 29 |         rate = rospy.get_param('~rate', 10)
 30 |         
 31 |         # Convert tic rate to a ROS rate
 32 |         tic = rospy.Rate(rate)
 33 |         
 34 |         # Where should the DOT file be stored.  Default location is $HOME/.ros/tree.dot
 35 |         dotfilepath = rospy.get_param('~dotfilepath', None)
 36 | 
 37 |         # Create a list to hold the move_base tasks
 38 |         MOVE_BASE_TASKS = list()
 39 |         
 40 |         n_waypoints = len(self.waypoints)
 41 |         
 42 |         # Create simple action navigation task for each waypoint
 43 |         for i in range(n_waypoints):
 44 |             goal = MoveBaseGoal()
 45 |             goal.target_pose.header.frame_id = 'map'
 46 |             goal.target_pose.header.stamp = rospy.Time.now()
 47 |             goal.target_pose.pose = self.waypoints[i % n_waypoints]
 48 |             
 49 |             move_base_task = SimpleActionTask("MOVE_BASE_TASK_" + str(i), "move_base", MoveBaseAction, goal, reset_after=False)
 50 |             
 51 |             MOVE_BASE_TASKS.append(move_base_task)
 52 |         
 53 |         # Set the docking station pose
 54 |         goal = MoveBaseGoal()
 55 |         goal.target_pose.header.frame_id = 'map'
 56 |         goal.target_pose.header.stamp = rospy.Time.now()
 57 |         goal.target_pose.pose = self.docking_station_pose
 58 |         
 59 |         # Assign the docking station pose to a move_base action task
 60 |         NAV_DOCK_TASK = SimpleActionTask("NAV_DOC_TASK", "move_base", MoveBaseAction, goal, reset_after=False)
 61 |         
 62 |         # Create the root node
 63 |         BEHAVE = Sequence("BEHAVE")
 64 |         
 65 |         # Create the "stay healthy" selector
 66 |         STAY_HEALTHY = Selector("STAY_HEALTHY")
 67 |         
 68 |         # Create the patrol loop decorator
 69 |         LOOP_PATROL = Loop("LOOP_PATROL", iterations=self.n_patrols)
 70 |         
 71 |         # Add the two subtrees to the root node in order of priority
 72 |         BEHAVE.add_child(STAY_HEALTHY)
 73 |         BEHAVE.add_child(LOOP_PATROL)
 74 |         
 75 |         # Create the patrol iterator
 76 |         PATROL = Iterator("PATROL")
 77 |         
 78 |         # Add the move_base tasks to the patrol task
 79 |         for task in MOVE_BASE_TASKS:
 80 |             PATROL.add_child(task)
 81 |   
 82 |         # Add the patrol to the loop decorator
 83 |         LOOP_PATROL.add_child(PATROL)
 84 |         
 85 |         # Add the battery check and recharge tasks to the "stay healthy" task
 86 |         with STAY_HEALTHY:
 87 |             # Monitor the fake battery level by subscribing to the /battery_level topic
 88 |             MONITOR_BATTERY = MonitorTask("MONITOR_BATTERY", "battery_level", Float32, self.monitor_battery)
 89 |             
 90 |             # Is the fake battery level below threshold?
 91 |             CHECK_BATTERY = CallbackTask("BATTERY_OK?", self.check_battery)  
 92 |             
 93 |             # Set the fake battery level back to 100 using a ServiceTask
 94 |             CHARGE_COMPLETE = ServiceTask("CHARGE_COMPLETE", "/battery_simulator/set_battery_level", SetBatteryLevel, 100, result_cb=self.recharge_cb)
 95 |             
 96 |             # Sleep for the given interval to simulate charging
 97 |             CHARGING = RechargeRobot("CHARGING", interval=3, blackboard=self.blackboard)
 98 |       
 99 |             # Build the recharge sequence using inline construction
100 |             RECHARGE = Sequence("RECHARGE", [NAV_DOCK_TASK, CHARGING, CHARGE_COMPLETE], reset_after=True)
101 |                 
102 |             # Add the check battery and recharge tasks to the stay healthy selector
103 |             STAY_HEALTHY.add_child(CHECK_BATTERY)
104 |             STAY_HEALTHY.add_child(RECHARGE)
105 |                 
106 |         # Display the tree before beginning execution
107 |         print "Patrol Behavior Tree"
108 |         print_tree(BEHAVE)
109 |         
110 |         # Run the tree
111 |         while not rospy.is_shutdown():
112 |             BEHAVE.status = BEHAVE.run()
113 |             tic.sleep()
114 |             print_dot_tree(BEHAVE, dotfilepath)
115 |             
116 |     def monitor_battery(self, msg):
117 |         # Store the battery level as published on the fake battery level topic
118 |         self.blackboard.battery_level = msg.data
119 |         return True
120 |     
121 |     def check_battery(self):
122 |         # Don't run the check if we are charging
123 |         if self.blackboard.charging:
124 |             return False
125 |         
126 |         if self.blackboard.battery_level is None:
127 |             return None
128 |         elif self.blackboard.battery_level < self.low_battery_threshold:
129 |             rospy.loginfo("LOW BATTERY - level: " + str(int(self.blackboard.battery_level)))
130 |             return False
131 |         else:
132 |             return True
133 |     
134 |     def recharge_cb(self, result):
135 |         rospy.loginfo("BATTERY CHARGED!")
136 |         self.blackboard.battery_level = 100
137 |         self.blackboard.charging = False
138 |         rospy.sleep(2)
139 |         return True
140 |             
141 |     def shutdown(self):
142 |         rospy.loginfo("Stopping the robot...")
143 |         self.move_base.cancel_all_goals()
144 |         self.cmd_vel_pub.publish(Twist())
145 |         rospy.sleep(1)
146 |         
147 | class RechargeRobot(Task):
148 |     def __init__(self, name, interval=3, blackboard=None):
149 |         super(RechargeRobot, self).__init__(name)
150 |        
151 |         self.name = name
152 |         self.interval = interval
153 |         self.blackboard = blackboard
154 |         
155 |         self.timer = 0
156 |          
157 |     def run(self):
158 |         if self.timer == 0:
159 |             rospy.loginfo("CHARGING THE ROBOT!")
160 |             
161 |         if self.timer < self.interval:
162 |             self.timer += 0.1
163 |             rospy.sleep(0.1)
164 |             self.blackboard.charging = True
165 |             return TaskStatus.RUNNING
166 |         else:
167 |             return TaskStatus.SUCCESS
168 |     
169 |     def reset(self):
170 |         self.status = None
171 |         self.timer = 0
172 | 
173 | if __name__ == '__main__':
174 |     tree = Patrol()
175 | 
176 | 


--------------------------------------------------------------------------------
/pi_trees_lib/examples/weighted_random_example.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from pi_trees_lib.pi_trees_lib import *
 4 | import time
 5 | 
 6 | class WeightedRandomExample():
 7 |     def __init__(self):
 8 |         # The root node
 9 |         BEHAVE = Sequence("behave")
10 |         
11 |         # Create a ParallelOne composite task (returns SUCCESS as soon as any subtask returns SUCCESS)
12 |         WEIGHTED_RANDOM_TASKS = WeightedRandomSelector("Weighted Random Selector", weights=[3, 2, 1], reset_after=True)
13 |         
14 |         # Create three counting tasks
15 |         TASK1 = Message("High_Probability", "Highest Probability Task")
16 |         TASK2 = Message("Medium_Probability", "Medium Probability Task")
17 |         TASK3 = Message("Low_Probability", "Lowest Probability Task")
18 | 
19 |         # Add the tasks to the parallel composite task
20 |         WEIGHTED_RANDOM_TASKS.add_child(TASK1)
21 |         WEIGHTED_RANDOM_TASKS.add_child(TASK2)
22 |         WEIGHTED_RANDOM_TASKS.add_child(TASK3)
23 |         
24 |         LOOP = Loop("Loop_Forever", iterations=-1)
25 |         
26 |         LOOP.add_child(WEIGHTED_RANDOM_TASKS)
27 |         
28 |         # Add the composite task to the root task
29 |         BEHAVE.add_child(LOOP)
30 |         
31 |         # Print a simple representation of the tree
32 |         print "Behavior Tree Structure"
33 |         print_tree(BEHAVE)
34 |             
35 |         # Run the tree
36 |         while True:
37 |             status = BEHAVE.run()
38 |             if status == TaskStatus.SUCCESS:
39 |                 print "Finished running tree."
40 |                 break
41 | 
42 | # A task to print a message
43 | class Message(Task):
44 |     def __init__(self, name, message, *args, **kwargs):
45 |         super(Message, self).__init__(name, *args, **kwargs)
46 |         
47 |         self.name = name
48 |         self.message = message
49 |         print "Creating message task: ", self.message
50 |  
51 |     def run(self):
52 |         print self.message
53 |         time.sleep(1)
54 | 
55 |         return TaskStatus.SUCCESS
56 | 
57 | 
58 | if __name__ == '__main__':
59 |     tree = WeightedRandomExample()
60 | 
61 | 


--------------------------------------------------------------------------------
/pi_trees_lib/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 | <name>pi_trees_lib</name>
 3 | <version>0.1.1</version>
 4 | <description>Python/ROS Library for Behavior Trees</description>
 5 | <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
 6 | <license>BSD</license>
 7 | <url type="website">http://ros.org/wiki/pi_trees</url>
 8 | <url type="bugtracker">https://github.com/pirobot/pi_trees/issues</url>
 9 | <author email="patrick@pirobot.org">Patrick Goebel</author>
10 | 
11 | <buildtool_depend>catkin</buildtool_depend>
12 | 
13 | <run_depend>rospy</run_depend>
14 | 
15 | <!-- Exclude for now -->
16 | <!-- exec_depend>python-pygraphviz</exec_depend -->
17 | <!-- exec_depend>python-pygraph</exec_depend -->>
18 | 
19 | </package>


--------------------------------------------------------------------------------
/pi_trees_lib/setup.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from distutils.core import setup
 4 | from catkin_pkg.python_setup import generate_distutils_setup
 5 | 
 6 | d = generate_distutils_setup(
 7 |     packages=['pi_trees_lib'],
 8 |     package_dir={'': 'src'},
 9 |     )
10 | 
11 | setup(**d)
12 | 


--------------------------------------------------------------------------------
/pi_trees_lib/src/pi_trees_lib/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pirobot/pi_trees/83738b675c3c197f4afb6c12f5b99b4e8444e707/pi_trees_lib/src/pi_trees_lib/__init__.py


--------------------------------------------------------------------------------
/pi_trees_lib/src/pi_trees_lib/pi_trees_lib.py:
--------------------------------------------------------------------------------
   1 | #!/usr/bin/env python
   2 | 
   3 | import string
   4 | from random import random, shuffle
   5 | 
   6 | import os
   7 | 
   8 | import pygraphviz as pgv
   9 | 
  10 | from time import sleep
  11 | 
  12 | class TaskStatus(object):
  13 |     """ A class for enumerating task statuses """
  14 |     FAILURE = 0
  15 |     SUCCESS = 1
  16 |     RUNNING = 2
  17 | 
  18 | # A global value to track when the tree's status has changed.  Used in the print_dot_tree() function.
  19 | last_dot_tree = ''
  20 | 
  21 | def weighted_choice(weights):
  22 |     rnd = random() * sum(weights)
  23 |     for i, w in enumerate(weights):
  24 |         rnd -= w
  25 |         if rnd < 0:
  26 |             return i
  27 | 
  28 | def weighted_shuffle(a,w):
  29 |     w = list(w) # make a copy of w
  30 |     if len(a) != len(w):
  31 |         print("ERROR: Weighted_shuffle: Number of weights does not match number of children.")
  32 |         os._exit(0)
  33 |         return
  34 | 
  35 |     r = [0]*len(a) # contains the random shuffle
  36 |     for i in range(len(a)):
  37 |         j = weighted_choice(w)
  38 |         r[i]=a[j]
  39 |         w[j] = 0
  40 |     return r
  41 |     
  42 | class Task(object):
  43 |     """ "The base Task class """
  44 |     def __init__(self, name, children=None, reset_after=False, announce=False, *args, **kwargs):
  45 |         self.name = name
  46 |         self.status = None
  47 |         self.reset_after = reset_after
  48 |         self._announce = announce
  49 |                 
  50 |         if children is None:
  51 |             children = []
  52 |             
  53 |         self.children = children
  54 |                          
  55 |     def run(self):
  56 |         pass
  57 | 
  58 |     def reset(self):
  59 |         for c in self.children:
  60 |             c.reset()
  61 |             
  62 |         self.status = None
  63 | 
  64 |     def add_child(self, c):
  65 |         self.children.append(c)
  66 | 
  67 |     def remove_child(self, c):
  68 |         self.children.remove(c)
  69 |         
  70 |     def prepend_child(self, c):
  71 |         self.children.insert(0, c)
  72 |         
  73 |     def insert_child(self, c, i):
  74 |         self.children.insert(i, c)
  75 |         
  76 |     def get_status(self):
  77 |         return self.status
  78 |     
  79 |     def set_status(self, s):
  80 |         self.status = s
  81 |     
  82 |     def announce(self):
  83 |         print("Executing task " + str(self.name))
  84 |         
  85 |     def get_type(self):
  86 |         return type(self)
  87 |     
  88 |     # These next two functions allow us to use the 'with' syntax
  89 |     def __enter__(self):
  90 |         return self.name
  91 |     
  92 |     def __exit__(self, exc_type, exc_val, exc_tb):
  93 |         if  exc_type is not None:
  94 |             return False
  95 |         return True
  96 |     
  97 | class Selector(Task):
  98 |     """ A selector runs each task in order until one succeeds,
  99 |         at which point it returns SUCCESS. If all tasks fail, a FAILURE
 100 |         status is returned.  If a subtask is still RUNNING, then a RUNNING
 101 |         status is returned and processing continues until either SUCCESS
 102 |         or FAILURE is returned from the subtask.
 103 |     """
 104 |     def __init__(self, name, *args, **kwargs):
 105 |         super(Selector, self).__init__(name, *args, **kwargs)
 106 |  
 107 |     def run(self):
 108 |         for c in self.children:
 109 |             
 110 |             c.status = c.run()
 111 |             
 112 |             if c.status != TaskStatus.FAILURE:
 113 |                 if c.status == TaskStatus.SUCCESS:
 114 |                     if self.reset_after:
 115 |                         self.reset()
 116 |                         return TaskStatus.SUCCESS
 117 |                     else:
 118 |                         return c.status
 119 |                 return c.status
 120 |             
 121 |         if self.reset_after:
 122 |             self.reset()
 123 |             
 124 |         return TaskStatus.FAILURE
 125 |  
 126 | class Sequence(Task):
 127 |     """
 128 |         A sequence runs each task in order until one fails,
 129 |         at which point it returns FAILURE. If all tasks succeed, a SUCCESS
 130 |         status is returned.  If a subtask is still RUNNING, then a RUNNING
 131 |         status is returned and processing continues until either SUCCESS
 132 |         or FAILURE is returned from the subtask.
 133 |     """
 134 |     def __init__(self, name, *args, **kwargs):
 135 |         super(Sequence, self).__init__(name, *args, **kwargs)
 136 |  
 137 |     def run(self):
 138 |         if self._announce:
 139 |             self.announce()
 140 |             
 141 |         for c in self.children:
 142 |             
 143 |             c.status = c.run()
 144 |                          
 145 |             if c.status != TaskStatus.SUCCESS:
 146 |                 if c.status == TaskStatus.FAILURE:
 147 |                     if self.reset_after:
 148 |                         self.reset()
 149 |                         return TaskStatus.FAILURE
 150 |                 return c.status   
 151 |         
 152 |         if self.reset_after:
 153 |             self.reset()
 154 |         
 155 |         return TaskStatus.SUCCESS
 156 |     
 157 | class RandomSelector(Task):
 158 |     """ A selector runs each task in random order until one succeeds,
 159 |         at which point it returns SUCCESS. If all tasks fail, a FAILURE
 160 |         status is returned.  If a subtask is still RUNNING, then a RUNNING
 161 |         status is returned and processing continues until either SUCCESS
 162 |         or FAILURE is returned from the subtask.
 163 |     """
 164 |     def __init__(self, name, *args, **kwargs):
 165 |         super(RandomSelector, self).__init__(name, *args, **kwargs)
 166 |         
 167 |         self.shuffled = False
 168 |  
 169 |     def run(self):
 170 |         if not self.shuffled:
 171 |             shuffle(self.children)
 172 |             self.shuffled = True
 173 |                     
 174 |         for c in self.children:
 175 |             
 176 |             c.status = c.run()
 177 |             
 178 |             if c.status != TaskStatus.FAILURE:
 179 |                 if c.status == TaskStatus.SUCCESS:
 180 |                     self.shuffled = False
 181 | 
 182 |                 return c.status
 183 | 
 184 |         self.shuffled = False
 185 |         
 186 |         if self.reset_after:
 187 |             self.reset()
 188 | 
 189 |         return TaskStatus.FAILURE
 190 |     
 191 | class RandomSequence(Task):
 192 |     """
 193 |         A sequence runs each task in random order until one fails,
 194 |         at which point it returns FAILURE. If all tasks succeed, a SUCCESS
 195 |         status is returned.  If a subtask is still RUNNING, then a RUNNING
 196 |         status is returned and processing continues until either SUCCESS
 197 |         or FAILURE is returned from the subtask.
 198 |     """
 199 |     def __init__(self, name, *args, **kwargs):
 200 |         super(RandomSequence, self).__init__(name, *args, **kwargs)
 201 |         
 202 |         self.shuffled = False
 203 |  
 204 |     def run(self):
 205 |         if not self.shuffled:
 206 |             shuffle(self.children)
 207 |             self.shuffled = True
 208 |         
 209 |         for c in self.children:
 210 |             
 211 |             c.status = c.run()
 212 |                          
 213 |             if c.status != TaskStatus.SUCCESS:
 214 |                 if c.status == TaskStatus.FAILURE:
 215 |                     self.shuffled = False
 216 |                     
 217 |                 return c.status   
 218 | 
 219 |         self.shuffled = False
 220 |         
 221 |         if self.reset_after:
 222 |             self.reset()
 223 | 
 224 |         return TaskStatus.SUCCESS
 225 |     
 226 | class WeightedRandomSequence(Task):
 227 |     """
 228 |         A sequence runs each task in random order until one fails,
 229 |         at which point it returns FAILURE. If all tasks succeed, a SUCCESS
 230 |         status is returned.  If a subtask is still RUNNING, then a RUNNING
 231 |         status is returned and processing continues until either SUCCESS
 232 |         or FAILURE is returned from the subtask.
 233 |     """
 234 |     def __init__(self, name, weights, *args, **kwargs):
 235 |         super(WeightedRandomSequence, self).__init__(name, *args, **kwargs)
 236 |         
 237 |         self.shuffled = False
 238 |         self.weights = weights
 239 |  
 240 |     def run(self):
 241 |         if not self.shuffled:
 242 |             shuffled_children = weighted_shuffle(self.children, self.weights)
 243 |             self.shuffled = True
 244 |         
 245 |         for c in shuffled_children:
 246 |             
 247 |             c.status = c.run()
 248 |                          
 249 |             if c.status != TaskStatus.SUCCESS:
 250 |                 if c.status == TaskStatus.FAILURE:
 251 |                     self.shuffled = False
 252 |                     
 253 |                 return c.status   
 254 | 
 255 |         self.shuffled = False
 256 |         
 257 |         if self.reset_after:
 258 |             self.reset()
 259 | 
 260 |         return TaskStatus.SUCCESS
 261 |     
 262 | class WeightedRandomSelector(Task):
 263 |     """ A selector runs each task in random order until one succeeds,
 264 |         at which point it returns SUCCESS. If all tasks fail, a FAILURE
 265 |         status is returned.  If a subtask is still RUNNING, then a RUNNING
 266 |         status is returned and processing continues until either SUCCESS
 267 |         or FAILURE is returned from the subtask.
 268 |     """
 269 |     def __init__(self, name, weights, *args, **kwargs):
 270 |         super(WeightedRandomSelector, self).__init__(name, *args, **kwargs)
 271 |         
 272 |         self.shuffled = False
 273 |         self.weights = weights
 274 |  
 275 |     def run(self):
 276 |         if not self.shuffled:
 277 |             shuffled_children = weighted_shuffle(self.children, self.weights)
 278 |             self.shuffled = True
 279 |                     
 280 |         for c in shuffled_children:
 281 |             
 282 |             c.status = c.run()
 283 |             
 284 |             if c.status != TaskStatus.FAILURE:
 285 |                 if c.status == TaskStatus.SUCCESS:
 286 |                     self.shuffled = False
 287 | 
 288 |                 return c.status
 289 | 
 290 |         self.shuffled = False
 291 |         
 292 |         if self.reset_after:
 293 |             self.reset()
 294 | 
 295 |         return TaskStatus.FAILURE
 296 |     
 297 | class Iterator(Task):
 298 |     """
 299 |         Iterate through all child tasks ignoring failure.
 300 |     """
 301 |     def __init__(self, name, *args, **kwargs):
 302 |         super(Iterator, self).__init__(name, *args, **kwargs)
 303 |  
 304 |     def run(self):
 305 |         for c in self.children:
 306 |                         
 307 |             c.status = c.run()
 308 |                          
 309 |             if c.status == TaskStatus.RUNNING:
 310 |                 return c.status
 311 |             
 312 |         if self.reset_after:
 313 |             self.reset()
 314 |             
 315 |         return TaskStatus.SUCCESS
 316 |     
 317 | class RandomIterator(Task):
 318 |     """
 319 |         Iterate through all child tasks randomly (without replacement) ignoring failure.
 320 |     """
 321 |     def __init__(self, name, *args, **kwargs):
 322 |         super(RandomIterator, self).__init__(name, *args, **kwargs)
 323 |         
 324 |         self.shuffled = False
 325 |  
 326 |     def run(self):
 327 |         if not self.shuffled:
 328 |             shuffle(self.children)
 329 |             self.shuffled = True
 330 | 
 331 |         for c in self.children:
 332 |             
 333 |             c.status = c.run()
 334 |                          
 335 |             if c.status == TaskStatus.RUNNING:
 336 |                 return c.status
 337 |             
 338 |         self.shuffled = False
 339 |         
 340 |         if self.reset_after:
 341 |             self.reset()
 342 | 
 343 |         return TaskStatus.SUCCESS
 344 |     
 345 | class WeightedRandomIterator(Task):
 346 |     """
 347 |         Iterate through all child tasks randomly (without replacement) ignoring failure.
 348 |     """
 349 |     def __init__(self, name, weights, *args, **kwargs):
 350 |         super(WeightedRandomIterator, self).__init__(name, *args, **kwargs)
 351 |         
 352 |         self.shuffled = False
 353 |         self.weights = weights
 354 |  
 355 |     def run(self):
 356 |         if not self.shuffled:
 357 |             suffled_children = weighted_shuffle(self.children, self.weights)
 358 |             self.shuffled = True
 359 | 
 360 |         for c in suffled_children:
 361 |             
 362 |             c.status = c.run()
 363 |                          
 364 |             if c.status == TaskStatus.RUNNING:
 365 |                 return c.status
 366 |             
 367 |         self.shuffled = False
 368 |         
 369 |         if self.reset_after:
 370 |             self.reset()
 371 | 
 372 |         return TaskStatus.SUCCESS
 373 |     
 374 | class ParallelOne(Task):
 375 |     """
 376 |         A parallel task runs each child task at (roughly) the same time.
 377 |         The ParallelOne task returns success as soon as any child succeeds.
 378 |     """
 379 |     def __init__(self, name, *args, **kwargs):
 380 |         super(ParallelOne, self).__init__(name, *args, **kwargs)
 381 |         
 382 |         self.failure = dict()
 383 |         self.index = 0
 384 |                  
 385 |     def run(self):
 386 |         n_children = len(self.children)
 387 | 
 388 |         if self.index < n_children:
 389 |             child = self.children[self.index]
 390 |             child.status = child.run()
 391 |             
 392 |             if child.status != TaskStatus.SUCCESS:
 393 |                 self.index += 1
 394 |                 
 395 |                 if child.status == TaskStatus.FAILURE:
 396 |                     self.failure[child.name] = TaskStatus.FAILURE
 397 |                     
 398 |                 return TaskStatus.RUNNING
 399 |             else:
 400 |                 if self.reset_after:
 401 |                     self.reset()
 402 |                     self.index = 0
 403 |                 return TaskStatus.SUCCESS
 404 | 
 405 |         elif len(self.failure) == n_children:
 406 |             if self.reset_after:
 407 |                 self.reset()
 408 |             return TaskStatus.FAILURE
 409 |         else:
 410 |             self.index = 0
 411 |             return TaskStatus.RUNNING
 412 |         
 413 |     def reset(self):
 414 |         super(ParallelOne, self).reset()
 415 |         self.failure = dict()
 416 |         self.index = 0
 417 |         
 418 | class ParallelAll(Task):
 419 |     """
 420 |         A parallel task runs each child task at (roughly) the same time.
 421 |         The ParallelAll task requires all subtasks to succeed for it to succeed.
 422 |     """
 423 |     def __init__(self, name, *args, **kwargs):
 424 |         super(ParallelAll, self).__init__(name, *args, **kwargs)
 425 |         
 426 |         self.success = dict()
 427 |         self.index = 0
 428 |                  
 429 |     def run(self):
 430 |         n_children = len(self.children)
 431 |         
 432 |         if self.index < n_children:
 433 |             child = self.children[self.index]
 434 |             child.status = child.run()
 435 |         
 436 |             if child.status != TaskStatus.FAILURE:
 437 |                 self.index += 1
 438 |                 
 439 |                 if child.status == TaskStatus.SUCCESS:
 440 |                     self.success[child.name] = TaskStatus.SUCCESS
 441 |                     
 442 |                 return TaskStatus.RUNNING
 443 |             else:
 444 |                 if self.reset_after:
 445 |                     self.reset()
 446 |                 return TaskStatus.FAILURE
 447 | 
 448 |         elif len(self.success) == n_children:
 449 |             if self.reset_after:
 450 |                 self.reset()
 451 |             return TaskStatus.SUCCESS
 452 |         else:
 453 |             self.index = 0
 454 |             return TaskStatus.RUNNING
 455 |         
 456 |     def reset(self):
 457 |         super(ParallelAll, self).reset()
 458 |         self.success = dict()
 459 |         self.index = 0
 460 |         
 461 | class Loop(Task):
 462 |     """
 463 |         Loop over one or more subtasks for the given number of iterations
 464 |         Use the value -1 to indicate a continual loop.
 465 |     """
 466 |     def __init__(self, name, announce=True, *args, **kwargs):
 467 |         super(Loop, self).__init__(name, *args, **kwargs)
 468 | 
 469 |         self.iterations = kwargs['iterations']
 470 |         self._announce = announce
 471 |         self.loop_count = 0
 472 |         self.name = name
 473 |         print("Loop iterations: " + str(self.iterations))
 474 | 
 475 |     def run(self):
 476 | 
 477 |         c = self.children[0]
 478 | 
 479 |         while self.iterations == -1 or self.loop_count < self.iterations:
 480 |             c.status = c.run()
 481 | 
 482 |             status = c.status
 483 | 
 484 |             self.status = status
 485 | 
 486 |             if status == TaskStatus.SUCCESS or status == TaskStatus.FAILURE:
 487 |                 self.loop_count += 1
 488 | 
 489 |                 if self._announce:
 490 |                     print(self.name + " COMPLETED " + str(self.loop_count) + " LOOP(S)")
 491 | 
 492 |                 c.reset()
 493 | 
 494 |             return status
 495 | 
 496 | class Limit(Task):
 497 |     """
 498 |         Limit the number of times a task can execute
 499 |     """
 500 |     def __init__(self, name, announce=True, *args, **kwargs):
 501 |         super(Limit, self).__init__(name, *args, **kwargs)
 502 |         
 503 |         self.max_executions = kwargs['max_executions']
 504 |         self._announce = announce
 505 |         self.execution_count = 0
 506 |         self.name = name
 507 |         print("Limit number of executions to: " + str(self.max_executions))
 508 |         
 509 |     def run(self):
 510 |         c = self.children[0]
 511 | 
 512 |         if self.execution_count >= self.max_executions:
 513 | 
 514 |             if self._announce:
 515 |                 print(self.name + " reached maximum number (" + str(self.max_executions) + ") of executions.")
 516 |                 
 517 |             if self.reset_after:
 518 |                 self.reset()
 519 | 
 520 |             return TaskStatus.FAILURE
 521 |                     
 522 |         c.status = c.run()
 523 | 
 524 |         if c.status != TaskStatus.RUNNING:
 525 |             self.execution_count += 1
 526 | 
 527 |         return c.status
 528 | 
 529 |     def reset(self):
 530 |         c = self.children[0]
 531 |         c.reset()
 532 |         self.execution_count = 0
 533 |         self.status = None
 534 | 
 535 | class IgnoreFailure(Task):
 536 |     """
 537 |         Always return either RUNNING or SUCCESS.
 538 |     """
 539 |     def __init__(self, name, *args, **kwargs):
 540 |         super(IgnoreFailure, self).__init__(name, *args, **kwargs)
 541 |  
 542 |     def run(self):
 543 |         
 544 |         for c in self.children:
 545 |             
 546 |             c.status = c.run()
 547 |             
 548 |             if c.status == TaskStatus.FAILURE:
 549 |                 return TaskStatus.SUCCESS
 550 |             else:
 551 |                 return c.status
 552 | 
 553 |         return TaskStatus.SUCCESS
 554 |     
 555 |     
 556 | class AlwaysFail(Task):
 557 |     """
 558 |         Always return FAILURE
 559 |     """
 560 |     def __init__(self, name, *args, **kwargs):
 561 |         super(AlwaysFail, self).__init__(name, *args, **kwargs)
 562 |  
 563 |     def run(self):
 564 |         
 565 |         for c in self.children:
 566 |             
 567 |             c.status = c.run()
 568 |             
 569 |             return c.status
 570 | 
 571 |         return TaskStatus.FAILURE
 572 |     
 573 | class AlwaysSucceed(Task):
 574 |     """
 575 |         Always return SUCCESS
 576 |     """
 577 |     def __init__(self, name, *args, **kwargs):
 578 |         super(AlwaysSucceed, self).__init__(name, *args, **kwargs)
 579 |  
 580 |     def run(self):
 581 |         
 582 |         for c in self.children:
 583 |             
 584 |             c.status = c.run()
 585 |             
 586 |             return c.status
 587 | 
 588 |         return TaskStatus.SUCCESS
 589 |     
 590 |     
 591 | class Invert(Task):
 592 |     """
 593 |         Turn SUCCESS into FAILURE and vice-versa
 594 |     """
 595 |     def __init__(self, name, *args, **kwargs):
 596 |         super(Invert, self).__init__(name, *args, **kwargs)
 597 |  
 598 |     def run(self):
 599 |         
 600 |         for c in self.children:
 601 |             
 602 |             c.status = c.run()
 603 |             
 604 |             if c.status == TaskStatus.FAILURE:
 605 |                 return TaskStatus.SUCCESS
 606 |             
 607 |             elif c.status == TaskStatus.SUCCESS:
 608 |                 return TaskStatus.FAILURE
 609 |             
 610 |             else:
 611 |                 return c.status
 612 | 
 613 | # Alias TaskNot to Invert for backwards compatibility
 614 | TaskNot = Invert
 615 | 
 616 | class UntilFailure(Task):
 617 |     """
 618 |         Continue executing a task until it fails or max_attempts is reached.
 619 |     """
 620 |     def __init__(self, name, max_attempts=-1, *args, **kwargs):
 621 |         super(UntilFailure, self).__init__(name, *args, **kwargs)
 622 | 
 623 |         self.max_attempts = max_attempts
 624 |         self.attempts = 0
 625 | 
 626 |     def run(self):
 627 |         c = self.children[0]
 628 | 
 629 |         while self.attempts < self.max_attempts or self.max_attempts == -1:
 630 | 
 631 |             c.status = c.run()
 632 | 
 633 |             if c.status == TaskStatus.FAILURE:
 634 |                 return TaskStatus.SUCCESS
 635 | 
 636 |             if c.status == TaskStatus.SUCCESS:
 637 |                 self.attempts += 1
 638 |                 c.reset()
 639 |             
 640 |             return TaskStatus.RUNNING
 641 |         
 642 |         return TaskStatus.FAILURE
 643 | 
 644 |     def reset(self):
 645 |         c = self.children[0]
 646 |         c.reset()
 647 |         self.attempts = 0
 648 | 
 649 | # Alias UntilFail to UntilFailure for backwards compatibility
 650 | UntilFail = UntilFailure
 651 | 
 652 | class UntilSuccess(Task):
 653 |     """
 654 |         Continue executing a task until it succeeds or max_attempts is reached.
 655 |     """
 656 |     def __init__(self, name, max_attempts=-1, *args, **kwargs):
 657 |         super(UntilSuccess, self).__init__(name, *args, **kwargs)
 658 | 
 659 |         self.max_attempts = max_attempts
 660 |         self.attempts = 0
 661 | 
 662 |     def run(self):
 663 |         c = self.children[0]
 664 | 
 665 |         while self.attempts < self.max_attempts or self.max_attempts == -1:
 666 | 
 667 |             c.status = c.run()
 668 |             
 669 |             if c.status == TaskStatus.SUCCESS:
 670 |                 return TaskStatus.SUCCESS
 671 | 
 672 |             if c.status == TaskStatus.FAILURE:
 673 |                 self.attempts += 1
 674 |                 c.reset()
 675 |             
 676 |             return TaskStatus.RUNNING
 677 |         
 678 |         if self.reset_after:
 679 |             self.reset()
 680 | 
 681 |         return TaskStatus.FAILURE
 682 | 
 683 |     def reset(self):
 684 |         c = self.children[0]
 685 |         c.reset()
 686 |         self.attempts = 0
 687 | 
 688 | class AutoRemoveSequence(Task):
 689 |     """ 
 690 |         Remove each successful subtask from a sequence 
 691 |     """
 692 |     def __init__(self, name, *args, **kwargs):
 693 |         super(AutoRemoveSequence, self).__init__(name, *args, **kwargs)
 694 |  
 695 |     def run(self):
 696 |         for c in self.children:
 697 |             c.status = c.run()
 698 |             
 699 |             if c.status == TaskStatus.FAILURE:
 700 |                 return TaskStatus.FAILURE
 701 |             
 702 |             if c.statuss == TaskStatus.RUNNING:
 703 |                 return TaskStatus.RUNNING
 704 |         
 705 |             try:
 706 |                 self.children.remove(self.children[0])
 707 |             except:
 708 |                 return TaskStatus.FAILURE
 709 |                 
 710 |         return TaskStatus.SUCCESS
 711 |     
 712 | class CallbackTask(Task):
 713 |     """ 
 714 |         Turn any callback function (cb) into a task
 715 |     """
 716 |     def __init__(self, name, cb=None, cb_args=[], cb_kwargs={}, **kwargs):
 717 |         super(CallbackTask, self).__init__(name, cb=None, cb_args=[], cb_kwargs={}, **kwargs)
 718 |         
 719 |         self.name = name
 720 |         self.cb = cb
 721 |         self.cb_args = cb_args
 722 |         self.cb_kwargs = cb_kwargs
 723 |   
 724 |     def run(self):
 725 |         status = self.cb(*self.cb_args, **self.cb_kwargs)
 726 |                 
 727 |         if status is None:
 728 |             self.status = TaskStatus.RUNNING
 729 |          
 730 |         elif status:
 731 |             self.status = TaskStatus.SUCCESS
 732 |  
 733 |         else:
 734 |             self.status = TaskStatus.FAILURE
 735 |             
 736 |         return self.status
 737 |     
 738 |     def reset(self):
 739 |         self.status = None
 740 |         
 741 | class WaitTask(Task):
 742 |     """
 743 |         This is a *blocking* wait task.  The interval argument is in seconds.
 744 |     """
 745 |     def __init__(self, name, interval, *args, **kwargs):
 746 |         super(WaitTask, self).__init__(name, interval, *args, **kwargs)
 747 |         
 748 |         self.interval = interval
 749 |  
 750 |     def run(self):
 751 |         sleep(self.interval)
 752 | 
 753 |         return TaskStatus.SUCCESS
 754 | 
 755 | class loop(Task):
 756 |     """
 757 |         Loop over one or more subtasks a given number of iterations
 758 |     """
 759 |     def __init__(self, task, iterations=-1):
 760 |         new_name = task.name + "_loop_" + str(iterations)
 761 |         super(loop, self).__init__(new_name)
 762 | 
 763 |         self.iterations = iterations
 764 |         self.old_run = task.run
 765 |         self.old_reset = task.reset
 766 |         self.old_children = task.children
 767 |         self.loop_count = 0
 768 |         
 769 |         print("Loop iterations: " + str(self.iterations))
 770 |         
 771 |     def run(self):
 772 |         if self.iterations != -1 and self.loop_count >= self.iterations:
 773 |             return TaskStatus.SUCCESS
 774 | 
 775 |         print("Loop " + str(self.loop_count + 1))
 776 |             
 777 |         while True:
 778 |             self.status = self.old_run()
 779 |             
 780 |             if self.status == TaskStatus.SUCCESS:
 781 |                 break
 782 |             else:
 783 |                 return self.status
 784 |                 
 785 |         self.old_reset()
 786 |         self.loop_count += 1
 787 |     
 788 | class limit(Task):
 789 |     """
 790 |         Limit a task to the given number of executions
 791 |     """
 792 |     def __init__(self, task, max_executions=-1):
 793 |         new_name = task.name + "_limit_" + str(max_executions)
 794 |         super(limit, self).__init__(new_name)
 795 | 
 796 |         self.max_executions = max_executions
 797 |         self.old_run = task.run
 798 |         self.old_reset = task.reset
 799 |         self.old_children = task.children
 800 |         self.execution_count = 0
 801 |         
 802 |         print("Limit number of executions to: " + str(self.max_executions))
 803 |         
 804 |     def run(self):
 805 |         if self.max_executions != -1 and self.execution_count >= self.max_executions:
 806 |             self.execution_count = 0
 807 |             
 808 |             if self._announce:
 809 |                 print(self.name + " reached maximum number (" + str(self.max_executions) + ") of executions.")
 810 |                 
 811 |             return TaskStatus.FAILURE
 812 |             
 813 |         while True:
 814 |             self.status = self.old_run()
 815 |             
 816 |             if self.status == TaskStatus.SUCCESS:
 817 |                 break
 818 |             else:
 819 |                 return self.status
 820 |                 
 821 |         self.old_reset()
 822 |         self.execution_count += 1
 823 |     
 824 | class ignore_failure(Task):
 825 |     """
 826 |         Always return either RUNNING or SUCCESS.
 827 |     """
 828 |     def __init__(self, task):
 829 |         new_name = task.name + "_ignore_failure"
 830 |         super(ignore_failure, self).__init__(new_name)
 831 | 
 832 |         self.old_run = task.run
 833 |         
 834 |     def run(self):
 835 |         while True:    
 836 |             self.status = self.old_run()
 837 |             
 838 |             if self.status == TaskStatus.FAILURE:
 839 |                 return TaskStatus.SUCCESS
 840 |             else:
 841 |                 return self.status
 842 |             
 843 | class ignore_success(Task):
 844 |     """
 845 |         Always return FAILURE or RUNNING
 846 |     """
 847 |     def __init__(self, task):
 848 |         new_name = task.name + "_ignore_success"
 849 |         super(ignore_success, self).__init__(new_name)
 850 | 
 851 |         self.old_run = task.run
 852 |         
 853 |     def run(self):
 854 |         while True:    
 855 |             self.status = self.old_run()
 856 |             
 857 |             if self.status == TaskStatus.SUCCESS:
 858 |                 return TaskStatus.FAILURE
 859 |             else:
 860 |                 return self.status
 861 | 
 862 | class task_not(Task):
 863 |     """
 864 |         Turn SUCCESS into FAILURE and vice-versa
 865 |     """
 866 |     def __init__(self, task):
 867 |         new_name = task.name + "_not"
 868 |         super(task_not, self).__init__(new_name)
 869 | 
 870 |         self.old_run = task.run
 871 |         
 872 |     def run(self):
 873 |         while True:    
 874 |             self.status = self.old_run()
 875 |             
 876 |             if self.status == TaskStatus.FAILURE:
 877 |                 return TaskStatus.SUCCESS
 878 |             
 879 |             elif self.status == TaskStatus.SUCCESS:
 880 |                 return TaskStatus.FAILURE
 881 |             
 882 |             else:
 883 |                 return self.status
 884 | 
 885 | # Alias task_not to invert which seems more intuitive
 886 | invert = task_not
 887 | 
 888 | class until_fail(Task):
 889 |     """
 890 |         Execute a task until it fails
 891 |     """
 892 |     def __init__(self, task):
 893 |         new_name = task.name + "_until_fail"
 894 |         super(until_fail, self).__init__(new_name)
 895 | 
 896 |         self.old_run = task.run
 897 |         
 898 |     def run(self):
 899 |         while True:    
 900 |             self.status = self.old_run()
 901 |             
 902 |             if self.status == TaskStatus.FAILURE:
 903 |                 break
 904 |             
 905 |             else:
 906 |                 return self.status
 907 |             
 908 |         return TaskStatus.SUCCESS
 909 |     
 910 | class always_fail(Task):
 911 |     """
 912 |         Execute a task but always return FAILTURE
 913 |     """
 914 |     def __init__(self, task):
 915 |         new_name = task.name + "_always_fail"
 916 |         super(always_fail, self).__init__(new_name)
 917 | 
 918 |         self.old_run = task.run
 919 |         
 920 |     def run(self):
 921 |         while True:    
 922 |             self.old_run()
 923 |             
 924 |             self.status = TaskStatus.FAILURE
 925 |             
 926 |         return TaskStatus.FAILURE
 927 |     
 928 |     
 929 | def print_tree(tree, indent=0, use_symbols=False):
 930 |     """
 931 |         Print an ASCII representation of the tree
 932 |     """
 933 |     if use_symbols:
 934 |         if indent == 0:
 935 |             print_tree_symbol(tree, indent)
 936 |             indent += 1
 937 | 
 938 |         for c in tree.children:
 939 |             print_tree_symbol(c, indent)
 940 | 
 941 |             try:
 942 |                 if c.children != []:
 943 |                     print_tree(c, indent+1, use_symbols)
 944 |             except:
 945 |                 pass
 946 |     else:
 947 |         for c in tree.children:
 948 |             print "    " * indent, "-->", c.name
 949 |              
 950 |             try:
 951 |                 if c.children != []:
 952 |                     print_tree(c, indent + 1)
 953 |             except:
 954 |                 pass
 955 |                 
 956 | def print_tree_symbol(c, indent):
 957 |     """
 958 |         Use ASCII symbols to represent Sequence, Selector, Task, etc.
 959 |     """
 960 |     if isinstance(c, Selector):
 961 |         print "    " * indent, "--?",
 962 |     elif isinstance(c, (Sequence, Iterator)):
 963 |         print "    " * indent, "-->",
 964 |     elif isinstance(c, (RandomSequence, RandomIterator, WeightedRandomSequence, WeightedRandomIterator)):
 965 |         print "    " * indent, "~~>",
 966 |     elif isinstance(c, (RandomSelector, WeightedRandomSelector)):
 967 |         print "    " * indent, "~~?",
 968 |     elif isinstance(c, ParallelOne):
 969 |         print "    " * indent, "==?",
 970 |     elif isinstance(c, ParallelAll):
 971 |         print "    " * indent, "==>",
 972 |     elif isinstance(c, Loop):
 973 |         print "    " * indent, "<->",
 974 |     elif isinstance(c, Invert):
 975 |         print "    " * indent, "--!",
 976 |     else:
 977 |         print "    " * indent, "--|",
 978 |     
 979 |     print c.name
 980 |             
 981 | def print_phpsyntax_tree(tree):    
 982 |     """
 983 |         Print an output compatible with ironcreek.net/phpSyntaxTree
 984 |     """
 985 |     for c in tree.children:
 986 |         print "[" + string.replace(c.name, "_", "."),
 987 |         if c.children != []:
 988 |             print_phpsyntax_tree(c),
 989 |         print "]",
 990 |     
 991 | def print_dot_tree(root, dotfilepath=None):
 992 |     """
 993 |         Print an output compatible with the DOT synatax and Graphiz
 994 |     """
 995 |     gr = pgv.AGraph(strict=True, directed=True, rotate='0', bgcolor='white', ordering="out")
 996 |     gr.node_attr['fontsize'] = '9'
 997 |     gr.node_attr['color'] = 'black'
 998 |     
 999 |     if dotfilepath is None:
1000 |         dotfilepath = os.path.expanduser('~') + '/.ros/tree.dot'
1001 |     
1002 |     global last_dot_tree
1003 |     
1004 |     # Add the root node
1005 |     gr.add_node(root.name)
1006 |     node = gr.get_node(root.name)
1007 |     
1008 |     if root.status == TaskStatus.RUNNING:
1009 |         node.attr['fillcolor'] = 'yellow'
1010 |         node.attr['style'] = 'filled'
1011 |         node.attr['border'] = 'bold'
1012 |     elif root.status == TaskStatus.SUCCESS:
1013 |         node.attr['color'] = 'green'
1014 |     elif root.status == TaskStatus.FAILURE:
1015 |         node.attr['color'] = 'red'
1016 |     else:
1017 |         node.attr['color'] = 'black'
1018 |                  
1019 |     def add_edges(root):
1020 |         for c in root.children:
1021 |             if isinstance(c, (Sequence, Iterator, RandomSequence, RandomIterator, WeightedRandomSequence, WeightedRandomIterator)):
1022 |                 gr.add_node(c.name, shape="cds")
1023 |             elif isinstance(c, (Selector, RandomSelector, WeightedRandomSelector)):
1024 |                 gr.add_node(c.name, shape="diamond")
1025 |             elif isinstance(c, (ParallelOne, ParallelAll)):
1026 |                 gr.add_node(c.name, shape="parallelogram")
1027 |             elif isinstance(c, Invert):
1028 |                 gr.add_node(c.name, shape="house")
1029 |             else:
1030 |                 gr.add_node(c.name)
1031 |                 
1032 |             gr.add_edge((root.name, c.name))
1033 |             node = gr.get_node(c.name)
1034 | 
1035 |             if c.status == TaskStatus.RUNNING:
1036 |                 node.attr['fillcolor'] = 'yellow'
1037 |                 node.attr['style'] = 'filled'
1038 |                 node.attr['border'] = 'bold'
1039 |             elif c.status == TaskStatus.SUCCESS:
1040 |                 node.attr['color'] = 'green'
1041 |             elif c.status == TaskStatus.FAILURE:
1042 |                 node.attr['color'] = 'red'
1043 |             else:
1044 |                 node.attr['color'] = 'black'
1045 | 
1046 |             if c.children != []:
1047 |                 add_edges(c)
1048 | 
1049 |     add_edges(root)
1050 |     
1051 |     current_dot_tree = gr.string()
1052 |         
1053 |     if current_dot_tree != last_dot_tree:
1054 |         gr.write(dotfilepath)
1055 |         last_dot_tree = gr.string()
1056 | 


--------------------------------------------------------------------------------
/pi_trees_lib/src/pi_trees_lib/task_setup.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | import rospy
  4 | import actionlib
  5 | from actionlib import GoalStatus
  6 | from geometry_msgs.msg import Pose, Point, Quaternion, Twist
  7 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
  8 | from tf.transformations import quaternion_from_euler
  9 | from visualization_msgs.msg import Marker
 10 | from math import  pi
 11 | from collections import OrderedDict
 12 | 
 13 | def setup_task_environment(self):
 14 |     # How big is the square we want the robot to patrol?
 15 |     self.square_size = rospy.get_param("~square_size", 1.0) # meters
 16 |     
 17 |     # Set the low battery threshold (between 0 and 100)
 18 |     self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
 19 |     
 20 |     # How many times should we execute the patrol loop
 21 |     self.n_patrols = rospy.get_param("~n_patrols", 2) # meters
 22 |     
 23 |     # How long do we have to get to each waypoint?
 24 |     self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds
 25 |     
 26 |     # Initialize the patrol counter
 27 |     self.patrol_count = 0
 28 |     
 29 |     # Subscribe to the move_base action server
 30 |     self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
 31 |     
 32 |     rospy.loginfo("Waiting for move_base action server...")
 33 |     
 34 |     # Wait up to 60 seconds for the action server to become available
 35 |     self.move_base.wait_for_server(rospy.Duration(60))    
 36 |     
 37 |     rospy.loginfo("Connected to move_base action server")
 38 |     
 39 |     # Create a list to hold the target quaternions (orientations)
 40 |     quaternions = list()
 41 |     
 42 |     # First define the corner orientations as Euler angles
 43 |     euler_angles = (pi/2, pi, 3*pi/2, 0)
 44 |     
 45 |     # Then convert the angles to quaternions
 46 |     for angle in euler_angles:
 47 |         q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
 48 |         q = Quaternion(*q_angle)
 49 |         quaternions.append(q)
 50 |     
 51 |     # Create a list to hold the waypoint poses
 52 |     self.waypoints = list()
 53 |             
 54 |     # Append each of the four waypoints to the list.  Each waypoint
 55 |     # is a pose consisting of a position and orientation in the map frame.
 56 |     self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
 57 |     self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
 58 |     self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1]))
 59 |     self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
 60 |     
 61 |     # Create a mapping of room names to waypoint locations
 62 |     room_locations = (('hallway', self.waypoints[0]),
 63 |                       ('living_room', self.waypoints[1]),
 64 |                       ('kitchen', self.waypoints[2]),
 65 |                       ('bathroom', self.waypoints[3]))
 66 |     
 67 |     # Store the mapping as an ordered dictionary so we can visit the rooms in sequence
 68 |     self.room_locations = OrderedDict(room_locations)
 69 |     
 70 |     # Where is the docking station?
 71 |     self.docking_station_pose = (Pose(Point(0.5, 0.5, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0)))            
 72 |     
 73 |     # Initialize the waypoint visualization markers for RViz
 74 |     init_waypoint_markers(self)
 75 |     
 76 |     # Set a visualization marker at each waypoint        
 77 |     for waypoint in self.waypoints:           
 78 |         p = Point()
 79 |         p = waypoint.position
 80 |         self.waypoint_markers.points.append(p)
 81 |         
 82 |     # Set a marker for the docking station
 83 |     init_docking_station_marker(self)
 84 |         
 85 |     # Publisher to manually control the robot (e.g. to stop it)
 86 |     self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
 87 |     
 88 |     rospy.loginfo("Starting Tasks")
 89 |     
 90 |     # Publish the waypoint markers
 91 |     self.marker_pub.publish(self.waypoint_markers)
 92 |     rospy.sleep(1)
 93 |     self.marker_pub.publish(self.waypoint_markers)
 94 |     
 95 |     # Publish the docking station marker
 96 |     self.docking_station_marker_pub.publish(self.docking_station_marker)
 97 |     rospy.sleep(1)
 98 | 
 99 | def init_waypoint_markers(self):
100 |     # Set up our waypoint markers
101 |     marker_scale = 0.2
102 |     marker_lifetime = 0 # 0 is forever
103 |     marker_ns = 'waypoints'
104 |     marker_id = 0
105 |     marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
106 |     
107 |     # Define a marker publisher.
108 |     self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5)
109 |     
110 |     # Initialize the marker points list.
111 |     self.waypoint_markers = Marker()
112 |     self.waypoint_markers.ns = marker_ns
113 |     self.waypoint_markers.id = marker_id
114 |     self.waypoint_markers.type = Marker.CUBE_LIST
115 |     self.waypoint_markers.action = Marker.ADD
116 |     self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
117 |     self.waypoint_markers.scale.x = marker_scale
118 |     self.waypoint_markers.scale.y = marker_scale
119 |     self.waypoint_markers.color.r = marker_color['r']
120 |     self.waypoint_markers.color.g = marker_color['g']
121 |     self.waypoint_markers.color.b = marker_color['b']
122 |     self.waypoint_markers.color.a = marker_color['a']
123 |     
124 |     self.waypoint_markers.header.frame_id = 'odom'
125 |     self.waypoint_markers.header.stamp = rospy.Time.now()
126 |     self.waypoint_markers.points = list()
127 | 
128 | def init_docking_station_marker(self):
129 |     # Define a marker for the charging station
130 |     marker_scale = 0.3
131 |     marker_lifetime = 0 # 0 is forever
132 |     marker_ns = 'waypoints'
133 |     marker_id = 0
134 |     marker_color = {'r': 0.7, 'g': 0.7, 'b': 0.0, 'a': 1.0}
135 |     
136 |     self.docking_station_marker_pub = rospy.Publisher('docking_station_marker', Marker, queue_size=5)
137 |     
138 |     self.docking_station_marker = Marker()
139 |     self.docking_station_marker.ns = marker_ns
140 |     self.docking_station_marker.id = marker_id
141 |     self.docking_station_marker.type = Marker.CYLINDER
142 |     self.docking_station_marker.action = Marker.ADD
143 |     self.docking_station_marker.lifetime = rospy.Duration(marker_lifetime)
144 |     self.docking_station_marker.scale.x = marker_scale
145 |     self.docking_station_marker.scale.y = marker_scale
146 |     self.docking_station_marker.scale.z = 0.02
147 |     self.docking_station_marker.color.r = marker_color['r']
148 |     self.docking_station_marker.color.g = marker_color['g']
149 |     self.docking_station_marker.color.b = marker_color['b']
150 |     self.docking_station_marker.color.a = marker_color['a']
151 |     
152 |     self.docking_station_marker.header.frame_id = 'odom'
153 |     self.docking_station_marker.header.stamp = rospy.Time.now()
154 |     self.docking_station_marker.pose = self.docking_station_pose
155 | 


--------------------------------------------------------------------------------
/pi_trees_ros/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package pi_trees_ros
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.1.2 (2016-09-25)
 6 | ------------------
 7 | * Tweaked behavior of SimpleActionTask when task is preempted so that the task will continue when control returns to that branch of the tree
 8 | * Tweaked comments in SimpleActionTask
 9 | * Fixed SimpleActionTask to better handle goal actions that do not succeed (e.g. aborted, preempted, etc.)
10 | * Tweaked done_cb behavior for SimpleActionTask so that a user-defined done_cb is appended to the default_done_cb instead of replacing it
11 | * adding sys import to resolve error in ServiceTask
12 | * Updated dependencies in package.xml files
13 | * Contributors: Patrick Goebel, maxfolley
14 | 
15 | 0.1.1 (2014-08-01)
16 | ------------------
17 | * Added initial CHANGELOG.rst files
18 | * Deleted obsolete examples directory from pi_trees_ros
19 | * Updates some comments
20 | * Uncommented feedback_cb for SimpleActionTask
21 | * Did some house cleaning
22 | * Cleanup
23 | * Initial commit
24 | * Contributors: Patrick Goebel
25 | 


--------------------------------------------------------------------------------
/pi_trees_ros/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | 
 3 | project(pi_trees_ros)
 4 | 
 5 | find_package(catkin REQUIRED)
 6 | 
 7 | catkin_package()
 8 | 
 9 | catkin_python_setup()
10 | 


--------------------------------------------------------------------------------
/pi_trees_ros/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>pi_trees_ros</name>
 3 |   <version>0.1.1</version>
 4 | 
 5 |   <description>ROS wrappers for the pi_trees_lib Library</description>
 6 | 
 7 |   <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
 8 |   <license>BSD</license>
 9 |   <url type="website">http://ros.org/wiki/pi_trees</url>
10 |   <url type="bugtracker">https://github.com/pirobot/pi_trees/issues</url>
11 |   <author email="patrick@pirobot.org">Patrick Goebel</author>
12 | 
13 |   <buildtool_depend>catkin</buildtool_depend>
14 | 
15 |   <!-- Dependencies needed after this package is compiled. -->
16 |   <run_depend>rospy</run_depend>
17 |   <run_depend>actionlib</run_depend>
18 |   <run_depend>actionlib_msgs</run_depend>
19 |   <run_depend>pi_trees_lib</run_depend>
20 | 
21 | </package>
22 | 


--------------------------------------------------------------------------------
/pi_trees_ros/setup.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from distutils.core import setup
 4 | from catkin_pkg.python_setup import generate_distutils_setup
 5 | 
 6 | d = generate_distutils_setup(
 7 |     packages=['pi_trees_ros'],
 8 |     package_dir={'': 'src'},
 9 |     )
10 | 
11 | setup(**d)
12 | 


--------------------------------------------------------------------------------
/pi_trees_ros/src/pi_trees_ros/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pirobot/pi_trees/83738b675c3c197f4afb6c12f5b99b4e8444e707/pi_trees_ros/src/pi_trees_ros/__init__.py


--------------------------------------------------------------------------------
/pi_trees_ros/src/pi_trees_ros/pi_trees_ros.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | import rospy
  4 | import actionlib
  5 | from actionlib_msgs.msg import GoalStatus
  6 | from pi_trees_lib.pi_trees_lib import *
  7 | import sys
  8 | 
  9 | class MonitorTask(Task):
 10 |     """
 11 |         Turn a ROS subscriber into a Task.
 12 |     """
 13 |     def __init__(self, name, topic, msg_type, msg_cb, wait_for_message=True, timeout=5):
 14 |         super(MonitorTask, self).__init__(name)
 15 |         
 16 |         self.topic = topic
 17 |         self.msg_type = msg_type
 18 |         self.timeout = timeout
 19 |         self.msg_cb = msg_cb
 20 |                 
 21 |         rospy.loginfo("Subscribing to topic " + topic)
 22 |         
 23 |         if wait_for_message:
 24 |             try:
 25 |                 rospy.wait_for_message(topic, msg_type, timeout=self.timeout)
 26 |                 rospy.loginfo("Connected.")
 27 |             except:
 28 |                 rospy.loginfo("Timed out waiting for " + topic)
 29 |                 
 30 |         # Subscribe to the given topic with the given callback function executed via run() 
 31 |         rospy.Subscriber(self.topic, self.msg_type, self._msg_cb)
 32 |         
 33 |     def _msg_cb(self, msg):
 34 |         self.set_status(self.msg_cb(msg))
 35 |         
 36 |     def run(self):
 37 |         return self.status
 38 |     
 39 | class ServiceTask(Task):
 40 |     """
 41 |         Turn a ROS service into a Task.
 42 |     """
 43 |     def __init__(self, name, service, service_type, request, result_cb=None, wait_for_service=True, timeout=5):
 44 |         super(ServiceTask, self).__init__(name)
 45 |         
 46 |         self.result = None
 47 |         self.request = request
 48 |         self.timeout = timeout
 49 |         self.result_cb = result_cb
 50 |                 
 51 |         rospy.loginfo("Connecting to service " + service)
 52 |         
 53 |         if wait_for_service:
 54 |             rospy.loginfo("Waiting for service")
 55 |             rospy.wait_for_service(service, timeout=self.timeout)
 56 |             rospy.loginfo("Connected.")
 57 |         
 58 |         # Create a service proxy
 59 |         self.service_proxy = rospy.ServiceProxy(service, service_type)
 60 |         
 61 |     def run(self):
 62 |         try:
 63 |             result = self.service_proxy(self.request)
 64 |             if self.result_cb is not None:
 65 |                 self.result_cb(result)
 66 |             return TaskStatus.SUCCESS
 67 |         except:
 68 |             rospy.logerr(sys.exc_info())
 69 |             return TaskStatus.FAILURE
 70 |         
 71 |     def reset(self):
 72 |         self.status = None
 73 |         
 74 | class SimpleActionTask(Task):
 75 |     """
 76 |         Turn a ROS action into a Task.
 77 |     """
 78 |     def __init__(self, name, action, action_type, goal, rate=5, connect_timeout=10, result_timeout=30, reset_after=False, active_cb=None, done_cb=None, feedback_cb=None):
 79 |         super(SimpleActionTask, self).__init__(name)
 80 |         
 81 |         self.action = action
 82 |         self.goal = goal
 83 |         self.tick = 1.0 / rate
 84 |         self.rate = rospy.Rate(rate)
 85 | 
 86 |         self.result = None
 87 |         self.connect_timeout = connect_timeout
 88 |         self.result_timeout = result_timeout
 89 |         self.reset_after = reset_after
 90 |         
 91 |         self.final_status = None
 92 |         
 93 |         if done_cb:
 94 |             self.user_done_cb = done_cb
 95 |         else:
 96 |             self.user_done_cb = None
 97 |         
 98 |         self.done_cb = self.default_done_cb
 99 |         
100 |         if active_cb == None:
101 |             active_cb = self.default_active_cb
102 |         self.active_cb = active_cb
103 |         
104 |         if feedback_cb == None:
105 |             feedback_cb = self.default_feedback_cb
106 |         self.feedback_cb = feedback_cb
107 |                 
108 |         self.action_started = False
109 |         self.action_finished = False
110 |         self.goal_status_reported = False
111 |         self.time_so_far = 0.0
112 |         
113 |         # Goal state return values
114 |         self.goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 
115 |                             'SUCCEEDED', 'ABORTED', 'REJECTED',
116 |                             'PREEMPTING', 'RECALLING', 'RECALLED',
117 |                             'LOST']
118 |         
119 |         self.retry_goal_states = [GoalStatus.PREEMPTED]
120 |             
121 |         rospy.loginfo("Connecting to action " + action)
122 | 
123 |         # Subscribe to the base action server
124 |         self.action_client = actionlib.SimpleActionClient(action, action_type)
125 | 
126 |         rospy.loginfo("Waiting for action server...")
127 |         
128 |         # Wait up to timeout seconds for the action server to become available
129 |         try:
130 |             self.action_client.wait_for_server(rospy.Duration(self.connect_timeout))
131 |         except:
132 |             rospy.loginfo("Timed out connecting to the action server " + action)
133 |     
134 |         rospy.loginfo("Connected to action server")
135 | 
136 |     def run(self):
137 |         # Send the goal
138 |         if not self.action_started:
139 |             rospy.loginfo("Sending " + str(self.name) + " goal to action server...")
140 |             self.action_client.send_goal(self.goal, done_cb=self.done_cb, active_cb=self.active_cb, feedback_cb=self.feedback_cb)
141 |             self.action_started = True
142 |             self.activate_time = rospy.Time.now()
143 |         
144 |         ''' We cannot use the wait_for_result() method here as it will block the entire
145 |             tree so we break it down in time slices of duration 1 / rate.
146 |         '''
147 |         if not self.action_finished:
148 |             self.time_so_far += self.tick
149 |             self.rate.sleep()
150 |             if self.time_so_far > self.result_timeout:
151 |                 self.action_client.cancel_goal()
152 |                 rospy.loginfo("Timed out achieving goal")
153 |                 self.action_finished = True
154 |                 return TaskStatus.FAILURE
155 |             else:
156 |                 return TaskStatus.RUNNING
157 |         else:
158 |             # Check the final goal status returned by default_done_cb
159 |             if self.goal_status == GoalStatus.SUCCEEDED:
160 |                 self.status = TaskStatus.SUCCESS
161 |             
162 |             # This case handles PREEMPTED
163 |             elif self.goal_status in self.retry_goal_states:
164 |                 self.status = TaskStatus.RUNNING
165 |                 self.action_started = False
166 |                 self.action_finished = False
167 |                 self.time_so_far = 0
168 |             
169 |             # Otherwise, consider the task to have failed
170 |             else:
171 |                 self.status = TaskStatus.FAILURE
172 |             
173 |             # Store the final status before we reset
174 |             self.final_status = self.status
175 | 
176 |             # Reset the task if the reset_after flag is True
177 |             if self.reset_after:
178 |                 self.reset()
179 |         
180 |         self.action_client.wait_for_result(rospy.Duration(10))
181 |         return self.final_status
182 |                             
183 |     def default_done_cb(self, result_state, result):
184 |         """Goal Done Callback
185 |         This callback resets the active flags and reports the duration of the action.
186 |         Also, if the user has defined a result_cb, it is called here before the
187 |         method returns.
188 |         """
189 |         self.goal_status = result_state
190 |         self.action_finished = True
191 |         
192 |         if not self.goal_status_reported:
193 |             self._duration = rospy.Time.now() - self.activate_time
194 |             
195 |             rospy.loginfo("Action " + self.name + " terminated after "\
196 |                     + str(self._duration.to_sec()) + " seconds with result "\
197 |                     + self.goal_states[self.action_client.get_state()] + ".")
198 |             
199 |             self.goal_status_reported = True
200 |             
201 |         if self.user_done_cb:
202 |             self.user_done_cb(result_state, result)
203 |     
204 |     def default_active_cb(self):
205 |         pass
206 |         
207 |     def default_feedback_cb(self, msg):
208 |         pass
209 |     
210 |     def reset(self):
211 |         rospy.logdebug("RESETTING " + str(self.name))
212 |         self.action_started = False
213 |         self.action_finished = False
214 |         self.goal_status_reported = False
215 |         self.status = self.final_status
216 |         self.time_so_far = 0.0
217 |         super(SimpleActionTask, self).reset()
218 | 


--------------------------------------------------------------------------------