changes before converting ActorPlugin to Action

src/btree_nodes/CMakeLists.txt

@@ -16,7 +16,7 @@ find_package(behaviortree_cpp_v3 REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_executable(tree src/Tree.cpp)
+add_executable(tree src/Tree.cpp src/Extensions.cpp)
 ament_target_dependencies(tree geometry_msgs behaviortree_cpp_v3 rclcpp)
 
 #add_executable(talker src/publisher_member_function.cpp)

src/btree_nodes/nodes.xml

@@ -17,5 +17,9 @@
             <input_port name="pose_listener">Where to get current pose</input_port>
             <input_port name="target_pose">Actual target pose</input_port>
         </Action>
+
+        <Condition ID="InTest">
+
+        </Condition>
     </TreeNodesModel>
 </root>

src/btree_nodes/src/Area.cpp

@@ -1,27 +0,0 @@
-//
-// Created by bastian on 21.02.22.
-//
-
-#include "Area.h"
-
-namespace BT
-{
-    template <> inline Area convertFromString(StringView str)
-    {
-        // The next line should be removed...
-        printf("Converting string: \"%s\"\n", str.data() );
-
-        // We expect real numbers separated by semicolons
-        auto parts = splitString(str, ';');
-        if (parts.size() != 2)
-        {
-            throw RuntimeError("invalid input)");
-        }
-        else{
-            Position2d output;
-            output.x     = convertFromString<double>(parts[0]);
-            output.y     = convertFromString<double>(parts[1]);
-            return output;
-        }
-    }
-}

src/btree_nodes/src/Area.h

@@ -8,15 +8,14 @@
 #include <behaviortree_cpp_v3/behavior_tree.h>
 #include <behaviortree_cpp_v3/bt_factory.h>
 
-class Position2d{
+struct Position2D{
 public:
     double x;
     double y;
 };
 
-class Area {
-    int triangleCount;
-    Position2d triangles[0] = {};
+struct Area {
+    std::vector<Position2D> triangles;
 };
 
 

src/btree_nodes/src/Extensions.cpp

@@ -0,0 +1,40 @@
+//
+// Created by bastian on 28.02.22.
+//
+
+#include "Area.h"
+
+namespace BT {
+    template<>
+    inline Position2D convertFromString(StringView str) {
+        printf("Converting string: \"%s\"\n", str.data());
+
+        auto split = splitString(str, ';');
+        if (split.size() != 2) {
+            throw RuntimeError("2 Arguments required.");
+        }
+
+        Position2D pos{};
+        pos.x = convertFromString<double>(split[0]);
+        pos.y = convertFromString<double>(split[1]);
+        return pos;
+    }
+
+    template<>
+    inline Area convertFromString(StringView str) {
+        printf("Converting string: \"%s\"\n", str.data());
+
+        auto parts = splitString(str, '|');
+        if (parts.size() < 3) {
+            throw RuntimeError("Not enough argument pairs.");
+        }
+
+        Area output{};
+        std::vector<Position2D> array(parts.size());
+        for (unsigned long i = 0; i < parts.size(); ++i) {
+            array[i] = convertFromString<Position2D>(parts[i]);
+        }
+        output.triangles = array;
+        return output;
+    }
+}

src/btree_nodes/src/Tree.cpp

@@ -2,6 +2,7 @@
 #include "Area.h"
 #include <behaviortree_cpp_v3/action_node.h>
 #include <rclcpp/rclcpp.hpp>
+#include <random>
 #include "MinimalSubscriber.cpp"
 
 //-------------------------------------------------------------
@@ -12,12 +13,10 @@ using namespace BT;
 
 typedef std::chrono::milliseconds Milliseconds;
 
-class MyAsyncAction: public CoroActionNode
-{
+class MyAsyncAction : public CoroActionNode {
 public:
     MyAsyncAction(const std::string &name) :
-            CoroActionNode(name, {})
-    {}
+            CoroActionNode(name, {}) {}
 
 private:
     // This is the ideal skeleton/template of an async action:
@@ -26,8 +25,7 @@ private:
     //  - You may call setStatusRunningAndYield() to "pause".
     //  - Code to execute after the reply.
     //  - A simple way to handle halt().
-    NodeStatus tick() override
-    {
+    NodeStatus tick() override {
         std::cout << name() << ": Started. Send Request to server." << std::endl;
 
         TimePoint initial_time = Now();
@@ -36,21 +34,17 @@ private:
         int count = 0;
         bool reply_received = false;
 
-        while( !reply_received )
-        {
-            if( count++ == 0)
-            {
+        while (!reply_received) {
+            if (count++ == 0) {
                 // call this only once
                 std::cout << name() << ": Waiting Reply..." << std::endl;
             }
             // pretend that we received a reply
-            if( Now() >= time_before_reply )
-            {
+            if (Now() >= time_before_reply) {
                 reply_received = true;
             }
 
-            if( !reply_received )
-            {
+            if (!reply_received) {
                 // set status to RUNNING and "pause/sleep"
                 // If halt() is called, we will NOT resume execution
                 setStatusRunningAndYield();
@@ -66,34 +60,138 @@ private:
     }
 
     // you might want to cleanup differently if it was halted or successful
-    void cleanup(bool halted)
-    {
-        if( halted )
-        {
+    void cleanup(bool halted) {
+        if (halted) {
             std::cout << name() << ": cleaning up after an halt()\n" << std::endl;
-        }
-        else{
+        } else {
             std::cout << name() << ": cleaning up after SUCCESS\n" << std::endl;
         }
     }
 
-    void halt() override
-    {
+    void halt() override {
         std::cout << name() << ": Halted." << std::endl;
         cleanup(true);
         // Do not forget to call this at the end.
         CoroActionNode::halt();
     }
 
-    TimePoint Now()
-    {
+    TimePoint Now() {
         return std::chrono::high_resolution_clock::now();
     };
 };
 
-int main(int argc, char **argv)
-{
+// 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) {
+        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));
+    }
+
+private:
+    double getRandom() {
+        std::uniform_real_distribution<double> zeroToOne(0, 1);
+        return zeroToOne(rng);
+    }
+
+private:
+    Position2D 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)};
+    }
+
+public:
+    GenerateTarget2D(const std::string &name, const 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;
+    }
+
+    static PortsList 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<Position2D>("target", description)};
+    }
+
+
+    NodeStatus tick() override {
+        double triangleRandom = getRandom();
+        unsigned long i = 0;
+        while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
+            i++;
+        }
+        Position2D randomPos = getRandomPosInTriangle(i);
+
+        setOutput<Position2D>("targetPosition", randomPos);
+        return NodeStatus::SUCCESS;
+    }
+};
+
+int main(int argc, char **argv) {
+    NodeBuilder builderGenerateSafeTarget = [](const std::string &name, const NodeConfiguration &config) {
+        const Area areaSafe = {std::vector<Position2D>({{1, 1},
+                                                        {2, 2},
+                                                        {1, 2}})};
+        return std::make_unique<GenerateTarget2D>(name, config, areaSafe);
+    };
+
     rclcpp::init(argc, argv);
+
+    new Area();
+
+    BehaviorTreeFactory factory;
+    factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
+    factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateSafeTarget);
+    factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateSafeTarget);
+
+    Tree tree = factory.createTreeFromFile("actor_tree.xml");
+
+    while (rclcpp::ok()) {
+        tree.tickRoot();
+    }
     /*
     NodeHandle nh;
 
@@ -107,9 +205,9 @@ int main(int argc, char **argv)
 
     NodeStatus status = NodeStatus::IDLE;
 
-    while( ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
+    while( rclcpp::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
     {
-        ros::spinOnce();
+        rclcpp::spinOnce();
         status = tree.tickRoot();
         std::cout << status << std::endl;
         ros::Duration sleep_time(0.01);

src/cpp_pubsub/src/publisher_member_function.cpp

@@ -50,5 +50,3 @@ int main(int argc, char *argv[]) {
     rclcpp::shutdown();
     return 0;
 }
-
-#pragma clang diagnostic pop