Switched i2c to action instead of service

mg_bt/i2cmodule/i2cnode.cpp

@@ -1,5 +1,7 @@
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
 #include "mg_msgs/srv/i2c.hpp"
+#include "mg_msgs/action/i2c.hpp"
 
 extern "C" {
 
@@ -9,13 +11,20 @@ extern "C" {
 #include <i2c/smbus.h>
 }
 class MgI2c : public rclcpp::Node {
-    using I2cSrv = mg_msgs::srv::I2c;
+    using I2cSrv        = mg_msgs::srv::I2c;
+    using I2cAction     = mg_msgs::action::I2c;
+    using GoalHandleI2c = rclcpp_action::ServerGoalHandle<I2cAction>;
 
   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<I2cSrv>("/i2c", cb);
+        using namespace std::placeholders;
+        i2c_action_ = rclcpp_action::create_server<I2cAction>(this,
+                                                              "/i2c_action",
+                                                              bind(&MgI2c::handle_goal, this, _1, _2),
+                                                              bind(&MgI2c::handle_cancel, this, _1),
+                                                              bind(&MgI2c::handle_accepted, this, _1));
     }
 
     void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
@@ -33,9 +42,41 @@ class MgI2c : public rclcpp::Node {
         RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front());
     }
 
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
+                                            std::shared_ptr<const I2cAction::Goal> goal) {
+        RCLCPP_INFO(this->get_logger(), "Received goal request with addr %d and data %d", goal->addr, goal->data);
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleI2c>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void handle_accepted(const std::shared_ptr<GoalHandleI2c> goal_handle) {
+        using namespace std::placeholders;
+        ioctl(i2c_fd_, I2C_SLAVE, goal_handle->get_goal()->addr); // NOLINT
+        i2c_smbus_write_byte(i2c_fd_, goal_handle->get_goal()->data);
+        int ch = 0;
+
+        rclcpp::Rate rate(100);
+
+        while (ch == 0 || (ch > 255 || ch < 0)) {
+            ch = i2c_smbus_read_byte(i2c_fd_);
+            rate.sleep();
+        }
+        auto res = std::make_shared<I2cAction::Result>();
+        RCLCPP_INFO(get_logger(), "Recieved %d", ch);
+        res->resp.push_back(ch);
+        goal_handle->succeed(res);
+        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    }
+
   private:
     rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
-    int                                           i2c_fd_;
+
+    rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
+
+    int i2c_fd_;
 };
 
 int main(int argc, const char* const* argv) {

mg_msgs/CMakeLists.txt

@@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/MovePoint.action"
   "action/LookAt.action"
   "action/Rotate.action"
+  "action/I2c.action"
   "srv/CalcPath.srv"
   "srv/SendDouble.srv"
   "srv/I2c.srv"