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

More Nodes for robot interaction, bugfixing, refactoring.

Browse Source 
probably final version
This commit is contained in:
Bastian Hofmann 2022-04-01 22:43:19 +02:00
parent 7e878f2298
commit 7f9b30083f
16 changed files with 319 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/btree_nodes/CMakeLists.txt
View File

@ -6,6 +6,7 @@ add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# find dependencies # find dependencies
set(DEPENDENCIES set(DEPENDENCIES
@ -42,7 +43,9 @@ set(CPP_FILES
src/nodes/MoveConnection.cpp src/nodes/MoveConnection.cpp
src/nodes/OffsetPose.cpp src/nodes/OffsetPose.cpp
src/nodes/RandomFailure.cpp src/nodes/RandomFailure.cpp
src/nodes/IsInArea.cpp src/nodes/InAreaTest.cpp
src/nodes/InterruptableSequence.cpp
src/nodes/SetRobotVelocity.cpp
) )
add_executable(tree ${CPP_FILES}) add_executable(tree ${CPP_FILES})

33
src/btree_nodes/actorTree.xml
View File

@ -2,31 +2,41 @@
<root main_tree_to_execute="BehaviorTree"> <root main_tree_to_execute="BehaviorTree">
<!-- ////////// --> <!-- ////////// -->
<BehaviorTree ID="BehaviorTree"> <BehaviorTree ID="BehaviorTree">
<Fallback>
<ReactiveSequence> <ReactiveSequence>
<Action ID="AmICalled" called="{called}"/> <Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence> <Sequence>
<Control ID="WeightedRandom" weights="10;2;1"> <Control ID="WeightedRandom" weights="10;2;1">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{target}"/> <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{target}"/> <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{target}"/> <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control> </Control>
<Action ID="MoveActorToTarget" current="{current}" target="{target}"/> <Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
</Sequence> </Sequence>
</ReactiveSequence> </ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</Fallback>
</BehaviorTree> </BehaviorTree>
<!-- ////////// --> <!-- ////////// -->
<TreeNodesModel> <TreeNodesModel>
<Action ID="AmICalled"> <Condition ID="IsCalled"/>
<input_port name="called">called status</input_port> <Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action> </Action>
<Action ID="GenerateXYPose"> <Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port> <input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port> <output_port name="pose">Generated pose in area</output_port>
</Action> </Action>
<Condition ID="InAreaTest"> <Condition ID="InAreaTest">
<input_port name="bounds">Bounds to check in</input_port> <input_port name="area">Bounds to check in</input_port>
<input_port name="position">Position of object</input_port> <input_port name="pose">Position of object</input_port>
<output_port name="result">Result of check if position is within bounds</output_port>
</Condition> </Condition>
<Action ID="MoveActorToTarget"> <Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port> <input_port name="current">Current actor position</input_port>
@ -38,6 +48,9 @@
<input_port name="output">generated new pose</input_port> <input_port name="output">generated new pose</input_port>
<input_port name="position">initial position as Position2D</input_port> <input_port name="position">initial position as Position2D</input_port>
</Action> </Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Action ID="RandomFailure"> <Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port> <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action> </Action>

25
src/btree_nodes/nodes.xml
View File

@ -7,8 +7,11 @@
<root> <root>
<TreeNodesModel> <TreeNodesModel>
<!-- ############################### ACTION NODES ################################# --> <!-- ############################### ACTION NODES ################################# -->
<Action ID="AmICalled"> <Condition ID="IsCalled">
<input_port name="called">called status</input_port> </Condition>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action> </Action>
<Action ID="GenerateXYPose"> <Action ID="GenerateXYPose">
@ -25,21 +28,27 @@
<input_port name="weights">Weights for the children, separated by semicolon.</input_port> <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control> </Control>
<Control ID="InterruptableSequence">
</Control>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Condition ID="InAreaTest"> <Condition ID="InAreaTest">
<input_port name="bounds">Bounds to check in</input_port> <input_port name="area">Area to check in</input_port>
<input_port name="position">Position of object</input_port> <input_port name="pose">Pose of object</input_port>
<output_port name="result">Result of check if position is within bounds</output_port>
</Condition> </Condition>
<Action ID="RobotMove"> <Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port> <input_port name="target">Target pose of robot.</input_port>
</Action> </Action>
<Action ID="PositionToPose"> <Action ID="OffsetPose">
<input_port name="position">initial position as Position2D</input_port> <input_port name="input">initial position as Pose</input_port>
<input_port name="offset">offset as a Point</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="orientation">rotation of resulting pose as Quaternion</input_port>
<input_port name="output">generated new pose</input_port> <output_port name="output">generated new pose</output_port>
</Action> </Action>
<Action ID="RandomFailure"> <Action ID="RandomFailure">

58
src/btree_nodes/robotTree.xml
View File

@ -2,42 +2,53 @@
<root main_tree_to_execute="BehaviorTree"> <root main_tree_to_execute="BehaviorTree">
<!-- ////////// --> <!-- ////////// -->
<BehaviorTree ID="BehaviorTree"> <BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="RobotMove" target="-0.2|-0.2|1.1"/>
<ReactiveSequence> <ReactiveSequence>
<Action ID="RandomFailure" failureChance="0.2"/> <Inverter>
<Action ID="RobotMove" target="0.2|0.2|1.1"/> <Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
</Inverter>
<IfThenElse>
<Condition ID="InAreaTest" area="{warningArea}" pose="{actorPos}"/>
<Action ID="SetRobotVelocity" velocity="0.1"/>
<Action ID="SetRobotVelocity" velocity="1"/>
</IfThenElse>
<Fallback>
<Control ID="InterruptableSequence">
<Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0|0|1.1" orientation="0|0|1|0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failureChance="0.15"/>
<Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0|0|1.1" orientation="0|0|1|0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failureChance="0.05"/>
</Control>
<Action ID="SetCalledTo" state="true"/>
</Fallback>
</ReactiveSequence> </ReactiveSequence>
</Sequence>
</BehaviorTree> </BehaviorTree>
<!-- ////////// --> <!-- ////////// -->
<TreeNodesModel> <TreeNodesModel>
<Action ID="AmICalled"> <Condition ID="IsCalled"/>
<input_port name="called">called status</input_port> <Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action> </Action>
<Action ID="GenerateSafeTarget"> <Action ID="GenerateXYPose">
<output_port name="target">Generated pose in safe area</output_port> <input_port name="area">Area to generate pose in</input_port>
</Action> <output_port name="pose">Generated pose in area</output_port>
<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> </Action>
<Condition ID="InAreaTest"> <Condition ID="InAreaTest">
<input_port name="bounds">Bounds to check in</input_port> <input_port name="area">Bounds to check in</input_port>
<input_port name="position">Position of object</input_port> <input_port name="pose">Position of object</input_port>
<output_port name="result">Result of check if position is within bounds</output_port>
</Condition> </Condition>
<Action ID="MoveActorToTarget"> <Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port> <input_port name="current">Current actor position</input_port>
<input_port name="target">Target to move actor to</input_port> <input_port name="target">Target to move actor to</input_port>
</Action> </Action>
<Action ID="PositionToPose"> <Action ID="OffsetPose">
<input_port name="input">initial position as Pose</input_port>
<input_port name="offset">offset as a Point</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="orientation">rotation of resulting pose as Quaternion</input_port>
<input_port name="output">generated new pose</input_port> <output_port name="output">generated new pose</output_port>
<input_port name="position">initial position as Position2D</input_port>
</Action> </Action>
<Action ID="RandomFailure"> <Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port> <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
@ -45,9 +56,14 @@
<Action ID="RobotMove"> <Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port> <input_port name="target">Target pose of robot.</input_port>
</Action> </Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Control ID="WeightedRandom"> <Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port> <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control> </Control>
<Control ID="InterruptableSequence">
</Control>
</TreeNodesModel> </TreeNodesModel>
<!-- ////////// --> <!-- ////////// -->
</root> </root>

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

@ -10,13 +10,13 @@
#include "helpers/MinimalSubscriber.h" #include "helpers/MinimalSubscriber.h"
#include "nodes/OffsetPose.h" #include "nodes/OffsetPose.h"
#include "nodes/RandomFailure.h" #include "nodes/RandomFailure.h"
#include "nodes/IsInArea.h" #include "nodes/InAreaTest.h"
#include "nodes/InterruptableSequence.h"
#include "nodes/SetRobotVelocity.h"
#include <chrono> #include <chrono>
#include <thread> #include <thread>
//using namespace BT;
int main(int argc, char **argv) { int main(int argc, char **argv) {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options; rclcpp::NodeOptions node_options;
@ -42,22 +42,22 @@ int main(int argc, char **argv) {
const std::shared_ptr<Area> warningArea = std::make_shared<Area>(); const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
warningArea->triangles = {std::vector<Position2D>({ 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, 2.5},
{-1, 3.5}, {-1, 3.5},
{2, 2.5}, {2, 2.5},
{3, 3.5}, {3, 3.5},
{2, -1}, {2, -1},
{3, -1} {3, -1}
})};
const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
unsafeArea->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> negativeYTable = std::make_shared<Area>(); const std::shared_ptr<Area> negativeYTable = std::make_shared<Area>();
@ -83,69 +83,79 @@ int main(int argc, char **argv) {
factory.registerNodeType<WeightedRandomNode>("WeightedRandom"); factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose"); factory.registerNodeType<OffsetPose>("OffsetPose");
factory.registerNodeType<RandomFailure>("RandomFailure"); factory.registerNodeType<RandomFailure>("RandomFailure");
factory.registerNodeType<IsInArea>("IsInArea"); factory.registerNodeType<InAreaTest>("InAreaTest");
factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
auto connection = std::make_shared<MoveConnection>(mainNode, "iisy"); auto connection = std::make_shared<MoveConnection>(mainNode, "iisy");
NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) { NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<RobotMove>(name, config, &connection); return std::make_unique<RobotMove>(name, config, &connection);
}; };
factory.registerBuilder<RobotMove>("RobotMove", builderIisyMove); factory.registerBuilder<RobotMove>("RobotMove", builderIisyMove);
NodeBuilder builderIisyVelocity = [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<SetRobotVelocity>(name, config, &connection);
};
factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", builderIisyVelocity);
bool called;
auto IsCalled = [&called](TreeNode& parent_node) -> NodeStatus{
return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
};
auto SetCalledTo = [&called](TreeNode& parent_node) -> NodeStatus{
if(!parent_node.getInput<bool>("state",called)){
std::cout<<"no state supplied."<<std::endl;
return NodeStatus::FAILURE;
}
std::cout<<"state changed"<<std::endl;
return NodeStatus::SUCCESS;
};
factory.registerSimpleCondition("IsCalled",IsCalled);
factory.registerSimpleAction("SetCalledTo",SetCalledTo,{InputPort("state","state to set called to")});
std::cout << "Creating tree." << std::endl; 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"); Tree robotTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/robotTree.xml");
auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()}; auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()};
auto init = std::make_shared<geometry_msgs::msg::Pose>();
for (const auto &item : trees){ for (const auto &item : trees){
item->set("safeArea",safeArea); item->set("safeArea",safeArea);
item->set("warningArea",warningArea); item->set("warningArea",warningArea);
item->set("unsafeArea",unsafeArea); item->set("unsafeArea",unsafeArea);
item->set("negativeYTable",negativeYTable); item->set("negativeYTable",negativeYTable);
item->set("positiveYTable",positiveYTable); item->set("positiveYTable",positiveYTable);
}
std::mutex mutex; item->set("actorPos", init);
}
std::cout << "Starting subscriber." << std::endl; std::cout << "Starting subscriber." << std::endl;
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", MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
[&actorTree, &mutex](geometry_msgs::msg::Pose pose) { [&trees](geometry_msgs::msg::Pose pose) {
auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose); auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose);
mutex.lock(); for (const auto &item : trees){
actorTree.rootBlackboard()->set("current", sharedPose); item->set("actorPos", sharedPose);
mutex.unlock(); }
}).detach(); }).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);
mutex.unlock();
}).detach();
std::cout << "Looping." << std::endl; std::cout << "Looping." << std::endl;
std::thread actor([&actorTree, &mutex]() { std::thread actor([&actorTree]() {
while (rclcpp::ok()) { while (rclcpp::ok()) {
mutex.lock();
actorTree.tickRoot(); actorTree.tickRoot();
mutex.unlock(); std::this_thread::sleep_for(std::chrono::milliseconds(20));
} }
}); });
while (rclcpp::ok()) { while (rclcpp::ok()) {
robotTree.tickRoot(); robotTree.tickRoot();
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(20));
} }
std::cout << "End." << std::endl; std::cout << "End." << std::endl;

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

