Added dwm rotate

Added rotate action
Added Zero and SetWidth services
2025-02-26 16:29:20 +01:00 · 2025-02-26 16:26:35 +01:00 · 2025-02-26 02:07:30 +01:00 · 2025-02-26 02:07:07 +01:00 · 2025-02-26 02:06:50 +01:00 · 2025-02-25 19:17:42 +01:00
84 changed files with 75 additions and 3715 deletions
--- a/.clang-tidy
+++ b/.clang-tidy
@ -25,10 +25,6 @@ Checks: "
        -cppcoreguidelines-pro-type-union-access,
        -cppcoreguidelines-macro-usage,
        -performance-unnecessary-value-param,
-        -cppcoreguidelines-pro-bounds-constant-array-index,
-        -readability-implicit-bool-conversion,
-        -cppcoreguidelines-avoid-magic-numbers,
-        -readability-magic-numbers,
    "
 WarningsAsErrors: ''
 HeaderFilterRegex: ''
--- a/.gitmodules
+++ b/.gitmodules
@ -1,3 +0,0 @@
-[submodule "ext/BehaviorTreeRos2"]
-	path = ext/BehaviorTreeRos2
-	url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
--- a/ext/BehaviorTreeRos2
+++ b/ext/BehaviorTreeRos2
--- a/firmware/COLCON_IGNORE
+++ b/firmware/COLCON_IGNORE
--- a/firmware/README.md
+++ b/firmware/README.md
@ -1,6 +0,0 @@
-# Magrob firmware
-
-This directory contains the firmware used for our robot.
-
-### Base
-This code was meant to be used for the wheel base 
--- a/firmware/base/CMakeLists.txt
+++ b/firmware/base/CMakeLists.txt
@ -1,50 +0,0 @@
-cmake_minimum_required(VERSION 3.12)
-
-set(PICO_BOARD pico CACHE STRING "Board type")
-set(TOP ${PICO_TINYUSB_PATH})
-include(pico_sdk_import.cmake)    
-
-set(CMAKE_C_STANDARD 11)
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
-
-project(wheelbase C CXX ASM)
-
-pico_sdk_init()
-
-
-add_executable(wheelbase
-    src/quadrature.c
-    src/main.c 
-    src/stepper.c
-    tusb/tusb_descriptors.c
-    )
-
-pico_set_program_name(wheelbase "wheelbase")
-pico_set_program_version(wheelbase "0.1")
-
-pico_enable_stdio_uart(wheelbase 0)
-pico_enable_stdio_usb(wheelbase 1)
-
-pico_generate_pio_header(wheelbase ${CMAKE_CURRENT_LIST_DIR}/pio/quadrature.pio)
-pico_generate_pio_header(wheelbase ${CMAKE_CURRENT_LIST_DIR}/pio/blink.pio)
-
-target_link_libraries(wheelbase
-    pico_stdlib
-    pico_stdio
-    pico_time
-    pico_multicore
-    hardware_pio
-    hardware_clocks
-    hardware_gpio
-    hardware_sync
-    tinyusb_board
-    tinyusb_device)
-
-target_include_directories(wheelbase PRIVATE
-  ${CMAKE_CURRENT_LIST_DIR}
-  src/
-  tusb/
-)
-
-pico_add_extra_outputs(wheelbase)
--- a/firmware/base/pico_sdk_import.cmake
+++ b/firmware/base/pico_sdk_import.cmake
@ -1,121 +0,0 @@
-# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
-
-# This can be dropped into an external project to help locate this SDK
-# It should be include()ed prior to project()
-
-# Copyright 2020 (c) 2020 Raspberry Pi (Trading) Ltd.
-#
-# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
-# following conditions are met:
-#
-# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
-# disclaimer.
-#
-# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other materials provided with the distribution.
-#
-# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products
-# derived from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
-# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
-# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
-# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
-    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
-    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
-    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
-    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
-    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
-    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
-    set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
-    message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
-endif ()
-
-if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
-  set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
-  message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
-endif()
-
-set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
-set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
-set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
-set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
-
-if (NOT PICO_SDK_PATH)
-    if (PICO_SDK_FETCH_FROM_GIT)
-        include(FetchContent)
-        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
-        if (PICO_SDK_FETCH_FROM_GIT_PATH)
-            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
-        endif ()
-        FetchContent_Declare(
-                pico_sdk
-                GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
-        )
-
-        if (NOT pico_sdk)
-            message("Downloading Raspberry Pi Pico SDK")
-            # GIT_SUBMODULES_RECURSE was added in 3.17
-            if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
-                FetchContent_Populate(
-                        pico_sdk
-                        QUIET
-                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
-                        GIT_SUBMODULES_RECURSE FALSE
-
-                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
-                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
-                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
-                )
-            else ()
-                FetchContent_Populate(
-                        pico_sdk
-                        QUIET
-                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
-
-                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
-                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
-                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
-                )
-            endif ()
-
-            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
-        endif ()
-        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
-    else ()
-        message(FATAL_ERROR
-                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
-                )
-    endif ()
-endif ()
-
-get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
-if (NOT EXISTS ${PICO_SDK_PATH})
-    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
-endif ()
-
-set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
-if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
-    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
-endif ()
-
-set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
-
-include(${PICO_SDK_INIT_CMAKE_FILE})
--- a/firmware/base/pio/blink.pio
+++ b/firmware/base/pio/blink.pio
@ -1,31 +0,0 @@
-.program oscillate
-pause:
-    pull block
-    out x,32
-.wrap_target
-    pull noblock
-    out x, 32
-    mov y, x
-    set pins, 1   ; Pin ON
-    jmp y-- lp1   ; Delay for (x + 1) cycles, x is a 32 bit number
-    jmp pause
-lp1:
-    jmp y-- lp1   ; Delay for (x + 1) cycles, x is a 32 bit number
-    mov y, x
-    set pins, 0 [2]  ; Pin OFF
-lp2:
-    jmp y-- lp2   ; Delay for the same number of cycles again
-.wrap             ; Blink forever!
-
-
-% c-sdk {
-// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin
-
-void oscillate_program_init(PIO pio, uint sm, uint offset, uint pin) {
-   pio_gpio_init(pio, pin);
-   pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
-   pio_sm_config c = oscillate_program_get_default_config(offset);
-   sm_config_set_set_pins(&c, pin, 1);
-   pio_sm_init(pio, sm, offset, &c);
-}
-%}
--- a/firmware/base/pio/quadrature.pio
+++ b/firmware/base/pio/quadrature.pio
@ -1,208 +0,0 @@
-;
-; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
-;
-; SPDX-License-Identifier: BSD-3-Clause
-;
-; quadrature_encoder_substep: reads a quadrature encoder with no CPU
-; intervention and provides the current position on request.
-;
-; the "substep" version uses not only the step counts, but also the timing of 
-; the steps to compute the current speed. See README.md for details
-
-
-.program quadrature_encoder_substep
-
-.origin 0
-
-; the PIO code counts steps like the standard quadrature encoder code, but also
-; keeps track of the time passed since the last transition. That allows the C
-; code to build a good estimate of a fractional step position based on the
-; latest speed and time passed
-;
-; since it needs to push two values, it only pushes new data when the FIFO has
-; enough space to hold both values. Otherwise it could either stall or go out
-; of sync
-;
-; because we need to count the time passed, all loops must take the same number
-; of cycles and there are delays added to the fastest branches to make sure it
-; always takes 13 cycles per loop (e.g., sysclk 133MHz, max step rate = ~10.2
-; Msteps/sec)
-
-	; push the step count and transition clock count to the RX FIFO (using
-	; auto push). This is reached by the "MOV PC, ~STATUS" instruction when
-	; status is all 1 (meaning fifo has space for this push). It also may
-	; execute once at program start, but that has little effect
-	IN X, 32
-	IN Y, 32
-
-update_state:
-	; build the state by using 2 bits from the negated previous state of the
-	; pins and the new 2 bit state of the pins
-	OUT ISR, 2
-	IN PINS, 2
-	MOV OSR, ~ISR
-	; use the jump table to update the step count accordingly
-	MOV PC, OSR
-
-decrement:
-	; decrement the step count
-	JMP Y--, decrement_cont
-decrement_cont:
-	; when decrementing, X is set to 2^31, when incrementing it is set to
-	; zero. That way the C code can infer in which direction the last
-	; transition was taken and how long ago
-	SET X, 1
-	MOV X, ::X
-	; after incrementing or decrementing, continue to "check_fifo"
-check_fifo:
-.wrap_target
-	; on each iteration we decrement X to count the number of loops since
-	; the last transition
-	JMP X--, check_fifo_cont
-check_fifo_cont:
-	; push data or continue, depending on the state of the fifo
-	MOV PC, ~STATUS
-
-increment:
-	; the PIO does not have a increment instruction, so to do that we do a
-	; negate, decrement, negate sequence
-	MOV Y, ~Y
-	JMP Y--, increment_cont
-increment_cont:
-	MOV Y, ~Y
-	; reset X to zero when incrementing
-	SET X, 0
-	; wrap above to check the fifo state
-	.wrap
-
-invalid:
-	; this is just a placeholder to document what the code does on invalid
-	; transitions, where the two phases change at the same time. We don't do
-	; anything special, but the encoder should note generate these invalid
-	; transitions anyway
-	JMP update_state
-
-	; this jump table starts at address 16 and is accessed by the
-	; "MOV PC, OSR" instruction above, that loads the PC with the state on
-	; the lower 4 bits and the 5th bit on. The delays here extend the faster
-	; branches to take the same time as the slower branches
-	JMP invalid
-	JMP increment		[0]
-	JMP decrement		[1]
-	JMP check_fifo		[4]
-
-	JMP decrement		[1]
-	JMP invalid
-	JMP check_fifo		[4]
-	JMP increment		[0]
-
-	JMP increment		[0]
-	JMP check_fifo		[4]
-	JMP invalid
-	JMP decrement		[1]
-
-	JMP check_fifo		[4]
-	JMP decrement		[1]
-	JMP increment		[0]
-	; this instruction should be usually reached by the "MOV PC, ~STATUS"
-	; instruction above when the status is zero, which means that the fifo
-	; has data and we don't want to push more data. This can also be reached
-	; on an invalid state transition, which should not happen. Even if it 
-	; happens, it should be a transient state and the only side effect is
-	; that we'll call update_state twice in a row
-	JMP update_state	[1]
-
-
-
-
-
-% c-sdk {
-
-#include "hardware/clocks.h"
-#include "hardware/timer.h"
-#include "hardware/gpio.h"
-#include "hardware/sync.h"
-
-
-// "substep" version low-level interface
-//
-// note: user code should use the high level functions in quadrature_encoder.c
-// and not call these directly
-
-// initialize the PIO state and the substep_state_t structure that keeps track
-// of the encoder state
-static inline void quadrature_encoder_substep_program_init(PIO pio, uint sm, uint pin_A)
-{
-	uint pin_state, position, ints;
-	pio_gpio_init(pio, pin_A);
-	pio_gpio_init(pio, pin_A + 1);
-
-	pio_sm_set_consecutive_pindirs(pio, sm, pin_A, 2, false);
-	gpio_pull_up(pin_A);
-	gpio_pull_up(pin_A + 1);
-
-	pio_sm_config c = quadrature_encoder_substep_program_get_default_config(0);
-	sm_config_set_in_pins(&c, pin_A); // for WAIT, IN
-	// shift to left, auto-push at 32 bits
-	sm_config_set_in_shift(&c, false, true, 32);
-	sm_config_set_out_shift(&c, true, false, 32);
-	// don't join FIFO's
-	sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
-
-	// always run at sysclk, to have the maximum possible time resolution
-	sm_config_set_clkdiv(&c, 1.0);
-
-	pio_sm_init(pio, sm, 0, &c);
-
-	// set up status to be rx_fifo < 1
-	pio->sm[sm].execctrl = ((pio->sm[sm].execctrl & 0xFFFFFF80) | 0x12);
-
-	// init the state machine according to the current phase. Since we are
-	// setting the state running PIO instructions from C state, the encoder may
-	// step during this initialization. This should not be a problem though,
-	// because as long as it is just one step, the state machine will update
-	// correctly when it starts. We disable interrupts anyway, to be safe
-	ints = save_and_disable_interrupts();
-
-	pin_state = (gpio_get_all() >> pin_A) & 3;
-
-	// to setup the state machine, we need to set the lower 2 bits of OSR to be
-	// the negated pin state
-	pio_sm_exec(pio, sm, pio_encode_set(pio_y, ~pin_state));
-	pio_sm_exec(pio, sm, pio_encode_mov(pio_osr, pio_y));
-
-	// also set the Y (current step) so that the lower 2 bits of Y have a 1:1
-	// mapping to the current phase (input pin state). That simplifies the code
-	// to compensate for differences in encoder phase sizes:
-	switch (pin_state) {
-		case 0: position = 0; break;
-		case 1: position = 3; break;
-		case 2: position = 1; break;
-		case 3: position = 2; break;
-	} 
-	pio_sm_exec(pio, sm, pio_encode_set(pio_y, position));
-
-	pio_sm_set_enabled(pio, sm, true);
-	
-	restore_interrupts(ints);
-}
-
-static inline void quadrature_encoder_substep_get_counts(PIO pio, uint sm, uint *step, int *cycles, uint *us)
-{
-	int i, pairs;
-	uint ints;
-	
-	pairs = pio_sm_get_rx_fifo_level(pio, sm) >> 1;
-
-	// read all data with interrupts disabled, so that there can not be a
-	// big time gap between reading the PIO data and the current us
-	ints = save_and_disable_interrupts();
-	for (i = 0; i < pairs + 1; i++) {
-		*cycles = pio_sm_get_blocking(pio, sm);
-		*step = pio_sm_get_blocking(pio, sm);
-	}
-	*us = time_us_32();
-	restore_interrupts(ints);
-}
-
-%}
--- a/firmware/base/src/config.h
+++ b/firmware/base/src/config.h
@ -1,25 +0,0 @@
-#pragma once
-
-//############## CONFIG STEPPERS ################
-#define DIR_PIN_A   26
-#define DIR_PIN_B   16
-#define PULSE_PIN_A 27
-#define PULSE_PIN_B 17
-
-#define SM_A 0
-#define SM_B 1
-
-#define PULSE_PER_REV (1 / 3200.0)
-//###############################################
-
-//================ CONFIG ENCODERS =====================
-#define ENCODER_RIGHT_PIN_A 12 // Lupio sam ove vrednosti
-#define ENCODER_RIGHT_PIN_B 13
-#define ENCODER_LEFT_PIN_A  6
-#define ENCODER_LEFT_PIN_B  7
-#define ENCODER_CPR         3840
-
-#define WHEEL_RADIUS     0.0279
-#define WHEEL_SEPARATION 0.310735
-#define TIMER_CYCLE_US   1000
-//======================================================
--- a/firmware/base/src/main.c
+++ b/firmware/base/src/main.c
@ -1,270 +0,0 @@
-#include <stdio.h>
-#include <math.h>
-#include <string.h>
-
-#include "bsp/board_api.h"
-#include "tusb.h"
-
-
-#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
-#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
-#endif
-
-#include "pico/stdlib.h"
-#include "pico/multicore.h"
-#include "pico/time.h"
-#include "quadrature.h"
-#include "quadrature.pio.h"
-
-#include "config.h"
-#include "stepper.h"
-
-static substep_state_t state_l;
-static substep_state_t state_r;
-
-static double base_x = 0;
-static double base_y = 0;
-static double base_theta = 0;
-static double wheel_separation = WHEEL_SEPARATION;
-static double wheel_ratio_l = 1;
-static double wheel_ratio_r = 1;
-
-static uint prev_time;
-static int prev_position_l = 0;
-static int prev_position_r = 0;
-
-
-static unsigned char stepper_instr[2+sizeof(double)*2] = {};
-
-typedef struct instr_buf_t{
-    char pos;
-    char data[2+sizeof(double) * 2];
-} instr_buf_t;
-
-instr_buf_t stepper_buf = {};
-
-typedef struct calib_diff_t {
-    long long left_pulse;
-    long long right_pulse;
-    double left_gain;
-    double right_gain;
-} calib_diff_t;
-
-static calib_diff_t calib_enc = {
-    .left_gain = 0.997648,
-    .right_gain = 1.002333
-};
-
-bool update_pos_cb() {
-
-    substep_update(&state_l);
-    substep_update(&state_r);
-
-    int position_l= state_l.position;
-    int position_r= state_r.position;
-
-    const int diff_l = position_l - prev_position_l;
-    const int diff_r = position_r - prev_position_r;
-
-    calib_enc.left_pulse += diff_l;
-    calib_enc.right_pulse -= diff_r;
-
-    double vel_l = diff_l;
-    double vel_r = diff_r;
-
-    prev_position_l = state_l.position;
-    prev_position_r = state_r.position;
-
-    vel_l /=64 * ENCODER_CPR;
-    vel_r /=64 * ENCODER_CPR;
-    vel_l *=  WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
-    vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
-
-    const double linear = (vel_l + vel_r) / 2;
-    const double angular = (vel_r - vel_l) / wheel_separation;
-    const double r = linear / angular;
-
-    if(fabs(r) > 100) {
-        const double dir = base_theta + angular * 0.5;
-
-        base_x += linear * cos(dir);
-        base_y += linear * sin(dir);
-        base_theta += angular;
-    }
-    else {
-        const double base_theta_old = base_theta;
-        base_theta += angular;
-        base_x += r * (sin(base_theta) - sin(base_theta_old));
-        base_y += -r * (cos(base_theta) - cos(base_theta_old));
-    }
-}
-
-void zero() {
-    base_x = 0;
-    base_y = 0; 
-    base_theta = 0;  
-    calib_enc.left_pulse = 0;
-    calib_enc.right_pulse = 0;
-}
-
-void start_calib() {
-    calib_enc.left_pulse = 0;
-    calib_enc.right_pulse = 0;
-}
-
-void end_calib() {
-    const double l_adjust = (double)calib_enc.left_pulse * calib_enc.left_gain;
-    const double r_adjust = (double)calib_enc.right_pulse * calib_enc.right_gain;
-    const double delta_angle = l_adjust - r_adjust;
-    const double distance = 0.5 * (l_adjust + r_adjust);
-    const double factor = delta_angle / distance * 0.7;
-    calib_enc.left_gain = (1.0 - factor) * calib_enc.left_gain;
-    calib_enc.right_gain = (1.0 + factor) * calib_enc.right_gain;
-}
-
-
-void core2_entry() 
-{
-
-    pio_add_program(pio1, &quadrature_encoder_substep_program);
-    substep_init_state(pio1, 0, ENCODER_LEFT_PIN_A , &state_l);
-    substep_init_state(pio1, 1, ENCODER_RIGHT_PIN_A, &state_r);
-
-    // The sets the positions to initial values
-    substep_update(&state_l);
-    substep_update(&state_r);
-    prev_position_l = state_l.position;
-    prev_position_r = state_r.position;
-
-    prev_time = time_us_32();
-
-    // alarm_pool_t *ap = alarm_pool_create_with_unused_hardware_alarm(2);
-    // repeating_timer_t rt;
-    // alarm_pool_add_repeating_timer_us(ap, TIMER_CYCLE_US, update_pos_cb, NULL, &rt);
-
-    uint16_t cmd = 0;
-    uint64_t data = 0;
-    uint readNum = 0;
-    char type = 'w';
-
-    while (true) {
-        uint ch;
-        if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
-            cmd = (cmd << 8) | ch;
-
-            if(readNum > 0) {
-                data = (data >> 8ll) | ((uint64_t)ch<<56ll);
-                readNum--;
-                if(!readNum) {
-                    if(type == 'w')
-                        wheel_separation = *((double*)&data);
-                    else
-                        wheel_ratio_l = 1 + (*((double *)&data));
-                        wheel_ratio_r = 1 - (*((double *)&data));
-                }
-            }
-            
-            if(cmd == (((uint16_t)'g' << 8) | ';')) {
-                printf("%lf %lf %lf\n", base_x, base_y, base_theta);
-                cmd = 0;
-            } else if(cmd == (((uint16_t)'w' << 8) | ';')) {
-                readNum = 8;
-                type = (cmd >> 8) & 0xFF;
-            } else if(cmd == (((uint16_t)'r' << 8) | ';')){
-                readNum = 8;
-                type = (cmd >> 8) & 0xFF;
-            } else if(cmd == (((uint16_t)'z' << 8) | ';')) {
-                zero();
-            } else if(cmd == (((uint16_t)'c' << 8) | ';')) {
-                end_calib();
-                printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain);
-            }
-        }
-
-        update_pos_cb();
-        sleep_ms(1);
-    }
-}
-
-double btod(unsigned char * input) {
-    uint64_t data = 0;
-    for (int i = 0; i<8; i++){
-        uint a = *(input + i);
-        data >>= 8;
-        data  |= (uint64_t)a<<56;
-    }
-    const double ret = *((double*)&data);
-    return ret;
-}
-
-void stepper_fifo(char c, uint8_t itf) {
-    char instrh = stepper_buf.data[(stepper_buf.pos + 1) % sizeof(stepper_buf.data)];
-    char instrl = stepper_buf.data[(stepper_buf.pos + 2) % sizeof(stepper_buf.data)];
-    stepper_buf.data[stepper_buf.pos] = c;
-    stepper_buf.pos = (stepper_buf.pos + 1) % sizeof(stepper_buf.data);
-
-    if(instrh == 's' && instrl == ';') {
-        memcpy(stepper_instr,stepper_buf.data + stepper_buf.pos, sizeof(stepper_buf.data) - stepper_buf.pos);
-        memcpy(stepper_instr - stepper_buf.pos + sizeof(stepper_buf.data),stepper_buf.data, stepper_buf.pos);
-
-        memset(&stepper_buf, 0, sizeof(stepper_buf));
-        double vl = btod(stepper_instr + 2);
-        double vr = btod(stepper_instr + 10);
-
-        set_speeds(vl, vr);
-
-        // tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
-        // tud_cdc_n_write_flush(itf);
-    }
-
-}
-
-void tud_cdc_rx_cb(uint8_t itf) {
-    // read the available data 
-    // | IMPORTANT: also do this for CDC0 because otherwise
-    // | you won't be able to print anymore to CDC0
-    // | next time this function is called
-    if (itf == 1) {
-        int c;
-        while((c = tud_cdc_n_read_char(itf)) > -1) {
-            stepper_fifo(c, itf);
-        }
-        // tud_cdc_n_write(itf, (uint8_t const *) "OK\r\n", 4);
-        // tud_cdc_n_write_flush(itf);
-    }
-}
-
-void run_init() 
-{
-    board_init();
-    tusb_init();
-    tud_init(BOARD_TUD_RHPORT);
-
-
-
-    if (board_init_after_tusb) {
-        board_init_after_tusb();
-    }
-
-    if(!stdio_usb_init()) {
-        board_led_write(1);
-    }
-    stepper_init();
-
-}
-
-
-int main()
-{
-    run_init();
-
-    multicore_launch_core1(core2_entry);
-
-    
-    
-    while (true) {
-        tud_task();
-        sleep_ms(1);
-    }
-}
-
--- a/firmware/base/src/quadrature.c
+++ b/firmware/base/src/quadrature.c
@ -1,184 +0,0 @@
-/**
- * Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
- * Modified by Pimpest in 2024
- * 
- * SPDX-License-Identifier: BSD-3-Clause
- * 
- * This software has been modified from its original version
- */
-#include "quadrature.pio.h"
-#include "quadrature.h"
-#include "memory.h"
-
-static void read_pio_data(substep_state_t *state, uint *step, uint *step_us, uint *transition_us, int *forward)
-{
-    int cycles;
-
-    // get the raw data from the PIO state machine
-    quadrature_encoder_substep_get_counts(state->pio, state->sm, step, &cycles, step_us);
-
-    // when the PIO program detects a transition, it sets cycles to either zero
-    // (when step is incrementing) or 2^31 (when step is decrementing) and keeps
-    // decrementing it on each 13 clock loop. We can use this information to get
-    // the time and direction of the last transition
-    if (cycles < 0) {
-        cycles = -cycles;
-        *forward = 1;
-    } else {
-        cycles = 0x80000000 - cycles;
-        *forward = 0;
-    }
-    *transition_us = *step_us - ((cycles * 13) / state->clocks_per_us);
-}
-
-// get the sub-step position of the start of a step
-static uint get_step_start_transition_pos(substep_state_t *state, uint step)
-{
-    return ((step << 6) & 0xFFFFFF00) | state->calibration_data[step & 3];
-}
-
-// compute speed in "sub-steps per 2^20 us" from a delta substep position and
-// delta time in microseconds. This unit is cheaper to compute and use, so we
-// only convert to "sub-steps per second" once per update, at most
-static int substep_calc_speed(int delta_substep, int delta_us)
-{
-    return ((int64_t) delta_substep << 20) / delta_us;
-}
-
-
-// main functions to be used by user code
-
-// initialize the substep state structure and start PIO code
-void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state)
-{
-    int forward;
-
-    // set all fields to zero by default
-    memset(state, 0, sizeof(substep_state_t));
-
-    // initialize the PIO program (and save the PIO reference)
-    state->pio = pio;
-    state->sm = sm;
-    quadrature_encoder_substep_program_init(pio, sm, pin_a);
-
-    // start with equal phase size calibration
-    state->calibration_data[0] = 0;
-    state->calibration_data[1] = 64;
-    state->calibration_data[2] = 128;
-    state->calibration_data[3] = 192;
-
-    state->idle_stop_samples = 3;
-
-    // start "stopped" so that we don't use stale data to compute speeds
-    state->stopped = 1;
-
-    // cache the PIO cycles per us
-    state->clocks_per_us = (clock_get_hz(clk_sys) + 500000) / 1000000;
-
-    // initialize the "previous state"
-    read_pio_data(state, &state->raw_step, &state->prev_step_us, &state->prev_trans_us, &forward);
-
-    state->position = get_step_start_transition_pos(state, state->raw_step) + 32;
-}
-
-// read the PIO data and update the speed / position estimate
-void substep_update(substep_state_t *state)
-{
-    uint step, step_us, transition_us, transition_pos, low, high;
-    int forward, speed_high, speed_low;
-
-    // read the current encoder state from the PIO
-    read_pio_data(state, &step, &step_us, &transition_us, &forward);
-
-    // from the current step we can get the low and high boundaries in substeps
-    // of the current position
-    low = get_step_start_transition_pos(state, step);
-    high = get_step_start_transition_pos(state, step + 1);
-
-    // if we were not stopped, but the last transition was more than
-    // "idle_stop_samples" ago, we are stopped now
-    if (step == state->raw_step)
-        state->idle_stop_sample_count++;
-    else
-        state->idle_stop_sample_count = 0;
-
-    if (!state->stopped && state->idle_stop_sample_count >= state->idle_stop_samples) {
-        state->speed = 0;
-        state->speed_2_20 = 0;
-        state->stopped = 1;
-    }
-
-    // when we are at a different step now, there is certainly a transition
-    if (state->raw_step != step) {
-        // the transition position depends on the direction of the move
-        transition_pos = forward ? low : high;
-
-        // if we are not stopped, that means there is valid previous transition
-        // we can use to estimate the current speed
-        if (!state->stopped)
-            state->speed_2_20 = substep_calc_speed(transition_pos - state->prev_trans_pos, transition_us - state->prev_trans_us);
-
-        // if we have a transition, we are not stopped now
-        state->stopped = 0;
-        // save the timestamp and position of this transition to use later to
-        // estimate speed
-        state->prev_trans_pos = transition_pos;
-        state->prev_trans_us = transition_us;
-    }
-
-    // if we are stopped, speed is zero and the position estimate remains
-    // constant. If we are not stopped, we have to update the position and speed
-    if (!state->stopped) {
-        // although the current step doesn't give us a precise position, it does
-        // give boundaries to the position, which together with the last
-        // transition gives us boundaries for the speed value. This can be very
-        // useful especially in two situations:
-        // - we have been stopped for a while and start moving quickly: although
-        //   we only have one transition initially, the number of steps we moved
-        //   can already give a non-zero speed estimate
-        // - we were moving but then stop: without any extra logic we would just
-        //   keep the last speed for a while, but we know from the step
-        //   boundaries that the speed must be decreasing
-
-        // if there is a transition between the last sample and now, and that
-        // transition is closer to now than the previous sample time, we should
-        // use the slopes from the last sample to the transition as these will
-        // have less numerical issues and produce a tighter boundary
-        if (state->prev_trans_us > state->prev_step_us && 
-            (int)(state->prev_trans_us - state->prev_step_us) > (int)(step_us - state->prev_trans_us)) {
-            speed_high = substep_calc_speed(state->prev_trans_pos - state->prev_low, state->prev_trans_us - state->prev_step_us);
-            speed_low = substep_calc_speed(state->prev_trans_pos - state->prev_high, state->prev_trans_us - state->prev_step_us);
-        } else {
-            // otherwise use the slopes from the last transition to now
-            speed_high = substep_calc_speed(high - state->prev_trans_pos, step_us - state->prev_trans_us);
-            speed_low = substep_calc_speed(low - state->prev_trans_pos, step_us - state->prev_trans_us);
-        }
-        // make sure the current speed estimate is between the maximum and
-        // minimum values obtained from the step slopes
-        if (state->speed_2_20 > speed_high)
-            state->speed_2_20 = speed_high;
-        if (state->speed_2_20 < speed_low)
-            state->speed_2_20 = speed_low;
-
-        // convert the speed units from "sub-steps per 2^20 us" to "sub-steps
-        // per second"
-        state->speed = (state->speed_2_20 * 62500LL) >> 16;
-
-        // estimate the current position by applying the speed estimate to the
-        // most recent transition
-        state->position = state->prev_trans_pos + (((int64_t)state->speed_2_20 * (step_us - transition_us)) >> 20);
-
-        // make sure the position estimate is between "low" and "high", as we
-        // can be sure the actual current position must be in this range
-        if ((int)(state->position - high) > 0)
-            state->position = high;
-        else if ((int)(state->position - low) < 0)
-            state->position = low;
-    }
-
-    // save the current values to use on the next sample
-    state->prev_low = low;
-    state->prev_high = high;
-    state->raw_step = step;
-    state->prev_step_us = step_us;
-}
--- a/firmware/base/src/quadrature.h
+++ b/firmware/base/src/quadrature.h
@ -1,27 +0,0 @@
-#pragma once
-#include "hardware/pio.h"
-
-typedef struct substep_state_t {
-
-    uint calibration_data[4]; // relative phase sizes
-    uint clocks_per_us;       // save the clk_sys frequency in clocks per us
-    uint idle_stop_samples;   // after these samples without transitions, assume the encoder is stopped
-    PIO pio;
-    uint sm;
-
-    uint prev_trans_pos, prev_trans_us;
-    uint prev_step_us;
-    uint prev_low, prev_high;
-    uint idle_stop_sample_count;
-    int speed_2_20;
-    int stopped;
-
-    int speed;     
-    uint position; 
-
-    uint raw_step; 
-} substep_state_t;
-
-
-void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state);
-void substep_update(substep_state_t *state);
--- a/firmware/base/src/stepper.c
+++ b/firmware/base/src/stepper.c
@ -1,43 +0,0 @@
-#include "blink.pio.h"
-#include "hardware/pio.h"
-#include "hardware/clocks.h"
-#include "pico/time.h"
-
-#include <math.h>
-
-#include "config.h"
-#include "stepper.h"
-
-void stepper_init() {
-    PIO pio = pio0;
-    uint offset = pio_add_program(pio, &oscillate_program);
-    
-    start_pulse(pio, SM_A, offset, PULSE_PIN_A, 1);
-    start_pulse(pio, SM_B, offset, PULSE_PIN_B, 1);
-
-    gpio_init(DIR_PIN_A);
-    gpio_init(DIR_PIN_B);
-    gpio_set_dir(DIR_PIN_A, GPIO_OUT);
-    gpio_set_dir(DIR_PIN_B, GPIO_OUT);
-}
-
-void set_speeds(double a, double b) {
-    update_sm(pio0, SM_A, DIR_PIN_A, a);
-    update_sm(pio0, SM_B, DIR_PIN_B, b);
-}
-
-void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
-    oscillate_program_init(pio, sm, offset, pin);
-    pio->txf[sm] = 0;
-    pio_sm_set_enabled(pio, sm, true);
-}
-
-void update_sm(PIO pio, uint sm, const uint pin ,double v) {
-    double u_v = fabs(v);
-    if(u_v > 0.0005)
-        pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
-    else
-        pio->txf[sm] = 0;
-    gpio_put(pin, v < 0);
-}
-
--- a/firmware/base/src/stepper.h
+++ b/firmware/base/src/stepper.h
@ -1,8 +0,0 @@
-#pragma once
-#include "hardware/pio.h"
-
-void stepper_init();
-void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq);
-
-void update_sm(PIO pio, uint sm, const uint pin, double v);
-void set_speeds(double vl, double vr);
--- a/firmware/base/tusb/tusb_config.h
+++ b/firmware/base/tusb/tusb_config.h
@ -1,47 +0,0 @@
-#pragma once
-
-#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
-#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
-#endif
-
-#define CFG_TUSB_RHPORT0_MODE (OPT_MODE_DEVICE | OPT_MODE_FULL_SPEED)
-#ifndef BOARD_TUD_RHPORT
-#define BOARD_TUD_RHPORT 0
-#endif
-
-#ifndef BOARD_TUD_MAX_SPEED
-#define BOARD_TUD_MAX_SPEED OPT_MODE_DEFAULT_SPEED
-#endif
-
-#ifndef CFG_TUSB_MCU
-#error CFG_TUSB_MCU must be defined
-#endif
-
-#ifndef CFG_TUSB_OS
-#define CFG_TUSB_OS OPT_OS_NONE
-#endif
-
-#ifndef CFG_TUD_ENABLED
-#define CFG_TUD_ENABLED 1
-#endif
-
-#ifndef CFG_TUD_MAX_SPEED
-#define CFG_TUD_MAX_SPEED BOARD_TUD_MAX_SPEED
-#endif
-
-#ifndef CFG_TUD_ENDPOINT0_SIZE
-#define CFG_TUD_ENDPOINT0_SIZE 64
-#endif
-
-#define CFG_TUD_CDC    2
-#define CFG_TUD_MSC    0
-#define CFG_TUD_HID    0
-#define CFG_TUD_MIDI   0
-#define CFG_TUD_VENDOR 0
-
-#define CFG_TUD_CDC_RX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
-#define CFG_TUD_CDC_TX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
-
-#define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
-
-#define CFG_TUD_MSC_EP_BUFSIZE 512
--- a/firmware/base/tusb/tusb_descriptors.c
+++ b/firmware/base/tusb/tusb_descriptors.c
@ -1,143 +0,0 @@
-#include "bsp/board_api.h"
-#include "tusb.h"
-
-#define _PID_MAP(itf,n) ( (CFG_TUD_##itf) << (n) )
-#define USB_PID (0x4d47 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1)| _PID_MAP(HID, 2) | \
-                _PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4) )
-
-#define USB_VID 0x1209
-#define USB_BCD 0x0200
-
-tusb_desc_device_t const desc_device = {
-    .bLength            = sizeof(tusb_desc_device_t),
-    .bDescriptorType    = TUSB_DESC_DEVICE,
-    .bcdUSB             = USB_BCD,
-
-    .bDeviceClass       = TUSB_CLASS_MISC,
-    .bDeviceSubClass    = MISC_SUBCLASS_COMMON,
-    .bDeviceProtocol    = MISC_PROTOCOL_IAD,
-    .bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
-    
-    .idVendor           = USB_VID,
-    .idProduct          = USB_PID,
-    .bcdDevice          = 0x0100,
-
-    .iManufacturer      = 0x01,
-    .iProduct           = 0x02,
-    .iSerialNumber      = 0x03,
-
-    .bNumConfigurations = 0x01
-};
-
-
-uint8_t const * tud_descriptor_device_cb(void) {
-    return (uint8_t const *) &desc_device;
-}
-
-enum
-{
-    ITF_NUM_CDC_0 = 0,
-    ITF_NUM_CDC_0_DATA,
-    ITF_NUM_CDC_1,
-    ITF_NUM_CDC_1_DATA,
-    ITF_NUM_TOTAL,
-};
-
-
-#define CONFIG_TOTAL_LEN    (TUD_CONFIG_DESC_LEN + CFG_TUD_CDC * TUD_CDC_DESC_LEN)
-
-#define EPNUM_CDC_NOTIF   0x81
-#define EPNUM_CDC_OUT     0x02
-#define EPNUM_CDC_IN      0x82
-
-#define EPNUM_CDC_1_NOTIF   0x84
-#define EPNUM_CDC_1_OUT     0x05
-#define EPNUM_CDC_1_IN      0x85
-
-
-uint8_t const desc_fs_configuration[] =
-{
-    TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 250),
-    TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_0, 4, EPNUM_CDC_NOTIF, 8, EPNUM_CDC_OUT, EPNUM_CDC_IN, 64),
-    TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_1, 4, EPNUM_CDC_1_NOTIF, 8, EPNUM_CDC_1_OUT, EPNUM_CDC_1_IN, 64)
-};
-
-
-tusb_desc_device_qualifier_t const desc_device_qualifier = 
-{
-    .bLength            = sizeof(tusb_desc_device_qualifier_t),
-    .bDescriptorType    = TUSB_DESC_DEVICE,
-    .bcdUSB             = USB_BCD,
-
-    .bDeviceClass       = TUSB_CLASS_MISC,
-    .bDeviceSubClass    = MISC_SUBCLASS_COMMON,
-    .bDeviceProtocol    = MISC_PROTOCOL_IAD,
-
-    .bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
-    .bNumConfigurations = 0x01
-};
-
-
-uint8_t const* tud_descriptor_device_qualifier_cb(void) {
-    return (uint8_t const*) &desc_device_qualifier;
-}
-
-
-uint8_t const* tud_descriptor_configuration_cb(uint8_t index) {
-    (void) index;
-
-    return  desc_fs_configuration;
-}
-
-enum {
-    STRID_LANGID = 0,
-    STRID_MANUFACTURER,
-    STRID_PRODUCT,
-    STRID_SERIAL,
-    STRID_CDC_0,
-    STRID_CDC_1
-};
-
-char const *string_desc_arr[] = {
-    (const char[]) { 0x09, 0x04},
-    "Mg Robotics",
-    "Magrob Odometry MCU",
-    NULL,
-    "Odometry CDC",
-    "Stepper CDC"
-};
-
-static uint16_t _desc_str[32+1];
-
-uint16_t const *tud_descriptor_string_cb(uint8_t index, uint16_t langid) {
-    (void) langid;
-    size_t chr_count;
-
-    switch (index) {
-    case STRID_LANGID:
-        memcpy(&_desc_str[1], string_desc_arr[0], 2);
-        chr_count = 1;
-        break;
-
-    case STRID_SERIAL:
-        chr_count = board_usb_get_serial(_desc_str + 1, 32);
-        break;
-
-    default:
-        if( !(index < sizeof(string_desc_arr) / sizeof(string_desc_arr[0])) ) return NULL;
-
-        const char *str = string_desc_arr[index];
-
-        chr_count = strlen(str);
-        size_t const max_count = sizeof(_desc_str) / sizeof(_desc_str[0]) - 1;
-        if ( chr_count > max_count ) chr_count = max_count;
-
-        for(size_t i = 0; i < chr_count; i++) {
-            _desc_str[i + 1] = str[i];
-        }
-        break;
-    }
-
-    _desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8) | (2 * chr_count + 2));
-    return _desc_str;
-}
--- a/mg_bringup/launch/launch.py
+++ b/mg_bringup/launch/launch.py
@ -33,7 +33,7 @@ def generate_launch_description():
            condition=UnlessCondition(LaunchConfiguration('local_test')),
            parameters=[{
                'odom': "odom",
-                'serial_path': "/dev/ttyACM0",
+                'serial_path': "/dev/ttyACM1",
            }],

            emulate_tty=True,
