Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Compare commits

base: Magnezijum-Robotics:side-switching
Branches Tags
Magnezijum-Robotics:main Magnezijum-Robotics:flake-dev Magnezijum-Robotics:side-switching Magnezijum-Robotics:odometry-calib Magnezijum-Robotics:licensing
..
compare: Magnezijum-Robotics:flake-dev
Branches Tags
Magnezijum-Robotics:flake-dev Magnezijum-Robotics:side-switching Magnezijum-Robotics:main Magnezijum-Robotics:odometry-calib Magnezijum-Robotics:licensing

1 Commits
side-switc ... flake-dev

Author SHA1 Message Date
Pimpest
d8b55f08de First flake environment 2025-11-06 15:59:22 +01:00
72 changed files with 532 additions and 1799 deletions
Expand all files Collapse all files

22
firmware/base/src/main.c
View File

@ -156,20 +156,11 @@ void core2_entry()
data = (data >> 8ll) | ((uint64_t)ch<<56ll); data = (data >> 8ll) | ((uint64_t)ch<<56ll);
readNum--; readNum--;
if(!readNum) { if(!readNum) {
if(type == 'w'){ if(type == 'w')
wheel_separation = *((double*)&data); wheel_separation = *((double*)&data);
} else if(type == 'X'){ else
base_x = *((double*)&data);
} else if(type == 'Y'){
base_y = *((double*)&data);
} else if(type == 'T'){
base_theta = *((double*)&data);
calib_enc.left_pulse = 0;
calib_enc.right_pulse = 0;
} else{
wheel_ratio_l = 1 + (*((double *)&data)); wheel_ratio_l = 1 + (*((double *)&data));
wheel_ratio_r = 1 - (*((double *)&data)); wheel_ratio_r = 1 - (*((double *)&data));
}
} }
} }
@ -182,15 +173,6 @@ void core2_entry()
} else if(cmd == (((uint16_t)'r' << 8) | ';')){ } else if(cmd == (((uint16_t)'r' << 8) | ';')){
readNum = 8; readNum = 8;
type = (cmd >> 8) & 0xFF; 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) | ';')) { } else if(cmd == (((uint16_t)'z' << 8) | ';')) {
zero(); zero();
} else if(cmd == (((uint16_t)'c' << 8) | ';')) { } else if(cmd == (((uint16_t)'c' << 8) | ';')) {

43
firmware/topfloor/CMakeLists.txt
View File

@ -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)

121
firmware/topfloor/pico_sdk_import.cmake
View File

@ -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})

62
firmware/topfloor/src/actions/user.c
View File

@ -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);
}

21
firmware/topfloor/src/actions/user.h
View File

@ -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;

85
firmware/topfloor/src/main.c
View File

@ -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
}
}

35
firmware/topfloor/src/servo/servo.c
View File

@ -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);
}

7
firmware/topfloor/src/servo/servo.h
View File

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

5
firmware/topfloor/src/stepper/indentity.h
View File

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

24
firmware/topfloor/src/stepper/stepper.c
View File

@ -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);
}
}

12
firmware/topfloor/src/stepper/stepper.h
View File

@ -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);

84
flake.lock generated Normal file
View File

@ -0,0 +1,84 @@
{
"nodes": {
"flake-utils": {
"inputs": {
"systems": "systems"
},
"locked": {
"lastModified": 1731533236,
"narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
"owner": "numtide",
"repo": "flake-utils",
"rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
"type": "github"
},
"original": {
"owner": "numtide",
"repo": "flake-utils",
"type": "github"
}
},
"nix-ros-overlay": {
"inputs": {
"flake-utils": "flake-utils",
"nixpkgs": "nixpkgs"
},
"locked": {
"lastModified": 1761810010,
"narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
"owner": "lopsided98",
"repo": "nix-ros-overlay",
"rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
"type": "github"
},
"original": {
"owner": "lopsided98",
"ref": "master",
"repo": "nix-ros-overlay",
"type": "github"
}
},
"nixpkgs": {
"locked": {
"lastModified": 1744849697,
"narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
"owner": "lopsided98",
"repo": "nixpkgs",
"rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
"type": "github"
},
"original": {
"owner": "lopsided98",
"ref": "nix-ros",
"repo": "nixpkgs",
"type": "github"
}
},
"root": {
"inputs": {
"nix-ros-overlay": "nix-ros-overlay",
"nixpkgs": [
"nix-ros-overlay",
"nixpkgs"
]
}
},
"systems": {
"locked": {
"lastModified": 1681028828,
"narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
"owner": "nix-systems",
"repo": "default",
"rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
"type": "github"
},
"original": {
"owner": "nix-systems",
"repo": "default",
"type": "github"
}
}
},
"root": "root",
"version": 7
}

