Implented point to point DWM action server

.vscode/c_cpp_properties.json

@@ -0,0 +1,17 @@
+{
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "${default}",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "compileCommands": "${workspaceFolder}/build/compile_commands.json",
+            "name": "ROS",
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/g++",
+            "cStandard": "gnu11",
+            "cppStandard": "c++17"
+        }
+    ],
+    "version": 4
+}

.vscode/launch.json

@@ -0,0 +1,17 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "C++ Debugger",
+            "request": "launch",
+            "type": "cppdbg",
+            "miDebuggerServerAddress": "localhost:3000",
+            "miDebuggerPath": "/usr/bin/gdb",
+            "cwd": "${workspaceFolder}",
+            "program": "${workspaceFolder}/install/mg_navigation/lib/mg_navigation/mg_nav_server"
+        }
+    ]
+}

.vscode/settings.json

@@ -0,0 +1,86 @@
+{
+    "files.associations": {
+        "bitset": "cpp",
+        "forward_list": "cpp",
+        "fstream": "cpp",
+        "ostream": "cpp",
+        "tuple": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstddef": "cpp",
+        "cstring": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "cfenv": "cpp",
+        "charconv": "cpp",
+        "chrono": "cpp",
+        "clocale": "cpp",
+        "codecvt": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdarg": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "expected": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "source_location": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "format": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ranges": "cpp",
+        "semaphore": "cpp",
+        "shared_mutex": "cpp",
+        "span": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stdfloat": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp"
+    }
+}

colcon_defaults.yaml

@@ -0,0 +1,6 @@
+{
+    "build": {
+        "cmake-args": ["-DCMAKE_EXPORT_COMPILE_COMMANDS=1"],
+        "symlink-install": true
+    }
+}

mg_msgs/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Point2d.msg"
+  "action/MoveStraight.action"
+  "action/MovePoint.action"
+  "action/LookAt.action"
+)
+
+ament_package()

mg_msgs/action/LookAt.action

@@ -0,0 +1,25 @@
+float64 x
+float64 y
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/action/MovePoint.action

@@ -0,0 +1,25 @@
+float64 x 
+float64 y 
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/action/MoveStraight.action

@@ -0,0 +1,24 @@
+float64 distance
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/msg/Point2d.msg

@@ -0,0 +1,2 @@
+float64 x
+float64 y

mg_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mg_msgs</name>
+  <version>0.0.0</version>
+  <description>This contains various msg/action used within the project</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mg_navigation/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_navigation)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(mg_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include(FindPkgConfig)
+pkg_search_module(LIBGLM REQUIRED glm)
+
+
+
+set(PACKAGE_DEPS
+  rclcpp
+  mg_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+add_executable(mg_nav_server 
+  src/mg_navigation.cpp
+)
+ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})
+
+target_include_directories(
+  mg_nav_server
+  PRIVATE
+  ${LIBGLM_INCLUDE_DIRS}
+  include
+)
+
+target_link_libraries(
+  mg_nav_server
+  ${LIBGLM_LIBRARIES}
+)
+
+install( TARGETS
+  mg_nav_server
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

mg_navigation/include/handlers/dwm.hpp

@@ -0,0 +1,6 @@
+#pragma once
+
+#include "handlers/dwm_core.hpp"
+#include "handlers/dwm_point.hpp"
+#include "handlers/dwm_forward.hpp"
+#include "handlers/dwm_lookat.hpp"

mg_navigation/include/handlers/dwm_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 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

mg_navigation/include/handlers/dwm_forward.hpp

@@ -0,0 +1,57 @@
+#pragma once
+#include "handlers/dwm_core.hpp"
+
+#include "glm/gtx/norm.hpp"
+#include "glm/gtx/rotate_vector.hpp"
+#include "glm/gtx/vector_angle.hpp"
+
+namespace mg {
+
+    template <>
+    inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
+        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
+        target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
+    }
+
+    template <>
+    inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
+        constexpr double delta = 0.8;
+
+        auto [v, w] = to_vel(step * vl, step * vr);
+
+        const glm::dvec2 dp    = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
+        const glm::dvec2 n_pos = dp + pos;
+
+        const double score = goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
+
+        return score;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
+                                                                           std::vector<int>& vr,
+                                                                           int               dimx,
+                                                                           int) {
+        vl.clear();
+        vr.clear();
+
+        for (int i = -dimx / 2; i <= dimx / 2; i++) {
+            auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));
+
+            if (step * abs(cvl + i) <= goal->max_wheel_speed && glm::abs(v) < goal->max_vel) {
+                vl.push_back(cvl + i);
+                vr.push_back(cvr + i);
+            }
+        }
+    }
+
+}

