29 lines
1.1 KiB
C++
29 lines
1.1 KiB
C++
#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;
|
|
}
|
|
};
|
|
} |