Added launch_files

*facepalm*

.clang-tidy

@@ -25,6 +25,10 @@ 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: ''

.gitmodules

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

firmware/README.md

@@ -0,0 +1,6 @@
+# Magrob firmware
+
+This directory contains the firmware used for our robot.
+
+### Base
+This code was meant to be used for the wheel base 

firmware/base/CMakeLists.txt

@@ -0,0 +1,50 @@
+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)

firmware/base/pico_sdk_import.cmake

@@ -0,0 +1,121 @@
+# 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})

firmware/base/pio/blink.pio

@@ -0,0 +1,31 @@
+.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);
+}
+%}

firmware/base/pio/quadrature.pio

@@ -0,0 +1,208 @@
+;
+; 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);
+}
+
+%}

firmware/base/src/config.h

@@ -0,0 +1,25 @@
+#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
+//======================================================

firmware/base/src/main.c

@@ -0,0 +1,288 @@
+#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 if(type == 'X'){
+                        base_x = *((double*)&data);
+                    } else if(type == 'Y'){
+                        base_y = *((double*)&data);
+                    } else if(type == 'T'){
+                        base_theta = *((double*)&data);
+                        calib_enc.left_pulse = 0;
+                        calib_enc.right_pulse = 0;
+                    } 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)'X' << 8) | ';')){
+                readNum = 8;
+                type = (cmd >> 8) & 0xFF;
+            } else if(cmd == (((uint16_t)'Y' << 8) | ';')){
+                readNum = 8;
+                type = (cmd >> 8) & 0xFF;
+            } else if(cmd == (((uint16_t)'T' << 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);
+    }
+}
+

firmware/base/src/quadrature.c

@@ -0,0 +1,184 @@
+/**
+ * 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;
+}

firmware/base/src/quadrature.h

@@ -0,0 +1,27 @@
+#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);

firmware/base/src/stepper.c

@@ -0,0 +1,43 @@
+#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);
+}
+

firmware/base/src/stepper.h

@@ -0,0 +1,8 @@
+#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);

firmware/base/tusb/tusb_config.h

@@ -0,0 +1,47 @@
+#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

firmware/base/tusb/tusb_descriptors.c

@@ -0,0 +1,143 @@
+#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;
+}

firmware/topfloor/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.12)
+
+set(PICO_BOARD pico CACHE STRING "Board type")
+include(pico_sdk_import.cmake)    
+
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+project(i2c C CXX ASM)
+
+pico_sdk_init()
+
+
+add_executable(topfloor
+    src/main.c 
+    src/actions/user.c 
+    src/servo/servo.c
+    src/stepper/stepper.c
+    )
+
+pico_set_program_name(topfloor "i2ctest")
+pico_set_program_version(topfloor "0.1")
+
+pico_enable_stdio_uart(topfloor 0)
+pico_enable_stdio_usb(topfloor 1)
+
+target_link_libraries(topfloor
+    pico_stdlib
+    pico_stdio
+    pico_time
+    pico_i2c_slave
+    pico_multicore
+    hardware_i2c
+    hardware_pwm
+)
+
+target_include_directories(topfloor PRIVATE
+  ${CMAKE_CURRENT_LIST_DIR}
+  src/
+)
+
+pico_add_extra_outputs(topfloor)

firmware/topfloor/pico_sdk_import.cmake

@@ -0,0 +1,121 @@
+# 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})

firmware/topfloor/src/actions/user.c

@@ -0,0 +1,62 @@
+#include "user.h"
+#include "../servo/servo.h"
+#include "../stepper/stepper.h"
+
+#include "stdio.h"
+
+volatile uint8_t state_complete = 0;
+
+static uint8_t current_state = 0;
+
+#define START_PIN 22
+
+void setup() {
+    gpio_init(START_PIN);
+    gpio_pull_up(START_PIN);
+    gpio_set_dir(START_PIN, GPIO_IN);
+
+    gpio_init(25);
+    gpio_set_dir(25, GPIO_OUT);
+}
+
+void loop(uint8_t requested_state) {
+    // printf("Requested state is %d\n", requested_state);
+    if(current_state == 0) {
+        current_state = requested_state;
+    }
+    switch (current_state) {
+        case 0: 
+            break;
+        case 1: 
+            startup_mode();
+            break;
+    }
+    return;
+}
+
+void startup_mode() {
+    printf("Waiting for pin\n");
+    int debounce = 0;
+    while(debounce < 10) {
+        if(gpio_get(START_PIN)) {
+            debounce++;
+        } else {
+            debounce = 0;
+        }
+        sleep_ms(10);
+    }
+    gpio_put(25,1);
+    printf("Waiting for pin to get yanked\n");
+    debounce = 0;
+    while(debounce < 10) {
+        if(!gpio_get(START_PIN)) {
+            debounce++;
+        } else {
+            debounce = 0;
+        }
+        sleep_ms(10);
+    }
+    current_state = 0;
+    state_complete = 1;
+    gpio_put(25,0);
+}

firmware/topfloor/src/actions/user.h

@@ -0,0 +1,21 @@
+#pragma once
+
+// Ne brisatic vec implementirati
+#include "stdint.h"
+
+void setup();
+void loop(uint8_t requested_state);
+
+void startup_mode();
+
+#define NOOP 0x0
+
+/* Actions to build tower */
+#define PIKCUP 0x1
+#define DROP_CAN 0x2
+
+#define RAISE_LEVEL_1 0x3
+#define RAISE_LEVEL_2 0x3
+#define RAISE_LEVEL_3 0x3
+
+extern volatile uint8_t state_complete;

firmware/topfloor/src/main.c

@@ -0,0 +1,85 @@
+#include "hardware/i2c.h"
+#include "pico/i2c_slave.h"
+#include "pico/stdlib.h"
+#include "pico/multicore.h"
+#include "pico/util/queue.h"
+#include "stdio.h"
+
+#include "actions/user.h"
+
+#define I2C_PORT i2c1       // Use I2C0 on GPIO 8 (SDA) and GPIO 9 (SCL)
+#define I2C_SLAVE_ADDR 0x41 // Must match Raspberry Pi 4
+
+#define GPIO_SDA 18
+#define GPIO_SCL 19
+
+volatile uint8_t requested_state = 0;
+
+queue_t queue;
+
+
+bool Start = true;
+
+// Callback function for I2C slave write operation
+void i2c_slave_callback(i2c_inst_t *i2c, i2c_slave_event_t event) {
+  if (event == I2C_SLAVE_RECEIVE) {
+    // Data is received
+    char byte = 0;
+    state_complete = 0;
+    byte = i2c_read_byte_raw(i2c); // Read data from master // Null-terminate string
+    printf("Received: %d\n", byte);
+    queue_add_blocking(&queue,&byte);
+  } else if (event == I2C_SLAVE_REQUEST) {
+    if (Start) {
+      if(state_complete > 0) {
+        printf("Sent: %d\n", state_complete);
+        i2c_write_byte_raw(i2c, state_complete);
+        state_complete = 0;
+      } else {
+        i2c_write_byte_raw(i2c, '\x00');
+      }
+      Start = false;
+    } else {
+      i2c_write_byte_raw(i2c, '\x00');
+    }
+  } else if (event == I2C_SLAVE_FINISH) {
+    Start = true;
+  }
+}
+
+void core2() {
+  setup();
+  while(true) {
+    uint8_t req = 0;
+    if(!queue_is_empty(&queue)) {
+      queue_remove_blocking(&queue, &req); 
+      printf("Recieved req %d", req);
+    }
+    loop(req);
+  }
+}
+
+int main() {
+  stdio_init_all();
+
+  // Initialize I2C as a slave
+  i2c_init(I2C_PORT, 100000);          // 100kHz is a safe speed
+  gpio_set_function(GPIO_SDA, GPIO_FUNC_I2C); // SDA pin (GPIO 8)
+  gpio_set_function(GPIO_SCL, GPIO_FUNC_I2C); // SCL pin (GPIO 9)
+  gpio_pull_up(GPIO_SDA);
+  gpio_pull_up(GPIO_SCL);
+
+  queue_init(&queue, 1, 2);
+
+
+  // Enable the I2C slave events
+  i2c_slave_init(I2C_PORT, I2C_SLAVE_ADDR, &i2c_slave_callback);
+
+  multicore_launch_core1(&core2);
+
+  printf("Pico I2C Slave Ready...\n");
+
+  while (1) {
+    tight_loop_contents(); // Keeps the CPU in a tight loop waiting for events
+  }
+}

