diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index d8d6c62..6469ed5 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(behaviortree_cpp_v3 REQUIRED) # further dependencies manually. # find_package( 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) diff --git a/src/btree_nodes/nodes.xml b/src/btree_nodes/nodes.xml index 8af5bc3..95c5c53 100644 --- a/src/btree_nodes/nodes.xml +++ b/src/btree_nodes/nodes.xml @@ -17,5 +17,9 @@ Where to get current pose Actual target pose + + + + diff --git a/src/btree_nodes/src/Area.cpp b/src/btree_nodes/src/Area.cpp deleted file mode 100644 index af72ef9..0000000 --- a/src/btree_nodes/src/Area.cpp +++ /dev/null @@ -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(parts[0]); - output.y = convertFromString(parts[1]); - return output; - } - } -} \ No newline at end of file diff --git a/src/btree_nodes/src/Area.h b/src/btree_nodes/src/Area.h index fadffec..7148d3c 100644 --- a/src/btree_nodes/src/Area.h +++ b/src/btree_nodes/src/Area.h @@ -8,15 +8,14 @@ #include #include -class Position2d{ +struct Position2D{ public: double x; double y; }; -class Area { - int triangleCount; - Position2d triangles[0] = {}; +struct Area { + std::vector triangles; }; diff --git a/src/btree_nodes/src/Extensions.cpp b/src/btree_nodes/src/Extensions.cpp new file mode 100644 index 0000000..4880a7f --- /dev/null +++ b/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(split[0]); + pos.y = convertFromString(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 array(parts.size()); + for (unsigned long i = 0; i < parts.size(); ++i) { + array[i] = convertFromString(parts[i]); + } + output.triangles = array; + return output; + } +} \ No newline at end of file diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index bb7d0b8..42dc868 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -2,6 +2,7 @@ #include "Area.h" #include #include +#include #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, {}) - {} + MyAsyncAction(const std::string &name) : + CoroActionNode(name, {}) {} private: // This is the ideal skeleton/template of an async action: @@ -26,9 +25,8 @@ private: // - You may call setStatusRunningAndYield() to "pause". // - Code to execute after the reply. // - A simple way to handle halt(). - NodeStatus tick() override - { - std::cout << name() <<": Started. Send Request to server." << std::endl; + NodeStatus tick() override { + std::cout << name() << ": Started. Send Request to server." << std::endl; TimePoint initial_time = Now(); TimePoint time_before_reply = initial_time + Milliseconds(100); @@ -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; + 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(); @@ -59,41 +53,145 @@ private: // This part of the code is never reached if halt() is invoked, // only if reply_received == true; - std::cout << name() <<": Done. 'Waiting Reply' loop repeated " + std::cout << name() << ": Done. 'Waiting Reply' loop repeated " << count << " times" << std::endl; cleanup(false); return NodeStatus::SUCCESS; } // you might want to cleanup differently if it was halted or successful - void cleanup(bool halted) - { - if( halted ) - { - std::cout << name() <<": cleaning up after an halt()\n" << std::endl; - } - else{ - std::cout << name() <<": cleaning up after SUCCESS\n" << std::endl; + void cleanup(bool halted) { + if (halted) { + std::cout << name() << ": cleaning up after an halt()\n" << std::endl; + } else { + std::cout << name() << ": cleaning up after SUCCESS\n" << std::endl; } } - void halt() override - { - std::cout << name() <<": Halted." << std::endl; + 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 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 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 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("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("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({{1, 1}, + {2, 2}, + {1, 2}})}; + return std::make_unique(name, config, areaSafe); + }; + rclcpp::init(argc, argv); + + new Area(); + + BehaviorTreeFactory factory; + factory.registerBuilder("GenerateSafeTarget", builderGenerateSafeTarget); + factory.registerBuilder("GenerateWarningTarget", builderGenerateSafeTarget); + factory.registerBuilder("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); diff --git a/src/cpp_pubsub/src/publisher_member_function.cpp b/src/cpp_pubsub/src/publisher_member_function.cpp index 77318b7..5d6d2ba 100644 --- a/src/cpp_pubsub/src/publisher_member_function.cpp +++ b/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 \ No newline at end of file