Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Add behavior tree node for i2c

Browse Source
This commit is contained in:
Pimpest
2025-05-05 18:43:09 +02:00
parent af79f4eb81
commit 07c4aefa07
2 changed files with 31 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mg_bt/src/mg_tree_executor.cpp
View File

@ -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<mg::MovePointNode>("MovePoint", node());
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

29
mg_bt/src/tree_nodes/i2c.hpp Normal file
View File

@ -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<mg_msgs::srv::I2c> {
public:
I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosServiceNode<mg_msgs::srv::I2c>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
BT::InputPort<int>("Data", 0, {}),
BT::OutputPort<int>("Result") });
}
bool setRequest(typename Request::SharedPtr& req) override {
req->addr = getInput<int>("Address").value_or(42);
req->data = getInput<int>("Result").value_or(0);
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
setOutput<int>("Result", resp->resp.front());
return BT::NodeStatus::SUCCESS;
}
};
}