firmware/topfloor/src/servo/servo.c

@@ -0,0 +1,35 @@
+#include <hardware/structs/clocks.h>
+#include <stdint.h>
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "hardware/clocks.h"
+
+#include "servo.h"
+
+#define ROTATE_180 2500
+#define ROTATE_0 500
+
+
+void setup_servo(uint8_t pin) {
+    // Initialize the GPIO pin for PWM output
+    gpio_set_function(pin, GPIO_FUNC_PWM);
+    
+    // Get the PWM slice for the given pin
+    uint slice_num = pwm_gpio_to_slice_num(pin);
+
+    // Set the frequency and wrap (period)
+    uint32_t clk  = clock_get_hz(clk_sys);
+    uint32_t div  = clk / 20000/ 50;
+    pwm_set_clkdiv_int_frac4(slice_num,div,1);  // Set the clock divider
+    pwm_set_wrap(slice_num, 20000);
+
+    // Enable the PWM
+    pwm_set_enabled(slice_num, true);
+}
+
+void set_degree(uint8_t pin, float deg) {
+
+    int duty = (((float)(ROTATE_180 - ROTATE_0) / 180.0) * deg) + ROTATE_0;
+    pwm_set_gpio_level(pin, duty);
+}

firmware/topfloor/src/servo/servo.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include "pico/stdlib.h"
+
+void setup_servo(uint8_t pin);
+
+void set_degree(uint8_t pin, float angle);

firmware/topfloor/src/stepper/indentity.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#define INVALID 0
+#define STEPPER_DRIVER 1
+#define SERVO_DRIVER 2

firmware/topfloor/src/stepper/stepper.c

@@ -0,0 +1,24 @@
+#include "stepper.h"
+
+
+
+
+stepper setup_stepper(uint8_t dir, uint8_t pulse) {
+    gpio_init(dir);
+    gpio_init(pulse);
+    gpio_set_dir(dir, GPIO_OUT);
+    gpio_set_dir(pulse, GPIO_OUT);
+    stepper s ={.dir = dir,.pulse = pulse}; 
+    return s;
+}
+
+void stepper_move_block(const stepper *stepper, int steps, uint32_t wait_ms) {
+    gpio_put(stepper->dir, steps > 0);
+    steps = (steps < 0)? -steps : steps;
+    for(int i = 0; i < steps; i++) {
+        gpio_put(stepper->pulse, 1);
+        sleep_us(wait_ms);
+        gpio_put(stepper->pulse, 0);
+        sleep_us(wait_ms);
+    }
+}

firmware/topfloor/src/stepper/stepper.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include "pico/stdlib.h"
+
+typedef struct servo {
+  uint8_t dir;
+  uint8_t pulse;
+} stepper;
+
+stepper setup_stepper(uint8_t dir, uint8_t pulse);
+
+void stepper_move_block(const stepper *stepper, int steps, uint32_t speed);

mg_bringup/launch/launch-external.py

@@ -0,0 +1,35 @@
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
+from launch.conditions import UnlessCondition 
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    is_local_test = DeclareLaunchArgument(
+        'local_test',
+        default_value="False",
+        description='Launch with simulated components'
+    )
+
+    return LaunchDescription([
+        is_local_test,
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare("mg_bt"),
+                'launch',
+                'launch.py'
+            ]),
+        ),
+        Node(
+            package="mg_planner",
+            executable="mg_planner",
+            name="mg_planner",
+            emulate_tty=True,
+            output='screen',
+        )
+    ])
+    

mg_bringup/launch/launch.py

