magrob/mg_navigation/include/handlers/dwm_core.hpp

#pragma once

#include "glm/glm.hpp"
#include "mg_navigation.hpp"

#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
#include <sys/types.h>

#ifndef WHEEL_SEP
#define WHEEL_SEP 0.258
#endif

#ifndef WHEEL_RAD
#define WHEEL_RAD ((0.075 / 2))
#endif

#ifndef UPDATE_RATE
#define UPDATE_RATE 100
#endif

#ifndef DIM_X
#define DIM_X 8
#endif

#ifndef DIM_Y
#define DIM_Y 8
#endif

namespace mg {

    template <typename T>
    class DWM {
      public:
        using Goal       = typename T::Goal;
        using Result     = typename T::Result;
        using Feedback   = typename T::Feedback;
        using GoalHandle = typename rclcpp_action::ServerGoalHandle<T>;
        using TwistPub   = rclcpp::Publisher<Geometry::TwistStamped>;

        enum DwmError {
            SUCCESS,
            BLOCKED,
            TIMEOUT,
            MISALIGNED,

            UNKNOWN = 254
        };

        MgNavigationServer&           baseNode; // NOLINT
        std::shared_ptr<GoalHandle>   hgoal;
        TwistPub::SharedPtr           pub_twist;
        typename Goal::ConstSharedPtr goal;
        tf2_ros::Buffer::SharedPtr    tfbuf;
        ulong                         dimx = DIM_X, dimy = DIM_Y;
        int                           cvl = 0, cvr = 0;
        double                        step;

        glm::dvec2 target_pos;
        double     target_ornt = 0;

        glm::dvec2 pos;
        double     theta = 0;

        DWM(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);

        void execute();

      protected:
        void   target_init() { target_update(); }
        bool   target_check() { return false; }
        void   target_update() { }
        bool   condition_check();
        void   populate_candidates(std::vector<int>& vl, std::vector<int>& vr, int dimx, int dimy);
        double calc_score(int vl, int vr);

        void abort(int error);
        void succeed();
        void cancel();

        void pos_query();
        void send_speed(double vl, double vr);

        static std::pair<double, double> to_vel(double vl, double vr) {
            return std::make_pair(WHEEL_RAD * (vl + vr) / 2.0, // NOLINT
                                  WHEEL_RAD * (vr - vl) / WHEEL_SEP);
        }
    };

    template <typename T>
    DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
      baseNode(mns),
      hgoal(g),
      pub_twist(mns.pub_twist),
      goal(g->get_goal()),
      tfbuf(buf),
      step(g->get_goal()->step),
      target_pos(0, 0),
      pos(0, 0) { }

    template <typename T>
    void DWM<T>::execute() {
        std::vector<int> spacevl(dimx * dimy);
        std::vector<int> spacevr(dimx * dimy);

        pos_query();
        target_init();

        rclcpp::Rate rate(UPDATE_RATE);

        while (target_check() && rclcpp::ok()) {
            if (hgoal->is_canceling()) {
                cancel();
                return;
            }

            if (condition_check()) {
                return;
            }

            pos_query();
            populate_candidates(spacevl, spacevr, dimx, dimy);

            double b_score = calc_score(spacevl[0], spacevr[0]);
            uint   b_ind   = 0;

            for (uint i = 1; i < spacevl.size(); i++) {
                const double score = calc_score(spacevl[i], spacevr[i]);
                if (score > b_score) {
                    b_score = score;
                    b_ind   = i;
                }
            }

            calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs

            cvl = spacevl[b_ind];
            cvr = spacevr[b_ind];
            send_speed(step * cvl, step * cvr);

            rate.sleep();
        }
        succeed();
    }

    template <typename T>
    void DWM<T>::pos_query() {
        try {
            double x = NAN;
            double y = NAN;

            auto tmsg = tfbuf->lookupTransform("odom", "base-link", tf2::TimePointZero);

            tf2::Transform t;
            tf2::convert(tmsg.transform, t);
            t.getBasis().getRPY(x, y, theta);
            auto vec3 = tmsg.transform.translation;

            pos = glm::dvec2(vec3.x, vec3.y);

        } catch (const tf2::TransformException& e) {
            RCLCPP_ERROR(baseNode.get_logger(), "Got following error when looking up transform:\n\t%s", e.what());
        }
    }

    template <typename T>
    void DWM<T>::send_speed(double vl, double vr) {
        auto [v, w] = to_vel(vl, vr);
        Geometry::TwistStamped twist;
        twist.twist.angular.z = w;
        twist.twist.linear.x  = v;

        pub_twist->publish(twist);
    }

    template <typename T>
    void DWM<T>::abort(int error) {
        baseNode.mtx.lock();
        send_speed(0, 0);
        auto x   = std::make_shared<Result>();
        x->error = error;
        hgoal->abort(x);
    }

    template <typename T>
    void DWM<T>::succeed() {
        baseNode.mtx.lock();
        send_speed(0, 0);
        auto x   = std::make_shared<Result>();
        x->error = SUCCESS;
        hgoal->succeed(x);
    }

    template <typename T>
    void DWM<T>::cancel() {
        baseNode.mtx.lock();
        send_speed(0, 0);
        auto x   = std::make_shared<Result>();
        x->error = UNKNOWN;
        hgoal->canceled(x);
    }

}; // namespace mg