Compare commits
2 Commits
side-switc
...
licensing
| Author | SHA1 | Date | |
|---|---|---|---|
| 2afb14d50a | |||
| 33d7154ba0 |
@ -25,10 +25,6 @@ Checks: "
|
|||||||
-cppcoreguidelines-pro-type-union-access,
|
-cppcoreguidelines-pro-type-union-access,
|
||||||
-cppcoreguidelines-macro-usage,
|
-cppcoreguidelines-macro-usage,
|
||||||
-performance-unnecessary-value-param,
|
-performance-unnecessary-value-param,
|
||||||
-cppcoreguidelines-pro-bounds-constant-array-index,
|
|
||||||
-readability-implicit-bool-conversion,
|
|
||||||
-cppcoreguidelines-avoid-magic-numbers,
|
|
||||||
-readability-magic-numbers,
|
|
||||||
"
|
"
|
||||||
WarningsAsErrors: ''
|
WarningsAsErrors: ''
|
||||||
HeaderFilterRegex: ''
|
HeaderFilterRegex: ''
|
||||||
|
|||||||
3
.gitmodules
vendored
3
.gitmodules
vendored
@ -1,3 +0,0 @@
|
|||||||
[submodule "ext/BehaviorTreeRos2"]
|
|
||||||
path = ext/BehaviorTreeRos2
|
|
||||||
url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
|
|
||||||
5
AUTHORS
Normal file
5
AUTHORS
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
# Authors of Magrob
|
||||||
|
#
|
||||||
|
# The following individuals and organizations hold copyright for this project:
|
||||||
|
|
||||||
|
Pimpest
|
||||||
201
LICENSE
Normal file
201
LICENSE
Normal file
@ -0,0 +1,201 @@
|
|||||||
|
Apache License
|
||||||
|
Version 2.0, January 2004
|
||||||
|
http://www.apache.org/licenses/
|
||||||
|
|
||||||
|
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||||
|
|
||||||
|
1. Definitions.
|
||||||
|
|
||||||
|
"License" shall mean the terms and conditions for use, reproduction,
|
||||||
|
and distribution as defined by Sections 1 through 9 of this document.
|
||||||
|
|
||||||
|
"Licensor" shall mean the copyright owner or entity authorized by
|
||||||
|
the copyright owner that is granting the License.
|
||||||
|
|
||||||
|
"Legal Entity" shall mean the union of the acting entity and all
|
||||||
|
other entities that control, are controlled by, or are under common
|
||||||
|
control with that entity. For the purposes of this definition,
|
||||||
|
"control" means (i) the power, direct or indirect, to cause the
|
||||||
|
direction or management of such entity, whether by contract or
|
||||||
|
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||||
|
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||||
|
|
||||||
|
"You" (or "Your") shall mean an individual or Legal Entity
|
||||||
|
exercising permissions granted by this License.
|
||||||
|
|
||||||
|
"Source" form shall mean the preferred form for making modifications,
|
||||||
|
including but not limited to software source code, documentation
|
||||||
|
source, and configuration files.
|
||||||
|
|
||||||
|
"Object" form shall mean any form resulting from mechanical
|
||||||
|
transformation or translation of a Source form, including but
|
||||||
|
not limited to compiled object code, generated documentation,
|
||||||
|
and conversions to other media types.
|
||||||
|
|
||||||
|
"Work" shall mean the work of authorship, whether in Source or
|
||||||
|
Object form, made available under the License, as indicated by a
|
||||||
|
copyright notice that is included in or attached to the work
|
||||||
|
(an example is provided in the Appendix below).
|
||||||
|
|
||||||
|
"Derivative Works" shall mean any work, whether in Source or Object
|
||||||
|
form, that is based on (or derived from) the Work and for which the
|
||||||
|
editorial revisions, annotations, elaborations, or other modifications
|
||||||
|
represent, as a whole, an original work of authorship. For the purposes
|
||||||
|
of this License, Derivative Works shall not include works that remain
|
||||||
|
separable from, or merely link (or bind by name) to the interfaces of,
|
||||||
|
the Work and Derivative Works thereof.
|
||||||
|
|
||||||
|
"Contribution" shall mean any work of authorship, including
|
||||||
|
the original version of the Work and any modifications or additions
|
||||||
|
to that Work or Derivative Works thereof, that is intentionally
|
||||||
|
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||||
|
or by an individual or Legal Entity authorized to submit on behalf of
|
||||||
|
the copyright owner. For the purposes of this definition, "submitted"
|
||||||
|
means any form of electronic, verbal, or written communication sent
|
||||||
|
to the Licensor or its representatives, including but not limited to
|
||||||
|
communication on electronic mailing lists, source code control systems,
|
||||||
|
and issue tracking systems that are managed by, or on behalf of, the
|
||||||
|
Licensor for the purpose of discussing and improving the Work, but
|
||||||
|
excluding communication that is conspicuously marked or otherwise
|
||||||
|
designated in writing by the copyright owner as "Not a Contribution."
|
||||||
|
|
||||||
|
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||||
|
on behalf of whom a Contribution has been received by Licensor and
|
||||||
|
subsequently incorporated within the Work.
|
||||||
|
|
||||||
|
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||||
|
this License, each Contributor hereby grants to You a perpetual,
|
||||||
|
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||||
|
copyright license to reproduce, prepare Derivative Works of,
|
||||||
|
publicly display, publicly perform, sublicense, and distribute the
|
||||||
|
Work and such Derivative Works in Source or Object form.
|
||||||
|
|
||||||
|
3. Grant of Patent License. Subject to the terms and conditions of
|
||||||
|
this License, each Contributor hereby grants to You a perpetual,
|
||||||
|
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||||
|
(except as stated in this section) patent license to make, have made,
|
||||||
|
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||||
|
where such license applies only to those patent claims licensable
|
||||||
|
by such Contributor that are necessarily infringed by their
|
||||||
|
Contribution(s) alone or by combination of their Contribution(s)
|
||||||
|
with the Work to which such Contribution(s) was submitted. If You
|
||||||
|
institute patent litigation against any entity (including a
|
||||||
|
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||||
|
or a Contribution incorporated within the Work constitutes direct
|
||||||
|
or contributory patent infringement, then any patent licenses
|
||||||
|
granted to You under this License for that Work shall terminate
|
||||||
|
as of the date such litigation is filed.
|
||||||
|
|
||||||
|
4. Redistribution. You may reproduce and distribute copies of the
|
||||||
|
Work or Derivative Works thereof in any medium, with or without
|
||||||
|
modifications, and in Source or Object form, provided that You
|
||||||
|
meet the following conditions:
|
||||||
|
|
||||||
|
(a) You must give any other recipients of the Work or
|
||||||
|
Derivative Works a copy of this License; and
|
||||||
|
|
||||||
|
(b) You must cause any modified files to carry prominent notices
|
||||||
|
stating that You changed the files; and
|
||||||
|
|
||||||
|
(c) You must retain, in the Source form of any Derivative Works
|
||||||
|
that You distribute, all copyright, patent, trademark, and
|
||||||
|
attribution notices from the Source form of the Work,
|
||||||
|
excluding those notices that do not pertain to any part of
|
||||||
|
the Derivative Works; and
|
||||||
|
|
||||||
|
(d) If the Work includes a "NOTICE" text file as part of its
|
||||||
|
distribution, then any Derivative Works that You distribute must
|
||||||
|
include a readable copy of the attribution notices contained
|
||||||
|
within such NOTICE file, excluding those notices that do not
|
||||||
|
pertain to any part of the Derivative Works, in at least one
|
||||||
|
of the following places: within a NOTICE text file distributed
|
||||||
|
as part of the Derivative Works; within the Source form or
|
||||||
|
documentation, if provided along with the Derivative Works; or,
|
||||||
|
within a display generated by the Derivative Works, if and
|
||||||
|
wherever such third-party notices normally appear. The contents
|
||||||
|
of the NOTICE file are for informational purposes only and
|
||||||
|
do not modify the License. You may add Your own attribution
|
||||||
|
notices within Derivative Works that You distribute, alongside
|
||||||
|
or as an addendum to the NOTICE text from the Work, provided
|
||||||
|
that such additional attribution notices cannot be construed
|
||||||
|
as modifying the License.
|
||||||
|
|
||||||
|
You may add Your own copyright statement to Your modifications and
|
||||||
|
may provide additional or different license terms and conditions
|
||||||
|
for use, reproduction, or distribution of Your modifications, or
|
||||||
|
for any such Derivative Works as a whole, provided Your use,
|
||||||
|
reproduction, and distribution of the Work otherwise complies with
|
||||||
|
the conditions stated in this License.
|
||||||
|
|
||||||
|
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||||
|
any Contribution intentionally submitted for inclusion in the Work
|
||||||
|
by You to the Licensor shall be under the terms and conditions of
|
||||||
|
this License, without any additional terms or conditions.
|
||||||
|
Notwithstanding the above, nothing herein shall supersede or modify
|
||||||
|
the terms of any separate license agreement you may have executed
|
||||||
|
with Licensor regarding such Contributions.
|
||||||
|
|
||||||
|
6. Trademarks. This License does not grant permission to use the trade
|
||||||
|
names, trademarks, service marks, or product names of the Licensor,
|
||||||
|
except as required for reasonable and customary use in describing the
|
||||||
|
origin of the Work and reproducing the content of the NOTICE file.
|
||||||
|
|
||||||
|
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||||
|
agreed to in writing, Licensor provides the Work (and each
|
||||||
|
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||||
|
implied, including, without limitation, any warranties or conditions
|
||||||
|
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||||
|
appropriateness of using or redistributing the Work and assume any
|
||||||
|
risks associated with Your exercise of permissions under this License.
|
||||||
|
|
||||||
|
8. Limitation of Liability. In no event and under no legal theory,
|
||||||
|
whether in tort (including negligence), contract, or otherwise,
|
||||||
|
unless required by applicable law (such as deliberate and grossly
|
||||||
|
negligent acts) or agreed to in writing, shall any Contributor be
|
||||||
|
liable to You for damages, including any direct, indirect, special,
|
||||||
|
incidental, or consequential damages of any character arising as a
|
||||||
|
result of this License or out of the use or inability to use the
|
||||||
|
Work (including but not limited to damages for loss of goodwill,
|
||||||
|
work stoppage, computer failure or malfunction, or any and all
|
||||||
|
other commercial damages or losses), even if such Contributor
|
||||||
|
has been advised of the possibility of such damages.
|
||||||
|
|
||||||
|
9. Accepting Warranty or Additional Liability. While redistributing
|
||||||
|
the Work or Derivative Works thereof, You may choose to offer,
|
||||||
|
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||||
|
or other liability obligations and/or rights consistent with this
|
||||||
|
License. However, in accepting such obligations, You may act only
|
||||||
|
on Your own behalf and on Your sole responsibility, not on behalf
|
||||||
|
of any other Contributor, and only if You agree to indemnify,
|
||||||
|
defend, and hold each Contributor harmless for any liability
|
||||||
|
incurred by, or claims asserted against, such Contributor by reason
|
||||||
|
of your accepting any such warranty or additional liability.
|
||||||
|
|
||||||
|
END OF TERMS AND CONDITIONS
|
||||||
|
|
||||||
|
APPENDIX: How to apply the Apache License to your work.
|
||||||
|
|
||||||
|
To apply the Apache License to your work, attach the following
|
||||||
|
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||||
|
replaced with your own identifying information. (Don't include
|
||||||
|
the brackets!) The text should be enclosed in the appropriate
|
||||||
|
comment syntax for the file format. We also recommend that a
|
||||||
|
file or class name and description of purpose be included on the
|
||||||
|
same "printed page" as the copyright notice for easier
|
||||||
|
identification within third-party archives.
|
||||||
|
|
||||||
|
Copyright [yyyy] [name of copyright owner]
|
||||||
|
|
||||||
|
Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
you may not use this file except in compliance with the License.
|
||||||
|
You may obtain a copy of the License at
|
||||||
|
|
||||||
|
http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
|
||||||
|
Unless required by applicable law or agreed to in writing, software
|
||||||
|
distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
See the License for the specific language governing permissions and
|
||||||
|
limitations under the License.
|
||||||
Submodule ext/BehaviorTreeRos2 deleted from 7a6ace6424
@ -1,6 +0,0 @@
|
|||||||
# Magrob firmware
|
|
||||||
|
|
||||||
This directory contains the firmware used for our robot.
|
|
||||||
|
|
||||||
### Base
|
|
||||||
This code was meant to be used for the wheel base
|
|
||||||
@ -1,50 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.12)
|
|
||||||
|
|
||||||
set(PICO_BOARD pico CACHE STRING "Board type")
|
|
||||||
set(TOP ${PICO_TINYUSB_PATH})
|
|
||||||
include(pico_sdk_import.cmake)
|
|
||||||
|
|
||||||
set(CMAKE_C_STANDARD 11)
|
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
|
||||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
|
||||||
|
|
||||||
project(wheelbase C CXX ASM)
|
|
||||||
|
|
||||||
pico_sdk_init()
|
|
||||||
|
|
||||||
|
|
||||||
add_executable(wheelbase
|
|
||||||
src/quadrature.c
|
|
||||||
src/main.c
|
|
||||||
src/stepper.c
|
|
||||||
tusb/tusb_descriptors.c
|
|
||||||
)
|
|
||||||
|
|
||||||
pico_set_program_name(wheelbase "wheelbase")
|
|
||||||
pico_set_program_version(wheelbase "0.1")
|
|
||||||
|
|
||||||
pico_enable_stdio_uart(wheelbase 0)
|
|
||||||
pico_enable_stdio_usb(wheelbase 1)
|
|
||||||
|
|
||||||
pico_generate_pio_header(wheelbase ${CMAKE_CURRENT_LIST_DIR}/pio/quadrature.pio)
|
|
||||||
pico_generate_pio_header(wheelbase ${CMAKE_CURRENT_LIST_DIR}/pio/blink.pio)
|
|
||||||
|
|
||||||
target_link_libraries(wheelbase
|
|
||||||
pico_stdlib
|
|
||||||
pico_stdio
|
|
||||||
pico_time
|
|
||||||
pico_multicore
|
|
||||||
hardware_pio
|
|
||||||
hardware_clocks
|
|
||||||
hardware_gpio
|
|
||||||
hardware_sync
|
|
||||||
tinyusb_board
|
|
||||||
tinyusb_device)
|
|
||||||
|
|
||||||
target_include_directories(wheelbase PRIVATE
|
|
||||||
${CMAKE_CURRENT_LIST_DIR}
|
|
||||||
src/
|
|
||||||
tusb/
|
|
||||||
)
|
|
||||||
|
|
||||||
pico_add_extra_outputs(wheelbase)
|
|
||||||
@ -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,31 +0,0 @@
|
|||||||
.program oscillate
|
|
||||||
pause:
|
|
||||||
pull block
|
|
||||||
out x,32
|
|
||||||
.wrap_target
|
|
||||||
pull noblock
|
|
||||||
out x, 32
|
|
||||||
mov y, x
|
|
||||||
set pins, 1 ; Pin ON
|
|
||||||
jmp y-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number
|
|
||||||
jmp pause
|
|
||||||
lp1:
|
|
||||||
jmp y-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number
|
|
||||||
mov y, x
|
|
||||||
set pins, 0 [2] ; Pin OFF
|
|
||||||
lp2:
|
|
||||||
jmp y-- lp2 ; Delay for the same number of cycles again
|
|
||||||
.wrap ; Blink forever!
|
|
||||||
|
|
||||||
|
|
||||||
% c-sdk {
|
|
||||||
// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin
|
|
||||||
|
|
||||||
void oscillate_program_init(PIO pio, uint sm, uint offset, uint pin) {
|
|
||||||
pio_gpio_init(pio, pin);
|
|
||||||
pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
|
|
||||||
pio_sm_config c = oscillate_program_get_default_config(offset);
|
|
||||||
sm_config_set_set_pins(&c, pin, 1);
|
|
||||||
pio_sm_init(pio, sm, offset, &c);
|
|
||||||
}
|
|
||||||
%}
|
|
||||||
@ -1,208 +0,0 @@
|
|||||||
;
|
|
||||||
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
|
|
||||||
;
|
|
||||||
; SPDX-License-Identifier: BSD-3-Clause
|
|
||||||
;
|
|
||||||
; quadrature_encoder_substep: reads a quadrature encoder with no CPU
|
|
||||||
; intervention and provides the current position on request.
|
|
||||||
;
|
|
||||||
; the "substep" version uses not only the step counts, but also the timing of
|
|
||||||
; the steps to compute the current speed. See README.md for details
|
|
||||||
|
|
||||||
|
|
||||||
.program quadrature_encoder_substep
|
|
||||||
|
|
||||||
.origin 0
|
|
||||||
|
|
||||||
; the PIO code counts steps like the standard quadrature encoder code, but also
|
|
||||||
; keeps track of the time passed since the last transition. That allows the C
|
|
||||||
; code to build a good estimate of a fractional step position based on the
|
|
||||||
; latest speed and time passed
|
|
||||||
;
|
|
||||||
; since it needs to push two values, it only pushes new data when the FIFO has
|
|
||||||
; enough space to hold both values. Otherwise it could either stall or go out
|
|
||||||
; of sync
|
|
||||||
;
|
|
||||||
; because we need to count the time passed, all loops must take the same number
|
|
||||||
; of cycles and there are delays added to the fastest branches to make sure it
|
|
||||||
; always takes 13 cycles per loop (e.g., sysclk 133MHz, max step rate = ~10.2
|
|
||||||
; Msteps/sec)
|
|
||||||
|
|
||||||
; push the step count and transition clock count to the RX FIFO (using
|
|
||||||
; auto push). This is reached by the "MOV PC, ~STATUS" instruction when
|
|
||||||
; status is all 1 (meaning fifo has space for this push). It also may
|
|
||||||
; execute once at program start, but that has little effect
|
|
||||||
IN X, 32
|
|
||||||
IN Y, 32
|
|
||||||
|
|
||||||
update_state:
|
|
||||||
; build the state by using 2 bits from the negated previous state of the
|
|
||||||
; pins and the new 2 bit state of the pins
|
|
||||||
OUT ISR, 2
|
|
||||||
IN PINS, 2
|
|
||||||
MOV OSR, ~ISR
|
|
||||||
; use the jump table to update the step count accordingly
|
|
||||||
MOV PC, OSR
|
|
||||||
|
|
||||||
decrement:
|
|
||||||
; decrement the step count
|
|
||||||
JMP Y--, decrement_cont
|
|
||||||
decrement_cont:
|
|
||||||
; when decrementing, X is set to 2^31, when incrementing it is set to
|
|
||||||
; zero. That way the C code can infer in which direction the last
|
|
||||||
; transition was taken and how long ago
|
|
||||||
SET X, 1
|
|
||||||
MOV X, ::X
|
|
||||||
; after incrementing or decrementing, continue to "check_fifo"
|
|
||||||
check_fifo:
|
|
||||||
.wrap_target
|
|
||||||
; on each iteration we decrement X to count the number of loops since
|
|
||||||
; the last transition
|
|
||||||
JMP X--, check_fifo_cont
|
|
||||||
check_fifo_cont:
|
|
||||||
; push data or continue, depending on the state of the fifo
|
|
||||||
MOV PC, ~STATUS
|
|
||||||
|
|
||||||
increment:
|
|
||||||
; the PIO does not have a increment instruction, so to do that we do a
|
|
||||||
; negate, decrement, negate sequence
|
|
||||||
MOV Y, ~Y
|
|
||||||
JMP Y--, increment_cont
|
|
||||||
increment_cont:
|
|
||||||
MOV Y, ~Y
|
|
||||||
; reset X to zero when incrementing
|
|
||||||
SET X, 0
|
|
||||||
; wrap above to check the fifo state
|
|
||||||
.wrap
|
|
||||||
|
|
||||||
invalid:
|
|
||||||
; this is just a placeholder to document what the code does on invalid
|
|
||||||
; transitions, where the two phases change at the same time. We don't do
|
|
||||||
; anything special, but the encoder should note generate these invalid
|
|
||||||
; transitions anyway
|
|
||||||
JMP update_state
|
|
||||||
|
|
||||||
; this jump table starts at address 16 and is accessed by the
|
|
||||||
; "MOV PC, OSR" instruction above, that loads the PC with the state on
|
|
||||||
; the lower 4 bits and the 5th bit on. The delays here extend the faster
|
|
||||||
; branches to take the same time as the slower branches
|
|
||||||
JMP invalid
|
|
||||||
JMP increment [0]
|
|
||||||
JMP decrement [1]
|
|
||||||
JMP check_fifo [4]
|
|
||||||
|
|
||||||
JMP decrement [1]
|
|
||||||
JMP invalid
|
|
||||||
JMP check_fifo [4]
|
|
||||||
JMP increment [0]
|
|
||||||
|
|
||||||
JMP increment [0]
|
|
||||||
JMP check_fifo [4]
|
|
||||||
JMP invalid
|
|
||||||
JMP decrement [1]
|
|
||||||
|
|
||||||
JMP check_fifo [4]
|
|
||||||
JMP decrement [1]
|
|
||||||
JMP increment [0]
|
|
||||||
; this instruction should be usually reached by the "MOV PC, ~STATUS"
|
|
||||||
; instruction above when the status is zero, which means that the fifo
|
|
||||||
; has data and we don't want to push more data. This can also be reached
|
|
||||||
; on an invalid state transition, which should not happen. Even if it
|
|
||||||
; happens, it should be a transient state and the only side effect is
|
|
||||||
; that we'll call update_state twice in a row
|
|
||||||
JMP update_state [1]
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
% c-sdk {
|
|
||||||
|
|
||||||
#include "hardware/clocks.h"
|
|
||||||
#include "hardware/timer.h"
|
|
||||||
#include "hardware/gpio.h"
|
|
||||||
#include "hardware/sync.h"
|
|
||||||
|
|
||||||
|
|
||||||
// "substep" version low-level interface
|
|
||||||
//
|
|
||||||
// note: user code should use the high level functions in quadrature_encoder.c
|
|
||||||
// and not call these directly
|
|
||||||
|
|
||||||
// initialize the PIO state and the substep_state_t structure that keeps track
|
|
||||||
// of the encoder state
|
|
||||||
static inline void quadrature_encoder_substep_program_init(PIO pio, uint sm, uint pin_A)
|
|
||||||
{
|
|
||||||
uint pin_state, position, ints;
|
|
||||||
pio_gpio_init(pio, pin_A);
|
|
||||||
pio_gpio_init(pio, pin_A + 1);
|
|
||||||
|
|
||||||
pio_sm_set_consecutive_pindirs(pio, sm, pin_A, 2, false);
|
|
||||||
gpio_pull_up(pin_A);
|
|
||||||
gpio_pull_up(pin_A + 1);
|
|
||||||
|
|
||||||
pio_sm_config c = quadrature_encoder_substep_program_get_default_config(0);
|
|
||||||
sm_config_set_in_pins(&c, pin_A); // for WAIT, IN
|
|
||||||
// shift to left, auto-push at 32 bits
|
|
||||||
sm_config_set_in_shift(&c, false, true, 32);
|
|
||||||
sm_config_set_out_shift(&c, true, false, 32);
|
|
||||||
// don't join FIFO's
|
|
||||||
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
|
|
||||||
|
|
||||||
// always run at sysclk, to have the maximum possible time resolution
|
|
||||||
sm_config_set_clkdiv(&c, 1.0);
|
|
||||||
|
|
||||||
pio_sm_init(pio, sm, 0, &c);
|
|
||||||
|
|
||||||
// set up status to be rx_fifo < 1
|
|
||||||
pio->sm[sm].execctrl = ((pio->sm[sm].execctrl & 0xFFFFFF80) | 0x12);
|
|
||||||
|
|
||||||
// init the state machine according to the current phase. Since we are
|
|
||||||
// setting the state running PIO instructions from C state, the encoder may
|
|
||||||
// step during this initialization. This should not be a problem though,
|
|
||||||
// because as long as it is just one step, the state machine will update
|
|
||||||
// correctly when it starts. We disable interrupts anyway, to be safe
|
|
||||||
ints = save_and_disable_interrupts();
|
|
||||||
|
|
||||||
pin_state = (gpio_get_all() >> pin_A) & 3;
|
|
||||||
|
|
||||||
// to setup the state machine, we need to set the lower 2 bits of OSR to be
|
|
||||||
// the negated pin state
|
|
||||||
pio_sm_exec(pio, sm, pio_encode_set(pio_y, ~pin_state));
|
|
||||||
pio_sm_exec(pio, sm, pio_encode_mov(pio_osr, pio_y));
|
|
||||||
|
|
||||||
// also set the Y (current step) so that the lower 2 bits of Y have a 1:1
|
|
||||||
// mapping to the current phase (input pin state). That simplifies the code
|
|
||||||
// to compensate for differences in encoder phase sizes:
|
|
||||||
switch (pin_state) {
|
|
||||||
case 0: position = 0; break;
|
|
||||||
case 1: position = 3; break;
|
|
||||||
case 2: position = 1; break;
|
|
||||||
case 3: position = 2; break;
|
|
||||||
}
|
|
||||||
pio_sm_exec(pio, sm, pio_encode_set(pio_y, position));
|
|
||||||
|
|
||||||
pio_sm_set_enabled(pio, sm, true);
|
|
||||||
|
|
||||||
restore_interrupts(ints);
|
|
||||||
}
|
|
||||||
|
|
||||||
static inline void quadrature_encoder_substep_get_counts(PIO pio, uint sm, uint *step, int *cycles, uint *us)
|
|
||||||
{
|
|
||||||
int i, pairs;
|
|
||||||
uint ints;
|
|
||||||
|
|
||||||
pairs = pio_sm_get_rx_fifo_level(pio, sm) >> 1;
|
|
||||||
|
|
||||||
// read all data with interrupts disabled, so that there can not be a
|
|
||||||
// big time gap between reading the PIO data and the current us
|
|
||||||
ints = save_and_disable_interrupts();
|
|
||||||
for (i = 0; i < pairs + 1; i++) {
|
|
||||||
*cycles = pio_sm_get_blocking(pio, sm);
|
|
||||||
*step = pio_sm_get_blocking(pio, sm);
|
|
||||||
}
|
|
||||||
*us = time_us_32();
|
|
||||||
restore_interrupts(ints);
|
|
||||||
}
|
|
||||||
|
|
||||||
%}
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
//############## CONFIG STEPPERS ################
|
|
||||||
#define DIR_PIN_A 26
|
|
||||||
#define DIR_PIN_B 16
|
|
||||||
#define PULSE_PIN_A 27
|
|
||||||
#define PULSE_PIN_B 17
|
|
||||||
|
|
||||||
#define SM_A 0
|
|
||||||
#define SM_B 1
|
|
||||||
|
|
||||||
#define PULSE_PER_REV (1 / 3200.0)
|
|
||||||
//###############################################
|
|
||||||
|
|
||||||
//================ CONFIG ENCODERS =====================
|
|
||||||
#define ENCODER_RIGHT_PIN_A 12 // Lupio sam ove vrednosti
|
|
||||||
#define ENCODER_RIGHT_PIN_B 13
|
|
||||||
#define ENCODER_LEFT_PIN_A 6
|
|
||||||
#define ENCODER_LEFT_PIN_B 7
|
|
||||||
#define ENCODER_CPR 3840
|
|
||||||
|
|
||||||
#define WHEEL_RADIUS 0.0279
|
|
||||||
#define WHEEL_SEPARATION 0.310735
|
|
||||||
#define TIMER_CYCLE_US 1000
|
|
||||||
//======================================================
|
|
||||||
@ -1,288 +0,0 @@
|
|||||||
#include <stdio.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
#include "bsp/board_api.h"
|
|
||||||
#include "tusb.h"
|
|
||||||
|
|
||||||
|
|
||||||
#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
|
|
||||||
#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "pico/stdlib.h"
|
|
||||||
#include "pico/multicore.h"
|
|
||||||
#include "pico/time.h"
|
|
||||||
#include "quadrature.h"
|
|
||||||
#include "quadrature.pio.h"
|
|
||||||
|
|
||||||
#include "config.h"
|
|
||||||
#include "stepper.h"
|
|
||||||
|
|
||||||
static substep_state_t state_l;
|
|
||||||
static substep_state_t state_r;
|
|
||||||
|
|
||||||
static double base_x = 0;
|
|
||||||
static double base_y = 0;
|
|
||||||
static double base_theta = 0;
|
|
||||||
static double wheel_separation = WHEEL_SEPARATION;
|
|
||||||
static double wheel_ratio_l = 1;
|
|
||||||
static double wheel_ratio_r = 1;
|
|
||||||
|
|
||||||
static uint prev_time;
|
|
||||||
static int prev_position_l = 0;
|
|
||||||
static int prev_position_r = 0;
|
|
||||||
|
|
||||||
|
|
||||||
static unsigned char stepper_instr[2+sizeof(double)*2] = {};
|
|
||||||
|
|
||||||
typedef struct instr_buf_t{
|
|
||||||
char pos;
|
|
||||||
char data[2+sizeof(double) * 2];
|
|
||||||
} instr_buf_t;
|
|
||||||
|
|
||||||
instr_buf_t stepper_buf = {};
|
|
||||||
|
|
||||||
typedef struct calib_diff_t {
|
|
||||||
long long left_pulse;
|
|
||||||
long long right_pulse;
|
|
||||||
double left_gain;
|
|
||||||
double right_gain;
|
|
||||||
} calib_diff_t;
|
|
||||||
|
|
||||||
static calib_diff_t calib_enc = {
|
|
||||||
.left_gain = 0.997648,
|
|
||||||
.right_gain = 1.002333
|
|
||||||
};
|
|
||||||
|
|
||||||
bool update_pos_cb() {
|
|
||||||
|
|
||||||
substep_update(&state_l);
|
|
||||||
substep_update(&state_r);
|
|
||||||
|
|
||||||
int position_l= state_l.position;
|
|
||||||
int position_r= state_r.position;
|
|
||||||
|
|
||||||
const int diff_l = position_l - prev_position_l;
|
|
||||||
const int diff_r = position_r - prev_position_r;
|
|
||||||
|
|
||||||
calib_enc.left_pulse += diff_l;
|
|
||||||
calib_enc.right_pulse -= diff_r;
|
|
||||||
|
|
||||||
double vel_l = diff_l;
|
|
||||||
double vel_r = diff_r;
|
|
||||||
|
|
||||||
prev_position_l = state_l.position;
|
|
||||||
prev_position_r = state_r.position;
|
|
||||||
|
|
||||||
vel_l /=64 * ENCODER_CPR;
|
|
||||||
vel_r /=64 * ENCODER_CPR;
|
|
||||||
vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
|
|
||||||
vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
|
|
||||||
|
|
||||||
const double linear = (vel_l + vel_r) / 2;
|
|
||||||
const double angular = (vel_r - vel_l) / wheel_separation;
|
|
||||||
const double r = linear / angular;
|
|
||||||
|
|
||||||
if(fabs(r) > 100) {
|
|
||||||
const double dir = base_theta + angular * 0.5;
|
|
||||||
|
|
||||||
base_x += linear * cos(dir);
|
|
||||||
base_y += linear * sin(dir);
|
|
||||||
base_theta += angular;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
const double base_theta_old = base_theta;
|
|
||||||
base_theta += angular;
|
|
||||||
base_x += r * (sin(base_theta) - sin(base_theta_old));
|
|
||||||
base_y += -r * (cos(base_theta) - cos(base_theta_old));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void zero() {
|
|
||||||
base_x = 0;
|
|
||||||
base_y = 0;
|
|
||||||
base_theta = 0;
|
|
||||||
calib_enc.left_pulse = 0;
|
|
||||||
calib_enc.right_pulse = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void start_calib() {
|
|
||||||
calib_enc.left_pulse = 0;
|
|
||||||
calib_enc.right_pulse = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void end_calib() {
|
|
||||||
const double l_adjust = (double)calib_enc.left_pulse * calib_enc.left_gain;
|
|
||||||
const double r_adjust = (double)calib_enc.right_pulse * calib_enc.right_gain;
|
|
||||||
const double delta_angle = l_adjust - r_adjust;
|
|
||||||
const double distance = 0.5 * (l_adjust + r_adjust);
|
|
||||||
const double factor = delta_angle / distance * 0.7;
|
|
||||||
calib_enc.left_gain = (1.0 - factor) * calib_enc.left_gain;
|
|
||||||
calib_enc.right_gain = (1.0 + factor) * calib_enc.right_gain;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void core2_entry()
|
|
||||||
{
|
|
||||||
|
|
||||||
pio_add_program(pio1, &quadrature_encoder_substep_program);
|
|
||||||
substep_init_state(pio1, 0, ENCODER_LEFT_PIN_A , &state_l);
|
|
||||||
substep_init_state(pio1, 1, ENCODER_RIGHT_PIN_A, &state_r);
|
|
||||||
|
|
||||||
// The sets the positions to initial values
|
|
||||||
substep_update(&state_l);
|
|
||||||
substep_update(&state_r);
|
|
||||||
prev_position_l = state_l.position;
|
|
||||||
prev_position_r = state_r.position;
|
|
||||||
|
|
||||||
prev_time = time_us_32();
|
|
||||||
|
|
||||||
// alarm_pool_t *ap = alarm_pool_create_with_unused_hardware_alarm(2);
|
|
||||||
// repeating_timer_t rt;
|
|
||||||
// alarm_pool_add_repeating_timer_us(ap, TIMER_CYCLE_US, update_pos_cb, NULL, &rt);
|
|
||||||
|
|
||||||
uint16_t cmd = 0;
|
|
||||||
uint64_t data = 0;
|
|
||||||
uint readNum = 0;
|
|
||||||
char type = 'w';
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
uint ch;
|
|
||||||
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
|
|
||||||
cmd = (cmd << 8) | ch;
|
|
||||||
|
|
||||||
if(readNum > 0) {
|
|
||||||
data = (data >> 8ll) | ((uint64_t)ch<<56ll);
|
|
||||||
readNum--;
|
|
||||||
if(!readNum) {
|
|
||||||
if(type == 'w'){
|
|
||||||
wheel_separation = *((double*)&data);
|
|
||||||
} else if(type == 'X'){
|
|
||||||
base_x = *((double*)&data);
|
|
||||||
} else if(type == 'Y'){
|
|
||||||
base_y = *((double*)&data);
|
|
||||||
} else if(type == 'T'){
|
|
||||||
base_theta = *((double*)&data);
|
|
||||||
calib_enc.left_pulse = 0;
|
|
||||||
calib_enc.right_pulse = 0;
|
|
||||||
} else{
|
|
||||||
wheel_ratio_l = 1 + (*((double *)&data));
|
|
||||||
wheel_ratio_r = 1 - (*((double *)&data));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(cmd == (((uint16_t)'g' << 8) | ';')) {
|
|
||||||
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
|
|
||||||
cmd = 0;
|
|
||||||
} else if(cmd == (((uint16_t)'w' << 8) | ';')) {
|
|
||||||
readNum = 8;
|
|
||||||
type = (cmd >> 8) & 0xFF;
|
|
||||||
} else if(cmd == (((uint16_t)'r' << 8) | ';')){
|
|
||||||
readNum = 8;
|
|
||||||
type = (cmd >> 8) & 0xFF;
|
|
||||||
} else if(cmd == (((uint16_t)'X' << 8) | ';')){
|
|
||||||
readNum = 8;
|
|
||||||
type = (cmd >> 8) & 0xFF;
|
|
||||||
} else if(cmd == (((uint16_t)'Y' << 8) | ';')){
|
|
||||||
readNum = 8;
|
|
||||||
type = (cmd >> 8) & 0xFF;
|
|
||||||
} else if(cmd == (((uint16_t)'T' << 8) | ';')){
|
|
||||||
readNum = 8;
|
|
||||||
type = (cmd >> 8) & 0xFF;
|
|
||||||
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
|
|
||||||
zero();
|
|
||||||
} else if(cmd == (((uint16_t)'c' << 8) | ';')) {
|
|
||||||
end_calib();
|
|
||||||
printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
update_pos_cb();
|
|
||||||
sleep_ms(1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
double btod(unsigned char * input) {
|
|
||||||
uint64_t data = 0;
|
|
||||||
for (int i = 0; i<8; i++){
|
|
||||||
uint a = *(input + i);
|
|
||||||
data >>= 8;
|
|
||||||
data |= (uint64_t)a<<56;
|
|
||||||
}
|
|
||||||
const double ret = *((double*)&data);
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
void stepper_fifo(char c, uint8_t itf) {
|
|
||||||
char instrh = stepper_buf.data[(stepper_buf.pos + 1) % sizeof(stepper_buf.data)];
|
|
||||||
char instrl = stepper_buf.data[(stepper_buf.pos + 2) % sizeof(stepper_buf.data)];
|
|
||||||
stepper_buf.data[stepper_buf.pos] = c;
|
|
||||||
stepper_buf.pos = (stepper_buf.pos + 1) % sizeof(stepper_buf.data);
|
|
||||||
|
|
||||||
if(instrh == 's' && instrl == ';') {
|
|
||||||
memcpy(stepper_instr,stepper_buf.data + stepper_buf.pos, sizeof(stepper_buf.data) - stepper_buf.pos);
|
|
||||||
memcpy(stepper_instr - stepper_buf.pos + sizeof(stepper_buf.data),stepper_buf.data, stepper_buf.pos);
|
|
||||||
|
|
||||||
memset(&stepper_buf, 0, sizeof(stepper_buf));
|
|
||||||
double vl = btod(stepper_instr + 2);
|
|
||||||
double vr = btod(stepper_instr + 10);
|
|
||||||
|
|
||||||
set_speeds(vl, vr);
|
|
||||||
|
|
||||||
// tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
|
|
||||||
// tud_cdc_n_write_flush(itf);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void tud_cdc_rx_cb(uint8_t itf) {
|
|
||||||
// read the available data
|
|
||||||
// | IMPORTANT: also do this for CDC0 because otherwise
|
|
||||||
// | you won't be able to print anymore to CDC0
|
|
||||||
// | next time this function is called
|
|
||||||
if (itf == 1) {
|
|
||||||
int c;
|
|
||||||
while((c = tud_cdc_n_read_char(itf)) > -1) {
|
|
||||||
stepper_fifo(c, itf);
|
|
||||||
}
|
|
||||||
// tud_cdc_n_write(itf, (uint8_t const *) "OK\r\n", 4);
|
|
||||||
// tud_cdc_n_write_flush(itf);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void run_init()
|
|
||||||
{
|
|
||||||
board_init();
|
|
||||||
tusb_init();
|
|
||||||
tud_init(BOARD_TUD_RHPORT);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (board_init_after_tusb) {
|
|
||||||
board_init_after_tusb();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!stdio_usb_init()) {
|
|
||||||
board_led_write(1);
|
|
||||||
}
|
|
||||||
stepper_init();
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main()
|
|
||||||
{
|
|
||||||
run_init();
|
|
||||||
|
|
||||||
multicore_launch_core1(core2_entry);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
tud_task();
|
|
||||||
sleep_ms(1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1,184 +0,0 @@
|
|||||||
/**
|
|
||||||
* Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
|
|
||||||
* Modified by Pimpest in 2024
|
|
||||||
*
|
|
||||||
* SPDX-License-Identifier: BSD-3-Clause
|
|
||||||
*
|
|
||||||
* This software has been modified from its original version
|
|
||||||
*/
|
|
||||||
#include "quadrature.pio.h"
|
|
||||||
#include "quadrature.h"
|
|
||||||
#include "memory.h"
|
|
||||||
|
|
||||||
static void read_pio_data(substep_state_t *state, uint *step, uint *step_us, uint *transition_us, int *forward)
|
|
||||||
{
|
|
||||||
int cycles;
|
|
||||||
|
|
||||||
// get the raw data from the PIO state machine
|
|
||||||
quadrature_encoder_substep_get_counts(state->pio, state->sm, step, &cycles, step_us);
|
|
||||||
|
|
||||||
// when the PIO program detects a transition, it sets cycles to either zero
|
|
||||||
// (when step is incrementing) or 2^31 (when step is decrementing) and keeps
|
|
||||||
// decrementing it on each 13 clock loop. We can use this information to get
|
|
||||||
// the time and direction of the last transition
|
|
||||||
if (cycles < 0) {
|
|
||||||
cycles = -cycles;
|
|
||||||
*forward = 1;
|
|
||||||
} else {
|
|
||||||
cycles = 0x80000000 - cycles;
|
|
||||||
*forward = 0;
|
|
||||||
}
|
|
||||||
*transition_us = *step_us - ((cycles * 13) / state->clocks_per_us);
|
|
||||||
}
|
|
||||||
|
|
||||||
// get the sub-step position of the start of a step
|
|
||||||
static uint get_step_start_transition_pos(substep_state_t *state, uint step)
|
|
||||||
{
|
|
||||||
return ((step << 6) & 0xFFFFFF00) | state->calibration_data[step & 3];
|
|
||||||
}
|
|
||||||
|
|
||||||
// compute speed in "sub-steps per 2^20 us" from a delta substep position and
|
|
||||||
// delta time in microseconds. This unit is cheaper to compute and use, so we
|
|
||||||
// only convert to "sub-steps per second" once per update, at most
|
|
||||||
static int substep_calc_speed(int delta_substep, int delta_us)
|
|
||||||
{
|
|
||||||
return ((int64_t) delta_substep << 20) / delta_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// main functions to be used by user code
|
|
||||||
|
|
||||||
// initialize the substep state structure and start PIO code
|
|
||||||
void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state)
|
|
||||||
{
|
|
||||||
int forward;
|
|
||||||
|
|
||||||
// set all fields to zero by default
|
|
||||||
memset(state, 0, sizeof(substep_state_t));
|
|
||||||
|
|
||||||
// initialize the PIO program (and save the PIO reference)
|
|
||||||
state->pio = pio;
|
|
||||||
state->sm = sm;
|
|
||||||
quadrature_encoder_substep_program_init(pio, sm, pin_a);
|
|
||||||
|
|
||||||
// start with equal phase size calibration
|
|
||||||
state->calibration_data[0] = 0;
|
|
||||||
state->calibration_data[1] = 64;
|
|
||||||
state->calibration_data[2] = 128;
|
|
||||||
state->calibration_data[3] = 192;
|
|
||||||
|
|
||||||
state->idle_stop_samples = 3;
|
|
||||||
|
|
||||||
// start "stopped" so that we don't use stale data to compute speeds
|
|
||||||
state->stopped = 1;
|
|
||||||
|
|
||||||
// cache the PIO cycles per us
|
|
||||||
state->clocks_per_us = (clock_get_hz(clk_sys) + 500000) / 1000000;
|
|
||||||
|
|
||||||
// initialize the "previous state"
|
|
||||||
read_pio_data(state, &state->raw_step, &state->prev_step_us, &state->prev_trans_us, &forward);
|
|
||||||
|
|
||||||
state->position = get_step_start_transition_pos(state, state->raw_step) + 32;
|
|
||||||
}
|
|
||||||
|
|
||||||
// read the PIO data and update the speed / position estimate
|
|
||||||
void substep_update(substep_state_t *state)
|
|
||||||
{
|
|
||||||
uint step, step_us, transition_us, transition_pos, low, high;
|
|
||||||
int forward, speed_high, speed_low;
|
|
||||||
|
|
||||||
// read the current encoder state from the PIO
|
|
||||||
read_pio_data(state, &step, &step_us, &transition_us, &forward);
|
|
||||||
|
|
||||||
// from the current step we can get the low and high boundaries in substeps
|
|
||||||
// of the current position
|
|
||||||
low = get_step_start_transition_pos(state, step);
|
|
||||||
high = get_step_start_transition_pos(state, step + 1);
|
|
||||||
|
|
||||||
// if we were not stopped, but the last transition was more than
|
|
||||||
// "idle_stop_samples" ago, we are stopped now
|
|
||||||
if (step == state->raw_step)
|
|
||||||
state->idle_stop_sample_count++;
|
|
||||||
else
|
|
||||||
state->idle_stop_sample_count = 0;
|
|
||||||
|
|
||||||
if (!state->stopped && state->idle_stop_sample_count >= state->idle_stop_samples) {
|
|
||||||
state->speed = 0;
|
|
||||||
state->speed_2_20 = 0;
|
|
||||||
state->stopped = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
// when we are at a different step now, there is certainly a transition
|
|
||||||
if (state->raw_step != step) {
|
|
||||||
// the transition position depends on the direction of the move
|
|
||||||
transition_pos = forward ? low : high;
|
|
||||||
|
|
||||||
// if we are not stopped, that means there is valid previous transition
|
|
||||||
// we can use to estimate the current speed
|
|
||||||
if (!state->stopped)
|
|
||||||
state->speed_2_20 = substep_calc_speed(transition_pos - state->prev_trans_pos, transition_us - state->prev_trans_us);
|
|
||||||
|
|
||||||
// if we have a transition, we are not stopped now
|
|
||||||
state->stopped = 0;
|
|
||||||
// save the timestamp and position of this transition to use later to
|
|
||||||
// estimate speed
|
|
||||||
state->prev_trans_pos = transition_pos;
|
|
||||||
state->prev_trans_us = transition_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
// if we are stopped, speed is zero and the position estimate remains
|
|
||||||
// constant. If we are not stopped, we have to update the position and speed
|
|
||||||
if (!state->stopped) {
|
|
||||||
// although the current step doesn't give us a precise position, it does
|
|
||||||
// give boundaries to the position, which together with the last
|
|
||||||
// transition gives us boundaries for the speed value. This can be very
|
|
||||||
// useful especially in two situations:
|
|
||||||
// - we have been stopped for a while and start moving quickly: although
|
|
||||||
// we only have one transition initially, the number of steps we moved
|
|
||||||
// can already give a non-zero speed estimate
|
|
||||||
// - we were moving but then stop: without any extra logic we would just
|
|
||||||
// keep the last speed for a while, but we know from the step
|
|
||||||
// boundaries that the speed must be decreasing
|
|
||||||
|
|
||||||
// if there is a transition between the last sample and now, and that
|
|
||||||
// transition is closer to now than the previous sample time, we should
|
|
||||||
// use the slopes from the last sample to the transition as these will
|
|
||||||
// have less numerical issues and produce a tighter boundary
|
|
||||||
if (state->prev_trans_us > state->prev_step_us &&
|
|
||||||
(int)(state->prev_trans_us - state->prev_step_us) > (int)(step_us - state->prev_trans_us)) {
|
|
||||||
speed_high = substep_calc_speed(state->prev_trans_pos - state->prev_low, state->prev_trans_us - state->prev_step_us);
|
|
||||||
speed_low = substep_calc_speed(state->prev_trans_pos - state->prev_high, state->prev_trans_us - state->prev_step_us);
|
|
||||||
} else {
|
|
||||||
// otherwise use the slopes from the last transition to now
|
|
||||||
speed_high = substep_calc_speed(high - state->prev_trans_pos, step_us - state->prev_trans_us);
|
|
||||||
speed_low = substep_calc_speed(low - state->prev_trans_pos, step_us - state->prev_trans_us);
|
|
||||||
}
|
|
||||||
// make sure the current speed estimate is between the maximum and
|
|
||||||
// minimum values obtained from the step slopes
|
|
||||||
if (state->speed_2_20 > speed_high)
|
|
||||||
state->speed_2_20 = speed_high;
|
|
||||||
if (state->speed_2_20 < speed_low)
|
|
||||||
state->speed_2_20 = speed_low;
|
|
||||||
|
|
||||||
// convert the speed units from "sub-steps per 2^20 us" to "sub-steps
|
|
||||||
// per second"
|
|
||||||
state->speed = (state->speed_2_20 * 62500LL) >> 16;
|
|
||||||
|
|
||||||
// estimate the current position by applying the speed estimate to the
|
|
||||||
// most recent transition
|
|
||||||
state->position = state->prev_trans_pos + (((int64_t)state->speed_2_20 * (step_us - transition_us)) >> 20);
|
|
||||||
|
|
||||||
// make sure the position estimate is between "low" and "high", as we
|
|
||||||
// can be sure the actual current position must be in this range
|
|
||||||
if ((int)(state->position - high) > 0)
|
|
||||||
state->position = high;
|
|
||||||
else if ((int)(state->position - low) < 0)
|
|
||||||
state->position = low;
|
|
||||||
}
|
|
||||||
|
|
||||||
// save the current values to use on the next sample
|
|
||||||
state->prev_low = low;
|
|
||||||
state->prev_high = high;
|
|
||||||
state->raw_step = step;
|
|
||||||
state->prev_step_us = step_us;
|
|
||||||
}
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#include "hardware/pio.h"
|
|
||||||
|
|
||||||
typedef struct substep_state_t {
|
|
||||||
|
|
||||||
uint calibration_data[4]; // relative phase sizes
|
|
||||||
uint clocks_per_us; // save the clk_sys frequency in clocks per us
|
|
||||||
uint idle_stop_samples; // after these samples without transitions, assume the encoder is stopped
|
|
||||||
PIO pio;
|
|
||||||
uint sm;
|
|
||||||
|
|
||||||
uint prev_trans_pos, prev_trans_us;
|
|
||||||
uint prev_step_us;
|
|
||||||
uint prev_low, prev_high;
|
|
||||||
uint idle_stop_sample_count;
|
|
||||||
int speed_2_20;
|
|
||||||
int stopped;
|
|
||||||
|
|
||||||
int speed;
|
|
||||||
uint position;
|
|
||||||
|
|
||||||
uint raw_step;
|
|
||||||
} substep_state_t;
|
|
||||||
|
|
||||||
|
|
||||||
void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state);
|
|
||||||
void substep_update(substep_state_t *state);
|
|
||||||
@ -1,43 +0,0 @@
|
|||||||
#include "blink.pio.h"
|
|
||||||
#include "hardware/pio.h"
|
|
||||||
#include "hardware/clocks.h"
|
|
||||||
#include "pico/time.h"
|
|
||||||
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
#include "config.h"
|
|
||||||
#include "stepper.h"
|
|
||||||
|
|
||||||
void stepper_init() {
|
|
||||||
PIO pio = pio0;
|
|
||||||
uint offset = pio_add_program(pio, &oscillate_program);
|
|
||||||
|
|
||||||
start_pulse(pio, SM_A, offset, PULSE_PIN_A, 1);
|
|
||||||
start_pulse(pio, SM_B, offset, PULSE_PIN_B, 1);
|
|
||||||
|
|
||||||
gpio_init(DIR_PIN_A);
|
|
||||||
gpio_init(DIR_PIN_B);
|
|
||||||
gpio_set_dir(DIR_PIN_A, GPIO_OUT);
|
|
||||||
gpio_set_dir(DIR_PIN_B, GPIO_OUT);
|
|
||||||
}
|
|
||||||
|
|
||||||
void set_speeds(double a, double b) {
|
|
||||||
update_sm(pio0, SM_A, DIR_PIN_A, a);
|
|
||||||
update_sm(pio0, SM_B, DIR_PIN_B, b);
|
|
||||||
}
|
|
||||||
|
|
||||||
void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
|
|
||||||
oscillate_program_init(pio, sm, offset, pin);
|
|
||||||
pio->txf[sm] = 0;
|
|
||||||
pio_sm_set_enabled(pio, sm, true);
|
|
||||||
}
|
|
||||||
|
|
||||||
void update_sm(PIO pio, uint sm, const uint pin ,double v) {
|
|
||||||
double u_v = fabs(v);
|
|
||||||
if(u_v > 0.0005)
|
|
||||||
pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
|
|
||||||
else
|
|
||||||
pio->txf[sm] = 0;
|
|
||||||
gpio_put(pin, v < 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1,8 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#include "hardware/pio.h"
|
|
||||||
|
|
||||||
void stepper_init();
|
|
||||||
void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq);
|
|
||||||
|
|
||||||
void update_sm(PIO pio, uint sm, const uint pin, double v);
|
|
||||||
void set_speeds(double vl, double vr);
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
|
|
||||||
#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define CFG_TUSB_RHPORT0_MODE (OPT_MODE_DEVICE | OPT_MODE_FULL_SPEED)
|
|
||||||
#ifndef BOARD_TUD_RHPORT
|
|
||||||
#define BOARD_TUD_RHPORT 0
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef BOARD_TUD_MAX_SPEED
|
|
||||||
#define BOARD_TUD_MAX_SPEED OPT_MODE_DEFAULT_SPEED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef CFG_TUSB_MCU
|
|
||||||
#error CFG_TUSB_MCU must be defined
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef CFG_TUSB_OS
|
|
||||||
#define CFG_TUSB_OS OPT_OS_NONE
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef CFG_TUD_ENABLED
|
|
||||||
#define CFG_TUD_ENABLED 1
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef CFG_TUD_MAX_SPEED
|
|
||||||
#define CFG_TUD_MAX_SPEED BOARD_TUD_MAX_SPEED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef CFG_TUD_ENDPOINT0_SIZE
|
|
||||||
#define CFG_TUD_ENDPOINT0_SIZE 64
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define CFG_TUD_CDC 2
|
|
||||||
#define CFG_TUD_MSC 0
|
|
||||||
#define CFG_TUD_HID 0
|
|
||||||
#define CFG_TUD_MIDI 0
|
|
||||||
#define CFG_TUD_VENDOR 0
|
|
||||||
|
|
||||||
#define CFG_TUD_CDC_RX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
|
|
||||||
#define CFG_TUD_CDC_TX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
|
|
||||||
|
|
||||||
#define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
|
|
||||||
|
|
||||||
#define CFG_TUD_MSC_EP_BUFSIZE 512
|
|
||||||
@ -1,143 +0,0 @@
|
|||||||
#include "bsp/board_api.h"
|
|
||||||
#include "tusb.h"
|
|
||||||
|
|
||||||
#define _PID_MAP(itf,n) ( (CFG_TUD_##itf) << (n) )
|
|
||||||
#define USB_PID (0x4d47 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1)| _PID_MAP(HID, 2) | \
|
|
||||||
_PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4) )
|
|
||||||
|
|
||||||
#define USB_VID 0x1209
|
|
||||||
#define USB_BCD 0x0200
|
|
||||||
|
|
||||||
tusb_desc_device_t const desc_device = {
|
|
||||||
.bLength = sizeof(tusb_desc_device_t),
|
|
||||||
.bDescriptorType = TUSB_DESC_DEVICE,
|
|
||||||
.bcdUSB = USB_BCD,
|
|
||||||
|
|
||||||
.bDeviceClass = TUSB_CLASS_MISC,
|
|
||||||
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
|
|
||||||
.bDeviceProtocol = MISC_PROTOCOL_IAD,
|
|
||||||
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
|
|
||||||
|
|
||||||
.idVendor = USB_VID,
|
|
||||||
.idProduct = USB_PID,
|
|
||||||
.bcdDevice = 0x0100,
|
|
||||||
|
|
||||||
.iManufacturer = 0x01,
|
|
||||||
.iProduct = 0x02,
|
|
||||||
.iSerialNumber = 0x03,
|
|
||||||
|
|
||||||
.bNumConfigurations = 0x01
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
uint8_t const * tud_descriptor_device_cb(void) {
|
|
||||||
return (uint8_t const *) &desc_device;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum
|
|
||||||
{
|
|
||||||
ITF_NUM_CDC_0 = 0,
|
|
||||||
ITF_NUM_CDC_0_DATA,
|
|
||||||
ITF_NUM_CDC_1,
|
|
||||||
ITF_NUM_CDC_1_DATA,
|
|
||||||
ITF_NUM_TOTAL,
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
#define CONFIG_TOTAL_LEN (TUD_CONFIG_DESC_LEN + CFG_TUD_CDC * TUD_CDC_DESC_LEN)
|
|
||||||
|
|
||||||
#define EPNUM_CDC_NOTIF 0x81
|
|
||||||
#define EPNUM_CDC_OUT 0x02
|
|
||||||
#define EPNUM_CDC_IN 0x82
|
|
||||||
|
|
||||||
#define EPNUM_CDC_1_NOTIF 0x84
|
|
||||||
#define EPNUM_CDC_1_OUT 0x05
|
|
||||||
#define EPNUM_CDC_1_IN 0x85
|
|
||||||
|
|
||||||
|
|
||||||
uint8_t const desc_fs_configuration[] =
|
|
||||||
{
|
|
||||||
TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 250),
|
|
||||||
TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_0, 4, EPNUM_CDC_NOTIF, 8, EPNUM_CDC_OUT, EPNUM_CDC_IN, 64),
|
|
||||||
TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_1, 4, EPNUM_CDC_1_NOTIF, 8, EPNUM_CDC_1_OUT, EPNUM_CDC_1_IN, 64)
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
tusb_desc_device_qualifier_t const desc_device_qualifier =
|
|
||||||
{
|
|
||||||
.bLength = sizeof(tusb_desc_device_qualifier_t),
|
|
||||||
.bDescriptorType = TUSB_DESC_DEVICE,
|
|
||||||
.bcdUSB = USB_BCD,
|
|
||||||
|
|
||||||
.bDeviceClass = TUSB_CLASS_MISC,
|
|
||||||
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
|
|
||||||
.bDeviceProtocol = MISC_PROTOCOL_IAD,
|
|
||||||
|
|
||||||
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
|
|
||||||
.bNumConfigurations = 0x01
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
uint8_t const* tud_descriptor_device_qualifier_cb(void) {
|
|
||||||
return (uint8_t const*) &desc_device_qualifier;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
uint8_t const* tud_descriptor_configuration_cb(uint8_t index) {
|
|
||||||
(void) index;
|
|
||||||
|
|
||||||
return desc_fs_configuration;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum {
|
|
||||||
STRID_LANGID = 0,
|
|
||||||
STRID_MANUFACTURER,
|
|
||||||
STRID_PRODUCT,
|
|
||||||
STRID_SERIAL,
|
|
||||||
STRID_CDC_0,
|
|
||||||
STRID_CDC_1
|
|
||||||
};
|
|
||||||
|
|
||||||
char const *string_desc_arr[] = {
|
|
||||||
(const char[]) { 0x09, 0x04},
|
|
||||||
"Mg Robotics",
|
|
||||||
"Magrob Odometry MCU",
|
|
||||||
NULL,
|
|
||||||
"Odometry CDC",
|
|
||||||
"Stepper CDC"
|
|
||||||
};
|
|
||||||
|
|
||||||
static uint16_t _desc_str[32+1];
|
|
||||||
|
|
||||||
uint16_t const *tud_descriptor_string_cb(uint8_t index, uint16_t langid) {
|
|
||||||
(void) langid;
|
|
||||||
size_t chr_count;
|
|
||||||
|
|
||||||
switch (index) {
|
|
||||||
case STRID_LANGID:
|
|
||||||
memcpy(&_desc_str[1], string_desc_arr[0], 2);
|
|
||||||
chr_count = 1;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case STRID_SERIAL:
|
|
||||||
chr_count = board_usb_get_serial(_desc_str + 1, 32);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
if( !(index < sizeof(string_desc_arr) / sizeof(string_desc_arr[0])) ) return NULL;
|
|
||||||
|
|
||||||
const char *str = string_desc_arr[index];
|
|
||||||
|
|
||||||
chr_count = strlen(str);
|
|
||||||
size_t const max_count = sizeof(_desc_str) / sizeof(_desc_str[0]) - 1;
|
|
||||||
if ( chr_count > max_count ) chr_count = max_count;
|
|
||||||
|
|
||||||
for(size_t i = 0; i < chr_count; i++) {
|
|
||||||
_desc_str[i + 1] = str[i];
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
_desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8) | (2 * chr_count + 2));
|
|
||||||
return _desc_str;
|
|
||||||
}
|
|
||||||
@ -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);
|
|
||||||
@ -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 import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
|
||||||
from launch.conditions import UnlessCondition, IfCondition
|
from launch.conditions import UnlessCondition
|
||||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
@ -26,14 +26,6 @@ def generate_launch_description():
|
|||||||
'local_test': LaunchConfiguration('local_test')
|
'local_test': LaunchConfiguration('local_test')
|
||||||
}.items()
|
}.items()
|
||||||
),
|
),
|
||||||
IncludeLaunchDescription(
|
|
||||||
PathJoinSubstitution([
|
|
||||||
FindPackageShare("mg_lidar"),
|
|
||||||
'launch',
|
|
||||||
'launch.py'
|
|
||||||
]),
|
|
||||||
condition=UnlessCondition(LaunchConfiguration('local_test')),
|
|
||||||
),
|
|
||||||
Node(
|
Node(
|
||||||
package="mg_odometry",
|
package="mg_odometry",
|
||||||
executable="mg_odom_publisher",
|
executable="mg_odom_publisher",
|
||||||
@ -41,28 +33,12 @@ def generate_launch_description():
|
|||||||
condition=UnlessCondition(LaunchConfiguration('local_test')),
|
condition=UnlessCondition(LaunchConfiguration('local_test')),
|
||||||
parameters=[{
|
parameters=[{
|
||||||
'odom': "odom",
|
'odom': "odom",
|
||||||
'serial_path': "/dev/ttyACM0",
|
'serial_path': "/dev/ttyACM1",
|
||||||
}],
|
}],
|
||||||
|
|
||||||
emulate_tty=True,
|
emulate_tty=True,
|
||||||
output='screen'
|
output='screen'
|
||||||
),
|
),
|
||||||
IncludeLaunchDescription(
|
|
||||||
PathJoinSubstitution([
|
|
||||||
FindPackageShare("mg_bt"),
|
|
||||||
'launch',
|
|
||||||
'launch.py'
|
|
||||||
]),
|
|
||||||
condition=IfCondition(LaunchConfiguration('local_test')),
|
|
||||||
),
|
|
||||||
Node(
|
|
||||||
package="mg_planner",
|
|
||||||
executable="mg_planner",
|
|
||||||
name="mg_planner",
|
|
||||||
emulate_tty=True,
|
|
||||||
condition=IfCondition(LaunchConfiguration('local_test')),
|
|
||||||
output='screen',
|
|
||||||
),
|
|
||||||
Node(
|
Node(
|
||||||
package="mg_navigation",
|
package="mg_navigation",
|
||||||
executable="mg_nav_server",
|
executable="mg_nav_server",
|
||||||
|
|||||||
@ -1,72 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.12)
|
|
||||||
project(mg_bt)
|
|
||||||
|
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
|
||||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
set(PACKAGE_DEPS
|
|
||||||
rclcpp
|
|
||||||
behaviortree_cpp
|
|
||||||
behaviortree_ros2
|
|
||||||
btcpp_ros2_interfaces
|
|
||||||
tf2
|
|
||||||
tf2_ros
|
|
||||||
tf2_geometry_msgs
|
|
||||||
mg_msgs
|
|
||||||
std_msgs
|
|
||||||
std_srvs
|
|
||||||
)
|
|
||||||
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
|
|
||||||
include(FindPkgConfig)
|
|
||||||
pkg_search_module(LIBGLM REQUIRED glm)
|
|
||||||
|
|
||||||
foreach(PACKAGE ${PACKAGE_DEPS})
|
|
||||||
find_package(${PACKAGE} REQUIRED)
|
|
||||||
endforeach()
|
|
||||||
|
|
||||||
set(SOURCES
|
|
||||||
src/mg_bt.cpp
|
|
||||||
src/mg_tree_executor.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
add_executable(mg_bt_executor ${SOURCES})
|
|
||||||
add_executable(mg_i2cnode i2cmodule/i2cnode.cpp)
|
|
||||||
|
|
||||||
target_include_directories(
|
|
||||||
mg_bt_executor
|
|
||||||
PRIVATE
|
|
||||||
${LIBGLM_INCLUDE_DIRS}
|
|
||||||
include
|
|
||||||
)
|
|
||||||
|
|
||||||
target_link_libraries(mg_i2cnode i2c)
|
|
||||||
|
|
||||||
ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
|
|
||||||
ament_target_dependencies(mg_i2cnode rclcpp rclcpp_action mg_msgs)
|
|
||||||
|
|
||||||
install( TARGETS
|
|
||||||
mg_bt_executor
|
|
||||||
mg_i2cnode
|
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY
|
|
||||||
behavior_trees
|
|
||||||
config
|
|
||||||
launch
|
|
||||||
DESTINATION share/${PROJECT_NAME}/
|
|
||||||
)
|
|
||||||
|
|
||||||
target_compile_features(mg_bt_executor PUBLIC
|
|
||||||
c_std_99
|
|
||||||
cxx_std_17
|
|
||||||
) # Require C99 and C++17
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@ -1,170 +0,0 @@
|
|||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<root BTCPP_format="4">
|
|
||||||
<BehaviorTree ID="calib2_bt">
|
|
||||||
<Sequence>
|
|
||||||
<ZeroNode service_name="/zero"/>
|
|
||||||
<MovePoint x_goal="0.7"
|
|
||||||
y_goal="0.0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="0.1"
|
|
||||||
max_vel="2.000000"
|
|
||||||
ornt_mult="4.000000"
|
|
||||||
tolerance="0.02"
|
|
||||||
action_name="/MovePoint"/>
|
|
||||||
<RotateNode angle="-2"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<RotateNode angle="-3.14"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<MovePoint x_goal="0.3"
|
|
||||||
y_goal="0.0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
max_vel="2.000000"
|
|
||||||
ornt_mult="4.000000"
|
|
||||||
tolerance="0.02"
|
|
||||||
action_name="/MovePoint"/>
|
|
||||||
<RotateNode angle="-2"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<RotateNode angle="0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<Fallback>
|
|
||||||
<Timeout msec="5000">
|
|
||||||
<MovePoint x_goal="-0.1"
|
|
||||||
y_goal="0.0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="0.1"
|
|
||||||
max_vel="2.000000"
|
|
||||||
ornt_mult="4.000000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/MovePoint"/>
|
|
||||||
</Timeout>
|
|
||||||
<AlwaysSuccess/>
|
|
||||||
</Fallback>
|
|
||||||
<ZeroNode service_name="/endCalib"/>
|
|
||||||
</Sequence>
|
|
||||||
</BehaviorTree>
|
|
||||||
|
|
||||||
<BehaviorTree ID="calib_bt">
|
|
||||||
<Sequence>
|
|
||||||
<SetBlackboard value="0.311010"
|
|
||||||
output_key="width"/>
|
|
||||||
<Delay delay_msec="10000">
|
|
||||||
<Repeat num_cycles="5">
|
|
||||||
<Sequence>
|
|
||||||
<ZeroNode service_name="/zero"/>
|
|
||||||
<MovePoint x_goal="0.7"
|
|
||||||
y_goal="0.0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="0.1"
|
|
||||||
max_vel="2.000000"
|
|
||||||
ornt_mult="4.000000"
|
|
||||||
tolerance="0.04"
|
|
||||||
action_name="/MovePoint"/>
|
|
||||||
<RotateNode angle="2"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<RotateNode angle="-2"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<RotateNode angle="0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="3.140000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/Rotate"/>
|
|
||||||
<Fallback>
|
|
||||||
<Timeout msec="20000">
|
|
||||||
<MovePoint x_goal="-0.1"
|
|
||||||
y_goal="0.0"
|
|
||||||
max_wheel_speed="3.000000"
|
|
||||||
max_angular="0.1"
|
|
||||||
max_vel="2.000000"
|
|
||||||
ornt_mult="4.000000"
|
|
||||||
tolerance="0.001000"
|
|
||||||
action_name="/MovePoint"/>
|
|
||||||
</Timeout>
|
|
||||||
<AlwaysSuccess/>
|
|
||||||
</Fallback>
|
|
||||||
<CalibWidth PreviousWidth="{width}"
|
|
||||||
Count="1"
|
|
||||||
NewWidth="{width}"
|
|
||||||
service_name="/set_width"/>
|
|
||||||
</Sequence>
|
|
||||||
</Repeat>
|
|
||||||
</Delay>
|
|
||||||
</Sequence>
|
|
||||||
</BehaviorTree>
|
|
||||||
|
|
||||||
<!-- Description of Node Models (used by Groot) -->
|
|
||||||
<TreeNodesModel>
|
|
||||||
<Action ID="CalibWidth">
|
|
||||||
<input_port name="PreviousWidth"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="Count"
|
|
||||||
default="1"
|
|
||||||
type="int"/>
|
|
||||||
<output_port name="NewWidth"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="service_name"
|
|
||||||
type="std::string">Service name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="MovePoint">
|
|
||||||
<input_port name="x_goal"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="y_goal"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="max_wheel_speed"
|
|
||||||
default="3.000000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="max_angular"
|
|
||||||
default="3.140000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="max_vel"
|
|
||||||
default="2.000000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="ornt_mult"
|
|
||||||
default="4.000000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="tolerance"
|
|
||||||
default="0.001000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="action_name"
|
|
||||||
type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="RotateNode">
|
|
||||||
<input_port name="angle"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="max_wheel_speed"
|
|
||||||
default="3.000000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="max_angular"
|
|
||||||
default="3.140000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="tolerance"
|
|
||||||
default="0.001000"
|
|
||||||
type="double"/>
|
|
||||||
<input_port name="action_name"
|
|
||||||
type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="ZeroNode">
|
|
||||||
<input_port name="service_name"
|
|
||||||
type="std::string">Service name</input_port>
|
|
||||||
</Action>
|
|
||||||
</TreeNodesModel>
|
|
||||||
|
|
||||||
</root>
|
|
||||||
@ -1,91 +0,0 @@
|
|||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<root BTCPP_format="4" project_name="Project">
|
|
||||||
<include path="calib_bt.xml"/>
|
|
||||||
<include path="tactics.xml"/>
|
|
||||||
<!-- Description of Node Models (used by Groot) -->
|
|
||||||
<TreeNodesModel>
|
|
||||||
<Action ID="CalibWidth">
|
|
||||||
<input_port name="PreviousWidth" type="double"/>
|
|
||||||
<input_port name="Count" default="1" type="int"/>
|
|
||||||
<output_port name="NewWidth" type="double"/>
|
|
||||||
<input_port name="service_name" type="std::string">Service name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="CanChooser">
|
|
||||||
<output_port name="orient" type="int"/>
|
|
||||||
<output_port name="y_loc" type="double"/>
|
|
||||||
<output_port name="x_loc" type="double"/>
|
|
||||||
<input_port name="canlist" type="std::string"/>
|
|
||||||
</Action>
|
|
||||||
<Action ID="I2CSignal">
|
|
||||||
<input_port name="Address" default="42" type="int"/>
|
|
||||||
<input_port name="Data" default="0" type="int"/>
|
|
||||||
<output_port name="Result" type="int"/>
|
|
||||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="LookAtNode">
|
|
||||||
<input_port name="x" type="double"/>
|
|
||||||
<input_port name="y" type="double"/>
|
|
||||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
|
||||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
|
||||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
|
||||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="MoveGlobal">
|
|
||||||
<input_port name="x_goal" type="double"/>
|
|
||||||
<input_port name="y_goal" type="double"/>
|
|
||||||
<input_port name="max_wheel_speed" default="6.000000" type="double"/>
|
|
||||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
|
||||||
<input_port name="max_vel" default="4.000000" type="double"/>
|
|
||||||
<input_port name="ornt_mult" default="3.000000" type="double"/>
|
|
||||||
<input_port name="tolerance" default="0.050000" type="double"/>
|
|
||||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="MoveLocal">
|
|
||||||
<input_port name="x_goal" type="double"/>
|
|
||||||
<input_port name="y_goal" type="double"/>
|
|
||||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
|
||||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
|
||||||
<input_port name="max_vel" default="2.000000" type="double"/>
|
|
||||||
<input_port name="pos_mult" default="15.000000" type="double"/>
|
|
||||||
<input_port name="ornt_mult" default="4.000000" type="double"/>
|
|
||||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
|
||||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="MovePoint">
|
|
||||||
<input_port name="x_goal" type="double"/>
|
|
||||||
<input_port name="y_goal" type="double"/>
|
|
||||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
|
||||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
|
||||||
<input_port name="max_vel" default="2.000000" type="double"/>
|
|
||||||
<input_port name="ornt_mult" default="4.000000" type="double"/>
|
|
||||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
|
||||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="RotateNode">
|
|
||||||
<input_port name="angle" type="double"/>
|
|
||||||
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
|
|
||||||
<input_port name="max_angular" default="3.140000" type="double"/>
|
|
||||||
<input_port name="tolerance" default="0.001000" type="double"/>
|
|
||||||
<input_port name="action_name" type="std::string">Action server name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Action ID="SendPose">
|
|
||||||
<input_port name="x" type="double"/>
|
|
||||||
<input_port name="y" type="double"/>
|
|
||||||
<input_port name="angle" type="double"/>
|
|
||||||
<input_port name="isDegree" default="true" type="bool"/>
|
|
||||||
<input_port name="service_name" type="std::string">Service name</input_port>
|
|
||||||
</Action>
|
|
||||||
<Condition ID="SideObstaclePub">
|
|
||||||
<input_port name="tactic" type="int"/>
|
|
||||||
<input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
|
|
||||||
</Condition>
|
|
||||||
<Action ID="TacticChooser">
|
|
||||||
<output_port name="IsBlue" type="bool"/>
|
|
||||||
<output_port name="tactic" type="int"/>
|
|
||||||
<input_port name="number" type="int"/>
|
|
||||||
</Action>
|
|
||||||
<Action ID="ZeroNode">
|
|
||||||
<input_port name="service_name" type="std::string">Service name</input_port>
|
|
||||||
</Action>
|
|
||||||
</TreeNodesModel>
|
|
||||||
</root>
|
|
||||||
@ -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,14 +0,0 @@
|
|||||||
bt_action_server:
|
|
||||||
ros__parameters:
|
|
||||||
action_name: "mg_bt_action_server"
|
|
||||||
tick_frequency: 100
|
|
||||||
groot2_port: 8420
|
|
||||||
ros_plugins_timeout: 1000
|
|
||||||
|
|
||||||
plugins:
|
|
||||||
- btcpp_ros2_samples/bt_plugins
|
|
||||||
|
|
||||||
behavior_trees:
|
|
||||||
- mg_bt/behavior_trees
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,88 +0,0 @@
|
|||||||
#include "rclcpp/rclcpp.hpp"
|
|
||||||
#include "rclcpp_action/rclcpp_action.hpp"
|
|
||||||
#include "mg_msgs/srv/i2c.hpp"
|
|
||||||
#include "mg_msgs/action/i2c.hpp"
|
|
||||||
|
|
||||||
extern "C" {
|
|
||||||
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <linux/i2c-dev.h>
|
|
||||||
#include <sys/ioctl.h>
|
|
||||||
#include <i2c/smbus.h>
|
|
||||||
}
|
|
||||||
class MgI2c : public rclcpp::Node {
|
|
||||||
using I2cSrv = mg_msgs::srv::I2c;
|
|
||||||
using I2cAction = mg_msgs::action::I2c;
|
|
||||||
using GoalHandleI2c = rclcpp_action::ServerGoalHandle<I2cAction>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
|
|
||||||
auto cb
|
|
||||||
= [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
|
|
||||||
using namespace std::placeholders;
|
|
||||||
i2c_action_ = rclcpp_action::create_server<I2cAction>(this,
|
|
||||||
"/i2c_action",
|
|
||||||
bind(&MgI2c::handle_goal, this, _1, _2),
|
|
||||||
bind(&MgI2c::handle_cancel, this, _1),
|
|
||||||
bind(&MgI2c::handle_accepted, this, _1));
|
|
||||||
}
|
|
||||||
|
|
||||||
void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
|
|
||||||
ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT
|
|
||||||
i2c_smbus_write_byte(i2c_fd_, req->data);
|
|
||||||
int ch = 0;
|
|
||||||
|
|
||||||
rclcpp::Rate rate(100);
|
|
||||||
|
|
||||||
while (ch == 0 || (ch > 255 || ch < 0)) {
|
|
||||||
ch = i2c_smbus_read_byte(i2c_fd_);
|
|
||||||
rate.sleep();
|
|
||||||
}
|
|
||||||
resp->resp.push_back(ch);
|
|
||||||
RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front());
|
|
||||||
}
|
|
||||||
|
|
||||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
|
|
||||||
std::shared_ptr<const I2cAction::Goal> goal) {
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Received goal request with addr %d and data %d", goal->addr, goal->data);
|
|
||||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
|
||||||
}
|
|
||||||
|
|
||||||
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleI2c>) {
|
|
||||||
return rclcpp_action::CancelResponse::ACCEPT;
|
|
||||||
}
|
|
||||||
|
|
||||||
void handle_accepted(const std::shared_ptr<GoalHandleI2c> goal_handle) {
|
|
||||||
using namespace std::placeholders;
|
|
||||||
ioctl(i2c_fd_, I2C_SLAVE, goal_handle->get_goal()->addr); // NOLINT
|
|
||||||
i2c_smbus_write_byte(i2c_fd_, goal_handle->get_goal()->data);
|
|
||||||
int ch = 0;
|
|
||||||
|
|
||||||
rclcpp::Rate rate(100);
|
|
||||||
|
|
||||||
while (ch == 0 || (ch > 255 || ch < 0)) {
|
|
||||||
ch = i2c_smbus_read_byte(i2c_fd_);
|
|
||||||
rate.sleep();
|
|
||||||
}
|
|
||||||
auto res = std::make_shared<I2cAction::Result>();
|
|
||||||
RCLCPP_INFO(get_logger(), "Recieved %d", ch);
|
|
||||||
res->resp.push_back(ch);
|
|
||||||
goal_handle->succeed(res);
|
|
||||||
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
|
|
||||||
|
|
||||||
rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;
|
|
||||||
|
|
||||||
int i2c_fd_;
|
|
||||||
};
|
|
||||||
|
|
||||||
int main(int argc, const char* const* argv) {
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
rclcpp::spin(std::make_shared<MgI2c>("i2cu"));
|
|
||||||
rclcpp::shutdown();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch.substitutions import PathJoinSubstitution
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.substitutions import FindPackageShare
|
|
||||||
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
|
|
||||||
basedir = FindPackageShare("mg_bt")
|
|
||||||
|
|
||||||
bt_exec_config = PathJoinSubstitution([basedir, "config/mg_bt_executor.yaml"])
|
|
||||||
|
|
||||||
return LaunchDescription([
|
|
||||||
Node(
|
|
||||||
package='mg_bt',
|
|
||||||
executable='mg_bt_executor',
|
|
||||||
output="screen",
|
|
||||||
parameters=[bt_exec_config]
|
|
||||||
),
|
|
||||||
Node(
|
|
||||||
package='mg_planner',
|
|
||||||
executable='mg_planner',
|
|
||||||
output="screen"
|
|
||||||
),
|
|
||||||
])
|
|
||||||
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>mg_bt</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>Behavior for MagRob</description>
|
|
||||||
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
|
||||||
<license>Apache 2.0</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
|
||||||
<depend>behaviortree_cpp</depend>
|
|
||||||
<depend>behaviortree_ros2</depend>
|
|
||||||
<depend>btcpp_ros2_interfaces</depend>
|
|
||||||
<depend>mg_msgs</depend>
|
|
||||||
<depend>libi2c-dev</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,23 +0,0 @@
|
|||||||
#include <iostream>
|
|
||||||
|
|
||||||
#include "mg_tree_executor.hpp"
|
|
||||||
|
|
||||||
int main(const int argc, const char** argv) {
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
|
|
||||||
std::cout << "Starting up Behavior Tree Executor" << std::endl;
|
|
||||||
|
|
||||||
rclcpp::NodeOptions options;
|
|
||||||
|
|
||||||
auto bt_exec_server = std::make_shared<mg::MgTreeExecutor>(options);
|
|
||||||
|
|
||||||
rclcpp::executors::MultiThreadedExecutor executor(
|
|
||||||
rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
|
|
||||||
|
|
||||||
executor.add_node(bt_exec_server->node());
|
|
||||||
executor.spin();
|
|
||||||
executor.remove_node(bt_exec_server->node());
|
|
||||||
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,75 +0,0 @@
|
|||||||
#include "mg_tree_executor.hpp"
|
|
||||||
|
|
||||||
#include "behaviortree_cpp/xml_parsing.h"
|
|
||||||
#include "tree_nodes/calib.hpp"
|
|
||||||
#include "tree_nodes/choose_can.hpp"
|
|
||||||
#include "tree_nodes/choose_tactic.hpp"
|
|
||||||
#include "tree_nodes/i2c.hpp"
|
|
||||||
#include "tree_nodes/move_point.hpp"
|
|
||||||
#include "tree_nodes/move_local.hpp"
|
|
||||||
#include "tree_nodes/move_global.hpp"
|
|
||||||
#include "tree_nodes/look_at.hpp"
|
|
||||||
#include "tree_nodes/rotate.hpp"
|
|
||||||
#include "tree_nodes/side_pub.hpp"
|
|
||||||
#include "tree_nodes/set_pos.hpp"
|
|
||||||
#include "tree_nodes/zero.hpp"
|
|
||||||
|
|
||||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
||||||
#include "tf2/LinearMath/Transform.h"
|
|
||||||
#include "tf2/convert.h"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
|
|
||||||
executeRegistration();
|
|
||||||
std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
|
|
||||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
|
|
||||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
|
||||||
|
|
||||||
const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }
|
|
||||||
|
|
||||||
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
|
|
||||||
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
|
|
||||||
factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
|
|
||||||
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
|
|
||||||
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
|
|
||||||
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
|
|
||||||
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
|
|
||||||
factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
|
|
||||||
factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
|
|
||||||
factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
|
|
||||||
factory.registerNodeType<mg::CanChooser>("CanChooser");
|
|
||||||
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
|
|
||||||
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
|
|
||||||
}
|
|
||||||
|
|
||||||
std::pair<glm::vec2, double> MgTreeExecutor::position() {
|
|
||||||
double x = NAN;
|
|
||||||
double y = NAN;
|
|
||||||
double theta;
|
|
||||||
glm::vec2 pos;
|
|
||||||
|
|
||||||
auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
|
|
||||||
|
|
||||||
tf2::Transform t;
|
|
||||||
tf2::convert(tmsg.transform, t);
|
|
||||||
t.getBasis().getRPY(x, y, theta);
|
|
||||||
auto vec3 = tmsg.transform.translation;
|
|
||||||
|
|
||||||
pos = glm::dvec2(vec3.x, vec3.y);
|
|
||||||
return { pos, theta };
|
|
||||||
}
|
|
||||||
|
|
||||||
std::optional<std::string> MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
|
|
||||||
(void)status;
|
|
||||||
logger_cout_.reset();
|
|
||||||
|
|
||||||
if (was_cancelled) {
|
|
||||||
std::cout << "Behavior tree was cancled" << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
return std::nullopt;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "behaviortree_ros2/tree_execution_server.hpp"
|
|
||||||
#include "behaviortree_cpp/loggers/bt_cout_logger.h"
|
|
||||||
#include "tf2_ros/buffer.h"
|
|
||||||
#include "tf2_ros/transform_listener.h"
|
|
||||||
#include "glm/glm.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
class MgTreeExecutor : public BT::TreeExecutionServer {
|
|
||||||
public:
|
|
||||||
MgTreeExecutor(const rclcpp::NodeOptions opts);
|
|
||||||
void onTreeCreated(BT::Tree& tree) override;
|
|
||||||
void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
|
|
||||||
|
|
||||||
std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
|
|
||||||
|
|
||||||
std::pair<glm::vec2, double> position();
|
|
||||||
|
|
||||||
private:
|
|
||||||
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
|
|
||||||
tf2_ros::Buffer::SharedPtr tf_buffer_;
|
|
||||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
@ -1,44 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
||||||
#include "mg_msgs/srv/send_double.hpp"
|
|
||||||
#include "glm/glm.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
|
|
||||||
|
|
||||||
class CalibWidthNode : public BT::RosServiceNode<mg_msgs::srv::SendDouble> {
|
|
||||||
public:
|
|
||||||
CalibWidthNode(const std::string& name,
|
|
||||||
const BT::NodeConfig& conf,
|
|
||||||
const BT::RosNodeParams& params,
|
|
||||||
const PosFuncSig pos_query) :
|
|
||||||
BT::RosServiceNode<mg_msgs::srv::SendDouble>(name, conf, params), pos_query_(pos_query) { }
|
|
||||||
|
|
||||||
static BT::PortsList providedPorts() {
|
|
||||||
return providedBasicPorts({ BT::InputPort<double>("PreviousWidth"),
|
|
||||||
BT::InputPort<int>("Count", 1, {}),
|
|
||||||
BT::OutputPort<double>("NewWidth") });
|
|
||||||
}
|
|
||||||
|
|
||||||
bool setRequest(typename Request::SharedPtr& request) override {
|
|
||||||
auto [pos, theta] = pos_query_();
|
|
||||||
double width = getInput<double>("PreviousWidth").value();
|
|
||||||
int count = getInput<int>("Count").value();
|
|
||||||
double new_width = width * (1 + (theta / (2 * M_PI * count)));
|
|
||||||
|
|
||||||
RCLCPP_INFO(logger(), "Setting width to: %lf", new_width);
|
|
||||||
|
|
||||||
request->set__data(new_width);
|
|
||||||
setOutput("Count", new_width);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
|
|
||||||
return BT::NodeStatus::SUCCESS;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
const PosFuncSig pos_query_;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
@ -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,29 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
||||||
#include "mg_msgs/action/i2c.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
class I2cNode : public BT::RosActionNode<mg_msgs::action::I2c> {
|
|
||||||
public:
|
|
||||||
I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
|
||||||
BT::RosActionNode<mg_msgs::action::I2c>(name, conf, params) { }
|
|
||||||
|
|
||||||
static BT::PortsList providedPorts() {
|
|
||||||
return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
|
|
||||||
BT::InputPort<int>("Data", 0, {}),
|
|
||||||
BT::OutputPort<int>("Result") });
|
|
||||||
}
|
|
||||||
|
|
||||||
bool setGoal(Goal& req) override {
|
|
||||||
req.addr = getInput<int>("Address").value_or(42);
|
|
||||||
req.data = getInput<int>("Data").value_or(0);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
BT::NodeStatus onResultReceived(const WrappedResult& resp) override {
|
|
||||||
setOutput<int>("Result", resp.result->resp.front());
|
|
||||||
return BT::NodeStatus::SUCCESS;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
@ -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;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,56 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
||||||
#include "mg_msgs/action/move_point.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
class MovePointNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
|
|
||||||
public:
|
|
||||||
MovePointNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
|
||||||
BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params) { }
|
|
||||||
|
|
||||||
static BT::PortsList providedPorts() {
|
|
||||||
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
|
|
||||||
BT::InputPort<double>("y_goal"),
|
|
||||||
BT::InputPort<double>("tolerance", 0.001, {}),
|
|
||||||
BT::InputPort<double>("max_wheel_speed", 3, {}),
|
|
||||||
BT::InputPort<double>("max_angular", 3.14, {}),
|
|
||||||
BT::InputPort<double>("max_vel", 2, {}),
|
|
||||||
BT::InputPort<double>("ornt_mult", 4, {}) });
|
|
||||||
}
|
|
||||||
|
|
||||||
bool setGoal(Goal& goal) override {
|
|
||||||
auto x_goal = getInput<double>("x_goal");
|
|
||||||
auto y_goal = getInput<double>("y_goal");
|
|
||||||
auto tolerance = getInput<double>("tolerance");
|
|
||||||
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
|
||||||
auto max_angular = getInput<double>("max_angular");
|
|
||||||
auto max_vel = getInput<double>("max_vel");
|
|
||||||
auto pos_mult = getInput<double>("pos_mult");
|
|
||||||
auto ornt_mult = getInput<double>("ornt_mult");
|
|
||||||
goal.x = x_goal.value();
|
|
||||||
goal.y = y_goal.value();
|
|
||||||
goal.tolerance = tolerance.value_or(goal.tolerance);
|
|
||||||
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
|
||||||
goal.max_angular = max_angular.value_or(goal.max_angular);
|
|
||||||
goal.max_vel = max_vel.value_or(goal.max_vel);
|
|
||||||
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
|
|
||||||
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
|
||||||
|
|
||||||
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
|
|
||||||
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
|
|
||||||
BT::NodeStatus::FAILURE;
|
|
||||||
}
|
|
||||||
|
|
||||||
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
|
|
||||||
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
|
|
||||||
return BT::NodeStatus::FAILURE;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
||||||
#include "mg_msgs/action/rotate.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
class RotateNode : public BT::RosActionNode<mg_msgs::action::Rotate> {
|
|
||||||
public:
|
|
||||||
RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
|
||||||
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
|
|
||||||
|
|
||||||
static BT::PortsList providedPorts() {
|
|
||||||
return providedBasicPorts({
|
|
||||||
BT::InputPort<double>("angle"),
|
|
||||||
BT::InputPort<double>("tolerance", 0.001, {}),
|
|
||||||
BT::InputPort<double>("max_wheel_speed", 3, {}),
|
|
||||||
BT::InputPort<double>("max_angular", 3.14, {}),
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
bool setGoal(Goal& goal) override {
|
|
||||||
auto angle = getInput<double>("angle");
|
|
||||||
auto tolerance = getInput<double>("tolerance");
|
|
||||||
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
|
||||||
auto max_angular = getInput<double>("max_angular");
|
|
||||||
goal.angle = angle.value();
|
|
||||||
goal.tolerance = tolerance.value_or(goal.tolerance);
|
|
||||||
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
|
||||||
goal.max_angular = max_angular.value_or(goal.max_angular);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
|
||||||
|
|
||||||
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
|
|
||||||
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
|
|
||||||
BT::NodeStatus::FAILURE;
|
|
||||||
}
|
|
||||||
|
|
||||||
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
|
|
||||||
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
|
|
||||||
return BT::NodeStatus::FAILURE;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -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,20 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
||||||
#include "std_srvs/srv/empty.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
class ZeroNode : public BT::RosServiceNode<std_srvs::srv::Empty> {
|
|
||||||
public:
|
|
||||||
ZeroNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
|
||||||
BT::RosServiceNode<std_srvs::srv::Empty>(name, conf, params) { }
|
|
||||||
|
|
||||||
bool setRequest(typename Request::SharedPtr&) override { return true; }
|
|
||||||
|
|
||||||
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
|
|
||||||
return BT::NodeStatus::SUCCESS;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -12,7 +12,7 @@ diffdrive_controller:
|
|||||||
left_wheel_names: ["left_motor"]
|
left_wheel_names: ["left_motor"]
|
||||||
right_wheel_names: ["right_motor"]
|
right_wheel_names: ["right_motor"]
|
||||||
|
|
||||||
enable_odom_tf: false
|
enable_odom_tf: true
|
||||||
odom_frame_id: odom_excpected
|
odom_frame_id: odom_excpected
|
||||||
base_frame_id: base-link
|
base_frame_id: base-link
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,17 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#ifndef MG_WHEEL_INTERFACE_HPP_
|
#ifndef MG_WHEEL_INTERFACE_HPP_
|
||||||
#define MG_WHEEL_INTERFACE_HPP_
|
#define MG_WHEEL_INTERFACE_HPP_
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|||||||
@ -1,3 +1,17 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#include "mg_control/mg_control.hpp"
|
#include "mg_control/mg_control.hpp"
|
||||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||||
|
|
||||||
@ -11,7 +25,7 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
|
|||||||
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
|
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
|
||||||
serial_port_name = info_.hardware_parameters["device_path"];
|
serial_port_name = info_.hardware_parameters["device_path"];
|
||||||
} else {
|
} else {
|
||||||
serial_port_name = "/dev/ttyACM1";
|
serial_port_name = "/dev/ttyACM0";
|
||||||
}
|
}
|
||||||
|
|
||||||
left_wheel_pos_state = 0;
|
left_wheel_pos_state = 0;
|
||||||
@ -66,18 +80,14 @@ hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&
|
|||||||
}
|
}
|
||||||
|
|
||||||
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
|
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
|
||||||
union {
|
std::string cmd_left;
|
||||||
std::array<u_char, sizeof(double)> bytes;
|
std::string cmd_right;
|
||||||
double data;
|
|
||||||
} value{};
|
cmd_left = std::to_string(left_wheel_vel_cmd / (2 * M_PI)) + " ";
|
||||||
|
cmd_right = std::to_string(-right_wheel_vel_cmd / (2 * M_PI));
|
||||||
|
|
||||||
try {
|
try {
|
||||||
odrive_serial_interface.Write("s;");
|
odrive_serial_interface.Write(cmd_left + cmd_right);
|
||||||
value.data = -left_wheel_vel_cmd / (2 * M_PI);
|
|
||||||
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
|
|
||||||
value.data = right_wheel_vel_cmd / (2 * M_PI);
|
|
||||||
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
|
|
||||||
|
|
||||||
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
|
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
|
||||||
return hardware_interface::return_type::OK;
|
return hardware_interface::return_type::OK;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,60 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.12)
|
|
||||||
project(mg_lidar)
|
|
||||||
|
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
|
||||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
set(PACKAGE_DEPS
|
|
||||||
rclcpp
|
|
||||||
tf2
|
|
||||||
tf2_ros
|
|
||||||
tf2_geometry_msgs
|
|
||||||
mg_msgs
|
|
||||||
sensor_msgs
|
|
||||||
)
|
|
||||||
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
|
|
||||||
include(FindPkgConfig)
|
|
||||||
pkg_search_module(LIBGLM REQUIRED glm)
|
|
||||||
|
|
||||||
foreach(PACKAGE ${PACKAGE_DEPS})
|
|
||||||
find_package(${PACKAGE} REQUIRED)
|
|
||||||
endforeach()
|
|
||||||
|
|
||||||
set(SOURCES
|
|
||||||
src/scanner.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
add_executable(mg_scanner ${SOURCES})
|
|
||||||
|
|
||||||
target_include_directories(
|
|
||||||
mg_scanner
|
|
||||||
PRIVATE
|
|
||||||
${LIBGLM_INCLUDE_DIRS}
|
|
||||||
include
|
|
||||||
)
|
|
||||||
|
|
||||||
ament_target_dependencies(mg_scanner ${PACKAGE_DEPS})
|
|
||||||
|
|
||||||
install( TARGETS
|
|
||||||
mg_scanner
|
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY
|
|
||||||
config
|
|
||||||
launch
|
|
||||||
DESTINATION share/${PROJECT_NAME}/
|
|
||||||
)
|
|
||||||
|
|
||||||
target_compile_features(mg_scanner PUBLIC
|
|
||||||
c_std_99
|
|
||||||
cxx_std_17
|
|
||||||
) # Require C99 and C++17
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
rplidar_node:
|
|
||||||
ros__parameters:
|
|
||||||
scan_mode: "ppbig"
|
|
||||||
topic_name: "base-link"
|
|
||||||
@ -1,28 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch.substitutions import PathJoinSubstitution
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.substitutions import FindPackageShare
|
|
||||||
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
|
|
||||||
basedir = FindPackageShare("mg_lidar")
|
|
||||||
|
|
||||||
lidar_config = PathJoinSubstitution([basedir, "config/lidar.yaml"])
|
|
||||||
|
|
||||||
return LaunchDescription([
|
|
||||||
Node(
|
|
||||||
package='mg_lidar',
|
|
||||||
executable='mg_scanner',
|
|
||||||
output="screen",
|
|
||||||
parameters=[lidar_config]
|
|
||||||
),
|
|
||||||
Node(
|
|
||||||
package='rplidar_ros',
|
|
||||||
executable='rplidar_composition',
|
|
||||||
output="screen",
|
|
||||||
parameters=[lidar_config]
|
|
||||||
),
|
|
||||||
])
|
|
||||||
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>mg_lidar</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>Lidar based opponent tracking</description>
|
|
||||||
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
|
||||||
<license>Apache 2.0</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
|
||||||
<depend>mg_msgs</depend>
|
|
||||||
<depend>std_msgs</depend>
|
|
||||||
<depend>tf2</depend>
|
|
||||||
<depend>tf2_ros</depend>
|
|
||||||
<depend>tf2_geometry_msgs</depend>
|
|
||||||
<depend>visualization_msgs</depend>
|
|
||||||
<depend>sensor_msgs</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,134 +0,0 @@
|
|||||||
#include "rclcpp/rclcpp.hpp"
|
|
||||||
|
|
||||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
|
||||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
|
||||||
#include "glm/glm.hpp"
|
|
||||||
|
|
||||||
#include "tf2_ros/buffer.h"
|
|
||||||
#include "tf2_ros/transform_listener.h"
|
|
||||||
#include "tf2/convert.hpp"
|
|
||||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
||||||
#include <glm/gtx/norm.hpp>
|
|
||||||
|
|
||||||
class MgScanner : public rclcpp::Node {
|
|
||||||
using LaserScan = sensor_msgs::msg::LaserScan;
|
|
||||||
using PointStamped = geometry_msgs::msg::PointStamped;
|
|
||||||
|
|
||||||
public:
|
|
||||||
MgScanner() : rclcpp::Node("EnemyScanner") {
|
|
||||||
gen_rotations();
|
|
||||||
scan_sup_ = create_subscription<LaserScan>(
|
|
||||||
"scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { process_scan(msg); });
|
|
||||||
enemy_pub_ = create_publisher<PointStamped>("/dynamicObstacle", rclcpp::QoS(1));
|
|
||||||
|
|
||||||
tf_buf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
|
|
||||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
rclcpp::Subscription<LaserScan>::SharedPtr scan_sup_;
|
|
||||||
rclcpp::Publisher<PointStamped>::SharedPtr enemy_pub_;
|
|
||||||
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
|
|
||||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
|
||||||
|
|
||||||
std::vector<glm::dmat2> rotations;
|
|
||||||
|
|
||||||
void gen_rotations() {
|
|
||||||
constexpr double min_angle = -3.1241393089294434;
|
|
||||||
constexpr double max_angle = 3.1415927410125732;
|
|
||||||
constexpr double angle_increment = 0.008714509196579456;
|
|
||||||
|
|
||||||
double curr = min_angle;
|
|
||||||
while (curr <= max_angle) {
|
|
||||||
rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
|
|
||||||
curr += angle_increment;
|
|
||||||
}
|
|
||||||
|
|
||||||
rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
|
|
||||||
}
|
|
||||||
|
|
||||||
glm::dvec3 pos_query() {
|
|
||||||
RCLCPP_ERROR(get_logger(), "Works to here");
|
|
||||||
try {
|
|
||||||
double x = NAN;
|
|
||||||
double y = NAN;
|
|
||||||
double rot = NAN;
|
|
||||||
|
|
||||||
auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
|
|
||||||
|
|
||||||
tf2::Transform t;
|
|
||||||
tf2::convert(tmsg.transform, t);
|
|
||||||
t.getBasis().getRPY(x, y, rot);
|
|
||||||
auto vec3 = tmsg.transform.translation;
|
|
||||||
|
|
||||||
return { vec3.x, vec3.y, rot };
|
|
||||||
|
|
||||||
} catch (const tf2::TransformException& e) {
|
|
||||||
RCLCPP_ERROR(get_logger(), "Got following error when looking up transform:\n\t%s", e.what());
|
|
||||||
}
|
|
||||||
return { 0, 0, 0 };
|
|
||||||
}
|
|
||||||
|
|
||||||
static bool part_of_clump(const glm::dvec2 prev, glm::dvec2 curr) {
|
|
||||||
return glm::length2(prev - curr) < 0.05 * 0.05;
|
|
||||||
}
|
|
||||||
|
|
||||||
void process_scan(LaserScan::ConstSharedPtr msg) {
|
|
||||||
// TODO: finish processing
|
|
||||||
const glm::dvec3 v = pos_query();
|
|
||||||
const glm::dvec2 pos = { v.x, v.y };
|
|
||||||
const glm::dmat2 rot = { glm::cos(v.z), glm::sin(v.z), -glm::sin(v.z), glm::cos(v.z) };
|
|
||||||
|
|
||||||
double mini = INFINITY;
|
|
||||||
|
|
||||||
glm::dvec2 mini_pos = { 0, 0 };
|
|
||||||
glm::dvec2 prev = { -1, -1 };
|
|
||||||
|
|
||||||
glm::dvec2 clump = { -1, -1 };
|
|
||||||
int clump_size = 0;
|
|
||||||
|
|
||||||
bool good_clump = false;
|
|
||||||
|
|
||||||
if (msg->ranges.size() != rotations.size()) {
|
|
||||||
RCLCPP_ERROR(get_logger(),
|
|
||||||
"The amount of rotations differs from amount of ranges(%lu != %lu)",
|
|
||||||
msg->ranges.size(),
|
|
||||||
rotations.size());
|
|
||||||
}
|
|
||||||
|
|
||||||
for (uint i = 0; i < msg->ranges.size(); i++) {
|
|
||||||
if (msg->intensities.at(i) < 35.0) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
const glm::dvec2 potential_pos = rot * rotations.at(i) * glm::dvec2{ msg->ranges.at(i), 0 } + pos;
|
|
||||||
if (0.1 < potential_pos.x && potential_pos.x < 2.9 && 0.1 < potential_pos.y && potential_pos.y < 1.9) {
|
|
||||||
if (!part_of_clump(prev, potential_pos)) {
|
|
||||||
clump_size = 0;
|
|
||||||
clump = { 0, 0 };
|
|
||||||
}
|
|
||||||
good_clump |= mini > msg->ranges.at(i);
|
|
||||||
clump += potential_pos;
|
|
||||||
clump_size++;
|
|
||||||
if (good_clump) {
|
|
||||||
mini = msg->ranges.at(i);
|
|
||||||
mini_pos = clump / (double)clump_size;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
prev = potential_pos;
|
|
||||||
}
|
|
||||||
if (mini < INFINITY) {
|
|
||||||
PointStamped m;
|
|
||||||
m.header.frame_id = "odom";
|
|
||||||
m.point.x = mini_pos.x;
|
|
||||||
m.point.y = mini_pos.y;
|
|
||||||
|
|
||||||
enemy_pub_->publish(m);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
int main(const int argc, const char** argv) {
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
rclcpp::spin(std::make_shared<MgScanner>());
|
|
||||||
rclcpp::shutdown();
|
|
||||||
}
|
|
||||||
@ -10,18 +10,10 @@ find_package(ament_cmake REQUIRED)
|
|||||||
find_package(rosidl_default_generators REQUIRED)
|
find_package(rosidl_default_generators REQUIRED)
|
||||||
|
|
||||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||||
"msg/Path.msg"
|
|
||||||
"msg/Point2d.msg"
|
"msg/Point2d.msg"
|
||||||
"action/MoveGlobal.action"
|
|
||||||
"action/MoveStraight.action"
|
"action/MoveStraight.action"
|
||||||
"action/MovePoint.action"
|
"action/MovePoint.action"
|
||||||
"action/LookAt.action"
|
"action/LookAt.action"
|
||||||
"action/Rotate.action"
|
|
||||||
"action/I2c.action"
|
|
||||||
"srv/CalcPath.srv"
|
|
||||||
"srv/SendDouble.srv"
|
|
||||||
"srv/SendPose2d.srv"
|
|
||||||
"srv/I2c.srv"
|
|
||||||
)
|
)
|
||||||
|
|
||||||
ament_package()
|
ament_package()
|
||||||
@ -1,5 +0,0 @@
|
|||||||
uint8 addr
|
|
||||||
uint8 data
|
|
||||||
---
|
|
||||||
uint8[] resp
|
|
||||||
---
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
float64[] x
|
|
||||||
float64[] y
|
|
||||||
|
|
||||||
float64 step 0.1
|
|
||||||
float64 tolerance 0.05
|
|
||||||
float64 max_wheel_speed 6.0
|
|
||||||
float64 max_angular 3.14
|
|
||||||
float64 max_vel 4
|
|
||||||
float64 pos_mult 14.0
|
|
||||||
float64 ornt_mult 3.0
|
|
||||||
float64 t_ornt_mult 0.1
|
|
||||||
float64 obst_mult_a 160
|
|
||||||
float64 obst_mult_b 4.0
|
|
||||||
float64 obst_mult_c 0
|
|
||||||
string[] ignore_tags []
|
|
||||||
---
|
|
||||||
uint8 SUCCESS=0
|
|
||||||
uint8 BLOCKED=1
|
|
||||||
uint8 TIMEOUT=2
|
|
||||||
uint8 MISALIGNED=3
|
|
||||||
uint8 UNKNOWN=254
|
|
||||||
|
|
||||||
uint8 error
|
|
||||||
---
|
|
||||||
float64 distance_passed
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
|
|
||||||
float64 angle
|
|
||||||
|
|
||||||
float64 step 0.01
|
|
||||||
float64 tolerance 0.001
|
|
||||||
float64 max_wheel_speed 3
|
|
||||||
float64 max_angular 3.14
|
|
||||||
float64 max_vel 2
|
|
||||||
float64 pos_mult 15
|
|
||||||
float64 ornt_mult 4
|
|
||||||
float64 t_ornt_mult 0.1
|
|
||||||
float64 obst_mult_a -5
|
|
||||||
float64 obst_mult_b -30
|
|
||||||
float64 obst_mult_c -9
|
|
||||||
string[] ignore_tags []
|
|
||||||
---
|
|
||||||
uint8 SUCCESS=0
|
|
||||||
uint8 BLOCKED=1
|
|
||||||
uint8 TIMEOUT=2
|
|
||||||
uint8 MISALIGNED=3
|
|
||||||
uint8 UNKNOWN=254
|
|
||||||
|
|
||||||
uint8 error
|
|
||||||
---
|
|
||||||
float64 distance_passed
|
|
||||||
@ -1 +0,0 @@
|
|||||||
int32[] indicies
|
|
||||||
@ -5,7 +5,7 @@
|
|||||||
<version>0.0.0</version>
|
<version>0.0.0</version>
|
||||||
<description>This contains various msg/action used within the project</description>
|
<description>This contains various msg/action used within the project</description>
|
||||||
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
||||||
<license>MIT</license>
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
|||||||
@ -1,4 +0,0 @@
|
|||||||
float64[] x
|
|
||||||
float64[] y
|
|
||||||
---
|
|
||||||
int32[] indicies
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
uint8 addr
|
|
||||||
uint8 data
|
|
||||||
---
|
|
||||||
uint8[] resp
|
|
||||||
@ -1,2 +0,0 @@
|
|||||||
float64 data
|
|
||||||
---
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
float64 x
|
|
||||||
float64 y
|
|
||||||
float64 theta
|
|
||||||
---
|
|
||||||
@ -13,7 +13,6 @@ find_package(geometry_msgs REQUIRED)
|
|||||||
find_package(tf2 REQUIRED)
|
find_package(tf2 REQUIRED)
|
||||||
find_package(tf2_ros REQUIRED)
|
find_package(tf2_ros REQUIRED)
|
||||||
find_package(tf2_geometry_msgs REQUIRED)
|
find_package(tf2_geometry_msgs REQUIRED)
|
||||||
find_package(mg_obstacles REQUIRED)
|
|
||||||
|
|
||||||
include(FindPkgConfig)
|
include(FindPkgConfig)
|
||||||
pkg_search_module(LIBGLM REQUIRED glm)
|
pkg_search_module(LIBGLM REQUIRED glm)
|
||||||
@ -27,12 +26,10 @@ set(PACKAGE_DEPS
|
|||||||
tf2
|
tf2
|
||||||
tf2_ros
|
tf2_ros
|
||||||
tf2_geometry_msgs
|
tf2_geometry_msgs
|
||||||
mg_obstacles
|
|
||||||
)
|
)
|
||||||
|
|
||||||
add_executable(mg_nav_server
|
add_executable(mg_nav_server
|
||||||
src/mg_navigation.cpp
|
src/mg_navigation.cpp
|
||||||
src/path_buffer.cpp
|
|
||||||
)
|
)
|
||||||
ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})
|
ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})
|
||||||
|
|
||||||
|
|||||||
@ -1,8 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "handlers/dwa_core.hpp"
|
|
||||||
#include "handlers/dwa_point.hpp"
|
|
||||||
#include "handlers/dwa_global.hpp"
|
|
||||||
#include "handlers/dwa_forward.hpp"
|
|
||||||
#include "handlers/dwa_lookat.hpp"
|
|
||||||
#include "handlers/dwa_rotate.hpp"
|
|
||||||
@ -1,106 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#include "handlers/dwa_core.hpp"
|
|
||||||
|
|
||||||
#include "glm/gtx/norm.hpp"
|
|
||||||
#include "glm/gtx/rotate_vector.hpp"
|
|
||||||
#include "glm/gtx/vector_angle.hpp"
|
|
||||||
|
|
||||||
#define LOOKAHEAD 0.2
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline bool DWA<MgNavigationServer::MoveGlobal>::target_check() {
|
|
||||||
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline void DWA<MgNavigationServer::MoveGlobal>::target_init() {
|
|
||||||
baseNode.path_buffer()->update_path_block(goal);
|
|
||||||
target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
|
|
||||||
|
|
||||||
dimy = 4;
|
|
||||||
dimx = 11;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline void DWA<MgNavigationServer::MoveGlobal>::target_update() {
|
|
||||||
baseNode.path_buffer()->update_path();
|
|
||||||
target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline bool DWA<MgNavigationServer::MoveGlobal>::condition_check() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline double DWA<MgNavigationServer::MoveGlobal>::calc_score(int vl, int vr) {
|
|
||||||
auto [v, w] = to_vel(step * vl, step * vr);
|
|
||||||
const double delta = 0.3;
|
|
||||||
glm::dvec2 n_pos;
|
|
||||||
double n_theta = NAN;
|
|
||||||
double score = 0;
|
|
||||||
|
|
||||||
n_theta = w * delta;
|
|
||||||
|
|
||||||
if (n_theta <= 1e-6) { //NOLINT
|
|
||||||
n_theta += theta;
|
|
||||||
const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
|
|
||||||
n_pos = dp + pos;
|
|
||||||
} else {
|
|
||||||
n_theta += theta;
|
|
||||||
const double r = v / w;
|
|
||||||
n_pos.x = r * (glm::sin(n_theta) - glm::sin(theta));
|
|
||||||
n_pos.y = -r * (glm::cos(n_theta) - glm::cos(theta));
|
|
||||||
n_pos += pos;
|
|
||||||
}
|
|
||||||
|
|
||||||
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
|
|
||||||
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
|
|
||||||
|
|
||||||
// const double angl = glm::angle(face, glm::normalize(target_pos - pos));
|
|
||||||
// const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
|
|
||||||
|
|
||||||
const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
|
|
||||||
const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
|
|
||||||
|
|
||||||
const double dist = baseNode.obstacle_manager()->box_colide(n_pos, { 0.29, 0.33 }, n_theta);
|
|
||||||
const double dist2 = baseNode.obstacle_manager()->dist_to_nearest(n_pos) - 0.22;
|
|
||||||
const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
|
|
||||||
const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);
|
|
||||||
|
|
||||||
// RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
|
|
||||||
// RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
|
|
||||||
|
|
||||||
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
|
|
||||||
// score += goal->ornt_mult * (angl - n_angl);
|
|
||||||
score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
|
|
||||||
score -= goal->obst_mult_a * obstacle_scoreA;
|
|
||||||
score -= goal->obst_mult_b * obstacle_scoreB;
|
|
||||||
|
|
||||||
return score;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline void DWA<MgNavigationServer::MoveGlobal>::populate_candidates(std::vector<int>& vl,
|
|
||||||
std::vector<int>& vr,
|
|
||||||
int dimx,
|
|
||||||
int dimy) {
|
|
||||||
vl.clear();
|
|
||||||
vr.clear();
|
|
||||||
|
|
||||||
for (int i = -dimx / 2; i <= dimx / 2; i++) {
|
|
||||||
for (int j = -dimy / 2; j <= dimy / 2; j++) {
|
|
||||||
auto [v, w] = to_vel(step * (cvl + i), step * (cvr + j));
|
|
||||||
|
|
||||||
if (step * abs(cvl + i) <= goal->max_wheel_speed && step * abs(cvr + j) <= goal->max_wheel_speed
|
|
||||||
&& glm::abs(v) < goal->max_vel && glm::abs(w) < goal->max_angular) {
|
|
||||||
vl.push_back(cvl + i);
|
|
||||||
vr.push_back(cvr + j);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,66 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#include "handlers/dwa_core.hpp"
|
|
||||||
|
|
||||||
#include "glm/gtx/norm.hpp"
|
|
||||||
#include "glm/gtx/rotate_vector.hpp"
|
|
||||||
#include "glm/gtx/vector_angle.hpp"
|
|
||||||
#include <glm/gtc/constants.hpp>
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline bool DWA<MgNavigationServer::Rotate>::target_check() {
|
|
||||||
|
|
||||||
const double a = glm::abs(theta - target_ornt);
|
|
||||||
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
|
|
||||||
|
|
||||||
return b > goal->tolerance;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline void DWA<MgNavigationServer::Rotate>::target_init() {
|
|
||||||
target_ornt = goal->angle;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline bool DWA<MgNavigationServer::Rotate>::condition_check() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline double DWA<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
|
|
||||||
constexpr double delta = 0.5;
|
|
||||||
const auto [v, w] = to_vel(step * vl, step * vr);
|
|
||||||
const double n_theta = theta + w * delta;
|
|
||||||
|
|
||||||
double dist_old = glm::abs(target_ornt - theta);
|
|
||||||
double dist_new = glm::abs(target_ornt - n_theta);
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
const double score = goal->ornt_mult * (dist_old - dist_new);
|
|
||||||
|
|
||||||
return score;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <>
|
|
||||||
inline void DWA<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
|
|
||||||
std::vector<int>& vr,
|
|
||||||
int dimx,
|
|
||||||
int) {
|
|
||||||
vl.clear();
|
|
||||||
vr.clear();
|
|
||||||
|
|
||||||
for (int i = -dimx / 2; i <= dimx / 2; i++) {
|
|
||||||
auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));
|
|
||||||
|
|
||||||
if (step * abs(cvl - i) <= goal->max_wheel_speed && step * abs(cvr + i) <= goal->max_wheel_speed
|
|
||||||
&& glm::abs(w) < goal->max_angular) {
|
|
||||||
vl.push_back(cvl - i);
|
|
||||||
vr.push_back(cvr + i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
20
mg_navigation/include/handlers/dwm.hpp
Normal file
20
mg_navigation/include/handlers/dwm.hpp
Normal file
@ -0,0 +1,20 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "handlers/dwm_core.hpp"
|
||||||
|
#include "handlers/dwm_point.hpp"
|
||||||
|
#include "handlers/dwm_forward.hpp"
|
||||||
|
#include "handlers/dwm_lookat.hpp"
|
||||||
@ -1,3 +1,17 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "glm/glm.hpp"
|
#include "glm/glm.hpp"
|
||||||
@ -30,7 +44,7 @@
|
|||||||
namespace mg {
|
namespace mg {
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
class DWA {
|
class DWM {
|
||||||
public:
|
public:
|
||||||
using Goal = typename T::Goal;
|
using Goal = typename T::Goal;
|
||||||
using Result = typename T::Result;
|
using Result = typename T::Result;
|
||||||
@ -62,7 +76,7 @@ namespace mg {
|
|||||||
glm::dvec2 pos;
|
glm::dvec2 pos;
|
||||||
double theta = 0;
|
double theta = 0;
|
||||||
|
|
||||||
DWA(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
|
DWM(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
|
||||||
|
|
||||||
void execute();
|
void execute();
|
||||||
|
|
||||||
@ -88,7 +102,7 @@ namespace mg {
|
|||||||
};
|
};
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
DWA<T>::DWA(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
|
DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
|
||||||
baseNode(mns),
|
baseNode(mns),
|
||||||
hgoal(g),
|
hgoal(g),
|
||||||
pub_twist(mns.pub_twist),
|
pub_twist(mns.pub_twist),
|
||||||
@ -99,20 +113,16 @@ namespace mg {
|
|||||||
pos(0, 0) { }
|
pos(0, 0) { }
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void DWA<T>::execute() {
|
void DWM<T>::execute() {
|
||||||
std::vector<int> spacevl(dimx * dimy);
|
std::vector<int> spacevl(dimx * dimy);
|
||||||
std::vector<int> spacevr(dimx * dimy);
|
std::vector<int> spacevr(dimx * dimy);
|
||||||
|
|
||||||
pos_query();
|
pos_query();
|
||||||
target_init();
|
target_init();
|
||||||
|
|
||||||
rclcpp::Time elapsed = baseNode.get_clock()->now();
|
|
||||||
rclcpp::Rate rate(UPDATE_RATE);
|
rclcpp::Rate rate(UPDATE_RATE);
|
||||||
|
|
||||||
while (target_check() && rclcpp::ok()) {
|
while (target_check() && rclcpp::ok()) {
|
||||||
target_update();
|
|
||||||
baseNode.obstacle_manager()->update_dynamic();
|
|
||||||
baseNode.obstacle_manager()->update_static();
|
|
||||||
if (hgoal->is_canceling()) {
|
if (hgoal->is_canceling()) {
|
||||||
cancel();
|
cancel();
|
||||||
return;
|
return;
|
||||||
@ -125,14 +135,11 @@ namespace mg {
|
|||||||
pos_query();
|
pos_query();
|
||||||
populate_candidates(spacevl, spacevr, dimx, dimy);
|
populate_candidates(spacevl, spacevr, dimx, dimy);
|
||||||
|
|
||||||
double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
|
double b_score = calc_score(spacevl[0], spacevr[0]);
|
||||||
int b_ind = -1;
|
uint b_ind = 0;
|
||||||
|
|
||||||
for (uint i = 1; i < spacevl.size(); i++) {
|
for (uint i = 1; i < spacevl.size(); i++) {
|
||||||
const double score = calc_score(spacevl[i], spacevr[i]);
|
const double score = calc_score(spacevl[i], spacevr[i]);
|
||||||
if (score == NAN) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (score > b_score) {
|
if (score > b_score) {
|
||||||
b_score = score;
|
b_score = score;
|
||||||
b_ind = i;
|
b_ind = i;
|
||||||
@ -141,21 +148,17 @@ namespace mg {
|
|||||||
|
|
||||||
calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
|
calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
|
||||||
|
|
||||||
if (b_ind >= 0) {
|
|
||||||
cvl = spacevl[b_ind];
|
cvl = spacevl[b_ind];
|
||||||
cvr = spacevr[b_ind];
|
cvr = spacevr[b_ind];
|
||||||
} else {
|
|
||||||
cvl = 0;
|
|
||||||
cvr = 0;
|
|
||||||
}
|
|
||||||
send_speed(step * cvl, step * cvr);
|
send_speed(step * cvl, step * cvr);
|
||||||
|
|
||||||
rate.sleep();
|
rate.sleep();
|
||||||
}
|
}
|
||||||
succeed();
|
succeed();
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void DWA<T>::pos_query() {
|
void DWM<T>::pos_query() {
|
||||||
try {
|
try {
|
||||||
double x = NAN;
|
double x = NAN;
|
||||||
double y = NAN;
|
double y = NAN;
|
||||||
@ -175,7 +178,7 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void DWA<T>::send_speed(double vl, double vr) {
|
void DWM<T>::send_speed(double vl, double vr) {
|
||||||
auto [v, w] = to_vel(vl, vr);
|
auto [v, w] = to_vel(vl, vr);
|
||||||
Geometry::TwistStamped twist;
|
Geometry::TwistStamped twist;
|
||||||
twist.twist.angular.z = w;
|
twist.twist.angular.z = w;
|
||||||
@ -185,7 +188,7 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void DWA<T>::abort(int error) {
|
void DWM<T>::abort(int error) {
|
||||||
baseNode.mtx.lock();
|
baseNode.mtx.lock();
|
||||||
send_speed(0, 0);
|
send_speed(0, 0);
|
||||||
auto x = std::make_shared<Result>();
|
auto x = std::make_shared<Result>();
|
||||||
@ -194,7 +197,7 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void DWA<T>::succeed() {
|
void DWM<T>::succeed() {
|
||||||
baseNode.mtx.lock();
|
baseNode.mtx.lock();
|
||||||
send_speed(0, 0);
|
send_speed(0, 0);
|
||||||
auto x = std::make_shared<Result>();
|
auto x = std::make_shared<Result>();
|
||||||
@ -203,7 +206,7 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void DWA<T>::cancel() {
|
void DWM<T>::cancel() {
|
||||||
baseNode.mtx.lock();
|
baseNode.mtx.lock();
|
||||||
send_speed(0, 0);
|
send_speed(0, 0);
|
||||||
auto x = std::make_shared<Result>();
|
auto x = std::make_shared<Result>();
|
||||||
@ -1,5 +1,19 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
#include "handlers/dwa_core.hpp"
|
#include "handlers/dwm_core.hpp"
|
||||||
|
|
||||||
#include "glm/gtx/norm.hpp"
|
#include "glm/gtx/norm.hpp"
|
||||||
#include "glm/gtx/rotate_vector.hpp"
|
#include "glm/gtx/rotate_vector.hpp"
|
||||||
@ -8,22 +22,22 @@
|
|||||||
namespace mg {
|
namespace mg {
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline bool DWA<MgNavigationServer::MoveStraight>::target_check() {
|
inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
|
||||||
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::MoveStraight>::target_init() {
|
inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
|
||||||
target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
|
target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline bool DWA<MgNavigationServer::MoveStraight>::condition_check() {
|
inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline double DWA<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
|
inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
|
||||||
constexpr double delta = 0.8;
|
constexpr double delta = 0.8;
|
||||||
|
|
||||||
auto [v, w] = to_vel(step * vl, step * vr);
|
auto [v, w] = to_vel(step * vl, step * vr);
|
||||||
@ -37,7 +51,7 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
|
inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
|
||||||
std::vector<int>& vr,
|
std::vector<int>& vr,
|
||||||
int dimx,
|
int dimx,
|
||||||
int) {
|
int) {
|
||||||
@ -1,5 +1,19 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
#include "handlers/dwa_core.hpp"
|
#include "handlers/dwm_core.hpp"
|
||||||
|
|
||||||
#include "glm/gtx/norm.hpp"
|
#include "glm/gtx/norm.hpp"
|
||||||
#include "glm/gtx/rotate_vector.hpp"
|
#include "glm/gtx/rotate_vector.hpp"
|
||||||
@ -9,17 +23,16 @@
|
|||||||
namespace mg {
|
namespace mg {
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline bool DWA<MgNavigationServer::LookAt>::target_check() {
|
inline bool DWM<MgNavigationServer::LookAt>::target_check() {
|
||||||
|
|
||||||
const double a = glm::abs(theta - target_ornt);
|
const double a = glm::abs(theta - target_ornt);
|
||||||
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
|
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
|
||||||
const double c = (b > glm::pi<double>() / 2) ? glm::pi<double>() - b : b;
|
|
||||||
|
|
||||||
return c > goal->tolerance;
|
return b > goal->tolerance;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::LookAt>::target_init() {
|
inline void DWM<MgNavigationServer::LookAt>::target_init() {
|
||||||
|
|
||||||
target_pos = glm::vec2(goal->x, goal->y);
|
target_pos = glm::vec2(goal->x, goal->y);
|
||||||
|
|
||||||
@ -31,7 +44,7 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::LookAt>::target_update() {
|
inline void DWM<MgNavigationServer::LookAt>::target_update() {
|
||||||
|
|
||||||
const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
|
const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
|
||||||
const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
|
const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
|
||||||
@ -41,12 +54,12 @@ namespace mg {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline bool DWA<MgNavigationServer::LookAt>::condition_check() {
|
inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline double DWA<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
|
inline double DWM<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
|
||||||
constexpr double delta = 0.5;
|
constexpr double delta = 0.5;
|
||||||
const auto [v, w] = to_vel(step * vl, step * vr);
|
const auto [v, w] = to_vel(step * vl, step * vr);
|
||||||
const double n_theta = theta + w * delta;
|
const double n_theta = theta + w * delta;
|
||||||
@ -57,16 +70,13 @@ namespace mg {
|
|||||||
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
|
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
|
||||||
dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
|
dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
|
||||||
|
|
||||||
dist_old = (dist_old > glm::pi<double>() / 2) ? glm::pi<double>() - dist_old : dist_old;
|
|
||||||
dist_new = (dist_new > glm::pi<double>() / 2) ? glm::pi<double>() - dist_new : dist_new;
|
|
||||||
|
|
||||||
const double score = goal->ornt_mult * (dist_old - dist_new);
|
const double score = goal->ornt_mult * (dist_old - dist_new);
|
||||||
|
|
||||||
return score;
|
return score;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
|
inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
|
||||||
std::vector<int>& vr,
|
std::vector<int>& vr,
|
||||||
int dimx,
|
int dimx,
|
||||||
int) {
|
int) {
|
||||||
@ -1,5 +1,19 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
#include "handlers/dwa_core.hpp"
|
#include "handlers/dwm_core.hpp"
|
||||||
|
|
||||||
#include "glm/gtx/norm.hpp"
|
#include "glm/gtx/norm.hpp"
|
||||||
#include "glm/gtx/rotate_vector.hpp"
|
#include "glm/gtx/rotate_vector.hpp"
|
||||||
@ -8,30 +22,28 @@
|
|||||||
namespace mg {
|
namespace mg {
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline bool DWA<MgNavigationServer::MovePoint>::target_check() {
|
inline bool DWM<MgNavigationServer::MovePoint>::target_check() {
|
||||||
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::MovePoint>::target_update() {
|
inline void DWM<MgNavigationServer::MovePoint>::target_update() {
|
||||||
target_pos = glm::dvec2(goal->x, goal->y);
|
target_pos = glm::dvec2(goal->x, goal->y);
|
||||||
dimy = 8;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline bool DWA<MgNavigationServer::MovePoint>::condition_check() {
|
inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
|
inline double DWM<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
|
||||||
auto [v, w] = to_vel(step * vl, step * vr);
|
auto [v, w] = to_vel(step * vl, step * vr);
|
||||||
const double delta = 0.3;
|
const double delta = 0.5;
|
||||||
glm::dvec2 n_pos;
|
glm::dvec2 n_pos;
|
||||||
double n_theta = NAN;
|
double n_theta = NAN;
|
||||||
double score = 0;
|
double score = 0;
|
||||||
|
|
||||||
// The angle we will be facing after lookahead
|
|
||||||
n_theta = w * delta;
|
n_theta = w * delta;
|
||||||
|
|
||||||
if (n_theta <= 1e-6) { //NOLINT
|
if (n_theta <= 1e-6) { //NOLINT
|
||||||
@ -49,25 +61,17 @@ namespace mg {
|
|||||||
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
|
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
|
||||||
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
|
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
|
||||||
|
|
||||||
const double dist = baseNode.obstacle_manager()->box_colide(
|
const double angl = glm::angle(face, glm::normalize(target_pos - pos));
|
||||||
n_pos, { 0.29, 0.33 }, n_theta, ObstacleManager::ObstacleMask::DYNAMIC);
|
const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
|
||||||
|
|
||||||
if (dist < 0.01) {
|
|
||||||
return NAN;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Calculate angle to goal post/pre movement
|
|
||||||
const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
|
|
||||||
const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
|
|
||||||
|
|
||||||
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
|
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
|
||||||
score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
|
score += goal->ornt_mult * (angl - n_angl);
|
||||||
|
|
||||||
return score;
|
return score;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
inline void DWA<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
|
inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
|
||||||
std::vector<int>& vr,
|
std::vector<int>& vr,
|
||||||
int dimx,
|
int dimx,
|
||||||
int dimy) {
|
int dimy) {
|
||||||
@ -1,3 +1,17 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <thread>
|
#include <thread>
|
||||||
@ -8,7 +22,6 @@
|
|||||||
#include "mg_msgs/action/move_point.hpp"
|
#include "mg_msgs/action/move_point.hpp"
|
||||||
#include "mg_msgs/action/move_straight.hpp"
|
#include "mg_msgs/action/move_straight.hpp"
|
||||||
#include "mg_msgs/action/look_at.hpp"
|
#include "mg_msgs/action/look_at.hpp"
|
||||||
#include "mg_msgs/action/rotate.hpp"
|
|
||||||
|
|
||||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||||
@ -17,10 +30,6 @@
|
|||||||
#include "tf2_ros/transform_listener.h"
|
#include "tf2_ros/transform_listener.h"
|
||||||
#include "tf2_ros/buffer.h"
|
#include "tf2_ros/buffer.h"
|
||||||
|
|
||||||
#include "mg_obstacles/mg_obstacles.hpp"
|
|
||||||
|
|
||||||
#include "path_buffer.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
namespace mg {
|
||||||
|
|
||||||
namespace Geometry = geometry_msgs::msg;
|
namespace Geometry = geometry_msgs::msg;
|
||||||
@ -29,18 +38,14 @@ namespace mg {
|
|||||||
public:
|
public:
|
||||||
RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
|
RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
|
||||||
using MovePoint = mg_msgs::action::MovePoint;
|
using MovePoint = mg_msgs::action::MovePoint;
|
||||||
using MoveGlobal = mg_msgs::action::MoveGlobal;
|
|
||||||
using MoveStraight = mg_msgs::action::MoveStraight;
|
using MoveStraight = mg_msgs::action::MoveStraight;
|
||||||
using LookAt = mg_msgs::action::LookAt;
|
using LookAt = mg_msgs::action::LookAt;
|
||||||
using Rotate = mg_msgs::action::Rotate;
|
|
||||||
|
|
||||||
rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
|
rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
|
||||||
|
|
||||||
rclcpp_action::Server<MoveGlobal>::SharedPtr sv_move_global;
|
rclcpp_action::Server<LookAt>::SharedPtr sv_look_at;
|
||||||
rclcpp_action::Server<MovePoint>::SharedPtr sv_move_point;
|
rclcpp_action::Server<MovePoint>::SharedPtr sv_move_point;
|
||||||
rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
|
rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
|
||||||
rclcpp_action::Server<LookAt>::SharedPtr sv_look_at;
|
|
||||||
rclcpp_action::Server<Rotate>::SharedPtr sv_rotate;
|
|
||||||
|
|
||||||
rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
|
rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
|
||||||
tf2_ros::Buffer::SharedPtr tf2_buffer;
|
tf2_ros::Buffer::SharedPtr tf2_buffer;
|
||||||
@ -49,15 +54,9 @@ namespace mg {
|
|||||||
|
|
||||||
MgNavigationServer(rclcpp::NodeOptions& opts);
|
MgNavigationServer(rclcpp::NodeOptions& opts);
|
||||||
|
|
||||||
std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
|
|
||||||
std::shared_ptr<ObstacleManager>& obstacle_manager() { return obstacle_manager_; }
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool is_processing = false;
|
bool is_processing = false;
|
||||||
|
|
||||||
std::shared_ptr<PathBuffer> path_buffer_;
|
|
||||||
std::shared_ptr<ObstacleManager> obstacle_manager_;
|
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
|
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
|
||||||
typename T::Goal::ConstSharedPtr,
|
typename T::Goal::ConstSharedPtr,
|
||||||
|
|||||||
@ -1,60 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <vector>
|
|
||||||
#include <mutex>
|
|
||||||
#include <thread>
|
|
||||||
#include <glm/glm.hpp>
|
|
||||||
|
|
||||||
#include "mg_msgs/srv/calc_path.hpp"
|
|
||||||
#include "mg_msgs/action/move_global.hpp"
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
|
||||||
|
|
||||||
#define AREAX 3000
|
|
||||||
#define AREAY 2000
|
|
||||||
#define GRID_Y 66
|
|
||||||
#define GRID_X 106
|
|
||||||
|
|
||||||
#define MIN_X 175
|
|
||||||
#define MIN_Y 175
|
|
||||||
|
|
||||||
#define MAX_X 2825
|
|
||||||
#define MAX_Y 1825
|
|
||||||
|
|
||||||
#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
|
|
||||||
#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
inline glm::vec2 GridToCoords(const glm::ivec2 grid) {
|
|
||||||
return glm::vec2{ grid.x, grid.y } * glm::vec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::vec2{ MIN_X, MIN_Y };
|
|
||||||
}
|
|
||||||
inline glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
|
|
||||||
inline glm::vec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)) / 1000.0F; }
|
|
||||||
|
|
||||||
class PathBuffer {
|
|
||||||
using PathService = mg_msgs::srv::CalcPath;
|
|
||||||
using PathGoal = mg_msgs::action::MoveGlobal::Goal::ConstSharedPtr;
|
|
||||||
using PathFuture = rclcpp::Client<PathService>::SharedFuture;
|
|
||||||
|
|
||||||
public:
|
|
||||||
PathBuffer(rclcpp::Node* node);
|
|
||||||
// ~PathBuffer();
|
|
||||||
|
|
||||||
glm::vec2 get_next(glm::vec2 pos, const double lookahead);
|
|
||||||
|
|
||||||
bool update_path(PathGoal goal = nullptr);
|
|
||||||
void update_path_block(PathGoal goal = nullptr);
|
|
||||||
|
|
||||||
private:
|
|
||||||
int current;
|
|
||||||
|
|
||||||
rclcpp::Node* node_;
|
|
||||||
|
|
||||||
PathGoal goal_;
|
|
||||||
PathService::Response::ConstSharedPtr path_msg_;
|
|
||||||
|
|
||||||
rclcpp::Client<PathService>::Client::SharedPtr client_;
|
|
||||||
|
|
||||||
PathFuture resp_;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -17,7 +17,6 @@
|
|||||||
<depend>tf2</depend>
|
<depend>tf2</depend>
|
||||||
<depend>tf2_ros</depend>
|
<depend>tf2_ros</depend>
|
||||||
<depend>tf2_geometry_msgs</depend>
|
<depend>tf2_geometry_msgs</depend>
|
||||||
<depend>mg_obstacles</depend>
|
|
||||||
|
|
||||||
|
|
||||||
<build_depend>libglm-dev</build_depend>
|
<build_depend>libglm-dev</build_depend>
|
||||||
|
|||||||
@ -1,10 +1,25 @@
|
|||||||
|
/** Copyright 2025 The Magrob Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "mg_navigation.hpp"
|
#include "mg_navigation.hpp"
|
||||||
#include "handlers/dwa.hpp"
|
#include "handlers/dwm.hpp"
|
||||||
|
|
||||||
namespace mg {
|
namespace mg {
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
@ -39,7 +54,7 @@ namespace mg {
|
|||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
|
void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
|
||||||
DWA<T> dwm = DWA<T>(gh, tf2_buffer, *this);
|
DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
|
||||||
dwm.execute();
|
dwm.execute();
|
||||||
is_processing = false;
|
is_processing = false;
|
||||||
mtx.unlock();
|
mtx.unlock();
|
||||||
@ -49,21 +64,9 @@ namespace mg {
|
|||||||
tf2_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
|
tf2_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
|
||||||
tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);
|
tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);
|
||||||
|
|
||||||
path_buffer_ = std::make_shared<PathBuffer>(this);
|
|
||||||
|
|
||||||
pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
|
pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
|
||||||
|
|
||||||
obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf2_buffer);
|
|
||||||
|
|
||||||
using namespace std::placeholders;
|
using namespace std::placeholders;
|
||||||
|
|
||||||
sv_move_global = rclcpp_action::create_server<MoveGlobal>(
|
|
||||||
this,
|
|
||||||
"MoveGlobal",
|
|
||||||
std::bind(&MgNavigationServer::handle_goal<MoveGlobal>, this, _1, _2, "MoveGlobal"),
|
|
||||||
std::bind(&MgNavigationServer::handle_cancel<MoveGlobal>, this, _1, "MoveGlobal"),
|
|
||||||
std::bind(&MgNavigationServer::handle_accepted<MoveGlobal>, this, _1, "MoveGlobal"));
|
|
||||||
|
|
||||||
sv_move_point = rclcpp_action::create_server<MovePoint>(
|
sv_move_point = rclcpp_action::create_server<MovePoint>(
|
||||||
this,
|
this,
|
||||||
"MovePoint",
|
"MovePoint",
|
||||||
@ -71,13 +74,6 @@ namespace mg {
|
|||||||
std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
|
std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
|
||||||
std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));
|
std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));
|
||||||
|
|
||||||
sv_move_straight = rclcpp_action::create_server<MoveStraight>(
|
|
||||||
this,
|
|
||||||
"MoveStraight",
|
|
||||||
std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
|
|
||||||
std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
|
|
||||||
std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
|
|
||||||
|
|
||||||
sv_look_at = rclcpp_action::create_server<LookAt>(
|
sv_look_at = rclcpp_action::create_server<LookAt>(
|
||||||
this,
|
this,
|
||||||
"LookAt",
|
"LookAt",
|
||||||
@ -85,12 +81,12 @@ namespace mg {
|
|||||||
std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
|
std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
|
||||||
std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));
|
std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));
|
||||||
|
|
||||||
sv_rotate = rclcpp_action::create_server<Rotate>(
|
sv_move_straight = rclcpp_action::create_server<MoveStraight>(
|
||||||
this,
|
this,
|
||||||
"Rotate",
|
"MoveStraight",
|
||||||
std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "Rotate"),
|
std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
|
||||||
std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "Rotate"),
|
std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
|
||||||
std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "Rotate"));
|
std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
@ -1,82 +0,0 @@
|
|||||||
#include "path_buffer.hpp"
|
|
||||||
|
|
||||||
#include <glm/gtx/norm.hpp>
|
|
||||||
|
|
||||||
#include <chrono>
|
|
||||||
using namespace std::chrono_literals;
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) {
|
|
||||||
client_ = node_->create_client<PathService>("GeneratePath");
|
|
||||||
}
|
|
||||||
|
|
||||||
glm::vec2 PathBuffer::get_next(glm::vec2 pos, const double lookahead) {
|
|
||||||
if (current < 0) {
|
|
||||||
current++;
|
|
||||||
while (current < (int)path_msg_->indicies.size()
|
|
||||||
&& glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) > lookahead * lookahead) {
|
|
||||||
current++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
while (current < (int)path_msg_->indicies.size()
|
|
||||||
&& glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) {
|
|
||||||
current++;
|
|
||||||
}
|
|
||||||
if (current == (int)path_msg_->indicies.size()) {
|
|
||||||
double best = 10000000;
|
|
||||||
int id = 0;
|
|
||||||
|
|
||||||
for (int i = 0; i < (int)goal_->x.size(); i++) {
|
|
||||||
const double dist2 = glm::distance2({ goal_->x[i], goal_->y[i] }, pos);
|
|
||||||
if (dist2 < best) {
|
|
||||||
best = dist2;
|
|
||||||
id = i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return { goal_->x[id], goal_->y[id] };
|
|
||||||
}
|
|
||||||
return IdToCoords(path_msg_->indicies[current]);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool PathBuffer::update_path(PathGoal goal) {
|
|
||||||
if (goal) {
|
|
||||||
goal_ = goal;
|
|
||||||
client_->prune_pending_requests();
|
|
||||||
auto req = std::make_shared<PathService::Request>();
|
|
||||||
req->x = goal_->x;
|
|
||||||
req->y = goal_->y;
|
|
||||||
resp_ = client_->async_send_request(req).share();
|
|
||||||
}
|
|
||||||
if (resp_.wait_for(0s) == std::future_status::ready) {
|
|
||||||
|
|
||||||
path_msg_ = resp_.get();
|
|
||||||
auto req = std::make_shared<PathService::Request>();
|
|
||||||
req->x = goal_->x;
|
|
||||||
req->y = goal_->y;
|
|
||||||
resp_ = client_->async_send_request(req).share();
|
|
||||||
current = -1;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void PathBuffer::update_path_block(PathGoal goal) {
|
|
||||||
if (goal) {
|
|
||||||
goal_ = goal;
|
|
||||||
client_->prune_pending_requests();
|
|
||||||
auto req = std::make_shared<PathService::Request>();
|
|
||||||
req->x = goal_->x;
|
|
||||||
req->y = goal_->y;
|
|
||||||
resp_ = client_->async_send_request(req).share();
|
|
||||||
}
|
|
||||||
|
|
||||||
resp_.wait();
|
|
||||||
path_msg_ = resp_.get();
|
|
||||||
auto req = std::make_shared<PathService::Request>();
|
|
||||||
req->x = goal_->x;
|
|
||||||
req->y = goal_->y;
|
|
||||||
resp_ = client_->async_send_request(req).share();
|
|
||||||
current = -1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,84 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.12)
|
|
||||||
|
|
||||||
project(mg_obstacles)
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "CLANG")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
|
|
||||||
set(PACKAGE_DEPS
|
|
||||||
rclcpp
|
|
||||||
ament_index_cpp
|
|
||||||
mg_msgs
|
|
||||||
geometry_msgs
|
|
||||||
std_msgs
|
|
||||||
tf2
|
|
||||||
tf2_ros
|
|
||||||
tf2_geometry_msgs
|
|
||||||
jsoncpp
|
|
||||||
)
|
|
||||||
|
|
||||||
foreach(PACKAGE ${PACKAGE_DEPS})
|
|
||||||
find_package(${PACKAGE} REQUIRED)
|
|
||||||
endforeach(PACKAGE ${PACKAGE_DEPS})
|
|
||||||
|
|
||||||
include(FindPkgConfig)
|
|
||||||
pkg_check_modules(JSONCPP jsoncpp)
|
|
||||||
# pkg_search_module(LIBGLM REQUIRED glm)
|
|
||||||
|
|
||||||
set(SOURCES
|
|
||||||
src/mg_obstacles.cpp
|
|
||||||
src/sdf.cpp
|
|
||||||
src/static_obstacle.cpp
|
|
||||||
src/permanent_obstacle.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
add_library(
|
|
||||||
mg_obstacles
|
|
||||||
SHARED
|
|
||||||
${SOURCES}
|
|
||||||
)
|
|
||||||
|
|
||||||
target_include_directories(
|
|
||||||
mg_obstacles
|
|
||||||
PRIVATE
|
|
||||||
${LIBGLM_INCLUDE_DIRS}
|
|
||||||
${JSONCPP_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
|
|
||||||
target_link_libraries(mg_obstacles ${JSONCPP_LINK_LIBRARIES})
|
|
||||||
|
|
||||||
target_include_directories(
|
|
||||||
mg_obstacles
|
|
||||||
PUBLIC
|
|
||||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
|
||||||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
|
|
||||||
)
|
|
||||||
|
|
||||||
ament_target_dependencies(
|
|
||||||
mg_obstacles
|
|
||||||
${PACKAGE_DEPS}
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
install(
|
|
||||||
TARGETS mg_obstacles
|
|
||||||
EXPORT export_${PROJECT_NAME}
|
|
||||||
ARCHIVE DESTINATION lib
|
|
||||||
LIBRARY DESTINATION lib
|
|
||||||
RUNTIME DESTINATION bin
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
|
|
||||||
install(DIRECTORY obstacles DESTINATION share/${PROJECT_NAME}/)
|
|
||||||
|
|
||||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
|
||||||
ament_export_include_directories(include)
|
|
||||||
|
|
||||||
target_compile_features(mg_obstacles PUBLIC c_std_99 cxx_std_17)
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
|
|
||||||
@ -1,65 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#include <string>
|
|
||||||
#include <glm/glm.hpp>
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
|
||||||
#include "mg_obstacles/permanent_obstacle.hpp"
|
|
||||||
#include "mg_obstacles/static_obstacle.hpp"
|
|
||||||
|
|
||||||
#include "std_msgs/msg/u_int64.hpp"
|
|
||||||
#include "std_msgs/msg/string.hpp"
|
|
||||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
|
||||||
|
|
||||||
#include "tf2_ros/buffer.h"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
class ObstacleManager {
|
|
||||||
using StaticListMsg = std_msgs::msg::UInt64;
|
|
||||||
using DynamicPosMsg = geometry_msgs::msg::PointStamped;
|
|
||||||
|
|
||||||
public:
|
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(ObstacleManager)
|
|
||||||
|
|
||||||
enum ObstacleMask { // NOLINT
|
|
||||||
DYNAMIC = 1,
|
|
||||||
STATIC = 2,
|
|
||||||
MOVABLE = 3,
|
|
||||||
PERMANENT = 4,
|
|
||||||
IMPORTANT = 5,
|
|
||||||
ALL = 7,
|
|
||||||
};
|
|
||||||
|
|
||||||
ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf);
|
|
||||||
bool contains(glm::dvec2 pos, double radius, uint mask = ObstacleMask::ALL);
|
|
||||||
double dist_to_nearest(glm::dvec2 pos, int mask = ObstacleMask::ALL);
|
|
||||||
double box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask = ObstacleMask::ALL);
|
|
||||||
|
|
||||||
static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat);
|
|
||||||
static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rotation);
|
|
||||||
|
|
||||||
void update_dynamic();
|
|
||||||
void update_static();
|
|
||||||
|
|
||||||
private:
|
|
||||||
rclcpp::Node* node_;
|
|
||||||
|
|
||||||
tf2_ros::Buffer::SharedPtr tf_buffer;
|
|
||||||
|
|
||||||
std::vector<StaticObstacle> static_obstacles_;
|
|
||||||
std::vector<PermanentObstacle> permanent_obstacles_;
|
|
||||||
|
|
||||||
glm::dvec3 oponent_position_ = { 0, 0, 0.20 };
|
|
||||||
bool openent_active_ = false;
|
|
||||||
|
|
||||||
rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
|
|
||||||
|
|
||||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr side_sub_;
|
|
||||||
|
|
||||||
rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
|
|
||||||
rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
|
|
||||||
|
|
||||||
std::string base_path;
|
|
||||||
|
|
||||||
void load_permanent(Json::Value& json);
|
|
||||||
void load_static(Json::Value& json);
|
|
||||||
};
|
|
||||||
}
|
|
||||||
@ -1,20 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <glm/glm.hpp>
|
|
||||||
#include <jsoncpp/json/json.h>
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
class PermanentObstacle {
|
|
||||||
public:
|
|
||||||
/* Create a permanent obstacle from a json value */
|
|
||||||
PermanentObstacle(const Json::Value& json);
|
|
||||||
|
|
||||||
glm::dvec2 pos;
|
|
||||||
glm::dvec2 size;
|
|
||||||
|
|
||||||
double distance(glm::dvec2 position) const;
|
|
||||||
bool contains(glm::dvec2 position, double radius) const;
|
|
||||||
double distanceBox(const glm::dvec2 position, const glm::dvec2 size, const glm::dmat2 rot_mat) const;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,16 +0,0 @@
|
|||||||
#include "glm/glm.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t);
|
|
||||||
|
|
||||||
double circleSdf(glm::dvec2 pos, glm::dvec2 t);
|
|
||||||
|
|
||||||
bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius);
|
|
||||||
bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius);
|
|
||||||
|
|
||||||
double boxToBox(const glm::dvec2 pos,
|
|
||||||
const glm::dvec2 size,
|
|
||||||
const glm::dvec2 t,
|
|
||||||
const glm::dvec2 sizet,
|
|
||||||
const glm::dmat2 rot_mat);
|
|
||||||
}
|
|
||||||
@ -1,26 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <glm/glm.hpp>
|
|
||||||
#include <jsoncpp/json/json.h>
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
class StaticObstacle {
|
|
||||||
public:
|
|
||||||
/* Create a static obstacle from a json value */
|
|
||||||
StaticObstacle(const Json::Value& json);
|
|
||||||
|
|
||||||
glm::dvec2 pos;
|
|
||||||
|
|
||||||
bool active = true;
|
|
||||||
|
|
||||||
bool horizontal;
|
|
||||||
|
|
||||||
static double width;
|
|
||||||
static double height;
|
|
||||||
|
|
||||||
double distance(glm::dvec2 position) const;
|
|
||||||
bool contains(glm::dvec2 position, double radius) const;
|
|
||||||
double distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -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
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
@ -1,56 +0,0 @@
|
|||||||
{
|
|
||||||
"staticObsacleHeight": 0.4,
|
|
||||||
"staticObsacleWidth": 0.1,
|
|
||||||
"staticObstacles": [
|
|
||||||
{
|
|
||||||
"horizontal": true,
|
|
||||||
"x": 0.625,
|
|
||||||
"y": 1.775
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": false,
|
|
||||||
"x": 0,
|
|
||||||
"y": 0.6
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": false,
|
|
||||||
"x": 0,
|
|
||||||
"y": 1.525
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": true,
|
|
||||||
"x": 0.575,
|
|
||||||
"y": 0.3
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": true,
|
|
||||||
"x": 2.025,
|
|
||||||
"y": 0.3
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": false,
|
|
||||||
"x": 2.9,
|
|
||||||
"y": 0.6
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": false,
|
|
||||||
"x": 2.9,
|
|
||||||
"y": 1.525
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": true,
|
|
||||||
"x": 1.975,
|
|
||||||
"y": 1.775
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": true,
|
|
||||||
"x": 0.9,
|
|
||||||
"y": 1
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"horizontal": true,
|
|
||||||
"x": 1.7,
|
|
||||||
"y": 1
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
@ -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": 1.05,
|
|
||||||
"y": 2
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"height": 0.45,
|
|
||||||
"width": 0.45,
|
|
||||||
"x": 1.55,
|
|
||||||
"y": 0.45
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"height": 0.2,
|
|
||||||
"width": 0.4,
|
|
||||||
"x": 0.65,
|
|
||||||
"y": 2
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"height": 0.15,
|
|
||||||
"width": 0.45,
|
|
||||||
"x": 2,
|
|
||||||
"y": 0.15
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"height": 0.15,
|
|
||||||
"width": 0.45,
|
|
||||||
"x": 0,
|
|
||||||
"y": 0.15
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"height": 0.45,
|
|
||||||
"width": 0.45,
|
|
||||||
"x": 0,
|
|
||||||
"y": 1.1
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
@ -1,29 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>mg_obstacles</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>Library for collision detection</description>
|
|
||||||
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
|
||||||
<license>Apache 2.0</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
|
||||||
<depend>rclcpp_action</depend>
|
|
||||||
<depend>rclcpp_components</depend>
|
|
||||||
<depend>mg_msgs</depend>
|
|
||||||
<depend>geometry_msgs</depend>
|
|
||||||
<depend>tf2</depend>
|
|
||||||
<depend>tf2_ros</depend>
|
|
||||||
<depend>tf2_geometry_msgs</depend>
|
|
||||||
<depend>libjsoncpp</depend>
|
|
||||||
|
|
||||||
|
|
||||||
<build_depend>libjsoncpp-dev</build_depend>
|
|
||||||
<build_depend>libglm-dev</build_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,216 +0,0 @@
|
|||||||
#include "mg_obstacles/mg_obstacles.hpp"
|
|
||||||
#include "mg_obstacles/sdf.hpp"
|
|
||||||
|
|
||||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
|
||||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
|
||||||
|
|
||||||
#include "glm/glm.hpp"
|
|
||||||
#include "glm/gtx/matrix_transform_2d.hpp"
|
|
||||||
|
|
||||||
#include <fstream>
|
|
||||||
|
|
||||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
ObstacleManager::ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf) :
|
|
||||||
node_(node), tf_buffer(buf) {
|
|
||||||
no_exec_cbg = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
|
|
||||||
rclcpp::SubscriptionOptions sub_opts;
|
|
||||||
sub_opts.callback_group = no_exec_cbg;
|
|
||||||
|
|
||||||
auto static_cb = [](StaticListMsg::ConstSharedPtr) {};
|
|
||||||
auto dynamic_cb = [](DynamicPosMsg::ConstSharedPtr) {};
|
|
||||||
|
|
||||||
static_obst_sub_ = node_->create_subscription<StaticListMsg>(
|
|
||||||
"/staticObstacles", rclcpp::QoS(1).keep_last(1), static_cb, sub_opts);
|
|
||||||
dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
|
|
||||||
"/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts);
|
|
||||||
|
|
||||||
side_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
|
||||||
"/side", rclcpp::QoS(1).keep_last(1), [this](std_msgs::msg::String::ConstSharedPtr msg) {
|
|
||||||
Json::Value json;
|
|
||||||
RCLCPP_INFO(node_->get_logger(), "Read file %s", (base_path + "/" + msg->data).c_str());
|
|
||||||
std::ifstream json_document(base_path + "/" + msg->data);
|
|
||||||
json_document >> json;
|
|
||||||
load_permanent(json);
|
|
||||||
load_static(json);
|
|
||||||
});
|
|
||||||
|
|
||||||
std::string obstacle_pkg;
|
|
||||||
std::string file_suffix = "/obstacles/yellow-side.json";
|
|
||||||
node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
|
|
||||||
node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
|
|
||||||
ulong n = obstacle_pkg.find_first_of('/');
|
|
||||||
if (n < obstacle_pkg.size()) {
|
|
||||||
file_suffix = obstacle_pkg.substr(n);
|
|
||||||
obstacle_pkg = obstacle_pkg.substr(0, n);
|
|
||||||
}
|
|
||||||
base_path = ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix;
|
|
||||||
Json::Value json;
|
|
||||||
RCLCPP_INFO(node_->get_logger(),
|
|
||||||
"Read file %s",
|
|
||||||
(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str());
|
|
||||||
std::ifstream json_document(base_path + "/yellow-side.json");
|
|
||||||
|
|
||||||
json_document >> json;
|
|
||||||
load_permanent(json);
|
|
||||||
load_static(json);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ObstacleManager::contains(glm::dvec2 pos, double radius, uint mask) {
|
|
||||||
bool contained = false;
|
|
||||||
if (mask & ObstacleMask::DYNAMIC && openent_active_) {
|
|
||||||
contained |= inCircleSdf({ oponent_position_.x, oponent_position_.y }, oponent_position_.z, pos, radius);
|
|
||||||
}
|
|
||||||
if (mask & ObstacleMask::STATIC) {
|
|
||||||
for (const auto& obstacle : static_obstacles_) {
|
|
||||||
if (!obstacle.active) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
// Check Static obstacles
|
|
||||||
contained |= obstacle.contains(pos, radius);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (mask & ObstacleMask::PERMANENT) {
|
|
||||||
for (const auto& obstacle : permanent_obstacles_) {
|
|
||||||
// Check Permanant obstacles
|
|
||||||
contained |= obstacle.contains(pos, radius);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// contained |= dist_to_bounds(pos, { radius, radius }, 0) < 0;
|
|
||||||
return contained;
|
|
||||||
}
|
|
||||||
|
|
||||||
double ObstacleManager::dist_to_nearest(glm::dvec2 pos, int mask) {
|
|
||||||
double nearest = INFINITY;
|
|
||||||
|
|
||||||
if (mask & ObstacleMask::DYNAMIC && openent_active_) {
|
|
||||||
const double x = circleSdf(pos, glm::dvec2(oponent_position_.x, oponent_position_.y));
|
|
||||||
|
|
||||||
nearest = glm::min(nearest, x - oponent_position_.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (mask & ObstacleMask::STATIC) {
|
|
||||||
for (const auto& obstacle : static_obstacles_) {
|
|
||||||
if (!obstacle.active) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
// Check Static obstacles
|
|
||||||
nearest = glm::min(nearest, obstacle.distance(pos));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (mask & ObstacleMask::PERMANENT) {
|
|
||||||
for (const auto& obstacle : permanent_obstacles_) {
|
|
||||||
// Check Permanant obstacles
|
|
||||||
nearest = glm::min(nearest, obstacle.distance(pos));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return nearest;
|
|
||||||
}
|
|
||||||
|
|
||||||
double ObstacleManager::box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask) {
|
|
||||||
const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } };
|
|
||||||
|
|
||||||
double nearest = INFINITY;
|
|
||||||
|
|
||||||
if (mask & ObstacleMask::DYNAMIC && openent_active_) {
|
|
||||||
const double x = circleSdf(xy, glm::dvec2(oponent_position_.x, oponent_position_.y));
|
|
||||||
|
|
||||||
nearest = glm::min(nearest, x - oponent_position_.z - glm::length(size) / 4);
|
|
||||||
}
|
|
||||||
if (mask & ObstacleMask::STATIC) {
|
|
||||||
for (const auto& obstacle : static_obstacles_) {
|
|
||||||
if (!obstacle.active) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
// Check Static obstacles
|
|
||||||
nearest = glm::min(nearest, obstacle.distanceBox(xy, size, rot_mat));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (mask & ObstacleMask::PERMANENT) {
|
|
||||||
for (const auto& obstacle : permanent_obstacles_) {
|
|
||||||
// Check Permanant obstacles
|
|
||||||
nearest = glm::min(nearest, obstacle.distanceBox(xy, size, rot_mat));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return nearest;
|
|
||||||
} // NOLINT
|
|
||||||
|
|
||||||
double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat) {
|
|
||||||
glm::dvec2 point1 = glm::dvec2{ -size.x, -size.y } * 0.5;
|
|
||||||
glm::dvec2 point2 = glm::dvec2{ -size.x, size.y } * 0.5;
|
|
||||||
glm::dvec2 point3 = glm::dvec2{ size.x, -size.y } * 0.5;
|
|
||||||
glm::dvec2 point4 = glm::dvec2{ size.x, size.y } * 0.5;
|
|
||||||
|
|
||||||
point1 = point1 * rot_mat + pos;
|
|
||||||
point2 = point2 * rot_mat + pos;
|
|
||||||
point3 = point3 * rot_mat + pos;
|
|
||||||
point4 = point4 * rot_mat + pos;
|
|
||||||
|
|
||||||
glm::dvec2 mini = glm::min(point1, glm::dvec2(3.0, 2.0) - point1);
|
|
||||||
|
|
||||||
mini = glm::min(glm::min(point2, glm::dvec2(3.0, 2.0) - point2), mini);
|
|
||||||
mini = glm::min(glm::min(point3, glm::dvec2(3.0, 2.0) - point3), mini);
|
|
||||||
mini = glm::min(glm::min(point4, glm::dvec2(3.0, 2.0) - point4), mini);
|
|
||||||
|
|
||||||
return glm::min(mini.x, mini.y);
|
|
||||||
}
|
|
||||||
|
|
||||||
double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rot) {
|
|
||||||
// Find me
|
|
||||||
const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } };
|
|
||||||
return ObstacleManager::dist_to_bounds(pos, size, rot_mat);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ObstacleManager::update_dynamic() {
|
|
||||||
DynamicPosMsg msg;
|
|
||||||
rclcpp::MessageInfo info;
|
|
||||||
if (dynamic_obst_sub_->take(msg, info)) {
|
|
||||||
auto point = tf_buffer->transform(msg, "odom");
|
|
||||||
|
|
||||||
oponent_position_.x = point.point.x;
|
|
||||||
oponent_position_.y = point.point.y;
|
|
||||||
|
|
||||||
openent_active_ = oponent_position_.x >= 0 && oponent_position_.y >= 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ObstacleManager::update_static() {
|
|
||||||
StaticListMsg msg;
|
|
||||||
rclcpp::MessageInfo info;
|
|
||||||
if (static_obst_sub_->take(msg, info)) {
|
|
||||||
uint ind = 1;
|
|
||||||
for (int i = (int)static_obstacles_.size() - 1; i >= 0; i--) {
|
|
||||||
static_obstacles_[i].active = ind & msg.data;
|
|
||||||
ind <<= 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ObstacleManager::load_permanent(Json::Value& json) {
|
|
||||||
permanent_obstacles_.clear();
|
|
||||||
|
|
||||||
Json::Value& obstacles = json["obstacles"];
|
|
||||||
|
|
||||||
for (const Json::Value& obstacle : obstacles) {
|
|
||||||
// Add obstacle
|
|
||||||
permanent_obstacles_.emplace_back(obstacle);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ObstacleManager::load_static(Json::Value& json) {
|
|
||||||
static_obstacles_.clear();
|
|
||||||
|
|
||||||
StaticObstacle::width = json["staticObstacleWidth"].asDouble();
|
|
||||||
StaticObstacle::height = json["staticObstacleHeight"].asDouble();
|
|
||||||
for (const Json::Value& obstacle : json["staticObstacles"]) { static_obstacles_.emplace_back(obstacle); }
|
|
||||||
for (const auto& obstacle : static_obstacles_) {
|
|
||||||
RCLCPP_INFO(node_->get_logger(), "Loaded static obstacle at %lf %lf", obstacle.pos.x, obstacle.pos.y);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,23 +0,0 @@
|
|||||||
#include "mg_obstacles/permanent_obstacle.hpp"
|
|
||||||
#include "mg_obstacles/sdf.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
PermanentObstacle::PermanentObstacle(const Json::Value& json) :
|
|
||||||
pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()),
|
|
||||||
size(json.get("width", 0.0).asDouble(), json.get("height", 0.0).asDouble()) { }
|
|
||||||
|
|
||||||
double PermanentObstacle::distance(glm::dvec2 position) const {
|
|
||||||
return boxSdf(pos + 0.5 * glm::dvec2{ size.x, -size.y }, size * 0.5, position);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool PermanentObstacle::contains(glm::dvec2 position, double radius) const {
|
|
||||||
return inBoxSdf(pos + 0.5 * glm::dvec2{ size.x, -size.y }, size * 0.5, position, radius);
|
|
||||||
}
|
|
||||||
|
|
||||||
double PermanentObstacle::distanceBox(const glm::dvec2 position,
|
|
||||||
const glm::dvec2 size,
|
|
||||||
const glm::dmat2 rot_mat) const {
|
|
||||||
|
|
||||||
return boxToBox(pos + 0.5 * glm::dvec2{ this->size.x, -this->size.y }, this->size, position, size, rot_mat);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,53 +0,0 @@
|
|||||||
#include "mg_obstacles/sdf.hpp"
|
|
||||||
#include <glm/gtx/norm.hpp>
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t) {
|
|
||||||
glm::dvec2 d = glm::abs(t - pos) - size;
|
|
||||||
return glm::length(glm::max(d, 0.0)) + glm::min(glm::max(d.x, d.y), 0.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
double circleSdf(glm::dvec2 pos, glm::dvec2 t) { return glm::length(pos - t); }
|
|
||||||
|
|
||||||
bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius) {
|
|
||||||
return glm::length2(pos - t) < glm::pow(rad + radius, 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius) {
|
|
||||||
const glm::dvec2 d = glm::abs(t - pos) - size;
|
|
||||||
// return (glm::length2(glm::max(d, 0.0))) < glm::pow(radius - glm::min(glm::max(d.x, d.y), 0.0), 2);
|
|
||||||
return boxSdf(pos, size, t) - radius < 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
double boxToBox(const glm::dvec2 pos,
|
|
||||||
const glm::dvec2 size,
|
|
||||||
const glm::dvec2 t,
|
|
||||||
const glm::dvec2 sizet,
|
|
||||||
const glm::dmat2 rot_mat) {
|
|
||||||
|
|
||||||
const glm::dmat2 irot_mat = glm::transpose(rot_mat);
|
|
||||||
|
|
||||||
glm::dvec2 pointA1 = rot_mat * (glm::dvec2{ -sizet.x, -sizet.y } * 0.5) + t;
|
|
||||||
glm::dvec2 pointA2 = rot_mat * (glm::dvec2{ sizet.x, -sizet.y } * 0.5) + t;
|
|
||||||
glm::dvec2 pointA3 = rot_mat * (glm::dvec2{ -sizet.x, sizet.y } * 0.5) + t;
|
|
||||||
glm::dvec2 pointA4 = rot_mat * (glm::dvec2{ sizet.x, sizet.y } * 0.5) + t;
|
|
||||||
|
|
||||||
double distance = boxSdf(pos, size * 0.5, pointA1);
|
|
||||||
|
|
||||||
distance = glm::min(boxSdf(pos, size * 0.5, pointA2), distance);
|
|
||||||
distance = glm::min(boxSdf(pos, size * 0.5, pointA3), distance);
|
|
||||||
distance = glm::min(boxSdf(pos, size * 0.5, pointA4), distance);
|
|
||||||
|
|
||||||
glm::dvec2 pointB1 = irot_mat * (glm::dvec2{ -size.x, -size.y } * 0.5 + pos - t);
|
|
||||||
glm::dvec2 pointB2 = irot_mat * (glm::dvec2{ size.x, -size.y } * 0.5 + pos - t);
|
|
||||||
glm::dvec2 pointB3 = irot_mat * (glm::dvec2{ -size.x, size.y } * 0.5 + pos - t);
|
|
||||||
glm::dvec2 pointB4 = irot_mat * (glm::dvec2{ size.x, size.y } * 0.5 + pos - t);
|
|
||||||
|
|
||||||
distance = glm::min(boxSdf({}, sizet * 0.5, pointB1), distance);
|
|
||||||
distance = glm::min(boxSdf({}, sizet * 0.5, pointB2), distance);
|
|
||||||
distance = glm::min(boxSdf({}, sizet * 0.5, pointB3), distance);
|
|
||||||
distance = glm::min(boxSdf({}, sizet * 0.5, pointB4), distance);
|
|
||||||
|
|
||||||
return distance;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,46 +0,0 @@
|
|||||||
#include "mg_obstacles/static_obstacle.hpp"
|
|
||||||
#include "mg_obstacles/sdf.hpp"
|
|
||||||
|
|
||||||
namespace mg {
|
|
||||||
|
|
||||||
double StaticObstacle::width = 0.1;
|
|
||||||
double StaticObstacle::height = 0.4;
|
|
||||||
|
|
||||||
StaticObstacle::StaticObstacle(const Json::Value& json) :
|
|
||||||
pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()),
|
|
||||||
horizontal(json.get("horizontal", true).asBool()) { }
|
|
||||||
|
|
||||||
double StaticObstacle::distance(glm::dvec2 position) const {
|
|
||||||
const double width = StaticObstacle::width;
|
|
||||||
const double height = StaticObstacle::height;
|
|
||||||
if (!horizontal) {
|
|
||||||
return boxSdf(pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position);
|
|
||||||
} else {
|
|
||||||
return boxSdf(pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
double StaticObstacle::distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const {
|
|
||||||
const double width = StaticObstacle::width;
|
|
||||||
const double height = StaticObstacle::height;
|
|
||||||
if (!horizontal) {
|
|
||||||
return boxToBox(
|
|
||||||
pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height }, position, size, rot_mat);
|
|
||||||
} else {
|
|
||||||
return boxToBox(
|
|
||||||
pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width }, position, size, rot_mat);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool StaticObstacle::contains(glm::dvec2 position, double radius) const {
|
|
||||||
const double width = StaticObstacle::width;
|
|
||||||
const double height = StaticObstacle::height;
|
|
||||||
if (!horizontal) {
|
|
||||||
return inBoxSdf(
|
|
||||||
pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position, radius);
|
|
||||||
} else {
|
|
||||||
return inBoxSdf(
|
|
||||||
pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position, radius);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user