@@ -1,6 +1,6 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
-from launch.conditions import UnlessCondition 
+from launch.conditions import UnlessCondition, IfCondition 
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -26,6 +26,14 @@ def generate_launch_description():
                 'local_test': LaunchConfiguration('local_test')
             }.items()
         ),
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare("mg_lidar"),
+                'launch',
+                'launch.py'
+            ]),
+            condition=UnlessCondition(LaunchConfiguration('local_test')),
+        ),
         Node(
             package="mg_odometry",
             executable="mg_odom_publisher",
@@ -33,12 +41,28 @@ def generate_launch_description():
             condition=UnlessCondition(LaunchConfiguration('local_test')),
             parameters=[{
                 'odom': "odom",
-                'serial_path': "/dev/ttyACM1",
+                'serial_path': "/dev/ttyACM0",
             }],
 
             emulate_tty=True,
             output='screen'
         ),
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare("mg_bt"),
+                'launch',
+                'launch.py'
+            ]),
+            condition=IfCondition(LaunchConfiguration('local_test')),
+        ),
+        Node(
+            package="mg_planner",
+            executable="mg_planner",
+            name="mg_planner",
+            emulate_tty=True,
+            condition=IfCondition(LaunchConfiguration('local_test')),
+            output='screen',
+        ),
         Node(
             package="mg_navigation",
             executable="mg_nav_server",

mg_bt/CMakeLists.txt

@@ -0,0 +1,72 @@
+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 rclcpp_action 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()

mg_bt/behavior_trees/calib_bt.xml

@@ -0,0 +1,170 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="calib2_bt">
+    <Sequence>
+      <ZeroNode service_name="/zero"/>
+      <MovePoint x_goal="0.7"
+                 y_goal="0.0"
+                 max_wheel_speed="3.000000"
+                 max_angular="0.1"
+                 max_vel="2.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.02"
+                 action_name="/MovePoint"/>
+      <RotateNode angle="-2"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
+                  action_name="/Rotate"/>
+      <RotateNode angle="-3.14"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
+                  action_name="/Rotate"/>
+      <MovePoint x_goal="0.3"
+                 y_goal="0.0"
+                 max_wheel_speed="3.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.02"
+                 action_name="/MovePoint"/>
+      <RotateNode angle="-2"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
+                  action_name="/Rotate"/>
+      <RotateNode angle="0"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
+                  action_name="/Rotate"/>
+      <Fallback>
+        <Timeout msec="5000">
+          <MovePoint x_goal="-0.1"
+                     y_goal="0.0"
+                     max_wheel_speed="3.000000"
+                     max_angular="0.1"
+                     max_vel="2.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </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 x_goal="0.7"
+                       y_goal="0.0"
+                       max_wheel_speed="3.000000"
+                       max_angular="0.1"
+                       max_vel="2.000000"
+                       ornt_mult="4.000000"
+                       tolerance="0.04"
+                       action_name="/MovePoint"/>
+            <RotateNode angle="2"
+                        max_wheel_speed="3.000000"
+                        max_angular="3.140000"
+                        tolerance="0.001000"
+                        action_name="/Rotate"/>
+            <RotateNode angle="-2"
+                        max_wheel_speed="3.000000"
+                        max_angular="3.140000"
+                        tolerance="0.001000"
+                        action_name="/Rotate"/>
+            <RotateNode angle="0"
+                        max_wheel_speed="3.000000"
+                        max_angular="3.140000"
+                        tolerance="0.001000"
+                        action_name="/Rotate"/>
+            <Fallback>
+              <Timeout msec="20000">
+                <MovePoint x_goal="-0.1"
+                           y_goal="0.0"
+                           max_wheel_speed="3.000000"
+                           max_angular="0.1"
+                           max_vel="2.000000"
+                           ornt_mult="4.000000"
+                           tolerance="0.001000"
+                           action_name="/MovePoint"/>
+              </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="x_goal"
+                  type="double"/>
+      <input_port name="y_goal"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="max_vel"
+                  default="2.000000"
+                  type="double"/>
+      <input_port name="ornt_mult"
+                  default="4.000000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="RotateNode">
+      <input_port name="angle"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  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>

mg_bt/behavior_trees/mg_bt.btproj

@@ -0,0 +1,91 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Project">
+    <include path="calib_bt.xml"/>
+    <include path="tactics.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="CanChooser">
+            <output_port name="orient" type="int"/>
+            <output_port name="y_loc" type="double"/>
+            <output_port name="x_loc" type="double"/>
+            <input_port name="canlist" type="std::string"/>
+        </Action>
+        <Action ID="I2CSignal">
+            <input_port name="Address" default="42" type="int"/>
+            <input_port name="Data" default="0" type="int"/>
+            <output_port name="Result" type="int"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="LookAtNode">
+            <input_port name="x" type="double"/>
+            <input_port name="y" type="double"/>
+            <input_port name="max_wheel_speed" default="3.000000" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
+            <input_port name="tolerance" default="0.001000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="MoveGlobal">
+            <input_port name="x_goal" type="double"/>
+            <input_port name="y_goal" type="double"/>
+            <input_port name="max_wheel_speed" default="6.000000" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
+            <input_port name="max_vel" default="4.000000" type="double"/>
+            <input_port name="ornt_mult" default="3.000000" type="double"/>
+            <input_port name="tolerance" default="0.050000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="MoveLocal">
+            <input_port name="x_goal" type="double"/>
+            <input_port name="y_goal" type="double"/>
+            <input_port name="max_wheel_speed" default="3.000000" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
+            <input_port name="max_vel" default="2.000000" type="double"/>
+            <input_port name="pos_mult" default="15.000000" type="double"/>
+            <input_port name="ornt_mult" default="4.000000" type="double"/>
+            <input_port name="tolerance" default="0.001000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="MovePoint">
+            <input_port name="x_goal" type="double"/>
+            <input_port name="y_goal" type="double"/>
+            <input_port name="max_wheel_speed" default="3.000000" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
+            <input_port name="max_vel" default="2.000000" type="double"/>
+            <input_port name="ornt_mult" default="4.000000" type="double"/>
+            <input_port name="tolerance" default="0.001000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="RotateNode">
+            <input_port name="angle" type="double"/>
+            <input_port name="max_wheel_speed" default="3.000000" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
+            <input_port name="tolerance" default="0.001000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="SendPose">
+            <input_port name="x" type="double"/>
+            <input_port name="y" type="double"/>
+            <input_port name="angle" type="double"/>
+            <input_port name="isDegree" default="true" type="bool"/>
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Condition ID="SideObstaclePub">
+            <input_port name="tactic" type="int"/>
+            <input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
+        </Condition>
+        <Action ID="TacticChooser">
+            <output_port name="IsBlue" type="bool"/>
+            <output_port name="tactic" type="int"/>
+            <input_port name="number" type="int"/>
+        </Action>
+        <Action ID="ZeroNode">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

mg_bt/behavior_trees/tactics.xml

@@ -0,0 +1,540 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="pickup_can">
+    <Sequence>
+      <CanChooser orient="{can_orient}"
+                  y_loc="{y_can}"
+                  x_loc="{x_can}"
+                  canlist="{target_can}"/>
+      <MoveGlobal x_goal="{x_can}"
+                  y_goal="{y_can}"
+                  max_wheel_speed="6.000000"
+                  max_angular="3.140000"
+                  max_vel="4.000000"
+                  ornt_mult="3.000000"
+                  tolerance="0.050000"
+                  action_name="/MoveGlobal"/>
+      <LookAtNode x="{x_can}"
+                  y="{y_can}"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/LookAt"/>
+      <MovePoint x_goal="{x_can}"
+                 y_goal="{y_can}"
+                 max_wheel_speed="5.000000"
+                 max_angular="0.1"
+                 max_vel="2.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <Sequence>
+        <SubTree ID="pickup_right"
+                 _skipIf="can_orient!=0"/>
+        <SubTree ID="pickup_down"
+                 _skipIf="can_orient!=1"/>
+        <SubTree ID="pickup_left"
+                 _skipIf="can_orient!=2"/>
+        <SubTree ID="pickup_up"
+                 _skipIf="can_orient!=3"
+                 _autoremap="false"/>
+        <MovePoint x_goal="0.8"
+                   y_goal="0.6"
+                   max_wheel_speed="3.000000"
+                   max_angular="3.140000"
+                   max_vel="2.000000"
+                   ornt_mult="4.000000"
+                   tolerance="0.001000"
+                   action_name="/MovePoint"
+                   _skipIf="target_can!=1"/>
+      </Sequence>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="pickup_down">
+    <Sequence>
+      <RotateNode angle="-1.57079632679"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="0"
+                 y_goal="-0.11"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="0"
+                 y_goal="0.11"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <RotateNode angle="1.57079632679"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="0"
+                 y_goal="-0.12"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="0"
+                 y_goal="0.12"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="pickup_left">
+    <Sequence>
+      <RotateNode angle="0"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="-0.11"
+                 y_goal="0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="0.11"
+                 y_goal="0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <RotateNode angle="-3.14159265359"
+                  max_wheel_speed="5.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="-0.12"
+                 y_goal="0.0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="0.12"
+                 y_goal="0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="pickup_right">
+    <Sequence>
+      <RotateNode angle="-3.14159265359"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="0.11"
+                 y_goal="0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="-0.11"
+                 y_goal="0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <RotateNode angle="0"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="0.12"
+                 y_goal="0.0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="-0.12"
+                 y_goal="0"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="pickup_up">
+    <Sequence>
+      <RotateNode angle="1.57079632679"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="0"
+                 y_goal="0.11"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="0"
+                 y_goal="-0.11"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <RotateNode angle="-1.57079632679"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.01000"
+                  action_name="/Rotate"/>
+      <MoveLocal x_goal="0"
+                 y_goal="0.12"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+      <MoveLocal x_goal="0.0"
+                 y_goal="-0.12"
+                 max_wheel_speed="5.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 pos_mult="15.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.001000"
+                 action_name="/MovePoint"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="tactic_base">
+    <Sequence>
+      <I2CSignal Address="65"
+                 Data="1"
+                 Result="{tactic_id}"
+                 action_name="/i2c_action"/>
+      <SideObstaclePub tactic="{tactic_id}"
+                       topic_name="/side"/>
+      <TacticChooser IsBlue="{isBlue}"
+                     tactic="{tactic}"
+                     number="{tactic_id}"/>
+      <SubTree ID="tactic_default_yellow"
+               _skipIf="isBlue"
+               _autoremap="false"/>
+      <SubTree ID="tatic_default_blue"
+               _skipIf="!isBlue"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="tactic_default_yellow">
+    <Sequence>
+      <I2CSignal Address="65"
+                 Data="1"
+                 Result="{i2c_res}"
+                 action_name="/i2c_action"/>
+      <SendPose x="1.0 + 16.5"
+                y="0.45 - 14"
+                angle="90"
+                isDegree="true"
+                service_name="/set_pose"/>
+      <SideObstaclePub tactic="1"
+                       topic_name="/side"/>
+      <Timeout msec="100000">
+        <Sequence>
+          <MoveLocal x_goal="0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+          <MoveLocal x_goal="-0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="tatic_default_blue">
+    <Sequence>
+      <I2CSignal Address="65"
+                 Data="1"
+                 Result="{i2c_res}"
+                 action_name="/i2c_action"/>
+      <SendPose x="0.31"
+                y="1.71"
+                angle="90"
+                isDegree="true"
+                service_name="/set_pose"/>
+      <SideObstaclePub tactic="2"
+                       topic_name="/side"/>
+      <Timeout msec="100000">
+        <Sequence>
+          <MoveLocal x_goal="0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+          <MoveLocal x_goal="-0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="testbed">
+    <Sequence>
+      <SubTree ID="pickup_can"
+               target_can="0"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="1"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="2"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="3"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="6"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="8"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="5"
+               _autoremap="false"/>
+      <SubTree ID="pickup_can"
+               target_can="4"
+               _autoremap="false"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="CanChooser">
+      <output_port name="orient"
+                   type="int"/>
+      <output_port name="y_loc"
+                   type="double"/>
+      <output_port name="x_loc"
+                   type="double"/>
+      <input_port name="canlist"
+                  type="std::string"/>
+    </Action>
+    <Action ID="I2CSignal">
+      <input_port name="Address"
+                  default="42"
+                  type="int"/>
+      <input_port name="Data"
+                  default="0"
+                  type="int"/>
+      <output_port name="Result"
+                   type="int"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="LookAtNode">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="MoveGlobal">
+      <input_port name="x_goal"
+                  type="double"/>
+      <input_port name="y_goal"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="6.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="max_vel"
+                  default="4.000000"
+                  type="double"/>
+      <input_port name="ornt_mult"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.050000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="MoveLocal">
+      <input_port name="x_goal"
+                  type="double"/>
+      <input_port name="y_goal"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="max_vel"
+                  default="2.000000"
+                  type="double"/>
+      <input_port name="pos_mult"
+                  default="15.000000"
+                  type="double"/>
+      <input_port name="ornt_mult"
+                  default="4.000000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="MovePoint">
+      <input_port name="x_goal"
+                  type="double"/>
+      <input_port name="y_goal"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="max_vel"
+                  default="2.000000"
+                  type="double"/>
+      <input_port name="ornt_mult"
+                  default="4.000000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="RotateNode">
+      <input_port name="angle"
+                  type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="SendPose">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="angle"
+                  type="double"/>
+      <input_port name="isDegree"
+                  default="true"
+                  type="bool"/>
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Condition ID="SideObstaclePub">
+      <input_port name="tactic"
+                  type="int"/>
+      <input_port name="topic_name"
+                  default="__default__placeholder__"
+                  type="std::string">Topic name</input_port>
+    </Condition>
+    <Action ID="TacticChooser">
+      <output_port name="IsBlue"
+                   type="bool"/>
+      <output_port name="tactic"
+                   type="int"/>
+      <input_port name="number"
+                  type="int"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

mg_bt/behavior_trees/tactics_blue.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="tactic_base">
+    <SideObstaclePub tactic="1"
+                     topic_name="/side"/>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Condition ID="SideObstaclePub">
+      <input_port name="tactic"
+                  type="int"/>
+      <input_port name="topic_name"
+                  default="__default__placeholder__"
+                  type="std::string">Topic name</input_port>
+    </Condition>
+  </TreeNodesModel>
+
+</root>

mg_bt/config/mg_bt_executor.yaml

@@ -0,0 +1,14 @@
+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
+
+

mg_bt/i2cmodule/i2cnode.cpp

@@ -0,0 +1,88 @@
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "mg_msgs/srv/i2c.hpp"
+#include "mg_msgs/action/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;
+    using I2cAction     = mg_msgs::action::I2c;
+    using GoalHandleI2c = rclcpp_action::ServerGoalHandle<I2cAction>;
+
+  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); };
+        using namespace std::placeholders;
+        i2c_action_ = rclcpp_action::create_server<I2cAction>(this,
+                                                              "/i2c_action",
+                                                              bind(&MgI2c::handle_goal, this, _1, _2),
+                                                              bind(&MgI2c::handle_cancel, this, _1),
+                                                              bind(&MgI2c::handle_accepted, this, _1));
+    }
+
+    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());
+    }
+
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
+                                            std::shared_ptr<const I2cAction::Goal> goal) {
+        RCLCPP_INFO(this->get_logger(), "Received goal request with addr %d and data %d", goal->addr, goal->data);
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleI2c>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void handle_accepted(const std::shared_ptr<GoalHandleI2c> goal_handle) {
+        using namespace std::placeholders;
+        ioctl(i2c_fd_, I2C_SLAVE, goal_handle->get_goal()->addr); // NOLINT
+        i2c_smbus_write_byte(i2c_fd_, goal_handle->get_goal()->data);
+        int ch = 0;
+
+        rclcpp::Rate rate(100);
+
+        while (ch == 0 || (ch > 255 || ch < 0)) {
+            ch = i2c_smbus_read_byte(i2c_fd_);
+            rate.sleep();
+        }
+        auto res = std::make_shared<I2cAction::Result>();
+        RCLCPP_INFO(get_logger(), "Recieved %d", ch);
+        res->resp.push_back(ch);
+        goal_handle->succeed(res);
+        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    }
+
+  private:
+    rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
+
+    rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
+
+    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;
+}

