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

Allow loading of trees from parameter

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-23 13:37:11 +00:00
parent 7193070220
commit 65df5d10c4
4 changed files with 40 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.clang-format Normal file
View File

@ -0,0 +1,3 @@
BasedOnStyle: LLVM
IndentWidth: 4
ColumnLimit: 140

8
.editorconfig Normal file
View File

@ -0,0 +1,8 @@
root = true
[*]
indent_size = 4
indent_style = space
charset = utf-8
end_of_line = lf
insert_final_newline = true

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

@ -1,5 +1,6 @@
#include "Area.h" #include "Area.h"
#include <behaviortree_cpp_v3/blackboard.h> #include <behaviortree_cpp_v3/blackboard.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <random> #include <random>
#include <rclcpp/utilities.hpp> #include <rclcpp/utilities.hpp>
@ -140,8 +141,16 @@ int main(int argc, char **argv) {
std::cout << "Creating tree." << std::endl; std::cout << "Creating tree." << std::endl;
Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml",blackboard); mainNode -> declare_parameter("trees",std::vector<std::string>({
Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml",blackboard); "/home/ros/workspace/src/btree_nodes/actorTree.xml",
"/home/ros/workspace/src/btree_nodes/robotTree.xml"
}));
auto treeFileNames = mainNode -> get_parameter("trees").as_string_array();
auto trees = std::vector<std::shared_ptr<BT::Tree>>();
for(auto treeFileName : treeFileNames){
trees.emplace_back(std::make_shared<BT::Tree>(factory.createTreeFromFile(treeFileName,blackboard)));
}
auto init = std::make_shared<geometry_msgs::msg::Pose>(); auto init = std::make_shared<geometry_msgs::msg::Pose>();
init->position.x = 6.0; init->position.x = 6.0;
@ -167,8 +176,9 @@ int main(int argc, char **argv) {
while (rclcpp::ok()) { while (rclcpp::ok()) {
blackboardMutex.lock(); blackboardMutex.lock();
actorTree.tickRoot(); for(auto tree : trees){
robotTree.tickRoot(); tree->tickRoot();
}
blackboardMutex.unlock(); blackboardMutex.unlock();
} }