@ -69,7 +69,7 @@ BT::NodeStatus BT::GenerateXYPose::tick() {
double triangleRandom = dist(re); double triangleRandom = dist(re);
unsigned long i = 0; unsigned long i = 0;
while (triangleRandom < weights[i + 1] && i < weights.size() - 1) { while (triangleRandom > weights[i + 1] && i < weights.size() - 1) {
i++; i++;
} }
auto pos = getRandomPosInTriangle(area,i); auto pos = getRandomPosInTriangle(area,i);

16
src/btree_nodes/src/nodes/IsInArea.cpp → src/btree_nodes/src/nodes/InAreaTest.cpp
View File

@ -2,9 +2,9 @@
// Created by bastian on 30.03.22. // Created by bastian on 30.03.22.
// //
#include "IsInArea.h" #include "InAreaTest.h"
BT::PortsList BT::IsInArea::providedPorts() { BT::PortsList BT::InAreaTest::providedPorts() {
return { return {
InputPort<std::shared_ptr<Area>>("area"), InputPort<std::shared_ptr<Area>>("area"),
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose") InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")
@ -23,26 +23,28 @@ bool pointInTri(Position2D point, Position2D triA, Position2D triB, Position2D t
return !(((d1 < 0) || (d2 < 0) || (d3 < 0)) && ((d1 > 0) || (d2 > 0) || (d3 > 0))); 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::InAreaTest::InAreaTest(const std::string &name, const BT::NodeConfiguration &config) : ConditionNode(name, config) {}
BT::NodeStatus BT::IsInArea::tick() { BT::NodeStatus BT::InAreaTest::tick() {
std::shared_ptr<Area> area; std::shared_ptr<Area> area;
if (!getInput<std::shared_ptr<Area>>("area", area)) { if (!getInput<std::shared_ptr<Area>>("area", area)) {
std::cout<<"No area given"<<std::endl;
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
std::shared_ptr<geometry_msgs::msg::Pose> pose; std::shared_ptr<geometry_msgs::msg::Pose> pose;
if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", pose)) { if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", pose)) {
std::cout<<"No pose given"<<std::endl;
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
Position2D point = {pose->position.x, pose->position.y}; Position2D point = {pose->position.x, pose->position.y};
for (unsigned long i = 0; i < area->triangles.size() - 2; i++) { 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])) { if (pointInTri(point, area->triangles[i], area->triangles[i + 1], area->triangles[i + 2])) {
return NodeStatus::FAILURE; return NodeStatus::SUCCESS;
} }
} }
return NodeStatus::SUCCESS; return NodeStatus::FAILURE;
} }

10
src/btree_nodes/src/nodes/IsInArea.h → src/btree_nodes/src/nodes/InAreaTest.h
View File

@ -2,8 +2,8 @@
// Created by bastian on 30.03.22. // Created by bastian on 30.03.22.
// //
#ifndef BUILD_ISINAREA_H #ifndef BUILD_INAREATEST_H
#define BUILD_ISINAREA_H #define BUILD_INAREATEST_H
#include <behaviortree_cpp_v3/condition_node.h> #include <behaviortree_cpp_v3/condition_node.h>
#include <geometry_msgs/msg/pose.hpp> #include <geometry_msgs/msg/pose.hpp>
@ -11,9 +11,9 @@
namespace BT { namespace BT {
class IsInArea : public ConditionNode { class InAreaTest : public ConditionNode {
public: public:
IsInArea(const std::string &name, const NodeConfiguration &config); InAreaTest(const std::string &name, const NodeConfiguration &config);
static PortsList providedPorts(); static PortsList providedPorts();
@ -22,4 +22,4 @@ namespace BT {
} }
#endif //BUILD_ISINAREA_H #endif //BUILD_INAREATEST_H

40
src/btree_nodes/src/nodes/InterruptableSequence.cpp Normal file
View File

@ -0,0 +1,40 @@
//
// Created by bastian on 01.04.22.
//
#include "InterruptableSequence.h"
BT::InterruptableSequence::InterruptableSequence(const std::string &name, const NodeConfiguration &config)
: ControlNode::ControlNode(name, config) {
}
void BT::InterruptableSequence::halt() {
ControlNode::halt();
}
BT::NodeStatus BT::InterruptableSequence::tick() {
auto result = children_nodes_[position]->executeTick();
setStatus(NodeStatus::RUNNING);
if(result==NodeStatus::FAILURE){
haltChildren();
position=0;
return NodeStatus::FAILURE;
}
if(result==NodeStatus::SUCCESS){
position++;
if(position>=children_nodes_.size()){
haltChildren();
position=0;
return NodeStatus::SUCCESS;
}
}
return NodeStatus::RUNNING;
}
BT::PortsList BT::InterruptableSequence::providedPorts() {
return {};
}

29
src/btree_nodes/src/nodes/InterruptableSequence.h Normal file
View File

@ -0,0 +1,29 @@
//
// Created by bastian on 01.04.22.
//
#ifndef BUILD_INTERRUPTABLESEQUENCE_H
#define BUILD_INTERRUPTABLESEQUENCE_H
#include <behaviortree_cpp_v3/control_node.h>
namespace BT {
class InterruptableSequence : public ControlNode{
public:
InterruptableSequence(const std::string &name, const NodeConfiguration &config);
~InterruptableSequence() override = default;
void halt() override;
static PortsList providedPorts();
BT::NodeStatus tick() override;
private:
unsigned long position=0;
};
}
#endif //BUILD_INTERRUPTABLESEQUENCE_H

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

@ -7,6 +7,11 @@
void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose, void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
const std::function<void(bool)> &callback) { const std::function<void(bool)> &callback) {
lock.lock(); lock.lock();
lastTarget = pose;
lastCallback = callback;
moveGroup.setMaxVelocityScalingFactor(currentSpeed);
moveGroup.setMaxAccelerationScalingFactor(1);
moveit::planning_interface::MoveGroupInterface::Plan my_plan; moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
@ -19,17 +24,45 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
moveGroup.setStartStateToCurrentState(); moveGroup.setStartStateToCurrentState();
moveGroup.setPoseTarget(*pose); moveGroup.setPoseTarget(*pose);
bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS; moveit::planning_interface::MoveGroupInterface::Plan plan;
moveGroup.plan(plan);
if(cancelled){
lastTarget = nullptr;
lock.unlock();
return;
}
bool success = this->moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
if(!cancelled) {
callback(success); callback(success);
}
lastTarget = nullptr;
lock.unlock(); lock.unlock();
}).detach(); }).detach();
} }
void MoveConnection::setSpeedMultiplier(const double speed){
if(currentSpeed!=speed) {
std::cout<<"Changing speed."<<std::endl;
currentSpeed = speed;
auto target = lastTarget;
auto callback = lastCallback;
if (target != nullptr) {
stop();
std::cout<<"Restarting"<<std::endl;
planAndExecute(target, callback);
}
}
}
MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) { MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {
} }
void MoveConnection::stop() { void MoveConnection::stop() {
std::cout<<"Stopping"<<std::endl;
cancelled = true;
moveGroup.stop(); moveGroup.stop();
while (lastTarget != nullptr) {}
cancelled = false;
} }