76
flake.nix Normal file
View File

@ -0,0 +1,76 @@
{
inputs = {
nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
nixpkgs.follows = "nix-ros-overlay/nixpkgs";
};
outputs =
{
self,
nix-ros-overlay,
nixpkgs,
}:
nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
system:
let
pkgs = import nixpkgs {
inherit system;
overlays = [ nix-ros-overlay.overlays.default ];
};
in
{
devShells.default = pkgs.mkShell {
name = "Project Robotoid";
packages = [
pkgs.colcon
pkgs.llvmPackages_20.clang-tools
(
with pkgs.rosPackages.jazzy;
buildEnv {
paths = [
# Dependencies from package.xml files
desktop
ament-cmake-core
python-cmake-module
ament-lint-auto
ament-lint-common
behaviortree-cpp
controller-manager
diff-drive-controller
geometry-msgs
hardware-interface
joint-state-broadcaster
launch
launch-ros
pkgs.glm
pkgs.jsoncpp.dev
pkgs.i2c-tools
pluginlib
rclcpp
rclcpp-action
rclcpp-components
rclcpp-lifecycle
robot-state-publisher
ros2-control
ros2launch
rosidl-default-generators
rosidl-default-runtime
sensor-msgs
std-msgs
std-srvs
tf2
tf2-geometry-msgs
tf2-ros
visualization-msgs
];
}
)
];
};
}
);
nixConfig = {
extra-substituters = [ "https://attic.iid.ciirc.cvut.cz/ros" ];
extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
};
}

2
mg_bringup/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.24)
set(CMAKE_EXPORT_COMPILE_COMMANDS OFF) set(CMAKE_EXPORT_COMPILE_COMMANDS OFF)
project(mg_bringup) project(mg_bringup)

35
mg_bringup/launch/launch-external.py
View File

@ -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',
)
])

26
mg_bringup/launch/launch.py
View File

