Added behavior that publishes tactic id

mg_bt/src/mg_tree_executor.cpp

@@ -5,6 +5,7 @@
 #include "tree_nodes/i2c.hpp"
 #include "tree_nodes/move_point.hpp"
 #include "tree_nodes/rotate.hpp"
+#include "tree_nodes/side_pub.hpp"
 #include "tree_nodes/zero.hpp"
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -28,6 +29,7 @@ namespace mg {
         factory.registerNodeType<mg::RotateNode>("RotateNode", node());
         factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
         factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
+        factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
         factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
     }
 

mg_bt/src/tree_nodes/side_pub.hpp

@@ -0,0 +1,22 @@
+#include "behaviortree_ros2/bt_topic_pub_node.hpp"
+#include "std_msgs/msg/string.hpp"
+
+namespace mg {
+
+    class SidePub : public BT::RosTopicPubNode<std_msgs::msg::String> {
+      public:
+        SidePub(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosTopicPubNode<std_msgs::msg::String>(name, conf, params) { }
+
+        static BT::PortsList providedPorts() { return providedBasicPorts({ BT::InputPort<int>("tactic") }); }
+
+        bool setMessage(std_msgs::msg::String& msg) override {
+            if (getInput<int>("tactic").has_value()) {
+                msg.data = (getInput<int>("tactic").value() % 2 == 0) ? "/blue-side.json" : "/yellow-side.json";
+                return true;
+            } else {
+                return false;
+            }
+        }
+    };
+}

mg_obstacles/include/mg_obstacles/mg_obstacles.hpp

@@ -6,6 +6,7 @@
 #include "mg_obstacles/static_obstacle.hpp"
 
 #include "std_msgs/msg/u_int64.hpp"
+#include "std_msgs/msg/string.hpp"
 #include "geometry_msgs/msg/point_stamped.hpp"
 
 #include "tf2_ros/buffer.h"
@@ -51,9 +52,13 @@ namespace mg {
 
         rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
 
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr side_sub_;
+
         rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
         rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
 
+        std::string base_path;
+
         void load_permanent(Json::Value& json);
         void load_static(Json::Value& json);
     };

mg_obstacles/obstacles/blue-side.json

@@ -0,0 +1,94 @@
+{
+  "staticObstacleHeight": 0.4,
+  "staticObstacleWidth": 0.1,
+  "staticObstacles": [
+    {
+      "horizontal": true,
+      "x": 0.625,
+      "y": 1.775
+    },
+    {
+      "horizontal": false,
+      "x": 0,
+      "y": 0.6
+    },
+    {
+      "horizontal": false,
+      "x": 0,
+      "y": 1.525
+    },
+    {
+      "horizontal": true,
+      "x": 0.575,
+      "y": 0.3
+    },
+    {
+      "horizontal": true,
+      "x": 2.025,
+      "y": 0.3
+    },
+    {
+      "horizontal": false,
+      "x": 2.9,
+      "y": 0.6
+    },
+    {
+      "horizontal": false,
+      "x": 2.9,
+      "y": 1.525
+    },
+    {
+      "horizontal": true,
+      "x": 1.975,
+      "y": 1.775
+    },
+    {
+      "horizontal": true,
+      "x": 0.9,
+      "y": 1
+    },
+    {
+      "horizontal": true,
+      "x": 1.7,
+      "y": 1
+    }
+  ],
+    "obstacles": [
+        {
+            "height": 0.45,
+            "width": 1.95,
+            "x": 0,
+            "y": 2
+        },
+        {
+            "height": 0.45,
+            "width": 0.45,
+            "x": 1,
+            "y": 0.45
+        },
+        {
+            "height": 0.2,
+            "width": 0.4,
+            "x": 1.95,
+            "y": 2
+        },
+        {
+            "height": 0.15,
+            "width": 0.45,
+            "x": 0.55,
+            "y": 0.15
+        },
+        {
+            "height": 0.15,
+            "width": 0.45,
+            "x": 2.55,
+            "y": 0.15
+        },
+        {
+            "height": 0.45,
+            "width": 0.45,
+            "x": 2.55,
+            "y": 1.1
+        }
+    ]
+}

mg_obstacles/src/mg_obstacles.cpp

@@ -27,23 +27,33 @@ namespace mg {
         dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
           "/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts);
 
+        side_sub_ = node_->create_subscription<std_msgs::msg::String>(
+          "/side", rclcpp::QoS(1).keep_last(1).transient_local(), [this](std_msgs::msg::String::ConstSharedPtr msg) {
+              Json::Value json;
+              RCLCPP_INFO(node_->get_logger(), "Read file %s", (base_path + "/" + msg->data).c_str());
+              std::ifstream json_document(base_path + "/" + msg->data);
+              json_document >> json;
+              load_permanent(json);
+              load_static(json);
+          });
+
         std::string obstacle_pkg;
         std::string file_suffix = "/obstacles/obstacles.json";
         node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
-        node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json");
+        node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
         ulong n = obstacle_pkg.find_first_of('/');
         if (n < obstacle_pkg.size()) {
             file_suffix  = obstacle_pkg.substr(n);
             obstacle_pkg = obstacle_pkg.substr(0, n);
         }
+        base_path = ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix;
         Json::Value json;
         RCLCPP_INFO(node_->get_logger(),
                     "Read file %s",
                     (ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str());
-        std::ifstream json_document(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix);
+        std::ifstream json_document(base_path + "/yellow-side.json");
 
         json_document >> json;
-
         load_permanent(json);
         load_static(json);
     }
@@ -183,6 +193,7 @@ namespace mg {
     }
 
     void ObstacleManager::load_permanent(Json::Value& json) {
+        permanent_obstacles_.clear();
 
         Json::Value& obstacles = json["obstacles"];
 
@@ -193,6 +204,7 @@ namespace mg {
     }
 
     void ObstacleManager::load_static(Json::Value& json) {
+        static_obstacles_.clear();
 
         StaticObstacle::width  = json["staticObstacleWidth"].asDouble();
         StaticObstacle::height = json["staticObstacleHeight"].asDouble();