mg_bt/launch/launch.py

@@ -0,0 +1,27 @@
+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]
+        ),
+        Node(
+            package='mg_planner',
+            executable='mg_planner',
+            output="screen"
+        ),
+    ])
+    

mg_bt/package.xml

@@ -0,0 +1,25 @@
+<?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>

mg_bt/src/mg_bt.cpp

@@ -0,0 +1,23 @@
+#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;
+}

mg_bt/src/mg_tree_executor.cpp

@@ -0,0 +1,75 @@
+#include "mg_tree_executor.hpp"
+
+#include "behaviortree_cpp/xml_parsing.h"
+#include "tree_nodes/calib.hpp"
+#include "tree_nodes/choose_can.hpp"
+#include "tree_nodes/choose_tactic.hpp"
+#include "tree_nodes/i2c.hpp"
+#include "tree_nodes/move_point.hpp"
+#include "tree_nodes/move_local.hpp"
+#include "tree_nodes/move_global.hpp"
+#include "tree_nodes/look_at.hpp"
+#include "tree_nodes/rotate.hpp"
+#include "tree_nodes/side_pub.hpp"
+#include "tree_nodes/set_pos.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::MoveGlobalNode>("MoveGlobal", node());
+        factory.registerNodeType<mg::RotateNode>("RotateNode", node());
+        factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
+        factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
+        factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
+        factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
+        factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
+        factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
+        factory.registerNodeType<mg::CanChooser>("CanChooser");
+        factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
+        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;
+    }
+}

