Some refactoring of names

Added new nodes

src/btree_nodes/CMakeLists.txt

@@ -35,11 +35,14 @@ set(CPP_FILES
         src/Extensions.cpp
         src/nodes/WeightedRandomNode.cpp
         src/nodes/AmICalled.cpp
-        src/nodes/GenerateTarget2D.cpp
+        src/nodes/GenerateXYPose.cpp
         src/nodes/MoveActorToTarget.cpp
         src/helpers/MinimalSubscriber.cpp
         src/nodes/RobotMove.cpp
         src/nodes/MoveConnection.cpp
+        src/nodes/OffsetPose.cpp
+        src/nodes/RandomFailure.cpp
+        src/nodes/IsInArea.cpp
         )
 
 add_executable(tree ${CPP_FILES})

src/btree_nodes/actorTree.xml

@@ -6,9 +6,9 @@
             <Action ID="AmICalled" called="{called}"/>
             <Sequence>
                 <Control ID="WeightedRandom" weights="10;2;1">
-                    <Action ID="GenerateSafeTarget" target="{target}"/>
+                    <Action ID="GenerateXYPose" area="{safeArea}" pose="{target}"/>
-                    <Action ID="GenerateWarningTarget" target="{target}"/>
+                    <Action ID="GenerateXYPose" area="{warningArea}" pose="{target}"/>
-                    <Action ID="GenerateUnsafeTarget" target="{target}"/>
+                    <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{target}"/>
                 </Control>
                 <Action ID="MoveActorToTarget" current="{current}" target="{target}"/>
             </Sequence>
@@ -19,20 +19,31 @@
         <Action ID="AmICalled">
             <input_port name="called">called status</input_port>
         </Action>
-        <Action ID="GenerateSafeTarget">
+        <Action ID="GenerateXYPose">
-            <output_port name="target">Generated pose in safe area</output_port>
+            <input_port name="area">Area to generate pose in</input_port>
+            <output_port name="pose">Generated pose in area</output_port>
         </Action>
-        <Action ID="GenerateUnsafeTarget">
+        <Condition ID="InAreaTest">
-            <output_port name="target">Generated pose in unsafe area</output_port>
+            <input_port name="bounds">Bounds to check in</input_port>
-        </Action>
+            <input_port name="position">Position of object</input_port>
-        <Action ID="GenerateWarningTarget">
+            <output_port name="result">Result of check if position is within bounds</output_port>
-            <output_port name="target">Generated pose in warning area</output_port>
+        </Condition>
-        </Action>
-        <Condition ID="InTest"/>
         <Action ID="MoveActorToTarget">
             <input_port name="current">Current actor position</input_port>
             <input_port name="target">Target to move actor to</input_port>
         </Action>
+        <Action ID="PositionToPose">
+            <input_port name="offset">offset as a Point</input_port>
+            <input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
+            <input_port name="output">generated new pose</input_port>
+            <input_port name="position">initial position as Position2D</input_port>
+        </Action>
+        <Action ID="RandomFailure">
+            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+        </Action>
+        <Action ID="RobotMove">
+            <input_port name="target">Target pose of robot.</input_port>
+        </Action>
         <Control ID="WeightedRandom">
             <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
         </Control>

src/btree_nodes/nodes.xml

@@ -11,14 +11,9 @@
             <input_port name="called">called status</input_port>
         </Action>
 
-        <Action ID="GenerateSafeTarget">
+        <Action ID="GenerateXYPose">
-            <output_port name="target">Generated pose in safe area</output_port>
+            <input_port name="area">Area to generate pose in</input_port>
-        </Action>
+            <output_port name="pose">Generated pose in area</output_port>
-        <Action ID="GenerateWarningTarget">
-            <output_port name="target">Generated pose in warning area</output_port>
-        </Action>
-        <Action ID="GenerateUnsafeTarget">
-            <output_port name="target">Generated pose in unsafe area</output_port>
         </Action>
 
         <Action ID="MoveActorToTarget">
