Added launch_files

*facepalm*

firmware/base/src/main.c

@@ -156,11 +156,20 @@ void core2_entry()
                 data = (data >> 8ll) | ((uint64_t)ch<<56ll);
                 readNum--;
                 if(!readNum) {
-                    if(type == 'w')
+                    if(type == 'w'){
                         wheel_separation = *((double*)&data);
-                    else
+                    } 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));
+                    }
                 }
             }
             
@@ -173,6 +182,15 @@ void core2_entry()
             } 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) | ';')) {

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",
@@ -39,6 +47,22 @@ def generate_launch_description():
             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

@@ -49,7 +49,7 @@ target_include_directories(
 target_link_libraries(mg_i2cnode i2c)
 
 ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
-ament_target_dependencies(mg_i2cnode rclcpp mg_msgs)
+ament_target_dependencies(mg_i2cnode rclcpp rclcpp_action mg_msgs)
 
 install( TARGETS
   mg_bt_executor

mg_bt/behavior_trees/calib_bt.xml

@@ -3,32 +3,55 @@
   <BehaviorTree ID="calib2_bt">
     <Sequence>
       <ZeroNode service_name="/zero"/>
-      <MovePoint action_name="/MovePoint"
+      <MovePoint x_goal="0.7"
-                 tolerance="0.02"
+                 y_goal="0.0"
+                 max_wheel_speed="3.000000"
                  max_angular="0.1"
-                 x_goal="0.7"
+                 max_vel="2.000000"
-                 y_goal="0.0"/>
+                 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 action_name="/MovePoint"
+      <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"
-                 x_goal="0.3"
+                 action_name="/MovePoint"/>
-                 y_goal="0.0"/>
       <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>
+      <Fallback>
-              <Timeout msec="5000">
+        <Timeout msec="5000">
-                <MovePoint action_name="/MovePoint"
+          <MovePoint x_goal="-0.1"
-                           max_angular="0.1"
+                     y_goal="0.0"
-                           x_goal="-0.1"
+                     max_wheel_speed="3.000000"
-                           y_goal="0.0"/>
+                     max_angular="0.1"
-              </Timeout>
+                     max_vel="2.000000"
-              <AlwaysSuccess/>
+                     ornt_mult="4.000000"
-            </Fallback>
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </Timeout>
+        <AlwaysSuccess/>
+      </Fallback>
       <ZeroNode service_name="/endCalib"/>
     </Sequence>
   </BehaviorTree>
@@ -41,23 +64,39 @@
         <Repeat num_cycles="5">
           <Sequence>
             <ZeroNode service_name="/zero"/>
-            <MovePoint action_name="/MovePoint"
+            <MovePoint x_goal="0.7"
-                       tolerance="0.04"
+                       y_goal="0.0"
+                       max_wheel_speed="3.000000"
                        max_angular="0.1"
-                       x_goal="0.7"
+                       max_vel="2.000000"
-                       y_goal="0.0"/>
+                       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 action_name="/MovePoint"
+                <MovePoint x_goal="-0.1"
+                           y_goal="0.0"
+                           max_wheel_speed="3.000000"
                            max_angular="0.1"
-                           x_goal="-0.1"
+                           max_vel="2.000000"
-                           y_goal="0.0"/>
+                           ornt_mult="4.000000"
+                           tolerance="0.001000"
+                           action_name="/MovePoint"/>
               </Timeout>
               <AlwaysSuccess/>
             </Fallback>
@@ -85,59 +124,39 @@
                   type="std::string">Service name</input_port>
     </Action>
     <Action ID="MovePoint">
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-      <input_port name="tolerance"
-                  type="double"/>
-      <input_port name="obst_mult_a"
-                  type="double"/>
-      <input_port name="ornt_mult"
-                  type="double"/>
-      <input_port name="pos_mult"
-                  type="double"/>
-      <input_port name="t_ornt_mult"
-                  type="double"/>
-      <input_port name="obst_mult_c"
-                  type="double"/>
-      <input_port name="obst_mult_b"
-                  type="double"/>
-      <input_port name="max_vel"
-                  type="double"/>
-      <input_port name="max_angular"
-                  type="double"/>
-      <input_port name="IgnoreList"
-                  type="std::string"/>
-      <input_port name="max_wheel_speed"
-                  type="double"/>
       <input_port name="x_goal"
                   type="double"/>
       <input_port name="y_goal"
                   type="double"/>
+      <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="pos_mult"
-                  type="double"/>
       <input_port name="max_wheel_speed"
+                  default="3.000000"
                   type="double"/>
-      <input_port name="IgnoreList"
-                  type="std::string"/>
       <input_port name="max_angular"
-                  type="double"/>
+                  default="3.140000"
-      <input_port name="max_vel"
-                  type="double"/>
-      <input_port name="obst_mult_b"
-                  type="double"/>
-      <input_port name="obst_mult_c"
-                  type="double"/>
-      <input_port name="ornt_mult"
-                  type="double"/>
-      <input_port name="t_ornt_mult"
-                  type="double"/>
-      <input_port name="obst_mult_a"
                   type="double"/>
       <input_port name="tolerance"
+                  default="0.001000"
                   type="double"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>

mg_bt/behavior_trees/mg_bt.btproj

@@ -1,6 +1,7 @@
 <?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">
@@ -9,37 +10,80 @@
             <output_port name="NewWidth" type="double"/>
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
-        <Action ID="MovePoint">
+        <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>
-            <input_port name="tolerance" type="double"/>
+        </Action>
-            <input_port name="obst_mult_a" type="double"/>
+        <Action ID="LookAtNode">
-            <input_port name="ornt_mult" type="double"/>
+            <input_port name="x" type="double"/>
-            <input_port name="pos_mult" type="double"/>
+            <input_port name="y" type="double"/>
-            <input_port name="t_ornt_mult" type="double"/>
+            <input_port name="max_wheel_speed" default="3.000000" type="double"/>
-            <input_port name="obst_mult_c" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
-            <input_port name="obst_mult_b" type="double"/>
+            <input_port name="tolerance" default="0.001000" type="double"/>
-            <input_port name="max_vel" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
-            <input_port name="max_angular" type="double"/>
+        </Action>
-            <input_port name="IgnoreList" type="std::string"/>
+        <Action ID="MoveGlobal">
-            <input_port name="max_wheel_speed" type="double"/>
             <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="pos_mult" type="double"/>
+            <input_port name="max_wheel_speed" default="3.000000" type="double"/>
-            <input_port name="max_wheel_speed" type="double"/>
+            <input_port name="max_angular" default="3.140000" type="double"/>
-            <input_port name="IgnoreList" type="std::string"/>
+            <input_port name="tolerance" default="0.001000" type="double"/>
-            <input_port name="max_angular" type="double"/>
-            <input_port name="max_vel" type="double"/>
-            <input_port name="obst_mult_b" type="double"/>
-            <input_port name="obst_mult_c" type="double"/>
-            <input_port name="ornt_mult" type="double"/>
-            <input_port name="t_ornt_mult" type="double"/>
-            <input_port name="obst_mult_a" type="double"/>
-            <input_port name="tolerance" type="double"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
+        <Action ID="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>

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/i2cmodule/i2cnode.cpp

@@ -1,5 +1,7 @@
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
 #include "mg_msgs/srv/i2c.hpp"
+#include "mg_msgs/action/i2c.hpp"
 
 extern "C" {
 
@@ -9,13 +11,20 @@ extern "C" {
 #include <i2c/smbus.h>
 }
 class MgI2c : public rclcpp::Node {
-    using I2cSrv = mg_msgs::srv::I2c;
+    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); };
-        i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
+        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 {
@@ -33,9 +42,41 @@ class MgI2c : public rclcpp::Node {
         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_;
-    int                                           i2c_fd_;
+
+    rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
+
+    int i2c_fd_;
 };
 
 int main(int argc, const char* const* argv) {

mg_bt/launch/launch.py

@@ -18,5 +18,10 @@ def generate_launch_description():
             output="screen",
             parameters=[bt_exec_config]
         ),
+        Node(
+            package='mg_planner',
+            executable='mg_planner',
+            output="screen"
+        ),
     ])
     

mg_bt/src/mg_tree_executor.cpp

@@ -2,9 +2,16 @@
 
 #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"
@@ -25,9 +32,16 @@ namespace mg {
 
     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(); });
     }
 

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

