88 lines
3.2 KiB
C++
88 lines
3.2 KiB
C++
#include "rclcpp/rclcpp.hpp"
|
|
#include "rclcpp_action/rclcpp_action.hpp"
|
|
#include "mg_msgs/srv/i2c.hpp"
|
|
#include "mg_msgs/action/i2c.hpp"
|
|
|
|
extern "C" {
|
|
|
|
#include <fcntl.h>
|
|
#include <linux/i2c-dev.h>
|
|
#include <sys/ioctl.h>
|
|
#include <i2c/smbus.h>
|
|
}
|
|
class MgI2c : public rclcpp::Node {
|
|
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); };
|
|
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 {
|
|
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 > 255 || 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());
|
|
}
|
|
|
|
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_;
|
|
|
|
rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
|
|
|
|
int i2c_fd_;
|
|
};
|
|
|
|
int main(int argc, const char* const* argv) {
|
|
rclcpp::init(argc, argv);
|
|
rclcpp::spin(std::make_shared<MgI2c>("i2cu"));
|
|
rclcpp::shutdown();
|
|
|
|
return 0;
|
|
} |