@@ -39,5 +34,16 @@
         <Action ID="RobotMove">
             <input_port name="target">Target pose of robot.</input_port>
         </Action>
+
+        <Action ID="PositionToPose">
+            <input_port name="position">initial position as Position2D</input_port>
+            <input_port name="offset">offset as a Point</input_port>
+            <input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
+            <input_port name="output">generated new pose</input_port>
+        </Action>
+
+        <Action ID="RandomFailure">
+            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+        </Action>
     </TreeNodesModel>
 </root>

src/btree_nodes/robotTree.xml

@@ -3,8 +3,11 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
-            <Action ID="RobotMove" target="0.2|0.2|1.1"/>
             <Action ID="RobotMove" target="-0.2|-0.2|1.1"/>
+            <ReactiveSequence>
+                <Action ID="RandomFailure" failureChance="0.2"/>
+                <Action ID="RobotMove" target="0.2|0.2|1.1"/>
+            </ReactiveSequence>
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
@@ -30,6 +33,15 @@
             <input_port name="current">Current actor position</input_port>
             <input_port name="target">Target to move actor to</input_port>
         </Action>
+        <Action ID="PositionToPose">
+            <input_port name="offset">offset as a Point</input_port>
+            <input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
+            <input_port name="output">generated new pose</input_port>
+            <input_port name="position">initial position as Position2D</input_port>
+        </Action>
+        <Action ID="RandomFailure">
+            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+        </Action>
         <Action ID="RobotMove">
             <input_port name="target">Target pose of robot.</input_port>
         </Action>

src/btree_nodes/src/Extensions.cpp

@@ -62,4 +62,35 @@ namespace BT {
 
         return pose;
     }
+
+    template<>
+    std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
+        auto parts = splitString(str, '|');
+        if (parts.size() != 3) {
+            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
+        }
+
+        auto point = std::make_shared<geometry_msgs::msg::Point>();
+        point->x= convertFromString<double>(parts[0]);
+        point->y= convertFromString<double>(parts[1]);
+        point->z= convertFromString<double>(parts[2]);
+
+        return point;
+    }
+
+    template<>
+    std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
+        auto parts = splitString(str, '|');
+        if (parts.size() != 4) {
+            throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>|<x>|<y>|<z>'.");
+        }
+
+        auto point = std::make_shared<geometry_msgs::msg::Quaternion>();
+        point->w= convertFromString<double>(parts[0]);
+        point->x= convertFromString<double>(parts[1]);
+        point->y= convertFromString<double>(parts[2]);
+        point->z= convertFromString<double>(parts[3]);
+
+        return point;
+    }
 }

src/btree_nodes/src/Tree.cpp

@@ -4,10 +4,13 @@
 #include <std_msgs/msg/bool.hpp>
 #include "nodes/WeightedRandomNode.h"
 #include "nodes/AmICalled.h"
-#include "nodes/GenerateTarget2D.h"
+#include "nodes/GenerateXYPose.h"
 #include "nodes/MoveActorToTarget.h"
 #include "nodes/RobotMove.h"
 #include "helpers/MinimalSubscriber.h"
+#include "nodes/OffsetPose.h"
+#include "nodes/RandomFailure.h"
+#include "nodes/IsInArea.h"
 #include <chrono>
 #include <thread>
 
