fixed docker image sripts and etc.

Dockerfile.base.env

@@ -1,4 +1,4 @@
-FROM arm64v8/ros:jazzy as magrob.env
+FROM arm64v8/ros:jazzy-ros-base AS magrob.base.env
 
 RUN mkdir -p /ros_ws/src
 WORKDIR /ros_ws/src
@@ -7,9 +7,7 @@ RUN --mount=type=bind,source=./,target=/ros_ws/src/ \
     --mount=type=cache,target=/var/cache/apt,sharing=locked \
     --mount=type=cache,target=/var/lib/apt,sharing=locked \
     . /opt/ros/jazzy/setup.sh && \
-    apt update && \
-    apt-get install -y ros-jazzy-hardware-interface && \
-    rosdep install -i --from-paths . -y
+    scripts/install_base_deps.sh
 
 RUN cat <<EOF >> /root/.bashrc
 source /opt/ros/jazzy/setup.bash

docker-compose.yaml

@@ -1,9 +1,9 @@
 services:
   magrob_base:
     network_mode: host
-    image: localhost/magrob.env
+    image: localhost/magrob.base.env
     build: 
-      dockerfile: Dockerfile.env
+      dockerfile: Dockerfile.base.env
 
     entrypoint: ["sleep","infinity"]
     volumes:

firmware/base/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "clangd.arguments": [
+        "--query-driver=/usr/bin/arm-none-eabi-g*"
+    ]
+}

firmware/base/src/config.h

@@ -10,6 +10,7 @@
 #define SM_B 1
 
 #define PULSE_PER_REV (1 / 3200.0)
+#define SPEED_SET_TIMEOUT 1000
 //###############################################
 
 //================ CONFIG ENCODERS =====================

firmware/base/src/main.c

@@ -32,6 +32,7 @@ static double wheel_ratio_r = 1;
 static uint prev_time;
 static int prev_position_l = 0;
 static int prev_position_r = 0;
+static uint last_speed_change = 100;
 
 
 static unsigned char stepper_instr[2+sizeof(double)*2] = {};
@@ -156,20 +157,11 @@ void core2_entry()
                 data = (data >> 8ll) | ((uint64_t)ch<<56ll);
                 readNum--;
                 if(!readNum) {
-                    if(type == 'w'){
+                    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{
+                    else
                         wheel_ratio_l = 1 + (*((double *)&data));
                         wheel_ratio_r = 1 - (*((double *)&data));
-                    }
                 }
             }
             