@ -1,6 +1,6 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction 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.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
from launch_ros.actions import Node from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
@ -26,14 +26,6 @@ def generate_launch_description():
'local_test': LaunchConfiguration('local_test') 'local_test': LaunchConfiguration('local_test')
}.items() }.items()
), ),
IncludeLaunchDescription(
PathJoinSubstitution([
FindPackageShare("mg_lidar"),
'launch',
'launch.py'
]),
condition=UnlessCondition(LaunchConfiguration('local_test')),
),
Node( Node(
package="mg_odometry", package="mg_odometry",
executable="mg_odom_publisher", executable="mg_odom_publisher",
@ -47,22 +39,6 @@ def generate_launch_description():
emulate_tty=True, emulate_tty=True,
output='screen' 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( Node(
package="mg_navigation", package="mg_navigation",
executable="mg_nav_server", executable="mg_nav_server",

18
mg_bringup/package.nix Normal file
View File

@ -0,0 +1,18 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, launch, launch-ros, mg-control, mg-navigation, mg-odometry }:
buildRosPackage rec {
pname = "ros-jazzy-mg-bringup";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake ];
propagatedBuildInputs = [ launch launch-ros mg-control mg-navigation mg-odometry ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "TODO: Package description";
license = with lib.licenses; [ asl20 ];
};
}

4
mg_bt/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.12) cmake_minimum_required(VERSION 3.24)
project(mg_bt) project(mg_bt)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
@ -49,7 +49,7 @@ target_include_directories(
target_link_libraries(mg_i2cnode i2c) target_link_libraries(mg_i2cnode i2c)
ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS}) 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 install( TARGETS
mg_bt_executor mg_bt_executor

147
mg_bt/behavior_trees/calib_bt.xml
View File

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

90
mg_bt/behavior_trees/mg_bt.btproj
View File

@ -1,7 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4" project_name="Project"> <root BTCPP_format="4" project_name="Project">
<include path="calib_bt.xml"/> <include path="calib_bt.xml"/>
<include path="tactics.xml"/>
<!-- Description of Node Models (used by Groot) --> <!-- Description of Node Models (used by Groot) -->
<TreeNodesModel> <TreeNodesModel>
<Action ID="CalibWidth"> <Action ID="CalibWidth">
@ -10,80 +9,37 @@
<output_port name="NewWidth" type="double"/> <output_port name="NewWidth" type="double"/>
<input_port name="service_name" type="std::string">Service name</input_port> <input_port name="service_name" type="std::string">Service name</input_port>
</Action> </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"> <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="x_goal" type="double"/>
<input_port name="y_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>
<Action ID="RotateNode"> <Action ID="RotateNode">
<input_port name="angle" type="double"/> <input_port name="angle" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/> <input_port name="pos_mult" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/> <input_port name="max_wheel_speed" type="double"/>
<input_port name="tolerance" default="0.001000" 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> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </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"> <Action ID="ZeroNode">
<input_port name="service_name" type="std::string">Service name</input_port> <input_port name="service_name" type="std::string">Service name</input_port>
</Action> </Action>

540
mg_bt/behavior_trees/tactics.xml
View File

@ -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>

19
mg_bt/behavior_trees/tactics_blue.xml
View File

@ -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>

49
mg_bt/i2cmodule/i2cnode.cpp
View File

@ -1,8 +1,5 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "mg_msgs/srv/i2c.hpp" #include "mg_msgs/srv/i2c.hpp"
#include "mg_msgs/action/i2c.hpp"
extern "C" { extern "C" {
#include <fcntl.h> #include <fcntl.h>
@ -10,21 +7,15 @@ extern "C" {
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <i2c/smbus.h> #include <i2c/smbus.h>
} }
class MgI2c : public rclcpp::Node { class MgI2c : public rclcpp::Node {
using I2cSrv = mg_msgs::srv::I2c; using I2cSrv = mg_msgs::srv::I2c;
using I2cAction = mg_msgs::action::I2c;
using GoalHandleI2c = rclcpp_action::ServerGoalHandle<I2cAction>;
public: public:
MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
auto cb auto cb
= [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); }; = [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
using namespace std::placeholders; i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
i2c_action_ = rclcpp_action::create_server<I2cAction>(this,
"/i2c_action",
bind(&MgI2c::handle_goal, this, _1, _2),
bind(&MgI2c::handle_cancel, this, _1),
bind(&MgI2c::handle_accepted, this, _1));
} }
void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const { 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_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: private:
rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_; rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
int i2c_fd_;
rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
int i2c_fd_;
}; };
int main(int argc, const char* const* argv) { int main(int argc, const char* const* argv) {

5
mg_bt/launch/launch.py
View File

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

19
mg_bt/package.nix Normal file
View File

@ -0,0 +1,19 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, ament-lint-auto, ament-lint-common, behaviortree-cpp, behaviortree-ros2, btcpp-ros2-interfaces, i2c-tools, mg-msgs, rclcpp }:
buildRosPackage rec {
pname = "ros-jazzy-mg-bt";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake ];
checkInputs = [ ament-lint-auto ament-lint-common ];
propagatedBuildInputs = [ behaviortree-cpp behaviortree-ros2 btcpp-ros2-interfaces i2c-tools mg-msgs rclcpp ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "Behavior for MagRob";
license = with lib.licenses; [ asl20 ];
};
}

14
mg_bt/src/mg_tree_executor.cpp
View File

@ -2,16 +2,9 @@
#include "behaviortree_cpp/xml_parsing.h" #include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp" #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/i2c.hpp"
#include "tree_nodes/move_point.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/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
#include "tree_nodes/set_pos.hpp"
#include "tree_nodes/zero.hpp" #include "tree_nodes/zero.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@ -32,16 +25,9 @@ namespace mg {
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) { void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
factory.registerNodeType<mg::MovePointNode>("MovePoint", node()); factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
factory.registerNodeType<mg::RotateNode>("RotateNode", node()); factory.registerNodeType<mg::RotateNode>("RotateNode", node());
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node()); factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::I2cNode>("I2CSignal", 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(); }); factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
} }

49
mg_bt/src/tree_nodes/choose_can.hpp
View File

@ -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;
}
};
}

22
mg_bt/src/tree_nodes/choose_tactic.hpp
View File

@ -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;
}
};
}

18
mg_bt/src/tree_nodes/i2c.hpp
View File

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

50
mg_bt/src/tree_nodes/look_at.hpp
View File

@ -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;
}
};
}

58
mg_bt/src/tree_nodes/move_global.hpp
View File

@ -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;
}
};
}

67
mg_bt/src/tree_nodes/move_local.hpp
View File

@ -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;
}
};
}

24
mg_bt/src/tree_nodes/move_point.hpp
View File

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

32
mg_bt/src/tree_nodes/rotate.hpp
View File

@ -11,12 +11,18 @@ namespace mg {
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { } BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
static BT::PortsList providedPorts() { static BT::PortsList providedPorts() {
return providedBasicPorts({ return providedBasicPorts({ BT::InputPort<double>("angle"),
BT::InputPort<double>("angle"), BT::InputPort<double>("tolerance"),
BT::InputPort<double>("tolerance", 0.001, {}), BT::InputPort<double>("max_wheel_speed"),
BT::InputPort<double>("max_wheel_speed", 3, {}), BT::InputPort<double>("max_angular"),
BT::InputPort<double>("max_angular", 3.14, {}), BT::InputPort<double>("max_vel"),
}); BT::InputPort<double>("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 { bool setGoal(Goal& goal) override {
@ -24,10 +30,24 @@ namespace mg {
auto tolerance = getInput<double>("tolerance"); auto tolerance = getInput<double>("tolerance");
auto max_wheel_speed = getInput<double>("max_wheel_speed"); auto max_wheel_speed = getInput<double>("max_wheel_speed");
auto max_angular = getInput<double>("max_angular"); 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.angle = angle.value();
goal.tolerance = tolerance.value_or(goal.tolerance); goal.tolerance = tolerance.value_or(goal.tolerance);
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed); 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_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; return true;
} }

35
mg_bt/src/tree_nodes/set_pos.hpp
View File

@ -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;
}
};
}

22
mg_bt/src/tree_nodes/side_pub.hpp
View File

@ -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;
}
}
};
}