--- a/mg_bt/CMakeLists.txt
+++ b/mg_bt/CMakeLists.txt
@ -1,72 +0,0 @@
-cmake_minimum_required(VERSION 3.12)
-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()
-
-set(PACKAGE_DEPS
-  rclcpp
-  behaviortree_cpp
-  behaviortree_ros2
-  btcpp_ros2_interfaces
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  mg_msgs
-  std_msgs
-  std_srvs
-  )
-
-find_package(ament_cmake REQUIRED)
-
-include(FindPkgConfig)
-pkg_search_module(LIBGLM REQUIRED glm)
-
-foreach(PACKAGE ${PACKAGE_DEPS})
-    find_package(${PACKAGE} REQUIRED)
-endforeach()
-
-set(SOURCES
-    src/mg_bt.cpp
-    src/mg_tree_executor.cpp
-)
-
-
-add_executable(mg_bt_executor ${SOURCES})
-add_executable(mg_i2cnode i2cmodule/i2cnode.cpp)
-
-target_include_directories(
-  mg_bt_executor
-  PRIVATE
-  ${LIBGLM_INCLUDE_DIRS}
-  include
-)
-
-target_link_libraries(mg_i2cnode i2c)
-
-ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
-ament_target_dependencies(mg_i2cnode rclcpp mg_msgs)
-
-install( TARGETS
-  mg_bt_executor
-  mg_i2cnode
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-install(DIRECTORY
-  behavior_trees
-  config
-  launch
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-target_compile_features(mg_bt_executor PUBLIC 
-  c_std_99
-  cxx_std_17
-)  # Require C99 and C++17
-
-ament_package()
--- a/mg_bt/behavior_trees/calib_bt.xml
+++ b/mg_bt/behavior_trees/calib_bt.xml
@ -1,151 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<root BTCPP_format="4">
-  <BehaviorTree ID="calib2_bt">
-    <Sequence>
-      <ZeroNode service_name="/zero"/>
-      <MovePoint action_name="/MovePoint"
-                 tolerance="0.02"
-                 max_angular="0.1"
-                 x_goal="0.7"
-                 y_goal="0.0"/>
-      <RotateNode angle="-2"
-                  action_name="/Rotate"/>
-      <RotateNode angle="-3.14"
-                  action_name="/Rotate"/>
-      <MovePoint action_name="/MovePoint"
-                 tolerance="0.02"
-                 x_goal="0.3"
-                 y_goal="0.0"/>
-      <RotateNode angle="-2"
-                  action_name="/Rotate"/>
-      <RotateNode angle="0"
-                  action_name="/Rotate"/>
-            <Fallback>
-              <Timeout msec="5000">
-                <MovePoint action_name="/MovePoint"
-                           max_angular="0.1"
-                           x_goal="-0.1"
-                           y_goal="0.0"/>
-              </Timeout>
-              <AlwaysSuccess/>
-            </Fallback>
-      <ZeroNode service_name="/endCalib"/>
-    </Sequence>
-  </BehaviorTree>
-
-  <BehaviorTree ID="calib_bt">
-    <Sequence>
-      <SetBlackboard value="0.311010"
-                     output_key="width"/>
-      <Delay delay_msec="10000">
-        <Repeat num_cycles="5">
-          <Sequence>
-            <ZeroNode service_name="/zero"/>
-            <MovePoint action_name="/MovePoint"
-                       tolerance="0.04"
-                       max_angular="0.1"
-                       x_goal="0.7"
-                       y_goal="0.0"/>
-            <RotateNode angle="2"
-                        action_name="/Rotate"/>
-            <RotateNode angle="-2"
-                        action_name="/Rotate"/>
-            <RotateNode angle="0"
-                        action_name="/Rotate"/>
-            <Fallback>
-              <Timeout msec="20000">
-                <MovePoint action_name="/MovePoint"
-                           max_angular="0.1"
-                           x_goal="-0.1"
-                           y_goal="0.0"/>
-              </Timeout>
-              <AlwaysSuccess/>
-            </Fallback>
-            <CalibWidth PreviousWidth="{width}"
-                        Count="1"
-                        NewWidth="{width}"
-                        service_name="/set_width"/>
-          </Sequence>
-        </Repeat>
-      </Delay>
-    </Sequence>
-  </BehaviorTree>
-
-  <!-- Description of Node Models (used by Groot) -->
-  <TreeNodesModel>
-    <Action ID="CalibWidth">
-      <input_port name="PreviousWidth"
-                  type="double"/>
-      <input_port name="Count"
-                  default="1"
-                  type="int"/>
-      <output_port name="NewWidth"
-                   type="double"/>
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-    <Action ID="MovePoint">
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-      <input_port name="tolerance"
-                  type="double"/>
-      <input_port name="obst_mult_a"
-                  type="double"/>
-      <input_port name="ornt_mult"
-                  type="double"/>
-      <input_port name="pos_mult"
-                  type="double"/>
-      <input_port name="t_ornt_mult"
-                  type="double"/>
-      <input_port name="obst_mult_c"
-                  type="double"/>
-      <input_port name="obst_mult_b"
-                  type="double"/>
-      <input_port name="max_vel"
-                  type="double"/>
-      <input_port name="max_angular"
-                  type="double"/>
-      <input_port name="IgnoreList"
-                  type="std::string"/>
-      <input_port name="max_wheel_speed"
-                  type="double"/>
-      <input_port name="x_goal"
-                  type="double"/>
-      <input_port name="y_goal"
-                  type="double"/>
-    </Action>
-    <Action ID="RotateNode">
-      <input_port name="angle"
-                  type="double"/>
-      <input_port name="pos_mult"
-                  type="double"/>
-      <input_port name="max_wheel_speed"
-                  type="double"/>
-      <input_port name="IgnoreList"
-                  type="std::string"/>
-      <input_port name="max_angular"
-                  type="double"/>
-      <input_port name="max_vel"
-                  type="double"/>
-      <input_port name="obst_mult_b"
-                  type="double"/>
-      <input_port name="obst_mult_c"
-                  type="double"/>
-      <input_port name="ornt_mult"
-                  type="double"/>
-      <input_port name="t_ornt_mult"
-                  type="double"/>
-      <input_port name="obst_mult_a"
-                  type="double"/>
-      <input_port name="tolerance"
-                  type="double"/>
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-    </Action>
-    <Action ID="ZeroNode">
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-  </TreeNodesModel>
-
-</root>
--- a/mg_bt/behavior_trees/mg_bt.btproj
+++ b/mg_bt/behavior_trees/mg_bt.btproj
@ -1,47 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<root BTCPP_format="4" project_name="Project">
-    <include path="calib_bt.xml"/>
-    <!-- Description of Node Models (used by Groot) -->
-    <TreeNodesModel>
-        <Action ID="CalibWidth">
-            <input_port name="PreviousWidth" type="double"/>
-            <input_port name="Count" default="1" type="int"/>
-            <output_port name="NewWidth" type="double"/>
-            <input_port name="service_name" type="std::string">Service name</input_port>
-        </Action>
-        <Action ID="MovePoint">
-            <input_port name="action_name" type="std::string">Action server name</input_port>
-            <input_port name="tolerance" type="double"/>
-            <input_port name="obst_mult_a" type="double"/>
-            <input_port name="ornt_mult" type="double"/>
-            <input_port name="pos_mult" type="double"/>
-            <input_port name="t_ornt_mult" type="double"/>
-            <input_port name="obst_mult_c" type="double"/>
-            <input_port name="obst_mult_b" type="double"/>
-            <input_port name="max_vel" type="double"/>
-            <input_port name="max_angular" type="double"/>
-            <input_port name="IgnoreList" type="std::string"/>
-            <input_port name="max_wheel_speed" type="double"/>
-            <input_port name="x_goal" type="double"/>
-            <input_port name="y_goal" type="double"/>
-        </Action>
-        <Action ID="RotateNode">
-            <input_port name="angle" type="double"/>
-            <input_port name="pos_mult" type="double"/>
-            <input_port name="max_wheel_speed" type="double"/>
-            <input_port name="IgnoreList" type="std::string"/>
-            <input_port name="max_angular" type="double"/>
-            <input_port name="max_vel" type="double"/>
-            <input_port name="obst_mult_b" type="double"/>
-            <input_port name="obst_mult_c" type="double"/>
-            <input_port name="ornt_mult" type="double"/>
-            <input_port name="t_ornt_mult" type="double"/>
-            <input_port name="obst_mult_a" type="double"/>
-            <input_port name="tolerance" type="double"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
-        </Action>
-        <Action ID="ZeroNode">
-            <input_port name="service_name" type="std::string">Service name</input_port>
-        </Action>
-    </TreeNodesModel>
-</root>
--- a/mg_bt/config/mg_bt_executor.yaml
+++ b/mg_bt/config/mg_bt_executor.yaml
@ -1,14 +0,0 @@
-bt_action_server:
-  ros__parameters:
-    action_name: "mg_bt_action_server" 
-    tick_frequency: 100
-    groot2_port: 8420
-    ros_plugins_timeout: 1000
-
-    plugins:
-       - btcpp_ros2_samples/bt_plugins
-
-    behavior_trees:
-      - mg_bt/behavior_trees
-
-
--- a/mg_bt/i2cmodule/i2cnode.cpp
+++ b/mg_bt/i2cmodule/i2cnode.cpp
@ -1,47 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "mg_msgs/srv/i2c.hpp"
-
-extern "C" {
-
-#include <fcntl.h>
-#include <linux/i2c-dev.h>
-#include <sys/ioctl.h>
-#include <i2c/smbus.h>
-}
-class MgI2c : public rclcpp::Node {
-    using I2cSrv = mg_msgs::srv::I2c;
-
-  public:
-    MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
-        auto cb
-          = [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
-        i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
-    }
-
-    void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
-        ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT
-        i2c_smbus_write_byte(i2c_fd_, req->data);
-        int ch = 0;
-
-        rclcpp::Rate rate(100);
-
-        while (ch == 0 || (ch > 255 || ch < 0)) {
-            ch = i2c_smbus_read_byte(i2c_fd_);
-            rate.sleep();
-        }
-        resp->resp.push_back(ch);
-        RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front());
-    }
-
-  private:
-    rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
-    int                                           i2c_fd_;
-};
-
-int main(int argc, const char* const* argv) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<MgI2c>("i2cu"));
-    rclcpp::shutdown();
-
-    return 0;
-}
--- a/mg_bt/launch/launch.py
+++ b/mg_bt/launch/launch.py
@ -1,22 +0,0 @@
-from launch import LaunchDescription
-from launch.substitutions import 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")
-
-    bt_exec_config = PathJoinSubstitution([basedir, "config/mg_bt_executor.yaml"])
-
-    return LaunchDescription([
-        Node(
-            package='mg_bt',
-            executable='mg_bt_executor',
-            output="screen",
-            parameters=[bt_exec_config]
-        ),
-    ])
-    
--- a/mg_bt/package.xml
+++ b/mg_bt/package.xml
@ -1,25 +0,0 @@
-<?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>Behavior for MagRob</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>behaviortree_cpp</depend>
-  <depend>behaviortree_ros2</depend>
-  <depend>btcpp_ros2_interfaces</depend>
-  <depend>mg_msgs</depend>
-  <depend>libi2c-dev</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
--- a/mg_bt/src/mg_bt.cpp
+++ b/mg_bt/src/mg_bt.cpp
@ -1,23 +0,0 @@
-#include <iostream>
-
-#include "mg_tree_executor.hpp"
-
-int main(const int argc, const char** argv) {
-    rclcpp::init(argc, argv);
-
-    std::cout << "Starting up Behavior Tree Executor" << std::endl;
-
-    rclcpp::NodeOptions options;
-
-    auto bt_exec_server = std::make_shared<mg::MgTreeExecutor>(options);
-
-    rclcpp::executors::MultiThreadedExecutor executor(
-      rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
-
-    executor.add_node(bt_exec_server->node());
-    executor.spin();
-    executor.remove_node(bt_exec_server->node());
-
-    rclcpp::shutdown();
-    return 0;
-}
--- a/mg_bt/src/mg_tree_executor.cpp
+++ b/mg_bt/src/mg_tree_executor.cpp
@ -1,61 +0,0 @@
-#include "mg_tree_executor.hpp"
-
-#include "behaviortree_cpp/xml_parsing.h"
-#include "tree_nodes/calib.hpp"
-#include "tree_nodes/i2c.hpp"
-#include "tree_nodes/move_point.hpp"
-#include "tree_nodes/rotate.hpp"
-#include "tree_nodes/zero.hpp"
-
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "tf2/LinearMath/Transform.h"
-#include "tf2/convert.h"
-
-namespace mg {
-    MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
-        executeRegistration();
-        std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
-        tf_buffer_   = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
-        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-
-        const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
-    }
-
-    void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }
-
-    void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
-        factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
-        factory.registerNodeType<mg::RotateNode>("RotateNode", node());
-        factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
-        factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
-        factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
-    }
-
-    std::pair<glm::vec2, double> MgTreeExecutor::position() {
-        double    x = NAN;
-        double    y = NAN;
-        double    theta;
-        glm::vec2 pos;
-
-        auto tmsg = tf_buffer_->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);
-        return { pos, theta };
-    }
-
-    std::optional<std::string> MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
-        (void)status;
-        logger_cout_.reset();
-
-        if (was_cancelled) {
-            std::cout << "Behavior tree was cancled" << std::endl;
-        }
-
-        return std::nullopt;
-    }
-}
--- a/mg_bt/src/mg_tree_executor.hpp
+++ b/mg_bt/src/mg_tree_executor.hpp
@ -1,25 +0,0 @@
-#pragma once
-
-#include "behaviortree_ros2/tree_execution_server.hpp"
-#include "behaviortree_cpp/loggers/bt_cout_logger.h"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "glm/glm.hpp"
-
-namespace mg {
-    class MgTreeExecutor : public BT::TreeExecutionServer {
-      public:
-        MgTreeExecutor(const rclcpp::NodeOptions opts);
-        void onTreeCreated(BT::Tree& tree) override;
-        void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
-
-        std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
-
-        std::pair<glm::vec2, double> position();
-
-      private:
-        std::shared_ptr<BT::StdCoutLogger>          logger_cout_;
-        tf2_ros::Buffer::SharedPtr                  tf_buffer_;
-        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-    };
-}
--- a/mg_bt/src/tree_nodes/calib.hpp
+++ b/mg_bt/src/tree_nodes/calib.hpp
@ -1,44 +0,0 @@
-#pragma once
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "mg_msgs/srv/send_double.hpp"
-#include "glm/glm.hpp"
-
-namespace mg {
-
-    using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
-
-    class CalibWidthNode : public BT::RosServiceNode<mg_msgs::srv::SendDouble> {
-      public:
-        CalibWidthNode(const std::string&       name,
-                       const BT::NodeConfig&    conf,
-                       const BT::RosNodeParams& params,
-                       const PosFuncSig         pos_query) :
-          BT::RosServiceNode<mg_msgs::srv::SendDouble>(name, conf, params), pos_query_(pos_query) { }
-
-        static BT::PortsList providedPorts() {
-            return providedBasicPorts({ BT::InputPort<double>("PreviousWidth"),
-                                        BT::InputPort<int>("Count", 1, {}),
-                                        BT::OutputPort<double>("NewWidth") });
-        }
-
-        bool setRequest(typename Request::SharedPtr& request) override {
-            auto [pos, theta] = pos_query_();
-            double width      = getInput<double>("PreviousWidth").value();
-            int    count      = getInput<int>("Count").value();
-            double new_width  = width * (1 + (theta / (2 * M_PI * count)));
-
-            RCLCPP_INFO(logger(), "Setting width to: %lf", new_width);
-
-            request->set__data(new_width);
-            setOutput("Count", new_width);
-            return true;
-        }
-
-        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
-            return BT::NodeStatus::SUCCESS;
-        }
-
-      private:
-        const PosFuncSig pos_query_;
-    };
-}
--- a/mg_bt/src/tree_nodes/i2c.hpp
+++ b/mg_bt/src/tree_nodes/i2c.hpp
@ -1,29 +0,0 @@
-#pragma once
-
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "mg_msgs/srv/i2c.hpp"
-
-namespace mg {
-    class I2cNode : public BT::RosServiceNode<mg_msgs::srv::I2c> {
-      public:
-        I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
-          BT::RosServiceNode<mg_msgs::srv::I2c>(name, conf, params) { }
-
-        static BT::PortsList providedPorts() {
-            return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
-                                        BT::InputPort<int>("Data", 0, {}),
-                                        BT::OutputPort<int>("Result") });
-        }
-
-        bool setRequest(typename Request::SharedPtr& req) override {
-            req->addr = getInput<int>("Address").value_or(42);
-            req->data = getInput<int>("Result").value_or(0);
-            return true;
-        }
-
-        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
-            setOutput<int>("Result", resp->resp.front());
-            return BT::NodeStatus::SUCCESS;
-        }
-    };
-}
--- a/mg_bt/src/tree_nodes/move_point.hpp
+++ b/mg_bt/src/tree_nodes/move_point.hpp
@ -1,70 +0,0 @@
-#pragma once
-
-#include "behaviortree_ros2/bt_action_node.hpp"
-#include "mg_msgs/action/move_point.hpp"
-
-namespace mg {
-
-    class MovePointNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
-      public:
-        MovePointNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
-          BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params) { }
-
-        static BT::PortsList providedPorts() {
-            return providedBasicPorts({ BT::InputPort<double>("x_goal"),
-                                        BT::InputPort<double>("y_goal"),
-                                        BT::InputPort<double>("tolerance"),
-                                        BT::InputPort<double>("max_wheel_speed"),
-                                        BT::InputPort<double>("max_angular"),
-                                        BT::InputPort<double>("max_vel"),
-                                        BT::InputPort<double>("pos_mult"),
-                                        BT::InputPort<double>("ornt_mult"),
-                                        BT::InputPort<double>("t_ornt_mult"),
-                                        BT::InputPort<double>("obst_mult_a"),
-                                        BT::InputPort<double>("obst_mult_b"),
-                                        BT::InputPort<double>("obst_mult_c"),
-                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
-        }
-
-        bool setGoal(Goal& goal) override {
-            auto x_goal          = getInput<double>("x_goal");
-            auto y_goal          = getInput<double>("y_goal");
-            auto tolerance       = getInput<double>("tolerance");
-            auto max_wheel_speed = getInput<double>("max_wheel_speed");
-            auto max_angular     = getInput<double>("max_angular");
-            auto max_vel         = getInput<double>("max_vel");
-            auto pos_mult        = getInput<double>("pos_mult");
-            auto ornt_mult       = getInput<double>("ornt_mult");
-            auto t_ornt_mult     = getInput<double>("t_ornt_mult");
-            auto obst_mult_a     = getInput<double>("obst_mult_a");
-            auto obst_mult_b     = getInput<double>("obst_mult_b");
-            auto obst_mult_c     = getInput<double>("obst_mult_c");
-            goal.x               = x_goal.value();
-            goal.y               = y_goal.value();
-            goal.tolerance       = tolerance.value_or(goal.tolerance);
-            goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
-            goal.max_angular     = max_angular.value_or(goal.max_angular);
-            goal.max_vel         = max_vel.value_or(goal.max_vel);
-            goal.pos_mult        = pos_mult.value_or(goal.pos_mult);
-            goal.ornt_mult       = ornt_mult.value_or(goal.ornt_mult);
-            goal.t_ornt_mult     = t_ornt_mult.value_or(goal.t_ornt_mult);
-            goal.obst_mult_a     = obst_mult_a.value_or(goal.obst_mult_a);
-            goal.obst_mult_b     = obst_mult_b.value_or(goal.obst_mult_b);
-            goal.obst_mult_c     = obst_mult_c.value_or(goal.obst_mult_c);
-            return true;
-        }
-
-        void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
-
-        BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
-            return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
-                                                                       BT::NodeStatus::FAILURE;
-        }
-
-        BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
-            RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
-            return BT::NodeStatus::FAILURE;
-        }
-    };
-
-}
--- a/mg_bt/src/tree_nodes/rotate.hpp
+++ b/mg_bt/src/tree_nodes/rotate.hpp
@ -1,67 +0,0 @@
-#pragma once
-
-#include "behaviortree_ros2/bt_action_node.hpp"
-#include "mg_msgs/action/rotate.hpp"
-
-namespace mg {
-
-    class RotateNode : public BT::RosActionNode<mg_msgs::action::Rotate> {
-      public:
-        RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
-          BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
-
-        static BT::PortsList providedPorts() {
-            return providedBasicPorts({ BT::InputPort<double>("angle"),
-                                        BT::InputPort<double>("tolerance"),
-                                        BT::InputPort<double>("max_wheel_speed"),
-                                        BT::InputPort<double>("max_angular"),
-                                        BT::InputPort<double>("max_vel"),
-                                        BT::InputPort<double>("pos_mult"),
-                                        BT::InputPort<double>("ornt_mult"),
-                                        BT::InputPort<double>("t_ornt_mult"),
-                                        BT::InputPort<double>("obst_mult_a"),
-                                        BT::InputPort<double>("obst_mult_b"),
-                                        BT::InputPort<double>("obst_mult_c"),
-                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
-        }
-
-        bool setGoal(Goal& goal) override {
-            auto angle           = getInput<double>("angle");
-            auto tolerance       = getInput<double>("tolerance");
-            auto max_wheel_speed = getInput<double>("max_wheel_speed");
-            auto max_angular     = getInput<double>("max_angular");
-            auto max_vel         = getInput<double>("max_vel");
-            auto pos_mult        = getInput<double>("pos_mult");
-            auto ornt_mult       = getInput<double>("ornt_mult");
-            auto t_ornt_mult     = getInput<double>("t_ornt_mult");
-            auto obst_mult_a     = getInput<double>("obst_mult_a");
-            auto obst_mult_b     = getInput<double>("obst_mult_b");
-            auto obst_mult_c     = getInput<double>("obst_mult_c");
-            goal.angle           = angle.value();
-            goal.tolerance       = tolerance.value_or(goal.tolerance);
-            goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
-            goal.max_angular     = max_angular.value_or(goal.max_angular);
-            goal.max_vel         = max_vel.value_or(goal.max_vel);
-            goal.pos_mult        = pos_mult.value_or(goal.pos_mult);
-            goal.ornt_mult       = ornt_mult.value_or(goal.ornt_mult);
-            goal.t_ornt_mult     = t_ornt_mult.value_or(goal.t_ornt_mult);
-            goal.obst_mult_a     = obst_mult_a.value_or(goal.obst_mult_a);
-            goal.obst_mult_b     = obst_mult_b.value_or(goal.obst_mult_b);
-            goal.obst_mult_c     = obst_mult_c.value_or(goal.obst_mult_c);
-            return true;
-        }
-
-        void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
-
-        BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
-            return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
-                                                                       BT::NodeStatus::FAILURE;
-        }
-
-        BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
-            RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
-            return BT::NodeStatus::FAILURE;
-        }
-    };
-
-}
--- a/mg_bt/src/tree_nodes/zero.hpp
+++ b/mg_bt/src/tree_nodes/zero.hpp
@ -1,20 +0,0 @@
-#pragma once
-
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "std_srvs/srv/empty.hpp"
-
-namespace mg {
-
-    class ZeroNode : public BT::RosServiceNode<std_srvs::srv::Empty> {
-      public:
-        ZeroNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
-          BT::RosServiceNode<std_srvs::srv::Empty>(name, conf, params) { }
-
-        bool setRequest(typename Request::SharedPtr&) override { return true; }
-
-        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
-            return BT::NodeStatus::SUCCESS;
-        }
-    };
-
-}
--- a/mg_control/assets/mg_base_controllers.yaml
+++ b/mg_control/assets/mg_base_controllers.yaml
@ -12,7 +12,7 @@ diffdrive_controller:
    left_wheel_names: ["left_motor"]
    right_wheel_names: ["right_motor"]
    