mg_bt/src/mg_tree_executor.hpp

@@ -0,0 +1,25 @@
+#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_;
+    };
+}

mg_bt/src/tree_nodes/calib.hpp

@@ -0,0 +1,44 @@
+#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_;
+    };
+}

mg_bt/src/tree_nodes/choose_can.hpp

@@ -0,0 +1,49 @@
+
+#include "behaviortree_cpp/action_node.h"
+
+namespace mg {
+
+    class CanChooser : public BT::SyncActionNode {
+        struct cans {
+            double x;
+            double y;
+            int    orientation;
+        };
+
+      public:
+        CanChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }
+
+        static BT::PortsList providedPorts() {
+            // This action has a single input port called "message"
+            return { BT::InputPort<std::string>("canlist"),
+                     BT::OutputPort<double>("x_loc"),
+                     BT::OutputPort<double>("y_loc"),
+                     BT::OutputPort<int>("orient") };
+        }
+
+        // You must override the virtual function tick()
+        BT::NodeStatus tick() override {
+            constexpr std::array<cans, 10> c{ { { 0.825, 1.425, 3 },
+                                                { 0.35, 0.4, 2 },
+                                                { 0.35, 1.325, 2 },
+                                                { 0.775, 0.55, 1 },
+                                                { 2.225, 0.55, 1 },
+                                                { 2.65, 0.4, 0 },
+                                                { 2.65, 0.4, 0 },
+                                                { 2.175, 1.425, 3 },
+                                                { 1.1, 0.65, 3 },
+                                                { 1.9, 0.65, 3 } } };
+
+            int idx = 0;
+            // Todo iterate through all cans and use a passthrough function that returns current can state
+            std::istringstream ss(getInput<std::string>("canlist").value());
+            ss >> idx;
+
+            setOutput<double>("x_loc", c[idx].x);
+            setOutput<double>("y_loc", c[idx].y);
+            setOutput<int>("orient", c[idx].orientation);
+
+            return BT::NodeStatus::SUCCESS;
+        }
+    };
+}

mg_bt/src/tree_nodes/choose_tactic.hpp

@@ -0,0 +1,22 @@
+#include "behaviortree_cpp/action_node.h"
+
+namespace mg {
+
+    class TacticsChooser : public BT::SyncActionNode {
+      public:
+        TacticsChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }
+
+        static BT::PortsList providedPorts() {
+            // This action has a single input port called "message"
+            return { BT::InputPort<int>("number"), BT::OutputPort<int>("tactic"), BT::OutputPort<bool>("IsBlue") };
+        }
+
+        // You must override the virtual function tick()
+        BT::NodeStatus tick() override {
+            const int num = getInput<int>("number").value();
+            setOutput<int>("tactic", (num + 1) / 2);
+            setOutput<bool>("IsBlue", num % 2);
+            return BT::NodeStatus::SUCCESS;
+        }
+    };
+}

mg_bt/src/tree_nodes/i2c.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/i2c.hpp"
+
+namespace mg {
+    class I2cNode : public BT::RosActionNode<mg_msgs::action::I2c> {
+      public:
+        I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosActionNode<mg_msgs::action::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 setGoal(Goal& req) override {
+            req.addr = getInput<int>("Address").value_or(42);
+            req.data = getInput<int>("Data").value_or(0);
+            return true;
+        }
+
+        BT::NodeStatus onResultReceived(const WrappedResult& resp) override {
+            setOutput<int>("Result", resp.result->resp.front());
+            return BT::NodeStatus::SUCCESS;
+        }
+    };
+}

mg_bt/src/tree_nodes/look_at.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/look_at.hpp"
+
+namespace mg {
+
+    class LookAtNode : public BT::RosActionNode<mg_msgs::action::LookAt> {
+      public:
+        LookAtNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosActionNode<mg_msgs::action::LookAt>(name, conf, params) { }
+
+        static BT::PortsList providedPorts() {
+            return providedBasicPorts({ BT::InputPort<double>("x"),
+                                        BT::InputPort<double>("y"),
+                                        BT::InputPort<double>("tolerance", 0.001, {}),
+                                        BT::InputPort<double>("max_wheel_speed", 3, {}),
+                                        BT::InputPort<double>("max_angular", 3.14, {})
+
+            });
+        }
+
+        bool setGoal(Goal& goal) override {
+            auto x               = getInput<double>("x");
+            auto y               = getInput<double>("y");
+            auto tolerance       = getInput<double>("tolerance");
+            auto max_wheel_speed = getInput<double>("max_wheel_speed");
+            auto max_angular     = getInput<double>("max_angular");
+            goal.x               = x.value();
+            goal.y               = y.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);
+            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;
+        }
+    };
+
+}

mg_bt/src/tree_nodes/move_global.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/move_global.hpp"
+
+namespace mg {
+
+    class MoveGlobalNode : public BT::RosActionNode<mg_msgs::action::MoveGlobal> {
+      public:
+        MoveGlobalNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosActionNode<mg_msgs::action::MoveGlobal>(name, conf, params) { }
+
+        static BT::PortsList providedPorts() {
+            return providedBasicPorts({ BT::InputPort<double>("x_goal"),
+                                        BT::InputPort<double>("y_goal"),
+                                        BT::InputPort<double>("tolerance", 0.05, {}),
+                                        BT::InputPort<double>("max_wheel_speed", 6.0, {}),
+                                        BT::InputPort<double>("max_angular", 3.14, {}),
+                                        BT::InputPort<double>("max_vel", 4.0, {}),
+                                        BT::InputPort<double>("ornt_mult", 3.0, {}) });
+        }
+
+        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");
+
+            goal.x.push_back(x_goal.value());
+            goal.y.push_back(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);
+            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;
+        }
+    };
+
+}