@@ -182,15 +174,6 @@ 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) | ';')) {
@@ -230,6 +213,7 @@ void stepper_fifo(char c, uint8_t itf) {
         double vr = btod(stepper_instr + 10);
 
         set_speeds(vl, vr);
+        last_speed_change = SPEED_SET_TIMEOUT;
 
         // tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
         // tud_cdc_n_write_flush(itf);
@@ -281,6 +265,10 @@ int main()
     
     
     while (true) {
+        if(last_speed_change--) {
+            set_speeds(0,  0);
+            last_speed_change = SPEED_SET_TIMEOUT;
+        }
         tud_task();
         sleep_ms(1);
     }

firmware/topfloor/CMakeLists.txt

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

@@ -1,121 +0,0 @@
-# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
-
-# This can be dropped into an external project to help locate this SDK
-# It should be include()ed prior to project()
-
-# Copyright 2020 (c) 2020 Raspberry Pi (Trading) Ltd.
-#
-# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
-# following conditions are met:
-#
-# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
-# disclaimer.
-#
-# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other materials provided with the distribution.
-#
-# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products
-# derived from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
-# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
-# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
-# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
-    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
-    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
-    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
-    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
-    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
-    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
-    set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
-    message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
-endif ()
-
-if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
-  set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
-  message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
-endif()
-
-set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
-set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
-set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
-set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
-
-if (NOT PICO_SDK_PATH)
-    if (PICO_SDK_FETCH_FROM_GIT)
-        include(FetchContent)
-        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
-        if (PICO_SDK_FETCH_FROM_GIT_PATH)
-            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
-        endif ()
-        FetchContent_Declare(
-                pico_sdk
-                GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
-        )
-
-        if (NOT pico_sdk)
-            message("Downloading Raspberry Pi Pico SDK")
-            # GIT_SUBMODULES_RECURSE was added in 3.17
-            if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
-                FetchContent_Populate(
-                        pico_sdk
-                        QUIET
-                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
-                        GIT_SUBMODULES_RECURSE FALSE
-
-                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
-                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
-                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
-                )
-            else ()
-                FetchContent_Populate(
-                        pico_sdk
-                        QUIET
-                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
-
-                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
-                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
-                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
-                )
-            endif ()
-
-            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
-        endif ()
-        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
-    else ()
-        message(FATAL_ERROR
-                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
-                )
-    endif ()
-endif ()
-
-get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
-if (NOT EXISTS ${PICO_SDK_PATH})
-    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
-endif ()
-
-set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
-if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
-    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
-endif ()
-
-set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
-
-include(${PICO_SDK_INIT_CMAKE_FILE})

firmware/topfloor/src/actions/user.c

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

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

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

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

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

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

firmware/topfloor/src/stepper/stepper.c

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

@@ -1,12 +0,0 @@
-#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/base.launch.py

@@ -0,0 +1,63 @@
+################################################################################
+#                                                                              #
+#  Launch file meant to be used on the robot itself                            #
+#      uses:                                                                   #
+#         * mg_control    - for sending commands to the stepper drivers        #
+#         * mg_odometry   - for reading and controlling the MCU encoders       #
+#         * mg_navigation - runs the local planner and handles move behaviors  #
+#         * mg_navigation - runs the local planner and handles move behaviors  #
+#         * mg_lidar      - optionally runs the lidar node to read opponents   #
+#                            positions                                         #
+#                                                                              #
+################################################################################
+
+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_control"),
+                'launch',
+                'toid.launch.py'
+            ]),
+            launch_arguments={
+                'use_mock': LaunchConfiguration('local_test')
+            }.items()
+        ),
+        Node(
+            package="mg_odometry",
+            executable="mg_odom_publisher",
+            name="mg_odom_publisher",
+            condition=UnlessCondition(LaunchConfiguration('local_test')),
+            parameters=[{
+                'odom': "odom",
+                'serial_path': "/dev/ttyACM0",
+            }],
+
+            emulate_tty=True,
+            output='screen'
+        ),
+        Node(
+            package="mg_navigation",
+            executable="mg_nav_server",
+            name="mg_nav_server",
+            emulate_tty=True,
+            output='screen',
+        )
+    ])
+    

mg_bringup/launch/bt.launch.py

@@ -0,0 +1,29 @@
+################################################################################
+#                                                                              #
+#  Launch file meant to be used on the robot itself                            #
+#      uses:                                                                   #
+#         * mg_bt         - runs the robot's behavior tree server              #
+#                                                                              #
+################################################################################
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+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]
+        ),
+    ])
+    

mg_bringup/launch/launch-external.py

@@ -1,35 +0,0 @@
-
-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, IfCondition 
+from launch.conditions import UnlessCondition 
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -26,14 +26,6 @@ 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",
@@ -47,22 +39,6 @@ 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_bringup/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>launch</depend>
   <depend>launch_ros</depend>
+  <depend>xacro</depend>
   <depend>mg_control</depend>
   <depend>mg_odometry</depend>
   <depend>mg_navigation</depend>

mg_bt/CMakeLists.txt