@@ -26,86 +29,119 @@ int main(int argc, char **argv) {
     std::thread([&executor]() { executor.spin(); }).detach();
 
     BehaviorTreeFactory factory;
-/*
+
+    const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
+    safeArea->triangles = {std::vector<Position2D>({
+            {-1, 3.5},
+            {-1, 7},
+            {3,  3.5},
+            {7,  7},
+            {3,  -1},
+            {7,  -1}
+    })};
+
+    const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
+    warningArea->triangles = {std::vector<Position2D>({
+            {-1, 1.5},
+            {-1, 2.5},
+            {1,  1.5},
+            {2,  2.5},
+            {1,  -1},
+            {2,  -1}
+    })};
+
+    const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
+    unsafeArea->triangles = std::vector<Position2D>({
+            {-1, 2.5},
+            {-1, 3.5},
+            {2,  2.5},
+            {3,  3.5},
+            {2,  -1},
+            {3,  -1}
+    });
+
+    const std::shared_ptr<Area> negativeYTable = std::make_shared<Area>();
+    negativeYTable->triangles = std::vector<Position2D>({
+            {0.3,-0.25},
+            {-0.3,-0.25},
+            {0,-0.4}
+    });
+
+    const std::shared_ptr<Area> positiveYTable = std::make_shared<Area>();
+    positiveYTable->triangles = std::vector<Position2D>({
+            {0.3,0.25},
+            {-0.3,0.25},
+            {0,0.4}
+    });
+
     std::cout << "Registering nodes." << std::endl;
-    NodeBuilder builderGenerateSafeTarget = [](const std::string &name, const NodeConfiguration &config) {
-        const Area area = {std::vector<Position2D>({{1, 1},
-                                                        {5, 1},
-                                                        {5, 5},
-                                                        {1, 5}})};
-        return std::make_unique<GenerateTarget2D>(name, config, area);
-    };
-    NodeBuilder builderGenerateWarningTarget = [](const std::string &name, const NodeConfiguration &config) {
-        const Area area = {std::vector<Position2D>({{1, 1},
-                                                        {2, 2},
-                                                        {1, 2}})};
-        return std::make_unique<GenerateTarget2D>(name, config, area);
-    };
-    NodeBuilder builderGenerateUnsafeTarget = [](const std::string &name, const NodeConfiguration &config) {
-        const Area area = {std::vector<Position2D>({{1, 1},
-                                                        {2, 2},
-                                                        {1, 2}})};
-        return std::make_unique<GenerateTarget2D>(name, config, area);
-    };
-
-    factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
-    factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
-    factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
 
+    factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
     factory.registerNodeType<AmICalled>("AmICalled");
 
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
-     */
+    factory.registerNodeType<OffsetPose>("OffsetPose");
+    factory.registerNodeType<RandomFailure>("RandomFailure");
+    factory.registerNodeType<IsInArea>("IsInArea");
 
-    auto connection = std::make_shared<MoveConnection>(mainNode,"iisy");
+    auto connection = std::make_shared<MoveConnection>(mainNode, "iisy");
 
     NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<RobotMove>(name, config, &connection);
     };
 
-    factory.registerBuilder<RobotMove>("RobotMove",builderIisyMove);
+    factory.registerBuilder<RobotMove>("RobotMove", builderIisyMove);
 
     std::cout << "Creating tree." << std::endl;
 
-    //Tree actorTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/actorTree.xml");
+    Tree actorTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/actorTree.xml");
     Tree robotTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/robotTree.xml");
 