-    enable_odom_tf: false 
+    enable_odom_tf: true
    odom_frame_id: odom_excpected
    base_frame_id: base-link

--- a/mg_control/src/mg_control.cpp
+++ b/mg_control/src/mg_control.cpp
@ -11,7 +11,7 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
    if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
        serial_port_name = info_.hardware_parameters["device_path"];
    } else {
-        serial_port_name = "/dev/ttyACM1";
+        serial_port_name = "/dev/ttyACM0";
    }

    left_wheel_pos_state  = 0;
@ -66,18 +66,14 @@ hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&
 }

 hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
-    union {
-        std::array<u_char, sizeof(double)> bytes;
-        double                             data;
-    } value{};
+    std::string cmd_left;
+    std::string cmd_right;
+
+    cmd_left  = std::to_string(left_wheel_vel_cmd / (2 * M_PI)) + " ";
+    cmd_right = std::to_string(-right_wheel_vel_cmd / (2 * M_PI));

    try {
-        odrive_serial_interface.Write("s;");
-        value.data = -left_wheel_vel_cmd / (2 * M_PI);
-        for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
-        value.data = right_wheel_vel_cmd / (2 * M_PI);
-        for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
-
+        odrive_serial_interface.Write(cmd_left + cmd_right);
    } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
    return hardware_interface::return_type::OK;
 }