mg_navigation/include/handlers/dwm_lookat.hpp

@@ -0,0 +1,83 @@
+#pragma once
+#include "handlers/dwm_core.hpp"
+
+#include "glm/gtx/norm.hpp"
+#include "glm/gtx/rotate_vector.hpp"
+#include "glm/gtx/vector_angle.hpp"
+#include <glm/gtc/constants.hpp>
+
+namespace mg {
+
+    template <>
+    inline bool DWM<MgNavigationServer::LookAt>::target_check() {
+
+        const double a = glm::abs(theta - target_ornt);
+        const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
+
+        return b > goal->tolerance;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::LookAt>::target_init() {
+
+        target_pos = glm::vec2(goal->x, goal->y);
+
+        const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
+        const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
+        const double sign  = (cross < 0) ? -1 : 1;
+
+        target_ornt = angle * sign;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::LookAt>::target_update() {
+
+        const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
+        const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
+        const double sign  = (cross < 0) ? -1 : 1;
+
+        target_ornt = angle * sign;
+    }
+
+    template <>
+    inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWM<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
+        constexpr double delta = 0.5;
+        const auto [v, w]      = to_vel(step * vl, step * vr);
+        const double n_theta   = theta + w * delta;
+
+        double dist_old = glm::abs(target_ornt - theta);
+        double dist_new = glm::abs(target_ornt - n_theta);
+
+        dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
+        dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
+
+        const double score = goal->ornt_mult * (dist_old - dist_new);
+
+        return score;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
+                                                                     std::vector<int>& vr,
+                                                                     int               dimx,
+                                                                     int) {
+        vl.clear();
+        vr.clear();
+
+        for (int i = -dimx / 2; i <= dimx / 2; i++) {
+            auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));
+
+            if (step * abs(cvl - i) <= goal->max_wheel_speed && step * abs(cvr + i) <= goal->max_wheel_speed
+                && glm::abs(w) < goal->max_angular) {
+                vl.push_back(cvl - i);
+                vr.push_back(cvr + i);
+            }
+        }
+    }
+
+}

mg_navigation/include/handlers/dwm_point.hpp

@@ -0,0 +1,80 @@
+#pragma once
+#include "handlers/dwm_core.hpp"
+
+#include "glm/gtx/norm.hpp"
+#include "glm/gtx/rotate_vector.hpp"
+#include "glm/gtx/vector_angle.hpp"
+
+namespace mg {
+
+    template <>
+    inline bool DWM<MgNavigationServer::MovePoint>::target_check() {
+        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::MovePoint>::target_update() {
+        target_pos = glm::dvec2(goal->x, goal->y);
+    }
+
+    template <>
+    inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWM<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
+        auto [v, w]        = to_vel(step * vl, step * vr);
+        const double delta = 0.5;
+        glm::dvec2   n_pos;
+        double       n_theta = NAN;
+        double       score   = 0;
+
+        n_theta = w * delta;
+
+        if (n_theta <= 1e-6) { //NOLINT
+            n_theta += theta;
+            const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
+            n_pos               = dp + pos;
+        } else {
+            n_theta += theta;
+            const double r = v / w;
+            n_pos.x        = r * (glm::sin(n_theta) - glm::sin(theta));
+            n_pos.y        = -r * (glm::cos(n_theta) - glm::cos(theta));
+            n_pos += pos;
+        }
+
+        const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
+        const glm::dvec2 face   = glm::rotate(glm::dvec2(1, 0), theta);
+
+        const double angl   = glm::angle(face, glm::normalize(target_pos - pos));
+        const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
+
+        score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
+        score += goal->ornt_mult * (angl - n_angl);
+
+        return score;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
+                                                                        std::vector<int>& vr,
+                                                                        int               dimx,
+                                                                        int               dimy) {
+        vl.clear();
+        vr.clear();
+
+        for (int i = -dimx / 2; i <= dimx / 2; i++) {
+            for (int j = -dimy / 2; j <= dimy / 2; j++) {
+                auto [v, w] = to_vel(step * (cvl + i), step * (cvr + j));
+
+                if (step * abs(cvl + i) <= goal->max_wheel_speed && step * abs(cvr + j) <= goal->max_wheel_speed
+                    && glm::abs(v) < goal->max_vel && glm::abs(w) < goal->max_angular) {
+                    vl.push_back(cvl + i);
+                    vr.push_back(cvr + j);
+                }
+            }
+        }
+    }
+
+}

