From 1f7d552c47108fce7296ba0cb9901c7f3a0cc990 Mon Sep 17 00:00:00 2001
From: Mohamed Fazil <mfazil@hello-robot.com>
Date: Thu, 16 Feb 2023 21:24:11 -0800
Subject: [PATCH] Py Trees test node first pass

---
 stretch_demos/launch/bt_test.launch | 47 +++++++++++++++++
 stretch_demos/nodes/stretch_bt_test | 78 +++++++++++++++++++++++++++++
 2 files changed, 125 insertions(+)
 create mode 100644 stretch_demos/launch/bt_test.launch
 create mode 100755 stretch_demos/nodes/stretch_bt_test

diff --git a/stretch_demos/launch/bt_test.launch b/stretch_demos/launch/bt_test.launch
new file mode 100644
index 0000000..b69397f
--- /dev/null
+++ b/stretch_demos/launch/bt_test.launch
@@ -0,0 +1,47 @@
+<launch>
+
+  <node pkg="rqt_py_trees" name="rqt_py_trees" type="rqt_py_trees"/>
+
+  <arg name="rviz" default="false" doc="whether to show Rviz" />
+  <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>
+
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_core)/launch/d435i_high_resolution.launch" />
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen" />
+  <!-- -->
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+  <!-- -->
+
+  <!-- MAPPING -->
+  <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen" >
+    <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
+  </node>
+  <!-- -->
+
+  <!-- LASER RANGE FINDER -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" />
+  <!-- -->
+
+  <!-- LASER SCAN MATCHER FOR ODOMETRY -->
+  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
+  <!-- -->
+
+  <!-- DOCK ROBOT -->
+  <node name="dock_robot" pkg="stretch_demos" type="dock_robot" output="screen">
+    <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
+  </node>
+  <!-- -->
+
+<!--  &lt;!&ndash; KEYBOARD TELEOP &ndash;&gt;-->
+<!--  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='&#45;&#45;mapping_on &#45;&#45;docking_on'/>-->
+<!--  &lt;!&ndash; &ndash;&gt;-->
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_demos)/rviz/dock_robot.rviz" if="$(arg rviz)" />
+  <!-- -->
+
+</launch>
diff --git a/stretch_demos/nodes/stretch_bt_test b/stretch_demos/nodes/stretch_bt_test
new file mode 100755
index 0000000..43ef42c
--- /dev/null
+++ b/stretch_demos/nodes/stretch_bt_test
@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+
+import py_trees
+import py_trees_ros
+import py_trees.console as console
+import rospy
+import sys
+from std_srvs.srv import Trigger, TriggerRequest
+import functools
+
+
+class trigger_service_behaviour(py_trees.behaviour.Behaviour):
+    def __init__(self, name, service_name):
+        super(trigger_service_behaviour, self).__init__(name=name)
+        self.service_name = service_name
+        self.service = rospy.ServiceProxy(service_name, Trigger)
+        self.feedback_message = f"Initializing {self.__class__.__name__}.{service_name} Trigger."
+        print(f"{self.feedback_message}")
+
+    def setup(self, timeout):
+        rospy.wait_for_service(self.service_name, timeout=timeout)
+        self.feedback_message = f"Connected with {self.service_name}"
+        return True
+
+    def update(self):
+        self.logger.debug("%s.update()" % self.__class__.__name__)
+        self.feedback_message = f"{self.__class__.__name__}.{self.service_name} : FAILURE"
+        try:
+            req = TriggerRequest()
+            result = self.service(req)
+            if result.success == True:
+                self.feedback_message = f"{self.__class__.__name__}.{self.service_name} : SUCESS"
+                print(self.feedback_message)
+                return py_trees.common.Status.SUCCESS
+            else:
+                print(self.feedback_message)
+                return py_trees.common.Status.FAILURE
+        except Exception as e:
+            rospy.logerr(e)
+            print(self.feedback_message)
+            return py_trees.common.Status.FAILURE
+
+
+def create_root():
+    # behaviours
+    root = py_trees.composites.Sequence("test")
+    stow_robot = trigger_service_behaviour(name='RobotStow', service_name='/stow_the_robot')
+
+    # tree
+    root.add_children([stow_robot])
+    return root
+
+
+def shutdown(behaviour_tree):
+    behaviour_tree.interrupt()
+
+
+##############################################################################
+# Main
+##############################################################################
+
+
+def main():
+    """
+    Entry point for the demo script.
+    """
+    rospy.init_node("tree")
+    root = create_root()
+    behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
+    rospy.on_shutdown(functools.partial(shutdown, behaviour_tree))
+    if not behaviour_tree.setup(timeout=15):
+        console.logerror("failed to setup the tree, aborting.")
+        sys.exit(1)
+    behaviour_tree.tick_tock(500)
+
+
+if __name__ == '__main__':
+    main()