def create_root(goal=std_msgs.Empty()):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Selector(name="Scan or Die")
die = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Uh Oh",
colour="red")
ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
undock = py_trees_ros.actions.ActionClient(
name="UnDock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(False))
scan_or_be_cancelled = py_trees.composites.Selector(
"Scan or Be Cancelled")
cancelling = py_trees.composites.Sequence("Cancelling?")
is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Cancel?",
variable_name='event_cancel_button',
expected_value=True)
move_home_after_cancel = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
move_base = py_trees_ros.actions.ActionClient(
name="Move Out",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
scanning = py_trees.composites.Parallel(
name="Scanning",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext(
"Context Switch")
scan_rotate = py_trees_ros.actions.ActionClient(
name="Rotate",
action_namespace="/rotate",
action_spec=py_trees_msgs.RotateAction,
action_goal=py_trees_msgs.RotateGoal())
scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Blue", colour="blue")
move_home_after_scan = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
celebrate = py_trees.composites.Parallel(
name="Celebrate",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Green", colour="green")
celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
dock = py_trees_ros.actions.ActionClient(
name="Dock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(True))
# Subtree
root.add_children([ere_we_go, die])
ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
cancelling.add_children([is_cancel_requested, move_home_after_cancel])
move_out_and_scan.add_children(
[move_base, scanning, move_home_after_scan])
scanning.add_children(
[scan_context_switch, scan_rotate, scan_flash_blue])
celebrate.add_children([celebrate_flash_green, celebrate_pause])
return root
def create_root():
# behaviours
root = py_trees.composites.Parallel("Tutorial")
topics2bb = py_trees.composites.Sequence("Topics2BB")
scan2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Scan2BB",
topic_name="/dashboard/scan",
variable_name="event_scan_button")
cancel2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Cancel2BB",
topic_name="/dashboard/cancel",
variable_name="event_cancel_button")
battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
topic_name="/battery/state",
threshold=30.0)
priorities = py_trees.composites.Selector("Priorities")
battery_check = py_trees.meta.success_is_failure(
py_trees.composites.Selector)(name="Battery Emergency")
is_battery_ok = py_trees.blackboard.CheckBlackboardVariable(
name="Battery Ok?",
variable_name='battery_low_warning',
expected_value=False)
flash_led_strip = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Red", colour="red")
scan = py_trees.composites.Sequence(name="Scan")
is_scan_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Scan?", variable_name='event_scan_button', expected_value=True)
scan_or_die = py_trees.composites.Selector(name="Scan or Die")
die = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Uh Oh",
colour="red")
ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
undock = py_trees_ros.actions.ActionClient(
name="UnDock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(False))
scan_or_be_cancelled = py_trees.composites.Selector("Scan or Be Cancelled")
cancelling = py_trees.composites.Sequence("Cancelling?")
is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Cancel?",
variable_name='event_cancel_button',
expected_value=True)
move_home_after_cancel = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
move_base = py_trees_ros.actions.ActionClient(
name="Move Out",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
scanning = py_trees.composites.Parallel(
name="Scanning", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext(
"Context Switch")
scan_rotate = py_trees_ros.actions.ActionClient(
name="Rotate",
action_namespace="/rotate",
action_spec=py_trees_msgs.RotateAction,
action_goal=py_trees_msgs.RotateGoal())
scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Blue", colour="blue")
move_home_after_scan = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
celebrate = py_trees.composites.Parallel(
name="Celebrate", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Green", colour="green")
celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
dock = py_trees_ros.actions.ActionClient(
name="Dock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(True))
idle = py_trees.behaviours.Running(name="Idle")
# tree
root.add_children([topics2bb, priorities])
topics2bb.add_children([scan2bb, cancel2bb, battery2bb])
priorities.add_children([battery_check, scan, idle])
battery_check.add_children([is_battery_ok, flash_led_strip])
scan.add_children([is_scan_requested, scan_or_die])
scan_or_die.add_children([ere_we_go, die])
ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
cancelling.add_children([is_cancel_requested, move_home_after_cancel])
move_out_and_scan.add_children([move_base, scanning, move_home_after_scan])
scanning.add_children([scan_context_switch, scan_rotate, scan_flash_blue])
celebrate.add_children([celebrate_flash_green, celebrate_pause])
return root