5
mg_control/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.24)
project(mg_control) project(mg_control)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -13,7 +13,6 @@ find_package(hardware_interface REQUIRED)
find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_lifecycle REQUIRED)
find_package(rclcpp_components REQUIRED) find_package(rclcpp_components REQUIRED)
include(FindPkgConfig) include(FindPkgConfig)
pkg_search_module(LIBSERIAL REQUIRED libserial)
add_library( add_library(
mg_control mg_control
@ -24,13 +23,11 @@ add_library(
target_include_directories( target_include_directories(
mg_control mg_control
PRIVATE PRIVATE
${LIBSERIAL_INCLUDE_DIRS}
include include
) )
target_link_libraries( target_link_libraries(
mg_control mg_control
${LIBSERIAL_LIBRARIES}
) )
ament_target_dependencies( ament_target_dependencies(

2
mg_control/assets/mg_base.urdf
View File

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

2
mg_control/assets/mg_base_local.urdf
View File

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

4
mg_control/include/mg_control/mg_control.hpp
View File

@ -3,7 +3,7 @@
#include <vector> #include <vector>
#include <string> #include <string>
#include "libserial/SerialPort.h" //#include "libserial/SerialPort.h"
#include "hardware_interface/handle.hpp" #include "hardware_interface/handle.hpp"
#include "hardware_interface/system_interface.hpp" #include "hardware_interface/system_interface.hpp"
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@ -26,7 +26,7 @@ namespace mg {
private: private:
std::string serial_port_name; std::string serial_port_name;
LibSerial::SerialPort odrive_serial_interface; // LibSerial::SerialPort odrive_serial_interface;
double left_wheel_vel_cmd = 0; double left_wheel_vel_cmd = 0;
double left_wheel_pos_state = 0; double left_wheel_pos_state = 0;

19
mg_control/package.nix Normal file
View File

@ -0,0 +1,19 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, ament-lint-auto, ament-lint-common, controller-manager, diff-drive-controller, hardware-interface, joint-state-broadcaster, libserial-dev, pluginlib, rclcpp, rclcpp-components, rclcpp-lifecycle, robot-state-publisher, ros2-control, ros2launch }:
buildRosPackage rec {
pname = "ros-jazzy-mg-control";
version = "0.0.1";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake ];
checkInputs = [ ament-lint-auto ament-lint-common ];
propagatedBuildInputs = [ controller-manager diff-drive-controller hardware-interface joint-state-broadcaster libserial-dev pluginlib rclcpp rclcpp-components rclcpp-lifecycle robot-state-publisher ros2-control ros2launch ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "Ros2 control compatible drivers for magrob";
license = with lib.licenses; [ asl20 ];
};
}

8
mg_control/src/mg_control.cpp
View File

@ -8,11 +8,13 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
/*
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) { if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
serial_port_name = info_.hardware_parameters["device_path"]; serial_port_name = info_.hardware_parameters["device_path"];
} else { } else {
serial_port_name = "/dev/ttyACM1"; serial_port_name = "/dev/ttyACM1";
} }
*/
left_wheel_pos_state = 0; left_wheel_pos_state = 0;
right_wheel_pos_state = 0; right_wheel_pos_state = 0;
@ -22,6 +24,7 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
} }
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State&) { CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State&) {
/*
try { try {
if (!odrive_serial_interface.IsOpen()) { if (!odrive_serial_interface.IsOpen()) {
odrive_serial_interface.Open(serial_port_name); odrive_serial_interface.Open(serial_port_name);
@ -31,13 +34,16 @@ CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::Stat
"Failed to open serial port (Is the stepper driver plugged in)"); "Failed to open serial port (Is the stepper driver plugged in)");
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
*/
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) { CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
/*
if (odrive_serial_interface.IsOpen()) { if (odrive_serial_interface.IsOpen()) {
odrive_serial_interface.Close(); odrive_serial_interface.Close();
} }
*/
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
@ -66,6 +72,7 @@ hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&
} }
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) { hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
/*
union { union {
std::array<u_char, sizeof(double)> bytes; std::array<u_char, sizeof(double)> bytes;
double data; double data;
@ -79,5 +86,6 @@ hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); } for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; } } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
*/
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }

