-
Notifications
You must be signed in to change notification settings - Fork 1
/
simple_move_2.py
86 lines (72 loc) · 3.37 KB
/
simple_move_2.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
#!/usr/bin/env python3
# Copyright 2024 - Andrew Kwok Fai LUI,
# Robotics and Autonomous Systems Group, REF, RI
# and the Queensland University of Technology
__author__ = 'Andrew Lui'
__copyright__ = 'Copyright 2024'
__license__ = 'GPL'
__version__ = '1.0'
__email__ = 'ak.lui@qut.edu.au'
__status__ = 'Development'
import threading, time, signal, sys
from py_trees.composites import Sequence, Parallel, Composite, Selector
from py_trees.trees import BehaviourTree
# robot control module
from arm_commander.commander_moveit import GeneralCommander, logger
from task_trees.behaviours_move import DoMoveXYZ
# The TaskManager specialized for this application
class SimpleMovePyTreesApplication():
""" This is an application building a behaviour tree using py-trees and the extension behaviour set from the task tree SDK
The behaviour tree contains two behaviours, each of which moves to a pose specified in xyz.
The end-effector is moving between the two poses
"""
def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10):
""" the constructor
:param arm_commander: a general commander for a particular arm manipulator
:type arm_commander: GeneralCommander
:param spin_period_ms: the tick_tock period, defaults to 10 seconds
:type spin_period_ms: int, optional
"""
signal.signal(signal.SIGINT, self.stop)
# setup the robotic manipulation platform through the commander
self.arm_commander:GeneralCommander = arm_commander
self.arm_commander.abort_move(wait=True)
self.arm_commander.reset_world()
self.arm_commander.wait_for_ready_to_move()
# build the behaviour tree
self.root_sequence = self.create_move_branch()
self.bt = BehaviourTree(self.root_sequence)
# py_trees.display.render_dot_tree(self.bt.root)
# spin the tree
self.the_thread = threading.Thread(target=lambda: self.bt.tick_tock(period_ms=spin_period_ms), daemon=True)
self.the_thread.start()
# -------------------------------------------------
# --- create the behaviour tree and its branches
# A behaviour tree branch that moves to a specified position xyz
def create_move_branch(self) -> Composite:
""" Return a behaviour tree branch that moves between two specified positions in xyz
:return: a branch for the behaviour tree
:rtype: Composite
"""
move_branch = Sequence (
'move_branch',
memory=True,
children=[
DoMoveXYZ('move_xyz', True, arm_commander=self.arm_commander, target_xyz=[0.3, 0.0, 0.2]),
DoMoveXYZ('move_xyz', True, arm_commander=self.arm_commander, target_xyz=[0.3, 0.0, 0.6]),
],
)
return move_branch
# The callback for the signal resulting from SIGINT pressing CTRL-C
def stop(self, *args, **kwargs):
sys.exit(0)
if __name__=='__main__':
# rospy.init_node('simple_move_example', anonymous=False)
try:
arm_commander = GeneralCommander('panda_arm')
the_application = SimpleMovePyTreesApplication(arm_commander)
logger.info('The simple_move_example is running')
# prevent the main thread from exit the program
while True: time.sleep(1.0)
except Exception as e:
logger.exception(e)