@@ -37,7 +37,7 @@ set(SOURCES
 
 
 add_executable(mg_bt_executor ${SOURCES})
-add_executable(mg_i2cnode i2cmodule/i2cnode.cpp)
+# add_executable(mg_i2cnode i2cmodule/i2cnode.cpp)
 
 target_include_directories(
   mg_bt_executor
@@ -46,14 +46,14 @@ target_include_directories(
   include
 )
 
-target_link_libraries(mg_i2cnode i2c)
+# target_link_libraries(mg_i2cnode i2c)
 
 ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
-ament_target_dependencies(mg_i2cnode rclcpp rclcpp_action mg_msgs)
+# ament_target_dependencies(mg_i2cnode rclcpp mg_msgs)
 
 install( TARGETS
   mg_bt_executor
-  mg_i2cnode
+#  mg_i2cnode
   DESTINATION lib/${PROJECT_NAME}
 )
 

mg_bt/behavior_trees/calib_bt.xml

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

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

@@ -1,19 +0,0 @@
-<?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,7 +1,5 @@
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
 #include "mg_msgs/srv/i2c.hpp"
-#include "mg_msgs/action/i2c.hpp"
 
 extern "C" {
 
@@ -11,20 +9,13 @@ extern "C" {
 #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>;
+    using I2cSrv = mg_msgs::srv::I2c;
 
   public:
     MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
         auto cb
           = [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
-        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));
+        i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
     }
 
     void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
@@ -42,41 +33,9 @@ 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_;
-
-    rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
-
-    int i2c_fd_;
+    int                                           i2c_fd_;
 };
 
 int main(int argc, const char* const* argv) {

mg_bt/launch/launch.py

@@ -18,10 +18,5 @@ 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,16 +2,9 @@
 
 #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"
@@ -32,16 +25,9 @@ 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(); });
     }
 
@@ -51,7 +37,7 @@ namespace mg {
         double    theta;
         glm::vec2 pos;
 
-        auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
+        auto tmsg = tf_buffer_->lookupTransform("odom", "base_footprint", tf2::TimePointZero);
 
         tf2::Transform t;
         tf2::convert(tmsg.transform, t);

mg_bt/src/tree_nodes/choose_can.hpp

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

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

mg_bt/src/tree_nodes/look_at.hpp

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

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

@@ -1,67 +0,0 @@
-#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,11 +13,17 @@ namespace mg {
         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, {}) });
+                                        BT::InputPort<double>("tolerance"),
+                                        BT::InputPort<double>("max_wheel_speed"),
+                                        BT::InputPort<double>("max_angular"),
+                                        BT::InputPort<double>("max_vel"),
+                                        BT::InputPort<double>("pos_mult"),
+                                        BT::InputPort<double>("ornt_mult"),
+                                        BT::InputPort<double>("t_ornt_mult"),
+                                        BT::InputPort<double>("obst_mult_a"),
+                                        BT::InputPort<double>("obst_mult_b"),
+                                        BT::InputPort<double>("obst_mult_c"),
+                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
         }
 
         bool setGoal(Goal& goal) override {
@@ -29,6 +35,10 @@ 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);
@@ -37,6 +47,10 @@ 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,12 +11,18 @@ namespace mg {
           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, {}),
