From 7e878f2298242fed2b94c3d59a66019c9fb4424f Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Wed, 30 Mar 2022 23:11:14 +0200 Subject: [PATCH] Some refactoring of names Added new nodes --- src/btree_nodes/CMakeLists.txt | 5 +- src/btree_nodes/actorTree.xml | 35 +++-- src/btree_nodes/nodes.xml | 22 +-- src/btree_nodes/robotTree.xml | 14 +- src/btree_nodes/src/Extensions.cpp | 31 +++++ src/btree_nodes/src/Tree.cpp | 130 +++++++++++------- .../src/nodes/GenerateTarget2D.cpp | 88 ------------ src/btree_nodes/src/nodes/GenerateTarget2D.h | 40 ------ src/btree_nodes/src/nodes/GenerateXYPose.cpp | 84 +++++++++++ src/btree_nodes/src/nodes/GenerateXYPose.h | 34 +++++ src/btree_nodes/src/nodes/IsInArea.cpp | 48 +++++++ src/btree_nodes/src/nodes/IsInArea.h | 25 ++++ .../src/nodes/MoveActorToTarget.cpp | 16 +-- src/btree_nodes/src/nodes/MoveActorToTarget.h | 2 +- src/btree_nodes/src/nodes/MoveConnection.cpp | 24 +--- src/btree_nodes/src/nodes/OffsetPose.cpp | 46 +++++++ src/btree_nodes/src/nodes/OffsetPose.h | 25 ++++ src/btree_nodes/src/nodes/RandomFailure.cpp | 27 ++++ src/btree_nodes/src/nodes/RandomFailure.h | 23 ++++ src/btree_nodes/src/nodes/RobotMove.cpp | 4 +- .../src/nodes/WeightedRandomNode.cpp | 2 +- 21 files changed, 496 insertions(+), 229 deletions(-) delete mode 100644 src/btree_nodes/src/nodes/GenerateTarget2D.cpp delete mode 100644 src/btree_nodes/src/nodes/GenerateTarget2D.h create mode 100644 src/btree_nodes/src/nodes/GenerateXYPose.cpp create mode 100644 src/btree_nodes/src/nodes/GenerateXYPose.h create mode 100644 src/btree_nodes/src/nodes/IsInArea.cpp create mode 100644 src/btree_nodes/src/nodes/IsInArea.h create mode 100644 src/btree_nodes/src/nodes/OffsetPose.cpp create mode 100644 src/btree_nodes/src/nodes/OffsetPose.h create mode 100644 src/btree_nodes/src/nodes/RandomFailure.cpp create mode 100644 src/btree_nodes/src/nodes/RandomFailure.h diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index 832cb50..d384636 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/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}) diff --git a/src/btree_nodes/actorTree.xml b/src/btree_nodes/actorTree.xml index 18e3ded..8fbb3fb 100644 --- a/src/btree_nodes/actorTree.xml +++ b/src/btree_nodes/actorTree.xml @@ -6,9 +6,9 @@ - - - + + + @@ -19,20 +19,31 @@ called status - - Generated pose in safe area + + Area to generate pose in + Generated pose in area - - Generated pose in unsafe area - - - Generated pose in warning area - - + + Bounds to check in + Position of object + Result of check if position is within bounds + Current actor position Target to move actor to + + offset as a Point + rotation of resulting pose as Quaternion + generated new pose + initial position as Position2D + + + Chance to fail from 0 to 1 + + + Target pose of robot. + Weights for the children, separated by semicolon. diff --git a/src/btree_nodes/nodes.xml b/src/btree_nodes/nodes.xml index bfa6dd8..7d94d0b 100644 --- a/src/btree_nodes/nodes.xml +++ b/src/btree_nodes/nodes.xml @@ -11,14 +11,9 @@ called status - - Generated pose in safe area - - - Generated pose in warning area - - - Generated pose in unsafe area + + Area to generate pose in + Generated pose in area @@ -39,5 +34,16 @@ Target pose of robot. + + + initial position as Position2D + offset as a Point + rotation of resulting pose as Quaternion + generated new pose + + + + Chance to fail from 0 to 1 + diff --git a/src/btree_nodes/robotTree.xml b/src/btree_nodes/robotTree.xml index 543625a..b5c3938 100644 --- a/src/btree_nodes/robotTree.xml +++ b/src/btree_nodes/robotTree.xml @@ -3,8 +3,11 @@ - + + + + @@ -30,6 +33,15 @@ Current actor position Target to move actor to + + offset as a Point + rotation of resulting pose as Quaternion + generated new pose + initial position as Position2D + + + Chance to fail from 0 to 1 + Target pose of robot. diff --git a/src/btree_nodes/src/Extensions.cpp b/src/btree_nodes/src/Extensions.cpp index 5cbcb8e..7e4eb1e 100644 --- a/src/btree_nodes/src/Extensions.cpp +++ b/src/btree_nodes/src/Extensions.cpp @@ -62,4 +62,35 @@ namespace BT { return pose; } + + template<> + std::shared_ptr convertFromString(StringView str) { + auto parts = splitString(str, '|'); + if (parts.size() != 3) { + throw RuntimeError("Incorrect number of arguments, expected 3 in format '||'."); + } + + auto point = std::make_shared(); + point->x= convertFromString(parts[0]); + point->y= convertFromString(parts[1]); + point->z= convertFromString(parts[2]); + + return point; + } + + template<> + std::shared_ptr convertFromString(StringView str) { + auto parts = splitString(str, '|'); + if (parts.size() != 4) { + throw RuntimeError("Incorrect number of arguments, expected 4 in format '|||'."); + } + + auto point = std::make_shared(); + point->w= convertFromString(parts[0]); + point->x= convertFromString(parts[1]); + point->y= convertFromString(parts[2]); + point->z= convertFromString(parts[3]); + + return point; + } } \ No newline at end of file diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index cb24ca1..5451362 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -4,10 +4,13 @@ #include #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 #include @@ -26,86 +29,119 @@ int main(int argc, char **argv) { std::thread([&executor]() { executor.spin(); }).detach(); BehaviorTreeFactory factory; -/* + + const std::shared_ptr safeArea = std::make_shared(); + safeArea->triangles = {std::vector({ + {-1, 3.5}, + {-1, 7}, + {3, 3.5}, + {7, 7}, + {3, -1}, + {7, -1} + })}; + + const std::shared_ptr warningArea = std::make_shared(); + warningArea->triangles = {std::vector({ + {-1, 1.5}, + {-1, 2.5}, + {1, 1.5}, + {2, 2.5}, + {1, -1}, + {2, -1} + })}; + + const std::shared_ptr unsafeArea = std::make_shared(); + unsafeArea->triangles = std::vector({ + {-1, 2.5}, + {-1, 3.5}, + {2, 2.5}, + {3, 3.5}, + {2, -1}, + {3, -1} + }); + + const std::shared_ptr negativeYTable = std::make_shared(); + negativeYTable->triangles = std::vector({ + {0.3,-0.25}, + {-0.3,-0.25}, + {0,-0.4} + }); + + const std::shared_ptr positiveYTable = std::make_shared(); + positiveYTable->triangles = std::vector({ + {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({{1, 1}, - {5, 1}, - {5, 5}, - {1, 5}})}; - return std::make_unique(name, config, area); - }; - NodeBuilder builderGenerateWarningTarget = [](const std::string &name, const NodeConfiguration &config) { - const Area area = {std::vector({{1, 1}, - {2, 2}, - {1, 2}})}; - return std::make_unique(name, config, area); - }; - NodeBuilder builderGenerateUnsafeTarget = [](const std::string &name, const NodeConfiguration &config) { - const Area area = {std::vector({{1, 1}, - {2, 2}, - {1, 2}})}; - return std::make_unique(name, config, area); - }; - - factory.registerBuilder("GenerateSafeTarget", builderGenerateSafeTarget); - factory.registerBuilder("GenerateWarningTarget", builderGenerateWarningTarget); - factory.registerBuilder("GenerateUnsafeTarget", builderGenerateUnsafeTarget); + factory.registerNodeType("GenerateXYPose"); factory.registerNodeType("AmICalled"); factory.registerNodeType("MoveActorToTarget"); factory.registerNodeType("WeightedRandom"); - */ + factory.registerNodeType("OffsetPose"); + factory.registerNodeType("RandomFailure"); + factory.registerNodeType("IsInArea"); - auto connection = std::make_shared(mainNode,"iisy"); + auto connection = std::make_shared(mainNode, "iisy"); NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) { return std::make_unique(name, config, &connection); }; - factory.registerBuilder("RobotMove",builderIisyMove); + factory.registerBuilder("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; - actorTree.rootBlackboard()->set("current",init); - actorTree.rootBlackboard()->set("target",init); - actorTree.rootBlackboard()->set("called",false); + auto init = std::make_shared(); + actorTree.rootBlackboard()->set("current", init); + actorTree.rootBlackboard()->set("target", init); + actorTree.rootBlackboard()->set("called", false); MinimalSubscriber::createAsThread("tree_get_currentPose", "currentPose", - [&actorTree, &mutex](geometry_msgs::msg::Pose pose) { - mutex.lock(); - actorTree.rootBlackboard()->set("current", pose); - mutex.unlock(); - }).detach(); + [&actorTree, &mutex](geometry_msgs::msg::Pose pose) { + auto sharedPose = std::make_shared(pose); + mutex.lock(); + actorTree.rootBlackboard()->set("current", sharedPose); + mutex.unlock(); + }).detach(); MinimalSubscriber::createAsThread("tree_get_called", "called", - [&actorTree, &mutex](std_msgs::msg::Bool called) { - mutex.lock(); - actorTree.rootBlackboard()->set("called", (bool) called.data); - mutex.unlock(); - }).detach();*/ - + [&actorTree, &mutex](std_msgs::msg::Bool called) { + mutex.lock(); + actorTree.rootBlackboard()->set("called", + (bool) called.data); + 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(); diff --git a/src/btree_nodes/src/nodes/GenerateTarget2D.cpp b/src/btree_nodes/src/nodes/GenerateTarget2D.cpp deleted file mode 100644 index f481e77..0000000 --- a/src/btree_nodes/src/nodes/GenerateTarget2D.cpp +++ /dev/null @@ -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 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 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("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("target", randomPose); - return NodeStatus::SUCCESS; -} diff --git a/src/btree_nodes/src/nodes/GenerateTarget2D.h b/src/btree_nodes/src/nodes/GenerateTarget2D.h deleted file mode 100644 index 7169e57..0000000 --- a/src/btree_nodes/src/nodes/GenerateTarget2D.h +++ /dev/null @@ -1,40 +0,0 @@ -// -// Created by bastian on 26.03.22. -// - -#ifndef BUILD_GENERATETARGET2D_H -#define BUILD_GENERATETARGET2D_H - -#include -#include "../Area.h" -#include -#include - -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 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 diff --git a/src/btree_nodes/src/nodes/GenerateXYPose.cpp b/src/btree_nodes/src/nodes/GenerateXYPose.cpp new file mode 100644 index 0000000..96e00f1 --- /dev/null +++ b/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, 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, 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>("area", "area of pose"), + OutputPort>("pose", "generated pose") + }; +} + +BT::NodeStatus BT::GenerateXYPose::tick() { + std::shared_ptr 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 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(); + randomPose->position.x = pos.x; + randomPose->position.y = pos.y; + + printf("Generated Target: %f %f\n", pos.x, pos.y); + setOutput>("pose", randomPose); + return NodeStatus::SUCCESS; +} diff --git a/src/btree_nodes/src/nodes/GenerateXYPose.h b/src/btree_nodes/src/nodes/GenerateXYPose.h new file mode 100644 index 0000000..8a9d744 --- /dev/null +++ b/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 +#include "../Area.h" +#include +#include + +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, unsigned long id); + Position2D getRandomPosInTriangle(const std::shared_ptr &area, unsigned long id); + std::default_random_engine re = std::default_random_engine(random()); + std::uniform_real_distribution dist = std::uniform_real_distribution(0.0, 1.0); + + + public: + GenerateXYPose(const std::string &name, const NodeConfiguration &config); + + static PortsList providedPorts(); + + NodeStatus tick() override; + }; +} + +#endif //BUILD_GENERATEXYPOSE_H diff --git a/src/btree_nodes/src/nodes/IsInArea.cpp b/src/btree_nodes/src/nodes/IsInArea.cpp new file mode 100644 index 0000000..6b37fd8 --- /dev/null +++ b/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>("area"), + InputPort>("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; + if (!getInput>("area", area)) { + return NodeStatus::FAILURE; + } + + std::shared_ptr pose; + if (!getInput>("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; +} diff --git a/src/btree_nodes/src/nodes/IsInArea.h b/src/btree_nodes/src/nodes/IsInArea.h new file mode 100644 index 0000000..f1ab1c2 --- /dev/null +++ b/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 +#include +#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 diff --git a/src/btree_nodes/src/nodes/MoveActorToTarget.cpp b/src/btree_nodes/src/nodes/MoveActorToTarget.cpp index 4a71de4..4c4d968 100644 --- a/src/btree_nodes/src/nodes/MoveActorToTarget.cpp +++ b/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("current"), - InputPort("target") + InputPort>("current"), + InputPort>("target") }; } @@ -19,30 +19,30 @@ BT::NodeStatus BT::MoveActorToTarget::onStart() { rclcpp::Node node("targetPublisherNode"); auto publisher = node.create_publisher("targetPose", 10); + auto res = getInput>("target", target); - auto res = getInput("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 current; - auto res = getInput("current", current); + auto res = getInput>("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 dY = target.position.y - current.position.y; + double dX = target->position.x - current->position.x; + double dY = target->position.y - current->position.y; auto distance = sqrt(dX * dX + dY * dY); diff --git a/src/btree_nodes/src/nodes/MoveActorToTarget.h b/src/btree_nodes/src/nodes/MoveActorToTarget.h index 704c283..af011a1 100644 --- a/src/btree_nodes/src/nodes/MoveActorToTarget.h +++ b/src/btree_nodes/src/nodes/MoveActorToTarget.h @@ -16,7 +16,7 @@ namespace BT { static PortsList providedPorts(); - geometry_msgs::msg::Pose target; + std::shared_ptr target; NodeStatus onStart() override; diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp index ac0a7fa..cde8a26 100644 --- a/src/btree_nodes/src/nodes/MoveConnection.cpp +++ b/src/btree_nodes/src/nodes/MoveConnection.cpp @@ -8,8 +8,6 @@ void MoveConnection::planAndExecute(const std::shared_ptr &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_ptrposition.x,pose->position.y,pose->position.z); - printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z); + moveGroup.setStartStateToCurrentState(); + moveGroup.setPoseTarget(*pose); - moveGroup.setStartStateToCurrentState(); - moveGroup.setPoseTarget(*pose); + bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS; + callback(success); - std::cout << "executing" << std::endl; - 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; - } + lock.unlock(); }).detach(); } @@ -43,7 +31,5 @@ MoveConnection::MoveConnection(const std::shared_ptr &node, const } void MoveConnection::stop() { - std::cout << "stopping move" << std::endl; moveGroup.stop(); - std::cout << "move stopped." << std::endl; } diff --git a/src/btree_nodes/src/nodes/OffsetPose.cpp b/src/btree_nodes/src/nodes/OffsetPose.cpp new file mode 100644 index 0000000..bca28bb --- /dev/null +++ b/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>("input", "initial position as Position2D"), + InputPort>("offset", "offset as a Point"), + InputPort>("orientation", "rotation of resulting pose as Quaternion"), + InputPort>("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 pose; + if (!getInput("input", pose)) { + return NodeStatus::FAILURE; + } + + std::shared_ptr 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 offset; + if(getInput("offset", offset)) { + pose->position.x += offset->x; + pose->position.y += offset->y; + pose->position.z += offset->z; + } + + setOutput>("output", pose); + + return BT::NodeStatus::SUCCESS; +} + diff --git a/src/btree_nodes/src/nodes/OffsetPose.h b/src/btree_nodes/src/nodes/OffsetPose.h new file mode 100644 index 0000000..0dcfe09 --- /dev/null +++ b/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 +#include +#include +#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 diff --git a/src/btree_nodes/src/nodes/RandomFailure.cpp b/src/btree_nodes/src/nodes/RandomFailure.cpp new file mode 100644 index 0000000..b517fed --- /dev/null +++ b/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("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("failureChance", "Chance to fail from 0 to 1")}; +} + +BT::RandomFailure::RandomFailure( + const std::string &name, const BT::NodeConfiguration &config +) : SyncActionNode(name,config) {} diff --git a/src/btree_nodes/src/nodes/RandomFailure.h b/src/btree_nodes/src/nodes/RandomFailure.h new file mode 100644 index 0000000..8b1e288 --- /dev/null +++ b/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 +#include + +namespace BT { + class RandomFailure : public SyncActionNode { + private: + std::default_random_engine re = std::default_random_engine(random()); + std::uniform_real_distribution dist = std::uniform_real_distribution(0.0, 1.0); + public: + RandomFailure(const std::string &name, const NodeConfiguration &config); + NodeStatus tick() override; + static PortsList providedPorts(); + }; +} + +#endif //BUILD_RANDOMFAILURE_H diff --git a/src/btree_nodes/src/nodes/RobotMove.cpp b/src/btree_nodes/src/nodes/RobotMove.cpp index 9375693..7c22fd0 100644 --- a/src/btree_nodes/src/nodes/RobotMove.cpp +++ b/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 - const char *description = "Generates a random position in an area defined by a triangle strip"; - return {InputPort("target", description)}; + return {InputPort>("target", "pose target for robot")}; } \ No newline at end of file diff --git a/src/btree_nodes/src/nodes/WeightedRandomNode.cpp b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp index 2ddac73..b4c134a 100644 --- a/src/btree_nodes/src/nodes/WeightedRandomNode.cpp +++ b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp @@ -70,5 +70,5 @@ NodeStatus WeightedRandomNode::tick() { } PortsList WeightedRandomNode::providedPorts() { - return { InputPort("weights") }; + return { InputPort("weights") }; }