diff --git a/mg_bt/src/mg_tree_executor.cpp b/mg_bt/src/mg_tree_executor.cpp index 95edb52..24d41d5 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/i2c.hpp" #include "tree_nodes/move_point.hpp" #include "tree_nodes/rotate.hpp" #include "tree_nodes/zero.hpp" @@ -26,6 +27,7 @@ namespace mg { factory.registerNodeType("MovePoint", node()); factory.registerNodeType("RotateNode", node()); factory.registerNodeType("ZeroNode", node()); + factory.registerNodeType("I2CSignal", node()); factory.registerNodeType("CalibWidth", node(), [this]() { return this->position(); }); } diff --git a/mg_bt/src/tree_nodes/i2c.hpp b/mg_bt/src/tree_nodes/i2c.hpp new file mode 100644 index 0000000..05f4f13 --- /dev/null +++ b/mg_bt/src/tree_nodes/i2c.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include "behaviortree_ros2/bt_service_node.hpp" +#include "mg_msgs/srv/i2c.hpp" + +namespace mg { + class I2cNode : public BT::RosServiceNode { + public: + I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : + BT::RosServiceNode(name, conf, params) { } + + static BT::PortsList providedPorts() { + return providedBasicPorts({ BT::InputPort("Address", 42, {}), + BT::InputPort("Data", 0, {}), + BT::OutputPort("Result") }); + } + + bool setRequest(typename Request::SharedPtr& req) override { + req->addr = getInput("Address").value_or(42); + req->data = getInput("Result").value_or(0); + return true; + } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override { + setOutput("Result", resp->resp.front()); + return BT::NodeStatus::SUCCESS; + } + }; +} \ No newline at end of file