magrob/mg_odometry/src/mg_odom_publisher.cpp

#include <array>
#include <chrono>
#include <cstddef>
#include <functional>
#include <memory>
#include <rclcpp/service.hpp>
#include <string>
#include <libserial/SerialStream.h>
#include <sys/types.h>

#include "mg_msgs/srv/send_double.hpp"
#include "mg_msgs/srv/send_pose2d.hpp"
#include "std_srvs/srv/empty.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Transform.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2/convert.hpp"

#include "rclcpp/rclcpp.hpp"

#define TIMEOUT 10u

constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";

using SendDoubleSrv = mg_msgs::srv::SendDouble;
using SendPoseSrv   = mg_msgs::srv::SendPose2d;
using ZeroSrv       = std_srvs::srv::Empty;

class MgOdomPublisher : public rclcpp::Node {
  public:
    MgOdomPublisher() : Node("mg_odom_publisher") {
        this->declare_parameter("odom", "odom");
        this->declare_parameter("target", "base-link");
        this->declare_parameter("serial_path", ENCODER_SERIAL_PATH_DEFAULT);

        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
                                         std::bind(&MgOdomPublisher::timer_callback, this));

        set_width_service_ = create_service<SendDoubleSrv>(
          "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
        set_ratio_service_ = create_service<SendDoubleSrv>(
          "/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
        set_pose_service_ = create_service<SendPoseSrv>(
          "/set_pose", std::bind(&MgOdomPublisher::set_pose, this, std::placeholders::_1));

        zero_service_
          = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));

        calib_service_
          = create_service<ZeroSrv>("/endCalib", std::bind(&MgOdomPublisher::calib, this, std::placeholders::_1));
    }

  private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_width_service_;
    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_ratio_service_;
    rclcpp::Service<SendPoseSrv>::SharedPtr        set_pose_service_;
    rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
    rclcpp::Service<ZeroSrv>::SharedPtr            calib_service_;
    rclcpp::TimerBase::SharedPtr                   timer_;
    LibSerial::SerialStream                        enc_serial_port_;

    void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);

        if (enc_serial_port_.IsOpen()) {
            union {
                std::array<u_char, sizeof(double)> bytes;
                double                             data;
            } value{};
            value.data = request->data;

            enc_serial_port_ << "w;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
        }
    }

    void set_pose(const std::shared_ptr<SendPoseSrv::Request> request) {
        RCLCPP_INFO(get_logger(), "Setting pose to: %lf %lf %lf", request->x, request->y, request->theta);

        if (enc_serial_port_.IsOpen()) {
            union {
                std::array<u_char, sizeof(double)> bytes;
                double                             data;
            } value{};
            value.data = request->x;

            enc_serial_port_ << "X;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }

            value.data = request->y;

            enc_serial_port_ << "Y;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }

            value.data = request->theta;

            enc_serial_port_ << "T;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
        }
    }

    void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
        RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);

        if (enc_serial_port_.IsOpen()) {
            union {
                std::array<u_char, sizeof(double)> bytes;
                double                             data;
            } value{};
            value.data = request->data;

            enc_serial_port_ << "r;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
        }
    }

    void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
        RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib");

        if (enc_serial_port_.IsOpen()) {
            enc_serial_port_ << "c;";
            double left_gain  = 0;
            double right_gain = 0;
            enc_serial_port_ >> left_gain >> right_gain;
            RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain);
        }
    }

    void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
        RCLCPP_INFO(get_logger(), "Zeroing odometry");

        if (enc_serial_port_.IsOpen()) {
            enc_serial_port_ << "z;";
        }
    }

    void timer_callback() {
        double _x     = NAN;
        double _y     = NAN;
        double _theta = NAN;

        RCLCPP_DEBUG(this->get_logger(), "Callback called");

        try {
            if (!enc_serial_port_.IsOpen()) {
                enc_serial_port_.Open(this->get_parameter("serial_path").as_string());
            }

            enc_serial_port_ << "g;";

            if (!(enc_serial_port_ >> _x >> _y >> _theta)) {
                RCLCPP_INFO(this->get_logger(), "Reading Serial Port failed. Closing...");
                throw "Unfortunately bug with libserial";
            }

            RCLCPP_DEBUG(this->get_logger(), "Got following from rpi:{ x: %lf, y: %lf, z: %lf}\n", _x, _y, _theta);

            make_transform(_x, _y, _theta);
        } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "%s\n", e.what()); }
    }

    void make_transform(double x, double y, double theta) {
        geometry_msgs::msg::TransformStamped t;

        t.header.stamp    = this->get_clock()->now();
        t.header.frame_id = this->get_parameter("odom").as_string();
        t.child_frame_id  = this->get_parameter("target").as_string();

        tf2::Quaternion q;
        q.setRPY(0, 0, theta);
        const tf2::Transform tf(tf2::Transform(q, tf2::Vector3(x, y, 0)));
        tf2::convert(tf, t.transform);

        tf_broadcaster_->sendTransform(t);
    }
};

int main(const int argc, const char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MgOdomPublisher>());
    rclcpp::shutdown();

    return 0;
}