From 07cc05685366a5bb6bc012ac6f3e6385f83408ac Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 7 May 2025 21:06:34 +0200 Subject: [PATCH] Add behavior for i2cnode and choosing side --- mg_bt/behavior_trees/mg_bt.btproj | 30 ++++++--- mg_bt/behavior_trees/tactics.xml | 61 +++++++++++++++++++ mg_bt/behavior_trees/tactics_blue.xml | 19 ++++++ mg_bt/src/mg_tree_executor.cpp | 2 + mg_bt/src/tree_nodes/choose_tactic.hpp | 22 +++++++ mg_bt/src/tree_nodes/i2c.hpp | 18 +++--- .../{yellow-json.json => yellow-side.json} | 0 mg_obstacles/src/mg_obstacles.cpp | 4 +- 8 files changed, 138 insertions(+), 18 deletions(-) create mode 100644 mg_bt/behavior_trees/tactics.xml create mode 100644 mg_bt/behavior_trees/tactics_blue.xml create mode 100644 mg_bt/src/tree_nodes/choose_tactic.hpp rename mg_obstacles/obstacles/{yellow-json.json => yellow-side.json} (100%) 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('/');