diff --git a/mg_bt/i2cmodule/i2cnode.cpp b/mg_bt/i2cmodule/i2cnode.cpp index 3cf0d52..9c1b7c4 100644 --- a/mg_bt/i2cmodule/i2cnode.cpp +++ b/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 } 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; 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); + using namespace std::placeholders; + i2c_action_ = rclcpp_action::create_server(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 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) { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr 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(); + 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::SharedPtr i2c_srv_; - int i2c_fd_; + + rclcpp_action::Server::SharedPtr i2c_action_; + + int i2c_fd_; }; int main(int argc, const char* const* argv) { diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index baf9be1..0a4fe97 100644 --- a/mg_msgs/CMakeLists.txt +++ b/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"