-    /*
+    auto trees = {actorTree.rootBlackboard(),robotTree.rootBlackboard()};
+    for (const auto &item : trees){
+        item->set("safeArea",safeArea);
+        item->set("warningArea",warningArea);
+        item->set("unsafeArea",unsafeArea);
+        item->set("negativeYTable",negativeYTable);
+        item->set("positiveYTable",positiveYTable);
+    }
+
     std::mutex mutex;
 
     std::cout << "Starting subscriber." << std::endl;
 
-    geometry_msgs::msg::Pose init;
+    auto init = std::make_shared<geometry_msgs::msg::Pose>();
-    actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
+    actorTree.rootBlackboard()->set("current", init);
-    actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
+    actorTree.rootBlackboard()->set("target", init);
-    actorTree.rootBlackboard()->set<bool>("called",false);
+    actorTree.rootBlackboard()->set<bool>("called", false);
 
     MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
-                                                    [&actorTree, &mutex](geometry_msgs::msg::Pose pose) {
+            [&actorTree, &mutex](geometry_msgs::msg::Pose pose) {
-                                                        mutex.lock();
+                auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose);
-                                                        actorTree.rootBlackboard()->set("current", pose);
+                mutex.lock();
-                                                        mutex.unlock();
+                actorTree.rootBlackboard()->set("current", sharedPose);
-                                                    }).detach();
+                mutex.unlock();
+            }).detach();
 
     MinimalSubscriber<std_msgs::msg::Bool>::createAsThread("tree_get_called", "called",
-                                                    [&actorTree, &mutex](std_msgs::msg::Bool called) {
+            [&actorTree, &mutex](std_msgs::msg::Bool called) {
-                                                        mutex.lock();
+                mutex.lock();
-                                                        actorTree.rootBlackboard()->set("called", (bool) called.data);
+                actorTree.rootBlackboard()->set("called",
-                                                        mutex.unlock();
+                        (bool) called.data);
-                                                    }).detach();*/
+                mutex.unlock();
-
+            }).detach();
 
 
     std::cout << "Looping." << std::endl;
 
-    /*std::thread actor([&actorTree, &mutex](){
+    std::thread actor([&actorTree, &mutex]() {
         while (rclcpp::ok()) {
             mutex.lock();
             actorTree.tickRoot();
             mutex.unlock();
         }
-    //});*/
+    });
 
     while (rclcpp::ok()) {
         robotTree.tickRoot();

src/btree_nodes/src/nodes/GenerateTarget2D.cpp

@@ -1,88 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#include "GenerateTarget2D.h"
-
-double BT::GenerateTarget2D::getArea(unsigned long id) {
-    Position2D a = area.triangles[id];
-    Position2D b = area.triangles[id + 1];
-    Position2D c = area.triangles[id + 2];
-
-    return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y));
-}
-
-double BT::GenerateTarget2D::getRandom() {
-    std::uniform_real_distribution<double> zeroToOne(0, 1);
-    return zeroToOne(rng);
-}
-
-Position2D BT::GenerateTarget2D::getRandomPosInTriangle(unsigned long id) {
-    Position2D a = area.triangles[id];
-    Position2D b = area.triangles[id + 1];
-    Position2D c = area.triangles[id + 2];
-
-    Position2D ab = {b.x - a.x, b.y - a.y};
-    Position2D ac = {c.x - a.x, c.y - a.y};
-
-    double u1 = getRandom();
-    double u2 = getRandom();
-
-    if (u1 + u2 > 1) {
-        u1 = 1 - u1;
-        u2 = 1 - u2;
-    }
-
-    return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
-}
-
-BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area& area) :
-        SyncActionNode(name, config) {
-    this->area = area;
-    unsigned long triangleCount = area.triangles.size() - 2;
-
-    uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
-    std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)};
-    rng.seed(ss);
-
-    std::vector<double> prob(triangleCount);
-    double sum = 0;
-    for (unsigned long i = 0; i < triangleCount; ++i) {
-        double currentArea = getArea(i);
-        prob[i] = currentArea;
-        sum += currentArea;
-    }
-    for (unsigned long i = 0; i < triangleCount; ++i) {
-        prob[i] = prob[i] / sum;
-    }
-    sum = 0;
-    for (unsigned long i = 0; i < triangleCount; ++i) {
-        prob[i] = prob[i] + sum;
-        sum += prob[i];
-    }
-
-    this->weights = prob;
-}
-
-BT::PortsList BT::GenerateTarget2D::providedPorts() {
-    // Optionally, a port can have a human readable description
-    const char *description = "Generates a random position in an area defined by a triangle strip";
-    return {OutputPort<geometry_msgs::msg::Pose>("target", description)};
-}
-
-BT::NodeStatus BT::GenerateTarget2D::tick() {
-    double triangleRandom = getRandom();
-    unsigned long i = 0;
-    while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
-        i++;
-    }
-    auto pos = getRandomPosInTriangle(i);
-
-    geometry_msgs::msg::Pose randomPose;
-    randomPose.position.x=pos.x;
-    randomPose.position.y=pos.y;
-
-    printf("Generated Target: %f %f\n",pos.x,pos.y);
-    setOutput<geometry_msgs::msg::Pose>("target", randomPose);
-    return NodeStatus::SUCCESS;
-}