6
src/btree_nodes/src/nodes/MoveConnection.h
View File

@ -13,6 +13,10 @@
class MoveConnection { class MoveConnection {
private: private:
moveit::planning_interface::MoveGroupInterface moveGroup; moveit::planning_interface::MoveGroupInterface moveGroup;
std::shared_ptr<geometry_msgs::msg::Pose> lastTarget = nullptr;
std::function<void(bool)> lastCallback = [](bool finished) {};
bool cancelled=false;
double currentSpeed = 1;
std::mutex lock; std::mutex lock;
public: public:
MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup); MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup);
@ -20,6 +24,8 @@ public:
void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)> &callback); void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)> &callback);
void stop(); void stop();
void setSpeedMultiplier(double speed);
}; };

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

@ -0,0 +1,27 @@
//
// Created by bastian on 01.04.22.
//
#include "SetRobotVelocity.h"
BT::NodeStatus BT::SetRobotVelocity::tick() {
double velocity;
if(!getInput<double>("velocity",velocity)){
std::cout<<"No velocity given."<<std::endl;
return NodeStatus::FAILURE;
}
connection->get()->setSpeedMultiplier(velocity);
return NodeStatus::SUCCESS;
}
BT::PortsList BT::SetRobotVelocity::providedPorts() {
return {
InputPort<double>("velocity"),
};
}
BT::SetRobotVelocity::SetRobotVelocity(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<MoveConnection> *connection) : SyncActionNode(
name, config) {
this->connection = connection;
}

