def __init__(self):
rospy.loginfo("Initialising behaviour tree")
# tuck the arm
"""b1 = tuckarm()
# lower head
b2 = movehead("down")
b3 = pick_cube()
b4 = move_towards_table()
b5 = place_cube()"""
picking_seq = RSequence(name="Pick sequence", children=[tuckarm(),movehead("down"),pick_cube()])
rotation = pt.composites.Selector("Rotate", children = [counter(28,"Rotational counter"),go("Rotation",0,-1)])
translation = pt.composites.Selector("Translate",children=[counter(11,"Translational counter"),go("Translation",0.8,0)])
move_to_table2 = RSequence(name="Move to second table",children=[rotation,translation])
place_behaviour = place_cube()
check_if_placed = is_placed()
rotation_back = pt.composites.Selector("Rotate back", children = [counter(28," Back rotation counter"),go("Rotation_back",0,-1)])
translation_back = pt.composites.Selector("Translate back",children=[counter(11,"Back translation counter"),go("Translation_back",0.8,0)])
move_back_to_table1 = RSequence(name="Move back to first table", children=[rotation_back,translation_back])
check_and_move_back = pt.composites.Selector("Check if cube is placed on table2, return back if not placed", children=[check_if_placed,move_back_to_table1])
# become the tree
main_sequence = RSequence(name="Main sequence", children=[picking_seq,move_to_table2,place_behaviour,check_and_move_back])
#tree = pt.composites.Selector("Main selector", children=[main_sequence,move_back_to_table1])
super(BehaviourTree, self).__init__(main_sequence)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown(): self.tick_tock(1)
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
Spinning = pt.composites.Sequence(name='Spin',
children=[Spin(359),
gotopos()])
InitFlight = pt.composites.Sequence(name='InitFlight',
children=[Up(0.35),
gotopos()])
tree = RSequence(name="Main sequence",
children=[
InitFlight, Spinning,
Localize(),
gotopos(),
Planner(),
gotopos(),
pathFollower(),
reset_behaviours()
])
super(BehaviourTree, self).__init__(tree)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
try:
self.tick_tock(1)
except KeyboardInterrupt:
exit()
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
Spinning = pt.composites.Sequence(name='Spin',
children=[Spin(359),
gotopos()])
Plan = pt.composites.Sequence(name='Plan Path', children=[Planner()])
PathFollow = pt.composites.Sequence(name='Follow Path',
childern=[pathFollower()])
InitFlight = pt.composites.Sequence(
name='InitFlight',
children=[
Up(0.35), gotopos()
] #[counter(5000, "Up_Counter"), Up(0.35)]#,counter(1000,"Wait")]
)
# NoCopyRoutine = pt.composites.Sequence(
# name = 'No copy movement',
# children = [Forward(1), gotopos(), Spin()]
# )
tree = RSequence(
name="Main sequence",
children=[
InitFlight, Spinning,
Localize(),
gotopos(),
Planner(),
gotopos(),
pathFollower(), Spinning,
reset_behaviours()
]
) #,counter(1000,"Wait"),Down(),reset_behaviours()])#Up(),reset_behaviours()])#,Up(),Image_2(),Down(),reset_behaviours()])
#tree = RSequence(name="Main sequence", children=[Up(),counter(1000,"Wait"),Image_1(),Uwp(),stay(),counter(1000,"Wait2"),Image_2(),Matching(),Down(),stay(),counter(1000,"Wait3"),reset_behaviours()])#,counter(1000,"Wait"),Down(),reset_behaviours()])#Up(),reset_behaviours()])#,Up(),Image_2(),Down(),reset_behaviours()])
#tree = RSequence(name="Main sequence", children=[InitFlight])#, gotopos(), Spin()]) # Localize(), gotopos(),Planner(), gotopos(), pathFollower(), Spinning])#,counter(1000,"Wait"),Down(),reset_behaviours()])#Up(),reset_behaviours()])#,Up(),Image_2(),Down(),reset_behaviours()])
#tree = RSequence(name="Main sequence", children=[Detect])#[InitFlight,Detect,arucoFollow()])
super(BehaviourTree, self).__init__(tree)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
try:
self.tick_tock(1)
except KeyboardInterrupt:
exit()
def build_change_table(name):
turn_around = pt.composites.Selector(
name="Turn around",
children=[counter(58, "Turned around?"), go("Turn", 0, -0.5)]
)
go_straight = pt.composites.Selector(
name="Walk",
children=[counter(20, "At table?"), go("Walk", 0.4, 0)]
)
timeout = pt.decorators.Timeout(
name="Timeout",
child=pt.behaviours.Success(name="Have a Beer!"),
duration=100.0
)
return RSequence(name=name, children=[turn_around, go_straight, timeout])
def __init__(self):
self.place_pose_topic = rospy.get_param(rospy.get_name() + '/place_pose_topic')
self.pickup_pose_topic = rospy.get_param(rospy.get_name() + '/pick_pose_topic')
rospy.loginfo("Initialising behaviour tree")
b00 = TuckArm()
b0 = activate_localizator() # activate global localizator
b1 = pt.composites.Selector(
name="Rotate for localization",
children=[counter(70, "Rotated?"), go("Rotate!", 0, 1)]) # rotate for betteer localization
b2 = Navigate(self.pickup_pose_topic)
b3 = Navigate(self.place_pose_topic)
# become the tree
tree = RSequence(name="Main sequence", children=[b00 ,b0, b1, b2, b3])
super(BehaviourTree, self).__init__(tree)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown(): self.tick_tock(1)
def __init__(self):
self.picked_up_cube_topic_name = rospy.get_name(
) + "/picked_up_cube_topic"
self.place_pose_topic = rospy.get_param(rospy.get_name() +
'/place_pose_topic')
self.pickup_pose_topic = rospy.get_param(rospy.get_name() +
'/pick_pose_topic')
rospy.loginfo("Initialising behaviour tree")
b0 = TuckArm()
b1 = activate_localizator() # activate global localizator
b2 = pt.composites.Selector(
name="Rotate for localization",
children=[counter(70, "Rotated?"),
go("Rotate!", 0, 1)]) # rotate for betteer localization
b22 = respawn_cube()
b3 = Navigate(self.pickup_pose_topic)
# pickup
b4 = LowerHead("Lower head!", "down")
b5 = PickCube(self.picked_up_cube_topic_name)
b6 = LowerHead("Rise head!", "up")
b7 = Navigate(self.place_pose_topic)
# place
b8 = LowerHead("Lower head!", "down")
b9 = PlaceCube()
b10 = LowerHead("Rise head!", "up")
# become the tree
tree = RSequence(
name="Main sequence",
children=[b0, b1, b2, b22, b3, b4, b5, b6, b7, b8, b9, b10])
super(BehaviourTree, self).__init__(tree)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
self.tick_tock(1)
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
global flag_repeat_tree_1
global flag_repeat_tree_2
global flag_repeat_tree_3
global flag_repeat_tree_4
global flag_repeat_tree_5
global flag_repeat_tree_6
global flag_repeat_tree_7
flag_repeat_tree_1 = False
flag_repeat_tree_2 = False
flag_repeat_tree_3 = False
flag_repeat_tree_4 = False
flag_repeat_tree_5 = False
flag_repeat_tree_6 = False
flag_repeat_tree_7 = False
b0 = movehead("up")
b1 = tuckarm()
b2 = localize()
b3 = navigate_pickup()
b4 = pickup()
b5 = navigate_place()
b6 = place()
b7 = cubecheck()
tree = RSequence(name="Main sequence",
children=[b0, b1, b2, b3, b4, b5, b6, b7])
super(BehaviourTree, self).__init__(tree)
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
self.tick_tock(1)
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
# go to door until at door
b0 = pt.composites.Selector(
name="Go to door fallback",
children=[counter(30, "At door?"),
go("Go to door!", 1, 0)])
# tuck the arm
b1 = tuckarm()
# go to table
b2 = pt.composites.Selector(
name="Go to table fallback",
children=[counter(5, "At table?"),
go("Go to table!", 0, -1)])
# move to chair
b3 = pt.composites.Selector(
name="Go to chair fallback",
children=[counter(13, "At chair?"),
go("Go to chair!", 1, 0)])
# lower head
b4 = movehead("down")
# become the tree
tree = RSequence(name="Main sequence", children=[b0, b1, b2, b3, b4])
super(BehaviourTree, self).__init__(tree)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
self.tick_tock(1)
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
is_placed = pt.composites.Selector(
name="twist",
children=[counter(2, "Is Placed?"), is_cube_placed()]
)
confirm_placed_cube = pt.composites.Chooser(
name="Confirm cube is on table",
children=[is_placed, build_change_table('Go back')])
place = pt.composites.Selector(
name="twist",
children=[counter(60, "Placed?"), placecube()])
pick = pt.composites.Selector(
name="twist",
children=[counter(60, "Picked?"), pickcube()])
tree = RSequence(name="Main sequence", children=[
tuckarm(),
movehead("down"),
pick,
build_change_table('Go to other table'),
place,
confirm_placed_cube,
])
super(BehaviourTree, self).__init__(tree)
# execute BT
rospy.sleep(1)
self.setup(timeout=10000)
while not rospy.is_shutdown():
self.tick_tock(1)
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
# self.pick_srv_nm = rospy.get_param(rospy.get_name() + '/pick_srv')
# rospy.wait_for_service(self.pick_srv_nm)
# self.cube_PoseStamped.header.stamp = rospy.Time.now()
# self.cube_PoseStamped.header.frame_id = self.robot_base_frame_nm
# tuck the arm
b0 = tuckarm()
# lower head
b1 = movehead("down")
# Keep
b2 = pt.composites.Selector(
name="Keep",
children=[counter(20, "Head Full down?"),
go("Keeping!", 0, 0)])
# pick
b3 = Pick()
# rotate
b4 = pt.composites.Selector(
name="Rotate",
children=[counter(60, "Finished?"),
go("Go rotating!", 0, -0.5)])
# forward
b5 = pt.composites.Selector(name="Forward",
children=[
counter(17, "Finished?"),
go("Go straight ahead!", 0.5, 0)
])
# place
b6 = Place()
# check
b7 = Check()
# tuck Arm
b8 = tuckarm()
# rotate
b9 = pt.composites.Selector(
name="RotateBack",
children=[counter(58, "Finished?"),
go("Go rotating!", 0, 0.5)])
# back to Table1
b10 = pt.composites.Selector(
name="Backward",
children=[counter(19, "Finished?"),
go("Backward!", 0.5, 0)])
# pick
# b11 = RSequence(name="GoingBack", children=[b8, b9, b10])
# b12 = pt.composites.Selector(
# name="Detect",
# children=[b7, b11]
# )
# up head (Unnessary)
# bn = movehead("up")
# become the tree
tree = RSequence(
name="Main sequence",
children=[b0, b1, b2, b3, b4, b5, b6, b7, b8, b9, b10])
super(BehaviourTree, self).__init__(tree)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
self.tick_tock(1)
def __init__(self):
rospy.loginfo("Initialising behaviour tree")
# check for kidnapping and reset if it has happened
b000 = check_kidnapping()
# request the global position (spawn particles)
b001 = global_localization()
# do orientation
b002 = orientation(100, 0, -1)
# respawn cube
b003 = respawn_cube()
# check standstill while moving
b004 = standstill(1000, 0.01)
# tuck the arm
b0 = tuckarm()
# raise head
b01 = movehead("up")
# request the global position (spawn particles)
#b02 = global_localization()
# turn in a circle
#b03 = pt.composites.Selector(
# name="Turn 360 fallback",
# children=[counter(62, "Turned?"), go("Turn!", 0, -1)]
#)
# wait a bit for localization
b04 = pt.composites.Selector(
name="Wait fallback",
children=[counter(20, "Waited?"),
go("Wait!", 0, 0)])
# go to cube
b05 = goto_action("table1")
# wait a bit for localization
b06 = pt.composites.Selector(
name="Wait fallback",
children=[counter(20, "Waited?"),
go("Wait!", 0, 0)])
# lower head
b07 = movehead("down")
# wait until cube detected
b08 = detect_cube("wait_for_detection")
# pick cube
b09 = pickplace_cube("pick")
# raise head
b10 = movehead("up")
# go to target
b11 = goto_action("table2")
# lower head
b12 = movehead("down")
# place cube
b13 = pickplace_cube("place")
# is cube there? ...
b14 = detect_cube("no_detection_reset")
'''
# lower head
b1 = movehead("down")
# detect cube
b2 = detect_cube("wait_for_detection")
# pick up cube when detected
b3 = pickplace_cube("pick")
# turn 180
b4 = pt.composites.Selector(
name="Turn 180 fallback",
children=[counter(31, "Turned?"), go("Turn!", 0, -1)]
)
# wait a bit
b5 = pt.composites.Selector(
name="Wait fallback",
children=[counter(18, "Waited?"), go("Wait!", 0, 0)]
)
# move strait to other table
b6 = pt.composites.Selector(
name="Go to table fallback",
children=[counter(15, "At table?"), go("Go to table!", 0.5, 0)]
)
# go to other table
b456 = pt.composites.Sequence(
name="Go to other table sequence",
children=[b4, b5, b6]
)
# place the cube
b7 = pickplace_cube("place")
# tuck the arm
b8 = tuckarm()
# turn 180
b42 = pt.composites.Selector(
name="Turn 180 fallback",
children=[counter(31, "Turned?"), go("Turn!", 0, -1)]
)
# wait a bit
b52 = pt.composites.Selector(
name="Wait fallback",
children=[counter(18, "Waited?"), go("Wait!", 0, 0)]
)
# move strait to other table
b62 = pt.composites.Selector(
name="Go to table fallback",
children=[counter(15, "At table?"), go("Go to table!", 0.5, 0)]
)
# go to other table
b4562 = pt.composites.Sequence(
name="Go to other table sequence",
children=[b42, b52, b62]
)
# check if cube is placed
b9 = pt.composites.Selector(
name="Go to table fallback",
children=[detect_cube("once"), b4562]
)
# be happy
b10 = pt.composites.Selector(
name="Happy fallback",
children=[counter(20, "Happy?"), go("Happy!", 0, 1)]
)
'''
'''
# go to door until at door
b0 = pt.composites.Selector(
name="Go to door fallback",
children=[counter(30, "At door?"), go("Go to door!", 1, 0)]
)
# tuck the arm
b1 = tuckarm()
# go to table
b2 = pt.composites.Selector(
name="Go to table fallback",
children=[counter(5, "At table?"), go("Go to table!", 0, -1)]
)
# move to chair
b3 = pt.composites.Selector(
name="Go to chair fallback",
children=[counter(13, "At chair?"), go("Go to chair!", 1, 0)]
)
# lower head
b4 = movehead("down")
'''
# become the tree
tree = RSequence(name="Main sequence",
children=[
b0, b01, b000, b001, b002, b003, b004, b04, b05,
b06, b07, b08, b09, b10, b11, b12, b13, b14
])
super(BehaviourTree, self).__init__(tree)
# https://py-trees.readthedocs.io/en/release-0.6.x/visualisation.html
def post_tick_handler(snapshot_visitor, behaviour_tree):
print(
pt.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = pt.visitors.SnapshotVisitor()
self.add_post_tick_handler(
functools.partial(post_tick_handler, snapshot_visitor))
self.visitors.append(snapshot_visitor)
# execute the behaviour tree
rospy.sleep(5)
self.setup(timeout=10000)
while not rospy.is_shutdown():
self.tick_tock(1)