-            });
+            return providedBasicPorts({ BT::InputPort<double>("angle"),
+                                        BT::InputPort<double>("tolerance"),
+                                        BT::InputPort<double>("max_wheel_speed"),
+                                        BT::InputPort<double>("max_angular"),
+                                        BT::InputPort<double>("max_vel"),
+                                        BT::InputPort<double>("pos_mult"),
+                                        BT::InputPort<double>("ornt_mult"),
+                                        BT::InputPort<double>("t_ornt_mult"),
+                                        BT::InputPort<double>("obst_mult_a"),
+                                        BT::InputPort<double>("obst_mult_b"),
+                                        BT::InputPort<double>("obst_mult_c"),
+                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
         }
 
         bool setGoal(Goal& goal) override {
@@ -24,10 +30,24 @@ 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

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

@@ -1,22 +0,0 @@
-#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_control/assets/mg_base.urdf

@@ -55,7 +55,7 @@
     </joint>
 
 
-    <ros2_control name="mg-base" type="system">
+    <ros2_control name="mg_base" type="system">
         <hardware>
             <plugin>mg_control/MgStepperInterface</plugin>
         </hardware>

mg_control/assets/mg_base_controllers.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 100
+    update_rate: 50
 
     diffdrive_controller:
       type: diff_drive_controller/DiffDriveController
@@ -14,7 +14,7 @@ diffdrive_controller:
     
     enable_odom_tf: false 
     odom_frame_id: odom_excpected
-    base_frame_id: base-link
+    base_frame_id: base_footprint
 
     open_loop: true 
 

mg_control/assets/mg_base_controllers_local.yaml

@@ -14,7 +14,7 @@ diffdrive_controller:
     
     enable_odom_tf: true
     odom_frame_id: odom
-    base_frame_id: base-link
+    base_frame_id: base_footprint
 
     open_loop: true 
 

mg_control/assets/mg_base_local.urdf

@@ -55,7 +55,7 @@
     </joint>
 
 
-    <ros2_control name="mg-base" type="system">
+    <ros2_control name="mg_base" type="system">
         <hardware>
             <plugin>mock_components/GenericSystem</plugin> 
             <param name="calculate_dynamics">true</param> 

mg_control/assets/toid_general_params.yaml

@@ -0,0 +1,22 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50
+
+    diffdrive_controller:
+      type: diff_drive_controller/DiffDriveController
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+diffdrive_controller:
+  ros__parameters:
+    left_wheel_names: ["drivewhl_l_joint"]
+    right_wheel_names: ["drivewhl_r_joint"]
+    
+    enable_odom_tf: true 
+    odom_frame_id: odom
+    base_frame_id: base_footprint
+
+    open_loop: true 
+
+    wheel_separation: 0.258
+    wheel_radius: 0.0375

mg_control/launch/toid.launch.py

@@ -0,0 +1,111 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition 
+from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
+from launch_ros.actions import Node, LifecycleNode
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    pkg_share = FindPackageShare("").find('mg_control')
+    params = os.path.join(pkg_share, 'assets', 'toid_general_params.yaml')
+    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz')
+    
+
+    description_pkg_share = FindPackageShare("").find('toid_bot_description')
+    default_model_path = os.path.join(
+        description_pkg_share, 
+        'src', 
+        'toid_bot_description.urdf'
+    )
+
+    visualize = LaunchConfiguration("visualize")
+
+    visualize_arg = DeclareLaunchArgument(
+        'visualize',
+        default_value='False',
+        description="Whether to launch rviz2" 
+    )
+
+    use_mock = LaunchConfiguration("use_mock")
+
+    use_mock_arg = DeclareLaunchArgument(
+        'use_mock',
+        default_value='True',
+        description="Whether to use mock controller" 
+    )
+
+    odom_broadcast = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='map_to_odom_broadcaster',
+        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
+        condition=IfCondition(LaunchConfiguration('visualize'))
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_sim:=', use_mock])}]
+    )
+
+    controller_manager = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        output='screen',
+        parameters=[params]
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=["joint_state_broadcaster"]
+    )
+
+    diffbot_base_controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='both',
+        arguments=[
+            "diffdrive_controller",
+            "-p",
+            params,
+            "--controller-ros-args",
+            "-r diffdrive_controller/cmd_vel:=/cmd_vel",
+            "--controller-ros-args",
+            "-r diffdrive_controller/odom:=/odom",
+            "--controller-ros-args",
+            IfElseSubstitution(use_mock, 
+                "--param odom_frame_id:=odom", 
+                "--param odom_frame_id:=odom_expected"
+            ),
+            "--controller-ros-args",
+            IfElseSubstitution(use_mock, 
+                "--param enable_odom_tf:=true", 
+                "--param enable_odom_tf:=false"
+            ),
+        ]
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', default_rviz_config_path],
+        condition=IfCondition(visualize)
+    )
+
+    return LaunchDescription([
+        visualize_arg,
+        use_mock_arg,
+        odom_broadcast,
+        robot_state_publisher,
+        controller_manager,
+        joint_state_broadcaster,
+        diffbot_base_controller,
+        rviz_node
+    ])

mg_lidar/config/lidar.yaml

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

mg_lidar/src/scanner.cpp