src/btree_nodes/src/nodes/GenerateTarget2D.h

@@ -1,40 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#ifndef BUILD_GENERATETARGET2D_H
-#define BUILD_GENERATETARGET2D_H
-
-#include <behaviortree_cpp_v3/action_node.h>
-#include "../Area.h"
-#include <random>
-#include <geometry_msgs/msg/pose.hpp>
-
-namespace BT{
-
-// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip.
-class GenerateTarget2D : public SyncActionNode {
-    Area area;
-    std::vector<double> weights;
-    std::mt19937_64 rng;
-
-private:
-    double getArea(unsigned long id);
-
-private:
-    double getRandom();
-
-private:
-    Position2D getRandomPosInTriangle(unsigned long id);
-
-public:
-    GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area& area);
-
-    static PortsList providedPorts();
-
-
-    NodeStatus tick() override;
-};
-}
-
-#endif //BUILD_GENERATETARGET2D_H

src/btree_nodes/src/nodes/GenerateXYPose.cpp

@@ -0,0 +1,84 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#include "GenerateXYPose.h"
+
+double BT::GenerateXYPose::getArea(const std::shared_ptr<Area> &area, unsigned long id) {
+    Position2D a = area->triangles[id];
+    Position2D b = area->triangles[id + 1];
+    Position2D c = area->triangles[id + 2];
+
+    return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y));
+}
+
+Position2D BT::GenerateXYPose::getRandomPosInTriangle(const std::shared_ptr<Area> &area, unsigned long id) {
+    Position2D a = area->triangles[id];
+    Position2D b = area->triangles[id + 1];
+    Position2D c = area->triangles[id + 2];
+
+    Position2D ab = {b.x - a.x, b.y - a.y};
+    Position2D ac = {c.x - a.x, c.y - a.y};
+
+    double u1 = dist(re);
+    double u2 = dist(re);
+
+    if (u1 + u2 > 1) {
+        u1 = 1 - u1;
+        u2 = 1 - u2;
+    }
+
+    return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
+}
+
+BT::GenerateXYPose::GenerateXYPose(const std::string &name, const BT::NodeConfiguration &config) :
+        SyncActionNode(name, config) {
+}
+
+BT::PortsList BT::GenerateXYPose::providedPorts() {
+    return {
+            InputPort<std::shared_ptr<Area>>("area", "area of pose"),
+            OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", "generated pose")
+    };
+}
+
+BT::NodeStatus BT::GenerateXYPose::tick() {
+    std::shared_ptr<Area> area;
+    getInput("area", area);
+
+    unsigned long triangleCount = area->triangles.size() - 2;
+
+    uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+    std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)};
+
+    std::vector<double> weights(triangleCount);
+    double sum = 0;
+    for (unsigned long i = 0; i < triangleCount; ++i) {
+        double currentArea = getArea(area,i);
+        weights[i] = currentArea;
+        sum += currentArea;
+    }
+    for (unsigned long i = 0; i < triangleCount; ++i) {
+        weights[i] = weights[i] / sum;
+    }
+    sum = 0;
+    for (unsigned long i = 0; i < triangleCount; ++i) {
+        weights[i] = weights[i] + sum;
+        sum += weights[i];
+    }
+
+    double triangleRandom = dist(re);
+    unsigned long i = 0;
+    while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
+        i++;
+    }
+    auto pos = getRandomPosInTriangle(area,i);
+
+    auto randomPose = std::make_shared<geometry_msgs::msg::Pose>();
+    randomPose->position.x = pos.x;
+    randomPose->position.y = pos.y;
+
+    printf("Generated Target: %f %f\n", pos.x, pos.y);
+    setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);
+    return NodeStatus::SUCCESS;
+}