--- a/mg_lidar/CMakeLists.txt
+++ b/mg_lidar/CMakeLists.txt
@ -1,60 +0,0 @@
-cmake_minimum_required(VERSION 3.12)
-project(mg_lidar)
-
-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()
-
-set(PACKAGE_DEPS
-  rclcpp
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  mg_msgs
-  sensor_msgs
-  )
-
-find_package(ament_cmake REQUIRED)
-
-include(FindPkgConfig)
-pkg_search_module(LIBGLM REQUIRED glm)
-
-foreach(PACKAGE ${PACKAGE_DEPS})
-    find_package(${PACKAGE} REQUIRED)
-endforeach()
-
-set(SOURCES
-    src/scanner.cpp
-)
-
-add_executable(mg_scanner ${SOURCES})
-
-target_include_directories(
-  mg_scanner
-  PRIVATE
-  ${LIBGLM_INCLUDE_DIRS}
-  include
-)
-
-ament_target_dependencies(mg_scanner ${PACKAGE_DEPS})
-
-install( TARGETS
-  mg_scanner
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-install(DIRECTORY
-  config
-  launch
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-target_compile_features(mg_scanner PUBLIC 
-  c_std_99
-  cxx_std_17
-)  # Require C99 and C++17
-
-ament_package()
--- a/mg_lidar/config/lidar.yaml
+++ b/mg_lidar/config/lidar.yaml
@ -1,4 +0,0 @@
-rplidar_node:
-  ros__parameters:
-    scan_mode: "ppbig"
-    topic_name: "base-link"
--- a/mg_lidar/launch/launch.py
+++ b/mg_lidar/launch/launch.py
@ -1,28 +0,0 @@
-from launch import LaunchDescription
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from pathlib import Path
-
-def generate_launch_description():
-
-    basedir = FindPackageShare("mg_lidar")
-
-    lidar_config = PathJoinSubstitution([basedir, "config/lidar.yaml"])
-
-    return LaunchDescription([
-        Node(
-            package='mg_lidar',
-            executable='mg_scanner',
-            output="screen",
-            parameters=[lidar_config]
-        ),
-        Node(
-            package='rplidar_ros',
-            executable='rplidar_composition',
-            output="screen",
-            parameters=[lidar_config]
-        ),
-    ])
-    
--- a/mg_lidar/package.xml
+++ b/mg_lidar/package.xml
@ -1,27 +0,0 @@
-<?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_lidar</name>
-  <version>0.0.0</version>
-  <description>Lidar based opponent tracking</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>mg_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>visualization_msgs</depend>
-  <depend>sensor_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>
--- a/mg_lidar/src/scanner.cpp
+++ b/mg_lidar/src/scanner.cpp
@ -1,134 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-
-#include "sensor_msgs/msg/laser_scan.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-#include "glm/glm.hpp"
-
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2/convert.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include <glm/gtx/norm.hpp>
-
-class MgScanner : public rclcpp::Node {
-    using LaserScan    = sensor_msgs::msg::LaserScan;
-    using PointStamped = geometry_msgs::msg::PointStamped;
-
-  public:
-    MgScanner() : rclcpp::Node("EnemyScanner") {
-        gen_rotations();
-        scan_sup_ = create_subscription<LaserScan>(
-          "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { process_scan(msg); });
-        enemy_pub_ = create_publisher<PointStamped>("/dynamicObstacle", rclcpp::QoS(1));
-
-        tf_buf_      = std::make_shared<tf2_ros::Buffer>(get_clock());
-        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
-    }
-
-  private:
-    rclcpp::Subscription<LaserScan>::SharedPtr  scan_sup_;
-    rclcpp::Publisher<PointStamped>::SharedPtr  enemy_pub_;
-    std::shared_ptr<tf2_ros::Buffer>            tf_buf_;
-    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-
-    std::vector<glm::dmat2> rotations;
-
-    void gen_rotations() {
-        constexpr double min_angle       = -3.1241393089294434;
-        constexpr double max_angle       = 3.1415927410125732;
-        constexpr double angle_increment = 0.008714509196579456;
-
-        double curr = min_angle;
-        while (curr <= max_angle) {
-            rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
-            curr += angle_increment;
-        }
-
-        rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
-    }
-
-    glm::dvec3 pos_query() {
-        RCLCPP_ERROR(get_logger(), "Works to here");
-        try {
-            double x   = NAN;
-            double y   = NAN;
-            double rot = NAN;
-
-            auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
-
-            tf2::Transform t;
-            tf2::convert(tmsg.transform, t);
-            t.getBasis().getRPY(x, y, rot);
-            auto vec3 = tmsg.transform.translation;
-
-            return { vec3.x, vec3.y, rot };
-
-        } catch (const tf2::TransformException& e) {
-            RCLCPP_ERROR(get_logger(), "Got following error when looking up transform:\n\t%s", e.what());
-        }
-        return { 0, 0, 0 };
-    }
-
-    static bool part_of_clump(const glm::dvec2 prev, glm::dvec2 curr) {
-        return glm::length2(prev - curr) < 0.05 * 0.05;
-    }
-
-    void process_scan(LaserScan::ConstSharedPtr msg) {
-        // TODO: finish processing
-        const glm::dvec3 v   = pos_query();
-        const glm::dvec2 pos = { v.x, v.y };
-        const glm::dmat2 rot = { glm::cos(v.z), glm::sin(v.z), -glm::sin(v.z), glm::cos(v.z) };
-
-        double mini = INFINITY;
-
-        glm::dvec2 mini_pos = { 0, 0 };
-        glm::dvec2 prev     = { -1, -1 };
-
-        glm::dvec2 clump      = { -1, -1 };
-        int        clump_size = 0;
-
-        bool good_clump = false;
-
-        if (msg->ranges.size() != rotations.size()) {
-            RCLCPP_ERROR(get_logger(),
-                         "The amount of rotations differs from amount of ranges(%lu != %lu)",
-                         msg->ranges.size(),
-                         rotations.size());
-        }
-
-        for (uint i = 0; i < msg->ranges.size(); i++) {
-            if (msg->intensities.at(i) < 35.0) {
-                continue;
-            }
-            const glm::dvec2 potential_pos = rot * rotations.at(i) * glm::dvec2{ msg->ranges.at(i), 0 } + pos;
-            if (0.1 < potential_pos.x && potential_pos.x < 2.9 && 0.1 < potential_pos.y && potential_pos.y < 1.9) {
-                if (!part_of_clump(prev, potential_pos)) {
-                    clump_size = 0;
-                    clump      = { 0, 0 };
-                }
-                good_clump |= mini > msg->ranges.at(i);
-                clump += potential_pos;
-                clump_size++;
-                if (good_clump) {
-                    mini     = msg->ranges.at(i);
-                    mini_pos = clump / (double)clump_size;
-                }
-            }
-            prev = potential_pos;
-        }
-        if (mini < INFINITY) {
-            PointStamped m;
-            m.header.frame_id = "odom";
-            m.point.x         = mini_pos.x;
-            m.point.y         = mini_pos.y;
-
-            enemy_pub_->publish(m);
-        }
-    }
-};
-
-int main(const int argc, const char** argv) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<MgScanner>());
-    rclcpp::shutdown();
-}
--- a/mg_msgs/CMakeLists.txt
+++ b/mg_msgs/CMakeLists.txt
@ -10,16 +10,12 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)

 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/Path.msg"
  "msg/Point2d.msg"
