Implented point to point DWM action server

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> >);
+    };
+
+};