src/btree_nodes/src/nodes/GenerateXYPose.h

@@ -0,0 +1,34 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#ifndef BUILD_GENERATEXYPOSE_H
+#define BUILD_GENERATEXYPOSE_H
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include "../Area.h"
+#include <random>
+#include <geometry_msgs/msg/pose.hpp>
+
+namespace BT {
+
+// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip.
+    class GenerateXYPose : public SyncActionNode {
+
+    private:
+        double getArea(const std::shared_ptr<Area> &area, unsigned long id);
+        Position2D getRandomPosInTriangle(const std::shared_ptr<Area> &area, unsigned long id);
+        std::default_random_engine re = std::default_random_engine(random());
+        std::uniform_real_distribution<double> dist = std::uniform_real_distribution<double>(0.0, 1.0);
+
+
+    public:
+        GenerateXYPose(const std::string &name, const NodeConfiguration &config);
+
+        static PortsList providedPorts();
+
+        NodeStatus tick() override;
+    };
+}
+
+#endif //BUILD_GENERATEXYPOSE_H

src/btree_nodes/src/nodes/IsInArea.cpp

@@ -0,0 +1,48 @@
+//
+// Created by bastian on 30.03.22.
+//
+
+#include "IsInArea.h"
+
+BT::PortsList BT::IsInArea::providedPorts() {
+    return {
+            InputPort<std::shared_ptr<Area>>("area"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")
+    };
+}
+
+double sign(Position2D p1, Position2D p2, Position2D p3) {
+    return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y);
+}
+
+bool pointInTri(Position2D point, Position2D triA, Position2D triB, Position2D triC) {
+    double d1 = sign(point, triA, triB);
+    double d2 = sign(point, triB, triC);
+    double d3 = sign(point, triC, triA);
+
+    return !(((d1 < 0) || (d2 < 0) || (d3 < 0)) && ((d1 > 0) || (d2 > 0) || (d3 > 0)));
+}
+
+BT::IsInArea::IsInArea(const std::string &name, const BT::NodeConfiguration &config) : ConditionNode(name, config) {}
+
+BT::NodeStatus BT::IsInArea::tick() {
+
+    std::shared_ptr<Area> area;
+    if (!getInput<std::shared_ptr<Area>>("area", area)) {
+        return NodeStatus::FAILURE;
+    }
+
+    std::shared_ptr<geometry_msgs::msg::Pose> pose;
+    if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", pose)) {
+        return NodeStatus::FAILURE;
+    }
+
+    Position2D point = {pose->position.x, pose->position.y};
+    for (unsigned long i = 0; i < area->triangles.size() - 2; i++) {
+        if (!pointInTri(point, area->triangles[i], area->triangles[i + 1], area->triangles[i + 1])) {
+            return NodeStatus::FAILURE;
+        }
+    }
+
+    return NodeStatus::SUCCESS;
+}

src/btree_nodes/src/nodes/IsInArea.h

@@ -0,0 +1,25 @@
+//
+// Created by bastian on 30.03.22.
+//
+
+#ifndef BUILD_ISINAREA_H
+#define BUILD_ISINAREA_H
+
+#include <behaviortree_cpp_v3/condition_node.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include "../Area.h"
+
+namespace BT {
+
+    class IsInArea : public ConditionNode {
+    public:
+        IsInArea(const std::string &name, const NodeConfiguration &config);
+
+        static PortsList providedPorts();
+
+        NodeStatus tick() override;
+    };
+
+}
+
+#endif //BUILD_ISINAREA_H

src/btree_nodes/src/nodes/MoveActorToTarget.cpp