-  "action/MoveGlobal.action"
  "action/MoveStraight.action"
  "action/MovePoint.action"
  "action/LookAt.action"
  "action/Rotate.action"
-  "srv/CalcPath.srv"
-  "srv/SendDouble.srv"
-  "srv/I2c.srv"
+  "srv/SetWidth.srv"
 )

 ament_package()
--- a/mg_msgs/action/MoveGlobal.action
+++ b/mg_msgs/action/MoveGlobal.action
@ -1,25 +0,0 @@
-float64[] x 
-float64[] y 
-
-float64 step 0.1
-float64 tolerance 0.05
-float64 max_wheel_speed 6.0
-float64 max_angular 3.14
-float64 max_vel 4
-float64 pos_mult 14.0
-float64 ornt_mult 3.0
-float64 t_ornt_mult 0.1
-float64 obst_mult_a 160
-float64 obst_mult_b 4.0
-float64 obst_mult_c 0
-string[] ignore_tags []
---
-uint8 SUCCESS=0
-uint8 BLOCKED=1
-uint8 TIMEOUT=2
-uint8 MISALIGNED=3
-uint8 UNKNOWN=254
-
-uint8 error
---
-float64 distance_passed
--- a/mg_msgs/msg/Path.msg
+++ b/mg_msgs/msg/Path.msg
@ -1 +0,0 @@
-int32[] indicies
--- a/mg_msgs/srv/CalcPath.srv
+++ b/mg_msgs/srv/CalcPath.srv
@ -1,4 +0,0 @@
-float64[] x
-float64[] y
---
-int32[] indicies
--- a/mg_msgs/srv/I2c.srv
+++ b/mg_msgs/srv/I2c.srv
@ -1,4 +0,0 @@
-uint8 addr
-uint8 data
---
-uint8[] resp
--- a/mg_msgs/srv/SendDouble.srv
+++ b/mg_msgs/srv/SendDouble.srv
@ -1,2 +0,0 @@
-float64 data 
---
--- a/mg_msgs/srv/SetWidth.srv
+++ b/mg_msgs/srv/SetWidth.srv
@ -0,0 +1,2 @@
+float64 width
+---
--- a/mg_navigation/CMakeLists.txt
+++ b/mg_navigation/CMakeLists.txt
@ -13,7 +13,6 @@ find_package(geometry_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(mg_obstacles REQUIRED)

 include(FindPkgConfig)
 pkg_search_module(LIBGLM REQUIRED glm)
@ -27,12 +26,10 @@ set(PACKAGE_DEPS
  tf2
  tf2_ros
  tf2_geometry_msgs
-  mg_obstacles
 )

 add_executable(mg_nav_server 
  src/mg_navigation.cpp
-  src/path_buffer.cpp
 )
 ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})

