DWM -> DWA (Dynamic window approach)

mg_navigation/include/handlers/dwa_core.hpp

@@ -0,0 +1,203 @@
+#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 DWA {
+      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;
+
+        DWA(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>
+    DWA<T>::DWA(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 DWA<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 DWA<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 DWA<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 DWA<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 DWA<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 DWA<T>::cancel() {
+        baseNode.mtx.lock();
+        send_speed(0, 0);
+        auto x   = std::make_shared<Result>();
+        x->error = UNKNOWN;
+        hgoal->canceled(x);
+    }
+
+}; // namespace mg