diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py
new file mode 100644
index 0000000000000000000000000000000000000000..07c6f1b7fa057d4192f3a00a9aeae4d42b627ec7
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py
@@ -0,0 +1,552 @@
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Template behaviour that is used for implementation of new behaviours.
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import py_trees
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+# import lhw_interfaces.actions as lhw_actions
+import std_msgs
+import time
+
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class Say(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ message:str
+ ):
+ super(Say, self).__init__(name=name)
+ self.message = message
+
+ def setup(self, **kwargs):
+ """
+ This function is executed when tree is setup.
+ Args:
+ **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+ Raises:
+ :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Fetch 'node' object passed down from the tree.
+ try:
+ self.node = kwargs['node']
+ except KeyError as e:
+ error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+ raise KeyError(error_message) from e # 'direct cause' traceability
+
+ self.say_pub = self.node.create_publisher(std_msgs.msg.String, 'say', 100)
+
+ self.is_talking_sub = self.node.create_subscription(
+ std_msgs.msg.Bool, 'is_talking', self._is_talking_callback, 10
+ )
+
+ self.is_talking = False
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ msg = std_msgs.msg.String()
+ msg.data = self.message
+ self.say_pub.publish(msg)
+
+ def update(self) -> py_trees.common.Status:
+ """
+ This function is second to be executed right after initialise
+ when behaviour is runned. The execution will continue as long as
+ return flag is RUNNING and will be stopped when return flag is
+ SUCCESS or FAILURE.
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ if self.is_talking:
+ return py_trees.common.Status.RUNNING
+
+ return py_trees.common.Status.SUCCESS
+
+ def terminate(self, new_status: py_trees.common.Status):
+ """
+ This function is executed when behaviour is terminated.
+ Args:
+ new_status: the behaviour is transitioning to this new status
+ """
+ self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+ # WRITE CODE HERE
+
+ def _is_talking_callback(self, msg):
+ self.is_talking = msg.data
+
+
+class Listen(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ category: str
+ ):
+ super(Listen, self).__init__(name=name)
+ self.category = category
+ self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.WRITE
+ )
+
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.WRITE
+ )
+
+
+
+ def setup(self, **kwargs):
+ """
+ This function is executed when tree is setup.
+ Args:
+ **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+ Raises:
+ :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Fetch 'node' object passed down from the tree.
+ try:
+ self.node = kwargs['node']
+ except KeyError as e:
+ error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+ raise KeyError(error_message) from e # 'direct cause' traceability
+
+
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ self.time_start = time.time()
+
+ self.callback_executed = False
+
+ self.dialog_sub = self.node.create_subscription(
+ lhw_msgs.Response,
+ 'dialogflow_response',
+ self._answer_callback,
+ 10
+ )
+
+ def update(self) -> py_trees.common.Status:
+ """
+ This function is second to be executed right after initialise
+ when behaviour is runned. The execution will continue as long as
+ return flag is RUNNING and will be stopped when return flag is
+ SUCCESS or FAILURE.
+ """
+ self.logger.debug("testing -----------------------------------------------------------")
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ # TODO: change to node timer
+ time_duration_sec = time.gmtime(time.time() - self.time_start).tm_sec
+ self.node.get_logger().info(str(time_duration_sec))
+
+ if self.callback_executed:
+ return py_trees.common.Status.SUCCESS
+ if time_duration_sec > 10.0: # If more than 10 sec --> fail
+ return py_trees.common.Status.FAILURE
+
+ return py_trees.common.Status.RUNNING
+
+ def terminate(self, new_status: py_trees.common.Status):
+ """
+ This function is executed when behaviour is terminated.
+ Args:
+ new_status: the behaviour is transitioning to this new status
+ """
+ self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+ # Destroy subscription if still exists
+ if self.dialog_sub:
+ self.node.destroy_subscription(self.dialog_sub)
+
+ # WRITE CODE HERE
+
+ def _answer_callback(self, msg):
+
+ # self.blackboard.recognized_people = False
+ print(self.blackboard)
+
+ self.logger.debug("aa parametes")
+ print(msg.parameters)
+ # self.get_logger().info(msg.parameters)
+ for i in range(len(msg.parameters)):
+ # Check if correct category was found
+
+ self.logger.debug(msg.parameters[i].key)
+ print(msg.parameters[i].key)
+
+ if msg.parameters[i].key == self.category:
+ self.callback_executed = True
+ self.answer = msg.parameters[i].value
+
+ self.blackboard.recognized_people[self.blackboard.current_person_id][self.category] \
+ = self.answer
+
+
+
+
+class BlackboardUpdated(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ category: str
+ ):
+ super(BlackboardUpdated, self).__init__(name=name)
+ self.category = category
+ self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+ def setup(self, **kwargs):
+ """
+ This function is executed when tree is setup.
+ Args:
+ **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+ Raises:
+ :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Fetch 'node' object passed down from the tree.
+ try:
+ self.node = kwargs['node']
+ except KeyError as e:
+ error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+ raise KeyError(error_message) from e # 'direct cause' traceability
+
+ # Tries to read person from blackboard.
+ # If person not found -> creates a mock person for debugging.
+ try:
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.WRITE
+ )
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.READ
+ )
+
+ self.blackboard.recognized_people
+ self.blackboard.current_person_id
+ self.node.get_logger().info("In TRY")
+ except:
+ self.node.get_logger().info("In Except")
+
+ # Temporary write access
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.WRITE
+ )
+ # recognized people borde se ut såhär: [{name: "", drink: "", id: 0}]
+ #self.blackboard.set(self.variable_name, self.variable_value, overwrite=True)
+ self.blackboard.set("recognized_people", [{"id": 0}], overwrite=True)
+ self.blackboard.set("current_person_id", 0, overwrite=True)
+
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ # WRITE CODE HERE
+
+ def update(self) -> py_trees.common.Status:
+ """
+ This function is second to be executed right after initialise
+ when behaviour is runned. The execution will continue as long as
+ return flag is RUNNING and will be stopped when return flag is
+ SUCCESS or FAILURE.
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ # value = self.blackboard.recognized_people[self.blackboard.current_person_id][self.key]
+
+ # Try to read a value from recognized people
+ try:
+ value = self.blackboard.recognized_people[self.blackboard.current_person_id][self.category]
+ confirmed = self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"]
+ except:
+ return py_trees.common.Status.FAILURE
+
+ # Return succes if value for key exists in blackboard and it is confirmed
+ if value and confirmed:
+ return py_trees.common.Status.SUCCESS
+ # Otherwise return false
+ return py_trees.common.Status.FAILURE
+
+ def terminate(self, new_status: py_trees.common.Status):
+ """
+ This function is executed when behaviour is terminated.
+ Args:
+ new_status: the behaviour is transitioning to this new status
+ """
+ self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+ # WRITE CODE HERE
+
+
+class ListenConfirmation(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ category: str
+ ):
+ super(ListenConfirmation, self).__init__(name=name)
+
+ self.category = category
+ self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.WRITE
+ )
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.READ
+ )
+
+ def setup(self, **kwargs):
+ """
+ This function is executed when tree is setup.
+ Args:
+ **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+ Raises:
+ :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Fetch 'node' object passed down from the tree.
+ try:
+ self.node = kwargs['node']
+ except KeyError as e:
+ error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+ raise KeyError(error_message) from e # 'direct cause' traceability
+
+ # WRITE CLIENT SETUP CODE HERE
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ self.time_start = time.time()
+
+ self.dialog_sub = self.node.create_subscription(
+ lhw_msgs.Response,
+ 'dialogflow_response',
+ self._answer_callback,
+ 10
+ )
+
+ self.confirmed = ""
+
+ def update(self) -> py_trees.common.Status:
+ """
+ This function is second to be executed right after initialise
+ when behaviour is runned. The execution will continue as long as
+ return flag is RUNNING and will be stopped when return flag is
+ SUCCESS or FAILURE.
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ # TODO: change to node timer
+ time_duration_sec = time.gmtime(time.time() - self.time_start).tm_sec
+
+ if self.confirmed == "True":
+ self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"] \
+ = True
+ return py_trees.common.Status.SUCCESS
+ elif self.confirmed == "False":
+ self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"] \
+ = False
+ return py_trees.common.Status.FAILURE
+
+ if time_duration_sec > 10.0: # If more than 10 sec --> fail
+ return py_trees.common.Status.FAILURE
+
+ return py_trees.common.Status.RUNNING
+
+
+ def terminate(self, new_status: py_trees.common.Status):
+ """
+ This function is executed when behaviour is terminated.
+ Args:
+ new_status: the behaviour is transitioning to this new status
+ """
+ self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+ # Destroy subscription if still exists
+ try:
+ self.node.destroy_subscription(self.dialog_sub)
+ except:
+ pass
+
+
+ def _answer_callback(self, msg):
+ if "yes" in msg.recognized_text:
+ self.confirmed = "True"
+
+ elif "no" in msg.recognized_text:
+ self.confirmed = "False"
+
+ # ADD PRIVATE FUNCTIONS HERE "_function_name"
+
+class Idle(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name:str
+ ):
+ super(Idle, self).__init__(name=name)
+
+
+
+ def update(self) -> py_trees.common.Status:
+ """
+ This function is second to be executed right after initialise
+ when behaviour is runned. The execution will continue as long as
+ return flag is RUNNING and will be stopped when return flag is
+ SUCCESS or FAILURE.
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ return py_trees.common.Status.RUNNING
+
+
+class AskForConfirmation(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ variable: str
+ ):
+ super(AskForConfirmation, self).__init__(name=name)
+ self.variable = variable
+
+ self.blackboard = py_trees.blackboard.Client(name=name)
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.READ
+ )
+
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.READ
+ )
+
+
+ def setup(self, **kwargs):
+ """
+ This function is executed when tree is setup.
+ Args:
+ **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+ Raises:
+ :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Fetch 'node' object passed down from the tree.
+ try:
+ self.node = kwargs['node']
+ except KeyError as e:
+ error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+ raise KeyError(error_message) from e # 'direct cause' traceability
+
+ self.say_pub = self.node.create_publisher(std_msgs.msg.String, 'say', 100)
+
+ self.is_talking_sub = self.node.create_subscription(
+ std_msgs.msg.Bool, 'is_talking', self._is_talking_callback, 10
+ )
+
+ self.is_talking = False
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ current_person_id = self.blackboard.current_person_id
+
+ # Get information about recognized people from blackboard
+ blackboard_variable = self.blackboard.recognized_people[current_person_id][self.variable]
+
+ print(blackboard_variable)
+
+ msg = std_msgs.msg.String()
+
+ if self.variable == "actors":
+ msg.data = "Is your name " + blackboard_variable + "?"
+ elif self.variable == "drinks":
+ msg.data = "Is your favorite drink " + blackboard_variable + "?"
+
+
+ self.say_pub.publish(msg)
+
+ def update(self) -> py_trees.common.Status:
+ """
+ This function is second to be executed right after initialise
+ when behaviour is runned. The execution will continue as long as
+ return flag is RUNNING and will be stopped when return flag is
+ SUCCESS or FAILURE.
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ if self.is_talking:
+ return py_trees.common.Status.RUNNING
+
+ return py_trees.common.Status.SUCCESS
+
+ def terminate(self, new_status: py_trees.common.Status):
+ """
+ This function is executed when behaviour is terminated.
+ Args:
+ new_status: the behaviour is transitioning to this new status
+ """
+ self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+ # WRITE CODE HERE
+
+ def _is_talking_callback(self, msg):
+ self.is_talking = msg.data
+
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree
new file mode 100644
index 0000000000000000000000000000000000000000..e00b3efa6c6fba84f0c9c73e1a37820e5eefdbb0
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree
@@ -0,0 +1,15 @@
+->;;Root
+| ?;;Find Name
+| | (got name)
+| | ->;;get name sequence
+| | | [Ask: What is your name]
+| | | [Listen for responce1]
+| | | [ask: is your name *name*?]
+| | | [Listen for yes/no responce1]
+| ?;;find drink
+| | (got drink)
+| | ->;; get drink sequence
+| | | [Ask: What is your favorite drink]
+| | | [Listen for responce2]
+| | | [ask: is your favorite drink *drink*?]
+| | | [Listen for yes/no responce2]
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree
index 9ff36c0c7ee3ea9e6d980310969c50b6a5e4c032..37afec979c622e0d4b5a797529a333439561e3b0 100644
--- a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree
@@ -1,19 +1,26 @@
->;;Root
-| (Is Head Touched)
-| [Say - open door promt]
-| ->;;Loop
-| | ->;;Greet Person
-| | | [Find person subtree]
-| | | [Greet and ask some questions subtree]
-| | [Say - follow me]
-| | [MoveTo - location sofa]
-| | ->;;Introduce all
-| | | ->;;Introduce person until all persons are introduced
-| | | | [Find person subtree]
-| | | | [Introduce person subtree]
-| | | (Is all introduced)
-| | ->;;Ask person to sit until them is seated
-| | | [Find empty spot]
-| | | [Ask person to sit]
-| | | (Is person seated)
-| | [MoveTo - location door]
\ No newline at end of file
+| ?;;TouchHead
+| | (Is Head Touched)
+| | [Wait and blink]
+| ?;;FindPersonToIntroduce
+| | (has person to introduce)
+| | ->;;DoorGreeter
+| | | [navigate door subtree]
+| | | ?;;DoorChecker
+| | | | (is door open)
+| | | | ->;;DoorActions
+| | | | | [say: open_door_promt]
+| | | | | [check door]
+| | | ?;;PersonChecker
+| | | | (found person)
+| | | | ->;;GreetActions
+| | | | | [find person]
+| | | | | [greet and ask subtree]
+| [navigate: sofa subtree]
+| ?;;IntroduceChecker
+| | (all introduced)
+| | ->;;IntroduceActions
+| | | [Introduce person subtree]
+| | | (all introduced)
+| [find empty spot subtree]
+| [reset conditions]
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree
new file mode 100644
index 0000000000000000000000000000000000000000..3c467cf830f48d32559ff9a7337a83db598acb28
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree
@@ -0,0 +1,27 @@
+->;;Root
+| ?;;TouchHead
+| | (Is Head Touched)
+| | [Wait and blink]
+| ?;;FindPersonToIntroduce
+| | (has person to introduce)
+| | ->;;DoorGreeter
+| | | [navigate door subtree]
+| | | ?;;DoorChecker
+| | | | (is door open)
+| | | | ->;;DoorActions
+| | | | | [say: open_door_promt]
+| | | | | [check door]
+| | | ?;;PersonChecker
+| | | | (found person)
+| | | | ->;;GreetActions
+| | | | | [find person]
+| | | | | [greet and ask subtree]
+| [navigate: sofa subtree]
+| ?;;IntroduceChecker
+| | (all introduced)
+| | ->;;IntroduceActions
+| | | [Introduce person subtree]
+| | | (all introduced)
+| [find empty spot subtree]
+| [reset conditions]
+|
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py
new file mode 100644
index 0000000000000000000000000000000000000000..08c4011c8a953c97d23acdee1bfc8bb89b031570
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py
@@ -0,0 +1,152 @@
+import py_trees
+import py_trees_ros
+import py_trees_ros.trees
+import py_trees.console as console
+import rclpy
+import sys
+
+# from ../behaviours import greet_person_behaviours
+from lhw_intelligence.behaviours import greet_person_behaviours
+
+def create_root() -> py_trees.behaviour.Behaviour:
+ root = py_trees.composites.Sequence(name="Root")
+
+ # -------------------------------------------
+ # | This is get name part |
+ # -------------------------------------------
+
+ find_name_selector = py_trees.composites.Selector(
+ name="FindNameSelector"
+ )
+
+ ask_for_name_sequence = py_trees.composites.Sequence(
+ name="AskForNameSequence"
+ )
+
+ ask_for_name = greet_person_behaviours.Say(
+ name="Ask For Name",
+ message="What is your name?"
+ )
+
+ answer_name = greet_person_behaviours.Listen(
+ name="Answer Name",
+ category="actors"
+ )
+
+ ask_for_confirmation_name = greet_person_behaviours.AskForConfirmation(
+ name="Ask For Confirmation",
+ variable="actors"
+ )
+
+ answer_confirmation_name = greet_person_behaviours.ListenConfirmation(
+ name="Answer confirmation",
+ category="actors"
+ )
+
+ already_have_name = greet_person_behaviours.BlackboardUpdated(
+ name="Already have name",
+ category="actors"
+ )
+
+ idle = greet_person_behaviours.Idle(name = "idle")
+
+ ask_for_name_sequence.add_children([
+ ask_for_name,
+ answer_name,
+ ask_for_confirmation_name,
+ answer_confirmation_name
+ ])
+
+
+ find_name_selector.add_children([
+ already_have_name, ask_for_name_sequence
+ ])
+
+ # -------------------------------------------
+ # | This is get drink part |
+ # -------------------------------------------
+
+ find_drink_selector = py_trees.composites.Selector(
+ name="FindDrinkSelector"
+ )
+
+ ask_for_drink_sequence = py_trees.composites.Sequence(
+ name="AskForDrinkSequence"
+ )
+
+ ask_for_drink = greet_person_behaviours.Say(
+ name="Ask For Drink",
+ message="What is your favorite drink?"
+ )
+
+ answer_drink = greet_person_behaviours.Listen(
+ name="Answer Drink",
+ category="drinks"
+ )
+
+ ask_for_confirmation_drink = greet_person_behaviours.AskForConfirmation(
+ name="Ask For Confirmation",
+ variable="drinks"
+ )
+
+ answer_confirmation_drink = greet_person_behaviours.ListenConfirmation(
+ name="Answer confirmation",
+ category="drinks"
+ )
+
+ already_have_drink = greet_person_behaviours.BlackboardUpdated(
+ name="Already have drink",
+ category="drinks"
+ )
+
+ ask_for_drink_sequence.add_children([
+ ask_for_drink,
+ answer_drink,
+ ask_for_confirmation_drink,
+ answer_confirmation_drink])
+
+ find_drink_selector.add_children([
+ already_have_drink,
+ ask_for_drink_sequence,
+ ])
+
+ root.add_children([find_name_selector, find_drink_selector])
+ # root.add_children([find_drink_selector])
+ # root.add_children([find_name_selector])
+
+ return root
+
+def main():
+ """
+ """
+ rclpy.init(args=None)
+
+
+ root = create_root()
+ tree = py_trees_ros.trees.BehaviourTree(
+ root=root,
+ unicode_tree_debug=True
+ )
+ try:
+ tree.setup(timeout=15)
+ except py_trees_ros.exceptions.TimedOutError as e:
+ console.logerror(console.red + "failed to setup the tree, aborting [{}]".format(str(e)) + console.reset)
+ tree.shutdown()
+ rclpy.shutdown()
+ sys.exit(1)
+ except KeyboardInterrupt:
+ # not a warning, nor error, usually a user-initiated shutdown
+ console.logerror("tree setup interrupted")
+ tree.shutdown()
+ rclpy.shutdown()
+ sys.exit(1)
+
+ tree.tick_tock(period_ms=1000.0)
+
+ try:
+ rclpy.spin(tree.node)
+ except KeyboardInterrupt:
+ pass
+
+ tree.shutdown()
+ rclpy.shutdown()
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py b/src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py
new file mode 100644
index 0000000000000000000000000000000000000000..39a899b9bfe1576c5cb8ca5052c4cd9ca01c2e5d
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py
@@ -0,0 +1,697 @@
+'''
+ Very rudimentary way of converting a .tree file to py_trees code.
+ It will not write all code for you, but it will fix the structure
+ and write all the initializations & tidy upp the adding of children to parents.
+
+ See https://github.com/jan-dolejsi/vscode-btree) for instruction on the syntax for the plugin
+
+ Creation of behaviour classes and minor code tweaking will still be needed
+
+ (<> are not included when writing, they are just used as clear dividers in this docstring)
+
+ See the example.tree file for examples of how everything should be written
+
+
+ USAGES:
+ # Disclaimer:
+ # ANY USAGE OF PY_TREES_ROS HAVE TO BE MANUALLY WRITTEN (for now)
+ # Please do not use numbers at the start of names :)
+
+ # Any <> with [Opt] inside means it is optional, though skipping it decreases the amount this script auto-completes for you
+ # Not giving a [Opt] name will cause py_trees to auto generate a name
+
+ Added syntax for .tree files to convert to py_trees code:
+ # Anything that start with <behaviour> will have it's name set as whatever is inside the behaviour clamps, unless specified otherwise
+ # i.e the behaviour "[Close Door]" will be named "Close Door" and it's variable will be "close_door"
+
+ # Any <> with [Opt] means it is optional, though it lessens the amount this script auto-completes for you
+
+ # Comments in the .tree file are allowed to be written on lines with no characters other than " " or "|" before ";;". See example.tree for clarification
+
+ ------ Composite based implementations ------
+ - Composites: <composite>;;<composite name>
+ - Optional Parallel "*" implementations:
+ - SuccessOnAll policy (default): =1;;<parallel name>
+ - SuccessOnOne policy: =2;;<parallel name>
+ - SuccessOnSelected policy: =3;;<parallel name>;;<child, child, ..., child>
+ - Idioms:
+ - <composite>;;<idiom name>;;<idiom> (Looks weird in tree preview but it was the only way to implement it)
+ ---------------------------------------------
+
+ ------ Behaviour based implementations ------
+ - Behaviour (Separate class name and variable name)
+ - <behaviour>;;<class name>
+ behaviour => variable name
+ name => class name
+ - Constant Behaviour Status:
+ - <behaviour>;;<behaviour status> (i.e. "(Constant Success);;Success" or "[Close door];;FAILURE or [Constant running];;running")
+ - Blackboard Behaviours:
+ - TODO: ADD THIS
+ - Meta:
+ <behaviour>;;Meta;;<function name [Opt]>
+ (Not giving a function name will leave it undefined and will need to be changed)
+ - Timers:
+ <behaviour>;;Timer;;<int [Opt]>
+ - Decorators:
+ Normal decorator syntax:
+ - <behaviour>;;<decorator>;;<decorator name [Opt]>
+
+ Special decorator syntax:
+ - Timeout: <behaviour>;;Timeout;;<timeout name [Opt]>;;<int [Opt]>
+
+ - Condition: <behaviour>;;Condition;;<condition name [Opt]>;;<behaviour status [Opt]>
+
+ - Inverter: !<behaviour>;;<inverter name [Opt]>
+ ---------------------------------------------
+
+ IMPLEMENTED:
+ Composites:
+ - Selector
+ - Sequence
+ - Parallel
+ - Parallel policies (policies)
+
+ Idioms:
+ - EitherOr
+ - EternalGuard
+ - OneShot
+ - PickUpWhereYouLeftOff / PUWYLO
+
+ Behaviour status:
+ - Success (constant)
+ - Running (constant)
+ - Failure (constant)
+
+ Meta:
+ - Meta (Create behaviour from function)
+
+ Timers:
+ - Timer
+
+ Decorators:
+ - Condition
+ - EternalGuard
+ - OneShot
+ - Inverter
+ - Timeout
+ - FailureIsRunning
+ - FailureIsSuccess
+ - RunningIsFailure
+ - RunningIsSuccess
+ - SuccessIsFailure
+ - SuccessIsRunning
+'''
+
+from io import TextIOWrapper
+import sys
+from typing import Iterable, List
+
+def to_variable(s: str) -> str:
+ return s.lower().strip().replace(' ', '_').replace('?','_QUESTION').replace("'", "").replace('-', '')
+
+def to_class(s: str) -> str:
+ return s.replace(' ', '').replace("'", " ").replace('?','_QUESTION').replace("'", "")
+
+def to_behaviour(s: str) -> str:
+ return s.strip().replace('!', '')[1:-1] # Remove ! from inverter and then ( ) or [ ]
+
+def get_name_from_composite(s: str) -> str:
+ return s.split(';;')[-1] # Always want the last index
+
+def get_name_from_dict_elem(d: str) -> str:
+ return get_name_from_composite(list(d.keys())[0])
+
+def dict_insert_or_append(adict: dict, key, val) -> None:
+ '''Insert a value in dict at key if one does not exist
+ Otherwise, convert value to list and append
+ '''
+ if key in adict:
+ if type(adict[key]) != list:
+ adict[key] = [adict[key]]
+ adict[key].append(val)
+ else:
+ adict[key] = val
+
+def refactor_input(file_to_read: str) -> List[dict]:
+ '''
+ Takes a file as input and returns a list of dictionaries, one dictionary for each line in the file with its contents and "level" of indentation
+ {
+ "name": Content of the line (excluding " " and "|"),
+ "value": Empty list for now, might be used later, #TODO: Decide if this is needed
+ "level": Level of indentation
+ }
+ '''
+ result = []
+ with open(file_to_read, 'r') as input:
+ for line in input:
+ curr_line = line.strip()
+ indents = 0
+ for i in range(0,len(curr_line)):
+ if curr_line[i] == '|':
+ indents += 1
+ elif curr_line[i] == ' ':
+ continue
+ elif curr_line[i] == ';': # Allows comments to be written in the .tree file
+ break
+ else:
+ result.append({'name': curr_line[i:], 'value': [], 'level': indents})
+ break
+ input.close
+ return result
+
+def convert_to_tree(ttree: dict, current_level: int =0) -> dict:
+ '''
+ Based on Tomcat's answer here https://www.py4u.net/discuss/22067
+ Returns a tree based structure of dictionaries
+ '''
+ result = {}
+ for i in range(0,len(ttree)):
+ current_dict = ttree[i]
+ try:
+ next_dict = ttree[i+1]
+ except:
+ next_dict = {'level':-1}
+
+ # Edge cases
+ if current_dict['level']>current_level:
+ continue
+ if current_dict['level']<current_level:
+ return result
+
+ # Recursion
+ # Next in line is on the same level, should add to current dict
+ if next_dict['level']==current_level:
+ dict_insert_or_append(result,current_dict['name'],current_dict['value'])
+
+ # Next in line is on a deeper level, create sub-dict and add to current dict
+ elif next_dict['level']>current_level:
+ rr = convert_to_tree(ttree[i+1:], current_level=next_dict['level'])
+ dict_insert_or_append(result,current_dict['name'],rr)
+ # Next in line is one level higher, add current dict to result and finish this recursion
+ else:
+ dict_insert_or_append(result,current_dict['name'],current_dict['value'])
+ return result
+ return result
+
+
+def write_formatted_elems_as_list(writer: TextIOWrapper, formatted_elements: Iterable) -> None:
+ '''
+ Enumerates through all elements of the iterable and writes them as a list
+ '''
+ writer.write('[')
+ for i, e in enumerate(formatted_elements):
+
+ writer.write(f'{to_variable(e)}')
+ if i+1 != len(formatted_elements):
+ writer.write(', ')
+ writer.write(']')
+
+def write_unformatted_elems_as_list(writer: TextIOWrapper, unformatted_elements: Iterable) -> None:
+ '''
+ Enumerates through all elements of the iterable, formats them and writes them as a list
+ '''
+ writer.write('[')
+ for i, e in enumerate(unformatted_elements):
+ elem = get_name_from_composite(e)
+ if elem[0] in BEHAVIOURS:
+ elem = elem[1:-1]
+ writer.write(f'{to_variable(elem)}')
+ if i+1 != len(unformatted_elements):
+ writer.write(', ')
+ writer.write(']')
+
+def write_tree(writer: TextIOWrapper, tree: list, parent: str ='') -> None:
+ '''
+ Main function performing the brute of the conversion of the .tree file
+ into py_trees code through recursion
+ '''
+
+ def write_idiom() -> None:
+ #TODO: Add functionality to specify condition behaviours
+
+ # Continue recursion so children of these idioms get initialized before they are needed in the declaration of the idiom
+ write_tree(writer, tree[elem])
+
+ if composite_extension == 'OneShot':
+ subtree_behaviour = to_variable(get_name_from_dict_elem(tree[elem]))
+ writer.write(f'{varr_name} = py_trees.idioms.oneshot(behaviour={subtree_behaviour}, name="{name}", variable_name="{varr_name}") # variable_name parameter might need to be changed\n')
+
+ elif composite_extension == 'EternalGuard':
+ writer.write(f'{varr_name} = py_trees.idioms.eternal_guard(subtree=')
+ write_unformatted_elems_as_list(writer, tree[elem])
+
+ writer.write(f', name="{name}", conditions=[ADD_CONDITION_BEHAVIOURS], blackboard_namespace=None) # Default blackboard_namespace variable\n')
+
+ elif composite_extension == 'PickUpWhereYouLeftOff' or composite_extension == 'PUWYLO':
+ writer.write(f'{varr_name} = py_trees.idioms.pick_up_where_you_left_off(behaviours=')
+ write_unformatted_elems_as_list(writer, tree[elem])
+ writer.write(', name="{name}") # Make sure behaviours are in correct order\n')
+
+ elif composite_extension == 'EitherOr':
+ writer.write(f'{varr_name} = py_trees.idioms.either_or(conditions=[ADD_CONDITION_BEHAVIOURS], subtrees=')
+ write_unformatted_elems_as_list(writer, tree[elem])
+ writer.write(f', name={varr_name}, namespace=None) # Default namespace\n')
+
+ def write_composite() -> None:
+ is_success_on_selected_policy = False
+ # Parallel
+ if c_key[0] == '=':
+ # Add different policies
+ if c_key[1] == '3': # SuccessOnSelected (Edge case)
+ # Recurse the children first since they will be needed for the 'children' parameter
+ write_tree(writer, tree[elem], varr_name)
+
+ is_success_on_selected_policy = True
+ writer.write(f'{varr_name} = py_trees.composites.Parallel(name="{name}", policy=py_trees.common.ParallelPolicy.SuccessOnSelected(children=')
+ parallel_extension = composite_extension
+ if parallel_extension is not None:
+ unformatted_children = parallel_extension.split(',')
+ formatted_children = []
+ for child in unformatted_children:
+ formatted_children.append(to_variable(child))
+ write_formatted_elems_as_list(writer, formatted_children)
+ else:
+ writer.write('[INPUT_OWN_CHILDREN_HERE]')
+
+ writer.write(', synchronize=True))\n')
+ else:
+ writer.write(f'{varr_name} = py_trees.composites.Parallel(name="{name}", ')
+ if c_key[1] == '2': # SuccessOnOne
+ writer.write('policy=py_trees.common.ParallelPolicy.SuccessOnOne)\n')
+
+ else: # SuccessOnAll (Default)
+ writer.write('policy=py_trees.common.ParallelPolicy.SuccessOnAll(synchronize=True))\n')
+ else:
+ # Selector
+ if c_key == '?':
+ writer.write(f'{varr_name} = py_trees.composites.Selector(name="{name}")\n')
+
+ # Sequence
+ elif c_key == '->':
+ writer.write(f'{varr_name} = py_trees.composites.Sequence(name="{name}")\n')
+
+ # Normally we want to continue recursion after initialization of the composite
+ if not is_success_on_selected_policy:
+ write_tree(writer, tree[elem], varr_name)
+
+ def write_behaviour() -> None:
+
+ desired_behaviour = None
+ class_name = None
+ try:
+ if key_name_list[1].lower() in BEHAVIOUR_EXTENSIONS:
+ desired_behaviour = key_name_list[1].lower().capitalize()
+
+ else:
+ # If a specific behaviour type is not specified then assume the name of the class comes after the ';;'
+ class_name = key_name_list[1]
+ except:
+ pass
+
+ varr_name = to_variable(name)
+
+ ### Extra implementations
+ if desired_behaviour is not None:
+ if desired_behaviour == 'Meta':
+ writer.write(f'{varr_name} = py_trees.meta.create_behaviour_from_function')
+ if len(key_name_list) == 3: # A desired function name has also been given
+ func_name = key_name_list[2]
+ writer.write(f'({func_name})\n')
+ else:
+ writer.write('(ADD_A_FUNCTION)\n')
+
+ elif desired_behaviour == 'Timer':
+ writer.write(f'{varr_name} = py_trees.timers.Timer(name="{name}", duration={key_name_list[2]})\n')
+
+ elif desired_behaviour == 'Subtree':
+
+ writer.write(f'{varr_name} = {varr_name.split("_subtree")[0]}({first_node})\n')
+ # Const behaviours (Success, Running, Fail)
+ else:
+ writer.write(f'{varr_name} = py_trees.behaviours.{desired_behaviour}(name="{name}")\n')
+ #TODO: Implement more kinds of behaviours
+
+ # Standard behaviour
+ else:
+ if class_name is not None:
+ behaviour = to_class(class_name)
+ else:
+ behaviour = to_class(name)
+ writer.write(f'{varr_name} = {behaviour}(name="{name}"{node})\n')
+ if parent:
+ writer.write(f'{parent}.add_child({varr_name})\n')
+
+ def write_decorator(decorator_name: str) -> None:
+
+ decorator_line = ''
+ tree_to_recurse = None
+
+ if c_key[0] == '!':
+ try:
+ decorator_name = key_name_list[1]
+ except:
+ decorator_name = None
+
+ # Remove the '!' from the name
+ tree_to_recurse = {name[1:]: tree[elem]}
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.Inverter(child={decorator_child}, name="{decorator_name}")\n'
+ else:
+ #TODO: If number of inverters cause any issues, add a counter
+ varr_name = decorator_child + '_INVERTER'
+ decorator_line = f'{varr_name} = py_trees.decorators.Inverter(child={decorator_child})\n'
+
+ elif decorator == 'Condition':
+ '''
+ 4 possible cases:
+ - <behaviour>;;Condition;;<condition name>;;<status>
+ - <behaviour>;;Condition;;<condition name>
+ - <behaviour>;;Condition;;<status>
+ - <behaviour>;;Condition
+ '''
+ # Remove the decorator from the name
+ tree_to_recurse = {name: tree[elem]}
+
+ # Check if there is a desired status
+ try:
+ status = key_name_list[3]
+ except:
+ try: # The status's already exist in BEHAVIOUR_EXTENSIONS
+ if key_name_list[2] in BEHAVIOUR_EXTENSIONS:
+ status = key_name_list[2]
+ else:
+ status = None
+ except:
+ status = None
+
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.Condition(child={decorator_child}, name="{decorator_name}"'
+ else:
+ varr_name = decorator_child + '_CONDITION'
+ decorator_line = f'{varr_name} = py_trees.decorators.Condition(child={decorator_child}'
+
+ if status is not None: # Add the desired status parameter
+ decorator_line += f', status=py_trees.common.Status.{status.upper()})\n'
+ else:
+ decorator_line += ')\n'
+
+ elif decorator == 'EternalGuard':
+ # Remove the decorator from the name
+ tree_to_recurse = {name: tree[elem]}
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.EternalGuard(child={decorator_child}, name="{decorator_name}", condition=ADD_CONDITION, blackboard_keys=[ADD_BLACKBOARD_KEYS])\n'
+ else:
+ varr_name = decorator_child + '_ETERNAL_GUARD'
+ decorator_line = f'{varr_name} = py_trees.decorators.EternalGuard(child={decorator_child}, condition=ADD_CONDITION, blackboard_keys=[ADD_BLACKBOARD_KEYS])\n'
+
+ elif decorator == 'OneShot':
+ # Remove the decorator from the name
+ tree_to_recurse = {name: tree[elem]}
+
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.OneShot(child={decorator_child}, name="{decorator_name}")\n'
+ else:
+ varr_name = decorator_child + '_ONESHOT'
+ decorator_line = f'{varr_name} = py_trees.decorator.OneShot(child={decorator_child})\n'
+
+ elif decorator == 'Timeout':
+ duration = None
+ try: # Check for the duration
+ duration = key_name_list[3]
+ except:
+ if isinstance(key_name_list[2], int):
+ duration = key_name_list[2]
+ tree_to_recurse = {name: tree[elem]}
+
+ # Write line with name if given
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.Timeout(child={decorator_child}, name="{decorator_name}"'
+ else:
+ varr_name = decorator_child + '_TIMEOUT'
+ decorator_line = f'{varr_name} = py_trees.decorator.Timeout(child={decorator_child}'
+
+ # Add duration if given
+ if decorator is not None:
+ decorator_line += f', duration={duration})\n'
+ else:
+ decorator_line += ')\n'
+ # X is Y cases
+ else:
+ # Remove the decorators from the name
+ tree_to_recurse = {name: tree[elem]}
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.{decorator}(child={decorator_child}, name="{decorator_name}")\n'
+ else:
+ varr_name = decorator_child + f'_{decorator.upper()}'
+ decorator_line = f'{varr_name} = py_trees.decorators.{decorator}(child={decorator_child})\n'
+
+ # Recurse the child first so it's initialized before parent (decorator) is written
+ write_tree(writer, tree_to_recurse)
+ writer.write(decorator_line)
+ writer.write(f'{parent}.add_child({varr_name})\n')
+
+ for elem in tree:
+ key_name_list = elem.split(';;')
+ c_key = key_name_list[0] # Guaranteed to always be one element
+
+ if c_key[0] in COMPOSITES:
+
+ try:
+ name = key_name_list[1] # If its a composite, there is a second element which is the desired name
+ except:
+ raise RuntimeError('Composite missing name: ' + key_name_list) # In case someone forgets to give a name
+ varr_name = to_variable(name)
+
+ composite_extension = None
+ try:
+ composite_extension = key_name_list[2] # If there is a third element, there is a desired idiom or parallel policy to be used
+ except:
+ # No idiom desired
+ pass
+
+ # ------ IDIOMS ------
+ # Idioms are decorators that handle entire sub-trees
+ if composite_extension in IDIOMS:
+ write_idiom()
+
+ # ------ COMPOSITES ------
+ else:
+ write_composite()
+
+ # Used to not write a parent to the root
+ if parent:
+ writer.write(f'{parent}.add_child({varr_name})\n')
+
+ else:
+ decorator = None
+ decorator_name = None
+ try: # Look if there is a decorator and a desired name for it
+ if isinstance(key_name_list[2], int): # If an int exist here, the decorator is Timeout and no name was given
+ decorator = key_name_list[1]
+ else:
+ if key_name_list[1] in DECORATORS:
+ decorator = key_name_list[1]
+ decorator_name = key_name_list[2]
+ except:
+ try: # Look if there is only a decorator
+ if key_name_list[1] in DECORATORS and key_name_list[1] not in BEHAVIOUR_EXTENSIONS:
+ decorator = key_name_list[1]
+ except: # None of the above => A behaviour was given
+ pass
+
+ # ------ BEHAVIOURS ------
+ if c_key[0] in BEHAVIOURS and decorator is None:
+ name = c_key[1:-1]
+
+ write_behaviour()
+
+ # ------ DECORATORS ------
+ # Decorators handle a SINGLE child
+ elif c_key[0] in DECORATORS or decorator in DECORATORS:
+ name = c_key
+ decorator_child = to_behaviour(to_variable(name))
+
+ write_decorator(decorator_name)
+
+ else: # Code was given something it can't handle, needs to be implemented
+ raise RuntimeError(f'Undefined behaviour for: "{elem}"')
+ writer.write("\n")
+
+
+def tidy_file(output_file_name: str, is_method: bool):
+ '''
+ Goes through the output file and reformats it to look better.
+
+ Groups the file into two sections:
+ 1. Initialization of all variables
+ 2. Structuring the tree (adds children to behaviours)
+
+ Gathers .add_child() calls to the same behaviour into one .add_children() call instead (still retains correct order of children)
+ Also adds new initialization of behaviour if the same behaviour is used several times in the tree (due to that a behaviour only can have a single parent)
+ '''
+ root = None
+ regular_lines = []
+ children_to_add = {}
+ seen = {} # K: Variable name ; V: List[Unused initialized variable (should swap between 1 and 0), Whole line]
+ special_case_parallels = {}
+ with open(output_file_name, 'r') as input:
+ first_line = True
+ for line in input:
+ if first_line: # Root should always be on the first line of the file
+ root = line.split(' = ')[0]
+ first_line = False
+
+ if 'add_child' in line:
+ s = line.split('.')
+ name = s[0]
+ rest = s[1]
+ dict_insert_or_append(children_to_add, name, rest[rest.find('(')+1:rest.find(')')]) # Adds the child within the () of the method call to the dict
+
+ else:
+ if 'SuccessOnSelected' in line:
+ children = line[line.find('[')+1: line.find(']')].split(',')
+ form_children = [child.strip() for child in children]
+ if form_children[0].isupper(): # Cheap way of checking if no children were specified
+ form_children = []
+ dict_insert_or_append(special_case_parallels, line, form_children)
+ regular_lines.append(line)
+
+ input.close
+ with open(output_file_name, 'w') as output:
+ output.write('import py_trees\n\n')
+ for line in regular_lines:
+ if line == '\n':
+ output.write(line)
+ continue
+
+ ls_rs = line.split(' = ')
+
+ name = ls_rs[0]
+ rest = ls_rs[1]
+
+ # Fill the dictionary with all variables
+ if name not in seen:
+ seen[name] = [1, line]
+ # Do not print the line yet, need to make sure it's children are initialized first
+ if line not in special_case_parallels:
+ output.write(line)
+
+ # Make sure all children for SuccessOnSelected policy is initialized before initializing the parallel
+ for special_parallel in special_case_parallels:
+ for child in special_case_parallels[special_parallel]:
+ # For each child found, remove it from the list of children for the policy
+ if child == name or child in seen:
+ special_case_parallels[special_parallel].remove(child)
+
+ if len(special_case_parallels[special_parallel]) == 0:
+ output.write(special_parallel)
+ special_case_parallels.pop(special_parallel)
+ break
+
+ for k, v in sorted(children_to_add.items()):
+
+ # Check if there are unused initialized variables, write one if not
+ # Checks if any of the elements in the dictionary match with a variable on the right side of the =
+ # If so, remove from the amount of unused variables of this kind
+
+ if isinstance(v, list):
+ for elem in v:
+ # If this crashes something is wrong with write_tree() function
+ if seen[elem][0] == 1:
+ seen[elem][0] -= 1
+ else:
+ output.write(seen[elem][1])
+
+ output.write(f'{k}.add_children([')
+ for i, elem in enumerate(v):
+ output.write(elem)
+ if i != len(v)-1:
+ output.write(', ')
+ output.write('])\n\n')
+ else:
+ # If this crashes something is wrong with write_tree() function
+ if seen[v][0] == 1:
+ seen[v][0] -= 1
+ else:
+ output.write(seen[v][1])
+
+ output.write(f'{k}.add_child({v})\n')
+
+ # Write the behaviour tree initialisation
+ if not is_method:
+ output.write(f'\n\nbehaviour_tree = py_trees.trees.BehaviourTree(root={root})\n')
+ output.write('behaviour_tree.setup(timeout=15)\n')
+ output.write('try:\n behaviour_tree.tick_tock(\n period_ms=500,\n number_of_iterations=py_trees.trees.CONTINOUS_TICK_TOCK,\n pre_tick_handler=None,\n post_tick_handler=None\n )\n')
+ output.write('except KeyboardInterrupt:\n behaviour_tree.interrupt()\n')
+ output.close
+
+def create_method() -> None:
+ method_name = file_to_read.split('/')[-1].split('.')[0]
+ root = ''
+ with open(output_file_name, 'r') as inp:
+ lines = inp.readlines()
+ inp.close()
+ with open(output_file_name, 'w') as out:
+ out.write(f'{lines[0]}\n') # "Import py_trees"
+ root = lines[2].split(" = ")[0] # Get the root for return
+ out.write(f'def {method_name}():')
+
+ for line in lines[1:]:
+ out.write(f' {line}')
+ out.write(f' return {root}')
+
+
+if __name__ == '__main__':
+
+ COMPOSITES = ['?', '-', '=']
+ IDIOMS = ['OneShot', 'EternalGuard', 'PickUpWhereYouLeftOff', 'PUWYLO', 'EitherOr']
+ BEHAVIOURS = ['[', '(']
+ BEHAVIOUR_EXTENSIONS = ['success', 'running', 'failure', 'meta', 'timer', "subtree"]
+ DECORATORS = ['!', 'Condition', 'EternalGuard', 'OneShot', 'Timeout', 'FailureIsRunning', 'FailureIsSuccess',
+ 'RunningIsFailure', 'RunningIsSuccess', 'SuccessIsFailure', 'SuccessIsRunning']
+
+ # This is used to add the 'node=self.node' parameter to behaviours
+ node = ""
+ first_node = ""
+ add_node = True
+ add_self_node = False
+ if add_node:
+ first_node = "node=node"
+ if add_self_node:
+ node = ", node=self.node"
+ else:
+ node = ", node=node"
+ if len(sys.argv) > 1:
+ file_to_read = sys.argv[1]
+ output_file_name = sys.argv[2]
+ else:
+ file_to_read = 'src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree'
+ output_file_name = 'src/lhw_intelligence/lhw_intelligence/trees/testing/output.py'
+ inp = refactor_input(file_to_read)
+ tree = convert_to_tree(inp)
+
+ with open(output_file_name, 'w') as output:
+ write_tree(output, tree)
+ output.close
+
+ # TODO: Correctly implement this
+ #ans = input('Do you wish this tree to be created as a method? (y/n)\n If no, a class will be created instead')
+
+ tidy_file(output_file_name, True)
+ create_method()
+ """
+ if ans == 'y':
+ else:
+ pass
+ """
+ print('-----------------------------------------------\n')
+ print(f' Conversion finished!\n')
+ print(f' Result can be found in:\n')
+ print(f'{output_file_name}\n')
+ print('-----------------------------------------------')
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py b/src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py
new file mode 100644
index 0000000000000000000000000000000000000000..96e28accc0f00275457e324ed53ccec2fc95f00e
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py
@@ -0,0 +1,124 @@
+
+
+#!/usr/bin/env python
+#
+# License: BSD
+# https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE
+#
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+.. argparse::
+ :module: py_trees.demos.selector
+ :func: command_line_argument_parser
+ :prog: py-trees-demo-selector
+
+.. graphviz:: dot/demo-selector.dot
+
+.. image:: images/selector.gif
+
+"""
+##############################################################################
+# Imports
+##############################################################################
+
+import argparse
+import py_trees
+import sys
+import time
+
+import py_trees.console as console
+
+##############################################################################
+# Classes
+##############################################################################
+
+
+def description():
+ content = "Higher priority switching and interruption in the children of a selector.\n"
+ content += "\n"
+ content += "In this example the higher priority child is setup to fail initially,\n"
+ content += "falling back to the continually running second child. On the third\n"
+ content += "tick, the first child succeeds and cancels the hitherto running child.\n"
+ if py_trees.console.has_colours:
+ banner_line = console.green + "*" * 79 + "\n" + console.reset
+ s = "\n"
+ s += banner_line
+ s += console.bold_white + "Selectors".center(79) + "\n" + console.reset
+ s += banner_line
+ s += "\n"
+ s += content
+ s += "\n"
+ s += banner_line
+ else:
+ s = content
+ return s
+
+
+def epilog():
+ if py_trees.console.has_colours:
+ return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset
+ else:
+ return None
+
+
+def command_line_argument_parser():
+ parser = argparse.ArgumentParser(description=description(),
+ epilog=epilog(),
+ formatter_class=argparse.RawDescriptionHelpFormatter,
+ )
+ parser.add_argument('-r', '--render', action='store_true', help='render dot tree to file')
+ return parser
+
+
+def create_root():
+ root = py_trees.composites.Selector("Sequende")
+ success_after_two = py_trees.behaviours.Count(name="founda persona",
+ fail_until=2,
+ running_until=2,
+ success_until=10)
+ always_running = py_trees.behaviours.Running(name="hola los personos")
+ root.add_children([success_after_two, always_running])
+ return root
+
+
+##############################################################################
+# Main
+##############################################################################
+
+def main():
+ """
+ Entry point for the demo script.
+ """
+ args = command_line_argument_parser().parse_args()
+ print(description())
+ py_trees.logging.level = py_trees.logging.Level.DEBUG
+
+ root = create_root()
+
+ ####################
+ # Rendering
+ ####################
+ if args.render:
+ py_trees.display.render_dot_tree(root)
+ sys.exit()
+
+ ####################
+ # Execute
+ ####################
+ root.setup_with_descendants()
+ for i in range(1, 11):
+ try:
+ print("\n--------- Tick {0} ---------\n".format(i))
+ root.tick_once()
+ print("\n")
+ print(py_trees.display.unicode_tree(root=root, show_status=True))
+ time.sleep(1.0)
+ except KeyboardInterrupt:
+ break
+ print("\n")
+
+if __name__ == "__main__":
+ main()
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/output.py b/src/lhw_intelligence/lhw_intelligence/trees/testing/output.py
new file mode 100644
index 0000000000000000000000000000000000000000..509563dabeab88bed762e8469dadd7946a197410
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/output.py
@@ -0,0 +1,61 @@
+import py_trees
+
+def receptionist_v2():
+ root = py_trees.composites.Sequence(name="Root")
+ touchhead = py_trees.composites.Selector(name="TouchHead")
+ is_head_touched = IsHeadTouched(name="Is Head Touched", node=node)
+ wait_and_blink = Waitandblink(name="Wait and blink", node=node)
+
+ findpersontointroduce = py_trees.composites.Selector(name="FindPersonToIntroduce")
+ has_person_to_introduce = haspersontointroduce(name="has person to introduce", node=node)
+ doorgreeter = py_trees.composites.Sequence(name="DoorGreeter")
+ navigate_door_subtree = navigatedoorsubtree(name="navigate door subtree", node=node)
+ doorchecker = py_trees.composites.Selector(name="DoorChecker")
+ is_door_open = isdooropen(name="is door open", node=node)
+ dooractions = py_trees.composites.Sequence(name="DoorActions")
+ say:_open_door_promt = say:open_door_promt(name="say: open_door_promt", node=node)
+ check_door = checkdoor(name="check door", node=node)
+
+
+ personchecker = py_trees.composites.Selector(name="PersonChecker")
+ found_person = foundperson(name="found person", node=node)
+ greetactions = py_trees.composites.Sequence(name="GreetActions")
+ find_person = findperson(name="find person", node=node)
+ greet_and_ask_subtree = greetandasksubtree(name="greet and ask subtree", node=node)
+
+
+
+
+ navigate:_sofa_subtree = navigate:sofasubtree(name="navigate: sofa subtree", node=node)
+ introducechecker = py_trees.composites.Selector(name="IntroduceChecker")
+ all_introduced = allintroduced(name="all introduced", node=node)
+ introduceactions = py_trees.composites.Sequence(name="IntroduceActions")
+ introduce_person_subtree = Introducepersonsubtree(name="Introduce person subtree", node=node)
+
+
+ find_empty_spot_subtree = findemptyspotsubtree(name="find empty spot subtree", node=node)
+ reset_conditions = resetconditions(name="reset conditions", node=node)
+
+
+ dooractions.add_children([say:_open_door_promt, check_door])
+
+ doorchecker.add_children([is_door_open, dooractions])
+
+ doorgreeter.add_children([navigate_door_subtree, doorchecker, personchecker])
+
+ findpersontointroduce.add_children([has_person_to_introduce, doorgreeter])
+
+ greetactions.add_children([find_person, greet_and_ask_subtree])
+
+ introduceactions.add_children([introduce_person_subtree, all_introduced])
+
+ all_introduced = allintroduced(name="all introduced", node=node)
+ introducechecker.add_children([all_introduced, introduceactions])
+
+ personchecker.add_children([found_person, greetactions])
+
+ root.add_children([touchhead, findpersontointroduce, navigate:_sofa_subtree, introducechecker, find_empty_spot_subtree, reset_conditions])
+
+ touchhead.add_children([is_head_touched, wait_and_blink])
+
+ return root
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree b/src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree
new file mode 100644
index 0000000000000000000000000000000000000000..3c467cf830f48d32559ff9a7337a83db598acb28
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree
@@ -0,0 +1,27 @@
+->;;Root
+| ?;;TouchHead
+| | (Is Head Touched)
+| | [Wait and blink]
+| ?;;FindPersonToIntroduce
+| | (has person to introduce)
+| | ->;;DoorGreeter
+| | | [navigate door subtree]
+| | | ?;;DoorChecker
+| | | | (is door open)
+| | | | ->;;DoorActions
+| | | | | [say: open_door_promt]
+| | | | | [check door]
+| | | ?;;PersonChecker
+| | | | (found person)
+| | | | ->;;GreetActions
+| | | | | [find person]
+| | | | | [greet and ask subtree]
+| [navigate: sofa subtree]
+| ?;;IntroduceChecker
+| | (all introduced)
+| | ->;;IntroduceActions
+| | | [Introduce person subtree]
+| | | (all introduced)
+| [find empty spot subtree]
+| [reset conditions]
+|
\ No newline at end of file
diff --git a/src/lhw_intelligence/setup.py b/src/lhw_intelligence/setup.py
index 7176c27b0a08fda820b958c23b46686bcd0d2c20..dfa409a078960896f2380a7ab874fdc720cee5f5 100644
--- a/src/lhw_intelligence/setup.py
+++ b/src/lhw_intelligence/setup.py
@@ -7,10 +7,12 @@ package_name = 'lhw_intelligence'
setup(
name=package_name,
version='0.0.0',
+
packages=[package_name,
package_name + ".trees.scenario_trees",
package_name + ".behaviours"
],
+
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
@@ -26,6 +28,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
+
#'ai_core = lhw_intelligence.ai_core:main',
#'find_person = lhw_intelligence.find_person:main',
#'fake_dialogflow = lhw_intelligence.fake_dialogflow:main',
@@ -35,7 +38,9 @@ setup(
#'battery_check_tree = lhw_intelligence.battery_check_tree:main',
#'tree-point-action = lhw_intelligence.tree_point_action:main',
#'tree-stage-two = lhw_intelligence.tree_stage_two:main',
- 'head_touch_tree = lhw_intelligence.trees.scenario_trees.head_touch_tree:main'
+ 'head_touch_tree = lhw_intelligence.trees.scenario_trees.head_touch_tree:main',
+ 'greet-person-tree = lhw_intelligence.trees.scenario_trees.greet_person_tree:main'
+
],
},
)