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

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

@ -50,13 +50,13 @@ namespace BT {
}
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
pose->orientation.w=0;
pose->orientation.x=0;
pose->orientation.y=1;
pose->orientation.z=0;
pose->position.x= convertFromString<double>(parts[0]);
pose->position.y= convertFromString<double>(parts[1]);
pose->position.z= convertFromString<double>(parts[2]);
pose->orientation.w = 0;
pose->orientation.x = 0;
pose->orientation.y = 1;
pose->orientation.z = 0;
pose->position.x = convertFromString<double>(parts[0]);
pose->position.y = convertFromString<double>(parts[1]);
pose->position.z = convertFromString<double>(parts[2]);
std::cout << "[ generated pose from string ]" << std::endl;
@ -71,9 +71,9 @@ namespace BT {
}
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]);
point->x = convertFromString<double>(parts[0]);
point->y = convertFromString<double>(parts[1]);
point->z = convertFromString<double>(parts[2]);
return point;
}
@ -86,10 +86,10 @@ namespace BT {
}
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]);
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;
}

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

@ -1,5 +1,6 @@
#include "Area.h"
#include <behaviortree_cpp_v3/blackboard.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <rclcpp/utilities.hpp>
@ -139,9 +140,17 @@ int main(int argc, char **argv) {
factory.registerSimpleAction("SetCalledTo",SetCalledTo,{InputPort("state","state to set called to")});
std::cout << "Creating tree." << std::endl;
Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml",blackboard);
Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml",blackboard);
mainNode -> declare_parameter("trees",std::vector<std::string>({
"/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>();
init->position.x = 6.0;
@ -167,8 +176,9 @@ int main(int argc, char **argv) {
while (rclcpp::ok()) {
blackboardMutex.lock();
actorTree.tickRoot();
robotTree.tickRoot();
for(auto tree : trees){
tree->tickRoot();
}
blackboardMutex.unlock();
}