diff --git a/mg_bt/behavior_trees/mg_bt.btproj b/mg_bt/behavior_trees/mg_bt.btproj
index 80c4e2f..f23638b 100644
--- a/mg_bt/behavior_trees/mg_bt.btproj
+++ b/mg_bt/behavior_trees/mg_bt.btproj
@@ -1,16 +1,23 @@
+
-
+
- Service name
+ Service name
+
+
+
+
+
+ Action server name
- Action server name
+ Action server name
@@ -20,7 +27,7 @@
-
+
@@ -29,7 +36,7 @@
-
+
@@ -38,10 +45,19 @@
- Action server name
+ Action server name
+
+
+
+ Topic name
+
+
+
+
+
- Service name
+ Service name
diff --git a/mg_bt/behavior_trees/tactics.xml b/mg_bt/behavior_trees/tactics.xml
new file mode 100644
index 0000000..1bc7dc7
--- /dev/null
+++ b/mg_bt/behavior_trees/tactics.xml
@@ -0,0 +1,61 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
+
+ Topic name
+
+
+
+
+
+
+
+
+
diff --git a/mg_bt/behavior_trees/tactics_blue.xml b/mg_bt/behavior_trees/tactics_blue.xml
new file mode 100644
index 0000000..624c260
--- /dev/null
+++ b/mg_bt/behavior_trees/tactics_blue.xml
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+ Topic name
+
+
+
+
diff --git a/mg_bt/src/mg_tree_executor.cpp b/mg_bt/src/mg_tree_executor.cpp
index b81a453..7e1712d 100644
--- a/mg_bt/src/mg_tree_executor.cpp
+++ b/mg_bt/src/mg_tree_executor.cpp
@@ -2,6 +2,7 @@
#include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp"
+#include "tree_nodes/choose_tactic.hpp"
#include "tree_nodes/i2c.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/rotate.hpp"
@@ -30,6 +31,7 @@ namespace mg {
factory.registerNodeType("ZeroNode", node());
factory.registerNodeType("I2CSignal", node());
factory.registerNodeType("SideObstaclePub", node());
+ factory.registerNodeType("TacticChooser");
factory.registerNodeType("CalibWidth", node(), [this]() { return this->position(); });
}
diff --git a/mg_bt/src/tree_nodes/choose_tactic.hpp b/mg_bt/src/tree_nodes/choose_tactic.hpp
new file mode 100644
index 0000000..4c11ae0
--- /dev/null
+++ b/mg_bt/src/tree_nodes/choose_tactic.hpp
@@ -0,0 +1,22 @@
+#include "behaviortree_cpp/action_node.h"
+
+namespace mg {
+
+ class TacticsChooser : public BT::SyncActionNode {
+ public:
+ TacticsChooser(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("number"), BT::OutputPort("tactic"), BT::OutputPort("IsBlue") };
+ }
+
+ // You must override the virtual function tick()
+ BT::NodeStatus tick() override {
+ const int num = getInput("number").value();
+ setOutput("tactic", (num + 1) / 2);
+ setOutput("IsBlue", num % 2);
+ return BT::NodeStatus::SUCCESS;
+ }
+ };
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/i2c.hpp b/mg_bt/src/tree_nodes/i2c.hpp
index 05f4f13..53cee56 100644
--- a/mg_bt/src/tree_nodes/i2c.hpp
+++ b/mg_bt/src/tree_nodes/i2c.hpp
@@ -1,13 +1,13 @@
#pragma once
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "mg_msgs/srv/i2c.hpp"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/i2c.hpp"
namespace mg {
- class I2cNode : public BT::RosServiceNode {
+ class I2cNode : public BT::RosActionNode {
public:
I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
- BT::RosServiceNode(name, conf, params) { }
+ BT::RosActionNode(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort("Address", 42, {}),
@@ -15,14 +15,14 @@ namespace mg {
BT::OutputPort("Result") });
}
- bool setRequest(typename Request::SharedPtr& req) override {
- req->addr = getInput("Address").value_or(42);
- req->data = getInput("Result").value_or(0);
+ bool setGoal(Goal& req) override {
+ req.addr = getInput("Address").value_or(42);
+ req.data = getInput("Data").value_or(0);
return true;
}
- BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
- setOutput("Result", resp->resp.front());
+ BT::NodeStatus onResultReceived(const WrappedResult& resp) override {
+ setOutput("Result", resp.result->resp.front());
return BT::NodeStatus::SUCCESS;
}
};
diff --git a/mg_obstacles/obstacles/yellow-json.json b/mg_obstacles/obstacles/yellow-side.json
similarity index 100%
rename from mg_obstacles/obstacles/yellow-json.json
rename to mg_obstacles/obstacles/yellow-side.json
diff --git a/mg_obstacles/src/mg_obstacles.cpp b/mg_obstacles/src/mg_obstacles.cpp
index 04c3da1..d371810 100644
--- a/mg_obstacles/src/mg_obstacles.cpp
+++ b/mg_obstacles/src/mg_obstacles.cpp
@@ -28,7 +28,7 @@ namespace mg {
"/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts);
side_sub_ = node_->create_subscription(
- "/side", rclcpp::QoS(1).keep_last(1).transient_local(), [this](std_msgs::msg::String::ConstSharedPtr msg) {
+ "/side", rclcpp::QoS(1).keep_last(1), [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);
@@ -38,7 +38,7 @@ namespace mg {
});
std::string obstacle_pkg;
- std::string file_suffix = "/obstacles/obstacles.json";
+ std::string file_suffix = "/obstacles/yellow-side.json";
node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
node_->get_parameter_or("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
ulong n = obstacle_pkg.find_first_of('/');