mg_navigation/include/mg_navigation.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "mg_msgs/action/move_point.hpp"
+#include "mg_msgs/action/move_straight.hpp"
+#include "mg_msgs/action/look_at.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+
+namespace mg {
+
+    namespace Geometry = geometry_msgs::msg;
+
+    class MgNavigationServer : public rclcpp::Node {
+      public:
+        RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
+        using MovePoint    = mg_msgs::action::MovePoint;
+        using MoveStraight = mg_msgs::action::MoveStraight;
+        using LookAt       = mg_msgs::action::LookAt;
+
+        rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
+
+        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
+        rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
+        rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
+
+        rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
+        tf2_ros::Buffer::SharedPtr                                  tf2_buffer;
+        std::shared_ptr<tf2_ros::TransformListener>                 tf2_listener;
+        std::mutex                                                  mtx;
+
+        MgNavigationServer(rclcpp::NodeOptions& opts);
+
+      private:
+        bool is_processing = false;
+
+        template <typename T>
+        rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
+                                                typename T::Goal::ConstSharedPtr,
+                                                const char*);
+        template <typename T>
+        rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >,
+                                                    const char*);
+        template <typename T>
+        void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char*);
+
+        template <typename T>
+        void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >);
+    };
+
+};

mg_navigation/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mg_navigation</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  <depend>mg_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  
+
+  <build_depend>libglm-dev</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mg_navigation/src/mg_navigation.cpp

@@ -0,0 +1,86 @@
+#include <iostream>
+#include <functional>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "mg_navigation.hpp"
+#include "handlers/dwm.hpp"
+
+namespace mg {
+
+    template <typename T>
+    rclcpp_action::GoalResponse MgNavigationServer::handle_goal(const rclcpp_action::GoalUUID& /*unused*/,
+                                                                typename T::Goal::ConstSharedPtr /*unused*/,
+                                                                const char* N) {
+        RCLCPP_INFO(get_logger(), "Recieved goal for action: [%s]", N);
+        const std::lock_guard<std::mutex> lock(mtx);
+        if (is_processing) {
+            return rclcpp_action::GoalResponse::REJECT;
+        } else {
+            is_processing = true;
+            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+        }
+    }
+
+    template <typename T>
+    rclcpp_action::CancelResponse MgNavigationServer::handle_cancel(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char* N) {
+        RCLCPP_INFO(this->get_logger(), "Recieved cancel request for action: [%s]", N);
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    template <typename T>
+    void MgNavigationServer::handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh,
+                                             const char*                                                N) {
+        RCLCPP_INFO(this->get_logger(), "Goal accepted for action: [%s]", N);
+        using namespace std::placeholders;
+        auto t = std::bind(&MgNavigationServer::execute<T>, this, _1);
+        std::thread{ t, gh }.detach();
+    }
+
+    template <typename T>
+    void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
+        DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
+        dwm.execute();
+        is_processing = false;
+        mtx.unlock();
+    }
+
+    MgNavigationServer::MgNavigationServer(rclcpp::NodeOptions& opts) : rclcpp::Node("MgNavigationServer", opts) {
+        tf2_buffer   = std::make_shared<tf2_ros::Buffer>(get_clock());
+        tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);
+
+        pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
+
+        using namespace std::placeholders;
+        sv_move_point = rclcpp_action::create_server<MovePoint>(
+          this,
+          "MovePoint",
+          std::bind(&MgNavigationServer::handle_goal<MovePoint>, this, _1, _2, "MovePoint"),
+          std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
+          std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));
+
+        sv_look_at = rclcpp_action::create_server<LookAt>(
+          this,
+          "LookAt",
+          std::bind(&MgNavigationServer::handle_goal<LookAt>, this, _1, _2, "LookAt"),
+          std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
+          std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));
+
+        sv_move_straight = rclcpp_action::create_server<MoveStraight>(
+          this,
+          "MoveStraight",
+          std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
+    }
+
+};
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::NodeOptions opts;
+    rclcpp::spin(std::make_shared<mg::MgNavigationServer>(opts));
+    rclcpp::shutdown();
+    return 0;
+}