Compare commits
1 Commits
side-switc
...
flake-dev
| Author | SHA1 | Date | |
|---|---|---|---|
| d8b55f08de |
@ -156,22 +156,13 @@ void core2_entry()
|
||||
data = (data >> 8ll) | ((uint64_t)ch<<56ll);
|
||||
readNum--;
|
||||
if(!readNum) {
|
||||
if(type == 'w'){
|
||||
if(type == 'w')
|
||||
wheel_separation = *((double*)&data);
|
||||
} else if(type == 'X'){
|
||||
base_x = *((double*)&data);
|
||||
} else if(type == 'Y'){
|
||||
base_y = *((double*)&data);
|
||||
} else if(type == 'T'){
|
||||
base_theta = *((double*)&data);
|
||||
calib_enc.left_pulse = 0;
|
||||
calib_enc.right_pulse = 0;
|
||||
} else{
|
||||
else
|
||||
wheel_ratio_l = 1 + (*((double *)&data));
|
||||
wheel_ratio_r = 1 - (*((double *)&data));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(cmd == (((uint16_t)'g' << 8) | ';')) {
|
||||
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
|
||||
@ -182,15 +173,6 @@ void core2_entry()
|
||||
} else if(cmd == (((uint16_t)'r' << 8) | ';')){
|
||||
readNum = 8;
|
||||
type = (cmd >> 8) & 0xFF;
|
||||
} else if(cmd == (((uint16_t)'X' << 8) | ';')){
|
||||
readNum = 8;
|
||||
type = (cmd >> 8) & 0xFF;
|
||||
} else if(cmd == (((uint16_t)'Y' << 8) | ';')){
|
||||
readNum = 8;
|
||||
type = (cmd >> 8) & 0xFF;
|
||||
} else if(cmd == (((uint16_t)'T' << 8) | ';')){
|
||||
readNum = 8;
|
||||
type = (cmd >> 8) & 0xFF;
|
||||
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
|
||||
zero();
|
||||
} else if(cmd == (((uint16_t)'c' << 8) | ';')) {
|
||||
|
||||
@ -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)
|
||||
@ -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})
|
||||
@ -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);
|
||||
}
|
||||
@ -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;
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
@ -1,7 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
void setup_servo(uint8_t pin);
|
||||
|
||||
void set_degree(uint8_t pin, float angle);
|
||||
@ -1,5 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#define INVALID 0
|
||||
#define STEPPER_DRIVER 1
|
||||
#define SERVO_DRIVER 2
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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
84
flake.lock
generated
Normal 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
76
flake.nix
Normal 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=" ];
|
||||
};
|
||||
}
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS OFF)
|
||||
|
||||
project(mg_bringup)
|
||||
|
||||
@ -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',
|
||||
)
|
||||
])
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
|
||||
from launch.conditions import UnlessCondition, IfCondition
|
||||
from launch.conditions import UnlessCondition
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
@ -26,14 +26,6 @@ def generate_launch_description():
|
||||
'local_test': LaunchConfiguration('local_test')
|
||||
}.items()
|
||||
),
|
||||
IncludeLaunchDescription(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("mg_lidar"),
|
||||
'launch',
|
||||
'launch.py'
|
||||
]),
|
||||
condition=UnlessCondition(LaunchConfiguration('local_test')),
|
||||
),
|
||||
Node(
|
||||
package="mg_odometry",
|
||||
executable="mg_odom_publisher",
|
||||
@ -47,22 +39,6 @@ def generate_launch_description():
|
||||
emulate_tty=True,
|
||||
output='screen'
|
||||
),
|
||||
IncludeLaunchDescription(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("mg_bt"),
|
||||
'launch',
|
||||
'launch.py'
|
||||
]),
|
||||
condition=IfCondition(LaunchConfiguration('local_test')),
|
||||
),
|
||||
Node(
|
||||
package="mg_planner",
|
||||
executable="mg_planner",
|
||||
name="mg_planner",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration('local_test')),
|
||||
output='screen',
|
||||
),
|
||||
Node(
|
||||
package="mg_navigation",
|
||||
executable="mg_nav_server",
|
||||
|
||||
18
mg_bringup/package.nix
Normal file
18
mg_bringup/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.12)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_bt)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
@ -49,7 +49,7 @@ target_include_directories(
|
||||
target_link_libraries(mg_i2cnode i2c)
|
||||
|
||||
ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
|
||||
ament_target_dependencies(mg_i2cnode rclcpp rclcpp_action mg_msgs)
|
||||
ament_target_dependencies(mg_i2cnode rclcpp mg_msgs)
|
||||
|
||||
install( TARGETS
|
||||
mg_bt_executor
|
||||
|
||||
@ -3,52 +3,29 @@
|
||||
<BehaviorTree ID="calib2_bt">
|
||||
<Sequence>
|
||||
<ZeroNode service_name="/zero"/>
|
||||
<MovePoint x_goal="0.7"
|
||||
y_goal="0.0"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="0.1"
|
||||
max_vel="2.000000"
|
||||
ornt_mult="4.000000"
|
||||
<MovePoint action_name="/MovePoint"
|
||||
tolerance="0.02"
|
||||
action_name="/MovePoint"/>
|
||||
max_angular="0.1"
|
||||
x_goal="0.7"
|
||||
y_goal="0.0"/>
|
||||
<RotateNode angle="-2"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<RotateNode angle="-3.14"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<MovePoint x_goal="0.3"
|
||||
y_goal="0.0"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
max_vel="2.000000"
|
||||
ornt_mult="4.000000"
|
||||
<MovePoint action_name="/MovePoint"
|
||||
tolerance="0.02"
|
||||
action_name="/MovePoint"/>
|
||||
x_goal="0.3"
|
||||
y_goal="0.0"/>
|
||||
<RotateNode angle="-2"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<RotateNode angle="0"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<Fallback>
|
||||
<Timeout msec="5000">
|
||||
<MovePoint x_goal="-0.1"
|
||||
y_goal="0.0"
|
||||
max_wheel_speed="3.000000"
|
||||
<MovePoint action_name="/MovePoint"
|
||||
max_angular="0.1"
|
||||
max_vel="2.000000"
|
||||
ornt_mult="4.000000"
|
||||
tolerance="0.001000"
|
||||
action_name="/MovePoint"/>
|
||||
x_goal="-0.1"
|
||||
y_goal="0.0"/>
|
||||
</Timeout>
|
||||
<AlwaysSuccess/>
|
||||
</Fallback>
|
||||
@ -64,39 +41,23 @@
|
||||
<Repeat num_cycles="5">
|
||||
<Sequence>
|
||||
<ZeroNode service_name="/zero"/>
|
||||
<MovePoint x_goal="0.7"
|
||||
y_goal="0.0"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="0.1"
|
||||
max_vel="2.000000"
|
||||
ornt_mult="4.000000"
|
||||
<MovePoint action_name="/MovePoint"
|
||||
tolerance="0.04"
|
||||
action_name="/MovePoint"/>
|
||||
max_angular="0.1"
|
||||
x_goal="0.7"
|
||||
y_goal="0.0"/>
|
||||
<RotateNode angle="2"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<RotateNode angle="-2"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<RotateNode angle="0"
|
||||
max_wheel_speed="3.000000"
|
||||
max_angular="3.140000"
|
||||
tolerance="0.001000"
|
||||
action_name="/Rotate"/>
|
||||
<Fallback>
|
||||
<Timeout msec="20000">
|
||||
<MovePoint x_goal="-0.1"
|
||||
y_goal="0.0"
|
||||
max_wheel_speed="3.000000"
|
||||
<MovePoint action_name="/MovePoint"
|
||||
max_angular="0.1"
|
||||
max_vel="2.000000"
|
||||
ornt_mult="4.000000"
|
||||
tolerance="0.001000"
|
||||
action_name="/MovePoint"/>
|
||||
x_goal="-0.1"
|
||||
y_goal="0.0"/>
|
||||
</Timeout>
|
||||
<AlwaysSuccess/>
|
||||
</Fallback>
|
||||
@ -124,39 +85,59 @@
|
||||
type="std::string">Service name</input_port>
|
||||
</Action>
|
||||
<Action ID="MovePoint">
|
||||
<input_port name="action_name"
|
||||
type="std::string">Action server name</input_port>
|
||||
<input_port name="tolerance"
|
||||
type="double"/>
|
||||
<input_port name="obst_mult_a"
|
||||
type="double"/>
|
||||
<input_port name="ornt_mult"
|
||||
type="double"/>
|
||||
<input_port name="pos_mult"
|
||||
type="double"/>
|
||||
<input_port name="t_ornt_mult"
|
||||
type="double"/>
|
||||
<input_port name="obst_mult_c"
|
||||
type="double"/>
|
||||
<input_port name="obst_mult_b"
|
||||
type="double"/>
|
||||
<input_port name="max_vel"
|
||||
type="double"/>
|
||||
<input_port name="max_angular"
|
||||
type="double"/>
|
||||
<input_port name="IgnoreList"
|
||||
type="std::string"/>
|
||||
<input_port name="max_wheel_speed"
|
||||
type="double"/>
|
||||
<input_port name="x_goal"
|
||||
type="double"/>
|
||||
<input_port name="y_goal"
|
||||
type="double"/>
|
||||
<input_port name="max_wheel_speed"
|
||||
default="3.000000"
|
||||
type="double"/>
|
||||
<input_port name="max_angular"
|
||||
default="3.140000"
|
||||
type="double"/>
|
||||
<input_port name="max_vel"
|
||||
default="2.000000"
|
||||
type="double"/>
|
||||
<input_port name="ornt_mult"
|
||||
default="4.000000"
|
||||
type="double"/>
|
||||
<input_port name="tolerance"
|
||||
default="0.001000"
|
||||
type="double"/>
|
||||
<input_port name="action_name"
|
||||
type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="RotateNode">
|
||||
<input_port name="angle"
|
||||
type="double"/>
|
||||
<input_port name="max_wheel_speed"
|
||||
default="3.000000"
|
||||
<input_port name="pos_mult"
|
||||
type="double"/>
|
||||
<input_port name="max_wheel_speed"
|
||||
type="double"/>
|
||||
<input_port name="IgnoreList"
|
||||
type="std::string"/>
|
||||
<input_port name="max_angular"
|
||||
default="3.140000"
|
||||
type="double"/>
|
||||
<input_port name="max_vel"
|
||||
type="double"/>
|
||||
<input_port name="obst_mult_b"
|
||||
type="double"/>
|
||||
<input_port name="obst_mult_c"
|
||||
type="double"/>
|
||||
<input_port name="ornt_mult"
|
||||
type="double"/>
|
||||
<input_port name="t_ornt_mult"
|
||||
type="double"/>
|
||||
<input_port name="obst_mult_a"
|
||||
type="double"/>
|
||||
<input_port name="tolerance"
|
||||
default="0.001000"
|
||||
type="double"/>
|
||||
<input_port name="action_name"
|
||||
type="std::string">Action server name</input_port>
|
||||
|
||||
@ -1,7 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<root BTCPP_format="4" project_name="Project">
|
||||
<include path="calib_bt.xml"/>
|
||||
<include path="tactics.xml"/>
|
||||
<!-- Description of Node Models (used by Groot) -->
|
||||
<TreeNodesModel>
|
||||
<Action ID="CalibWidth">
|
||||
@ -10,80 +9,37 @@
|
||||
<output_port name="NewWidth" type="double"/>
|
||||
<input_port name="service_name" type="std::string">Service name</input_port>
|
||||
</Action>
|
||||
<Action ID="CanChooser">
|
||||
<output_port name="orient" type="int"/>
|
||||
<output_port name="y_loc" type="double"/>
|
||||
<output_port name="x_loc" type="double"/>
|
||||
<input_port name="canlist" type="std::string"/>
|
||||
</Action>
|
||||
<Action ID="I2CSignal">
|
||||
<input_port name="Address" default="42" type="int"/>
|
||||
<input_port name="Data" default="0" type="int"/>
|
||||
<output_port name="Result" type="int"/>
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="LookAtNode">
|
||||
<input_port name="x" type="double"/>
|
||||
<input_port name="y" type="double"/>
|
||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="MoveGlobal">
|
||||
<input_port name="x_goal" type="double"/>
|
||||
<input_port name="y_goal" type="double"/>
|
||||
<input_port name="max_wheel_speed" default="6.000000" type="double"/>
|
||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
||||
<input_port name="max_vel" default="4.000000" type="double"/>
|
||||
<input_port name="ornt_mult" default="3.000000" type="double"/>
|
||||
<input_port name="tolerance" default="0.050000" type="double"/>
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="MoveLocal">
|
||||
<input_port name="x_goal" type="double"/>
|
||||
<input_port name="y_goal" type="double"/>
|
||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
||||
<input_port name="max_vel" default="2.000000" type="double"/>
|
||||
<input_port name="pos_mult" default="15.000000" type="double"/>
|
||||
<input_port name="ornt_mult" default="4.000000" type="double"/>
|
||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="MovePoint">
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
<input_port name="tolerance" type="double"/>
|
||||
<input_port name="obst_mult_a" type="double"/>
|
||||
<input_port name="ornt_mult" type="double"/>
|
||||
<input_port name="pos_mult" type="double"/>
|
||||
<input_port name="t_ornt_mult" type="double"/>
|
||||
<input_port name="obst_mult_c" type="double"/>
|
||||
<input_port name="obst_mult_b" type="double"/>
|
||||
<input_port name="max_vel" type="double"/>
|
||||
<input_port name="max_angular" type="double"/>
|
||||
<input_port name="IgnoreList" type="std::string"/>
|
||||
<input_port name="max_wheel_speed" type="double"/>
|
||||
<input_port name="x_goal" type="double"/>
|
||||
<input_port name="y_goal" type="double"/>
|
||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
||||
<input_port name="max_vel" default="2.000000" type="double"/>
|
||||
<input_port name="ornt_mult" default="4.000000" type="double"/>
|
||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="RotateNode">
|
||||
<input_port name="angle" type="double"/>
|
||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
||||
<input_port name="pos_mult" type="double"/>
|
||||
<input_port name="max_wheel_speed" type="double"/>
|
||||
<input_port name="IgnoreList" type="std::string"/>
|
||||
<input_port name="max_angular" type="double"/>
|
||||
<input_port name="max_vel" type="double"/>
|
||||
<input_port name="obst_mult_b" type="double"/>
|
||||
<input_port name="obst_mult_c" type="double"/>
|
||||
<input_port name="ornt_mult" type="double"/>
|
||||
<input_port name="t_ornt_mult" type="double"/>
|
||||
<input_port name="obst_mult_a" type="double"/>
|
||||
<input_port name="tolerance" type="double"/>
|
||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||
</Action>
|
||||
<Action ID="SendPose">
|
||||
<input_port name="x" type="double"/>
|
||||
<input_port name="y" type="double"/>
|
||||
<input_port name="angle" type="double"/>
|
||||
<input_port name="isDegree" default="true" type="bool"/>
|
||||
<input_port name="service_name" type="std::string">Service name</input_port>
|
||||
</Action>
|
||||
<Condition ID="SideObstaclePub">
|
||||
<input_port name="tactic" type="int"/>
|
||||
<input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
|
||||
</Condition>
|
||||
<Action ID="TacticChooser">
|
||||
<output_port name="IsBlue" type="bool"/>
|
||||
<output_port name="tactic" type="int"/>
|
||||
<input_port name="number" type="int"/>
|
||||
</Action>
|
||||
<Action ID="ZeroNode">
|
||||
<input_port name="service_name" type="std::string">Service name</input_port>
|
||||
</Action>
|
||||
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -1,8 +1,5 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "mg_msgs/srv/i2c.hpp"
|
||||
#include "mg_msgs/action/i2c.hpp"
|
||||
|
||||
extern "C" {
|
||||
|
||||
#include <fcntl.h>
|
||||
@ -10,21 +7,15 @@ extern "C" {
|
||||
#include <sys/ioctl.h>
|
||||
#include <i2c/smbus.h>
|
||||
}
|
||||
|
||||
class MgI2c : public rclcpp::Node {
|
||||
using I2cSrv = mg_msgs::srv::I2c;
|
||||
using I2cAction = mg_msgs::action::I2c;
|
||||
using GoalHandleI2c = rclcpp_action::ServerGoalHandle<I2cAction>;
|
||||
|
||||
public:
|
||||
MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
|
||||
auto cb
|
||||
= [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
|
||||
using namespace std::placeholders;
|
||||
i2c_action_ = rclcpp_action::create_server<I2cAction>(this,
|
||||
"/i2c_action",
|
||||
bind(&MgI2c::handle_goal, this, _1, _2),
|
||||
bind(&MgI2c::handle_cancel, this, _1),
|
||||
bind(&MgI2c::handle_accepted, this, _1));
|
||||
i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
|
||||
}
|
||||
|
||||
void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
|
||||
@ -42,40 +33,8 @@ class MgI2c : public rclcpp::Node {
|
||||
RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front());
|
||||
}
|
||||
|
||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
|
||||
std::shared_ptr<const I2cAction::Goal> goal) {
|
||||
RCLCPP_INFO(this->get_logger(), "Received goal request with addr %d and data %d", goal->addr, goal->data);
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleI2c>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleI2c> goal_handle) {
|
||||
using namespace std::placeholders;
|
||||
ioctl(i2c_fd_, I2C_SLAVE, goal_handle->get_goal()->addr); // NOLINT
|
||||
i2c_smbus_write_byte(i2c_fd_, goal_handle->get_goal()->data);
|
||||
int ch = 0;
|
||||
|
||||
rclcpp::Rate rate(100);
|
||||
|
||||
while (ch == 0 || (ch > 255 || ch < 0)) {
|
||||
ch = i2c_smbus_read_byte(i2c_fd_);
|
||||
rate.sleep();
|
||||
}
|
||||
auto res = std::make_shared<I2cAction::Result>();
|
||||
RCLCPP_INFO(get_logger(), "Recieved %d", ch);
|
||||
res->resp.push_back(ch);
|
||||
goal_handle->succeed(res);
|
||||
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
|
||||
|
||||
rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
|
||||
|
||||
int i2c_fd_;
|
||||
};
|
||||
|
||||
|
||||
@ -18,10 +18,5 @@ def generate_launch_description():
|
||||
output="screen",
|
||||
parameters=[bt_exec_config]
|
||||
),
|
||||
Node(
|
||||
package='mg_planner',
|
||||
executable='mg_planner',
|
||||
output="screen"
|
||||
),
|
||||
])
|
||||
|
||||
19
mg_bt/package.nix
Normal file
19
mg_bt/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -2,16 +2,9 @@
|
||||
|
||||
#include "behaviortree_cpp/xml_parsing.h"
|
||||
#include "tree_nodes/calib.hpp"
|
||||
#include "tree_nodes/choose_can.hpp"
|
||||
#include "tree_nodes/choose_tactic.hpp"
|
||||
#include "tree_nodes/i2c.hpp"
|
||||
#include "tree_nodes/move_point.hpp"
|
||||
#include "tree_nodes/move_local.hpp"
|
||||
#include "tree_nodes/move_global.hpp"
|
||||
#include "tree_nodes/look_at.hpp"
|
||||
#include "tree_nodes/rotate.hpp"
|
||||
#include "tree_nodes/side_pub.hpp"
|
||||
#include "tree_nodes/set_pos.hpp"
|
||||
#include "tree_nodes/zero.hpp"
|
||||
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
@ -32,16 +25,9 @@ namespace mg {
|
||||
|
||||
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
|
||||
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
|
||||
factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
|
||||
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
|
||||
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
|
||||
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
|
||||
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
|
||||
factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
|
||||
factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
|
||||
factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
|
||||
factory.registerNodeType<mg::CanChooser>("CanChooser");
|
||||
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
|
||||
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
}
|
||||
@ -1,13 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
#include "behaviortree_ros2/bt_action_node.hpp"
|
||||
#include "mg_msgs/action/i2c.hpp"
|
||||
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||
#include "mg_msgs/srv/i2c.hpp"
|
||||
|
||||
namespace mg {
|
||||
class I2cNode : public BT::RosActionNode<mg_msgs::action::I2c> {
|
||||
class I2cNode : public BT::RosServiceNode<mg_msgs::srv::I2c> {
|
||||
public:
|
||||
I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
||||
BT::RosActionNode<mg_msgs::action::I2c>(name, conf, params) { }
|
||||
BT::RosServiceNode<mg_msgs::srv::I2c>(name, conf, params) { }
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
|
||||
@ -15,14 +15,14 @@ namespace mg {
|
||||
BT::OutputPort<int>("Result") });
|
||||
}
|
||||
|
||||
bool setGoal(Goal& req) override {
|
||||
req.addr = getInput<int>("Address").value_or(42);
|
||||
req.data = getInput<int>("Data").value_or(0);
|
||||
bool setRequest(typename Request::SharedPtr& req) override {
|
||||
req->addr = getInput<int>("Address").value_or(42);
|
||||
req->data = getInput<int>("Result").value_or(0);
|
||||
return true;
|
||||
}
|
||||
|
||||
BT::NodeStatus onResultReceived(const WrappedResult& resp) override {
|
||||
setOutput<int>("Result", resp.result->resp.front());
|
||||
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
|
||||
setOutput<int>("Result", resp->resp.front());
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@ -13,11 +13,17 @@ namespace mg {
|
||||
static BT::PortsList providedPorts() {
|
||||
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
|
||||
BT::InputPort<double>("y_goal"),
|
||||
BT::InputPort<double>("tolerance", 0.001, {}),
|
||||
BT::InputPort<double>("max_wheel_speed", 3, {}),
|
||||
BT::InputPort<double>("max_angular", 3.14, {}),
|
||||
BT::InputPort<double>("max_vel", 2, {}),
|
||||
BT::InputPort<double>("ornt_mult", 4, {}) });
|
||||
BT::InputPort<double>("tolerance"),
|
||||
BT::InputPort<double>("max_wheel_speed"),
|
||||
BT::InputPort<double>("max_angular"),
|
||||
BT::InputPort<double>("max_vel"),
|
||||
BT::InputPort<double>("pos_mult"),
|
||||
BT::InputPort<double>("ornt_mult"),
|
||||
BT::InputPort<double>("t_ornt_mult"),
|
||||
BT::InputPort<double>("obst_mult_a"),
|
||||
BT::InputPort<double>("obst_mult_b"),
|
||||
BT::InputPort<double>("obst_mult_c"),
|
||||
BT::InputPort<std::string>("IgnoreList", "", {}) });
|
||||
}
|
||||
|
||||
bool setGoal(Goal& goal) override {
|
||||
@ -29,6 +35,10 @@ namespace mg {
|
||||
auto max_vel = getInput<double>("max_vel");
|
||||
auto pos_mult = getInput<double>("pos_mult");
|
||||
auto ornt_mult = getInput<double>("ornt_mult");
|
||||
auto t_ornt_mult = getInput<double>("t_ornt_mult");
|
||||
auto obst_mult_a = getInput<double>("obst_mult_a");
|
||||
auto obst_mult_b = getInput<double>("obst_mult_b");
|
||||
auto obst_mult_c = getInput<double>("obst_mult_c");
|
||||
goal.x = x_goal.value();
|
||||
goal.y = y_goal.value();
|
||||
goal.tolerance = tolerance.value_or(goal.tolerance);
|
||||
@ -37,6 +47,10 @@ namespace mg {
|
||||
goal.max_vel = max_vel.value_or(goal.max_vel);
|
||||
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
|
||||
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
|
||||
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
|
||||
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
|
||||
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
|
||||
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -11,12 +11,18 @@ namespace mg {
|
||||
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return providedBasicPorts({
|
||||
BT::InputPort<double>("angle"),
|
||||
BT::InputPort<double>("tolerance", 0.001, {}),
|
||||
BT::InputPort<double>("max_wheel_speed", 3, {}),
|
||||
BT::InputPort<double>("max_angular", 3.14, {}),
|
||||
});
|
||||
return providedBasicPorts({ BT::InputPort<double>("angle"),
|
||||
BT::InputPort<double>("tolerance"),
|
||||
BT::InputPort<double>("max_wheel_speed"),
|
||||
BT::InputPort<double>("max_angular"),
|
||||
BT::InputPort<double>("max_vel"),
|
||||
BT::InputPort<double>("pos_mult"),
|
||||
BT::InputPort<double>("ornt_mult"),
|
||||
BT::InputPort<double>("t_ornt_mult"),
|
||||
BT::InputPort<double>("obst_mult_a"),
|
||||
BT::InputPort<double>("obst_mult_b"),
|
||||
BT::InputPort<double>("obst_mult_c"),
|
||||
BT::InputPort<std::string>("IgnoreList", "", {}) });
|
||||
}
|
||||
|
||||
bool setGoal(Goal& goal) override {
|
||||
@ -24,10 +30,24 @@ namespace mg {
|
||||
auto tolerance = getInput<double>("tolerance");
|
||||
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
||||
auto max_angular = getInput<double>("max_angular");
|
||||
auto max_vel = getInput<double>("max_vel");
|
||||
auto pos_mult = getInput<double>("pos_mult");
|
||||
auto ornt_mult = getInput<double>("ornt_mult");
|
||||
auto t_ornt_mult = getInput<double>("t_ornt_mult");
|
||||
auto obst_mult_a = getInput<double>("obst_mult_a");
|
||||
auto obst_mult_b = getInput<double>("obst_mult_b");
|
||||
auto obst_mult_c = getInput<double>("obst_mult_c");
|
||||
goal.angle = angle.value();
|
||||
goal.tolerance = tolerance.value_or(goal.tolerance);
|
||||
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
||||
goal.max_angular = max_angular.value_or(goal.max_angular);
|
||||
goal.max_vel = max_vel.value_or(goal.max_vel);
|
||||
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
|
||||
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
|
||||
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
|
||||
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
|
||||
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
|
||||
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_control)
|
||||
|
||||
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_components REQUIRED)
|
||||
include(FindPkgConfig)
|
||||
pkg_search_module(LIBSERIAL REQUIRED libserial)
|
||||
|
||||
add_library(
|
||||
mg_control
|
||||
@ -24,13 +23,11 @@ add_library(
|
||||
target_include_directories(
|
||||
mg_control
|
||||
PRIVATE
|
||||
${LIBSERIAL_INCLUDE_DIRS}
|
||||
include
|
||||
)
|
||||
|
||||
target_link_libraries(
|
||||
mg_control
|
||||
${LIBSERIAL_LIBRARIES}
|
||||
)
|
||||
|
||||
ament_target_dependencies(
|
||||
|
||||
@ -55,7 +55,7 @@
|
||||
</joint>
|
||||
|
||||
|
||||
<ros2_control name="mg-base" type="system">
|
||||
<ros2_control name="mg_base" type="system">
|
||||
<hardware>
|
||||
<plugin>mg_control/MgStepperInterface</plugin>
|
||||
</hardware>
|
||||
|
||||
@ -55,7 +55,7 @@
|
||||
</joint>
|
||||
|
||||
|
||||
<ros2_control name="mg-base" type="system">
|
||||
<ros2_control name="mg_base" type="system">
|
||||
<hardware>
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
<param name="calculate_dynamics">true</param>
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "libserial/SerialPort.h"
|
||||
//#include "libserial/SerialPort.h"
|
||||
#include "hardware_interface/handle.hpp"
|
||||
#include "hardware_interface/system_interface.hpp"
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
@ -26,7 +26,7 @@ namespace mg {
|
||||
|
||||
private:
|
||||
std::string serial_port_name;
|
||||
LibSerial::SerialPort odrive_serial_interface;
|
||||
// LibSerial::SerialPort odrive_serial_interface;
|
||||
|
||||
double left_wheel_vel_cmd = 0;
|
||||
double left_wheel_pos_state = 0;
|
||||
|
||||
19
mg_control/package.nix
Normal file
19
mg_control/package.nix
Normal 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,11 +8,13 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
|
||||
serial_port_name = info_.hardware_parameters["device_path"];
|
||||
} else {
|
||||
serial_port_name = "/dev/ttyACM1";
|
||||
}
|
||||
*/
|
||||
|
||||
left_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&) {
|
||||
/*
|
||||
try {
|
||||
if (!odrive_serial_interface.IsOpen()) {
|
||||
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)");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
*/
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
|
||||
/*
|
||||
if (odrive_serial_interface.IsOpen()) {
|
||||
odrive_serial_interface.Close();
|
||||
}
|
||||
*/
|
||||
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&) {
|
||||
/*
|
||||
union {
|
||||
std::array<u_char, sizeof(double)> bytes;
|
||||
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); }
|
||||
|
||||
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
|
||||
*/
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.12)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_lidar)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
|
||||
19
mg_lidar/package.nix
Normal file
19
mg_lidar/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -8,6 +8,7 @@
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "tf2/convert.hpp"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
#define GLM_ENABLE_EXPERIMENTAL
|
||||
#include <glm/gtx/norm.hpp>
|
||||
|
||||
class MgScanner : public rclcpp::Node {
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_msgs)
|
||||
|
||||
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/LookAt.action"
|
||||
"action/Rotate.action"
|
||||
"action/I2c.action"
|
||||
"srv/CalcPath.srv"
|
||||
"srv/SendDouble.srv"
|
||||
"srv/SendPose2d.srv"
|
||||
"srv/I2c.srv"
|
||||
)
|
||||
|
||||
|
||||
@ -1,5 +0,0 @@
|
||||
uint8 addr
|
||||
uint8 data
|
||||
---
|
||||
uint8[] resp
|
||||
---
|
||||
18
mg_msgs/package.nix
Normal file
18
mg_msgs/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -1,4 +0,0 @@
|
||||
float64 x
|
||||
float64 y
|
||||
float64 theta
|
||||
---
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_navigation)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
|
||||
@ -125,14 +125,11 @@ namespace mg {
|
||||
pos_query();
|
||||
populate_candidates(spacevl, spacevr, dimx, dimy);
|
||||
|
||||
double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
|
||||
int b_ind = -1;
|
||||
double b_score = calc_score(spacevl[0], spacevr[0]);
|
||||
uint b_ind = 0;
|
||||
|
||||
for (uint i = 1; i < spacevl.size(); i++) {
|
||||
const double score = calc_score(spacevl[i], spacevr[i]);
|
||||
if (score == NAN) {
|
||||
continue;
|
||||
}
|
||||
if (score > b_score) {
|
||||
b_score = score;
|
||||
b_ind = i;
|
||||
@ -141,13 +138,8 @@ namespace mg {
|
||||
|
||||
calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
|
||||
|
||||
if (b_ind >= 0) {
|
||||
cvl = spacevl[b_ind];
|
||||
cvr = spacevr[b_ind];
|
||||
} else {
|
||||
cvl = 0;
|
||||
cvr = 0;
|
||||
}
|
||||
send_speed(step * cvl, step * cvr);
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
@ -70,8 +70,8 @@ namespace mg {
|
||||
const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
|
||||
const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);
|
||||
|
||||
// RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
|
||||
// RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
|
||||
RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
|
||||
RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
|
||||
|
||||
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
|
||||
// score += goal->ornt_mult * (angl - n_angl);
|
||||
|
||||
@ -13,9 +13,8 @@ namespace mg {
|
||||
|
||||
const double a = glm::abs(theta - target_ornt);
|
||||
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
|
||||
const double c = (b > glm::pi<double>() / 2) ? glm::pi<double>() - b : b;
|
||||
|
||||
return c > goal->tolerance;
|
||||
return b > goal->tolerance;
|
||||
}
|
||||
|
||||
template <>
|
||||
@ -57,9 +56,6 @@ namespace mg {
|
||||
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
|
||||
dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
|
||||
|
||||
dist_old = (dist_old > glm::pi<double>() / 2) ? glm::pi<double>() - dist_old : dist_old;
|
||||
dist_new = (dist_new > glm::pi<double>() / 2) ? glm::pi<double>() - dist_new : dist_new;
|
||||
|
||||
const double score = goal->ornt_mult * (dist_old - dist_new);
|
||||
|
||||
return score;
|
||||
|
||||
@ -49,13 +49,6 @@ namespace mg {
|
||||
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
|
||||
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
|
||||
|
||||
const double dist = baseNode.obstacle_manager()->box_colide(
|
||||
n_pos, { 0.29, 0.33 }, n_theta, ObstacleManager::ObstacleMask::DYNAMIC);
|
||||
|
||||
if (dist < 0.01) {
|
||||
return NAN;
|
||||
}
|
||||
|
||||
// Calculate angle to goal post/pre movement
|
||||
const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
|
||||
const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
|
||||
|
||||
18
mg_navigation/package.nix
Normal file
18
mg_navigation/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -2,6 +2,7 @@
|
||||
#include <functional>
|
||||
#include <thread>
|
||||
|
||||
#define GLM_ENABLE_EXPERIMENTAL
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "mg_navigation.hpp"
|
||||
#include "handlers/dwa.hpp"
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "path_buffer.hpp"
|
||||
|
||||
#define GLM_ENABLE_EXPERIMENTAL
|
||||
#include <glm/gtx/norm.hpp>
|
||||
|
||||
#include <chrono>
|
||||
|
||||
@ -6,7 +6,6 @@
|
||||
#include "mg_obstacles/static_obstacle.hpp"
|
||||
|
||||
#include "std_msgs/msg/u_int64.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
||||
|
||||
#include "tf2_ros/buffer.h"
|
||||
@ -52,13 +51,9 @@ namespace mg {
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr side_sub_;
|
||||
|
||||
rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
|
||||
rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
|
||||
|
||||
std::string base_path;
|
||||
|
||||
void load_permanent(Json::Value& json);
|
||||
void load_static(Json::Value& json);
|
||||
};
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <glm/glm.hpp>
|
||||
#include <jsoncpp/json/json.h>
|
||||
#include <json/json.h>
|
||||
|
||||
namespace mg {
|
||||
class PermanentObstacle {
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <glm/glm.hpp>
|
||||
#include <jsoncpp/json/json.h>
|
||||
#include <json/json.h>
|
||||
|
||||
namespace mg {
|
||||
class StaticObstacle {
|
||||
|
||||
@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
18
mg_obstacles/package.nix
Normal file
18
mg_obstacles/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -5,6 +5,7 @@
|
||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#define GLM_ENABLE_EXPERIMENTAL
|
||||
#include "glm/glm.hpp"
|
||||
#include "glm/gtx/matrix_transform_2d.hpp"
|
||||
|
||||
@ -27,33 +28,23 @@ namespace mg {
|
||||
dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
|
||||
"/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts);
|
||||
|
||||
side_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
||||
"/side", rclcpp::QoS(1).keep_last(1), [this](std_msgs::msg::String::ConstSharedPtr msg) {
|
||||
Json::Value json;
|
||||
RCLCPP_INFO(node_->get_logger(), "Read file %s", (base_path + "/" + msg->data).c_str());
|
||||
std::ifstream json_document(base_path + "/" + msg->data);
|
||||
json_document >> json;
|
||||
load_permanent(json);
|
||||
load_static(json);
|
||||
});
|
||||
|
||||
std::string obstacle_pkg;
|
||||
std::string file_suffix = "/obstacles/yellow-side.json";
|
||||
std::string file_suffix = "/obstacles/obstacles.json";
|
||||
node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
|
||||
node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
|
||||
node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json");
|
||||
ulong n = obstacle_pkg.find_first_of('/');
|
||||
if (n < obstacle_pkg.size()) {
|
||||
file_suffix = obstacle_pkg.substr(n);
|
||||
obstacle_pkg = obstacle_pkg.substr(0, n);
|
||||
}
|
||||
base_path = ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix;
|
||||
Json::Value json;
|
||||
RCLCPP_INFO(node_->get_logger(),
|
||||
"Read file %s",
|
||||
(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str());
|
||||
std::ifstream json_document(base_path + "/yellow-side.json");
|
||||
std::ifstream json_document(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix);
|
||||
|
||||
json_document >> json;
|
||||
|
||||
load_permanent(json);
|
||||
load_static(json);
|
||||
}
|
||||
@ -193,7 +184,6 @@ namespace mg {
|
||||
}
|
||||
|
||||
void ObstacleManager::load_permanent(Json::Value& json) {
|
||||
permanent_obstacles_.clear();
|
||||
|
||||
Json::Value& obstacles = json["obstacles"];
|
||||
|
||||
@ -204,7 +194,6 @@ namespace mg {
|
||||
}
|
||||
|
||||
void ObstacleManager::load_static(Json::Value& json) {
|
||||
static_obstacles_.clear();
|
||||
|
||||
StaticObstacle::width = json["staticObstacleWidth"].asDouble();
|
||||
StaticObstacle::height = json["staticObstacleHeight"].asDouble();
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
#include "mg_obstacles/sdf.hpp"
|
||||
#define GLM_ENABLE_EXPERIMENTAL
|
||||
#include <glm/gtx/norm.hpp>
|
||||
|
||||
namespace mg {
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_odometry)
|
||||
|
||||
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(mg_msgs REQUIRED)
|
||||
include(FindPkgConfig)
|
||||
pkg_search_module(LIBSERIAL REQUIRED libserial)
|
||||
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
@ -36,11 +35,10 @@ ament_target_dependencies(
|
||||
target_include_directories(mg_odom_publisher PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
|
||||
${LIBSERIAL_INCLUDE_DIRS})
|
||||
)
|
||||
|
||||
target_link_libraries(
|
||||
mg_odom_publisher
|
||||
${LIBSERIAL_LIBRARIES}
|
||||
)
|
||||
|
||||
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
19
mg_odometry/package.nix
Normal 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" ];
|
||||
};
|
||||
}
|
||||
@ -5,11 +5,10 @@
|
||||
#include <memory>
|
||||
#include <rclcpp/service.hpp>
|
||||
#include <string>
|
||||
#include <libserial/SerialStream.h>
|
||||
//#include <libserial/SerialStream.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include "mg_msgs/srv/send_double.hpp"
|
||||
#include "mg_msgs/srv/send_pose2d.hpp"
|
||||
#include "std_srvs/srv/empty.hpp"
|
||||
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
@ -27,7 +26,6 @@
|
||||
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
|
||||
|
||||
using SendDoubleSrv = mg_msgs::srv::SendDouble;
|
||||
using SendPoseSrv = mg_msgs::srv::SendPose2d;
|
||||
using ZeroSrv = std_srvs::srv::Empty;
|
||||
|
||||
class MgOdomPublisher : public rclcpp::Node {
|
||||
@ -46,8 +44,6 @@ class MgOdomPublisher : public rclcpp::Node {
|
||||
"/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
|
||||
set_ratio_service_ = create_service<SendDoubleSrv>(
|
||||
"/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
|
||||
set_pose_service_ = create_service<SendPoseSrv>(
|
||||
"/set_pose", std::bind(&MgOdomPublisher::set_pose, this, std::placeholders::_1));
|
||||
|
||||
zero_service_
|
||||
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
|
||||
@ -60,15 +56,14 @@ class MgOdomPublisher : public rclcpp::Node {
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
|
||||
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
|
||||
rclcpp::Service<SendPoseSrv>::SharedPtr set_pose_service_;
|
||||
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
|
||||
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
LibSerial::SerialStream enc_serial_port_;
|
||||
//LibSerial::SerialStream enc_serial_port_;
|
||||
|
||||
void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
|
||||
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
|
||||
|
||||
/*
|
||||
if (enc_serial_port_.IsOpen()) {
|
||||
union {
|
||||
std::array<u_char, sizeof(double)> bytes;
|
||||
@ -79,36 +74,12 @@ class MgOdomPublisher : public rclcpp::Node {
|
||||
enc_serial_port_ << "w;";
|
||||
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
|
||||
}
|
||||
}
|
||||
|
||||
void set_pose(const std::shared_ptr<SendPoseSrv::Request> request) {
|
||||
RCLCPP_INFO(get_logger(), "Setting pose to: %lf %lf %lf", request->x, request->y, request->theta);
|
||||
|
||||
if (enc_serial_port_.IsOpen()) {
|
||||
union {
|
||||
std::array<u_char, sizeof(double)> bytes;
|
||||
double data;
|
||||
} value{};
|
||||
value.data = request->x;
|
||||
|
||||
enc_serial_port_ << "X;";
|
||||
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
|
||||
|
||||
value.data = request->y;
|
||||
|
||||
enc_serial_port_ << "Y;";
|
||||
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
|
||||
|
||||
value.data = request->theta;
|
||||
|
||||
enc_serial_port_ << "T;";
|
||||
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
|
||||
RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);
|
||||
|
||||
/*
|
||||
if (enc_serial_port_.IsOpen()) {
|
||||
union {
|
||||
std::array<u_char, sizeof(double)> bytes;
|
||||
@ -119,11 +90,13 @@ class MgOdomPublisher : public rclcpp::Node {
|
||||
enc_serial_port_ << "r;";
|
||||
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
|
||||
RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib");
|
||||
|
||||
/*
|
||||
if (enc_serial_port_.IsOpen()) {
|
||||
enc_serial_port_ << "c;";
|
||||
double left_gain = 0;
|
||||
@ -131,14 +104,16 @@ class MgOdomPublisher : public rclcpp::Node {
|
||||
enc_serial_port_ >> left_gain >> right_gain;
|
||||
RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain);
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
|
||||
RCLCPP_INFO(get_logger(), "Zeroing odometry");
|
||||
|
||||
/*
|
||||
if (enc_serial_port_.IsOpen()) {
|
||||
enc_serial_port_ << "z;";
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void timer_callback() {
|
||||
@ -148,6 +123,7 @@ class MgOdomPublisher : public rclcpp::Node {
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Callback called");
|
||||
|
||||
/*
|
||||
try {
|
||||
if (!enc_serial_port_.IsOpen()) {
|
||||
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);
|
||||
} catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "%s\n", e.what()); }
|
||||
*/
|
||||
}
|
||||
|
||||
void make_transform(double x, double y, double theta) {
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(mg_planner)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
@ -35,6 +35,7 @@ add_executable(mg_planner
|
||||
src/a_star.cpp
|
||||
)
|
||||
|
||||
|
||||
ament_target_dependencies(mg_planner ${PACKAGE_DEPS})
|
||||
|
||||
target_include_directories(
|
||||
@ -49,6 +50,8 @@ target_link_libraries(
|
||||
${LIBGLM_LIBRARIES}
|
||||
)
|
||||
|
||||
target_compile_definitions(mg_planner PRIVATE GLM_ENABLE_EXPERIMENTAL)
|
||||
|
||||
install( TARGETS
|
||||
mg_planner
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
#include "mg_planner/config.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
|
||||
#define GLM_ENABLE_EXPERIMENTAL
|
||||
#include <glm/fwd.hpp>
|
||||
#include <glm/glm.hpp>
|
||||
#include <glm/gtx/fast_square_root.hpp>
|
||||
|
||||
18
mg_planner/package.nix
Normal file
18
mg_planner/package.nix
Normal 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 ];
|
||||
};
|
||||
}
|
||||
@ -17,6 +17,7 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
|
||||
"GeneratePath",
|
||||
[this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req,
|
||||
mg_msgs::srv::CalcPath::Response::SharedPtr resp) -> void {
|
||||
std::cout << "Bro recieved request\n";
|
||||
auto elapsed = get_clock()->now();
|
||||
std::vector<glm::ivec2> goals;
|
||||
std::transform(req->x.begin(),
|
||||
@ -37,6 +38,7 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
|
||||
mg_msgs::msg::Path path;
|
||||
path.indicies = std::vector<int>(resp->indicies);
|
||||
path_pub_->publish(path);
|
||||
RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
|
||||
});
|
||||
|
||||
path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));
|
||||
|
||||
Reference in New Issue
Block a user