--- a/mg_navigation/include/handlers/dwa.hpp
+++ b/mg_navigation/include/handlers/dwa.hpp
@ -1,8 +0,0 @@
-#pragma once
-
-#include "handlers/dwa_core.hpp"
-#include "handlers/dwa_point.hpp"
-#include "handlers/dwa_global.hpp"
-#include "handlers/dwa_forward.hpp"
-#include "handlers/dwa_lookat.hpp"
-#include "handlers/dwa_rotate.hpp"
--- a/mg_navigation/include/handlers/dwa_global.hpp
+++ b/mg_navigation/include/handlers/dwa_global.hpp
@ -1,106 +0,0 @@
-#pragma once
-#include "handlers/dwa_core.hpp"
-
-#include "glm/gtx/norm.hpp"
-#include "glm/gtx/rotate_vector.hpp"
-#include "glm/gtx/vector_angle.hpp"
-
-#define LOOKAHEAD 0.2
-
-namespace mg {
-
-    template <>
-    inline bool DWA<MgNavigationServer::MoveGlobal>::target_check() {
-        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
-    }
-
-    template <>
-    inline void DWA<MgNavigationServer::MoveGlobal>::target_init() {
-        baseNode.path_buffer()->update_path_block(goal);
-        target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
-
-        dimy = 4;
-        dimx = 11;
-    }
-
-    template <>
-    inline void DWA<MgNavigationServer::MoveGlobal>::target_update() {
-        baseNode.path_buffer()->update_path();
-        target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
-    }
-
-    template <>
-    inline bool DWA<MgNavigationServer::MoveGlobal>::condition_check() {
-        return false;
-    }
-
-    template <>
-    inline double DWA<MgNavigationServer::MoveGlobal>::calc_score(int vl, int vr) {
-        auto [v, w]        = to_vel(step * vl, step * vr);
-        const double delta = 0.3;
-        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));
-
-        const double angl   = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
-        const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
-
-        const double dist            = baseNode.obstacle_manager()->box_colide(n_pos, { 0.29, 0.33 }, n_theta);
-        const double dist2           = baseNode.obstacle_manager()->dist_to_nearest(n_pos) - 0.22;
-        const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
-        const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);
-
-        RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
-        RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
-
-        score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
-        // score += goal->ornt_mult * (angl - n_angl);
-        score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
-        score -= goal->obst_mult_a * obstacle_scoreA;
-        score -= goal->obst_mult_b * obstacle_scoreB;
-
-        return score;
-    }
-
-    template <>
-    inline void DWA<MgNavigationServer::MoveGlobal>::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);
-                }
-            }
-        }
-    }
-
-}
--- a/mg_navigation/include/handlers/dwm.hpp
+++ b/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"
--- a/mg_navigation/include/handlers/dwm_core.hpp
+++ b/mg_navigation/include/handlers/dwm_core.hpp
@ -30,7 +30,7 @@
 namespace mg {

    template <typename T>
-    class DWA {
+    class DWM {
      public:
        using Goal       = typename T::Goal;
        using Result     = typename T::Result;
@ -62,7 +62,7 @@ namespace mg {
        glm::dvec2 pos;
        double     theta = 0;

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

        void execute();

@ -88,7 +88,7 @@ namespace mg {
    };

    template <typename T>
-    DWA<T>::DWA(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
+    DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
      baseNode(mns),
      hgoal(g),
      pub_twist(mns.pub_twist),
@ -99,20 +99,16 @@ namespace mg {
      pos(0, 0) { }

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

        pos_query();
        target_init();

-        rclcpp::Time elapsed = baseNode.get_clock()->now();
        rclcpp::Rate rate(UPDATE_RATE);

        while (target_check() && rclcpp::ok()) {
-            target_update();
-            baseNode.obstacle_manager()->update_dynamic();
-            baseNode.obstacle_manager()->update_static();
            if (hgoal->is_canceling()) {
                cancel();
                return;
@ -141,13 +137,14 @@ namespace mg {
            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() {
+    void DWM<T>::pos_query() {
        try {
            double x = NAN;
            double y = NAN;
@ -167,7 +164,7 @@ namespace mg {
    }

    template <typename T>
-    void DWA<T>::send_speed(double vl, double vr) {
+    void DWM<T>::send_speed(double vl, double vr) {
        auto [v, w] = to_vel(vl, vr);
        Geometry::TwistStamped twist;
        twist.twist.angular.z = w;
@ -177,7 +174,7 @@ namespace mg {
    }

    template <typename T>
-    void DWA<T>::abort(int error) {
+    void DWM<T>::abort(int error) {
        baseNode.mtx.lock();
        send_speed(0, 0);
        auto x   = std::make_shared<Result>();
@ -186,7 +183,7 @@ namespace mg {
    }

    template <typename T>
-    void DWA<T>::succeed() {
+    void DWM<T>::succeed() {
        baseNode.mtx.lock();
        send_speed(0, 0);
        auto x   = std::make_shared<Result>();
@ -195,7 +192,7 @@ namespace mg {
    }

    template <typename T>
-    void DWA<T>::cancel() {
+    void DWM<T>::cancel() {
        baseNode.mtx.lock();
        send_speed(0, 0);
        auto x   = std::make_shared<Result>();
--- a/mg_navigation/include/handlers/dwm_forward.hpp
+++ b/mg_navigation/include/handlers/dwm_forward.hpp
@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwa_core.hpp"
+#include "handlers/dwm_core.hpp"

 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@ -8,22 +8,22 @@
 namespace mg {

    template <>
-    inline bool DWA<MgNavigationServer::MoveStraight>::target_check() {
+    inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
    }

    template <>
-    inline void DWA<MgNavigationServer::MoveStraight>::target_init() {
+    inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
        target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
    }

    template <>
-    inline bool DWA<MgNavigationServer::MoveStraight>::condition_check() {
+    inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
        return false;
    }

    template <>
-    inline double DWA<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
+    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);
@ -37,7 +37,7 @@ namespace mg {
    }

    template <>
-    inline void DWA<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
+    inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
                                                                           std::vector<int>& vr,
                                                                           int               dimx,
                                                                           int) {
--- a/mg_navigation/include/handlers/dwm_lookat.hpp
+++ b/mg_navigation/include/handlers/dwm_lookat.hpp
@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwa_core.hpp"
+#include "handlers/dwm_core.hpp"

 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@ -9,7 +9,7 @@
 namespace mg {

    template <>
-    inline bool DWA<MgNavigationServer::LookAt>::target_check() {
+    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;
@ -18,7 +18,7 @@ namespace mg {
    }

    template <>
-    inline void DWA<MgNavigationServer::LookAt>::target_init() {
+    inline void DWM<MgNavigationServer::LookAt>::target_init() {

        target_pos = glm::vec2(goal->x, goal->y);

@ -30,7 +30,7 @@ namespace mg {
    }

    template <>
-    inline void DWA<MgNavigationServer::LookAt>::target_update() {
+    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;
@ -40,12 +40,12 @@ namespace mg {
    }

    template <>
-    inline bool DWA<MgNavigationServer::LookAt>::condition_check() {
+    inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
        return false;
    }

    template <>
-    inline double DWA<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
+    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;
@ -62,7 +62,7 @@ namespace mg {
    }

    template <>
-    inline void DWA<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
+    inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
                                                                     std::vector<int>& vr,
                                                                     int               dimx,
                                                                     int) {
--- a/mg_navigation/include/handlers/dwm_point.hpp
+++ b/mg_navigation/include/handlers/dwm_point.hpp
@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwa_core.hpp"
+#include "handlers/dwm_core.hpp"

 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@ -8,30 +8,28 @@
 namespace mg {

    template <>
-    inline bool DWA<MgNavigationServer::MovePoint>::target_check() {
+    inline bool DWM<MgNavigationServer::MovePoint>::target_check() {
        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
    }

    template <>
-    inline void DWA<MgNavigationServer::MovePoint>::target_update() {
+    inline void DWM<MgNavigationServer::MovePoint>::target_update() {
        target_pos = glm::dvec2(goal->x, goal->y);
-        dimy       = 8;
    }

    template <>
-    inline bool DWA<MgNavigationServer::MovePoint>::condition_check() {
+    inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
        return false;
    }

    template <>
-    inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
+    inline double DWM<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
        auto [v, w]        = to_vel(step * vl, step * vr);
-        const double delta = 0.3;
+        const double delta = 0.5;
        glm::dvec2   n_pos;
        double       n_theta = NAN;
        double       score   = 0;

-        // The angle we will be facing after lookahead
        n_theta = w * delta;

        if (n_theta <= 1e-6) { //NOLINT
@ -49,18 +47,17 @@ namespace mg {
        const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
        const glm::dvec2 face   = glm::rotate(glm::dvec2(1, 0), theta);

-        // Calculate angle to goal post/pre movement
-        const double angl   = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
-        const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
+        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 * (glm::abs(angl) - glm::abs(n_angl));
+        score += goal->ornt_mult * (angl - n_angl);

        return score;
    }

    template <>
-    inline void DWA<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
+    inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
                                                                        std::vector<int>& vr,
                                                                        int               dimx,
                                                                        int               dimy) {
--- a/mg_navigation/include/handlers/dwm_rotate.hpp
+++ b/mg_navigation/include/handlers/dwm_rotate.hpp
@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwa_core.hpp"
+#include "handlers/dwm_core.hpp"

 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@ -9,7 +9,7 @@
 namespace mg {

    template <>
-    inline bool DWA<MgNavigationServer::Rotate>::target_check() {
+    inline bool DWM<MgNavigationServer::Rotate>::target_check() {

        const double a = glm::abs(theta - target_ornt);
        const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
@ -18,17 +18,17 @@ namespace mg {
    }

    template <>
-    inline void DWA<MgNavigationServer::Rotate>::target_init() {
+    inline void DWM<MgNavigationServer::Rotate>::target_init() {
        target_ornt = goal->angle;
    }

    template <>
-    inline bool DWA<MgNavigationServer::Rotate>::condition_check() {
+    inline bool DWM<MgNavigationServer::Rotate>::condition_check() {
        return false;
    }

    template <>
-    inline double DWA<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
+    inline double DWM<MgNavigationServer::Rotate>::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;
@ -45,7 +45,7 @@ namespace mg {
    }

    template <>
-    inline void DWA<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
+    inline void DWM<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
                                                                     std::vector<int>& vr,
                                                                     int               dimx,
                                                                     int) {
--- a/mg_navigation/include/mg_navigation.hpp
+++ b/mg_navigation/include/mg_navigation.hpp
@ -17,10 +17,6 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/buffer.h"

-#include "mg_obstacles/mg_obstacles.hpp"
-
-#include "path_buffer.hpp"
-
 namespace mg {

    namespace Geometry = geometry_msgs::msg;
@ -29,14 +25,12 @@ namespace mg {
      public:
        RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
        using MovePoint    = mg_msgs::action::MovePoint;
-        using MoveGlobal   = mg_msgs::action::MoveGlobal;
        using MoveStraight = mg_msgs::action::MoveStraight;
        using LookAt       = mg_msgs::action::LookAt;
        using Rotate       = mg_msgs::action::Rotate;

        rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;

-        rclcpp_action::Server<MoveGlobal>::SharedPtr   sv_move_global;
        rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
        rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
@ -49,15 +43,9 @@ namespace mg {

        MgNavigationServer(rclcpp::NodeOptions& opts);

-        std::shared_ptr<PathBuffer>       path_buffer() { return path_buffer_; }
-        std::shared_ptr<ObstacleManager>& obstacle_manager() { return obstacle_manager_; }
-
      private:
        bool is_processing = false;

-        std::shared_ptr<PathBuffer>      path_buffer_;
-        std::shared_ptr<ObstacleManager> obstacle_manager_;
-
        template <typename T>
        rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
                                                typename T::Goal::ConstSharedPtr,
--- a/mg_navigation/include/path_buffer.hpp
+++ b/mg_navigation/include/path_buffer.hpp
@ -1,60 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <mutex>
-#include <thread>
-#include <glm/glm.hpp>
-
-#include "mg_msgs/srv/calc_path.hpp"
-#include "mg_msgs/action/move_global.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-#define AREAX  3000
-#define AREAY  2000
-#define GRID_Y 66
-#define GRID_X 106
-
-#define MIN_X 175
-#define MIN_Y 175
-
-#define MAX_X 2825
-#define MAX_Y 1825
-
-#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
-#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)
-
-namespace mg {
-    inline glm::vec2 GridToCoords(const glm::ivec2 grid) {
-        return glm::vec2{ grid.x, grid.y } * glm::vec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::vec2{ MIN_X, MIN_Y };
-    }
-    inline glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
-    inline glm::vec2  IdToCoords(const int id) { return GridToCoords(IdToGrid(id)) / 1000.0F; }
-
-    class PathBuffer {
-        using PathService = mg_msgs::srv::CalcPath;
-        using PathGoal    = mg_msgs::action::MoveGlobal::Goal::ConstSharedPtr;
-        using PathFuture  = rclcpp::Client<PathService>::SharedFuture;
-
-      public:
-        PathBuffer(rclcpp::Node* node);
-        // ~PathBuffer();
-
-        glm::vec2 get_next(glm::vec2 pos, const double lookahead);
-
-        bool update_path(PathGoal goal = nullptr);
-        void update_path_block(PathGoal goal = nullptr);
-
-      private:
-        int current;
-
-        rclcpp::Node* node_;
-
-        PathGoal                              goal_;
-        PathService::Response::ConstSharedPtr path_msg_;
-
-        rclcpp::Client<PathService>::Client::SharedPtr client_;
-
-        PathFuture resp_;
-    };
-
-}
--- a/mg_navigation/package.xml
+++ b/mg_navigation/package.xml
@ -17,7 +17,6 @@
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
-  <depend>mg_obstacles</depend>
  

  <build_depend>libglm-dev</build_depend>
--- a/mg_navigation/src/mg_navigation.cpp
+++ b/mg_navigation/src/mg_navigation.cpp
@ -4,7 +4,8 @@

 #include "rclcpp/rclcpp.hpp"
 #include "mg_navigation.hpp"
-#include "handlers/dwa.hpp"
+#include "handlers/dwm.hpp"
+
 namespace mg {

    template <typename T>
@ -39,7 +40,7 @@ namespace mg {

    template <typename T>
    void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
-        DWA<T> dwm = DWA<T>(gh, tf2_buffer, *this);
+        DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
        dwm.execute();
        is_processing = false;
        mtx.unlock();
@ -49,21 +50,9 @@ namespace mg {
        tf2_buffer   = std::make_shared<tf2_ros::Buffer>(get_clock());
        tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);

-        path_buffer_ = std::make_shared<PathBuffer>(this);
-
        pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);

-        obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf2_buffer);
-
        using namespace std::placeholders;
-
-        sv_move_global = rclcpp_action::create_server<MoveGlobal>(
-          this,
-          "MoveGlobal",
-          std::bind(&MgNavigationServer::handle_goal<MoveGlobal>, this, _1, _2, "MoveGlobal"),
-          std::bind(&MgNavigationServer::handle_cancel<MoveGlobal>, this, _1, "MoveGlobal"),
-          std::bind(&MgNavigationServer::handle_accepted<MoveGlobal>, this, _1, "MoveGlobal"));
-
        sv_move_point = rclcpp_action::create_server<MovePoint>(
          this,
          "MovePoint",
@ -87,10 +76,10 @@ namespace mg {

        sv_rotate = rclcpp_action::create_server<Rotate>(
          this,
-          "Rotate",
-          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "Rotate"),
-          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "Rotate"),
-          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "Rotate"));
+          "MoveStraight",
+          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "MoveStraight"));
    }

 };
--- a/mg_navigation/src/path_buffer.cpp
+++ b/mg_navigation/src/path_buffer.cpp
@ -1,82 +0,0 @@
-#include "path_buffer.hpp"
-
-#include <glm/gtx/norm.hpp>
-
-#include <chrono>
-using namespace std::chrono_literals;
-
-namespace mg {
-
-    PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) {
-        client_ = node_->create_client<PathService>("GeneratePath");
-    }
-
-    glm::vec2 PathBuffer::get_next(glm::vec2 pos, const double lookahead) {
-        if (current < 0) {
-            current++;
-            while (current < (int)path_msg_->indicies.size()
-                   && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) > lookahead * lookahead) {
-                current++;
-            }
-        }
-        while (current < (int)path_msg_->indicies.size()
-               && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) {
-            current++;
-        }
-        if (current == (int)path_msg_->indicies.size()) {
-            double best = 10000000;
-            int    id   = 0;
-
-            for (int i = 0; i < (int)goal_->x.size(); i++) {
-                const double dist2 = glm::distance2({ goal_->x[i], goal_->y[i] }, pos);
-                if (dist2 < best) {
-                    best = dist2;
-                    id   = i;
-                }
-            }
-            return { goal_->x[id], goal_->y[id] };
-        }
-        return IdToCoords(path_msg_->indicies[current]);
-    }
-
-    bool PathBuffer::update_path(PathGoal goal) {
-        if (goal) {
-            goal_ = goal;
-            client_->prune_pending_requests();
-            auto req = std::make_shared<PathService::Request>();
-            req->x   = goal_->x;
-            req->y   = goal_->y;
-            resp_    = client_->async_send_request(req).share();
-        }
-        if (resp_.wait_for(0s) == std::future_status::ready) {
-
-            path_msg_ = resp_.get();
-            auto req  = std::make_shared<PathService::Request>();
-            req->x    = goal_->x;
-            req->y    = goal_->y;
-            resp_     = client_->async_send_request(req).share();
-            current   = -1;
-            return true;
-        }
-        return false;
-    }
-
-    void PathBuffer::update_path_block(PathGoal goal) {
-        if (goal) {
-            goal_ = goal;
-            client_->prune_pending_requests();
-            auto req = std::make_shared<PathService::Request>();
-            req->x   = goal_->x;
-            req->y   = goal_->y;
-            resp_    = client_->async_send_request(req).share();
-        }
-
-        resp_.wait();
-        path_msg_ = resp_.get();
-        auto req  = std::make_shared<PathService::Request>();
-        req->x    = goal_->x;
-        req->y    = goal_->y;
-        resp_     = client_->async_send_request(req).share();
-        current   = -1;
-    }
-}
--- a/mg_obstacles/CMakeLists.txt
+++ b/mg_obstacles/CMakeLists.txt
@ -1,84 +0,0 @@
-cmake_minimum_required(VERSION 3.12)
-
-project(mg_obstacles)
-
-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)
-
-set(PACKAGE_DEPS
-    rclcpp
-    ament_index_cpp
-    mg_msgs
-    geometry_msgs
-    std_msgs
-    tf2
-    tf2_ros
-    tf2_geometry_msgs
-    jsoncpp
-)
-
-foreach(PACKAGE ${PACKAGE_DEPS})
-    find_package(${PACKAGE} REQUIRED)
-endforeach(PACKAGE ${PACKAGE_DEPS})
-
-include(FindPkgConfig)
-pkg_check_modules(JSONCPP jsoncpp)
-# pkg_search_module(LIBGLM REQUIRED glm)
-
-set(SOURCES
-    src/mg_obstacles.cpp
-    src/sdf.cpp
-    src/static_obstacle.cpp
-    src/permanent_obstacle.cpp
-)
-
-add_library(
-    mg_obstacles
-    SHARED
-    ${SOURCES}
-)
-
-target_include_directories(
-    mg_obstacles
-    PRIVATE
-    ${LIBGLM_INCLUDE_DIRS}
-    ${JSONCPP_INCLUDE_DIRS}
-)
-
-target_link_libraries(mg_obstacles ${JSONCPP_LINK_LIBRARIES})
-
-target_include_directories(
-    mg_obstacles
-    PUBLIC 
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-)
-
-ament_target_dependencies(
-    mg_obstacles
-    ${PACKAGE_DEPS}
-)
-
-
-install(
-    TARGETS mg_obstacles
-    EXPORT export_${PROJECT_NAME}
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin
-)
-
-install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
-install(DIRECTORY obstacles DESTINATION share/${PROJECT_NAME}/)
-
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_include_directories(include)
-
-target_compile_features(mg_obstacles PUBLIC c_std_99 cxx_std_17)
-
-ament_package()
-
--- a/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp
+++ b/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp
@ -1,60 +0,0 @@
-#pragma once
-#include <string>
-#include <glm/glm.hpp>
-#include "rclcpp/rclcpp.hpp"
-#include "mg_obstacles/permanent_obstacle.hpp"
-#include "mg_obstacles/static_obstacle.hpp"
-
-#include "std_msgs/msg/u_int64.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-
-#include "tf2_ros/buffer.h"
-
-namespace mg {
-    class ObstacleManager {
-        using StaticListMsg = std_msgs::msg::UInt64;
-        using DynamicPosMsg = geometry_msgs::msg::PointStamped;
-
-      public:
-        RCLCPP_SMART_PTR_DEFINITIONS(ObstacleManager)
-
-        enum ObstacleMask { // NOLINT
-            DYNAMIC   = 1,
-            STATIC    = 2,
-            MOVABLE   = 3,
-            PERMANENT = 4,
-            IMPORTANT = 5,
-            ALL       = 7,
-        };
-
-        ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf);
-        bool   contains(glm::dvec2 pos, double radius, uint mask = ObstacleMask::ALL);
-        double dist_to_nearest(glm::dvec2 pos, int mask = ObstacleMask::ALL);
-        double box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask = ObstacleMask::ALL);
-
-        static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat);
-        static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rotation);
-
-        void update_dynamic();
-        void update_static();
-
-      private:
-        rclcpp::Node* node_;
-
-        tf2_ros::Buffer::SharedPtr tf_buffer;
-
-        std::vector<StaticObstacle>    static_obstacles_;
-        std::vector<PermanentObstacle> permanent_obstacles_;
-
-        glm::dvec3 oponent_position_ = { 0, 0, 0.20 };
-        bool       openent_active_   = false;
-
-        rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
-
-        rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
-        rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
-
-        void load_permanent(Json::Value& json);
-        void load_static(Json::Value& json);
-    };
-}
--- a/mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp
+++ b/mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp
@ -1,20 +0,0 @@
-#pragma once
-
-#include <glm/glm.hpp>
-#include <jsoncpp/json/json.h>
-
-namespace mg {
-    class PermanentObstacle {
-      public:
-        /* Create a permanent obstacle from a json value */
-        PermanentObstacle(const Json::Value& json);
-
-        glm::dvec2 pos;
-        glm::dvec2 size;
-
-        double distance(glm::dvec2 position) const;
-        bool   contains(glm::dvec2 position, double radius) const;
-        double distanceBox(const glm::dvec2 position, const glm::dvec2 size, const glm::dmat2 rot_mat) const;
-    };
-
-}
--- a/mg_obstacles/include/mg_obstacles/sdf.hpp
+++ b/mg_obstacles/include/mg_obstacles/sdf.hpp
@ -1,16 +0,0 @@
-#include "glm/glm.hpp"
-
-namespace mg {
-    double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t);
-
-    double circleSdf(glm::dvec2 pos, glm::dvec2 t);
-
-    bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius);
-    bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius);
-
-    double boxToBox(const glm::dvec2 pos,
-                    const glm::dvec2 size,
-                    const glm::dvec2 t,
-                    const glm::dvec2 sizet,
-                    const glm::dmat2 rot_mat);
-}
--- a/mg_obstacles/include/mg_obstacles/static_obstacle.hpp
+++ b/mg_obstacles/include/mg_obstacles/static_obstacle.hpp
@ -1,26 +0,0 @@
-#pragma once
-
-#include <glm/glm.hpp>
-#include <jsoncpp/json/json.h>
-
-namespace mg {
-    class StaticObstacle {
-      public:
-        /* Create a static obstacle from a json value */
-        StaticObstacle(const Json::Value& json);
-
-        glm::dvec2 pos;
-
-        bool active = true;
-
-        bool horizontal;
-
-        static double width;
-        static double height;
-
-        double distance(glm::dvec2 position) const;
-        bool   contains(glm::dvec2 position, double radius) const;
-        double distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const;
-    };
-
-}
--- a/mg_obstacles/obstacles/obstacles-static.json
+++ b/mg_obstacles/obstacles/obstacles-static.json
@ -1,56 +0,0 @@
-{
-    "staticObsacleHeight": 0.4,
-    "staticObsacleWidth": 0.1,
-    "staticObstacles": [
-        {
-            "horizontal": true,
-            "x": 0.625,
-            "y": 1.775
-        },
-        {
-            "horizontal": false,
-            "x": 0,
-            "y": 0.6
-        },
-        {
-            "horizontal": false,
-            "x": 0,
-            "y": 1.525
-        },
-        {
-            "horizontal": true,
-            "x": 0.575,
-            "y": 0.3
-        },
-        {
-            "horizontal": true,
-            "x": 2.025,
-            "y": 0.3
-        },
-        {
-            "horizontal": false,
-            "x": 2.9,
-            "y": 0.6
-        },
-        {
-            "horizontal": false,
-            "x": 2.9,
-            "y": 1.525
-        },
-        {
-            "horizontal": true,
-            "x": 1.975,
-            "y": 1.775
-        },
-        {
-            "horizontal": true,
-            "x": 0.9,
-            "y": 1
-        },
-        {
-            "horizontal": true,
-            "x": 1.7,
-            "y": 1
-        }
-    ]
-}
--- a/mg_obstacles/obstacles/obstacles.json
+++ b/mg_obstacles/obstacles/obstacles.json
@ -1,94 +0,0 @@
-{
-  "staticObstacleHeight": 0.4,
-  "staticObstacleWidth": 0.1,
-  "staticObstacles": [
-    {
-      "horizontal": true,
-      "x": 0.625,
-      "y": 1.775
-    },
-    {
-      "horizontal": false,
-      "x": 0,
-      "y": 0.6
-    },
-    {
-      "horizontal": false,
-      "x": 0,
-      "y": 1.525
-    },
-    {
-      "horizontal": true,
-      "x": 0.575,
-      "y": 0.3
-    },
-    {
-      "horizontal": true,
-      "x": 2.025,
-      "y": 0.3
-    },
-    {
-      "horizontal": false,
-      "x": 2.9,
-      "y": 0.6
-    },
-    {
-      "horizontal": false,
-      "x": 2.9,
-      "y": 1.525
-    },
-    {
-      "horizontal": true,
-      "x": 1.975,
-      "y": 1.775
-    },
-    {
-      "horizontal": true,
-      "x": 0.9,
-      "y": 1
-    },
-    {
-      "horizontal": true,
-      "x": 1.7,
-      "y": 1
-    }
-  ],
-  "obstacles": [
-    {
-      "height": 0.45,
-      "width": 1.95,
-      "x": 1.05,
-      "y": 2
-    },
-    {
-      "height": 0.45,
-      "width": 0.45,
-      "x": 1.55,
-      "y": 0.45
-    },
-    {
-      "height": 0.2,
-      "width": 0.4,
-      "x": 0.65,
-      "y": 2
-    },
-    {
-      "height": 0.15,
-      "width": 0.45,
-      "x": 2,
-      "y": 0.15
-    },
-    {
-      "height": 0.15,
-      "width": 0.45,
-      "x": 0,
-      "y": 0.15
-    },
-    {
-      "height": 0.45,
-      "width": 0.45,
-      "x": 0,
-      "y": 1.1
-    }
-  ]
-}
--- a/mg_obstacles/package.xml
+++ b/mg_obstacles/package.xml
@ -1,29 +0,0 @@
-<?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_obstacles</name>
-  <version>0.0.0</version>
-  <description>Library for collision detection</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>
-  <depend>libjsoncpp</depend>
-  
-
-  <build_depend>libjsoncpp-dev</build_depend>
-  <build_depend>libglm-dev</build_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
--- a/mg_obstacles/src/mg_obstacles.cpp
+++ b/mg_obstacles/src/mg_obstacles.cpp
@ -1,204 +0,0 @@
-#include "mg_obstacles/mg_obstacles.hpp"
-#include "mg_obstacles/sdf.hpp"
-
-#include "ament_index_cpp/get_package_share_directory.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-#include "glm/glm.hpp"
-#include "glm/gtx/matrix_transform_2d.hpp"
-
-#include <fstream>
-
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-
-namespace mg {
-    ObstacleManager::ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf) :
-      node_(node), tf_buffer(buf) {
-        no_exec_cbg = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
-        rclcpp::SubscriptionOptions sub_opts;
-        sub_opts.callback_group = no_exec_cbg;
-
-        auto static_cb  = [](StaticListMsg::ConstSharedPtr) {};
-        auto dynamic_cb = [](DynamicPosMsg::ConstSharedPtr) {};
-
-        static_obst_sub_ = node_->create_subscription<StaticListMsg>(
-          "/staticObstacles", rclcpp::QoS(1).keep_last(1), static_cb, sub_opts);
-        dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
-          "/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts);
-
-        std::string obstacle_pkg;
-        std::string file_suffix = "/obstacles/obstacles.json";
-        node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
-        node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json");
-        ulong n = obstacle_pkg.find_first_of('/');
-        if (n < obstacle_pkg.size()) {
-            file_suffix  = obstacle_pkg.substr(n);
-            obstacle_pkg = obstacle_pkg.substr(0, n);
-        }
-        Json::Value json;
-        RCLCPP_INFO(node_->get_logger(),
-                    "Read file %s",
-                    (ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str());
-        std::ifstream json_document(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix);
-
-        json_document >> json;
-
-        load_permanent(json);
-        load_static(json);
-    }
-
-    bool ObstacleManager::contains(glm::dvec2 pos, double radius, uint mask) {
-        bool contained = false;
-        if (mask & ObstacleMask::DYNAMIC && openent_active_) {
-            contained |= inCircleSdf({ oponent_position_.x, oponent_position_.y }, oponent_position_.z, pos, radius);
-        }
-        if (mask & ObstacleMask::STATIC) {
-            for (const auto& obstacle : static_obstacles_) {
-                if (!obstacle.active) {
-                    continue;
-                }
-                // Check Static obstacles
-                contained |= obstacle.contains(pos, radius);
-            }
-        }
-        if (mask & ObstacleMask::PERMANENT) {
-            for (const auto& obstacle : permanent_obstacles_) {
-                // Check Permanant obstacles
-                contained |= obstacle.contains(pos, radius);
-            }
-        }
-        // contained |= dist_to_bounds(pos, { radius, radius }, 0) < 0;
-        return contained;
-    }
-
-    double ObstacleManager::dist_to_nearest(glm::dvec2 pos, int mask) {
-        double nearest = INFINITY;
-
-        if (mask & ObstacleMask::DYNAMIC && openent_active_) {
-            const double x = circleSdf(pos, glm::dvec2(oponent_position_.x, oponent_position_.y));
-
-            nearest = glm::min(nearest, x - oponent_position_.z);
-        }
-
-        if (mask & ObstacleMask::STATIC) {
-            for (const auto& obstacle : static_obstacles_) {
-                if (!obstacle.active) {
-                    continue;
-                }
-                // Check Static obstacles
-                nearest = glm::min(nearest, obstacle.distance(pos));
-            }
-        }
-
-        if (mask & ObstacleMask::PERMANENT) {
-            for (const auto& obstacle : permanent_obstacles_) {
-                // Check Permanant obstacles
-                nearest = glm::min(nearest, obstacle.distance(pos));
-            }
-        }
-
-        return nearest;
-    }
-
-    double ObstacleManager::box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask) {
-        const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } };
-
-        double nearest = INFINITY;
-
-        if (mask & ObstacleMask::DYNAMIC && openent_active_) {
-            const double x = circleSdf(xy, glm::dvec2(oponent_position_.x, oponent_position_.y));
-
-            nearest = glm::min(nearest, x - oponent_position_.z - glm::length(size) / 4);
-        }
-        if (mask & ObstacleMask::STATIC) {
-            for (const auto& obstacle : static_obstacles_) {
-                if (!obstacle.active) {
-                    continue;
-                }
-                // Check Static obstacles
-                nearest = glm::min(nearest, obstacle.distanceBox(xy, size, rot_mat));
-            }
-        }
-
-        if (mask & ObstacleMask::PERMANENT) {
-            for (const auto& obstacle : permanent_obstacles_) {
-                // Check Permanant obstacles
-                nearest = glm::min(nearest, obstacle.distanceBox(xy, size, rot_mat));
-            }
-        }
-
-        return nearest;
-    } // NOLINT
-
-    double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat) {
-        glm::dvec2 point1 = glm::dvec2{ -size.x, -size.y } * 0.5;
-        glm::dvec2 point2 = glm::dvec2{ -size.x, size.y } * 0.5;
-        glm::dvec2 point3 = glm::dvec2{ size.x, -size.y } * 0.5;
-        glm::dvec2 point4 = glm::dvec2{ size.x, size.y } * 0.5;
-
-        point1 = point1 * rot_mat + pos;
-        point2 = point2 * rot_mat + pos;
-        point3 = point3 * rot_mat + pos;
-        point4 = point4 * rot_mat + pos;
-
-        glm::dvec2 mini = glm::min(point1, glm::dvec2(3.0, 2.0) - point1);
-
-        mini = glm::min(glm::min(point2, glm::dvec2(3.0, 2.0) - point2), mini);
-        mini = glm::min(glm::min(point3, glm::dvec2(3.0, 2.0) - point3), mini);
-        mini = glm::min(glm::min(point4, glm::dvec2(3.0, 2.0) - point4), mini);
-
-        return glm::min(mini.x, mini.y);
-    }
-
-    double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rot) {
-        // Find me
-        const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } };
-        return ObstacleManager::dist_to_bounds(pos, size, rot_mat);
-    }
-
-    void ObstacleManager::update_dynamic() {
-        DynamicPosMsg       msg;
-        rclcpp::MessageInfo info;
-        if (dynamic_obst_sub_->take(msg, info)) {
-            auto point = tf_buffer->transform(msg, "odom");
-
-            oponent_position_.x = point.point.x;
-            oponent_position_.y = point.point.y;
-
-            openent_active_ = oponent_position_.x >= 0 && oponent_position_.y >= 0;
-        }
-    }
-
-    void ObstacleManager::update_static() {
-        StaticListMsg       msg;
-        rclcpp::MessageInfo info;
-        if (static_obst_sub_->take(msg, info)) {
-            uint ind = 1;
-            for (int i = (int)static_obstacles_.size() - 1; i >= 0; i--) {
-                static_obstacles_[i].active = ind & msg.data;
-                ind <<= 1;
-            }
-        }
-    }
-
-    void ObstacleManager::load_permanent(Json::Value& json) {
-
-        Json::Value& obstacles = json["obstacles"];
-
-        for (const Json::Value& obstacle : obstacles) {
-            // Add obstacle
-            permanent_obstacles_.emplace_back(obstacle);
-        }
-    }
-
-    void ObstacleManager::load_static(Json::Value& json) {
-
-        StaticObstacle::width  = json["staticObstacleWidth"].asDouble();
-        StaticObstacle::height = json["staticObstacleHeight"].asDouble();
-        for (const Json::Value& obstacle : json["staticObstacles"]) { static_obstacles_.emplace_back(obstacle); }
-        for (const auto& obstacle : static_obstacles_) {
-            RCLCPP_INFO(node_->get_logger(), "Loaded static obstacle at %lf %lf", obstacle.pos.x, obstacle.pos.y);
-        }
-    }
-}
--- a/mg_obstacles/src/permanent_obstacle.cpp
+++ b/mg_obstacles/src/permanent_obstacle.cpp
@ -1,23 +0,0 @@
-#include "mg_obstacles/permanent_obstacle.hpp"
-#include "mg_obstacles/sdf.hpp"
-
-namespace mg {
-    PermanentObstacle::PermanentObstacle(const Json::Value& json) :
-      pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()),
-      size(json.get("width", 0.0).asDouble(), json.get("height", 0.0).asDouble()) { }
-
-    double PermanentObstacle::distance(glm::dvec2 position) const {
-        return boxSdf(pos + 0.5 * glm::dvec2{ size.x, -size.y }, size * 0.5, position);
-    }
-
-    bool PermanentObstacle::contains(glm::dvec2 position, double radius) const {
-        return inBoxSdf(pos + 0.5 * glm::dvec2{ size.x, -size.y }, size * 0.5, position, radius);
-    }
-
-    double PermanentObstacle::distanceBox(const glm::dvec2 position,
-                                          const glm::dvec2 size,
-                                          const glm::dmat2 rot_mat) const {
-
-        return boxToBox(pos + 0.5 * glm::dvec2{ this->size.x, -this->size.y }, this->size, position, size, rot_mat);
-    }
-}
--- a/mg_obstacles/src/sdf.cpp
+++ b/mg_obstacles/src/sdf.cpp
@ -1,53 +0,0 @@
-#include "mg_obstacles/sdf.hpp"
-#include <glm/gtx/norm.hpp>
-
-namespace mg {
-    double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t) {
-        glm::dvec2 d = glm::abs(t - pos) - size;
-        return glm::length(glm::max(d, 0.0)) + glm::min(glm::max(d.x, d.y), 0.0);
-    }
-
-    double circleSdf(glm::dvec2 pos, glm::dvec2 t) { return glm::length(pos - t); }
-
-    bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius) {
-        return glm::length2(pos - t) < glm::pow(rad + radius, 2);
-    }
-
-    bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius) {
-        const glm::dvec2 d = glm::abs(t - pos) - size;
-        // return (glm::length2(glm::max(d, 0.0))) < glm::pow(radius - glm::min(glm::max(d.x, d.y), 0.0), 2);
-        return boxSdf(pos, size, t) - radius < 0;
-    }
-
-    double boxToBox(const glm::dvec2 pos,
-                    const glm::dvec2 size,
-                    const glm::dvec2 t,
-                    const glm::dvec2 sizet,
-                    const glm::dmat2 rot_mat) {
-
-        const glm::dmat2 irot_mat = glm::transpose(rot_mat);
-
-        glm::dvec2 pointA1 = rot_mat * (glm::dvec2{ -sizet.x, -sizet.y } * 0.5) + t;
-        glm::dvec2 pointA2 = rot_mat * (glm::dvec2{ sizet.x, -sizet.y } * 0.5) + t;
-        glm::dvec2 pointA3 = rot_mat * (glm::dvec2{ -sizet.x, sizet.y } * 0.5) + t;
-        glm::dvec2 pointA4 = rot_mat * (glm::dvec2{ sizet.x, sizet.y } * 0.5) + t;
-
-        double distance = boxSdf(pos, size * 0.5, pointA1);
-
-        distance = glm::min(boxSdf(pos, size * 0.5, pointA2), distance);
-        distance = glm::min(boxSdf(pos, size * 0.5, pointA3), distance);
-        distance = glm::min(boxSdf(pos, size * 0.5, pointA4), distance);
-
-        glm::dvec2 pointB1 = irot_mat * (glm::dvec2{ -size.x, -size.y } * 0.5 + pos - t);
-        glm::dvec2 pointB2 = irot_mat * (glm::dvec2{ size.x, -size.y } * 0.5 + pos - t);
-        glm::dvec2 pointB3 = irot_mat * (glm::dvec2{ -size.x, size.y } * 0.5 + pos - t);
-        glm::dvec2 pointB4 = irot_mat * (glm::dvec2{ size.x, size.y } * 0.5 + pos - t);
-
-        distance = glm::min(boxSdf({}, sizet * 0.5, pointB1), distance);
-        distance = glm::min(boxSdf({}, sizet * 0.5, pointB2), distance);
-        distance = glm::min(boxSdf({}, sizet * 0.5, pointB3), distance);
-        distance = glm::min(boxSdf({}, sizet * 0.5, pointB4), distance);
-
-        return distance;
-    }
-}
--- a/mg_obstacles/src/static_obstacle.cpp
+++ b/mg_obstacles/src/static_obstacle.cpp
@ -1,46 +0,0 @@
-#include "mg_obstacles/static_obstacle.hpp"
-#include "mg_obstacles/sdf.hpp"
-
-namespace mg {
-
-    double StaticObstacle::width  = 0.1;
-    double StaticObstacle::height = 0.4;
-
-    StaticObstacle::StaticObstacle(const Json::Value& json) :
-      pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()),
-      horizontal(json.get("horizontal", true).asBool()) { }
-
-    double StaticObstacle::distance(glm::dvec2 position) const {
-        const double width  = StaticObstacle::width;
-        const double height = StaticObstacle::height;
-        if (!horizontal) {
-            return boxSdf(pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position);
-        } else {
-            return boxSdf(pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position);
-        }
-    }
-
-    double StaticObstacle::distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const {
-        const double width  = StaticObstacle::width;
-        const double height = StaticObstacle::height;
-        if (!horizontal) {
-            return boxToBox(
-              pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height }, position, size, rot_mat);
-        } else {
-            return boxToBox(
-              pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width }, position, size, rot_mat);
-        }
-    }
-
-    bool StaticObstacle::contains(glm::dvec2 position, double radius) const {
-        const double width  = StaticObstacle::width;
-        const double height = StaticObstacle::height;
-        if (!horizontal) {
-            return inBoxSdf(
-              pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position, radius);
-        } else {
-            return inBoxSdf(
-              pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position, radius);
-        }
-    }
-}
--- a/mg_odometry/src/mg_odom_publisher.cpp
+++ b/mg_odometry/src/mg_odom_publisher.cpp
@ -6,9 +6,8 @@
 #include <rclcpp/service.hpp>
 #include <string>
 #include <libserial/SerialStream.h>
-#include <sys/types.h>

-#include "mg_msgs/srv/send_double.hpp"
+#include "mg_msgs/srv/set_width.hpp"
 #include "std_srvs/srv/empty.hpp"

 #include "geometry_msgs/msg/transform_stamped.hpp"
@ -25,8 +24,8 @@

 constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";

-using SendDoubleSrv = mg_msgs::srv::SendDouble;
-using ZeroSrv       = std_srvs::srv::Empty;
+using SetWidthSrv = mg_msgs::srv::SetWidth;
+using ZeroSrv     = std_srvs::srv::Empty;

 class MgOdomPublisher : public rclcpp::Node {
  public:
@ -40,73 +39,39 @@ class MgOdomPublisher : public rclcpp::Node {
        timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
                                         std::bind(&MgOdomPublisher::timer_callback, this));

-        set_width_service_ = create_service<SendDoubleSrv>(
+        set_width_service_ = create_service<SetWidthSrv>(
          "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
-        set_ratio_service_ = create_service<SendDoubleSrv>(
-          "/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));

        zero_service_
          = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
-
-        calib_service_
-          = create_service<ZeroSrv>("/endCalib", std::bind(&MgOdomPublisher::calib, this, std::placeholders::_1));
    }

  private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_width_service_;
-    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_ratio_service_;
+    rclcpp::Service<SetWidthSrv>::SharedPtr        set_width_service_;
    rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
-    rclcpp::Service<ZeroSrv>::SharedPtr            calib_service_;
    rclcpp::TimerBase::SharedPtr                   timer_;
    LibSerial::SerialStream                        enc_serial_port_;

-    void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
-        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
+    void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
+        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);

-        if (enc_serial_port_.IsOpen()) {
+        if (!enc_serial_port_.IsOpen()) {
            union {
-                std::array<u_char, sizeof(double)> bytes;
+                std::array<double, sizeof(double)> bytes;
                double                             data;
            } value{};
-            value.data = request->data;
+            value.data = request->width;

            enc_serial_port_ << "w;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
        }
    }

-    void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
-        RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);
-
-        if (enc_serial_port_.IsOpen()) {
-            union {
-                std::array<u_char, sizeof(double)> bytes;
-                double                             data;
-            } value{};
-            value.data = request->data;
-
-            enc_serial_port_ << "r;";
-            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
-        }
-    }
-
-    void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
-        RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib");
-
-        if (enc_serial_port_.IsOpen()) {
-            enc_serial_port_ << "c;";
-            double left_gain  = 0;
-            double right_gain = 0;
-            enc_serial_port_ >> left_gain >> right_gain;
-            RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain);
-        }
-    }
-
    void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
        RCLCPP_INFO(get_logger(), "Zeroing odometry");

-        if (enc_serial_port_.IsOpen()) {
+        if (!enc_serial_port_.IsOpen()) {
            enc_serial_port_ << "z;";
        }
    }
--- a/mg_planner/CMakeLists.txt
+++ b/mg_planner/CMakeLists.txt
@ -1,60 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(mg_planner)
-
-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)
-find_package(mg_obstacles REQUIRED)
-
-include(FindPkgConfig)
-pkg_search_module(LIBGLM REQUIRED glm)
-
-set(PACKAGE_DEPS
-  rclcpp
-  mg_msgs
-  geometry_msgs
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  mg_obstacles
-)
-
-add_executable(mg_planner
-  src/mg_planner.cpp
-  src/mg_planner_node.cpp
-  src/obstacleManager.cpp
-  src/a_star.cpp
-)
-
-ament_target_dependencies(mg_planner ${PACKAGE_DEPS})
-
-target_include_directories(
-  mg_planner
-  PRIVATE
-  ${LIBGLM_INCLUDE_DIRS}
-  include
-)
-
-target_link_libraries(
-  mg_planner
-  ${LIBGLM_LIBRARIES}
-)
-
-install( TARGETS
-  mg_planner
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-target_compile_features(mg_planner PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-
-ament_package()
-
--- a/mg_planner/include/mg_planner/a_star/a_star.hpp
+++ b/mg_planner/include/mg_planner/a_star/a_star.hpp
@ -1,72 +0,0 @@
-#pragma once
-
-#include "mg_planner/config.hpp"
-#include "rclcpp/node.hpp"
-
-#include <glm/fwd.hpp>
-#include <glm/glm.hpp>
-#include <glm/gtx/fast_square_root.hpp>
-#include <memory>
-#include <queue>
-#include <rclcpp/parameter.hpp>
-#include <vector>
-
-#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
-#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)
-
-namespace mg {
-
-    class PlannerNode;
-
-    constexpr std::array<glm::ivec2, 9> neighbors{ glm::ivec2(1, 0), glm::ivec2(1, 1), glm::ivec2(1, 2),
-                                                   glm::ivec2(1, 3), glm::ivec2(2, 1), glm::ivec2(2, 3),
-                                                   glm::ivec2(3, 1), glm::ivec2(3, 2), glm::ivec2(0, 1) };
-
-    constexpr std::array<glm::ivec2, 4> directions{
-        glm::ivec2(1, 1), glm::ivec2(1, -1), glm::ivec2(-1, -1), glm::ivec2(-1, 1)
-    };
-
-    static inline std::array<float, 9> precalc_Distances() {
-        std::array<float, 9> dist{};
-        for (int i = 0; i < 9; i++) { dist[i] = glm::fastLength((glm::vec2)neighbors[i]); }
-        return dist;
-    }
-
-    const std::array<float, 9> distances = precalc_Distances();
-
-    class AStar {
-      public:
-        friend mg::PlannerNode;
-        AStar()        = delete;
-        AStar(AStar&&) = delete;
-        AStar(AStar&)  = delete;
-
-        static glm::ivec2 CoordsToGrid(const glm::ivec2 pos) {
-            return (pos - glm::ivec2{ MIN_X, MIN_Y }) / glm::ivec2{ GRID_X_SIZE, GRID_Y_SIZE };
-        }
-
-        static glm::ivec2 GridToCoords(const glm::ivec2 grid) {
-            return grid * glm::ivec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::ivec2{ MIN_X, MIN_Y };
-        }
-
-        static glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
-        static int        GridToId(const glm::ivec2 grid) { return grid.x + GRID_X * grid.y; }
-
-        static glm::ivec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)); }
-        static int        CoordsToId(const glm::ivec2 pos) { return GridToId(CoordsToGrid(pos)); }
-
-        static void gen_path(int start, std::array<int, (GRID_X * GRID_Y)>& parents, int end, std::vector<int>& path);
-        float       dist_to_goal(glm::ivec2 pos);
-
-        void execute(glm::ivec2 pos, std::vector<glm::ivec2>&& goals, std::vector<int>& path);
-
-        bool isRunning() const { return running; }
-
-      private:
-        AStar(PlannerNode* node) : node_(node) { }
-
-        bool                    running;
-        PlannerNode*            node_;
-        std::vector<glm::ivec2> targets_;
-    };
-}
--- a/mg_planner/include/mg_planner/config.hpp
+++ b/mg_planner/include/mg_planner/config.hpp
@ -1,24 +0,0 @@
-#pragma once
-
-#define AREAX  3000
-#define AREAY  2000
-#define GRID_Y 66ul
-#define GRID_X 106ul
-
-#define MIN_X 175
-#define MIN_Y 175
-
-#define MAX_X 2825
-#define MAX_Y 1825
-
-#define SEARCH_DISTANCE 5
-
-#define CLOSE       100000
-#define MAXOBSTC    50
-#define CLOSEDIST   110
-#define GOALDELTA2  2500
-#define ROBOTRADIUS 165
-
-#define ELIPSEPERCENT 0.5
-#define GOALPERCENT   0.1
-#define MAXDTIME      0.1
--- a/mg_planner/include/mg_planner/mg_planner_node.hpp
+++ b/mg_planner/include/mg_planner/mg_planner_node.hpp
@ -1,43 +0,0 @@
-#pragma once
-
-#include <array>
-#include <glm/fwd.hpp>
-#include <memory>
-#include <rclcpp/client.hpp>
-#include <rclcpp/service.hpp>
-#include "mg_planner/a_star/a_star.hpp"
-#include "mg_obstacles/mg_obstacles.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-
-#include "mg_msgs/msg/path.hpp"
-#include "mg_msgs/srv/calc_path.hpp"
-
-#define TREE_SIZE 9
-
-namespace mg {
-
-    class PlannerNode : public rclcpp::Node {
-      public:
-        PlannerNode();
-
-        void fill_obstacles() { }
-
-        ObstacleManager::SharedPtr getObstacleManager() { return obstacle_manager_; }
-
-        glm::ivec2 get_pos();
-
-      private:
-        rclcpp::Service<mg_msgs::srv::CalcPath>::SharedPtr astar_service;
-        rclcpp::Publisher<mg_msgs::msg::Path>::SharedPtr   path_pub_;
-
-        AStar                      astar_;
-        ObstacleManager::SharedPtr obstacle_manager_;
-
-        std::shared_ptr<tf2_ros::Buffer>            tf_buf_;
-        std::shared_ptr<tf2_ros::TransformListener> tf_listener;
-    };
-
-}
--- a/mg_planner/include/mg_planner/obstacleManager.hpp
+++ b/mg_planner/include/mg_planner/obstacleManager.hpp
@ -1,8 +0,0 @@
-#pragma once
-
-#include <glm/glm.hpp>
-#include <vector>
-
-namespace mg {
-    bool check_collision(glm::ivec2 pos, float size);
-}
--- a/mg_planner/package.xml
+++ b/mg_planner/package.xml
@ -1,26 +0,0 @@
-<?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_planner</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>mg_msgs</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>mg_obstacles</depend>
-  
-
-  <build_depend>libglm-dev</build_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
--- a/mg_planner/src/a_star.cpp
+++ b/mg_planner/src/a_star.cpp
@ -1,108 +0,0 @@
-#include "mg_planner/a_star/a_star.hpp"
-#include "mg_planner/config.hpp"
-#include "mg_planner/mg_planner_node.hpp"
-#include "mg_planner/obstacleManager.hpp"
-#include <array>
-#include <cmath>
-#include <codecvt>
-#include <functional>
-#include <glm/fwd.hpp>
-#include <glm/geometric.hpp>
-#include <iterator>
-#include <memory>
-#include <queue>
-#include <rclcpp/logging.hpp>
-#include <rcutils/types/rcutils_ret.h>
-#include <unordered_set>
-#include <vector>
-
-namespace mg {
-
-    void AStar::execute(glm::ivec2 pos, std::vector<glm::ivec2>&& goals, std::vector<int>& path) {
-        std::
-          priority_queue<std::pair<float, int>, std::vector<std::pair<float, int>>, std::greater<std::pair<float, int>>>
-                                             pq_tasks;
-        std::array<bool, (GRID_X * GRID_Y)>  visited{};
-        std::array<int, (GRID_X * GRID_Y)>   parent{};
-        std::array<float, (GRID_X * GRID_Y)> total_distance{};
-
-        targets_         = std::move(goals);
-        const int posIdx = CoordsToId(pos);
-
-        RCLCPP_INFO(node_->get_logger(), "Got pos: %d %d", pos.x, pos.y);
-
-        pq_tasks.emplace(0, posIdx);
-        visited.at(posIdx) = true;
-        node_->fill_obstacles();
-        int          id_close   = posIdx;
-        float        dist_close = INFINITY;
-        rclcpp::Time time       = node_->get_clock()->now();
-
-        node_->getObstacleManager()->update_static();
-        node_->getObstacleManager()->update_dynamic();
-
-        while (!pq_tasks.empty() && (node_->get_clock()->now() - time).seconds() < 0.1) {
-            const auto [score, idx]   = pq_tasks.top();
-            const glm::ivec2 grid_loc = IdToGrid(idx);
-            pq_tasks.pop();
-
-            RCLCPP_DEBUG(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y);
-
-            for (int i = 0; i < 4; i++) {
-                for (int j = 0; j < 9; j++) {
-                    const glm::ivec2 gridy = IdToGrid(idx) + neighbors[j] * directions[i];
-                    const int        idy   = GridToId(gridy);
-                    //RCLCPP_INFO(node_->get_logger(), "Checking [idx, pos]: %d, %d;%d", idy, gridy.x, gridy.y);
-
-                    if (check_collision(GridToCoords(gridy), 0)
-                        && !node_->getObstacleManager()->contains(
-                          glm::dvec2(GridToCoords(gridy)) / 1000.0, 0.195, ObstacleManager::ObstacleMask::ALL)) {
-                        if (!visited[idy]) {
-                            parent[idy]         = idx;
-                            visited[idy]        = true;
-                            total_distance[idy] = distances[j] + total_distance[idx];
-
-                            RCLCPP_DEBUG(node_->get_logger(), "Gridy: %d %d", gridy.x, gridy.y);
-                            RCLCPP_DEBUG(node_->get_logger(), "idy: %d", idy);
-                            RCLCPP_DEBUG(node_->get_logger(), "Distance: %f", total_distance[idy]);
-                            const float distance = dist_to_goal(gridy);
-                            if (distance < dist_close) {
-                                id_close   = idy;
-                                dist_close = distance;
-                            }
-                            pq_tasks.emplace(total_distance[idy] + distance, idy);
-                            for (const auto& goal : targets_) {
-                                if (goal == gridy) {
-                                    gen_path(posIdx, parent, idy, path);
-                                    return;
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-        gen_path(posIdx, parent, id_close, path);
-    }
-
-    void AStar::gen_path(int start, std::array<int, (GRID_X * GRID_Y)>& parents, int end, std::vector<int>& path) {
-
-        int curr = end;
-
-        while (curr != start) {
-            path.push_back(curr);
-            curr = parents[curr];
-        }
-
-        path.push_back(start);
-    }
-
-    float AStar::dist_to_goal(glm::ivec2 pos) {
-        float mini = INFINITY;
-        for (const auto& goal : targets_) {
-            // Find closest of multiple targets
-            mini = glm::min(glm::distance((glm::vec2)goal, (glm::vec2)pos), mini);
-        }
-        return mini;
-    }
-}
--- a/mg_planner/src/mg_planner.cpp
+++ b/mg_planner/src/mg_planner.cpp
@ -1,14 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-
-#include "mg_planner/mg_planner_node.hpp"
-#include <rclcpp/executors.hpp>
-
-int main(int argc, char** argv) {
-    rclcpp::init(argc, argv);
-    rclcpp::executors::MultiThreadedExecutor executor;
-    rclcpp::Node::SharedPtr                  node = std::make_shared<mg::PlannerNode>();
-    executor.add_node(node);
-    executor.spin();
-
-    return 0;
-}
--- a/mg_planner/src/mg_planner_node.cpp
+++ b/mg_planner/src/mg_planner_node.cpp
@ -1,65 +0,0 @@
-#include "mg_planner/mg_planner_node.hpp"
-#include "mg_msgs/srv/calc_path.hpp"
-#include "mg_planner/a_star/a_star.hpp"
-#include "mg_planner/config.hpp"
-#include <array>
-#include <exception>
-#include <iostream>
-#include <memory>
-#include <rclcpp/time.hpp>
-#include <rclcpp/wait_result_kind.hpp>
-
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "tf2/convert.hpp"
-
-mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
-    astar_service = create_service<mg_msgs::srv::CalcPath>(
-      "GeneratePath",
-      [this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req,
-             mg_msgs::srv::CalcPath::Response::SharedPtr     resp) -> void {
-          std::cout << "Bro recieved request\n";
-          auto                    elapsed = get_clock()->now();
-          std::vector<glm::ivec2> goals;
-          std::transform(req->x.begin(),
-                         req->x.end(),
-                         req->y.begin(),
-                         std::back_inserter(goals),
-                         [](const auto& aa, const auto& bb) {
-                             std::cout << aa << " " << bb << "\n";
-                             return AStar::CoordsToGrid(glm::ivec2(aa * 1000, bb * 1000));
-                         });
-          for (const auto& goal : goals) {
-              if (goal.x < 0 || GRID_X < goal.x || goal.y < 0 || GRID_Y < goal.y) {
-                  return;
-              }
-          }
-          astar_.execute(get_pos(), std::move(goals), resp->indicies);
-          std::reverse(resp->indicies.begin(), resp->indicies.end());
-          mg_msgs::msg::Path path;
-          path.indicies = std::vector<int>(resp->indicies);
-          path_pub_->publish(path);
-          RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
-      });
-
-    path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));
-
-    tf_buf_     = std::make_shared<tf2_ros::Buffer>(get_clock());
-    tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
-
-    obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf_buf_);
-}
-
-glm::ivec2 mg::PlannerNode::get_pos() {
-    try {
-        auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
-
-        tf2::Transform t;
-        tf2::convert(tmsg.transform, t);
-        auto vec3 = tmsg.transform.translation;
-
-        return glm::clamp(
-          glm::ivec2{ vec3.x * 1000, vec3.y * 1000 }, glm::ivec2(MIN_X, MIN_Y), glm::ivec2(MAX_X, MAX_Y));
-
-    } catch (const std::exception& e) { RCLCPP_DEBUG(get_logger(), "Failed to find transform, ohno"); }
-    return { 1000, 1000 };
-}
--- a/mg_planner/src/obstacleManager.cpp
+++ b/mg_planner/src/obstacleManager.cpp
@ -1,8 +0,0 @@
-#include "mg_planner/obstacleManager.hpp"
-#include "mg_planner/config.hpp"
-
-namespace mg {
-    bool check_collision(glm::ivec2 pos, float size) {
-        return (pos.x >= MIN_X && pos.x < MAX_X) && (pos.y >= MIN_Y && pos.y < MAX_Y);
-    }
-}
Author	SHA1	Message	Date
Pimpest	4b30d10783	Added dwm rotate	2025-02-26 16:29:20 +01:00
Pimpest	74c5bc77cc	Added rotate action	2025-02-26 16:26:35 +01:00
Pimpest	4835dd8eda	Added Zero and SetWidth services	2025-02-26 02:07:30 +01:00
Pimpest	7311085be0	Added SetWidth service	2025-02-26 02:07:07 +01:00
Pimpest	447d9a5fa6	Added more missing dependencies	2025-02-26 02:06:50 +01:00
Pimpest	b28463635c	Added mg_msgs as requirement to magrob	2025-02-25 19:17:42 +01:00