Fixed build

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

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})
 

src/btree/src/Extensions.cpp

@@ -1,93 +0,0 @@
-//
-// Created by bastian on 28.02.22.
-//
-
-#include "Extensions.hpp"
-
-namespace BT {
-    template<> [[nodiscard]]
-    std::shared_ptr<Position2D> convertFromString(StringView str) {
-        auto split = splitString(str, ',');
-        if (split.size() != 2) {
-            throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
-        }
-
-        auto pos = std::make_shared<Position2D>();
-        pos->x = convertFromString<double>(split[0]);
-        pos->y = convertFromString<double>(split[1]);
-        return pos;
-    }
-
-    template<> [[nodiscard]]
-    std::shared_ptr<Area> convertFromString(StringView str) {
-        auto parts = splitString(str, '|');
-        if (parts.size() < 3) {
-            throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
-        }
-
-        auto output = std::make_shared<Area>();
-        std::vector<Position2D> array(parts.size());
-        for (unsigned long i = 0; i < parts.size(); ++i) {
-            array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
-        }
-        output->triangles = array;
-        return output;
-    }
-
-    template<> [[nodiscard]]
-    std::shared_ptr<geometry_msgs::msg::Pose> 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 '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
-        }
-
-        auto pose = std::make_shared<geometry_msgs::msg::Pose>();
-        if(parts.size() == 7){
-            pose->orientation.w = convertFromString<double>(parts[3]);
-            pose->orientation.x = convertFromString<double>(parts[4]);
-            pose->orientation.y = convertFromString<double>(parts[5]);
-            pose->orientation.z = convertFromString<double>(parts[6]);
-        }else{
-            pose->orientation.w = 1;
-            pose->orientation.x = 0;
-            pose->orientation.y = 0;
-            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]);
-
-        return pose;
-    }
-
-    template<> [[nodiscard]]
-    std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
-        auto parts = splitString(str, ',');
-        if (parts.size() != 3) {
-            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
-        }
-
-        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]);
-
-        return point;
-    }
-
-    template<> [[nodiscard]]
-    std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
-        auto parts = splitString(str, ',');
-        if (parts.size() != 4) {
-            throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
-        }
-
-        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]);
-
-        return point;
-    }
-}

src/btree/src/Extensions.hpp

@@ -8,20 +8,91 @@
 #include <geometry_msgs/msg/pose.hpp>
 
 namespace BT {
-    template<> [[nodiscard]]
+    template<> inline
-    std::shared_ptr<Position2D> convertFromString(StringView str);
+    std::shared_ptr<Position2D> convertFromString(StringView str) {
-    
+        auto split = splitString(str, ',');
-    template<> [[nodiscard]]
+        if (split.size() != 2) {
-    std::shared_ptr<Area> convertFromString(StringView str);
+            throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
-    
+        }
-    template<> [[nodiscard]]
+
-    std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str);
+        auto pos = std::make_shared<Position2D>();
-    
+        pos->x = convertFromString<double>(split[0]);
-    template<> [[nodiscard]]
+        pos->y = convertFromString<double>(split[1]);
-    std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str);
+        return pos;
-    
+    }
-    template<> [[nodiscard]]
+
-    std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str);
+    template<> inline
+    std::shared_ptr<Area> convertFromString(StringView str) {
+        auto parts = splitString(str, '|');
+        if (parts.size() < 3) {
+            throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
+        }
+
+        auto output = std::make_shared<Area>();
+        std::vector<Position2D> array(parts.size());
+        for (unsigned long i = 0; i < parts.size(); ++i) {
+            array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
+        }
+        output->triangles = array;
+        return output;
+    }
+
+    template<> inline
+    std::shared_ptr<geometry_msgs::msg::Pose> 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 '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
+        }
+
+        auto pose = std::make_shared<geometry_msgs::msg::Pose>();
+        if(parts.size() == 7){
+            pose->orientation.w = convertFromString<double>(parts[3]);
+            pose->orientation.x = convertFromString<double>(parts[4]);
+            pose->orientation.y = convertFromString<double>(parts[5]);
+            pose->orientation.z = convertFromString<double>(parts[6]);
+        }else{
+            pose->orientation.w = 1;
+            pose->orientation.x = 0;
+            pose->orientation.y = 0;
+            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]);
+
+        return pose;
+    }
+
+    template<> inline
+    std::shared_ptr<geometry_msgs::msg::Point> BT::convertFromString(StringView str) {
+        auto parts = splitString(str, ',');
+        if (parts.size() != 3) {
+            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
+        }
+
+        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]);
+
+        return point;
+    }
+
+    template<> inline
+    std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
+        auto parts = splitString(str, ',');
+        if (parts.size() != 4) {
+            throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
+        }
+
+        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]);
+
+        return point;
+    }
 }
 
 #endif //BUILD_EXTENSIONS_H

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<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboard,&blackboardMutex](std::shared_ptr<const Movement::Feedback> feedback){
             blackboardMutex.lock();
-            blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
+            //blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
             blackboardMutex.unlock();
         });
     };

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<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);
     return NodeStatus::SUCCESS;

src/btree/src/nodes/InAreaTest.cpp

@@ -3,6 +3,7 @@
 //
 
 #include "InAreaTest.h"
+#include "../Extensions.hpp"
 
 BT::PortsList BT::InAreaTest::providedPorts() {
     return {

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;
 }
 

src/btree/src/nodes/OffsetPose.cpp

@@ -2,6 +2,7 @@
 // Created by bastian on 29.03.22.
 //
 
+#include "../Extensions.hpp"
 #include "OffsetPose.h"
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <memory>
@@ -23,6 +24,7 @@ BT::NodeStatus BT::OffsetPose::tick() {
     
     std::shared_ptr<geometry_msgs::msg::Pose> pose;
     if (!getInput("input", pose)) {
+        std::cout << "Missing {input} pose" << std::endl;
         return NodeStatus::FAILURE;
     }
     

src/btree/trees/robotTreeCoex.xml

@@ -11,14 +11,12 @@
                 <Action ID="SetRobotVelocity" velocity="1"/>
             </IfThenElse>
             <Control ID="InterruptableSequence">
-                <Fallback>
+                <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
-                    <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="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
+                <Action ID="RobotMove" target="{target}"/>
-                    <Action ID="RobotMove" target="{target}"/>
+                <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
-                    <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="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
+                <Action ID="RobotMove" target="{target}"/>
-                    <Action ID="RobotMove" target="{target}"/>
-                </Fallback>
             </Control>
         </ReactiveSequence>
     </BehaviorTree>