@@ -54,7 +54,7 @@ class MgScanner : public rclcpp::Node {
             double y   = NAN;
             double rot = NAN;
 
-            auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
+            auto tmsg = tf_buf_->lookupTransform("odom", "base_footprint", tf2::TimePointZero);
 
             tf2::Transform t;
             tf2::convert(tmsg.transform, t);

mg_msgs/CMakeLists.txt

@@ -17,10 +17,8 @@ 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

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

mg_msgs/srv/SendPose2d.srv

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

mg_navigation/include/handlers/dwa_core.hpp

@@ -125,14 +125,11 @@ namespace mg {
             pos_query();
             populate_candidates(spacevl, spacevr, dimx, dimy);
 
-            double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
-            int    b_ind   = -1;
+            double b_score = calc_score(spacevl[0], spacevr[0]);
+            uint   b_ind   = 0;
 
             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;
@@ -141,13 +138,8 @@ namespace mg {
 
             calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
 
-            if (b_ind >= 0) {
-                cvl = spacevl[b_ind];
-                cvr = spacevr[b_ind];
-            } else {
-                cvl = 0;
-                cvr = 0;
-            }
+            cvl = spacevl[b_ind];
+            cvr = spacevr[b_ind];
             send_speed(step * cvl, step * cvr);
             rate.sleep();
         }
@@ -160,7 +152,7 @@ namespace mg {
             double x = NAN;
             double y = NAN;
 
-            auto tmsg = tfbuf->lookupTransform("odom", "base-link", tf2::TimePointZero);
+            auto tmsg = tfbuf->lookupTransform("odom", "base_footprint", tf2::TimePointZero);
 
             tf2::Transform t;
             tf2::convert(tmsg.transform, t);

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 Stuck2 %lf", 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);

mg_navigation/include/handlers/dwa_lookat.hpp

@@ -13,9 +13,8 @@ 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 c > goal->tolerance;
+        return b > goal->tolerance;
     }
 
     template <>
@@ -57,9 +56,6 @@ 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,13 +49,6 @@ 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_navigation/src/mg_navigation.cpp

@@ -51,7 +51,7 @@ namespace mg {
 
         path_buffer_ = std::make_shared<PathBuffer>(this);
 
-        pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
+        pub_twist = create_publisher<Geometry::TwistStamped>("/cmd_vel", 2);
 
         obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf2_buffer);
 

mg_obstacles/include/mg_obstacles/mg_obstacles.hpp

@@ -6,7 +6,6 @@
 #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"
@@ -52,13 +51,9 @@ 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

@@ -1,94 +0,0 @@
-{
-  "staticObstacleHeight": 0.4,
-  "staticObstacleWidth": 0.1,
-  "staticObstacles": [
-    {
-      "horizontal": true,
-      "x": 0.625,
-      "y": 1.775
-    },
-    {
-      "horizontal": false,
-      "x": 0,
-      "y": 0.6
-    },
-    {
-      "horizontal": false,
-      "x": 0,
-      "y": 1.525
-    },
-    {
-      "horizontal": true,
-      "x": 0.575,
-      "y": 0.3
-    },
-    {
-      "horizontal": true,
-      "x": 2.025,
-      "y": 0.3
-    },
-    {
-      "horizontal": false,
-      "x": 2.9,
-      "y": 0.6
-    },
-    {
-      "horizontal": false,
-      "x": 2.9,
-      "y": 1.525
-    },
-    {
-      "horizontal": true,
-      "x": 1.975,
-      "y": 1.775
-    },
-    {
-      "horizontal": true,
-      "x": 0.9,
-      "y": 1
-    },
-    {
-      "horizontal": true,
-      "x": 1.7,
-      "y": 1
-    }
-  ],
-    "obstacles": [
-        {
-            "height": 0.45,
-            "width": 1.95,
-            "x": 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,33 +27,23 @@ 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/yellow-side.json";
+        std::string file_suffix = "/obstacles/obstacles.json";
         node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
-        node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
+        node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json");
         ulong n = obstacle_pkg.find_first_of('/');
         if (n < obstacle_pkg.size()) {
             file_suffix  = obstacle_pkg.substr(n);
             obstacle_pkg = obstacle_pkg.substr(0, n);
         }
-        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");
+        std::ifstream json_document(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix);
 
         json_document >> json;
+
         load_permanent(json);
         load_static(json);
     }
@@ -193,7 +183,6 @@ namespace mg {
     }
 
     void ObstacleManager::load_permanent(Json::Value& json) {
-        permanent_obstacles_.clear();
 
         Json::Value& obstacles = json["obstacles"];
 
@@ -204,7 +193,6 @@ 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,7 +9,6 @@
 #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"
@@ -24,17 +23,16 @@
 
 #define TIMEOUT 10u
 
-constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
+constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM0";
 
 using SendDoubleSrv = mg_msgs::srv::SendDouble;
-using SendPoseSrv   = mg_msgs::srv::SendPose2d;
 using ZeroSrv       = std_srvs::srv::Empty;
 
 class MgOdomPublisher : public rclcpp::Node {
   public:
     MgOdomPublisher() : Node("mg_odom_publisher") {
         this->declare_parameter("odom", "odom");
-        this->declare_parameter("target", "base-link");
+        this->declare_parameter("target", "base_footprint");
         this->declare_parameter("serial_path", ENCODER_SERIAL_PATH_DEFAULT);
 
         tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
@@ -46,8 +44,6 @@ 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));
@@ -60,7 +56,6 @@ 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_;
@@ -81,31 +76,6 @@ 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,6 +17,7 @@ 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(),
@@ -37,6 +38,7 @@ 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));
@@ -49,7 +51,7 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
 
 glm::ivec2 mg::PlannerNode::get_pos() {
     try {
-        auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
+        auto tmsg = tf_buf_->lookupTransform("odom", "base_footprint", tf2::TimePointZero);
 
         tf2::Transform t;
         tf2::convert(tmsg.transform, t);

scripts/docker_build.sh

@@ -0,0 +1,55 @@
+usage() {
+	cat <<EOF
+Usage:
+	docker_build.sh <target> <registry> [<args>]
+	docker_build.sh -h
+
+	Build and push a docker image to a arm64v8 registry
+
+	Targets: 
+		base     The pacakges aimed at running on the raspberry pi
+EOF
+}
+
+
+TARGET="$1"
+REGISTRY="$2"
+ARGS="$2"
+
+if [[ $TARGET == "-h" || $TARGET == "--help" ]]; then
+	usage
+	exit 0
+fi
+
+if [[ $# -lt 2 ]]; then
+	echo "Error: Expected at least 2 arguments."
+	usage
+	exit 1
+fi
+
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
+case $TARGET in
+	"base_env") 
+		ssh -N \
+			-o StrictHostKeyChecking=no \
+			-o UserKnownHostsFile=/dev/null \
+			-L 0.0.0.0:5000:localhost:5000 \
+			$REGISTRY &
+
+		echo "Building target: base"
+
+		sudo docker buildx build \
+			--platform linux/arm64 \
+			-t localhost:5000/magrob.base.env:latest \
+			-f Dockerfile.base.env \
+			--output type=registry \
+			.
+		;;
+	*)
+		echo "Target not defined"
+		exit 1
+		;;
+esac
+
+

scripts/install_base_deps.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+
+ros_distro="jazzy"
+script_dir=$(dirname "$(readlink -f "${bash_source[0]}")")
+
+source "/opt/ros/${ROS_DISTRO}/setup.bash"
+apt update
+apt-get install -y ros-jazzy-hardware-interface # For some reason this package is problamatic
+rosdep install -i -y \
+    --from-paths ./mg_bringup \
+    --from-paths ./mg_msgs \
+    --from-paths ./mg_navigation \
+    --from-paths ./mg_control \
+    --from-paths ./mg_odometry \
+    --from-paths ./mg_obstacles \
+    --from-paths ./mg_lidar

scripts/run_base.sh

@@ -0,0 +1,9 @@
+#!/bin/bash
+
+ros_distro="jazzy"
+script_dir=$(dirname "$(readlink -f "${bash_source[0]}")")
+
+source "/opt/ros/${ROS_DISTRO}/setup.bash"
+source ./install/setup.bash
+
+ros2 launch mg_bringup base.launch.py $@

scripts/run_bt.sh

@@ -0,0 +1,19 @@
+#!/bin/bash
+
+
+ros_distro="jazzy"
+script_dir=$(dirname "$(readlink -f "${bash_source[0]}")")
+pids=()
+
+source "/opt/ros/${ROS_DISTRO}/setup.bash"
+source "./install/setup.bash"
+
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
+ros2 launch mg_bringup bt.launch.py & pids+=($!)
+
+sleep 1
+
+ros2 action send_goal /mg_bt_action_server btcpp_ros2_interfaces/action/ExecuteTree "target_tree: $1" & pids+=($!)
+
+wait

toid_bot_description/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_bot_description)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY 
+    src 
+    launch
+    rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

toid_bot_description/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

toid_bot_description/launch/display.launch.py

@@ -0,0 +1,50 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition 
+from launch.substitutions import Command, LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    pkg_share = FindPackageShare("").find('toid_bot_description')
+    default_model_path = os.path.join(pkg_share, 'src', 'toid_bot_description.urdf')
+    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'default.rviz')
+
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
+    )
+
+    joint_state_publisher_node = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        parameters=[{'robot_description': Command(['xacro ', default_model_path])}],
+        condition=UnlessCondition(LaunchConfiguration('gui'))
+    )
+
+    joint_state_publisher_gui_node = Node(
+        package='joint_state_publisher_gui',
+        executable='joint_state_publisher_gui',
+        name='joint_state_publisher_gui',
+        condition=IfCondition(LaunchConfiguration('gui'))
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', default_rviz_config_path]
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(name='gui', default_value='True', description='Flag to enable joint_state_publisher'),
+        DeclareLaunchArgument(name='model', default_value=default_model_path, description='Path to model'),
+        robot_state_publisher_node,
+        joint_state_publisher_node,
+        joint_state_publisher_gui_node,
+        rviz_node
+    ])

