86: Added confirmation behaviour & dialogflow_response subscriber/callback

src/lhw_intelligence/lhw_intelligence/safety_check/py_tree_behaviours.py

@@ -9,7 +9,7 @@ from ..behaviour_copy import Behaviour #TODO: Move behaviours to here
 from std_msgs.msg import Bool, String, Empty, Int16, Header, Int32
 from builtin_interfaces.msg import Time
 import time, random
-from lhw_interfaces.msg import Entities, Response, Entity, Result, RobotAction, HeadTouch
+from lhw_interfaces.msg import Entities, Response, Entity, Result, RobotAction
 from nav2_msgs.action import NavigateToPose
 from geometry_msgs.msg import Twist, Pose, PoseStamped
 from diagnostic_msgs.msg._diagnostic_array import DiagnosticArray
@@ -76,6 +76,7 @@ class MockNavigation(py_trees.behaviour.Behaviour):
         pass
 
     def update(self):
+        return py_trees.common.Status.SUCCESS
         rand = random.random()
         if rand < 0.3:
             return py_trees.common.Status.FAILURE
@@ -216,6 +217,7 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
 
 
 """ Checks if the head has been tapped
+    This is a blocking behaviour
 """
 class IsHeadTouched(py_trees.behaviour.Behaviour):
     def __init__(self, name, node) -> None:
@@ -322,3 +324,48 @@ class IsBatteryLow(py_trees.behaviour.Behaviour):
 
     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):
+        self.msg = String()
+        self.msg.data = self.message
+        pass 
+    def initialise(self):
+        self.say_pub.publish(self.msg)
+
+    def update(self):
+        return py_trees.common.Status.SUCCESS
+
+    def terminate(self, new_status):
+        pass
+
+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="dialogflow_response", access=py_trees.common.Access.READ)
+    def initialise(self):
+        pass
+
+    def update(self):
+        msg = String()
+        if "yes" in self.blackboard.dialogflow_response.recognized_text.lower():
+            msg.data = "Confirmed"
+            self.say_pub.publish(msg)
+            return py_trees.common.Status.SUCCESS
+        msg.data = "No"
+        self.say_pub.publish(msg)
+        return py_trees.common.Status.FAILURE
+
+    def terminate(self, new_status):
+        pass

src/lhw_intelligence/lhw_intelligence/safety_check/scenario_trees.py

 import py_trees
 from .py_tree_behaviours import *
+from lhw_intelligence.behaviour_copy import Behaviour
 
 """ Py_tree for the safety check procedure
 """
-def safety_check_tree(node, safety_point: Pose, exit: Pose):
-    root = py_trees.composites.Selector(name="Standby Selector")
+def safety_check_tree(node, config: dict):
+    behaviour = Behaviour(node)
+    safety_pose = behaviour.position_to_pose(config["safety_point"])
+    exit_pose = behaviour.position_to_pose(config["exit"])
+
+    root = py_trees.composites.Sequence(name="Standby Sequence")
     
     safety_check_sequence = py_trees.composites.Sequence(name="Safety Check Sequence")
     is_door_open = IsDoorOpen(name="Is Door Open", node=node)
-    move_forward = MoveForward(name="Move Forward", node=node, x=1.0)
+    mock_forward = MockNavigation(name="Mock Forward Move", node=node)
+    #move_forward = MoveForward(name="Move Forward", node=node, x=1.0)
     wait = py_trees.timers.Timer(name="Wait", duration=2)
-    move_to_safety_point = MoveToPoint(name="Move to Point",node=node, pose=safety_point)
-    signal_given = IsHeadTouched(name="Signal given", node=node)
-    move_to_exit = MoveToPoint(name="Move to Exit", node=node, pose=exit)
-    mock_nav = MockNavigation(name="Mock move", node=node)
+    mock_safety = MockNavigation(name="Mock Safety Point Move", node=node)
+    #move_to_safety_point = MoveToPoint(name="Move to Point",node=node, pose=safety_pose)
+    head_touched_selector_wait = py_trees.composites.Selector(name="Head Touched Selector Wait")
+    is_head_touched = IsHeadTouched(name="Is Head Touched")
+    head_touched_retry = Standby(name="Head Touched Retry")
+    #move_to_exit = MoveToPoint(name="Move to Exit", node=node, pose=exit_pose)
+    mock_exit = MockNavigation(name="Mock Exit Move", node=node)
     standby = Standby(name="Standby", node=node)
 
+    head_touched_selector_wait.add_children([is_head_touched, head_touched_retry])
+
 
 
-    safety_check_sequence.add_children([is_door_open, move_forward, wait, move_to_safety_point, signal_given, mock_nav])
+    safety_check_sequence.add_children([is_door_open, mock_forward, wait, mock_safety, head_touched_selector_wait, mock_exit])
     root.add_children([safety_check_sequence, standby])
 
     return root # Root