diff --git a/mg_bt/CMakeLists.txt b/mg_bt/CMakeLists.txt index 837a473..9a9932d 100644 --- a/mg_bt/CMakeLists.txt +++ b/mg_bt/CMakeLists.txt @@ -37,6 +37,7 @@ set(SOURCES add_executable(mg_bt_executor ${SOURCES}) +add_executable(mg_i2cnode i2cmodule/i2cnode.cpp) target_include_directories( mg_bt_executor @@ -45,10 +46,14 @@ target_include_directories( include ) +target_link_libraries(mg_i2cnode i2c) + ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS}) +ament_target_dependencies(mg_i2cnode rclcpp mg_msgs) install( TARGETS mg_bt_executor + mg_i2cnode DESTINATION lib/${PROJECT_NAME} ) diff --git a/mg_bt/i2cmodule/i2cnode.cpp b/mg_bt/i2cmodule/i2cnode.cpp new file mode 100644 index 0000000..6d3e333 --- /dev/null +++ b/mg_bt/i2cmodule/i2cnode.cpp @@ -0,0 +1,47 @@ +#include "rclcpp/rclcpp.hpp" +#include "mg_msgs/srv/i2c.hpp" + +extern "C" { + +#include +#include +#include +#include +} +class MgI2c : public rclcpp::Node { + using I2cSrv = mg_msgs::srv::I2c; + + public: + MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT + auto cb + = [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); }; + i2c_srv_ = create_service("/i2c", cb); + } + + void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const { + ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT + i2c_smbus_write_byte(i2c_fd_, req->data); + int ch = 0; + + rclcpp::Rate rate(100); + + while (ch == 0 || (ch < 256 && ch > 0)) { + ch = i2c_smbus_read_byte(i2c_fd_); + rate.sleep(); + } + resp->resp.push_back(ch); + RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front()); + } + + private: + rclcpp::Service::SharedPtr i2c_srv_; + int i2c_fd_; +}; + +int main(int argc, const char* const* argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("i2cu")); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/mg_bt/package.xml b/mg_bt/package.xml index 0c34014..c3161ab 100644 --- a/mg_bt/package.xml +++ b/mg_bt/package.xml @@ -14,6 +14,7 @@ behaviortree_ros2 btcpp_ros2_interfaces mg_msgs + libi2c-dev ament_lint_auto ament_lint_common 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 diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index d062b16..baf9be1 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Rotate.action" "srv/CalcPath.srv" "srv/SendDouble.srv" + "srv/I2c.srv" ) ament_package() \ No newline at end of file diff --git a/mg_msgs/srv/I2c.srv b/mg_msgs/srv/I2c.srv new file mode 100644 index 0000000..7ef0a5b --- /dev/null +++ b/mg_msgs/srv/I2c.srv @@ -0,0 +1,4 @@ +uint8 addr +uint8 data +--- +uint8[] resp \ No newline at end of file