From 315ec77812802e263891d25181bfdb09dd57ba56 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Mon, 5 May 2025 15:04:42 +0200 Subject: [PATCH 1/4] Added simple node fore i2c comunication --- mg_bt/CMakeLists.txt | 3 +++ mg_bt/i2cmodule/i2cnode.cpp | 35 +++++++++++++++++++++++++++++++++++ mg_bt/package.xml | 1 + mg_msgs/CMakeLists.txt | 1 + mg_msgs/srv/I2c.srv | 4 ++++ 5 files changed, 44 insertions(+) create mode 100644 mg_bt/i2cmodule/i2cnode.cpp create mode 100644 mg_msgs/srv/I2c.srv diff --git a/mg_bt/CMakeLists.txt b/mg_bt/CMakeLists.txt index 837a473..a684a21 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 @@ -46,9 +47,11 @@ target_include_directories( ) 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..398cf2a --- /dev/null +++ b/mg_bt/i2cmodule/i2cnode.cpp @@ -0,0 +1,35 @@ +#include "rclcpp/rclcpp.hpp" +#include "mg_msgs/srv/i2c.hpp" + +#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 + int ch = i2c_smbus_read_byte_data(i2c_fd_, req->data); + 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); + + 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_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 -- 2.49.0 From af79f4eb8185caded4de2fe87ac5619f4c2cabd1 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Mon, 5 May 2025 18:11:17 +0200 Subject: [PATCH 2/4] Properly link libi2c to i2cnode --- mg_bt/CMakeLists.txt | 2 ++ mg_bt/i2cmodule/i2cnode.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/mg_bt/CMakeLists.txt b/mg_bt/CMakeLists.txt index a684a21..9a9932d 100644 --- a/mg_bt/CMakeLists.txt +++ b/mg_bt/CMakeLists.txt @@ -46,6 +46,8 @@ 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) diff --git a/mg_bt/i2cmodule/i2cnode.cpp b/mg_bt/i2cmodule/i2cnode.cpp index 398cf2a..1c9f65c 100644 --- a/mg_bt/i2cmodule/i2cnode.cpp +++ b/mg_bt/i2cmodule/i2cnode.cpp @@ -1,10 +1,13 @@ #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; @@ -30,6 +33,8 @@ class MgI2c : public rclcpp::Node { 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 -- 2.49.0 From 07c4aefa07c7c109b4dc1e8fdc445d1c6a258a71 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Mon, 5 May 2025 18:43:09 +0200 Subject: [PATCH 3/4] Add behavior tree node for i2c --- mg_bt/src/mg_tree_executor.cpp | 2 ++ mg_bt/src/tree_nodes/i2c.hpp | 29 +++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) create mode 100644 mg_bt/src/tree_nodes/i2c.hpp 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 -- 2.49.0 From abf571728649e81d95c403bbf6de3cf84491b038 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Tue, 6 May 2025 12:49:21 +0200 Subject: [PATCH 4/4] Changed service to wait for proper response from i2c --- mg_bt/i2cmodule/i2cnode.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/mg_bt/i2cmodule/i2cnode.cpp b/mg_bt/i2cmodule/i2cnode.cpp index 1c9f65c..6d3e333 100644 --- a/mg_bt/i2cmodule/i2cnode.cpp +++ b/mg_bt/i2cmodule/i2cnode.cpp @@ -8,7 +8,6 @@ extern "C" { #include #include } - class MgI2c : public rclcpp::Node { using I2cSrv = mg_msgs::srv::I2c; @@ -21,7 +20,15 @@ class MgI2c : public rclcpp::Node { void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const { ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT - int ch = i2c_smbus_read_byte_data(i2c_fd_, req->data); + 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()); } -- 2.49.0