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") };
}