Merge pull request miccol#20 from piraka9011/master

Examples and Abstract Classes

behavior_tree_leaves/example_nodes/cpp/bt_action.h

@@ -0,0 +1,65 @@
+/*
+ * Abstract class used to create different BT actions.
+ * Usage: Pass the action's name to this class when constructing your action.
+ * Nodehandles and other variables should be part of your implementation.
+ * Maintainer: Anas Abou Allaban
+ */
+
+#ifndef BTACTION_CLASS
+#define BTACTION_CLASS
+
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <behavior_tree_core/BTAction.h>
+
+#include <functional>   // std::bind
+
+enum Status {RUNNING, SUCCESS, FAILURE};  // BT return statuses
+
+class BTAction
+{
+protected:
+    // Create action server
+    actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;
+    std::string action_name_;
+    behavior_tree_core::BTFeedback feedback_;  // Action feedback (SUCCESS, FAILURE)
+    behavior_tree_core::BTResult result_;      // Action feedback (feedback for us)
+
+public:
+    BTAction(std::string name);
+    ~BTAction(void) {}
+    virtual void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal) = 0;
+    void set_status(int status);    // Returns the status to the client (Behavior Tree)
+
+};
+
+BTAction::BTAction(std::string name):
+        as_(name, std::bind(&BTAction::execute_callback, this, _1), false),
+        action_name_(name)
+{
+    as_.start(); // Starts the action server
+}
+
+void BTAction::set_status(int status)
+{
+    // Set The feedback and result of BT.action
+    feedback_.status = status;
+    result_.status = feedback_.status;
+    as_.publishFeedback(feedback_); // Publish feedback
+    // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
+    as_.setSucceeded(result_);
+
+    switch (status)  // Print for convenience
+    {
+        case SUCCESS:
+            ROS_INFO("%s Succeeded", ros::this_node::getName().c_str() );
+            break;
+        case FAILURE:
+            ROS_INFO("%s Failed", ros::this_node::getName().c_str() );
+            break;
+        default:
+            break;
+    }
+}
+
+#endif  // BTACTION_CLASS

behavior_tree_leaves/example_nodes/cpp/class_example.cpp

@@ -0,0 +1,38 @@
+/*
+ * Example for abstract class implementation
+ */
+
+#include <ros/ros.h>
+#include <bt_action.h>
+
+
+class StartCondition: public BTAction
+{
+public:
+    bool start_condition = false;
+    ros::NodeHandle nh_;
+
+    StartCondition(ros::NodeHandle* n) :
+            BTAction("StartCondition"),
+            nh_(*n)
+    {
+        nh_.param("/start_condition", start_condition, false);
+    }
+
+    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
+    {
+        if (start_condition)
+            set_status(SUCCESS);
+        else
+            set_status(FAILURE);
+    }
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "start_conditions");
+    ros::NodeHandle nh;
+    StartCondition sc(&nh);
+    ros::spin();
+    return 0;
+}

behavior_tree_leaves/example_nodes/python/bt_action.py

@@ -0,0 +1,37 @@
+#! /usr/bin/env python
+# Author: Anas Abou Allaban
+
+import rospy
+import actionlib
+import behavior_tree_core.msg
+from abc import ABCMeta, abstractmethod
+
+
+class BTAction:
+    __metaclass__ = ABCMeta
+    _feedback = behavior_tree_core.msg.BTFeedback()
+    _result = behavior_tree_core.msg.BTResult()
+
+    def __init__(self, name):
+        # Behavior tree action server
+        self._action_name = name
+        self._as = actionlib.SimpleActionServer(self._action_name, behavior_tree_core.msg.BTAction,
+                                                execute_cb=self.execute_cb, auto_start=False)
+        self._as.start()
+        rospy.loginfo("{} Server Started".format(self._action_name))
+
+    @abstractmethod
+    def execute_cb(self, goal):
+        raise NotImplementedError()
+
+    def set_status(self, status):
+        if status:
+            self._feedback.status = 1
+            self._result.status = self._feedback.status
+            rospy.loginfo('Action %s: Succeeded' % self._action_name)
+            self._as.set_succeeded(self._result)
+        else:
+            self._feedback.status = 2
+            self._result.status = self._feedback.status
+            rospy.loginfo('Action %s: Failed' % self._action_name)
+            self._as.set_succeeded(self._result)

behavior_tree_leaves/example_nodes/python/class_example.py

@@ -0,0 +1,25 @@
+#! /usr/bin/env python
+# Author: Anas Abou Allaban
+
+import rospy
+from bt_action import BTAction
+
+
+class Start(BTAction):
+
+    def __init__(self):
+        self.condition = true
+        # Behavior tree action server
+        super(Start, self).__init__('Start')
+
+    def execute_cb(self, goal):
+        while not rospy.is_shutdown():
+            if self._as.is_preempt_requested():
+                rospy.loginfo("{} halted".format(self._action_name))
+                self._as.set_preempted()
+                self.set_status(False)
+                break
+            if condition:
+                self.set_status(True)
+            else:
+                self.set_status(False)