@@ -9,8 +9,8 @@ BT::MoveActorToTarget::MoveActorToTarget(const std::string &name, const BT::Node
 
 BT::PortsList BT::MoveActorToTarget::providedPorts() {
     return {
-            InputPort<geometry_msgs::msg::Pose>("current"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
-            InputPort<geometry_msgs::msg::Pose>("target")
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
     };
 }
 
@@ -19,30 +19,30 @@ BT::NodeStatus BT::MoveActorToTarget::onStart() {
 
     rclcpp::Node node("targetPublisherNode");
     auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
 
-    auto res = getInput<geometry_msgs::msg::Pose>("target", target);
     if (!res) {
         std::cout << "[ no target available ]" << std::endl;
         std::cout << res.error() << std::endl;
         return NodeStatus::FAILURE;
     }
 
-    publisher->publish(target);
+    publisher->publish(*target);
     return NodeStatus::RUNNING;
 }
 
 BT::NodeStatus BT::MoveActorToTarget::onRunning() {
-    geometry_msgs::msg::Pose current;
+    std::shared_ptr<geometry_msgs::msg::Pose> current;
 
-    auto res = getInput<geometry_msgs::msg::Pose>("current", current);
+    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("current", current);
     if (!res) {
         std::cout << "[ no current position available ]" << std::endl;
         std::cout << res.error() << std::endl;
         return NodeStatus::FAILURE;
     }
 
-    double dX = target.position.x - current.position.x;
+    double dX = target->position.x - current->position.x;
-    double dY = target.position.y - current.position.y;
+    double dY = target->position.y - current->position.y;
 
     auto distance = sqrt(dX * dX + dY * dY);
 

src/btree_nodes/src/nodes/MoveActorToTarget.h

@@ -16,7 +16,7 @@ namespace BT {
 
         static PortsList providedPorts();
 
-        geometry_msgs::msg::Pose target;
+        std::shared_ptr<geometry_msgs::msg::Pose> target;
 
         NodeStatus onStart() override;
 

src/btree_nodes/src/nodes/MoveConnection.cpp

@@ -8,8 +8,6 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
                                     const std::function<void(bool)> &callback) {
     lock.lock();
 
-    std::cout << "planning" << std::endl;
-
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
     bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
     if(!success){
@@ -18,23 +16,13 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
     }
 
     std::thread([this,callback,pose](){
-        try {
+        moveGroup.setStartStateToCurrentState();
-            printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
+        moveGroup.setPoseTarget(*pose);
-            printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
 
-            moveGroup.setStartStateToCurrentState();
+        bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
-            moveGroup.setPoseTarget(*pose);
+        callback(success);
 
-            std::cout << "executing" << std::endl;
+        lock.unlock();
-            bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
-            std::cout << "callback" << std::endl;
-            callback(success);
-            std::cout << "end" << std::endl;
-            lock.unlock();
-        } catch (...) {
-            std::exception_ptr p = std::current_exception();
-            std::clog <<(p ? p.__cxa_exception_type()->name() : "null") << std::endl;
-        }
     }).detach();
 }
 
@@ -43,7 +31,5 @@ MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const
 }
 
 void MoveConnection::stop() {
-    std::cout << "stopping move" << std::endl;
     moveGroup.stop();
-    std::cout << "move stopped." << std::endl;
 }

src/btree_nodes/src/nodes/OffsetPose.cpp

@@ -0,0 +1,46 @@
+//
+// Created by bastian on 29.03.22.
+//
+
+#include "OffsetPose.h"
+
+
+BT::PortsList BT::OffsetPose::providedPorts() {
+    return {
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Position2D"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
+    };
+}
+
+BT::OffsetPose::OffsetPose(const std::string &name,
+        const BT::NodeConfiguration &config) : SyncActionNode(name, config) {}
+
+BT::NodeStatus BT::OffsetPose::tick() {
+
+    std::shared_ptr<geometry_msgs::msg::Pose> pose;
+    if (!getInput("input", pose)) {
+        return NodeStatus::FAILURE;
+    }
+
+    std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
+    if(getInput("orientation", quaternion)) {
+        pose->orientation.w = quaternion->w;
+        pose->orientation.x = quaternion->x;
+        pose->orientation.y = quaternion->y;
+        pose->orientation.z = quaternion->z;
+    }
+
+    std::shared_ptr<geometry_msgs::msg::Point> offset;
+    if(getInput("offset", offset)) {
+        pose->position.x += offset->x;
+        pose->position.y += offset->y;
+        pose->position.z += offset->z;
+    }
+
+    setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("output", pose);
+
+    return BT::NodeStatus::SUCCESS;
+}
+

src/btree_nodes/src/nodes/OffsetPose.h

@@ -0,0 +1,25 @@
+//
+// Created by bastian on 29.03.22.
+//
+
+#ifndef BUILD_OFFSETPOSE_H
+#define BUILD_OFFSETPOSE_H
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include "../Area.h"
+
+namespace BT {
+
+    class OffsetPose : public SyncActionNode {
+    public:
+        OffsetPose(const std::string &name, const NodeConfiguration &config);
+
+        static PortsList providedPorts();
+
+        NodeStatus tick() override;
+    };
+}
+
+#endif //BUILD_OFFSETPOSE_H

src/btree_nodes/src/nodes/RandomFailure.cpp

@@ -0,0 +1,27 @@
+//
+// Created by bastian on 29.03.22.
+//
+
+#include "RandomFailure.h"
+
+BT::NodeStatus BT::RandomFailure::tick() {
+    double failureChance;
+    if (!getInput<double>("failureChance", failureChance)) {
+        std::cout << "could not get failure chance." << std::endl;
+        return NodeStatus::FAILURE;
+    }
+    auto threshold = dist(re);
+    if (threshold < failureChance) {
+        return BT::NodeStatus::FAILURE;
+    } else {
+        return BT::NodeStatus::SUCCESS;
+    }
+}
+
+BT::PortsList BT::RandomFailure::providedPorts() {
+    return {InputPort<double>("failureChance", "Chance to fail from 0 to 1")};
+}
+
+BT::RandomFailure::RandomFailure(
+        const std::string &name, const BT::NodeConfiguration &config
+) : SyncActionNode(name,config) {}

src/btree_nodes/src/nodes/RandomFailure.h

@@ -0,0 +1,23 @@
+//
+// Created by bastian on 29.03.22.
+//
+
+#ifndef BUILD_RANDOMFAILURE_H
+#define BUILD_RANDOMFAILURE_H
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <random>
+
+namespace BT {
+    class RandomFailure : public SyncActionNode {
+    private:
+        std::default_random_engine re = std::default_random_engine(random());
+        std::uniform_real_distribution<double> dist = std::uniform_real_distribution<double>(0.0, 1.0);
+    public:
+        RandomFailure(const std::string &name, const NodeConfiguration &config);
+        NodeStatus tick() override;
+        static PortsList providedPorts();
+    };
+}
+
+#endif //BUILD_RANDOMFAILURE_H

src/btree_nodes/src/nodes/RobotMove.cpp

@@ -43,7 +43,5 @@ void BT::RobotMove::onHalted() {
 }
 
 BT::PortsList BT::RobotMove::providedPorts() {
-    // Optionally, a port can have a human readable description
+    return {InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target", "pose target for robot")};
-    const char *description = "Generates a random position in an area defined by a triangle strip";
-    return {InputPort<geometry_msgs::msg::Pose>("target", description)};
 }

src/btree_nodes/src/nodes/WeightedRandomNode.cpp

@@ -70,5 +70,5 @@ NodeStatus WeightedRandomNode::tick() {
 }
 
 PortsList WeightedRandomNode::providedPorts() {
-    return { InputPort<bool>("weights") };
+    return { InputPort<std::string>("weights") };
 }