2
mg_lidar/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.12) cmake_minimum_required(VERSION 3.24)
project(mg_lidar) project(mg_lidar)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)

19
mg_lidar/package.nix Normal file
View File

@ -0,0 +1,19 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, ament-lint-auto, ament-lint-common, mg-msgs, rclcpp, sensor-msgs, std-msgs, tf2, tf2-geometry-msgs, tf2-ros, visualization-msgs }:
buildRosPackage rec {
pname = "ros-jazzy-mg-lidar";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake ];
checkInputs = [ ament-lint-auto ament-lint-common ];
propagatedBuildInputs = [ mg-msgs rclcpp sensor-msgs std-msgs tf2 tf2-geometry-msgs tf2-ros visualization-msgs ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "Lidar based opponent tracking";
license = with lib.licenses; [ asl20 ];
};
}

1
mg_lidar/src/scanner.cpp
View File

@ -8,6 +8,7 @@
#include "tf2_ros/transform_listener.h" #include "tf2_ros/transform_listener.h"
#include "tf2/convert.hpp" #include "tf2/convert.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp> #include <glm/gtx/norm.hpp>
class MgScanner : public rclcpp::Node { class MgScanner : public rclcpp::Node {

4
mg_msgs/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.24)
project(mg_msgs) project(mg_msgs)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -17,10 +17,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"action/MovePoint.action" "action/MovePoint.action"
"action/LookAt.action" "action/LookAt.action"
"action/Rotate.action" "action/Rotate.action"
"action/I2c.action"
"srv/CalcPath.srv" "srv/CalcPath.srv"
"srv/SendDouble.srv" "srv/SendDouble.srv"
"srv/SendPose2d.srv"
"srv/I2c.srv" "srv/I2c.srv"
) )

5
mg_msgs/action/I2c.action
View File

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

18
mg_msgs/package.nix Normal file
View File

@ -0,0 +1,18 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, rosidl-default-generators, rosidl-default-runtime }:
buildRosPackage rec {
pname = "ros-jazzy-mg-msgs";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake rosidl-default-generators ];
propagatedBuildInputs = [ rosidl-default-runtime ];
nativeBuildInputs = [ ament-cmake rosidl-default-generators ];
meta = {
description = "This contains various msg/action used within the project";
license = with lib.licenses; [ mit ];
};
}

4
mg_msgs/srv/SendPose2d.srv
View File

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

2
mg_navigation/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.24)
project(mg_navigation) project(mg_navigation)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

16
mg_navigation/include/handlers/dwa_core.hpp
View File

@ -125,14 +125,11 @@ namespace mg {
pos_query(); pos_query();
populate_candidates(spacevl, spacevr, dimx, dimy); populate_candidates(spacevl, spacevr, dimx, dimy);
double b_score = calc_score(spacevl[0], spacevr[0]) - 1; double b_score = calc_score(spacevl[0], spacevr[0]);
int b_ind = -1; uint b_ind = 0;
for (uint i = 1; i < spacevl.size(); i++) { for (uint i = 1; i < spacevl.size(); i++) {
const double score = calc_score(spacevl[i], spacevr[i]); const double score = calc_score(spacevl[i], spacevr[i]);
if (score == NAN) {
continue;
}
if (score > b_score) { if (score > b_score) {
b_score = score; b_score = score;
b_ind = i; 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 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];
cvl = spacevl[b_ind]; cvr = spacevr[b_ind];
cvr = spacevr[b_ind];
} else {
cvl = 0;
cvr = 0;
}
send_speed(step * cvl, step * cvr); send_speed(step * cvl, step * cvr);
rate.sleep(); rate.sleep();
} }

4
mg_navigation/include/handlers/dwa_global.hpp
View File

@ -70,8 +70,8 @@ namespace mg {
const double obstacle_scoreA = glm::max(0.0, 0.02 - dist); const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2); const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);
// RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist); RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
// RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2); RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos)); score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
// score += goal->ornt_mult * (angl - n_angl); // score += goal->ornt_mult * (angl - n_angl);

6
mg_navigation/include/handlers/dwa_lookat.hpp
View File

@ -13,9 +13,8 @@ namespace mg {
const double a = glm::abs(theta - target_ornt); const double a = glm::abs(theta - target_ornt);
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a; 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 <> template <>
@ -57,9 +56,6 @@ namespace mg {
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old; 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_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); const double score = goal->ornt_mult * (dist_old - dist_new);
return score; return score;

7
mg_navigation/include/handlers/dwa_point.hpp
View File

@ -49,13 +49,6 @@ namespace mg {
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta); 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 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 // Calculate angle to goal post/pre movement
const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) })); const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) })); const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));

