Initial Commit

.gitignore

@@ -0,0 +1,3 @@
+build/
+install/
+log/

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "ext/BehaviorTree.ROS2"]
+	path = ext/BehaviorTree.ROS2
+	url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git

.vscode/c_cpp_properties.json

@@ -0,0 +1,25 @@
+{
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "${default}",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "includePath": [
+                "/opt/ros/jazzy/include/**",
+                "${workspaceFolder}/ext/**/src",
+                "${workspaceFolder}/ext/**/**/**",
+                "${workspaceFolder}/install/btcpp_ros2_interfaces/include/btcpp_ros2_interfaces",
+                "${workspaceFolder}/install/mg_msgs/include/mg_msgs",
+                "/usr/include/glm",
+                "${workspaceFolder}/mg_bt/include"
+            ],
+            "name": "ROS",
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/g++",
+            "cStandard": "gnu11",
+            "cppStandard": "c++17"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,88 @@
+{
+    "files.associations": {
+        "*.embeddedhtml": "html",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "cfenv": "cpp",
+        "charconv": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "deque": "cpp",
+        "forward_list": "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",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "format": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "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"
+    },
+    "cmake.ignoreCMakeListsMissing": true
+}

mg_bt/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_bt)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+
+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(behaviortree_cpp REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
+find_package(btcpp_ros2_interfaces REQUIRED)
+find_package(mg_msgs REQUIRED)
+
+set(PACKAGE_DEPS
+  rclcpp
+  behaviortree_cpp
+  behaviortree_ros2
+  btcpp_ros2_interfaces
+  mg_msgs
+)
+
+
+add_executable(mg_bt_executor 
+  src/mg_bt_executor.cpp
+  src/MoveStraightBehavior.cpp)
+ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
+
+target_include_directories(
+  mg_bt_executor
+  PRIVATE
+  include
+)
+
+install( TARGETS
+  mg_bt_executor
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  behavior_tree_nodes
+  behavior_trees
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+
+ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces)
+
+ament_package()

mg_bt/behavior_tree_nodes/nodes.xml

@@ -0,0 +1,16 @@
+<root BTCPP_format="4">
+    <TreeNodesModel>
+        <Action ID="MoveStraightAction">
+            <input_port name="distance" type="double"/>
+            <input_port name="tolerance" type="double" default="0.100000"/>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
+        <Action ID="SimpleBehavior">
+            <input_port name="message" type="std::string"/>
+        </Action>
+        <Action ID="SleepAction">
+            <input_port name="msec" type="unsigned int"/>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

mg_bt/behavior_trees/mg_bt.btproj

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Testing">
+    <include path="untitled_1.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+        <Action ID="MoveStraightAction">
+            <input_port name="distance" type="double"/>
+            <input_port name="tolerance" default="0.100000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="SimpleBehavior">
+            <input_port name="message" type="std::string"/>
+        </Action>
+        <Action ID="SleepAction">
+            <input_port name="msec" type="unsigned int"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

mg_bt/behavior_trees/untitled_1.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4"
+      main_tree_to_execute="Main_strat">
+  <BehaviorTree ID="Main_strat">
+    <Repeat num_cycles="25">
+      <Sequence>
+        <MoveStraightAction distance="5"
+                            tolerance="0.001"
+                            action_name="/MoveStraight"/>
+        <Sleep msec="1000"/>
+        <SimpleBehavior message="YoYoYo Mr. White, what up?"/>
+        <MoveStraightAction distance="-5"
+                            tolerance="0.001"
+                            action_name="/MoveStraight"/>
+      </Sequence>
+    </Repeat>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="MoveStraightAction">
+      <input_port name="distance"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.100000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="SimpleBehavior">
+      <input_port name="message"
+                  type="std::string"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

mg_bt/config/mg_bt_executor_config.yaml

@@ -0,0 +1,14 @@
+bt_action_server:
+  ros__parameters:
+    action_name: "mg_bt_action_server" # Optional (defaults to `bt_action_server`)
+    tick_frequency: 100 # Optional (defaults to 100 Hz)
+    groot2_port: 8420 # Optional (defaults to 1667)
+    ros_plugins_timeout: 1000  # Optional (defaults 1000 ms)
+
+    plugins:
+      # - behaviortree_cpp/bt_plugins
+      - btcpp_ros2_samples/bt_plugins
+
+    behavior_trees:
+      - mg_bt/behavior_trees
+

mg_bt/include/mg_bt/MoveStraightBehavior.hpp

@@ -0,0 +1,30 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/move_straight.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+
+class MoveStraightAction : public BT::RosActionNode<mg_msgs::action::MoveStraight>
+{
+public:
+  MoveStraightAction(const std::string& name, const BT::NodeConfig& conf,
+              const BT::RosNodeParams& params)
+    : BT::RosActionNode<mg_msgs::action::MoveStraight>(name, conf, params)
+  {}
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({ 
+      BT::InputPort<double>("distance"),
+      BT::InputPort<double>("tolerance", 0.1, {}) 
+      });
+  }
+
+  bool setGoal(Goal& goal) override;
+
+  void onHalt() override;
+
+  BT::NodeStatus onResultReceived(const WrappedResult& wr) override;
+
+  virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override;
+};