toid_bot_description/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_bot_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="example@example.com">pimpest</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_bot_description/rviz/default.rviz

@@ -0,0 +1,235 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 542
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.30000001192092896
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        drivewhl_l_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        drivewhl_r_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_caster:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_caster:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        drivewhl_l_link:
+          Value: true
+        drivewhl_r_link:
+          Value: true
+        left_caster:
+          Value: true
+        lidar:
+          Value: true
+        right_caster:
+          Value: true
+      Marker Scale: 0.30000001192092896
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          base_footprint:
+            {}
+          drivewhl_l_link:
+            {}
+          drivewhl_r_link:
+            {}
+          left_caster:
+            {}
+          lidar:
+            {}
+          right_caster:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_footprint
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/XYOrbit
+      Distance: 1.59040105342865
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6053992509841919
+      Target Frame: <Fixed Frame>
+      Value: XYOrbit (rviz_default_plugins)
+      Yaw: 1.088563323020935
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016c000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000229000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 60
+  Y: 60

toid_bot_description/src/toid_bot_control.xacro

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true">
+
+    <ros2_control name="${name}" type="system">
+      <xacro:unless value="${use_mock_hardware}">
+        <hardware>
+            <plugin>mg_control/MgStepperInterface</plugin>
+        </hardware>
+      </xacro:unless>
+      <xacro:if value="${use_mock_hardware}">
+        <hardware>
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="calculate_dynamics">true</param>
+        </hardware>
+      </xacro:if>
+
+      <joint name="drivewhl_l_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+
+      <joint name="drivewhl_r_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>