18
mg_navigation/package.nix Normal file
View File

@ -0,0 +1,18 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, geometry-msgs, glm, mg-msgs, mg-obstacles, rclcpp, rclcpp-action, rclcpp-components, tf2, tf2-geometry-msgs, tf2-ros }:
buildRosPackage rec {
pname = "ros-jazzy-mg-navigation";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake glm ];
propagatedBuildInputs = [ geometry-msgs mg-msgs mg-obstacles rclcpp rclcpp-action rclcpp-components tf2 tf2-geometry-msgs tf2-ros ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "TODO: Package description";
license = with lib.licenses; [ asl20 ];
};
}

1
mg_navigation/src/mg_navigation.cpp
View File

@ -2,6 +2,7 @@
#include <functional> #include <functional>
#include <thread> #include <thread>
#define GLM_ENABLE_EXPERIMENTAL
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "mg_navigation.hpp" #include "mg_navigation.hpp"
#include "handlers/dwa.hpp" #include "handlers/dwa.hpp"

1
mg_navigation/src/path_buffer.cpp
View File

@ -1,5 +1,6 @@
#include "path_buffer.hpp" #include "path_buffer.hpp"
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp> #include <glm/gtx/norm.hpp>
#include <chrono> #include <chrono>

5
mg_obstacles/include/mg_obstacles/mg_obstacles.hpp
View File

@ -6,7 +6,6 @@
#include "mg_obstacles/static_obstacle.hpp" #include "mg_obstacles/static_obstacle.hpp"
#include "std_msgs/msg/u_int64.hpp" #include "std_msgs/msg/u_int64.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/buffer.h" #include "tf2_ros/buffer.h"
@ -52,13 +51,9 @@ namespace mg {
rclcpp::CallbackGroup::SharedPtr no_exec_cbg; rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr side_sub_;
rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_; rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_; rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
std::string base_path;
void load_permanent(Json::Value& json); void load_permanent(Json::Value& json);
void load_static(Json::Value& json); void load_static(Json::Value& json);
}; };