mg_bt/launch/mg_bt_launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from pathlib import Path
+
+def generate_launch_description():
+
+    basedir = FindPackageShare("mg_bt")
+
+    yamlBT = PathJoinSubstitution([basedir, "config/mg_bt_executor_config.yaml"])
+
+    return LaunchDescription([
+        Node(
+            package='mg_bt',
+            executable='mg_bt_executor',
+            output="screen",
+            parameters=[yamlBT]
+        ),
+    ])
+    
+

mg_bt/package.xml

@@ -0,0 +1,24 @@
+<?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_bt</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>behaviortree_cpp</depend>
+  <depend>behaviortree_ros2</depend>
+  <depend>btcpp_ros2_interfaces</depend>
+  <depend>mg_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mg_bt/src/MoveStraightBehavior.cpp

@@ -0,0 +1,26 @@
+#include "mg_bt/MoveStraightBehavior.hpp"
+
+
+bool MoveStraightAction::setGoal(Goal& goal) {
+    auto dist = getInput<double>("distance");
+    auto tolerance = getInput<double>("tolerance");
+    goal.distance = dist.value();
+    goal.tolerance = tolerance.value();
+    return true;
+}
+
+BT::NodeStatus MoveStraightAction::onResultReceived(const RosActionNode::WrappedResult& wr)
+{
+  return (!wr.result->error) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus MoveStraightAction::onFailure(BT::ActionNodeErrorCode error)
+{
+  RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+  return BT::NodeStatus::FAILURE;
+}
+
+void MoveStraightAction::onHalt()
+{
+  RCLCPP_INFO(logger(), "%s: onHalt", name().c_str());
+}

mg_bt/src/mg_bt_executor.cpp

@@ -0,0 +1,95 @@
+#include <behaviortree_ros2/tree_execution_server.hpp>
+#include <behaviortree_cpp/loggers/bt_cout_logger.h>
+#include <behaviortree_cpp/xml_parsing.h>
+#include <behaviortree_cpp/bt_factory.h>
+
+
+#include <memory>
+#include <iostream>
+
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "mg_bt/MoveStraightBehavior.hpp"
+
+class MgSimpleBehavior : public BT::SyncActionNode {
+public:
+    MgSimpleBehavior( const std::string& name, const BT::NodeConfig& config ) 
+        : BT::SyncActionNode(name, config) {
+    }
+
+    static BT::PortsList providedPorts()
+    {
+        return { BT::InputPort<std::string>("message") };
+    }
+    BT::NodeStatus tick() override{
+
+        std::cout << "This is an example action called " << this->name() << std::endl;
+
+        auto msg = getInput<std::string>("message");
+
+        if(!msg) {
+            std::cout << "message was not provided" << std::endl;
+        }
+        else {
+            std::cout << "The message is " << msg.value() << std::endl;
+        }
+    
+        return BT::NodeStatus::SUCCESS;
+    }
+
+};
+
+
+class MgBtExecutionServer : public BT::TreeExecutionServer {
+public:
+    MgBtExecutionServer(const rclcpp::NodeOptions options) 
+        : TreeExecutionServer(options) {
+            executeRegistration();
+            std::cout <<  BT::writeTreeNodesModelXML(factory())  << std::endl;
+        }
+
+    void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override {
+        factory.registerNodeType<MgSimpleBehavior>("SimpleBehavior");
+        factory.registerNodeType<MoveStraightAction>("MoveStraightAction", node());
+
+    }
+
+    void onTreeCreated(BT::Tree&  tree) override {
+        logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
+    }
+
+    std::optional<std::string>
+    onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override {
+        (void)status;
+        logger_cout_.reset();
+
+        if (was_cancelled) {
+            std::cout << "The tree execution was cancled" << std::endl;
+        }
+
+        return  std::nullopt;
+    }
+
+private:
+    std::shared_ptr<BT::StdCoutLogger> logger_cout_;
+};
+
+int main(int argc, const char** argv) {
+    rclcpp::init(argc,argv);   
+
+
+    rclcpp::NodeOptions options;
+    auto bt_server = std::make_shared<MgBtExecutionServer>(options);
+
+    
+    rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
+
+
+    exec.add_node(bt_server->node());
+    exec.spin();
+    exec.remove_node(bt_server->node());
+
+    rclcpp::shutdown();
+    return 0;
+}

mg_msgs/CMakeLists.txt

@@ -0,0 +1,30 @@
+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"
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

mg_msgs/action/MoveStraight.action

@@ -0,0 +1,12 @@
+float64 distance
+float64 tolerance 0.005
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+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>TODO: Package description</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
+  <license>TODO: License declaration</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,61 @@
+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)
+
+include(FindPkgConfig)
+pkg_search_module(LIBGLM REQUIRED glm)
+
+
+
+set(PACKAGE_DEPS
+  rclcpp
+  mg_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+)
+
+add_executable(mg_move_server src/mg_move_server.cpp)
+ament_target_dependencies(mg_move_server ${PACKAGE_DEPS})
+
+target_include_directories(
+  mg_move_server
+  PRIVATE
+  ${LIBGLM_INCLUDE_DIRS}
+  include
+)
+
+target_link_libraries(
+  mg_move_server
+  ${LIBGLM_LIBRARIES}
+)
+
+install( TARGETS
+  mg_move_server
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

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">petar</maintainer>
+  <license>TODO: License declaration</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>
+  <build_depend>libglm-dev</build_depend>
+
+  <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/src/mg_move_server.cpp

@@ -0,0 +1,237 @@
+#include <functional>
+#include <memory>
+#include <thread>
+#include <string>
+#include <cstring>
+#include <glm/glm.hpp>
+#include <glm/gtc/matrix_transform.hpp>
+#include <glm/gtx/norm.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+
+
+#include "mg_msgs/action/move_straight.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2/LinearMath/Vector3.h"
+#include "tf2/LinearMath/Quaternion.h"
+
+namespace mg {
+
+namespace Geometry = geometry_msgs::msg;
+
+
+class MoveServer : public rclcpp::Node {
+public:
+    using MoveAction = mg_msgs::action::MoveStraight;
+    using MoveGoalHandle = rclcpp_action::ServerGoalHandle<MoveAction>;
+
+
+    MoveServer(const rclcpp::NodeOptions options)
+        : rclcpp::Node("MgMoveServer",options)
+    {
+
+        std::cout << "something works?" << std::endl;
+        tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+        tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+        using namespace std::placeholders;
+        command_publisher_ = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
+
+        move_action_server_  = rclcpp_action::create_server<MoveAction>(
+            this,
+            "MoveStraight",
+            std::bind(&MoveServer::handle_goal, this, _1, _2),
+            std::bind(&MoveServer::handle_cancel, this, _1),
+            std::bind(&MoveServer::handle_accepted, this, _1)
+        );
+        RCLCPP_INFO(this->get_logger(), "Registered Server");
+    }
+
+private:
+
+
+    std::shared_ptr<rclcpp_action::Server<MoveAction> > move_action_server_;
+    std::shared_ptr<rclcpp::Publisher<Geometry::TwistStamped> > command_publisher_;
+
+
+    std::shared_ptr<rclcpp::Subscription<Geometry::TransformStamped> > tf2_subscription_;
+    std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+    std::mutex point_mtx_;
+    glm::dvec2 point2d_;
+    double theta_;
+    
+    std::mutex processing_mtx_;
+    bool is_processing_goal_ = false;
+
+    void process_tf2() {
+
+
+        if(tf2_buffer_->canTransform("odom", "base-link", rclcpp::Time(0))) {
+
+            auto tf2_frame = tf2_buffer_->lookupTransform("odom", "base-link", rclcpp::Time(0)).transform;
+
+            tf2::Quaternion quaternion;
+            quaternion.setW(tf2_frame.rotation.w);
+            quaternion.setX(tf2_frame.rotation.x);
+            quaternion.setY(tf2_frame.rotation.y);
+            quaternion.setZ(tf2_frame.rotation.z);
+
+            double theta,ro,phi;
+
+            tf2::Matrix3x3(quaternion).getRPY(ro,phi,theta);
+            point_mtx_.lock();
+            point2d_ = {tf2_frame.translation.x,tf2_frame.translation.y};
+
+            point_mtx_.unlock();
+
+            RCLCPP_INFO(this->get_logger(), "Got a tf2: x = %lf; y = %lf; theta = %lf", point2d_.x, point2d_.y, theta_);
+        }
+    }
+
+    rclcpp_action::GoalResponse handle_goal(
+        const rclcpp_action::GoalUUID& uuid,
+        std::shared_ptr<const MoveAction::Goal>
+    ) {
+        RCLCPP_INFO(this->get_logger(), "RECIEVED GOAL");
+        (void)uuid;
+
+        std::lock_guard<std::mutex> lock(processing_mtx_);
+
+        if(is_processing_goal_) {
+            return rclcpp_action::GoalResponse::REJECT;
+        } else {
+            is_processing_goal_ = true;
+            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+        }
+    }
+    
+    rclcpp_action::CancelResponse handle_cancel(
+        const std::shared_ptr<MoveGoalHandle> goal_handle
+    ) {
+        (void)goal_handle;
+        RCLCPP_INFO(this->get_logger(), "We will now canel the move request");
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void handle_accepted(const std::shared_ptr<MoveGoalHandle> goal_handle) {
+        RCLCPP_INFO(this->get_logger(),"GOAL_ACCEPTED");
+        using namespace std::placeholders;
+        auto t = std::bind(&MoveServer::execute_move, this, _1);
+        std::thread{t,goal_handle}.detach();
+    }
+
+    Geometry::TwistStamped to_twist(double v, double w) {
+        auto twist = Geometry::TwistStamped();
+        twist.twist.angular.z = w;
+        twist.twist.linear.x = v;
+        return twist;
+    }
+
+    void execute_move(const std::shared_ptr<MoveGoalHandle> goal_handle) {
+
+        auto goal = goal_handle->get_goal();
+
+        double target_distance = goal->distance;
+        double tolerance = goal->tolerance;
+
+        double increments = 0.01;
+
+        glm::dvec2 c_point, t_point;
+        process_tf2();
+        double c_theta;
+        {
+            std::lock_guard _lock(point_mtx_);
+            c_point = point2d_;
+            c_theta = theta_;
+        }
+
+        glm::dmat2 rotmat = glm::dmat2(glm::rotate(glm::dmat4(1), c_theta, glm::dvec3(0,0,1)));
+
+        RCLCPP_INFO(this->get_logger(), "Rotmat: %lf;%lf;%lf;%lf;", rotmat[0].x,rotmat[0].y,rotmat[1].x,rotmat[1].y);
+        glm::dvec2 dir(1,0);
+
+        dir = rotmat * dir; 
+
+        t_point = c_point + dir*target_distance;
+
+        RCLCPP_INFO(this->get_logger(), "Target set to %lf;%lf", t_point.x,t_point.y);
+        int speed_multi=0;
+            
+        rclcpp::Rate rate(100);
+        while(glm::length2(c_point-t_point) >= tolerance * tolerance
+            && rclcpp::ok()){
+            
+            if(goal_handle->is_canceling()) {
+                auto x = std::make_shared<MoveAction::Result>();
+                x->error = MoveAction::Result::UNKNOWN;
+                command_publisher_->publish(to_twist(0,0));
+                is_processing_goal_ = false;
+                goal_handle->canceled(x); 
+                return;
+            }
+
+            double best_score = -INFINITY;
+            int i_b = 0;
+            for(int i = -2; i<3 ;i++){
+                double lookahead = glm::abs(speed_multi+i);
+                glm::dvec2 d_point = c_point + dir * ((increments * (speed_multi+i))*lookahead/100.0);
+
+                RCLCPP_INFO(this->get_logger(), "The  lookahead is: %lf. I will be multiplying that with %lf", lookahead, (increments * (speed_multi+i)));
+                RCLCPP_INFO(this->get_logger(), "The estimated position is x: %lf y: %lf", d_point.x, d_point.y);
+                RCLCPP_INFO(this->get_logger(), "The score is %lf",score(d_point, c_point, t_point) );
+
+                if(score(d_point, c_point, t_point) > best_score) {
+                    best_score = score(d_point, c_point, t_point); 
+                    i_b = i; 
+                }
+            }
+
+            speed_multi += i_b;
+
+            command_publisher_->publish(to_twist(increments * speed_multi,0.0));
+
+            RCLCPP_INFO(this->get_logger(), "Would've sent the following %lf", increments * speed_multi);
+
+
+            
+            process_tf2();
+            {
+                std::lock_guard _lock(point_mtx_);
+                c_point = point2d_;
+                c_theta = theta_;
+            }
+
+            rate.sleep();
+        }
+        if(rclcpp::ok()) {
+            auto x = std::make_shared<MoveAction::Result>();
+            x->error = MoveAction::Result::SUCCESS;
+            is_processing_goal_ = false;
+            goal_handle->succeed(x);
+        }
+    }
+
+    double score(glm::dvec2 point,  glm::dvec2 c_point, glm::dvec2 t_point) {
+        return glm::length2(t_point-c_point) - glm::length2(t_point-point);
+    }
+};
+
+
+};
+
+int main(int argc, const char** argv) {
+    rclcpp::init(argc,argv);   
+    std::cout << "something works?" << std::endl;
+    rclcpp::NodeOptions options;
+    auto mg_move_server = std::make_shared<mg::MoveServer>(options);
+    rclcpp::spin(mg_move_server);
+    rclcpp::shutdown();
+    return 0;
+}