diff --git a/src/lhw_intelligence/lhw_intelligence/safety_check/actions.py b/src/lhw_intelligence/lhw_intelligence/actions.py similarity index 100% rename from src/lhw_intelligence/lhw_intelligence/safety_check/actions.py rename to src/lhw_intelligence/lhw_intelligence/actions.py diff --git a/src/lhw_intelligence/lhw_intelligence/battery_check_tree.py b/src/lhw_intelligence/lhw_intelligence/battery_check_tree.py deleted file mode 100644 index d241d4ee489aa394283e5877d6553ed123687f1b..0000000000000000000000000000000000000000 --- a/src/lhw_intelligence/lhw_intelligence/battery_check_tree.py +++ /dev/null @@ -1,110 +0,0 @@ -""" - This is the main AI file for the whole of lhw. If you want to perform a task, - you also have to launch one of the state machines in this package. - In other words, this is a hub which reacts to sensory input. -""" - -from lhw_intelligence.extra_behaviours import * - -import py_trees -import py_trees_ros - -import rclpy -from rclpy.node import Node - -from lhw_interfaces.msg._goal import Goal -from lhw_interfaces.msg._feedback import Feedback -from lhw_interfaces.msg._result import Result -from lhw_interfaces.msg import RobotState - -from std_msgs.msg import String -from lhw_interfaces.msg import Entities, Response, Event -from geometry_msgs.msg import Twist, Point, PoseStamped -from nav_msgs.msg import OccupancyGrid -from .help_functions.DataBase import DataBase - -class BatteryCheckTree(Node): - def __init__(self): - super().__init__('battery_check_tree') - self.log = self.get_logger() - - # Publishers - self.robot_state_pub = self.create_publisher(RobotState, "robot_state", 10) - - # Subscribers - self.battery_sub = self.create_subscription(DiagnosticArray, "diagnostics", self.battery_cb, 10) - - - # BLACKBOARD_KEYS - self.blackboard = py_trees.blackboard.Client(name="Battery") - self.blackboard.register_key(key="percentage", access=py_trees.common.Access.WRITE) - self.blackboard.register_key(key="low_percentage", access=py_trees.common.Access.WRITE) - - self.log.info("py_tree started") - self.iterations = 0 - root = py_trees.composites.Sequence("Sequence") - - # THE ORDER IN THE LIST MATTERS! - #root.add_children([high, med, low]) - - #root2 = py_trees.composites.Parallel("Parallel") - #root2.add_children([high, med, low]) - #diagnostics = py_trees_ros.subscribers.ToBlackboard(topic_name="/diagnostics", topic_type=DiagnosticArray, qos_profile=py_trees_ros.utilities.qos_profile_latched(), blackboard_variables={"diagnostics": "status"}) - print_diagnostics = PepperRest("Pepper Rest", self) - - root2 = py_trees.composites.Sequence("Sequence") - root2.add_child(print_diagnostics) - #root2 = create_root2() - behaviour_tree = py_trees.trees.BehaviourTree( - root=root2 - ) - - #test = behaviours.create_root() - - print(py_trees.display.unicode_tree(root=root)) - behaviour_tree.setup(timeout=15) - - def pre_tick_spin(tree): - rclpy.spin_once(self) - - def tick_print(tree): - print(py_trees.display.unicode_tree(root=tree.root, show_status=True)) - print(self.blackboard) - - try: - behaviour_tree.tick_tock( - period_ms=500, - number_of_iterations=py_trees.trees.CONTINUOUS_TICK_TOCK, - pre_tick_handler=None, - post_tick_handler=tick_print - ) - except KeyboardInterrupt: - behaviour_tree.interrupt() - - def battery_cb(self, data): - self.log.info("CALLBACK") - - # How to get the data for the battery from the DiagnosticArray - # data.status[len(data.status)-3].values - #self.log.info(data) - battery_index = len(data.status)-3 - percentage = float(data.status[battery_index].values[0].value) - self.blackboard.percentage =percentage - if percentage < 30: - self.blackboard.low_percentage = True - elif percentage > 30 + 5: # To prevent ping-pong behaviour - self.blackboard.low_percentage = False - - -def main(args=None): - rclpy.init(args=args) - ai_node = BatteryCheckTree() - - rclpy.spin(ai_node) - - ai_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/lhw_intelligence/lhw_intelligence/extra_behaviours.py b/src/lhw_intelligence/lhw_intelligence/example_py_tree_behaviour.py similarity index 54% rename from src/lhw_intelligence/lhw_intelligence/extra_behaviours.py rename to src/lhw_intelligence/lhw_intelligence/example_py_tree_behaviour.py index 8076f9917feddaa3fee6bb32221ea3aa209b0b98..3d094005597e38a8a7917cc2498830c9b68b43c1 100644 --- a/src/lhw_intelligence/lhw_intelligence/extra_behaviours.py +++ b/src/lhw_intelligence/lhw_intelligence/example_py_tree_behaviour.py @@ -1,13 +1,6 @@ import py_trees import py_trees_ros import random -import rclpy - -from std_msgs.msg import String -from diagnostic_msgs.msg._diagnostic_array import DiagnosticArray -from diagnostic_msgs.msg._diagnostic_status import DiagnosticStatus -from geometry_msgs.msg import Twist -from lhw_interfaces.msg import RobotState class ExampleBehaviour(py_trees.behaviour.Behaviour): def __init__(self, name): @@ -93,101 +86,4 @@ class ExampleBehaviour(py_trees.behaviour.Behaviour): - SUCCESS || FAILURE : your behaviour's work cycle has finished - INVALID : a higher priority branch has interrupted, or shutting down """ - self.logger.debug(" %s [ExampleBehaviour::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) - -class BatteryBehaviour(py_trees.behaviour.Behaviour): - def __init__(self, name) -> None: - super(BatteryBehaviour, self).__init__(name) - - def setup(self, **kwargs): - self.blackboard = py_trees.blackboard.Client(name="Battery") - self.blackboard.register_key(key="percentage", access=py_trees.common.Access.READ) - - def update(self): - try: - return py_trees.common.Status.RUNNING - except: - return py_trees.common.Status.FAILURE - - - def terminate(self, new_status): - pass - -class IsBatteryLow(py_trees.behaviour.Behaviour): - def __init__(self, name) -> None: - super(IsBatteryLow, self).__init__(name) - - def setup(self, **kwargs): - self.blackboard = py_trees.blackboard.Client(name="Battery") - self.blackboard.register_key(key="low_percentage", access=py_trees.common.Access.READ) - - def initialise(self): - pass - - def update(self): - if self.blackboard.low_percentage: - return py_trees.common.Status.SUCCESS - else: - return py_trees.common.Status.FAILURE - - def terminate(self, new_status): - pass - - -class PepperRest(py_trees.behaviour.Behaviour): - def __init__(self, name, node) -> None: - super(PepperRest, self).__init__(name) - self.node = node - - def setup(self, **kwargs): - self.pub = self.node.robot_state_pub - - def initialise(self): - - # TODO: Test this - msg = RobotState() - msg.type = RobotState.REST - print(msg) - - self.pub.publish(msg) - - def update(self): - return py_trees.common.Status.RUNNING - - def terminate(self, new_status): - pass - -class Say(py_trees.behaviour.Behaviour): - def __init__(self, name, node, message: str) -> None: - super(Say, self).__init__(name) - self.node = node - self.message = message - self.say_pub = self.node.say_publisher - - def setup(self, **kwargs): - pass - def initialise(self): - self.say_pub.publish(self.message) - - def update(self): - return py_trees.common.Status.SUCCESS - - def terminate(self, new_status): - pass - -class RobotPoint(py_trees.behaviour.Behaviour): - def __init__(self, name, node) -> None: - super(RobotPoint, self).__init__(name) - self.node = node - - def setup(self, **kwargs): - self.blackboard = py_trees.blackboard.Client(name="bb") - pass - def initialise(self): - self.say_pub.publish(self.message) - - def update(self): - return py_trees.common.Status.SUCCESS - - def terminate(self, new_status): - pass \ No newline at end of file + self.logger.debug(" %s [ExampleBehaviour::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) \ No newline at end of file diff --git a/src/lhw_intelligence/lhw_intelligence/motion_behaviours.py b/src/lhw_intelligence/lhw_intelligence/motion_behaviours.py new file mode 100644 index 0000000000000000000000000000000000000000..99615a5a3f49d19a075e0b0d1e7f0efd0ed3f7bc --- /dev/null +++ b/src/lhw_intelligence/lhw_intelligence/motion_behaviours.py @@ -0,0 +1,267 @@ +import py_trees +from lhw_interfaces.msg import RobotAction +from lhw_interfaces.srv import PixelToXYZ +import numpy as np + +""" Generic way of sending a premade action msg to Pepper +""" +class PepperAction(py_trees.behaviour.Behaviour): + def __init__(self, name, node, action: RobotAction) -> None: + super(PepperAction, self).__init__(name) + self.node = node + self.action = action + + def setup(self, **kwargs): + self.pub = self.node.robot_action_pub + + def initialise(self): + self.pub.publish(self.action) + + def update(self): + return py_trees.common.Status.SUCCESS + + +""" Wake up Pepper +""" +class PepperWakeUp(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(PepperWakeUp, self).__init__(name) + self.node = node + self.pub = self.node.robot_action_pub + + def initialise(self): + self.node.log.info("Waking up Pepper...") + action = RobotAction() + action.type = RobotAction.WAKEUP + self.pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + + + +""" Put Pepper to rest +""" +class PepperRest(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(PepperRest, self).__init__(name) + self.node = node + self.pub = self.node.robot_action_pub + + def initialise(self): + self.node.log.info("Putting Pepper to Rest...") + action = RobotAction() + action.type = RobotAction.REST + self.pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + + +""" Set the safety of Pepper, default is ON +""" +class PepperSetSafety(py_trees.behaviour.Behaviour): + def __init__(self, name, node, safety_on: bool = True) -> None: + super(PepperSetSafety, self).__init__(name) + self.node = node + self.pub = self.node.robot_action_pub + self.safety_on = safety_on + + def initialise(self): + action = RobotAction() + if self.safety_on: + self.node.log.info("Safety is now ON") + action.type = RobotAction.SAFETY_ON + else: + self.node.log.info("Safety is now OFF") + action.type = RobotAction.SAFETY_OFF + self.pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + +class PepperCenterHead(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(PepperCenterHead, self).__init__(name) + self.node = node + self.pub = self.node.robot_action_pub + + def initialise(self): + action = RobotAction() + action.type = RobotAction.CENTER_HEAD + self.pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + + +class PepperTiltHead(py_trees.behaviour.Behaviour): + def __init__(self, name, node, tilt_up: bool = True) -> None: + super(PepperTiltHead, self).__init__(name) + self.node = node + self.pub = self.node.robot_action_pub + self.tilt_up = tilt_up + + def initialise(self): + action = RobotAction() + if self.tilt_up: + action.type = RobotAction.TILT_HEAD_UP + else: + action.type = RobotAction.TILT_HEAD_DOWN + self.pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + +""" + Transforms a xy-pixel on the blackboard variable 'point_to_pixel' + to a coordinate and makes Pepper point at this location +""" +class PointAtPixel(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(PointAtPixel, self).__init__(name) + self.node = node + self.pub = self.node.robot_action_pub + self.serv_client = node.pixel_to_xyz_serv + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="point_to_pixel", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="point_to_pixel", access=py_trees.common.Access.WRITE) + + def initialise(self): + while not self.serv_client.wait_for_service(timeout_sec=1.0): + print("Waiting for service connection") + self.req = PixelToXYZ.Request() + print(self.blackboard.point_to_pixel) + self.req.col = int(self.blackboard.point_to_pixel[0]/4) + self.req.row = int(self.blackboard.point_to_pixel[1]/4) + self.future = self.serv_client.call_async(self.req) + + def update(self): + if self.future.done(): + try: + response = self.future.result() + + except Exception as e: + self.node.get_logger().info( + 'Service call failed %r' % (e,)) + # TODO: Will we ever end up here? + self.future = self.serv_client.call_async(self.req) + return py_trees.common.Status.RUNNING + else: + #self.node.get_logger().info( + # 'Result of point at: for row %d & col %d gives [%d, %d, %d]' % + # (self.req.row, self.req.col, response.xyz[0], response.xyz[1], response.xyz[2])) + for i in range(3): + # If any of the values are NaN then we want to continue trying, since we need all coordinates + if np.isnan(response.xyz[i]): + print("Found NaN at index: " + str(i)) + self.future = self.serv_client.call_async(self.req) + return py_trees.common.Status.RUNNING + + action = RobotAction() + action.type = RobotAction.POINT_AT + #print("POINTING AT: " + response.xyz[0]) + # Currently only point at a x,y-coordinate. Ignore the rest + action.xyz[0] = response.xyz[2] # Nya x = gamla z + action.xyz[1] = -response.xyz[0] # Nya y = - gamla x + action.xyz[2] = -response.xyz[1] + 0.3 # Nya z = - gamla y + + action.right_arm = True + self.pub.publish(action) + return py_trees.common.Status.SUCCESS + else: + + return py_trees.common.Status.RUNNING + +class SetIdle(py_trees.behaviour.Behaviour): + def __init__(self, name, node, go_idle) -> None: + super(SetIdle, self).__init__(name) + self.node = node + self.robot_action_pub = self.node.robot_action_pub + self.go_idle = go_idle + + def initialise(self): + action = RobotAction() + if self.go_idle: + action.type = RobotAction.IDLE_POSTURE_ON + else: + action.type = RobotAction.IDLE_POSTURE_OFF + self.robot_action_pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + + +class PalmOpen(py_trees.behaviour.Behaviour): + def __init__(self, name, node, right_hand: bool) -> None: + super(PalmOpen, self).__init__(name) + self.node = node + self.robot_action_pub = self.node.robot_action_pub + self.is_right_hand = right_hand + + def initialise(self): + action = RobotAction() + action.hand = self.is_right_hand + self.action.type = RobotAction.PALM_OPEN + self.robot_action_pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + + + +class PalmClose(py_trees.behaviour.Behaviour): + def __init__(self, name, node, right_hand: bool) -> None: + super(PalmClose, self).__init__(name) + self.node = node + self.robot_action_pub = self.node.robot_action_pub + self.is_right_hand = right_hand + + def initialise(self): + action = RobotAction() + action.right_arm = self.is_right_hand + action.type = RobotAction.PALM_CLOSE + self.robot_action_pub.publish(action) + + def update(self): + return py_trees.common.Status.SUCCESS + + +class YawElbowOpen(py_trees.behaviour.Behaviour): + def __init__(self, name, node, right_hand: bool) -> None: + super(YawElbowOpen, self).__init__(name) + self.node = node + self.robot_action_pub = self.node.robot_action_pub + self.is_right_hand = right_hand + + def setup(self, **kwargs): + self.action = RobotAction() + self.action.hand = self.is_right_hand + self.action.type = RobotAction.YAW_ELBOW_OPEN + + def initialise(self): + self.robot_action_pub.publish(self.action) + + def update(self): + return py_trees.common.Status.SUCCESS + +class YawElbowClose(py_trees.behaviour.Behaviour): + def __init__(self, name, node, right_hand: bool) -> None: + super(YawElbowClose, self).__init__(name) + self.node = node + self.robot_action_pub = self.node.robot_action_pub + self.is_right_hand = right_hand + + def setup(self, **kwargs): + self.action = RobotAction() + self.action.hand = self.is_right_hand + self.action.type = RobotAction.YAW_ELBOW_CLOSE + + def initialise(self): + self.robot_action_pub.publish(self.action) + + def update(self): + return py_trees.common.Status.SUCCESS + diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree.py b/src/lhw_intelligence/lhw_intelligence/py_tree.py index 8cec91ec7016b2cb28b9814908e214275c846949..6dce3e4ac2da08fbb130fc22d9378f3c46e16019 100644 --- a/src/lhw_intelligence/lhw_intelligence/py_tree.py +++ b/src/lhw_intelligence/lhw_intelligence/py_tree.py @@ -1,7 +1,6 @@ import json -from .py_tree_behaviours import IsDoorOpen, IsHeadTouched, MoveForward, MoveToPoint, RotateRobot, Standby, StopRobot -from .pepper_action_behaviours import * +from .motion_behaviours import * from .scenario_trees import * from .py_tree_test_functions import Py_Tree_Tests @@ -11,18 +10,13 @@ import rclpy from rclpy.node import Node from rclpy.action import ActionClient -from lhw_interfaces.msg._goal import Goal -from lhw_interfaces.msg._feedback import Feedback -from lhw_interfaces.msg._result import Result from diagnostic_msgs.msg._diagnostic_array import DiagnosticArray -from diagnostic_msgs.msg._diagnostic_status import DiagnosticStatus from move_base_msgs.action import MoveBase from std_msgs.msg import String, Bool from sensor_msgs.msg import Range from naoqi_bridge_msgs.msg import HeadTouch -from lhw_interfaces.msg import Entities, Response, Event, RobotAction, FaceDetected, FacesDetected, Parameter -from geometry_msgs.msg import Twist, Point, PoseStamped -from nav_msgs.msg import OccupancyGrid, Odometry +from lhw_interfaces.msg import Entities, Response, RobotAction, FaceDetected, FacesDetected +from nav_msgs.msg import OccupancyGrid from nav2_msgs.action import NavigateToPose @@ -88,6 +82,7 @@ class Py_Tree(Node): center_head = PepperCenterHead(name="Center Head", node=self) idle_off = SetIdle(name="Idle Off", node=self, go_idle=True) + mock = MockNavigation(name="test mock", node=self) ###################################################################################### @@ -107,12 +102,9 @@ class Py_Tree(Node): receptionist_tree = receptionist(self) #Root is set to the tree which we want to run - self.behaviour_tree = py_trees.trees.BehaviourTree(root=test_functions.test_touch_head_and_say()) + self.behaviour_tree = py_trees.trees.BehaviourTree(root=mock) self.behaviour_tree.setup(timeout=15) - - - """ Run before every tick to clear all backlogged callbacks """ diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_behaviours.py b/src/lhw_intelligence/lhw_intelligence/py_tree_behaviours.py new file mode 100644 index 0000000000000000000000000000000000000000..e0813f285c49320123dea50ba56934595ecd3988 --- /dev/null +++ b/src/lhw_intelligence/lhw_intelligence/py_tree_behaviours.py @@ -0,0 +1,573 @@ +import py_trees +from .actions import Actions + +#from naoqi_bridge_msgs.msg import HeadTouch + +from .behaviour_copy import Behaviour #TODO: Move behaviours to here + +from move_base_msgs.action import MoveBase +#TODO: Tidy this up when finished +from std_msgs.msg import String, Header +from builtin_interfaces.msg import Time +import time, random +from geometry_msgs.msg import Twist, Pose, PoseStamped + + +""" Rotates the robot counter (?) clockwise. +""" +class RotateRobot(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(RotateRobot, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.actions = Actions(self.node) + + def update(self): + self.actions.rotate_robot() + return py_trees.common.Status.RUNNING + """if (Robotidle): + self.node.log.info("Rotation Succeeded") + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.RUNNING""" + + +""" Stops the robot +""" +class StopRobot(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(StopRobot, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.actions = Actions(self.node) + + def initialise(self): + self.actions.stop_robot() + + def update(self): + return py_trees.common.Status.SUCCESS + + +""" mocking navigation for testing +""" +class MockNavigation(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(MockNavigation, self).__init__(name) + self.node = node + + def update(self): + return py_trees.common.Status.SUCCESS + rand = random.random() + if rand < 0.3: + return py_trees.common.Status.FAILURE + elif rand < 0.6: + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS + + +""" Moves the robot forward x meters +""" +class MoveForward(py_trees.behaviour.Behaviour): + def __init__(self, name, node, x: float) -> None: + super(MoveForward, self).__init__(name) + self.node = node + self.x = x + + def setup(self, **kwargs): + self.action = Actions(self.node) + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="in_motion", access=py_trees.common.Access.READ) + + + def initialise(self): + + self.node.log.info(f"Moving forward {self.x}m") + msg = Twist() + speed = 1.0 + msg.linear.x = speed + msg.linear.y = 0.0 + msg.linear.z = 0.0 + msg.angular.z = 0.0 + self.node.cmd_vel_publisher.publish(msg) + self.start_time = time.time() + self.time_diff = self.x * 2 * speed + 1 + print(self.start_time) + + + def update(self): + + if float(time.time() - self.start_time) < self.time_diff: + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS + + """ + if (self.blackboard.in_motion): + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS + """ + + + #TODO: Få tag i data från navigation, när är vi stilla, har vi nått slutmålet + + """if (<idle> && <in location>): + return py_trees.common.Status.SUCCESS + elif (<idle> && <not at localtion>): + return py_trees.common.Status.FAILURE + else: + return py_trees.common.Status.RUNNING""" + + + +""" Checks using ultra sound if the door in front of the robot is open or not +""" +class IsDoorOpen(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(IsDoorOpen, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="door_open", access=py_trees.common.Access.READ) + + def update(self): + if self.blackboard.door_open == True: + print("Door is open!") + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.RUNNING + +""" Move the robot to a specified location +""" +class MoveToPoint(py_trees.behaviour.Behaviour): + def __init__(self, name, node, pose: Pose) -> None: + super(MoveToPoint, self).__init__(name) + self.node = node + self.pose = pose + + self.result = False + self.action_client = self.node._action_client + + def setup(self, **kwargs): + self.action = Actions(self.node) + # TODO: How these are setup depends on what data we get from NavigateToPose + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="in_motion", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="at_destination", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="goal_received", access=py_trees.common.Access.READ) + # TODO: Do something for checking for failures + + + def initialise(self): + while not self.action_client.wait_for_server(): + print("Waiting for action server...") + + goal = MoveBase.Goal() + + poseStamped = PoseStamped() + poseStamped.pose = self.pose + goal.target_pose = poseStamped + header = Header() + stamp = Time() + stamp.sec = int(time.time() % 60) + stamp.nanosec = 0 + header.stamp = stamp + header.frame_id = "map" + poseStamped.header = header + print(goal) + + # goal.behaviour_tree = "write behaviour tree name" + # print(goal) + self.future = self.action_client.send_goal_async(goal) + self.future.add_done_callback(self.goal_response_callback) + + def update(self): + self.initialise() + if self.result: + #Can use the self.chosen_option to get the chosen response + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.RUNNING + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + return + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + self.result = True + self.chosen_option = future.result().result + + + +""" Checks if the head has been tapped + This is a blocking behaviour +""" +class IsHeadTouched(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(IsHeadTouched, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="head_touched", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="head_touched", access=py_trees.common.Access.WRITE) + + def update(self): + if self.blackboard.head_touched == True: + self.node.log.info(f"Head has been touched") + + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.RUNNING + + +class ResetHeadTouched(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(ResetHeadTouched, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="head_touched", access=py_trees.common.Access.WRITE) + + def update(self): + self.blackboard.head_touched = False + return py_trees.common.Status.SUCCESS + +class ResetPixel(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(ResetPixel, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="point_to_pixel", access=py_trees.common.Access.WRITE) + + + def update(self): + self.blackboard.point_to_pixel = None + return py_trees.common.Status.SUCCESS + + + +""" Standby +""" +class Standby(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(Standby, self).__init__(name) + self.node = node + + def update(self): + return py_trees.common.Status.RUNNING + +class CalculateGapPointLocation(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(CalculateGapPointLocation, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="entities", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="point_to_pixel", access=py_trees.common.Access.WRITE) + + def update(self): + + # TODO: Test this + GAP_LIM = 50 + if "couch" not in self.blackboard.entities or "person" not in self.blackboard.entities: + return py_trees.common.Status.RUNNING + + print(self.blackboard.entities) + couch = {"left":self.blackboard.entities["couch"][0], "right": self.blackboard.entities["couch"][1]} + persons = self.blackboard.entities["person"] + spaces = [] + + # Calculate all gaps in the sofa + for i in range(len(persons)): + left = persons[i][0] + right = persons[i][1] + + if i == 0: + spaces.append({"left": couch["left"], "right": left, "gap": couch["left"] - left}) + else: + prev_right = persons[i-1][1] + spaces.append({"left": prev_right, "right": left, "gap": prev_right - right}) + right = persons[len(persons)-1][1] + spaces.append({"left": right, "right": couch["right"], "gap": right - couch["right"]}) + + largest_gap = max(spaces, key= lambda x: x["gap"]) + + # Check if gap is actually big enough + if largest_gap["gap"] >= GAP_LIM: + # Gets middle location of the left and right x pixel + mid_x = (largest_gap["right"] + largest_gap["left"])/2 + y = self.blackboard.entities["height"]/2 # Decide for value (if even needed) + self.blackboard.point_to_pixel = [mid_x, y] + print(self.blackboard.point_to_pixel) + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.FAILURE + + +""" + Gets and saves the pixel coordinate of the first person found which has not + been introduced yet and adds them to the list of introduced people. + If such a person cannot be found, halts the tree +""" +class GetFacePixelCoordinate(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(GetFacePixelCoordinate, self).__init__(name) + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="already_introduced_people", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="recognized_people", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="current_person", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="already_introduced_people", access=py_trees.common.Access.WRITE) + self.blackboard.register_key(key="point_to_pixel", access=py_trees.common.Access.WRITE) + self.blackboard.register_key(key="say_dynamic_str", access=py_trees.common.Access.WRITE) + + + def update(self): + + for id in self.blackboard.recognized_people: + name = self.blackboard.recognized_people[id]["name"] + + # If name is None, that means that it's a person that we have not introduced but + # the facial recognition has found + if name is None: + continue #pass?? + # This means we have found a person that we have not introduced before and that has a name + + if name not in self.blackboard.already_introduced_people: # and name != self.blackboard.current_person: + # face_center will be None if the person was not in frame the last time faces were captured + if self.blackboard.recognized_people[id]["face_center"] is None: + continue + print("Face pixel: " + str(self.blackboard.recognized_people[id]["face_center"])) + x = self.blackboard.recognized_people[id]["face_center"][0] + y = self.blackboard.recognized_people[id]["face_center"][1] + #print("Face pixel for: " + name) + print([x,y]) + + self.blackboard.point_to_pixel = [x, y] + #self.blackboard.already_introduced_people.append(name) + #self.blackboard.say_dynamic_str = "This is " + name + return py_trees.common.Status.SUCCESS + + return py_trees.common.Status.RUNNING + + +""" + Checks if everyone that is currently saved has been introduced, + if so return SUCCESS otherwise return FAILURE +""" +class IsEveryoneIntroduced(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(IsEveryoneIntroduced, self).__init__(name) + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="already_introduced_people", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="already_introduced_people", access=py_trees.common.Access.WRITE) + self.blackboard.register_key(key="recognized_people", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="current_person", access=py_trees.common.Access.READ) + + def update(self): + # Succeed when everyone has been introduced + print(self.blackboard.recognized_people) + for id in self.blackboard.recognized_people: + id_name = self.blackboard.recognized_people[id]['name'] + #if we have detected a person more than once, the name will be None for id + if id_name is not None: + if id_name not in self.blackboard.already_introduced_people: #and id_name != self.blackboard.current_person: + return py_trees.common.Status.FAILURE + + + self.blackboard.already_introduced_people = [] + + return py_trees.common.Status.SUCCESS + +class IsBatteryLow(py_trees.behaviour.Behaviour): + def __init__(self, name) -> None: + super(IsBatteryLow, self).__init__(name) + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="Battery") + self.blackboard.register_key(key="low_percentage", access=py_trees.common.Access.READ) + + def update(self): + if self.blackboard.low_percentage: + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.FAILURE + + +class Say(py_trees.behaviour.Behaviour): + def __init__(self, name, node, message: str) -> None: + super(Say, self).__init__(name) + self.node = node + self.message = message + self.say_pub = self.node.say_publisher + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="is_talking", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="is_talking", access=py_trees.common.Access.WRITE) + + def initialise(self): + msg = String() + msg.data = self.message + self.say_pub.publish(msg) + self.blackboard.is_talking = True + + def update(self): + if self.blackboard.is_talking: + return py_trees.common.Status.RUNNING + return py_trees.common.Status.SUCCESS + + +class SayDynamic(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(SayDynamic, self).__init__(name) + self.node = node + self.say_pub = self.node.say_publisher + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="say_dynamic_str", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="say_dynamic_str", access=py_trees.common.Access.WRITE) + + def initialise(self): + msg = String() + msg.data = self.blackboard.say_dynamic_str + self.say_pub.publish(msg) + self.blackboard.say_dynamic_str = "" + + def update(self): + return py_trees.common.Status.SUCCESS + + +class WaitForItem(py_trees.behaviour.Behaviour): + def __init__(self, name, node, key: str) -> None: + super(WaitForItem, self).__init__(name) + self.node = node + self.key = key + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="last_recognized_item", access=py_trees.common.Access.READ) + + + def update(self): + print(self.blackboard.last_recognized_item) + if self.blackboard.last_recognized_item and self.blackboard.last_recognized_item.key == self.key: + return py_trees.common.Status.SUCCESS + return py_trees.common.Status.RUNNING + + + +class RemoveItem(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(RemoveItem, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="last_recognized_item", access=py_trees.common.Access.WRITE) + + def initialise(self): + self.blackboard.last_recognized_item = None + + def update(self): + self.blackboard.last_recognized_item = None + return py_trees.common.Status.SUCCESS + +class WaitForPerson(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(WaitForPerson, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="found_new_person", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="found_new_person", access=py_trees.common.Access.WRITE) + + def update(self): + if self.blackboard.found_new_person: + self.blackboard.found_new_person = False + return py_trees.common.Status.SUCCESS + return py_trees.common.Status.RUNNING + + +class ConfirmSay(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(ConfirmSay, self).__init__(name) + self.node = node + self.say_pub = self.node.say_publisher # FOR TESTING + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="recognized_text", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="recognized_text", access=py_trees.common.Access.WRITE) + self.blackboard.register_key(key="last_recognized_item", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="last_recognized_item", access=py_trees.common.Access.WRITE) + + + def update(self): + msg = String() # TESTING + + if "yes" in self.blackboard.recognized_text.lower(): + msg.data = "Confirmed" + self.say_pub.publish(msg) + self.blackboard.recognized_text = "" + return py_trees.common.Status.SUCCESS + elif "no" in self.blackboard.recognized_text.lower(): + msg.data = "No" + self.say_pub.publish(msg) + self.blackboard.recognized_text = "" + self.blackboard.last_recognized_item = None # TODO: Is this too "hard-coded"? + return py_trees.common.Status.FAILURE + return py_trees.common.Status.RUNNING + + +class AddItemToPerson(py_trees.behaviour.Behaviour): + def __init__(self, name, node) -> None: + super(AddItemToPerson, self).__init__(name) + self.node = node + + def setup(self, **kwargs): + self.blackboard = py_trees.blackboard.Client(name="bb") + self.blackboard.register_key(key="recognized_people", access=py_trees.common.Access.WRITE) + self.blackboard.register_key(key="last_recognized_item", access=py_trees.common.Access.READ) + self.blackboard.register_key(key="last_recognized_item", access=py_trees.common.Access.WRITE) + self.blackboard.register_key(key="current_person", access=py_trees.common.Access.WRITE) + def initialise(self): + # Get the closest person to Pepper + closest_person = min(self.blackboard.recognized_people.values(), key=lambda x: x["z_index"]) + param = self.blackboard.last_recognized_item + + closest_person[param.key] = param.value + + # Save the name of the current person being introduced + if param.key == "name": + self.blackboard.current_person = param.value + + self.blackboard.last_recognized_item = None + + # TODO: Code if multiple parameters can be passed through last_recognized_item + """for key in params: + # In case we ever use other parameters with dialogflow + if key in ["name", "drink", "age"]: + value = params[key] + closest_person[key] = value""" + + + def update(self): + return py_trees.common.Status.SUCCESS diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_test_functions.py b/src/lhw_intelligence/lhw_intelligence/py_tree_test_functions.py new file mode 100644 index 0000000000000000000000000000000000000000..6f6d2d407ccbc7194f80e1a06b43c39433181b3c --- /dev/null +++ b/src/lhw_intelligence/lhw_intelligence/py_tree_test_functions.py @@ -0,0 +1,114 @@ +from .py_tree_behaviours import IsDoorOpen, IsHeadTouched, MoveToPoint, RotateRobot, Standby, StopRobot, Say +from .motion_behaviours import * +from .behaviour_copy import Behaviour +import py_trees +from lhw_interfaces.msg import RobotAction + + +class Py_Tree_Tests(): + + def __init__(self, py_trees_node): + self.py_trees_node = py_trees_node + + """Test for rotating and stopping the robot. This works""" + def rotate_and_stop(self) -> py_trees.composites.Sequence: + + timer = py_trees.timers.Timer(name="Timer", duration=2) + rotate_robot = RotateRobot(name="Rotate Robot", node=self.py_trees_node) + timer2 = py_trees.timers.Timer(name="Timer2", duration=10) + stop_robot = StopRobot(name="Stop Robot", node=self.py_trees_node) + timer3 = py_trees.timers.Timer(name="Timer3", duration=10) + root = py_trees.composites.Sequence(name="rotate sequence", children= [timer, rotate_robot, timer2, stop_robot, timer3]) + return root + + """ + For testing repeating behaviour with resetting of blackboard variables + """ + def very_random_test(self): + greet_person_subtree = py_trees.composites.Sequence(name="Greet Person Subtree") + greet = Say(name="Greet", node=self.py_trees_node, message="Hello!") + name_wait_sel = py_trees.composites.Selector(name="Name Wait Sel") + get_name_seq = py_trees.composites.Sequence(name="Get Name Seq") + ask_for_name = Say(name="Ask For Name", node=self.py_trees_node, message="Whats your name?") + head_touched = IsHeadTouched(name="Head Touched", node=self.py_trees_node) + is_door_open = IsDoorOpen(name="Is Door Open", node=self.py_trees_node) + complete = Say(name="Complete", node=self.py_trees_node, message="Complete") + + ask_name_again_seq = py_trees.composites.Sequence(name="Ask Name Again Seq") + ask_name_again = Say(name="Ask Name Again", node=self.py_trees_node, message="I'm sorry I didnt catch that") + #reset_head_touched = ResetHeadTouched(name="Reset Head Touched", node=node) + fail = py_trees.behaviours.Failure(name="Failure") + + name_retry = Standby(name="Name Retry", node=self.py_trees_node) + + + ask_name_again_seq.add_children([ask_name_again, fail]) + + get_name_seq.add_children([ask_for_name, head_touched, is_door_open, complete]) + + greet_person_subtree.add_children([greet, name_wait_sel]) + + name_wait_sel.add_children([get_name_seq, ask_name_again_seq, name_retry]) + + return greet_person_subtree + + """ To help test the navstack """ + def navigation_test(self): + behaviour = Behaviour(self) + + move_to_safety_point = MoveToPoint(name="Move To Safety Point", node=self.py_trees_node, pose=behaviour.position_to_pose({"x": 1.0,"y": 0.0,"z": 0.0})) + standby2 = Standby(name="Standby", node=self.py_trees_node) + + return py_trees.composites.Sequence(name="Nav Test Seq", children=[move_to_safety_point, standby2]) + + + def test_pointing_straight_forward(self): + action = RobotAction() + action.type = RobotAction.POINT_ABS_STRAIGHT + action.right_arm = False + + pepper_point_straight_l = PepperAction(name="Point", node=self.py_trees_node, action=action) + + action2 = RobotAction() + action2.type = RobotAction.POINT_ABS_STRAIGHT + action2.right_arm = True + pepper_point_straight_r = PepperAction(name="Pepper Point", node=self.py_trees_node, action=action2) + + return py_trees.composites.Parallel(name="Pointing Test", children=[pepper_point_straight_r, pepper_point_straight_l]) + + """In development (WIP)""" + def point_at_face_test(self): + + find_person = WaitForPerson(name="Find person", node=self.py_trees_node) + getPixel = GetFacePixelCoordinate(name="getPixel", node=self.py_trees_node) + pointAt = PointAtPixel(name="Point At", node=self.py_trees_node) + timer = py_trees.timers.Timer(name="Timer", duration=5) + #reset_pixel = ResetPixel(name="Reset", node=self) + + return py_trees.composites.Sequence(name="POINT", children=[getPixel, pointAt, timer]) + + def test_pepper_security(self): + + rotate_robot = RotateRobot(name="Rotate Robot", node=self.py_trees_node) + timer = py_trees.timers.Timer(name="Timer2", duration=20) + stop_robot = StopRobot(name="Stop Robot", node=self.py_trees_node) + safety_off = PepperSetSafety(name="Safety Off", node=self.py_trees_node, safety_on = False) + + return py_trees.composites.Sequence(name="ROTATE", children=[safety_off, rotate_robot, timer, stop_robot]) + + def pepper_nod(self): + + timer1 = py_trees.timers.Timer(name="Timer1", duration=2) + timer2 = py_trees.timers.Timer(name="Timer2", duration=2) + tilt_up = PepperTiltHead(name="Tilt Up", node=self.py_trees_node, tilt_up=True) + tilt_down = PepperTiltHead(name="Tilt Down", node=self.py_trees_node, tilt_up=False) + + return py_trees.composites.Sequence(name="Head Bob", children=[tilt_down, timer1, tilt_up, timer2]) + + def test_touch_head_and_say(self): + + timer = py_trees.timers.Timer(name="Timer1", duration=5) + head_touched = IsHeadTouched(name="Head Touched", node=self.py_trees_node) + speak = Say(name="speak", node=self.py_trees_node, message="Hello World") + return py_trees.composites.Sequence(name="TEST", children=[head_touched, speak, timer]) + diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees.py b/src/lhw_intelligence/lhw_intelligence/scenario_trees.py new file mode 100644 index 0000000000000000000000000000000000000000..8e7ab7382b50ef6fa4e1311e99ad01a93bff8af8 --- /dev/null +++ b/src/lhw_intelligence/lhw_intelligence/scenario_trees.py @@ -0,0 +1,214 @@ +import py_trees +import operator +from .py_tree_behaviours import * +from .motion_behaviours import * +from lhw_intelligence.behaviour_copy import Behaviour + +""" Py_tree for the safety check procedure +""" +def safety_check_tree(node, config: dict): + + behaviour = Behaviour(node) + waiting_point = behaviour.position_to_pose(config["waiting_point"]) + safety_pose = behaviour.position_to_pose(config["safety_point"]) + exit_pose = behaviour.position_to_pose(config["exit"]) + + #say_door_open = Say(name="test say", node=node, message="Checking that door is open") + is_door_open = IsDoorOpen(name="Is Door Open", node=node) + move_forward = MoveToPoint(name="Move Forward", node=node, pose=waiting_point) + wait = py_trees.timers.Timer(name="Wait", duration=2) + move_to_safety_point = MoveToPoint(name="Move to Point",node=node, pose=safety_pose) + is_head_touched = IsHeadTouched(name="Is Head Touched", node=node) + head_touched_retry = Standby(name="Head Touched Retry", node=node) + move_to_exit = MoveToPoint(name="Move to Exit", node=node, pose=exit_pose) + standby = Standby(name="Standby", node=node) + + head_touched_selector_wait = py_trees.composites.Selector( + name="Head Touched Selector Wait", + children=[is_head_touched, head_touched_retry] + ) + + safety_check_sequence = py_trees.composites.Sequence( + name="Safety Check Sequence", + children=[is_door_open, move_forward, wait, move_to_safety_point, head_touched_selector_wait, move_to_exit] + ) + + return py_trees.composites.Sequence(name="Standby Sequence", children=[safety_check_sequence, standby]) + + +""" Py_tree for the safety check procedure +""" +def enter_arena(node): + + #tell_referee_to_open_door = Say(name="Open The Door", node=node, message="Please open the door for me") + is_door_open = IsDoorOpen(name="Is Door Open", node=node) + move_forward = MoveForward(name="Move Forward", node=node, x=1.0) + timer = py_trees.timers.Timer(name="Wait", duration=5) + #rotate = RotateRobot(name="Rotate", node=node) # TODO: Add radian + standby = Standby(name="Standby", node=node) + + enter_arena_sequence = py_trees.composites.Sequence( + name="Enter Arena Sequence", + children=[is_door_open, move_forward, timer] + ) + + return py_trees.composites.Sequence(name="Standby Sequence", children=[enter_arena_sequence, standby]) + + +""" + Behaviour tree to greet new people and get what their name and favourite drink is. + + Required running nodes: + - face_analysis node & deepface server + - Dialogflow + - Naoqi_driver & animated speech & microphone + Reads/Writes to blackboard variables: + - last_recognized_item + - recognized_people + - recognized_text +""" + +def greet_person(node, repeat_message="Okay, can you repeat that?"): + + + start = Say(name="Start", node=node, message="Hello! Could you please look into my eyes so I can identify you?") + find_person = WaitForPerson(name="Find person", node=node) + greet = Say(name="Greet", node=node, message="Welcome!") + + ask_for_name = Say(name="Ask For Name", node=node, message="What's your name?") + wait_for_name = WaitForItem(name="Wait For Name", node=node, key="name") + confirm_name = ConfirmSay(name="Confirm Name", node=node) + add_name = AddItemToPerson(name="Add Name", node=node) + + ask_name_again = Say(name="Ask Name Again", node=node, message=repeat_message) + fail_name = py_trees.behaviours.Failure(name='Name Failure') + name_retry = Standby(name="Name Retry", node=node) + + ask_for_favorite_drink = Say(name="Ask For Favorite Drink", node=node, message="What's your favorite drink?") + wait_for_drink = WaitForItem(name="Wait For Drink", node=node, key="drinks") + confirm_drink = ConfirmSay(name="Confirm Drink", node=node) + add_drink = AddItemToPerson(name="Add Drink", node=node) + + ask_drink_again = Say(name="Ask Drink Again", node=node,message=repeat_message) + fail_drink = py_trees.behaviours.Failure(name='Drink Failure') + drink_retry = Standby(name="Drink Retry", node=node) + + completed_greet = Say(name="Complete", node=node, message="Thank you! That should be all I need to know.") + + + get_name_seq = py_trees.composites.Sequence( + name="Get Name Seq", + children=[ask_for_name, wait_for_name, confirm_name, add_name] + ) + + ask_name_again_seq = py_trees.composites.Sequence( + name="Ask Name Again Seq", + children=[ask_name_again, fail_name] + ) + + name_wait_sel = py_trees.composites.Selector( + name="Name Wait Sel", + children=[get_name_seq, ask_name_again_seq, name_retry] + ) + + get_drink_seq = py_trees.composites.Sequence( + name="Get Drink Seq", + children=[ask_for_favorite_drink, wait_for_drink, confirm_drink, add_drink] + ) + + ask_drink_again_seq = py_trees.composites.Sequence( + name="Ask Drink Again Seq", + children=[ask_drink_again, fail_drink] + ) + + drink_wait_sel = py_trees.composites.Selector( + name="Drink Wait Sel", + children=[get_drink_seq, ask_drink_again_seq, drink_retry] + ) + + return py_trees.composites.Sequence( + name="Greet Person Subtree", + children=[start, find_person, greet, name_wait_sel, drink_wait_sel, completed_greet]) + +def receptionist(node): + + + wait = py_trees.timers.Timer(name="Wait", duration=2) + is_head_touched = IsHeadTouched(name="Is Head Touched", node=node) + + tell_referee_to_open_door = Say(name="Tell Referee To Open Door", node=node, message="Please open the door for me") + greet_person_subtree = greet_person(node=node) + tell_person_to_follow = Say(name="Tell Person To Follow", node=node, message="Please come with me") + #look_down = PepperTiltHead(name="Look Down", node=node) + #move_to_sofa = MoveToPoint(name="Move To Sofa", node=node) + #pepper_center_head = PepperCenterHead(name="Pepper Center Head", node=node) + + all_people_introduced = IsEveryoneIntroduced(name="All People Introduced", node=node) + get_face_pixel = GetFacePixelCoordinate(name="Get Face Pixel", node=node) + point_at_person = PointAtPixel(name="Point At Person", node=node) + say_name_of_person = SayDynamic(name="Say Name Of Person", node=node) + const_fail = py_trees.behaviours.Failure(name="Const Fail") + + introduce_retry = Standby(name="Introduce Retry", node=node) + + get_couch_gap_pixel = CalculateGapPointLocation(name="Get Couch Gap Pixel", node=node) + point_at_empty_seat = PointAtPixel(name="Point At Empty Seat", node=node) + tell_person_to_sit = Say(name="Tell Person To Sit", node=node, message="Sit here") + + say_there_is_no_seat = Say(name="Say There Is No Seat", node=node, message="There is no seat for you") + + #move_to_door = MoveToPoint(name="Move To Door", node=node) + + + introduce_person = py_trees.composites.Sequence( + name=" Introduce person", + children=[get_face_pixel, point_at_person, say_name_of_person, const_fail] + ) + + unsuccessful_seating = py_trees.composites.Sequence( + name="Unsuccessful Seating", + children=[say_there_is_no_seat] + ) + + successful_seating = py_trees.composites.Sequence( + name="Successful Seating", + children=[get_couch_gap_pixel, point_at_empty_seat, tell_person_to_sit] + ) + + empty_seat_sel = py_trees.composites.Selector( + name="Empty Seat Sel", + children=[successful_seating, unsuccessful_seating] + ) + + intros_wait_sel = py_trees.composites.Selector( + name="Intros Wait Sel", + children=[all_people_introduced, introduce_person, introduce_retry] + ) + + at_sofa_seq = py_trees.composites.Sequence( + name="At Sofa Seq", + children=[intros_wait_sel, empty_seat_sel] + ) + + not_head_touched = py_trees.composites.Sequence( + name="Not Head Touched", + children=[is_head_touched, wait] + ) + + head_touched = py_trees.composites.Sequence( + name="Head Touched", + children=[tell_referee_to_open_door, greet_person_subtree, tell_person_to_follow, at_sofa_seq] + ) + + either_or = py_trees.idioms.either_or( + name="EitherOr", + conditions=[ + py_trees.common.ComparisonExpression("head_touched", False, operator.eq), + py_trees.common.ComparisonExpression("head_touched", True, operator.eq)], + subtrees=[not_head_touched, head_touched] + ) + + #root.add_children([is_head_touched, wait, tell_referee_to_open_door, greet_person_subtree, tell_person_to_follow, at_sofa_seq]) + return py_trees.composites.Sequence(name="Root", children=[either_or]) + +