yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Some refactoring of names

Browse Source 
Added new nodes
This commit is contained in:
Bastian Hofmann 2022-03-30 23:11:14 +02:00
parent 58d1d08d01
commit 7e878f2298
21 changed files with 496 additions and 229 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/btree_nodes/CMakeLists.txt
View File

@ -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})

35
src/btree_nodes/actorTree.xml
View File

@ -6,9 +6,9 @@
<Action ID="AmICalled" called="{called}"/>
<Sequence>
<Control ID="WeightedRandom" weights="10;2;1">
<Action ID="GenerateSafeTarget" target="{target}"/>
<Action ID="GenerateWarningTarget" target="{target}"/>
<Action ID="GenerateUnsafeTarget" target="{target}"/>
<Action ID="GenerateXYPose" area="{safeArea}" pose="{target}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{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">
<output_port name="target">Generated pose in safe area</output_port>
<Action ID="GenerateXYPose">
<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">
<output_port name="target">Generated pose in unsafe area</output_port>
</Action>
<Action ID="GenerateWarningTarget">
<output_port name="target">Generated pose in warning area</output_port>
</Action>
<Condition ID="InTest"/>
<Condition ID="InAreaTest">
<input_port name="bounds">Bounds to check in</input_port>
<input_port name="position">Position of object</input_port>
<output_port name="result">Result of check if position is within bounds</output_port>
</Condition>
<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>

22
src/btree_nodes/nodes.xml
View File

@ -11,14 +11,9 @@
<input_port name="called">called status</input_port>
</Action>
<Action ID="GenerateSafeTarget">
<output_port name="target">Generated pose in safe area</output_port>
</Action>
<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 ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in 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>

14
src/btree_nodes/robotTree.xml
View File

@ -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>

31
src/btree_nodes/src/Extensions.cpp
View File

@ -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;
}
}

110
src/btree_nodes/src/Tree.cpp
View File

@ -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,37 +29,61 @@ 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");
@ -68,44 +95,53 @@ int main(int argc, char **argv) {
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<geometry_msgs::msg::Pose>("current",init);
actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
auto init = std::make_shared<geometry_msgs::msg::Pose>();
actorTree.rootBlackboard()->set("current", init);
actorTree.rootBlackboard()->set("target", init);
actorTree.rootBlackboard()->set<bool>("called", false);
MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
[&actorTree, &mutex](geometry_msgs::msg::Pose pose) {
auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose);
mutex.lock();
actorTree.rootBlackboard()->set("current", pose);
actorTree.rootBlackboard()->set("current", sharedPose);
mutex.unlock();
}).detach();
MinimalSubscriber<std_msgs::msg::Bool>::createAsThread("tree_get_called", "called",
[&actorTree, &mutex](std_msgs::msg::Bool called) {
mutex.lock();
actorTree.rootBlackboard()->set("called", (bool) called.data);
actorTree.rootBlackboard()->set("called",
(bool) called.data);
mutex.unlock();
}).detach();*/
}).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();

88
src/btree_nodes/src/nodes/GenerateTarget2D.cpp
View File

@ -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;
}

40
src/btree_nodes/src/nodes/GenerateTarget2D.h
View File

@ -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

84
src/btree_nodes/src/nodes/GenerateXYPose.cpp Normal file
View File

@ -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;
}

34
src/btree_nodes/src/nodes/GenerateXYPose.h Normal file
View File

@ -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

48
src/btree_nodes/src/nodes/IsInArea.cpp Normal file
View File

@ -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;
}

25
src/btree_nodes/src/nodes/IsInArea.h Normal file
View File

@ -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

16
src/btree_nodes/src/nodes/MoveActorToTarget.cpp
View File

@ -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<geometry_msgs::msg::Pose>("target")
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
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 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);

2
src/btree_nodes/src/nodes/MoveActorToTarget.h
View File

@ -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;

16
src/btree_nodes/src/nodes/MoveConnection.cpp
View File

@ -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 {
printf("X:%f Y:%f Z:%f\n",pose->position.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);
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;
}
}).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;
}

46
src/btree_nodes/src/nodes/OffsetPose.cpp Normal file
View File

@ -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;
}

25
src/btree_nodes/src/nodes/OffsetPose.h Normal file
View File

@ -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

27
src/btree_nodes/src/nodes/RandomFailure.cpp Normal file
View File

@ -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) {}

23
src/btree_nodes/src/nodes/RandomFailure.h Normal file
View File

@ -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

4
src/btree_nodes/src/nodes/RobotMove.cpp
View File

@ -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<geometry_msgs::msg::Pose>("target", description)};
return {InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target", "pose target for robot")};
}

2
src/btree_nodes/src/nodes/WeightedRandomNode.cpp
View File

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