@@ -1,13 +1,13 @@
 #pragma once
 
-#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/bt_action_node.hpp"
-#include "mg_msgs/srv/i2c.hpp"
+#include "mg_msgs/action/i2c.hpp"
 
 namespace mg {
-    class I2cNode : public BT::RosServiceNode<mg_msgs::srv::I2c> {
+    class I2cNode : public BT::RosActionNode<mg_msgs::action::I2c> {
       public:
         I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
-          BT::RosServiceNode<mg_msgs::srv::I2c>(name, conf, params) { }
+          BT::RosActionNode<mg_msgs::action::I2c>(name, conf, params) { }
 
         static BT::PortsList providedPorts() {
             return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
@@ -15,14 +15,14 @@ namespace mg {
                                         BT::OutputPort<int>("Result") });
         }
 
-        bool setRequest(typename Request::SharedPtr& req) override {
+        bool setGoal(Goal& req) override {
-            req->addr = getInput<int>("Address").value_or(42);
+            req.addr = getInput<int>("Address").value_or(42);
-            req->data = getInput<int>("Result").value_or(0);
+            req.data = getInput<int>("Data").value_or(0);
             return true;
         }
 
-        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
+        BT::NodeStatus onResultReceived(const WrappedResult& resp) override {
-            setOutput<int>("Result", resp->resp.front());
+            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

@@ -13,17 +13,11 @@ namespace mg {
         static BT::PortsList providedPorts() {
             return providedBasicPorts({ BT::InputPort<double>("x_goal"),
                                         BT::InputPort<double>("y_goal"),
-                                        BT::InputPort<double>("tolerance"),
+                                        BT::InputPort<double>("tolerance", 0.001, {}),
-                                        BT::InputPort<double>("max_wheel_speed"),
+                                        BT::InputPort<double>("max_wheel_speed", 3, {}),
-                                        BT::InputPort<double>("max_angular"),
+                                        BT::InputPort<double>("max_angular", 3.14, {}),
-                                        BT::InputPort<double>("max_vel"),
+                                        BT::InputPort<double>("max_vel", 2, {}),
-                                        BT::InputPort<double>("pos_mult"),
+                                        BT::InputPort<double>("ornt_mult", 4, {}) });
-                                        BT::InputPort<double>("ornt_mult"),
-                                        BT::InputPort<double>("t_ornt_mult"),
-                                        BT::InputPort<double>("obst_mult_a"),
-                                        BT::InputPort<double>("obst_mult_b"),
-                                        BT::InputPort<double>("obst_mult_c"),
-                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
         }
 
         bool setGoal(Goal& goal) override {
@@ -35,10 +29,6 @@ namespace mg {
             auto max_vel         = getInput<double>("max_vel");
             auto pos_mult        = getInput<double>("pos_mult");
             auto ornt_mult       = getInput<double>("ornt_mult");
-            auto t_ornt_mult     = getInput<double>("t_ornt_mult");
-            auto obst_mult_a     = getInput<double>("obst_mult_a");
-            auto obst_mult_b     = getInput<double>("obst_mult_b");
-            auto obst_mult_c     = getInput<double>("obst_mult_c");
             goal.x               = x_goal.value();
             goal.y               = y_goal.value();
             goal.tolerance       = tolerance.value_or(goal.tolerance);
@@ -47,10 +37,6 @@ namespace mg {
             goal.max_vel         = max_vel.value_or(goal.max_vel);
             goal.pos_mult        = pos_mult.value_or(goal.pos_mult);
             goal.ornt_mult       = ornt_mult.value_or(goal.ornt_mult);
-            goal.t_ornt_mult     = t_ornt_mult.value_or(goal.t_ornt_mult);
-            goal.obst_mult_a     = obst_mult_a.value_or(goal.obst_mult_a);
-            goal.obst_mult_b     = obst_mult_b.value_or(goal.obst_mult_b);
-            goal.obst_mult_c     = obst_mult_c.value_or(goal.obst_mult_c);
             return true;
         }
 

mg_bt/src/tree_nodes/rotate.hpp

@@ -11,18 +11,12 @@ namespace mg {
           BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
 
         static BT::PortsList providedPorts() {
-            return providedBasicPorts({ BT::InputPort<double>("angle"),
+            return providedBasicPorts({
-                                        BT::InputPort<double>("tolerance"),
+              BT::InputPort<double>("angle"),
-                                        BT::InputPort<double>("max_wheel_speed"),
+              BT::InputPort<double>("tolerance", 0.001, {}),
-                                        BT::InputPort<double>("max_angular"),
+              BT::InputPort<double>("max_wheel_speed", 3, {}),
-                                        BT::InputPort<double>("max_vel"),
+              BT::InputPort<double>("max_angular", 3.14, {}),
-                                        BT::InputPort<double>("pos_mult"),
+            });
-                                        BT::InputPort<double>("ornt_mult"),
-                                        BT::InputPort<double>("t_ornt_mult"),
-                                        BT::InputPort<double>("obst_mult_a"),
-                                        BT::InputPort<double>("obst_mult_b"),
-                                        BT::InputPort<double>("obst_mult_c"),
-                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
         }
 
         bool setGoal(Goal& goal) override {
@@ -30,24 +24,10 @@ namespace mg {
             auto tolerance       = getInput<double>("tolerance");
             auto max_wheel_speed = getInput<double>("max_wheel_speed");
             auto max_angular     = getInput<double>("max_angular");
-            auto max_vel         = getInput<double>("max_vel");
-            auto pos_mult        = getInput<double>("pos_mult");
-            auto ornt_mult       = getInput<double>("ornt_mult");
-            auto t_ornt_mult     = getInput<double>("t_ornt_mult");
-            auto obst_mult_a     = getInput<double>("obst_mult_a");
-            auto obst_mult_b     = getInput<double>("obst_mult_b");
-            auto obst_mult_c     = getInput<double>("obst_mult_c");
             goal.angle           = angle.value();
             goal.tolerance       = tolerance.value_or(goal.tolerance);
             goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
             goal.max_angular     = max_angular.value_or(goal.max_angular);
-            goal.max_vel         = max_vel.value_or(goal.max_vel);
-            goal.pos_mult        = pos_mult.value_or(goal.pos_mult);
-            goal.ornt_mult       = ornt_mult.value_or(goal.ornt_mult);
-            goal.t_ornt_mult     = t_ornt_mult.value_or(goal.t_ornt_mult);
-            goal.obst_mult_a     = obst_mult_a.value_or(goal.obst_mult_a);
-            goal.obst_mult_b     = obst_mult_b.value_or(goal.obst_mult_b);
-            goal.obst_mult_c     = obst_mult_c.value_or(goal.obst_mult_c);
             return true;
         }
 

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_msgs/CMakeLists.txt

@@ -17,8 +17,10 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/MovePoint.action"
   "action/LookAt.action"
   "action/Rotate.action"
+  "action/I2c.action"
   "srv/CalcPath.srv"
   "srv/SendDouble.srv"
+  "srv/SendPose2d.srv"
   "srv/I2c.srv"
 )
 

mg_msgs/action/I2c.action

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

mg_msgs/srv/SendPose2d.srv

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

mg_navigation/include/handlers/dwa_core.hpp

@@ -125,11 +125,14 @@ namespace mg {
             pos_query();
             populate_candidates(spacevl, spacevr, dimx, dimy);
 
-            double b_score = calc_score(spacevl[0], spacevr[0]);
+            double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
-            uint   b_ind   = 0;
+            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;
@@ -138,8 +141,13 @@ namespace mg {
 
             calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
 
-            cvl = spacevl[b_ind];
+            if (b_ind >= 0) {
-            cvr = spacevr[b_ind];
+                cvl = spacevl[b_ind];
+                cvr = spacevr[b_ind];
+            } else {
+                cvl = 0;
+                cvr = 0;
+            }
             send_speed(step * cvl, step * cvr);
             rate.sleep();
         }

mg_navigation/include/handlers/dwa_global.hpp

@@ -70,8 +70,8 @@ namespace mg {
         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 Stuck %lf", dist);
-        RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
+        // 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);

mg_navigation/include/handlers/dwa_lookat.hpp

@@ -13,8 +13,9 @@ namespace mg {
 
         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 <>
@@ -56,6 +57,9 @@ 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;

mg_navigation/include/handlers/dwa_point.hpp

@@ -49,6 +49,13 @@ 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 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) }));

mg_obstacles/include/mg_obstacles/mg_obstacles.hpp

@@ -6,6 +6,7 @@
 #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"
@@ -51,9 +52,13 @@ namespace mg {
 
         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/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/src/mg_obstacles.cpp

@@ -27,23 +27,33 @@ namespace mg {
         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/obstacles.json";
+        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/obstacles.json");
+        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(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix);
+        std::ifstream json_document(base_path + "/yellow-side.json");
 
         json_document >> json;
-
         load_permanent(json);
         load_static(json);
     }
@@ -183,6 +193,7 @@ namespace mg {
     }
 
     void ObstacleManager::load_permanent(Json::Value& json) {
+        permanent_obstacles_.clear();
 
         Json::Value& obstacles = json["obstacles"];
 
@@ -193,6 +204,7 @@ namespace mg {
     }
 
     void ObstacleManager::load_static(Json::Value& json) {
+        static_obstacles_.clear();
 
         StaticObstacle::width  = json["staticObstacleWidth"].asDouble();
         StaticObstacle::height = json["staticObstacleHeight"].asDouble();

mg_odometry/src/mg_odom_publisher.cpp

@@ -9,6 +9,7 @@
 #include <sys/types.h>
 
 #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"
@@ -26,6 +27,7 @@
 constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
 
 using SendDoubleSrv = mg_msgs::srv::SendDouble;
+using SendPoseSrv   = mg_msgs::srv::SendPose2d;
 using ZeroSrv       = std_srvs::srv::Empty;
 
 class MgOdomPublisher : public rclcpp::Node {
@@ -44,6 +46,8 @@ class MgOdomPublisher : public rclcpp::Node {
           "/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));
@@ -56,6 +60,7 @@ class MgOdomPublisher : public rclcpp::Node {
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     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_;
@@ -76,6 +81,31 @@ class MgOdomPublisher : public rclcpp::Node {
         }
     }
 
+    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);
 

mg_planner/src/mg_planner_node.cpp

@@ -17,7 +17,6 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
       "GeneratePath",
       [this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req,
              mg_msgs::srv::CalcPath::Response::SharedPtr     resp) -> void {
-          std::cout << "Bro recieved request\n";
           auto                    elapsed = get_clock()->now();
           std::vector<glm::ivec2> goals;
           std::transform(req->x.begin(),
@@ -38,7 +37,6 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
           mg_msgs::msg::Path path;
           path.indicies = std::vector<int>(resp->indicies);
           path_pub_->publish(path);
-          RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
       });
 
     path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));