27
src/btree_nodes/src/nodes/SetRobotVelocity.h Normal file
View File

@ -0,0 +1,27 @@
//
// Created by bastian on 01.04.22.
//
#ifndef BUILD_SETROBOTVELOCITY_H
#define BUILD_SETROBOTVELOCITY_H
#include <behaviortree_cpp_v3/action_node.h>
#include "MoveConnection.h"
namespace BT {
class SetRobotVelocity: public SyncActionNode {
private:
std::shared_ptr<MoveConnection> *connection{};
public:
SetRobotVelocity(const std::string &name, const NodeConfiguration &config,
std::shared_ptr<MoveConnection> *connection);
static PortsList providedPorts();
NodeStatus tick() override;
};
}
#endif //BUILD_SETROBOTVELOCITY_H

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

@ -41,7 +41,6 @@ NodeStatus WeightedRandomNode::tick() {
} }
if(weights.size()!=children_count){ if(weights.size()!=children_count){
std::cout<< "Number of weights doesn't match child count." <<std::endl;
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
@ -57,13 +56,11 @@ NodeStatus WeightedRandomNode::tick() {
select_next = false; select_next = false;
} }
std::cout<< "Running child " << selected_child_index << std::endl;
TreeNode *current_child_node = children_nodes_[selected_child_index]; TreeNode *current_child_node = children_nodes_[selected_child_index];
const NodeStatus child_status = current_child_node->executeTick(); const NodeStatus child_status = current_child_node->executeTick();
if (child_status != NodeStatus::RUNNING) { if (child_status != NodeStatus::RUNNING) {
select_next = true; select_next = true;
printf("node %zu finished.\n",selected_child_index);
} }
return child_status; return child_status;

6
src/btree_nodes/src/nodes/WeightedRandomNode.h
View File

@ -17,9 +17,9 @@ class WeightedRandomNode : public ControlNode
public: public:
WeightedRandomNode(const std::string& name, const NodeConfiguration &config); WeightedRandomNode(const std::string& name, const NodeConfiguration &config);
virtual ~WeightedRandomNode() override = default; ~WeightedRandomNode() override = default;
virtual void halt() override; void halt() override;
static PortsList providedPorts(); static PortsList providedPorts();
private: private:
@ -27,7 +27,7 @@ private:
bool select_next{}; bool select_next{};
std::mt19937_64 rng; std::mt19937_64 rng;
virtual BT::NodeStatus tick() override; BT::NodeStatus tick() override;
}; };