diff --git a/build.sh b/build.sh index 69676d7..39ba5ec 100755 --- a/build.sh +++ b/build.sh @@ -1,4 +1,4 @@ #!/bin/bash pushd "$(dirname "$0")" || exit -colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja +colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja popd || exit diff --git a/src/btree/CMakeLists.txt b/src/btree/CMakeLists.txt index f52bfd1..7db86a1 100644 --- a/src/btree/CMakeLists.txt +++ b/src/btree/CMakeLists.txt @@ -36,7 +36,6 @@ endforeach() set(CPP_FILES src/Tree.cpp - src/Extensions.cpp src/nodes/WeightedRandomNode.cpp src/nodes/GenerateXYPose.cpp src/nodes/RobotMove.cpp @@ -49,8 +48,9 @@ set(CPP_FILES src/nodes/ActorAnimation.cpp src/nodes/ActorMovement.cpp ) - add_executable(tree ${CPP_FILES}) +SET_SOURCE_FILES_PROPERTIES(src/Extensions.hpp PROPERTIES COMPILE_FLAGS -O0) +target_include_directories(tree PUBLIC /src /src/nodes) set_property(TARGET tree PROPERTY CXX_STANDARD 17) ament_target_dependencies(tree ${DEPENDENCIES}) diff --git a/src/btree/src/Extensions.cpp b/src/btree/src/Extensions.cpp deleted file mode 100644 index d90a6d9..0000000 --- a/src/btree/src/Extensions.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// -// Created by bastian on 28.02.22. -// - -#include "Extensions.hpp" - -namespace BT { - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str) { - auto split = splitString(str, ','); - if (split.size() != 2) { - throw RuntimeError("Incorrect number of arguments, expected 2 in format ','."); - } - - auto pos = std::make_shared(); - pos->x = convertFromString(split[0]); - pos->y = convertFromString(split[1]); - return pos; - } - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str) { - auto parts = splitString(str, '|'); - if (parts.size() < 3) { - throw RuntimeError("Incorrect number of arguments, expected at least 3 in format ',,|,,|,,'."); - } - - auto output = std::make_shared(); - std::vector array(parts.size()); - for (unsigned long i = 0; i < parts.size(); ++i) { - array[i] = *convertFromString>(parts[i]); - } - output->triangles = array; - return output; - } - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str) { - auto parts = splitString(str, ','); - if (!(parts.size() == 3 || parts.size() == 7)) { - throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format ',,[,,,,]'."); - } - - auto pose = std::make_shared(); - if(parts.size() == 7){ - pose->orientation.w = convertFromString(parts[3]); - pose->orientation.x = convertFromString(parts[4]); - pose->orientation.y = convertFromString(parts[5]); - pose->orientation.z = convertFromString(parts[6]); - }else{ - pose->orientation.w = 1; - pose->orientation.x = 0; - pose->orientation.y = 0; - pose->orientation.z = 0; - } - pose->position.x = convertFromString(parts[0]); - pose->position.y = convertFromString(parts[1]); - pose->position.z = convertFromString(parts[2]); - - return pose; - } - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str) { - auto parts = splitString(str, ','); - if (parts.size() != 3) { - throw RuntimeError("Incorrect number of arguments, expected 3 in format ',,'."); - } - - auto point = std::make_shared(); - point->x = convertFromString(parts[0]); - point->y = convertFromString(parts[1]); - point->z = convertFromString(parts[2]); - - return point; - } - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str) { - auto parts = splitString(str, ','); - if (parts.size() != 4) { - throw RuntimeError("Incorrect number of arguments, expected 4 in format ',,,'."); - } - - auto point = std::make_shared(); - point->w = convertFromString(parts[0]); - point->x = convertFromString(parts[1]); - point->y = convertFromString(parts[2]); - point->z = convertFromString(parts[3]); - - return point; - } -} \ No newline at end of file diff --git a/src/btree/src/Extensions.hpp b/src/btree/src/Extensions.hpp index cdca2c2..f1835aa 100644 --- a/src/btree/src/Extensions.hpp +++ b/src/btree/src/Extensions.hpp @@ -8,20 +8,91 @@ #include namespace BT { - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str); - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str); - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str); - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str); - - template<> [[nodiscard]] - std::shared_ptr convertFromString(StringView str); + template<> inline + std::shared_ptr convertFromString(StringView str) { + auto split = splitString(str, ','); + if (split.size() != 2) { + throw RuntimeError("Incorrect number of arguments, expected 2 in format ','."); + } + + auto pos = std::make_shared(); + pos->x = convertFromString(split[0]); + pos->y = convertFromString(split[1]); + return pos; + } + + template<> inline + std::shared_ptr convertFromString(StringView str) { + auto parts = splitString(str, '|'); + if (parts.size() < 3) { + throw RuntimeError("Incorrect number of arguments, expected at least 3 in format ',,|,,|,,'."); + } + + auto output = std::make_shared(); + std::vector array(parts.size()); + for (unsigned long i = 0; i < parts.size(); ++i) { + array[i] = *convertFromString>(parts[i]); + } + output->triangles = array; + return output; + } + + template<> inline + std::shared_ptr convertFromString(StringView str) { + auto parts = splitString(str, ','); + if (!(parts.size() == 3 || parts.size() == 7)) { + throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format ',,[,,,,]'."); + } + + auto pose = std::make_shared(); + if(parts.size() == 7){ + pose->orientation.w = convertFromString(parts[3]); + pose->orientation.x = convertFromString(parts[4]); + pose->orientation.y = convertFromString(parts[5]); + pose->orientation.z = convertFromString(parts[6]); + }else{ + pose->orientation.w = 1; + pose->orientation.x = 0; + pose->orientation.y = 0; + pose->orientation.z = 0; + } + pose->position.x = convertFromString(parts[0]); + pose->position.y = convertFromString(parts[1]); + pose->position.z = convertFromString(parts[2]); + + return pose; + } + + template<> inline + std::shared_ptr BT::convertFromString(StringView str) { + auto parts = splitString(str, ','); + if (parts.size() != 3) { + throw RuntimeError("Incorrect number of arguments, expected 3 in format ',,'."); + } + + auto point = std::make_shared(); + point->x = convertFromString(parts[0]); + point->y = convertFromString(parts[1]); + point->z = convertFromString(parts[2]); + + return point; + } + + template<> inline + std::shared_ptr convertFromString(StringView str) { + auto parts = splitString(str, ','); + if (parts.size() != 4) { + throw RuntimeError("Incorrect number of arguments, expected 4 in format ',,,'."); + } + + auto point = std::make_shared(); + point->w = convertFromString(parts[0]); + point->x = convertFromString(parts[1]); + point->y = convertFromString(parts[2]); + point->z = convertFromString(parts[3]); + + return point; + } } #endif //BUILD_EXTENSIONS_H \ No newline at end of file diff --git a/src/btree/src/Tree.cpp b/src/btree/src/Tree.cpp index 0b04fbd..285ed5d 100644 --- a/src/btree/src/Tree.cpp +++ b/src/btree/src/Tree.cpp @@ -78,7 +78,7 @@ int main(int argc, char **argv) { NodeBuilder builderActorMovement = [&actorMovementNode, &blackboard, &blackboardMutex](const std::string &name, const NodeConfiguration &config) { return std::make_unique(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboard,&blackboardMutex](std::shared_ptr feedback){ blackboardMutex.lock(); - blackboard->set("actorPos", std::make_shared(feedback->current)); + //blackboard->set("actorPos", std::make_shared(feedback->current)); blackboardMutex.unlock(); }); }; diff --git a/src/btree/src/nodes/GenerateXYPose.cpp b/src/btree/src/nodes/GenerateXYPose.cpp index 51a156e..9c1c39d 100644 --- a/src/btree/src/nodes/GenerateXYPose.cpp +++ b/src/btree/src/nodes/GenerateXYPose.cpp @@ -82,6 +82,8 @@ BT::NodeStatus BT::GenerateXYPose::tick() { randomPose->orientation.x = 0; randomPose->orientation.y = 0; randomPose->orientation.z = 0; + + std::cout << "Generated Pose." << std::endl; setOutput>("pose", randomPose); return NodeStatus::SUCCESS; diff --git a/src/btree/src/nodes/InAreaTest.cpp b/src/btree/src/nodes/InAreaTest.cpp index 42d7d49..3cda105 100644 --- a/src/btree/src/nodes/InAreaTest.cpp +++ b/src/btree/src/nodes/InAreaTest.cpp @@ -3,6 +3,7 @@ // #include "InAreaTest.h" +#include "../Extensions.hpp" BT::PortsList BT::InAreaTest::providedPorts() { return { diff --git a/src/btree/src/nodes/InterruptableSequence.cpp b/src/btree/src/nodes/InterruptableSequence.cpp index 84a6b7a..5fc8215 100644 --- a/src/btree/src/nodes/InterruptableSequence.cpp +++ b/src/btree/src/nodes/InterruptableSequence.cpp @@ -15,11 +15,10 @@ void BT::InterruptableSequence::halt() { BT::NodeStatus BT::InterruptableSequence::tick() { auto result = children_nodes_[position]->executeTick(); - setStatus(NodeStatus::RUNNING); - if(result==NodeStatus::FAILURE){ resetChildren(); position=0; + setStatus(NodeStatus::FAILURE); return NodeStatus::FAILURE; } @@ -28,10 +27,12 @@ BT::NodeStatus BT::InterruptableSequence::tick() { if(position>=children_nodes_.size()){ resetChildren(); position=0; + setStatus(NodeStatus::SUCCESS); return NodeStatus::SUCCESS; } } + setStatus(NodeStatus::RUNNING); return NodeStatus::RUNNING; } diff --git a/src/btree/src/nodes/OffsetPose.cpp b/src/btree/src/nodes/OffsetPose.cpp index 79c14f1..6c42082 100644 --- a/src/btree/src/nodes/OffsetPose.cpp +++ b/src/btree/src/nodes/OffsetPose.cpp @@ -2,6 +2,7 @@ // Created by bastian on 29.03.22. // +#include "../Extensions.hpp" #include "OffsetPose.h" #include #include @@ -23,6 +24,7 @@ BT::NodeStatus BT::OffsetPose::tick() { std::shared_ptr pose; if (!getInput("input", pose)) { + std::cout << "Missing {input} pose" << std::endl; return NodeStatus::FAILURE; } diff --git a/src/btree/trees/robotTreeCoex.xml b/src/btree/trees/robotTreeCoex.xml index e3bb1f1..34b7182 100644 --- a/src/btree/trees/robotTreeCoex.xml +++ b/src/btree/trees/robotTreeCoex.xml @@ -11,14 +11,12 @@ - - - - - - - - + + + + + +