back compatible API change to improve the wrapping of legacy code (issue #15)

Eventually, SimpleAction, SimpleDecorators and SimpleCondition can use
blackboard and NodeParameters too.

Author: facontidavide

examples/CMakeLists.txt

@@ -25,3 +25,6 @@ target_link_libraries(t04_blackboard  movebase_node ${BEHAVIOR_TREE_LIBRARY}  )
 
 add_executable(t05_crossdoor         t05_crossdoor.cpp )
 target_link_libraries(t05_crossdoor  crossdoor_nodes ${BEHAVIOR_TREE_LIBRARY}  )
+
+add_executable(t06_wrap_legacy         t06_wrap_legacy.cpp )
+target_link_libraries(t06_wrap_legacy  crossdoor_nodes ${BEHAVIOR_TREE_LIBRARY}  )

examples/t06_wrap_legacy.cpp

@@ -0,0 +1,113 @@
+#include "behaviortree_cpp/xml_parsing.h"
+#include "behaviortree_cpp/loggers/bt_cout_logger.h"
+#include "behaviortree_cpp/blackboard/blackboard_local.h"
+
+
+/** In this tutorial we will see how to wrap legacy code into a
+ * BehaviorTree in a non-intrusive way, i.e. without modifying the
+ * original class
+*/
+
+// clang-format off
+const std::string xml_text = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <SequenceStar name="root">
+            <MoveTo  goal="-1;3;0.5" />
+            <MoveTo  goal="${myGoal}" />
+        </SequenceStar>
+     </BehaviorTree>
+ </root>
+ )";
+
+// clang-format on
+
+// This is my custom type.
+// By default, we don't know how to read this from a NodeParameter.
+struct Point3D { double x,y,z; };
+
+// We want to create an ActionNode that calls the method MyLegacyMoveTo::go
+class MyLegacyMoveTo
+{
+public:
+    bool go(Point3D goal)
+    {
+        printf("Going to: %f %f %f\n", goal.x, goal.y, goal.z);
+        return true; // true means success in my legacy code
+    }
+};
+
+// Similarly to the previous tutorials, we need to implement this parsing method,
+// deplaring a specialization of BT::convertFromString
+namespace BT
+{
+template <> Point3D convertFromString(const StringView& key)
+{
+    // three real numbers separated by semicolons
+    auto parts = BT::splitString(key, ';');
+    if (parts.size() != 3)
+    {
+        throw std::runtime_error("invalid input)");
+    }
+    else
+    {
+        Point3D output;
+        output.x  = convertFromString<double>(parts[0]);
+        output.y  = convertFromString<double>(parts[1]);
+        output.z  = convertFromString<double>(parts[2]);
+        return output;
+    }
+}
+}
+
+
+int main()
+{
+    using namespace BT;
+
+    MyLegacyMoveTo move_to;
+
+    // Here we use a lambda that captures the reference of move_to
+    auto MoveToWrapperWithLambda = [&move_to](TreeNode& parent_node) -> NodeStatus
+    {
+        Point3D goal;
+        // thanks to paren_node, you can access easily the NodeParameters and the blackboard
+        parent_node.getParam("goal", goal);
+
+        // you can write and read the blabkcoard if you like
+        //parent_node.blackboard() ....
+
+        bool res = move_to.go( goal );
+        // convert bool to NodeStatus
+        return res ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+    };
+
+    BehaviorTreeFactory factory;
+    // Regoister the lambda with BehaviorTreeFactory::registerSimpleAction
+    factory.registerSimpleAction("MoveTo", MoveToWrapperWithLambda);
+
+    auto blackboard = Blackboard::create<BlackboardLocal>();
+    auto tree = buildTreeFromText(factory, xml_text, blackboard);
+
+    // We set the entry "myGoal" in the blackboard.
+    Point3D my_goal = {3,4,5};
+    blackboard->set("myGoal", my_goal);
+
+    NodeStatus status = NodeStatus::RUNNING;
+    while (status == NodeStatus::RUNNING)
+    {
+        status = tree.root_node->executeTick();
+    }
+    return 0;
+}
+
+/* Expected output:
+
+Going to: -1.000000 3.000000 0.500000
+Going to: 3.000000 4.000000 5.000000
+
+The first MoveTo read the parameter from the string "-1;3;0.5"
+whilst the second from the blackboard, that contains a copy of the Point3D my_goal.
+
+*/

include/behaviortree_cpp/action_node.h

@@ -52,7 +52,8 @@ class SimpleActionNode : public ActionNodeBase
     typedef std::function<NodeStatus(TreeNode&)> TickFunctor;
 
     // Constructor: you must provide the function to call when tick() is invoked
-    SimpleActionNode(const std::string& name, TickFunctor tick_functor);
+    SimpleActionNode(const std::string& name, TickFunctor tick_functor,
+                     const NodeParameters &params = NodeParameters());
 
     ~SimpleActionNode() override = default;
 

include/behaviortree_cpp/condition_node.h

@@ -51,7 +51,8 @@ class SimpleConditionNode : public ConditionNode
     typedef std::function<NodeStatus(TreeNode&)> TickFunctor;
 
     // Constructor: you must provide the function to call when tick() is invoked