mg_bt/src/tree_nodes/move_local.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/move_point.hpp"
+
+#include "glm/glm.hpp"
+
+namespace mg {
+
+    class MoveLocalNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
+        using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
+
+        PosFuncSig pos_query_;
+
+      public:
+        MoveLocalNode(const std::string&       name,
+                      const BT::NodeConfig&    conf,
+                      const BT::RosNodeParams& params,
+                      const PosFuncSig         pos_query) :
+          BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params), pos_query_(pos_query) { }
+
+        static BT::PortsList providedPorts() {
+            return providedBasicPorts({ BT::InputPort<double>("x_goal"),
+                                        BT::InputPort<double>("y_goal"),
+                                        BT::InputPort<double>("tolerance", 0.001, {}),
+                                        BT::InputPort<double>("max_wheel_speed", 3, {}),
+                                        BT::InputPort<double>("max_angular", 3.14, {}),
+                                        BT::InputPort<double>("max_vel", 2, {}),
+                                        BT::InputPort<double>("pos_mult", 15, {}),
+                                        BT::InputPort<double>("ornt_mult", 4, {}) });
+        }
+
+        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 [pos, theta]    = pos_query_();
+            goal.x               = x_goal.value() + pos.x;
+            goal.y               = y_goal.value() + pos.y;
+            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);
+            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;
+        }
+    };
+
+}

mg_bt/src/tree_nodes/move_point.hpp

@@ -0,0 +1,56 @@
+#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", 0.001, {}),
+                                        BT::InputPort<double>("max_wheel_speed", 3, {}),
+                                        BT::InputPort<double>("max_angular", 3.14, {}),
+                                        BT::InputPort<double>("max_vel", 2, {}),
+                                        BT::InputPort<double>("ornt_mult", 4, {}) });
+        }
+
+        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");
+            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);
+            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;
+        }
+    };
+
+}

mg_bt/src/tree_nodes/rotate.hpp

@@ -0,0 +1,47 @@
+#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", 0.001, {}),
+              BT::InputPort<double>("max_wheel_speed", 3, {}),
+              BT::InputPort<double>("max_angular", 3.14, {}),
+            });
+        }
+
+        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");
+            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);
+            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;
+        }
+    };
+
+}

mg_bt/src/tree_nodes/set_pos.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "mg_msgs/srv/send_pose2d.hpp"
+
+namespace mg {
+
+    class SendPoseNode : public BT::RosServiceNode<mg_msgs::srv::SendPose2d> {
+      public:
+        SendPoseNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosServiceNode<mg_msgs::srv::SendPose2d>(name, conf, params) { }
+
+        static BT::PortsList providedPorts() {
+            return providedBasicPorts({ BT::InputPort<double>("x"),
+                                        BT::InputPort<double>("y"),
+                                        BT::InputPort<double>("angle"),
+                                        BT::InputPort<bool>("isDegree", "True", {}) });
+        }
+
+        bool setRequest(typename Request::SharedPtr& req) override {
+            req->x     = getInput<double>("x").value();
+            req->y     = getInput<double>("y").value();
+            req->theta = getInput<double>("angle").value();
+            if (getInput<bool>("isDegree").value()) {
+                req->theta *= M_PI / 180;
+            }
+            return true;
+        }
+
+        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
+            return BT::NodeStatus::SUCCESS;
+        }
+    };
+
+}

mg_bt/src/tree_nodes/side_pub.hpp

@@ -0,0 +1,22 @@
+#include "behaviortree_ros2/bt_topic_pub_node.hpp"
+#include "std_msgs/msg/string.hpp"
+
+namespace mg {
+
+    class SidePub : public BT::RosTopicPubNode<std_msgs::msg::String> {
+      public:
+        SidePub(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosTopicPubNode<std_msgs::msg::String>(name, conf, params) { }
+
+        static BT::PortsList providedPorts() { return providedBasicPorts({ BT::InputPort<int>("tactic") }); }
+
+        bool setMessage(std_msgs::msg::String& msg) override {
+            if (getInput<int>("tactic").has_value()) {
+                msg.data = (getInput<int>("tactic").value() % 2 == 0) ? "/blue-side.json" : "/yellow-side.json";
+                return true;
+            } else {
+                return false;
+            }
+        }
+    };
+}

mg_bt/src/tree_nodes/zero.hpp

@@ -0,0 +1,20 @@
+#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;
+        }
+    };
+
+}

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: true
+    enable_odom_tf: false 
     odom_frame_id: odom_excpected
     base_frame_id: base-link
 

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/ttyACM0";
+        serial_port_name = "/dev/ttyACM1";
     }
 
     left_wheel_pos_state  = 0;
@@ -73,9 +73,9 @@ hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time
 
     try {
         odrive_serial_interface.Write("s;");
-        value.data = left_wheel_vel_cmd / (2 * M_PI);
+        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);
+        value.data = right_wheel_vel_cmd / (2 * M_PI);
         for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
 
     } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }

mg_lidar/CMakeLists.txt

@@ -0,0 +1,60 @@
+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()

mg_lidar/config/lidar.yaml

@@ -0,0 +1,4 @@
+rplidar_node:
+  ros__parameters:
+    scan_mode: "ppbig"
+    topic_name: "base-link"

mg_lidar/launch/launch.py

@@ -0,0 +1,28 @@
+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]
+        ),
+    ])
+    

mg_lidar/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mg_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>

mg_lidar/src/scanner.cpp

@@ -0,0 +1,134 @@
+#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();
+}

mg_msgs/CMakeLists.txt

@@ -10,12 +10,18 @@ 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/SetWidth.srv"
+  "action/I2c.action"
+  "srv/CalcPath.srv"
+  "srv/SendDouble.srv"
+  "srv/SendPose2d.srv"
+  "srv/I2c.srv"
 )
 
 ament_package()

mg_msgs/action/I2c.action

@@ -0,0 +1,5 @@
+uint8 addr
+uint8 data
+---
+uint8[] resp
+---

mg_msgs/action/MoveGlobal.action

@@ -0,0 +1,25 @@
+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

mg_msgs/msg/Path.msg

@@ -0,0 +1 @@
+int32[] indicies

mg_msgs/srv/CalcPath.srv

@@ -0,0 +1,4 @@
+float64[] x
+float64[] y
+---
+int32[] indicies

mg_msgs/srv/I2c.srv

@@ -0,0 +1,4 @@
+uint8 addr
+uint8 data
+---
+uint8[] resp

mg_msgs/srv/SendDouble.srv

@@ -0,0 +1,2 @@
+float64 data 
+---

mg_msgs/srv/SendPose2d.srv

@@ -0,0 +1,4 @@
+float64 x
+float64 y
+float64 theta
+---

