First set of behaviors

mg_bt/src/tree_nodes/choose_can.hpp

@@ -0,0 +1,49 @@
+
+#include "behaviortree_cpp/action_node.h"
+
+namespace mg {
+
+    class CanChooser : public BT::SyncActionNode {
+        struct cans {
+            double x;
+            double y;
+            int    orientation;
+        };
+
+      public:
+        CanChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }
+
+        static BT::PortsList providedPorts() {
+            // This action has a single input port called "message"
+            return { BT::InputPort<std::string>("canlist"),
+                     BT::OutputPort<double>("x_loc"),
+                     BT::OutputPort<double>("y_loc"),
+                     BT::OutputPort<int>("orient") };
+        }
+
+        // You must override the virtual function tick()
+        BT::NodeStatus tick() override {
+            constexpr std::array<cans, 10> c{ { { 0.825, 1.425, 3 },
+                                                { 0.35, 0.4, 2 },
+                                                { 0.35, 1.325, 2 },
+                                                { 0.775, 0.55, 1 },
+                                                { 2.225, 0.55, 1 },
+                                                { 2.65, 0.4, 0 },
+                                                { 2.65, 0.4, 0 },
+                                                { 2.175, 1.425, 3 },
+                                                { 1.1, 0.65, 3 },
+                                                { 1.9, 0.65, 3 } } };
+
+            int idx = 0;
+            // Todo iterate through all cans and use a passthrough function that returns current can state
+            std::istringstream ss(getInput<std::string>("canlist").value());
+            ss >> idx;
+
+            setOutput<double>("x_loc", c[idx].x);
+            setOutput<double>("y_loc", c[idx].y);
+            setOutput<int>("orient", c[idx].orientation);
+
+            return BT::NodeStatus::SUCCESS;
+        }
+    };
+}