-    SimpleConditionNode(const std::string& name, TickFunctor tick_functor);
+    SimpleConditionNode(const std::string& name, TickFunctor tick_functor,
+                        const NodeParameters& params = NodeParameters());
 
     ~SimpleConditionNode() override = default;
 

include/behaviortree_cpp/decorator_node.h

@@ -50,7 +50,8 @@ class SimpleDecoratorNode : public DecoratorNode
     typedef std::function<NodeStatus(NodeStatus, TreeNode&)> TickFunctor;
 
     // Constructor: you must provide the function to call when tick() is invoked
-    SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor);
+    SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor,
+                        const NodeParameters& params = NodeParameters());
 
     ~SimpleDecoratorNode() override = default;
 

include/behaviortree_cpp/tree_node.h

@@ -103,10 +103,6 @@ class TreeNode
     /// creation of the TreeNode instance.
     const NodeParameters& initializationParameters() const;
 
-  protected:
-    /// Method to be implemented by the user
-    virtual BT::NodeStatus tick() = 0;
-
     /** Get a parameter from the passed NodeParameters and convert it to type T.
      */
     template <typename T>
@@ -146,8 +142,7 @@ class TreeNode
                 bool found = bb_->get(stripped_key, destination);
                 return found;
             }
-            else
-            {
+            else{
                 destination = convertFromString<T>(str.c_str());
                 return true;
             }
@@ -159,6 +154,10 @@ class TreeNode
         }
     }
 
+  protected:
+    /// Method to be implemented by the user
+    virtual BT::NodeStatus tick() = 0;
+
     /// registrationName() is set by the BehaviorTreeFactory
     void setRegistrationName(const std::string& registration_name);
 

src/action_node.cpp

@@ -34,8 +34,9 @@ NodeStatus ActionNodeBase::executeTick()
 //-------------------------------------------------------
 
 SimpleActionNode::SimpleActionNode(const std::string& name,
-                                   SimpleActionNode::TickFunctor tick_functor)
-  : ActionNodeBase(name, NodeParameters()), tick_functor_(std::move(tick_functor))
+                                   SimpleActionNode::TickFunctor tick_functor,
+                                   const NodeParameters& params)
+  : ActionNodeBase(name, params), tick_functor_(std::move(tick_functor))
 {
 }
 

src/bt_factory.cpp

@@ -66,8 +66,8 @@ void BehaviorTreeFactory::registerBuilder(const std::string& ID, NodeBuilder bui
 void BehaviorTreeFactory::registerSimpleCondition(
     const std::string& ID, const SimpleConditionNode::TickFunctor& tick_functor)
 {
-    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeParameters&) {
-        return std::unique_ptr<TreeNode>(new SimpleConditionNode(name, tick_functor));
+    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeParameters& params) {
+        return std::unique_ptr<TreeNode>(new SimpleConditionNode(name, tick_functor, params));
     };
 
     registerBuilder(ID, builder);
@@ -77,8 +77,8 @@ void BehaviorTreeFactory::registerSimpleCondition(
 void BehaviorTreeFactory::registerSimpleAction(const std::string& ID,
                                                const SimpleActionNode::TickFunctor& tick_functor)
 {
-    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeParameters&) {
-        return std::unique_ptr<TreeNode>(new SimpleActionNode(name, tick_functor));
+    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeParameters& params) {
+        return std::unique_ptr<TreeNode>(new SimpleActionNode(name, tick_functor, params));
     };
 
     registerBuilder(ID, builder);
@@ -88,8 +88,8 @@ void BehaviorTreeFactory::registerSimpleAction(const std::string& ID,
 void BehaviorTreeFactory::registerSimpleDecorator(
     const std::string& ID, const SimpleDecoratorNode::TickFunctor& tick_functor)
 {
-    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeParameters&) {
-        return std::unique_ptr<TreeNode>(new SimpleDecoratorNode(name, tick_functor));
+    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeParameters& params) {
+        return std::unique_ptr<TreeNode>(new SimpleDecoratorNode(name, tick_functor, params));
     };
 
     registerBuilder(ID, builder);

src/condition_node.cpp

@@ -24,8 +24,9 @@ void ConditionNode::halt()
 {
 }
 
-SimpleConditionNode::SimpleConditionNode(const std::string& name, TickFunctor tick_functor)
-  : ConditionNode(name, NodeParameters()), tick_functor_(std::move(tick_functor))
+SimpleConditionNode::SimpleConditionNode(const std::string& name, TickFunctor tick_functor,
+                                         const NodeParameters &params)
+  : ConditionNode(name, params), tick_functor_(std::move(tick_functor))
 {
 }
 

src/decorator_node.cpp

@@ -56,8 +56,9 @@ void DecoratorNode::haltChild()
     }
 }
 
-SimpleDecoratorNode::SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor)
-  : DecoratorNode(name, NodeParameters()), tick_functor_(std::move(tick_functor))
+SimpleDecoratorNode::SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor,
+                                         const NodeParameters &params)
+  : DecoratorNode(name, params), tick_functor_(std::move(tick_functor))
 {
 }