toid_bot_description/src/toid_bot_description.urdf

@@ -0,0 +1,117 @@
+<?xml version="1.0"?>
+<robot name="toid_bot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+<xacro:arg name="use_mock" default="true" />
+
+<xacro:property name="base_width" value="0.30"/>
+<xacro:property name="base_length" value="0.30"/>
+<xacro:property name="base_height" value="0.34"/>
+
+<xacro:property name="wheel_radius" value="0.04"/>
+<xacro:property name="wheel_width" value="0.03"/>
+
+<xacro:property name="wheel_zoff" value="0.03"/>
+<xacro:property name="wheel_xoff" value="-0.07"/>
+<xacro:property name="wheel_inset" value="0.01"/>
+
+<xacro:property name="caster_radius" value="0.02"/>
+
+<xacro:property name="caster_inset" value="0.04"/>
+<xacro:property name="caster_xoff" value="0.10"/>
+
+
+<xacro:property name="lidar_radius" value="0.03"/>
+<xacro:property name="lidar_height" value="0.02"/>
+<xacro:property name="lidar_xoff" value="0.09"/>
+
+<xacro:property name="base_zoff" value="${-wheel_zoff + wheel_radius + base_height/2}"/>
+
+<link name="base_link">
+    <visual>
+        <geometry>
+            <box size="${base_length} ${base_width} ${base_height}"/>
+        </geometry>
+        <material name="Cyan">
+            <color rgba="0 1.0 1.0 1.0"/>
+        </material>
+    </visual>
+</link>
+
+<link name="base_footprint"/>
+
+<link name="lidar">
+    <visual>
+        <geometry>
+            <cylinder radius="${lidar_radius}" length="${lidar_height}"/>
+        </geometry>
+        <material name="White">
+            <color rgba="0.9 0.9 0.9 1.0"/>
+        </material>
+    </visual>
+</link>
+
+<joint name="base_joint" type="fixed">
+    <child link="base_link"/>
+    <parent link="base_footprint"/>
+    <origin xyz="${-wheel_xoff} 0.0 ${base_zoff}" rpy="0 0 0"/>
+</joint>
+
+<joint name="lidar_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="lidar"/>
+    <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 0"/>
+</joint>
+
+
+<xacro:macro name="wheel" params="prefix y_reflect">
+    <link name="${prefix}_link">
+        <visual>
+            <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+            <geometry>
+                <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+            </geometry>
+            <material name="Yellow">
+                <color rgba="0.8 0.8 0.3 1.0"/>
+            </material>
+        </visual>
+    </link>
+
+    <joint name="${prefix}_joint" type="continuous">
+        <parent link="base_link"/>
+        <child link="${prefix}_link"/>
+        <origin xyz="${wheel_xoff} ${y_reflect*(base_width/2 - wheel_inset)} ${wheel_zoff-base_height/2}" rpy="0 0 0"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+</xacro:macro>
+
+
+<xacro:macro name="cstr" params="prefix y_reflect">
+    <link name="${prefix}_caster">
+        <visual>
+            <geometry>
+                <sphere radius="${caster_radius}"/>
+            </geometry>
+            <material name="White">
+                <color rgba="1.0 1.0 1.0 1.0"/>
+            </material>
+        </visual>
+    </link>
+
+    <joint name="${prefix}_caster_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="${prefix}_caster"/>
+        <origin xyz="${caster_xoff} ${y_reflect*(base_width/2 - caster_inset)} ${caster_radius-base_zoff}" rpy="0 0 0"/>
+    </joint>
+</xacro:macro>
+
+
+<xacro:wheel prefix="drivewhl_l" y_reflect="1" />
+<xacro:wheel prefix="drivewhl_r" y_reflect="-1" />
+
+<xacro:cstr prefix="left" y_reflect="1" />
+<xacro:cstr prefix="right" y_reflect="-1" />
+
+<xacro:include filename="$(find toid_bot_description)/src/toid_bot_control.xacro"/>
+<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)"/>
+
+</robot>

util/docker_registry/config/config.yml

@@ -0,0 +1,9 @@
+version: 0.1
+storage:
+  filesystem:
+    rootdirectory: /var/lib/registry
+http:
+  addr: :5000
+  headers:
+    X-Content-Type-Options: [nosniff]
+

util/docker_registry/docker-compose.yaml

@@ -0,0 +1,13 @@
+services:
+  registry:
+    restart: always
+    image: registry:2
+    ports:
+      - 5000:5000
+    environment:
+      REGISTRY_HTTP_TLS_CERTIFICATE: /certs/domain.crt
+      REGISTRY_HTTP_TLS_KEY: /certs/domain.key
+      REGISTRY_STORAGE_FILESYSTEM_ROOTDIRECTORY: /var/lib/registry
+    volumes:
+      - ./config/config.yml:/etc/docker/registry/config.yml:ro
+      - ./data:/var/lib/registry:rw