2
mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp
View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include <glm/glm.hpp> #include <glm/glm.hpp>
#include <jsoncpp/json/json.h> #include <json/json.h>
namespace mg { namespace mg {
class PermanentObstacle { class PermanentObstacle {

2
mg_obstacles/include/mg_obstacles/static_obstacle.hpp
View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include <glm/glm.hpp> #include <glm/glm.hpp>
#include <jsoncpp/json/json.h> #include <json/json.h>
namespace mg { namespace mg {
class StaticObstacle { class StaticObstacle {

94
mg_obstacles/obstacles/blue-side.json
View File

@ -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
}
]
}

0
mg_obstacles/obstacles/yellow-side.json → mg_obstacles/obstacles/obstacles.json
View File

18
mg_obstacles/package.nix Normal file
View File

@ -0,0 +1,18 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, geometry-msgs, glm, jsoncpp, mg-msgs, rclcpp, rclcpp-action, rclcpp-components, tf2, tf2-geometry-msgs, tf2-ros }:
buildRosPackage rec {
pname = "ros-jazzy-mg-obstacles";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake glm ];
propagatedBuildInputs = [ geometry-msgs jsoncpp mg-msgs rclcpp rclcpp-action rclcpp-components tf2 tf2-geometry-msgs tf2-ros ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "Library for collision detection";
license = with lib.licenses; [ asl20 ];
};
}

21
mg_obstacles/src/mg_obstacles.cpp
View File

@ -5,6 +5,7 @@
#include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/point_stamped.hpp"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#define GLM_ENABLE_EXPERIMENTAL
#include "glm/glm.hpp" #include "glm/glm.hpp"
#include "glm/gtx/matrix_transform_2d.hpp" #include "glm/gtx/matrix_transform_2d.hpp"
@ -27,33 +28,23 @@ namespace mg {
dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>( dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
"/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts); "/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 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_->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('/'); ulong n = obstacle_pkg.find_first_of('/');
if (n < obstacle_pkg.size()) { if (n < obstacle_pkg.size()) {
file_suffix = obstacle_pkg.substr(n); file_suffix = obstacle_pkg.substr(n);
obstacle_pkg = obstacle_pkg.substr(0, n); obstacle_pkg = obstacle_pkg.substr(0, n);
} }
base_path = ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix;
Json::Value json; Json::Value json;
RCLCPP_INFO(node_->get_logger(), RCLCPP_INFO(node_->get_logger(),
"Read file %s", "Read file %s",
(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str()); (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; json_document >> json;
load_permanent(json); load_permanent(json);
load_static(json); load_static(json);
} }
@ -193,7 +184,6 @@ namespace mg {
} }
void ObstacleManager::load_permanent(Json::Value& json) { void ObstacleManager::load_permanent(Json::Value& json) {
permanent_obstacles_.clear();
Json::Value& obstacles = json["obstacles"]; Json::Value& obstacles = json["obstacles"];
@ -204,7 +194,6 @@ namespace mg {
} }
void ObstacleManager::load_static(Json::Value& json) { void ObstacleManager::load_static(Json::Value& json) {
static_obstacles_.clear();
StaticObstacle::width = json["staticObstacleWidth"].asDouble(); StaticObstacle::width = json["staticObstacleWidth"].asDouble();
StaticObstacle::height = json["staticObstacleHeight"].asDouble(); StaticObstacle::height = json["staticObstacleHeight"].asDouble();

1
mg_obstacles/src/sdf.cpp
View File

@ -1,4 +1,5 @@
#include "mg_obstacles/sdf.hpp" #include "mg_obstacles/sdf.hpp"
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp> #include <glm/gtx/norm.hpp>
namespace mg { namespace mg {

6
mg_odometry/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.24)
project(mg_odometry) project(mg_odometry)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -14,7 +14,6 @@ find_package(tf2_geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED) find_package(std_srvs REQUIRED)
find_package(mg_msgs REQUIRED) find_package(mg_msgs REQUIRED)
include(FindPkgConfig) include(FindPkgConfig)
pkg_search_module(LIBSERIAL REQUIRED libserial)
# uncomment the following section in order to fill in # uncomment the following section in order to fill in
# further dependencies manually. # further dependencies manually.
@ -36,11 +35,10 @@ ament_target_dependencies(
target_include_directories(mg_odom_publisher PUBLIC target_include_directories(mg_odom_publisher PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/${PROJECT_NAME}> $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
${LIBSERIAL_INCLUDE_DIRS}) )
target_link_libraries( target_link_libraries(
mg_odom_publisher mg_odom_publisher
${LIBSERIAL_LIBRARIES}
) )
target_compile_features(mg_odom_publisher PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_features(mg_odom_publisher PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17

19
mg_odometry/package.nix Normal file
View File

@ -0,0 +1,19 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, ament-lint-auto, ament-lint-common, libserial-dev, mg-msgs, rclcpp, std-srvs, tf2, tf2-geometry-msgs, tf2-ros }:
buildRosPackage rec {
pname = "ros-jazzy-mg-odometry";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake ];
checkInputs = [ ament-lint-auto ament-lint-common ];
propagatedBuildInputs = [ mg-msgs rclcpp std-srvs tf2 tf2-geometry-msgs tf2-ros ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "TODO: Package description";
license = with lib.licenses; [ "TODO-License-declaration" ];
};
}

47
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -5,11 +5,10 @@
#include <memory> #include <memory>
#include <rclcpp/service.hpp> #include <rclcpp/service.hpp>
#include <string> #include <string>
#include <libserial/SerialStream.h> //#include <libserial/SerialStream.h>
#include <sys/types.h> #include <sys/types.h>
#include "mg_msgs/srv/send_double.hpp" #include "mg_msgs/srv/send_double.hpp"
#include "mg_msgs/srv/send_pose2d.hpp"
#include "std_srvs/srv/empty.hpp" #include "std_srvs/srv/empty.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp"
@ -27,7 +26,6 @@
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1"; constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
using SendDoubleSrv = mg_msgs::srv::SendDouble; using SendDoubleSrv = mg_msgs::srv::SendDouble;
using SendPoseSrv = mg_msgs::srv::SendPose2d;
using ZeroSrv = std_srvs::srv::Empty; using ZeroSrv = std_srvs::srv::Empty;
class MgOdomPublisher : public rclcpp::Node { class MgOdomPublisher : public rclcpp::Node {
@ -46,8 +44,6 @@ class MgOdomPublisher : public rclcpp::Node {
"/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1)); "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
set_ratio_service_ = create_service<SendDoubleSrv>( set_ratio_service_ = create_service<SendDoubleSrv>(
"/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1)); "/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_ zero_service_
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1)); = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@ -60,15 +56,14 @@ class MgOdomPublisher : public rclcpp::Node {
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_; rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_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 zero_service_;
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_; rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
LibSerial::SerialStream enc_serial_port_; //LibSerial::SerialStream enc_serial_port_;
void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) { void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data); RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
/*
if (enc_serial_port_.IsOpen()) { if (enc_serial_port_.IsOpen()) {
union { union {
std::array<u_char, sizeof(double)> bytes; std::array<u_char, sizeof(double)> bytes;
@ -79,36 +74,12 @@ class MgOdomPublisher : public rclcpp::Node {
enc_serial_port_ << "w;"; enc_serial_port_ << "w;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; } for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
} }
} */
void set_pose(const std::shared_ptr<SendPoseSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting pose to: %lf %lf %lf", request->x, request->y, request->theta);
if (enc_serial_port_.IsOpen()) {
union {
std::array<u_char, sizeof(double)> bytes;
double data;
} value{};
value.data = request->x;
enc_serial_port_ << "X;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
value.data = request->y;
enc_serial_port_ << "Y;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
value.data = request->theta;
enc_serial_port_ << "T;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
}
} }
void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) { void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data); RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);
/*
if (enc_serial_port_.IsOpen()) { if (enc_serial_port_.IsOpen()) {
union { union {
std::array<u_char, sizeof(double)> bytes; std::array<u_char, sizeof(double)> bytes;
@ -119,11 +90,13 @@ class MgOdomPublisher : public rclcpp::Node {
enc_serial_port_ << "r;"; enc_serial_port_ << "r;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; } for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
} }
*/
} }
void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) { void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib"); RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib");
/*
if (enc_serial_port_.IsOpen()) { if (enc_serial_port_.IsOpen()) {
enc_serial_port_ << "c;"; enc_serial_port_ << "c;";
double left_gain = 0; double left_gain = 0;
@ -131,14 +104,16 @@ class MgOdomPublisher : public rclcpp::Node {
enc_serial_port_ >> left_gain >> right_gain; enc_serial_port_ >> left_gain >> right_gain;
RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain); RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain);
} }
*/
} }
void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) { void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Zeroing odometry"); RCLCPP_INFO(get_logger(), "Zeroing odometry");
/*
if (enc_serial_port_.IsOpen()) { if (enc_serial_port_.IsOpen()) {
enc_serial_port_ << "z;"; enc_serial_port_ << "z;";
} }
*/
} }
void timer_callback() { void timer_callback() {
@ -148,6 +123,7 @@ class MgOdomPublisher : public rclcpp::Node {
RCLCPP_DEBUG(this->get_logger(), "Callback called"); RCLCPP_DEBUG(this->get_logger(), "Callback called");
/*
try { try {
if (!enc_serial_port_.IsOpen()) { if (!enc_serial_port_.IsOpen()) {
enc_serial_port_.Open(this->get_parameter("serial_path").as_string()); enc_serial_port_.Open(this->get_parameter("serial_path").as_string());
@ -164,6 +140,7 @@ class MgOdomPublisher : public rclcpp::Node {
make_transform(_x, _y, _theta); make_transform(_x, _y, _theta);
} catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "%s\n", e.what()); } } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "%s\n", e.what()); }
*/
} }
void make_transform(double x, double y, double theta) { void make_transform(double x, double y, double theta) {

5
mg_planner/CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.24)
project(mg_planner) project(mg_planner)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -35,6 +35,7 @@ add_executable(mg_planner
src/a_star.cpp src/a_star.cpp
) )
ament_target_dependencies(mg_planner ${PACKAGE_DEPS}) ament_target_dependencies(mg_planner ${PACKAGE_DEPS})
target_include_directories( target_include_directories(
@ -49,6 +50,8 @@ target_link_libraries(
${LIBGLM_LIBRARIES} ${LIBGLM_LIBRARIES}
) )
target_compile_definitions(mg_planner PRIVATE GLM_ENABLE_EXPERIMENTAL)
install( TARGETS install( TARGETS
mg_planner mg_planner
DESTINATION lib/${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}

1
mg_planner/include/mg_planner/a_star/a_star.hpp
View File

@ -3,6 +3,7 @@
#include "mg_planner/config.hpp" #include "mg_planner/config.hpp"
#include "rclcpp/node.hpp" #include "rclcpp/node.hpp"
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/fwd.hpp> #include <glm/fwd.hpp>
#include <glm/glm.hpp> #include <glm/glm.hpp>
#include <glm/gtx/fast_square_root.hpp> #include <glm/gtx/fast_square_root.hpp>

18
mg_planner/package.nix Normal file
View File

@ -0,0 +1,18 @@
# Automatically generated by: ros2nix --distro jazzy
{ lib, buildRosPackage, ament-cmake, glm, mg-msgs, mg-obstacles, rclcpp, rclcpp-action, tf2, tf2-geometry-msgs, tf2-ros }:
buildRosPackage rec {
pname = "ros-jazzy-mg-planner";
version = "0.0.0";
src = ./.;
buildType = "ament_cmake";
buildInputs = [ ament-cmake glm ];
propagatedBuildInputs = [ mg-msgs mg-obstacles rclcpp rclcpp-action tf2 tf2-geometry-msgs tf2-ros ];
nativeBuildInputs = [ ament-cmake ];
meta = {
description = "TODO: Package description";
license = with lib.licenses; [ asl20 ];
};
}

2
mg_planner/src/mg_planner_node.cpp
View File

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