mg_msgs/srv/SetWidth.srv

@@ -1,2 +0,0 @@
-float64 width
----

mg_navigation/CMakeLists.txt

@@ -13,6 +13,7 @@ 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)
@@ -26,10 +27,12 @@ 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})
 

mg_navigation/include/handlers/dwa.hpp

@@ -0,0 +1,8 @@
+#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"

mg_navigation/include/handlers/dwa_core.hpp

@@ -30,7 +30,7 @@
 namespace mg {
 
     template <typename T>
-    class DWM {
+    class DWA {
       public:
         using Goal       = typename T::Goal;
         using Result     = typename T::Result;
@@ -62,7 +62,7 @@ namespace mg {
         glm::dvec2 pos;
         double     theta = 0;
 
-        DWM(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
+        DWA(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
 
         void execute();
 
@@ -88,7 +88,7 @@ namespace mg {
     };
 
     template <typename T>
-    DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
+    DWA<T>::DWA(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
       baseNode(mns),
       hgoal(g),
       pub_twist(mns.pub_twist),
@@ -99,16 +99,20 @@ namespace mg {
       pos(0, 0) { }
 
     template <typename T>
-    void DWM<T>::execute() {
+    void DWA<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;
@@ -121,11 +125,14 @@ namespace mg {
             pos_query();
             populate_candidates(spacevl, spacevr, dimx, dimy);
 
-            double b_score = calc_score(spacevl[0], spacevr[0]);
-            uint   b_ind   = 0;
+            double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
+            int    b_ind   = -1;
 
             for (uint i = 1; i < spacevl.size(); i++) {
                 const double score = calc_score(spacevl[i], spacevr[i]);
+                if (score == NAN) {
+                    continue;
+                }
                 if (score > b_score) {
                     b_score = score;
                     b_ind   = i;
@@ -134,17 +141,21 @@ namespace mg {
 
             calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
 
-            cvl = spacevl[b_ind];
-            cvr = spacevr[b_ind];
+            if (b_ind >= 0) {
+                cvl = spacevl[b_ind];
+                cvr = spacevr[b_ind];
+            } else {
+                cvl = 0;
+                cvr = 0;
+            }
             send_speed(step * cvl, step * cvr);
-
             rate.sleep();
         }
         succeed();
     }
 
     template <typename T>
-    void DWM<T>::pos_query() {
+    void DWA<T>::pos_query() {
         try {
             double x = NAN;
             double y = NAN;
@@ -164,7 +175,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::send_speed(double vl, double vr) {
+    void DWA<T>::send_speed(double vl, double vr) {
         auto [v, w] = to_vel(vl, vr);
         Geometry::TwistStamped twist;
         twist.twist.angular.z = w;
@@ -174,7 +185,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::abort(int error) {
+    void DWA<T>::abort(int error) {
         baseNode.mtx.lock();
         send_speed(0, 0);
         auto x   = std::make_shared<Result>();
@@ -183,7 +194,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::succeed() {
+    void DWA<T>::succeed() {
         baseNode.mtx.lock();
         send_speed(0, 0);
         auto x   = std::make_shared<Result>();
@@ -192,7 +203,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::cancel() {
+    void DWA<T>::cancel() {
         baseNode.mtx.lock();
         send_speed(0, 0);
         auto x   = std::make_shared<Result>();

mg_navigation/include/handlers/dwa_forward.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -8,22 +8,22 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
+    inline bool DWA<MgNavigationServer::MoveStraight>::target_check() {
         return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
+    inline void DWA<MgNavigationServer::MoveStraight>::target_init() {
         target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
+    inline bool DWA<MgNavigationServer::MoveStraight>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
+    inline double DWA<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 DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
                                                                            std::vector<int>& vr,
                                                                            int               dimx,
                                                                            int) {

mg_navigation/include/handlers/dwa_global.hpp

@@ -0,0 +1,106 @@
+#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);
+                }
+            }
+        }
+    }
+
+}

mg_navigation/include/handlers/dwa_lookat.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -9,16 +9,17 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::LookAt>::target_check() {
+    inline bool DWA<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;
+        const double c = (b > glm::pi<double>() / 2) ? glm::pi<double>() - b : b;
 
-        return b > goal->tolerance;
+        return c > goal->tolerance;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::LookAt>::target_init() {
+    inline void DWA<MgNavigationServer::LookAt>::target_init() {
 
         target_pos = glm::vec2(goal->x, goal->y);
 
@@ -30,7 +31,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::LookAt>::target_update() {
+    inline void DWA<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 +41,12 @@ namespace mg {
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
+    inline bool DWA<MgNavigationServer::LookAt>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
+    inline double DWA<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;
@@ -56,13 +57,16 @@ namespace mg {
         dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
         dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
 
+        dist_old = (dist_old > glm::pi<double>() / 2) ? glm::pi<double>() - dist_old : dist_old;
+        dist_new = (dist_new > glm::pi<double>() / 2) ? glm::pi<double>() - dist_new : dist_new;
+
         const double score = goal->ornt_mult * (dist_old - dist_new);
 
         return score;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
                                                                      std::vector<int>& vr,
                                                                      int               dimx,
                                                                      int) {

mg_navigation/include/handlers/dwa_point.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -8,28 +8,30 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::MovePoint>::target_check() {
+    inline bool DWA<MgNavigationServer::MovePoint>::target_check() {
         return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MovePoint>::target_update() {
+    inline void DWA<MgNavigationServer::MovePoint>::target_update() {
         target_pos = glm::dvec2(goal->x, goal->y);
+        dimy       = 8;
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
+    inline bool DWA<MgNavigationServer::MovePoint>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
+    inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
         auto [v, w]        = to_vel(step * vl, step * vr);
-        const double delta = 0.5;
+        const double delta = 0.3;
         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
@@ -47,17 +49,25 @@ 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);
 
-        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 dist = baseNode.obstacle_manager()->box_colide(
+          n_pos, { 0.29, 0.33 }, n_theta, ObstacleManager::ObstacleMask::DYNAMIC);
+
+        if (dist < 0.01) {
+            return NAN;
+        }
+
+        // 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) }));
 
         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));
 
         return score;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
                                                                         std::vector<int>& vr,
                                                                         int               dimx,
                                                                         int               dimy) {

mg_navigation/include/handlers/dwa_rotate.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -9,7 +9,7 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::Rotate>::target_check() {
+    inline bool DWA<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 DWM<MgNavigationServer::Rotate>::target_init() {
+    inline void DWA<MgNavigationServer::Rotate>::target_init() {
         target_ornt = goal->angle;
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::Rotate>::condition_check() {
+    inline bool DWA<MgNavigationServer::Rotate>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
+    inline double DWA<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 DWM<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
                                                                      std::vector<int>& vr,
                                                                      int               dimx,
                                                                      int) {

mg_navigation/include/handlers/dwm.hpp

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

mg_navigation/include/mg_navigation.hpp

@@ -17,6 +17,10 @@
 #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;
@@ -25,12 +29,14 @@ 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;
@@ -43,9 +49,15 @@ 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,

mg_navigation/include/path_buffer.hpp

@@ -0,0 +1,60 @@
+#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_;
+    };
+
+}

mg_navigation/package.xml

@@ -17,6 +17,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>mg_obstacles</depend>
   
 
   <build_depend>libglm-dev</build_depend>

mg_navigation/src/mg_navigation.cpp

@@ -4,8 +4,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "mg_navigation.hpp"
-#include "handlers/dwm.hpp"
-
+#include "handlers/dwa.hpp"
 namespace mg {
 
     template <typename T>
@@ -40,7 +39,7 @@ namespace mg {
 
     template <typename T>
     void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
-        DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
+        DWA<T> dwm = DWA<T>(gh, tf2_buffer, *this);
         dwm.execute();
         is_processing = false;
         mtx.unlock();
@@ -50,9 +49,21 @@ 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",

mg_navigation/src/path_buffer.cpp

@@ -0,0 +1,82 @@
+#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;
+    }
+}

mg_obstacles/CMakeLists.txt

@@ -0,0 +1,84 @@
+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()
+

mg_obstacles/include/mg_obstacles/mg_obstacles.hpp

@@ -0,0 +1,65 @@
+#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 "std_msgs/msg/string.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<std_msgs::msg::String>::SharedPtr side_sub_;
+
+        rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
+        rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
+
+        std::string base_path;
+
+        void load_permanent(Json::Value& json);
+        void load_static(Json::Value& json);
+    };
+}

mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp

@@ -0,0 +1,20 @@
+#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;
+    };
+
+}

mg_obstacles/include/mg_obstacles/sdf.hpp

@@ -0,0 +1,16 @@
+#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);
+}

mg_obstacles/include/mg_obstacles/static_obstacle.hpp

@@ -0,0 +1,26 @@
+#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;
+    };
+
+}

mg_obstacles/obstacles/blue-side.json

@@ -0,0 +1,94 @@
+{
+  "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": 0,
+            "y": 2
+        },
+        {
+            "height": 0.45,
+            "width": 0.45,
+            "x": 1,
+            "y": 0.45
+        },
+        {
+            "height": 0.2,
+            "width": 0.4,
+            "x": 1.95,
+            "y": 2
+        },
+        {
+            "height": 0.15,
+            "width": 0.45,
+            "x": 0.55,
+            "y": 0.15
+        },
+        {
+            "height": 0.15,
+            "width": 0.45,
+            "x": 2.55,
+            "y": 0.15
+        },
+        {
+            "height": 0.45,
+            "width": 0.45,
+            "x": 2.55,
+            "y": 1.1
+        }
+    ]
+}

mg_obstacles/obstacles/obstacles-static.json

@@ -0,0 +1,56 @@
+{
+    "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
+        }
+    ]
+}

mg_obstacles/obstacles/yellow-side.json

@@ -0,0 +1,94 @@
+{
+  "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
+    }
+  ]
+}

mg_obstacles/package.xml

@@ -0,0 +1,29 @@
+<?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>

mg_obstacles/src/mg_obstacles.cpp

@@ -0,0 +1,216 @@
+#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);
+
+        side_sub_ = node_->create_subscription<std_msgs::msg::String>(
+          "/side", rclcpp::QoS(1).keep_last(1), [this](std_msgs::msg::String::ConstSharedPtr msg) {
+              Json::Value json;
+              RCLCPP_INFO(node_->get_logger(), "Read file %s", (base_path + "/" + msg->data).c_str());
+              std::ifstream json_document(base_path + "/" + msg->data);
+              json_document >> json;
+              load_permanent(json);
+              load_static(json);
+          });
+
+        std::string obstacle_pkg;
+        std::string file_suffix = "/obstacles/yellow-side.json";
+        node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
+        node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
+        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);
+        }
+        base_path = ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix;
+        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(base_path + "/yellow-side.json");
+
+        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) {
+        permanent_obstacles_.clear();
+
+        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) {
+        static_obstacles_.clear();
+
+        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);
+        }
+    }
+}

mg_obstacles/src/permanent_obstacle.cpp

@@ -0,0 +1,23 @@
+#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);
+    }
+}

mg_obstacles/src/sdf.cpp

@@ -0,0 +1,53 @@
+#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;
+    }
+}

mg_obstacles/src/static_obstacle.cpp

@@ -0,0 +1,46 @@
+#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);
+        }
+    }
+}

mg_odometry/src/mg_odom_publisher.cpp

@@ -8,7 +8,8 @@
 #include <libserial/SerialStream.h>
 #include <sys/types.h>
 
-#include "mg_msgs/srv/set_width.hpp"
+#include "mg_msgs/srv/send_double.hpp"
+#include "mg_msgs/srv/send_pose2d.hpp"
 #include "std_srvs/srv/empty.hpp"
 
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -25,8 +26,9 @@
 
 constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
 
-using SetWidthSrv = mg_msgs::srv::SetWidth;
-using ZeroSrv     = std_srvs::srv::Empty;
+using SendDoubleSrv = mg_msgs::srv::SendDouble;
+using SendPoseSrv   = mg_msgs::srv::SendPose2d;
+using ZeroSrv       = std_srvs::srv::Empty;
 
 class MgOdomPublisher : public rclcpp::Node {
   public:
@@ -40,35 +42,97 @@ 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<SetWidthSrv>(
+        set_width_service_ = create_service<SendDoubleSrv>(
           "/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));
+        set_pose_service_ = create_service<SendPoseSrv>(
+          "/set_pose", std::bind(&MgOdomPublisher::set_pose, 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<SetWidthSrv>::SharedPtr        set_width_service_;
+    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_width_service_;
+    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_ratio_service_;
+    rclcpp::Service<SendPoseSrv>::SharedPtr        set_pose_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<SetWidthSrv::Request> request) {
-        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
+    void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
+        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
 
         if (enc_serial_port_.IsOpen()) {
             union {
                 std::array<u_char, sizeof(double)> bytes;
                 double                             data;
             } value{};
-            value.data = request->width;
+            value.data = request->data;
 
             enc_serial_port_ << "w;";
             for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
         }
     }
 
+    void set_pose(const std::shared_ptr<SendPoseSrv::Request> request) {
+        RCLCPP_INFO(get_logger(), "Setting pose to: %lf %lf %lf", request->x, request->y, request->theta);
+
+        if (enc_serial_port_.IsOpen()) {
+            union {
+                std::array<u_char, sizeof(double)> bytes;
+                double                             data;
+            } value{};
+            value.data = request->x;
+
+            enc_serial_port_ << "X;";
+            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+
+            value.data = request->y;
+
+            enc_serial_port_ << "Y;";
+            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+
+            value.data = request->theta;
+
+            enc_serial_port_ << "T;";
+            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");
 

mg_planner/CMakeLists.txt

@@ -0,0 +1,60 @@
+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()
+

mg_planner/include/mg_planner/a_star/a_star.hpp

@@ -0,0 +1,72 @@
+#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_;
+    };
+}

mg_planner/include/mg_planner/config.hpp

@@ -0,0 +1,24 @@
+#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