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])
+
+