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_POSITION_INDEPENDENT_CODE ON)
# find dependencies
set(DEPENDENCIES
@ -42,7 +43,9 @@ set(CPP_FILES
src/nodes/MoveConnection.cpp
src/nodes/OffsetPose.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})

41
src/btree_nodes/actorTree.xml
View File

@ -2,31 +2,41 @@
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<ReactiveSequence>
<Action ID="AmICalled" called="{called}"/>
<Fallback>
<ReactiveSequence>
<Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence>
<Control ID="WeightedRandom" weights="10;2;1">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Control ID="WeightedRandom" weights="10;2;1">
<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}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</ReactiveSequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="AmICalled">
<input_port name="called">called status</input_port>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<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>
<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>
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="MoveActorToTarget">
<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="position">initial position as Position2D</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>

25
src/btree_nodes/nodes.xml
View File

@ -7,8 +7,11 @@
<root>
<TreeNodesModel>
<!-- ############################### ACTION NODES ################################# -->
<Action ID="AmICalled">
<input_port name="called">called status</input_port>
<Condition ID="IsCalled">
</Condition>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
@ -25,21 +28,27 @@
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Control ID="InterruptableSequence">
</Control>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<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>
<input_port name="area">Area to check in</input_port>
<input_port name="pose">Pose of object</input_port>
</Condition>
<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>
<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="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 ID="RandomFailure">

62
src/btree_nodes/robotTree.xml
View File

@ -2,42 +2,53 @@
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<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>
<ReactiveSequence>
<Inverter>
<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>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="AmICalled">
<input_port name="called">called status</input_port>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateSafeTarget">
<output_port name="target">Generated pose in safe 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 ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<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>
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_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">
<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="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>
<output_port name="output">generated new pose</output_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
@ -45,9 +56,14 @@
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Control ID="InterruptableSequence">
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

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

@ -10,13 +10,13 @@
#include "helpers/MinimalSubscriber.h"
#include "nodes/OffsetPose.h"
#include "nodes/RandomFailure.h"
#include "nodes/IsInArea.h"
#include "nodes/InAreaTest.h"
#include "nodes/InterruptableSequence.h"
#include "nodes/SetRobotVelocity.h"
#include <chrono>
#include <thread>
//using namespace BT;
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
@ -42,22 +42,22 @@ int main(int argc, char **argv) {
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> 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>();
@ -83,69 +83,79 @@ int main(int argc, char **argv) {
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose");
factory.registerNodeType<RandomFailure>("RandomFailure");
factory.registerNodeType<IsInArea>("IsInArea");
factory.registerNodeType<InAreaTest>("InAreaTest");
factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
auto connection = std::make_shared<MoveConnection>(mainNode, "iisy");
NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<RobotMove>(name, config, &connection);
};
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;
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()};
auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()};
auto init = std::make_shared<geometry_msgs::msg::Pose>();
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;
item->set("actorPos", init);
}
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",
[&actorTree, &mutex](geometry_msgs::msg::Pose pose) {
[&trees](geometry_msgs::msg::Pose pose) {
auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose);
mutex.lock();
actorTree.rootBlackboard()->set("current", sharedPose);
mutex.unlock();
for (const auto &item : trees){
item->set("actorPos", sharedPose);
}
}).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::thread actor([&actorTree, &mutex]() {
std::thread actor([&actorTree]() {
while (rclcpp::ok()) {
mutex.lock();
actorTree.tickRoot();
mutex.unlock();
std::this_thread::sleep_for(std::chrono::milliseconds(20));
}
});
while (rclcpp::ok()) {
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;

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

@ -69,7 +69,7 @@ BT::NodeStatus BT::GenerateXYPose::tick() {
double triangleRandom = dist(re);
unsigned long i = 0;
while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
while (triangleRandom > weights[i + 1] && i < weights.size() - 1) {
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.
//
#include "IsInArea.h"
#include "InAreaTest.h"
BT::PortsList BT::IsInArea::providedPorts() {
BT::PortsList BT::InAreaTest::providedPorts() {
return {
InputPort<std::shared_ptr<Area>>("area"),
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)));
}
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;
if (!getInput<std::shared_ptr<Area>>("area", area)) {
std::cout<<"No area given"<<std::endl;
return NodeStatus::FAILURE;
}
std::shared_ptr<geometry_msgs::msg::Pose> pose;
if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", pose)) {
std::cout<<"No pose given"<<std::endl;
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;
if (pointInTri(point, area->triangles[i], area->triangles[i + 1], area->triangles[i + 2])) {
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.
//
#ifndef BUILD_ISINAREA_H
#define BUILD_ISINAREA_H
#ifndef BUILD_INAREATEST_H
#define BUILD_INAREATEST_H
#include <behaviortree_cpp_v3/condition_node.h>
#include <geometry_msgs/msg/pose.hpp>
@ -11,9 +11,9 @@
namespace BT {
class IsInArea : public ConditionNode {
class InAreaTest : public ConditionNode {
public:
IsInArea(const std::string &name, const NodeConfiguration &config);
InAreaTest(const std::string &name, const NodeConfiguration &config);
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

37
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,
const std::function<void(bool)> &callback) {
lock.lock();
lastTarget = pose;
lastCallback = callback;
moveGroup.setMaxVelocityScalingFactor(currentSpeed);
moveGroup.setMaxAccelerationScalingFactor(1);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
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.setPoseTarget(*pose);
bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
callback(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);
}
lastTarget = nullptr;
lock.unlock();
}).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) {
}
void MoveConnection::stop() {
std::cout<<"Stopping"<<std::endl;
cancelled = true;
moveGroup.stop();
while (lastTarget != nullptr) {}
cancelled = false;
}

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

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

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

@ -17,9 +17,9 @@ class WeightedRandomNode : public ControlNode
public:
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();
private:
@ -27,7 +27,7 @@ private:
bool select_next{};
std::mt19937_64 rng;
virtual BT::NodeStatus tick() override;
BT::NodeStatus tick() override;
};