add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,524 @@
|
||||
version: 2.1
|
||||
|
||||
_commands:
|
||||
common_commands: &common_commands
|
||||
ccache_stats:
|
||||
description: "CCache Stats"
|
||||
parameters:
|
||||
workspace:
|
||||
type: string
|
||||
when:
|
||||
type: string
|
||||
default: on_success
|
||||
steps:
|
||||
- run:
|
||||
name: CCache Stats
|
||||
working_directory: << parameters.workspace >>
|
||||
environment:
|
||||
CCACHE_DIR: << parameters.workspace >>/.ccache
|
||||
command: |
|
||||
ccache -s # show stats
|
||||
ccache -z # zero stats
|
||||
ccache -V # show version
|
||||
ccache -p # show config
|
||||
when: << parameters.when >>
|
||||
restore_from_cache:
|
||||
description: "Restore From Cache"
|
||||
parameters:
|
||||
key:
|
||||
type: string
|
||||
workspace:
|
||||
type: string
|
||||
steps:
|
||||
- restore_cache:
|
||||
name: Restore Cache << parameters.key >>
|
||||
keys:
|
||||
- "<< parameters.key >>-v13\
|
||||
-{{ arch }}\
|
||||
-{{ .Branch }}\
|
||||
-{{ .Environment.CIRCLE_PR_NUMBER }}\
|
||||
-{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}"
|
||||
- "<< parameters.key >>-v13\
|
||||
-{{ arch }}\
|
||||
-main\
|
||||
-<no value>\
|
||||
-{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}"
|
||||
save_to_cache:
|
||||
description: "Save To Cache"
|
||||
parameters:
|
||||
key:
|
||||
type: string
|
||||
workspace:
|
||||
type: string
|
||||
path:
|
||||
type: string
|
||||
when:
|
||||
type: string
|
||||
default: on_success
|
||||
steps:
|
||||
- save_cache:
|
||||
name: Save Cache << parameters.key >>
|
||||
key: "<< parameters.key >>-v13\
|
||||
-{{ arch }}\
|
||||
-{{ .Branch }}\
|
||||
-{{ .Environment.CIRCLE_PR_NUMBER }}\
|
||||
-{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}\
|
||||
-{{ epoch }}"
|
||||
paths:
|
||||
- << parameters.path >>/.ccache
|
||||
- << parameters.path >>/build
|
||||
- << parameters.path >>/install
|
||||
- << parameters.path >>/log
|
||||
- << parameters.path >>/test_results
|
||||
when: << parameters.when >>
|
||||
install_dependencies:
|
||||
description: "Install Dependencies"
|
||||
parameters:
|
||||
underlay:
|
||||
type: string
|
||||
workspace:
|
||||
type: string
|
||||
steps:
|
||||
- run:
|
||||
name: Install Dependencies | << parameters.workspace >>
|
||||
working_directory: << parameters.workspace >>
|
||||
command: |
|
||||
. << parameters.underlay >>/install/setup.sh
|
||||
AMENT_PREFIX_PATH=$(echo "$AMENT_PREFIX_PATH" | \
|
||||
sed -e 's|:/opt/ros/'$ROS_DISTRO'$||')
|
||||
if [ "$AMENT_PREFIX_PATH" == "/opt/ros/$ROS_DISTRO" ]
|
||||
then
|
||||
unset AMENT_PREFIX_PATH
|
||||
fi
|
||||
|
||||
cat << parameters.underlay >>/lockfile.txt > lockfile.txt
|
||||
vcs export --exact << parameters.underlay >>/src | \
|
||||
(echo vcs_export && cat) >> lockfile.txt
|
||||
sha256sum $PWD/lockfile.txt >> lockfile.txt
|
||||
|
||||
apt-get update
|
||||
rosdep update --rosdistro $ROS_DISTRO
|
||||
dependencies=$(
|
||||
rosdep install -q -y \
|
||||
--from-paths src \
|
||||
--ignore-src \
|
||||
--skip-keys " \
|
||||
slam_toolbox \
|
||||
" \
|
||||
--verbose | \
|
||||
awk '$1 ~ /^resolution\:/' | \
|
||||
awk -F'[][]' '{print $2}' | \
|
||||
tr -d \, | xargs -n1 | sort -u | xargs)
|
||||
dpkg --list dpkg $dependencies | \
|
||||
(echo workspace_dependencies && cat) >> lockfile.txt
|
||||
sha256sum $PWD/lockfile.txt >> lockfile.txt
|
||||
setup_workspace:
|
||||
description: "Setup Workspace"
|
||||
parameters:
|
||||
underlay:
|
||||
type: string
|
||||
key:
|
||||
type: string
|
||||
workspace:
|
||||
type: string
|
||||
mixins:
|
||||
type: string
|
||||
build:
|
||||
default: true
|
||||
type: boolean
|
||||
steps:
|
||||
- store_artifacts:
|
||||
path: << parameters.workspace >>/lockfile.txt
|
||||
- restore_from_cache:
|
||||
key: << parameters.key >>
|
||||
workspace: << parameters.workspace >>
|
||||
- when:
|
||||
condition: << parameters.build >>
|
||||
steps:
|
||||
- ccache_stats:
|
||||
workspace: << parameters.workspace >>
|
||||
when: always
|
||||
- run:
|
||||
name: Build Workspace | << parameters.workspace >>
|
||||
working_directory: << parameters.workspace >>
|
||||
environment:
|
||||
CCACHE_DIR: << parameters.workspace >>/.ccache
|
||||
command: |
|
||||
colcon cache lock
|
||||
|
||||
BUILD_UNFINISHED=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-skip-build-finished \
|
||||
| xargs)
|
||||
echo BUILD_UNFINISHED: $BUILD_UNFINISHED
|
||||
|
||||
BUILD_FAILED=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-select-build-failed \
|
||||
| xargs)
|
||||
echo BUILD_FAILED: $BUILD_FAILED
|
||||
|
||||
BUILD_INVALID=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-select-cache-invalid \
|
||||
--packages-select-cache-key build \
|
||||
| xargs)
|
||||
echo BUILD_INVALID: $BUILD_INVALID
|
||||
|
||||
BUILD_PACKAGES=""
|
||||
if [ -n "$BUILD_UNFINISHED" ] || \
|
||||
[ -n "$BUILD_FAILED" ] || \
|
||||
[ -n "$BUILD_INVALID" ]
|
||||
then
|
||||
BUILD_PACKAGES=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-above \
|
||||
$BUILD_UNFINISHED \
|
||||
$BUILD_FAILED \
|
||||
$BUILD_INVALID \
|
||||
| xargs)
|
||||
fi
|
||||
echo BUILD_PACKAGES: $BUILD_PACKAGES
|
||||
|
||||
colcon clean packages --yes \
|
||||
--packages-select ${BUILD_PACKAGES} \
|
||||
--base-select install
|
||||
|
||||
. << parameters.underlay >>/install/setup.sh
|
||||
colcon build \
|
||||
--packages-select ${BUILD_PACKAGES} \
|
||||
--mixin << parameters.mixins >>
|
||||
- ccache_stats:
|
||||
workspace: << parameters.workspace >>
|
||||
when: always
|
||||
- save_to_cache:
|
||||
key: << parameters.key >>
|
||||
path: << parameters.workspace >>
|
||||
workspace: << parameters.workspace >>
|
||||
when: always
|
||||
- run:
|
||||
name: Copy Build Logs
|
||||
working_directory: << parameters.workspace >>
|
||||
command: cp -rH log/latest_build log/build
|
||||
when: always
|
||||
- store_artifacts:
|
||||
path: << parameters.workspace >>/log/build
|
||||
test_workspace:
|
||||
description: "Test Workspace"
|
||||
parameters:
|
||||
key:
|
||||
type: string
|
||||
workspace:
|
||||
type: string
|
||||
cache_test:
|
||||
type: boolean
|
||||
steps:
|
||||
- run:
|
||||
name: Test Workspace | << parameters.workspace >>
|
||||
working_directory: << parameters.workspace >>
|
||||
command: |
|
||||
TEST_UNPASSED=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-skip-test-passed \
|
||||
| xargs)
|
||||
echo TEST_UNPASSED: $TEST_UNPASSED
|
||||
|
||||
TEST_FAILURES=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-select-test-failures \
|
||||
| xargs)
|
||||
echo TEST_FAILURES: $TEST_FAILURES
|
||||
|
||||
TEST_INVALID=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-select-cache-invalid \
|
||||
--packages-select-cache-key test \
|
||||
| xargs)
|
||||
echo TEST_INVALID: $TEST_INVALID
|
||||
|
||||
TEST_PACKAGES=""
|
||||
if [ -n "$TEST_UNPASSED" ] || \
|
||||
[ -n "$TEST_FAILURES" ] || \
|
||||
[ -n "$TEST_INVALID" ]
|
||||
then
|
||||
TEST_PACKAGES=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-above \
|
||||
$TEST_UNPASSED \
|
||||
$TEST_FAILURES \
|
||||
$TEST_INVALID)
|
||||
fi
|
||||
if ( ! << parameters.cache_test >> )
|
||||
then
|
||||
TEST_PACKAGES=$(
|
||||
colcon list \
|
||||
--names-only)
|
||||
fi
|
||||
TEST_PACKAGES=$(
|
||||
echo $TEST_PACKAGES \
|
||||
| circleci tests split \
|
||||
--split-by=timings \
|
||||
--timings-type=classname \
|
||||
--show-counts \
|
||||
| xargs)
|
||||
echo TEST_PACKAGES: $TEST_PACKAGES
|
||||
|
||||
colcon clean packages --yes \
|
||||
--packages-select ${TEST_PACKAGES} \
|
||||
--base-select test_result
|
||||
colcon clean packages --yes \
|
||||
--packages-select ${TEST_PACKAGES} \
|
||||
--base-select build \
|
||||
--clean-match \
|
||||
"*.gcda"
|
||||
|
||||
. install/setup.sh
|
||||
set -o xtrace
|
||||
colcon test \
|
||||
--packages-select ${TEST_PACKAGES}
|
||||
colcon test-result \
|
||||
--verbose
|
||||
- when:
|
||||
condition: << parameters.cache_test >>
|
||||
steps:
|
||||
- save_to_cache:
|
||||
key: << parameters.key >>
|
||||
path: << parameters.workspace >>
|
||||
workspace: << parameters.workspace >>
|
||||
when: always
|
||||
- run:
|
||||
name: Copy Test Logs
|
||||
working_directory: << parameters.workspace >>
|
||||
command: cp -rH log/latest_test log/test
|
||||
when: always
|
||||
- store_artifacts:
|
||||
path: << parameters.workspace >>/log/test
|
||||
- store_artifacts:
|
||||
path: << parameters.workspace >>/test_results
|
||||
- run:
|
||||
name: Prepare Test Results
|
||||
working_directory: << parameters.workspace >>
|
||||
command: find test_results -name "Test.xml" -type f -delete
|
||||
when: always
|
||||
- store_test_results:
|
||||
path: << parameters.workspace >>/test_results
|
||||
|
||||
_steps:
|
||||
pre_checkout: &pre_checkout
|
||||
run:
|
||||
name: Pre Checkout
|
||||
command: |
|
||||
mkdir -p $ROS_WS/src && cd $ROS_WS
|
||||
ln -s /opt/ros/$ROS_DISTRO install
|
||||
|
||||
echo $CACHE_NONCE | \
|
||||
(echo cache_nonce && cat) >> lockfile.txt
|
||||
sha256sum $PWD/lockfile.txt >> lockfile.txt
|
||||
|
||||
TZ=utc stat -c '%y' /ros_entrypoint.sh | \
|
||||
(echo ros_entrypoint && cat) >> lockfile.txt
|
||||
sha256sum $PWD/lockfile.txt >> lockfile.txt
|
||||
|
||||
rm -rf $OVERLAY_WS/*
|
||||
on_checkout: &on_checkout
|
||||
checkout:
|
||||
path: src/navigation2
|
||||
post_checkout: &post_checkout
|
||||
run:
|
||||
name: Post Checkout
|
||||
command: |
|
||||
cp $OVERLAY_WS/src/navigation2/.circleci/defaults.yaml $COLCON_DEFAULTS_FILE
|
||||
if ! cmp \
|
||||
$OVERLAY_WS/src/navigation2/tools/underlay.repos \
|
||||
$UNDERLAY_WS/underlay.repos >/dev/null 2>&1
|
||||
then
|
||||
echo "Importing Underlay"
|
||||
cp $OVERLAY_WS/src/navigation2/tools/underlay.repos \
|
||||
$UNDERLAY_WS/underlay.repos
|
||||
vcs import $UNDERLAY_WS/src \
|
||||
< $UNDERLAY_WS/underlay.repos
|
||||
fi
|
||||
install_underlay_dependencies: &install_underlay_dependencies
|
||||
install_dependencies:
|
||||
underlay: /opt/ros_ws
|
||||
workspace: /opt/underlay_ws
|
||||
setup_underlay_workspace: &setup_underlay_workspace
|
||||
setup_workspace: &setup_workspace_underlay
|
||||
key: underlay_ws
|
||||
underlay: /opt/ros_ws
|
||||
workspace: /opt/underlay_ws
|
||||
mixins: ${UNDERLAY_MIXINS}
|
||||
restore_underlay_workspace: &restore_underlay_workspace
|
||||
setup_workspace:
|
||||
<<: *setup_workspace_underlay
|
||||
build: false
|
||||
install_overlay_dependencies: &install_overlay_dependencies
|
||||
install_dependencies:
|
||||
underlay: /opt/underlay_ws
|
||||
workspace: /opt/overlay_ws
|
||||
setup_overlay_workspace: &setup_overlay_workspace
|
||||
setup_workspace: &setup_workspace_overlay
|
||||
key: overlay_ws
|
||||
underlay: /opt/underlay_ws
|
||||
workspace: /opt/overlay_ws
|
||||
mixins: ${OVERLAY_MIXINS}
|
||||
restore_overlay_workspace: &restore_overlay_workspace
|
||||
setup_workspace:
|
||||
<<: *setup_workspace_overlay
|
||||
build: false
|
||||
test_overlay_workspace: &test_overlay_workspace
|
||||
test_workspace:
|
||||
key: overlay_ws
|
||||
workspace: /opt/overlay_ws
|
||||
cache_test: << parameters.cache_test >>
|
||||
collect_overlay_coverage: &collect_overlay_coverage
|
||||
run:
|
||||
name: Collect Code Coverage
|
||||
working_directory: /opt/overlay_ws
|
||||
command: src/navigation2/tools/code_coverage_report.bash ci
|
||||
when: always
|
||||
upload_overlay_coverage: &upload_overlay_coverage
|
||||
run:
|
||||
name: Upload Code Coverage
|
||||
working_directory: /opt/overlay_ws
|
||||
command: |
|
||||
curl -s https://codecov.io/bash > codecov
|
||||
codecov_version=$(grep -o 'VERSION=\"[0-9\.]*\"' codecov | cut -d'"' -f2)
|
||||
shasum -a 512 -c <(curl -s "https://raw.githubusercontent.com/codecov/codecov-bash/${codecov_version}/SHA512SUM" | grep -w "codecov")
|
||||
bash codecov \
|
||||
-f "lcov/total_coverage.info" \
|
||||
-R "src/navigation2" \
|
||||
-n "$RMW_IMPLEMENTATION" \
|
||||
-Z || echo 'Codecov upload failed'
|
||||
when: always
|
||||
|
||||
commands:
|
||||
<<: *common_commands
|
||||
checkout_source:
|
||||
description: "Checkout Source"
|
||||
steps:
|
||||
- *pre_checkout
|
||||
- *on_checkout
|
||||
- *post_checkout
|
||||
setup_dependencies:
|
||||
description: "Setup Dependencies"
|
||||
steps:
|
||||
- *install_underlay_dependencies
|
||||
- *setup_underlay_workspace
|
||||
- *install_overlay_dependencies
|
||||
build_source:
|
||||
description: "Build Source"
|
||||
steps:
|
||||
- *setup_overlay_workspace
|
||||
restore_build:
|
||||
description: "Restore Build"
|
||||
steps:
|
||||
- checkout_source
|
||||
- *install_underlay_dependencies
|
||||
- *restore_underlay_workspace
|
||||
- *install_overlay_dependencies
|
||||
- *restore_overlay_workspace
|
||||
test_build:
|
||||
description: "Test Build"
|
||||
parameters:
|
||||
cache_test:
|
||||
type: boolean
|
||||
steps:
|
||||
- *test_overlay_workspace
|
||||
report_coverage:
|
||||
description: "Report Coverage"
|
||||
steps:
|
||||
- *collect_overlay_coverage
|
||||
- *upload_overlay_coverage
|
||||
|
||||
_environments:
|
||||
common_environment: &common_environment
|
||||
ROS_WS: "/opt/ros_ws"
|
||||
UNDERLAY_WS: "/opt/underlay_ws"
|
||||
OVERLAY_WS: "/opt/overlay_ws"
|
||||
UNDERLAY_MIXINS: "release ccache lld"
|
||||
CCACHE_LOGFILE: "/tmp/ccache.log"
|
||||
CCACHE_MAXSIZE: "200M"
|
||||
MAKEFLAGS: "-j 2 -l 2 "
|
||||
COLCON_DEFAULTS_FILE: "/tmp/defaults.yaml"
|
||||
RCUTILS_LOGGING_BUFFERED_STREAM: "0"
|
||||
RCUTILS_LOGGING_USE_STDOUT: "0"
|
||||
DEBIAN_FRONTEND: "noninteractive"
|
||||
PYTHONUNBUFFERED: "1"
|
||||
|
||||
executors:
|
||||
release_exec:
|
||||
docker:
|
||||
- image: ghcr.io/ros-navigation/navigation2:humble
|
||||
resource_class: large
|
||||
working_directory: /opt/overlay_ws
|
||||
environment:
|
||||
<<: *common_environment
|
||||
CACHE_NONCE: "Release"
|
||||
OVERLAY_MIXINS: "release ccache coverage-gcc lld"
|
||||
|
||||
_jobs:
|
||||
job_test: &job_test
|
||||
parameters:
|
||||
cache_test:
|
||||
type: boolean
|
||||
default: false
|
||||
rmw:
|
||||
default: "rmw_cyclonedds_cpp"
|
||||
type: string
|
||||
parallelism: 1
|
||||
environment:
|
||||
RMW_IMPLEMENTATION: << parameters.rmw >>
|
||||
|
||||
jobs:
|
||||
release_build: &release_build
|
||||
executor: release_exec
|
||||
steps:
|
||||
- checkout_source
|
||||
- setup_dependencies
|
||||
- build_source
|
||||
release_test: &release_test
|
||||
<<: *job_test
|
||||
executor: release_exec
|
||||
steps:
|
||||
- restore_build
|
||||
- test_build:
|
||||
cache_test: << parameters.cache_test >>
|
||||
- report_coverage
|
||||
|
||||
workflows:
|
||||
version: 2
|
||||
build_and_test:
|
||||
jobs:
|
||||
- release_build
|
||||
- release_test:
|
||||
requires:
|
||||
- release_build
|
||||
cache_test: true
|
||||
nightly:
|
||||
jobs:
|
||||
- release_build
|
||||
- release_test:
|
||||
requires:
|
||||
- release_build
|
||||
matrix:
|
||||
parameters:
|
||||
rmw:
|
||||
- rmw_connextdds
|
||||
- rmw_cyclonedds_cpp
|
||||
- rmw_fastrtps_cpp
|
||||
triggers:
|
||||
- schedule:
|
||||
cron: "0 13 * * *"
|
||||
filters:
|
||||
branches:
|
||||
only:
|
||||
- main
|
||||
@@ -0,0 +1,16 @@
|
||||
_common: &common
|
||||
"test-result-base": "test_results"
|
||||
|
||||
"clean.packages":
|
||||
<<: *common
|
||||
"build":
|
||||
<<: *common
|
||||
"executor": "parallel"
|
||||
"parallel-workers": 4
|
||||
"symlink-install": true
|
||||
"test":
|
||||
<<: *common
|
||||
"executor": "parallel"
|
||||
"parallel-workers": 1
|
||||
"test-result":
|
||||
<<: *common
|
||||
@@ -0,0 +1,132 @@
|
||||
# Snippet for global matchers and variables
|
||||
# to logically expression request conditions
|
||||
# E.g. for conditionally changing redirects
|
||||
(globals) {
|
||||
# Use gzip compression for all responses
|
||||
encode gzip
|
||||
|
||||
# Matcher for http request scheme. E.g. "http" or "https"
|
||||
@http_scheme {
|
||||
expression {http.request.scheme}=="https" || {header.X-Forwarded-Scheme}=="https" || {header.X-Forwarded-Proto}=="https"
|
||||
}
|
||||
# If any http scheme is "https", then use "wss"
|
||||
vars @http_scheme WsScheme "wss"
|
||||
# Else default to "ws"
|
||||
vars WsScheme "ws"
|
||||
|
||||
# Matcher for forwarded request headers
|
||||
@host_forwarded {
|
||||
header X-Forwarded-Host *
|
||||
}
|
||||
# If http headers exists, then use them
|
||||
vars @host_forwarded ReqHost {header.X-Forwarded-Host}
|
||||
# Else default to host in request
|
||||
vars ReqHost {http.request.hostport}
|
||||
|
||||
# Matcher for websocket connection upgrade requests
|
||||
@websockets {
|
||||
# Avoid case sensitivity issues when matching field values
|
||||
# E.g. when values are rewritten by Codespace port forwarding
|
||||
header_regexp Connection (?i)(Upgrade)
|
||||
header Upgrade websocket
|
||||
}
|
||||
}
|
||||
|
||||
# Snippet for redirect with given URL queries values
|
||||
# to simplify remote development with web apps
|
||||
# E.g auto redirect websocket URL to match request scheme
|
||||
(redirect) {
|
||||
# Configure redirect to match request scheme
|
||||
vars LayoutUrl "/assets/foxglove/nav2_layout.json"
|
||||
vars DataSourceUrl "{vars.WsScheme}://{vars.ReqHost}{args.0}/"
|
||||
redir /autoconnect "{args.0}/?ds=foxglove-websocket&ds.url={vars.DataSourceUrl}"
|
||||
redir /autolayout "{args.0}/?ds=foxglove-websocket&ds.url={vars.DataSourceUrl}&layoutUrl={vars.LayoutUrl}"
|
||||
}
|
||||
|
||||
# Snippet for dummy imports
|
||||
(dummy) {
|
||||
}
|
||||
|
||||
# Snippet for enabling mobile web app features
|
||||
# to improve user experience on small screen devices
|
||||
# E.g. for enabling fullscreen mode on iOS and Android
|
||||
(mobile) {
|
||||
# Match for directory redirects to index.html
|
||||
route / {
|
||||
# Inject link to manifest just after <head> tag
|
||||
# https://developer.mozilla.org/docs/Web/Manifest
|
||||
replace `<head>` `<head><link rel="manifest" href="manifest.json" crossorigin="use-credentials"/>`
|
||||
}
|
||||
# Redirect relative handle_path'ed manifest.json to /manifests directory
|
||||
redir /manifest.json /assets{http.request.orig_uri.path.dir}manifest.json
|
||||
}
|
||||
|
||||
# Snippet for hosted web app using websockets
|
||||
# to serve static files and reverse proxying connections
|
||||
# E.g. for serving GzWeb and Foxglove web apps
|
||||
(app) {
|
||||
# handle and strip path prefix from redirect
|
||||
handle_path {args.0}/* {
|
||||
# Set root directory for static files
|
||||
root * {http.vars.root}{args.0}
|
||||
# Enable mobile web app features
|
||||
import mobile
|
||||
# Reverse proxy websockets to backend address
|
||||
reverse_proxy @websockets {args.1}
|
||||
# Import custom snippets
|
||||
import {args.2} {args.0}
|
||||
}
|
||||
}
|
||||
|
||||
# Listen for http requests on port 8080
|
||||
# regardless of hostname or domain address
|
||||
# E.g. whatever Codespaces assigns to host
|
||||
:8080 {
|
||||
# Include global matchers and variables
|
||||
import globals
|
||||
root * {$ROOT_SRV:/srv}
|
||||
file_server browse
|
||||
|
||||
# Handle root content
|
||||
# I.e. assets internal to workspace
|
||||
handle /* {
|
||||
# Template manifest.json files
|
||||
templates */manifest.json {
|
||||
mime application/json
|
||||
}
|
||||
}
|
||||
|
||||
# Handle nav2 web app
|
||||
# I.e. main landing page
|
||||
handle_path /nav2/* {
|
||||
root * {http.vars.root}/nav2
|
||||
import mobile
|
||||
# Render markdown files as html
|
||||
templates
|
||||
}
|
||||
|
||||
# Matcher for requests without browse query
|
||||
@no_browse {
|
||||
path /
|
||||
not query browse=true
|
||||
}
|
||||
# Redirect to nav2 web app by default
|
||||
redir @no_browse /nav2/
|
||||
|
||||
# Import app snippets for web apps
|
||||
import app "/gzweb" "localhost:9090" "dummy"
|
||||
import app "/foxglove" "localhost:8765" "redirect"
|
||||
|
||||
# Handle glances web app
|
||||
redir /glances /glances/
|
||||
handle_path /glances/* {
|
||||
import mobile
|
||||
# Reverse proxy to glances backend
|
||||
reverse_proxy * "localhost:61208"
|
||||
}
|
||||
|
||||
# For debugging
|
||||
# log {
|
||||
# output file /var/log/caddy/server.log
|
||||
# }
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
{
|
||||
"name": "Foxglove: {{placeholder "http.vars.ReqHost"}}",
|
||||
"short_name": "Foxglove: {{placeholder "http.vars.ReqHost"}}",
|
||||
"icons": [
|
||||
{
|
||||
"src": "/media/icons/foxglove/any_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "any"
|
||||
},
|
||||
{
|
||||
"src": "/media/icons/foxglove/maskable_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "maskable"
|
||||
}
|
||||
],
|
||||
"id": "/foxglove/",
|
||||
"start_url": "/foxglove/autoconnect",
|
||||
"theme_color": "#6F3BE8",
|
||||
"background_color": "#6F3BE8",
|
||||
"display": "fullscreen",
|
||||
"shortcuts" : [
|
||||
{
|
||||
"name": "Auto Connect",
|
||||
"url": "/foxglove/autoconnect",
|
||||
"description": "Auto connect to default data source"
|
||||
},
|
||||
{
|
||||
"name": "Auto Layout",
|
||||
"url": "/foxglove/autolayout",
|
||||
"description": "Auto connect using default layout"
|
||||
},
|
||||
{
|
||||
"name": "Manual Connect",
|
||||
"url": "/foxglove/",
|
||||
"description": "Manually connect to data source"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,463 @@
|
||||
{
|
||||
"configById": {
|
||||
"3D!18i6zy7": {
|
||||
"layers": {
|
||||
"845139cb-26bc-40b3-8161-8ab60af4baf5": {
|
||||
"visible": true,
|
||||
"frameLocked": true,
|
||||
"label": "Grid",
|
||||
"instanceId": "845139cb-26bc-40b3-8161-8ab60af4baf5",
|
||||
"layerId": "foxglove.Grid",
|
||||
"size": 10,
|
||||
"divisions": 10,
|
||||
"lineWidth": 1,
|
||||
"color": "#A0A0A4ff",
|
||||
"position": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"rotation": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"order": 1
|
||||
}
|
||||
},
|
||||
"cameraState": {
|
||||
"perspective": true,
|
||||
"distance": 21.05263157877764,
|
||||
"phi": 38.925517117715195,
|
||||
"thetaOffset": -138.92710744521386,
|
||||
"targetOffset": [
|
||||
-2.6847696124888896,
|
||||
0.2191229688744439,
|
||||
3.6086809432821955e-16
|
||||
],
|
||||
"target": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"targetOrientation": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1
|
||||
],
|
||||
"fovy": 45,
|
||||
"near": 0.5,
|
||||
"far": 5000
|
||||
},
|
||||
"followMode": "follow-pose",
|
||||
"scene": {
|
||||
"transforms": {
|
||||
"showLabel": false,
|
||||
"editable": false,
|
||||
"labelSize": 0.049999999999999975,
|
||||
"enablePreloading": false,
|
||||
"lineWidth": 2
|
||||
}
|
||||
},
|
||||
"transforms": {
|
||||
"frame:camera_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_depth_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_depth_optical_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_rgb_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_rgb_optical_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:imu_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:caster_back_right_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:caster_back_left_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:odom": {
|
||||
"visible": true
|
||||
},
|
||||
"frame:base_footprint": {
|
||||
"visible": true
|
||||
},
|
||||
"frame:wheel_left_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:wheel_right_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:base_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:base_scan": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:map": {
|
||||
"visible": true
|
||||
}
|
||||
},
|
||||
"topics": {
|
||||
"/scan": {
|
||||
"visible": true,
|
||||
"colorField": "intensity",
|
||||
"colorMode": "flat",
|
||||
"colorMap": "turbo",
|
||||
"pointSize": 5,
|
||||
"flatColor": "#ff0000"
|
||||
},
|
||||
"/global_costmap/costmap": {
|
||||
"visible": true,
|
||||
"maxColor": "#E800174d",
|
||||
"unknownColor": "#5700ff4d",
|
||||
"minColor": "#ffffff4d",
|
||||
"invalidColor": "#ff00ff4d",
|
||||
"colorMode": "costmap",
|
||||
"alpha": 0.3
|
||||
},
|
||||
"/global_costmap/obstacle_layer": {
|
||||
"visible": false
|
||||
},
|
||||
"/global_costmap/voxel_marked_cloud": {
|
||||
"visible": false
|
||||
},
|
||||
"/goal_pose": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_costmap/costmap": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_costmap/voxel_layer": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_costmap/clearing_endpoints": {
|
||||
"visible": false,
|
||||
"colorField": "x",
|
||||
"colorMode": "colormap",
|
||||
"colorMap": "turbo"
|
||||
},
|
||||
"/map": {
|
||||
"visible": true,
|
||||
"minColor": "#ffffff",
|
||||
"maxColor": "#000000",
|
||||
"unknownColor": "#708986ff",
|
||||
"frameLocked": false,
|
||||
"colorMode": "map"
|
||||
},
|
||||
"/amcl_pose": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_plan": {
|
||||
"visible": true,
|
||||
"lineWidth": 0.01,
|
||||
"gradient": [
|
||||
"#c8ff00c7",
|
||||
"#00c8ffba"
|
||||
]
|
||||
},
|
||||
"/plan": {
|
||||
"visible": false,
|
||||
"gradient": [
|
||||
"rgba(124, 107, 255, 1)",
|
||||
"#ff6b6b"
|
||||
],
|
||||
"lineWidth": 1
|
||||
},
|
||||
"/plan_smoothed": {
|
||||
"visible": false
|
||||
},
|
||||
"/received_global_plan": {
|
||||
"visible": true,
|
||||
"gradient": [
|
||||
"#ff0000c7",
|
||||
"#6b70ffc2"
|
||||
],
|
||||
"lineWidth": 0.02,
|
||||
"type": "line",
|
||||
"arrowScale": [
|
||||
0.02,
|
||||
0.0015,
|
||||
0.0015
|
||||
]
|
||||
},
|
||||
"/transformed_global_plan": {
|
||||
"visible": false
|
||||
},
|
||||
"/robot_description": {
|
||||
"visible": false
|
||||
},
|
||||
"/cost_cloud": {
|
||||
"visible": false
|
||||
},
|
||||
"/initialpose": {
|
||||
"visible": false
|
||||
}
|
||||
},
|
||||
"publish": {
|
||||
"type": "pose_estimate",
|
||||
"poseTopic": "/move_base_simple/goal",
|
||||
"pointTopic": "",
|
||||
"poseEstimateTopic": "/initialpose",
|
||||
"poseEstimateXDeviation": 0.5,
|
||||
"poseEstimateYDeviation": 0.5,
|
||||
"poseEstimateThetaDeviation": 0.26179939
|
||||
},
|
||||
"followTf": "map"
|
||||
},
|
||||
"DiagnosticSummary!3bo4e39": {
|
||||
"minLevel": 0,
|
||||
"pinnedIds": [],
|
||||
"hardwareIdFilter": "",
|
||||
"topicToRender": "/diagnostics",
|
||||
"sortByLevel": true
|
||||
},
|
||||
"RosOut!1iib9dq": {
|
||||
"searchTerms": [],
|
||||
"minLogLevel": 1
|
||||
},
|
||||
"3D!2agiaqk": {
|
||||
"layers": {
|
||||
"845139cb-26bc-40b3-8161-8ab60af4baf5": {
|
||||
"visible": false,
|
||||
"frameLocked": true,
|
||||
"label": "Grid",
|
||||
"instanceId": "845139cb-26bc-40b3-8161-8ab60af4baf5",
|
||||
"layerId": "foxglove.Grid",
|
||||
"size": 10,
|
||||
"divisions": 10,
|
||||
"lineWidth": 1,
|
||||
"color": "#A0A0A4ff",
|
||||
"position": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"rotation": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"order": 1
|
||||
}
|
||||
},
|
||||
"cameraState": {
|
||||
"perspective": true,
|
||||
"distance": 4.078136514883917,
|
||||
"phi": 56.068374260572,
|
||||
"thetaOffset": 92.50000000000723,
|
||||
"targetOffset": [
|
||||
0.03816360663426963,
|
||||
0.15755079607173259,
|
||||
7.341598429161142e-18
|
||||
],
|
||||
"target": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"targetOrientation": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1
|
||||
],
|
||||
"fovy": 45,
|
||||
"near": 0.5,
|
||||
"far": 5000
|
||||
},
|
||||
"followMode": "follow-pose",
|
||||
"scene": {
|
||||
"transforms": {
|
||||
"showLabel": true,
|
||||
"editable": false,
|
||||
"labelSize": 0.049999999999999975,
|
||||
"enablePreloading": false,
|
||||
"lineWidth": 2
|
||||
}
|
||||
},
|
||||
"transforms": {
|
||||
"frame:camera_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_depth_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_depth_optical_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_rgb_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:camera_rgb_optical_frame": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:imu_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:caster_back_right_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:caster_back_left_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:odom": {
|
||||
"visible": true
|
||||
},
|
||||
"frame:base_footprint": {
|
||||
"visible": true
|
||||
},
|
||||
"frame:wheel_left_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:wheel_right_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:base_link": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:base_scan": {
|
||||
"visible": false
|
||||
},
|
||||
"frame:map": {
|
||||
"visible": true
|
||||
}
|
||||
},
|
||||
"topics": {
|
||||
"/scan": {
|
||||
"visible": true,
|
||||
"colorField": "intensity",
|
||||
"colorMode": "flat",
|
||||
"colorMap": "turbo",
|
||||
"pointSize": 5,
|
||||
"flatColor": "#ff0000"
|
||||
},
|
||||
"/global_costmap/costmap": {
|
||||
"visible": true,
|
||||
"maxColor": "#E800174d",
|
||||
"unknownColor": "#5700ff4d",
|
||||
"minColor": "#ffffff4d",
|
||||
"invalidColor": "#ff00ff4d",
|
||||
"colorMode": "costmap",
|
||||
"alpha": 0.3
|
||||
},
|
||||
"/global_costmap/obstacle_layer": {
|
||||
"visible": false
|
||||
},
|
||||
"/global_costmap/voxel_marked_cloud": {
|
||||
"visible": false
|
||||
},
|
||||
"/goal_pose": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_costmap/costmap": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_costmap/voxel_layer": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_costmap/clearing_endpoints": {
|
||||
"visible": false,
|
||||
"colorField": "x",
|
||||
"colorMode": "colormap",
|
||||
"colorMap": "turbo"
|
||||
},
|
||||
"/map": {
|
||||
"visible": true,
|
||||
"minColor": "#ffffff",
|
||||
"maxColor": "#000000",
|
||||
"unknownColor": "#708986ff",
|
||||
"frameLocked": false,
|
||||
"colorMode": "map"
|
||||
},
|
||||
"/amcl_pose": {
|
||||
"visible": false
|
||||
},
|
||||
"/local_plan": {
|
||||
"visible": true,
|
||||
"lineWidth": 0.01,
|
||||
"gradient": [
|
||||
"#c8ff00c7",
|
||||
"#00c8ffba"
|
||||
]
|
||||
},
|
||||
"/plan": {
|
||||
"visible": false,
|
||||
"gradient": [
|
||||
"rgba(124, 107, 255, 1)",
|
||||
"#ff6b6b"
|
||||
],
|
||||
"lineWidth": 1
|
||||
},
|
||||
"/plan_smoothed": {
|
||||
"visible": false
|
||||
},
|
||||
"/received_global_plan": {
|
||||
"visible": true,
|
||||
"gradient": [
|
||||
"#ff0000c7",
|
||||
"#6b70ffc2"
|
||||
],
|
||||
"lineWidth": 0.02,
|
||||
"type": "line",
|
||||
"arrowScale": [
|
||||
0.02,
|
||||
0.0015,
|
||||
0.0015
|
||||
]
|
||||
},
|
||||
"/transformed_global_plan": {
|
||||
"visible": false
|
||||
},
|
||||
"/robot_description": {
|
||||
"visible": true
|
||||
},
|
||||
"/cost_cloud": {
|
||||
"visible": false
|
||||
},
|
||||
"/initialpose": {
|
||||
"visible": false
|
||||
}
|
||||
},
|
||||
"publish": {
|
||||
"type": "pose_estimate",
|
||||
"poseTopic": "/move_base_simple/goal",
|
||||
"pointTopic": "",
|
||||
"poseEstimateTopic": "/initialpose",
|
||||
"poseEstimateXDeviation": 0.5,
|
||||
"poseEstimateYDeviation": 0.5,
|
||||
"poseEstimateThetaDeviation": 0.26179939
|
||||
},
|
||||
"followTf": "base_link"
|
||||
}
|
||||
},
|
||||
"globalVariables": {},
|
||||
"userNodes": {},
|
||||
"playbackConfig": {
|
||||
"speed": 1
|
||||
},
|
||||
"layout": {
|
||||
"first": "3D!18i6zy7",
|
||||
"second": {
|
||||
"first": "DiagnosticSummary!3bo4e39",
|
||||
"second": {
|
||||
"first": "RosOut!1iib9dq",
|
||||
"second": "3D!2agiaqk",
|
||||
"direction": "column"
|
||||
},
|
||||
"direction": "column",
|
||||
"splitPercentage": 28.227360308285164
|
||||
},
|
||||
"direction": "row",
|
||||
"splitPercentage": 74.87855655794587
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
{
|
||||
"name": "Glances: {{placeholder "http.vars.ReqHost"}}",
|
||||
"short_name": "Glances: {{placeholder "http.vars.ReqHost"}}",
|
||||
"icons": [
|
||||
{
|
||||
"src": "/media/icons/glances/any_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "any"
|
||||
},
|
||||
{
|
||||
"src": "/media/icons/glances/maskable_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "maskable"
|
||||
}
|
||||
],
|
||||
"id": "/glances/",
|
||||
"start_url": "/glances/",
|
||||
"theme_color": "#2C363F",
|
||||
"background_color": "#2C363F",
|
||||
"display": "fullscreen",
|
||||
"shortcuts" : [
|
||||
{
|
||||
"name": "Refresh 1sec",
|
||||
"url": "/glances/1",
|
||||
"description": "Refresh page every 1 second"
|
||||
},
|
||||
{
|
||||
"name": "Refresh 5sec",
|
||||
"url": "/glances/5",
|
||||
"description": "Refresh page every 5 seconds"
|
||||
},
|
||||
{
|
||||
"name": "Refresh 10sec",
|
||||
"url": "/glances/10",
|
||||
"description": "Refresh page every 10 seconds"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
{
|
||||
"name": "Gzweb: {{placeholder "http.vars.ReqHost"}}",
|
||||
"short_name": "Gzweb: {{placeholder "http.vars.ReqHost"}}",
|
||||
"icons": [
|
||||
{
|
||||
"src": "/media/icons/gzweb/any_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "any"
|
||||
},
|
||||
{
|
||||
"src": "/media/icons/gzweb/maskable_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "maskable"
|
||||
}
|
||||
],
|
||||
"id": "/gzweb/",
|
||||
"start_url": "/gzweb/",
|
||||
"theme_color": "#ffffff",
|
||||
"background_color": "#ffffff",
|
||||
"display": "fullscreen"
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
{
|
||||
"name": "Nav2: {{placeholder "http.vars.ReqHost"}}",
|
||||
"short_name": "Nav2: {{placeholder "http.vars.ReqHost"}}",
|
||||
"icons": [
|
||||
{
|
||||
"src": "/media/icons/nav2/any_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "any"
|
||||
},
|
||||
{
|
||||
"src": "/media/icons/nav2/maskable_icon_x512.webp",
|
||||
"sizes": "512x512",
|
||||
"type": "image/webp",
|
||||
"purpose": "maskable"
|
||||
}
|
||||
],
|
||||
"id": "/nav2/",
|
||||
"start_url": "/nav2/",
|
||||
"theme_color": "#ffffff",
|
||||
"background_color": "#ffffff",
|
||||
"display": "standalone"
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,36 @@
|
||||
{{$pathParts := splitList "/" .OriginalReq.URL.Path}}
|
||||
{{$markdownFilename := default "index" (slice $pathParts 2 | join "/")}}
|
||||
{{$markdownFilePath := printf "/%s.md" $markdownFilename}}
|
||||
{{if not (fileExists $markdownFilePath)}}{{httpError 404}}{{end}}
|
||||
{{$markdownFile := (include $markdownFilePath | splitFrontMatter)}}
|
||||
{{$title := default $markdownFilename $markdownFile.Meta.title}}
|
||||
<!DOCTYPE html>
|
||||
<html>
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1, minimal-ui">
|
||||
<title>{{$title}}</title>
|
||||
<meta name="color-scheme" content="light dark">
|
||||
<link rel="stylesheet" href="github-markdown.css">
|
||||
<link rel="icon" type="image/webp" href="/media/icons/nav2/any_icon_x512.webp">
|
||||
|
||||
<style>
|
||||
body {
|
||||
box-sizing: border-box;
|
||||
min-width: 200px;
|
||||
max-width: 980px;
|
||||
margin: 0 auto;
|
||||
padding: 45px;
|
||||
}
|
||||
|
||||
@media (prefers-color-scheme: dark) {
|
||||
body {
|
||||
background-color: #0d1117;
|
||||
}
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<article class="markdown-body">{{markdown $markdownFile.Body}}</article>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,51 @@
|
||||
{
|
||||
"title": "Nav2 App"
|
||||
}
|
||||
## Progressive Web Apps
|
||||
|
||||
| PWAs | Shortcuts |
|
||||
|-|-|
|
||||
| [<img src="/media/icons/foxglove/any_icon_x512.webp" height="64">](/foxglove/autoconnect)<br>**Foxglove** | [**Auto Connect**](/foxglove/autoconnect)<br>[Auto Layout](/foxglove/autolayout)<br>[Manual](/foxglove/) |
|
||||
| [<img src="/media/icons/gzweb/any_icon_x512.webp" height="64">](/gzweb/)<br>**Gzweb** | [**Auto Connect**](/gzweb/) |
|
||||
| [<img src="/media/icons/glances/any_icon_x512.webp" height="64">](/glances/)<br>**Glances** | [**System Monitor**](/glances/)<br>[Refresh 1sec](/glances/1)<br>[Refresh 10sec](/glances/10) |
|
||||
| [<img src="/media/icons/nav2/any_icon_x512.webp" height="64">](/nav2/)<br>**Nav2** | [**App Launcher**](/nav2/)<br>[File Browser](/?browse=true) |
|
||||
|
||||
## External Resources
|
||||
|
||||
For more related documentation:
|
||||
|
||||
- [Nav2 Documentation](https://navigation.ros.org)
|
||||
- [Development Guides](https://navigation.ros.org/development_guides)
|
||||
- [Dev Containers](https://navigation.ros.org/development_guides/devcontainer_docs)
|
||||
|
||||
## Session Info
|
||||
|
||||
Useful information about host server and remote client:
|
||||
|
||||
|Key | Value |
|
||||
|-|-|
|
||||
| Host | `{{.Host}}` |
|
||||
| Remote IP | `{{placeholder "http.request.remote.host"}}` |
|
||||
| Date | `{{now}}` |
|
||||
|
||||
### Server Diagnostics
|
||||
|
||||
<details>
|
||||
<summary>Websocket Debug</summary>
|
||||
|
||||
For troubleshooting websocket connections:
|
||||
|
||||
|Key | Value |
|
||||
|-|-|
|
||||
| `header.X-Forwarded-Host` | `{{placeholder "http.request.header.X-Forwarded-Host"}}` |
|
||||
| `http.request.hostport` | `{{placeholder "http.request.hostport"}}` |
|
||||
| `http.vars.ReqHost` | `{{placeholder "http.vars.ReqHost"}}` |
|
||||
|
||||
|Key | Value |
|
||||
|-|-|
|
||||
| `http.request.scheme` | `{{placeholder "http.request.scheme"}}` |
|
||||
| `header.X-Forwarded-Scheme` | `{{placeholder "http.request.header.X-Forwarded-Scheme"}}` |
|
||||
| `header.X-Forwarded-Proto` | `{{placeholder "http.request.header.X-Forwarded-Proto"}}` |
|
||||
| `http.vars.WsScheme` | `{{placeholder "http.vars.WsScheme"}}` |
|
||||
|
||||
</details>
|
||||
@@ -0,0 +1,61 @@
|
||||
{
|
||||
"name": "Nav2",
|
||||
"build": {
|
||||
"dockerfile": "../Dockerfile",
|
||||
"context": "..",
|
||||
"target": "visualizer",
|
||||
"cacheFrom": "ghcr.io/ros-planning/navigation2:humble"
|
||||
},
|
||||
"runArgs": [
|
||||
// "--cap-add=SYS_PTRACE", // enable debugging, e.g. gdb
|
||||
// "--ipc=host", // shared memory transport with host, e.g. rviz GUIs
|
||||
// "--network=host", // network access to host interfaces, e.g. eth0
|
||||
// "--pid=host", // DDS discovery with host, without --network=host
|
||||
// "--privileged", // device access to host peripherals, e.g. USB
|
||||
// "--security-opt=seccomp=unconfined", // enable debugging, e.g. gdb
|
||||
],
|
||||
"workspaceFolder": "/opt/overlay_ws/src/navigation2",
|
||||
"workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind",
|
||||
"onCreateCommand": ".devcontainer/on-create-command.sh",
|
||||
"updateContentCommand": ".devcontainer/update-content-command.sh",
|
||||
"postCreateCommand": ".devcontainer/post-create-command.sh",
|
||||
"remoteEnv": {
|
||||
"OVERLAY_MIXINS": "release ccache lld",
|
||||
"CCACHE_DIR": "/tmp/.ccache"
|
||||
},
|
||||
"mounts": [
|
||||
{
|
||||
"source": "ccache-${devcontainerId}",
|
||||
"target": "/tmp/.ccache",
|
||||
"type": "volume"
|
||||
},
|
||||
{
|
||||
"source": "overlay-${devcontainerId}",
|
||||
"target": "/opt/overlay_ws",
|
||||
"type": "volume"
|
||||
}
|
||||
],
|
||||
"features": {
|
||||
// "ghcr.io/devcontainers/features/desktop-lite:1": {},
|
||||
"ghcr.io/devcontainers/features/github-cli:1": {}
|
||||
},
|
||||
"customizations": {
|
||||
"codespaces": {
|
||||
"openFiles": [
|
||||
"doc/development/codespaces.md"
|
||||
]
|
||||
},
|
||||
"vscode": {
|
||||
"settings": {},
|
||||
"extensions": [
|
||||
"althack.ament-task-provider",
|
||||
"eamodio.gitlens",
|
||||
"esbenp.prettier-vscode",
|
||||
"GitHub.copilot",
|
||||
"ms-iot.vscode-ros",
|
||||
"streetsidesoftware.code-spell-checker",
|
||||
"twxs.cmake"
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
+12
@@ -0,0 +1,12 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Immediately catch all errors
|
||||
set -eo pipefail
|
||||
|
||||
# Uncomment for debugging
|
||||
# set -x
|
||||
# env
|
||||
|
||||
git config --global --add safe.directory "*"
|
||||
|
||||
.devcontainer/update-content-command.sh
|
||||
+18
@@ -0,0 +1,18 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Immediately catch all errors
|
||||
set -eo pipefail
|
||||
|
||||
# Uncomment for debugging
|
||||
# set -x
|
||||
# env
|
||||
|
||||
# Enable autocomplete for user
|
||||
cp /etc/skel/.bashrc ~/
|
||||
|
||||
# Check if srv folder exists
|
||||
if [ -d "$ROOT_SRV" ]; then
|
||||
# Setup Nav2 web app
|
||||
for dir in $OVERLAY_WS/src/navigation2/.devcontainer/caddy/srv/*; \
|
||||
do if [ -d "$dir" ]; then ln -s "$dir" $ROOT_SRV; fi done
|
||||
fi
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Immediately catch all errors
|
||||
set -eo pipefail
|
||||
|
||||
# Uncomment for debugging
|
||||
# set -x
|
||||
# env
|
||||
|
||||
cd $OVERLAY_WS
|
||||
|
||||
colcon cache lock
|
||||
|
||||
BUILD_UNFINISHED=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-skip-build-finished \
|
||||
| xargs)
|
||||
echo BUILD_UNFINISHED: $BUILD_UNFINISHED
|
||||
|
||||
BUILD_FAILED=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-select-build-failed \
|
||||
| xargs)
|
||||
echo BUILD_FAILED: $BUILD_FAILED
|
||||
|
||||
BUILD_INVALID=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-select-cache-invalid \
|
||||
--packages-select-cache-key build \
|
||||
| xargs)
|
||||
echo BUILD_INVALID: $BUILD_INVALID
|
||||
|
||||
BUILD_PACKAGES=""
|
||||
if [ -n "$BUILD_UNFINISHED" ] || \
|
||||
[ -n "$BUILD_FAILED" ] || \
|
||||
[ -n "$BUILD_INVALID" ]
|
||||
then
|
||||
BUILD_PACKAGES=$(
|
||||
colcon list \
|
||||
--names-only \
|
||||
--packages-above \
|
||||
$BUILD_UNFINISHED \
|
||||
$BUILD_FAILED \
|
||||
$BUILD_INVALID \
|
||||
| xargs)
|
||||
fi
|
||||
echo BUILD_PACKAGES: $BUILD_PACKAGES
|
||||
|
||||
# colcon clean packages --yes \
|
||||
# --packages-select ${BUILD_PACKAGES} \
|
||||
# --base-select install
|
||||
|
||||
. $UNDERLAY_WS/install/setup.sh
|
||||
colcon build \
|
||||
--symlink-install \
|
||||
--mixin $OVERLAY_MIXINS \
|
||||
--packages-select ${BUILD_PACKAGES}
|
||||
@@ -0,0 +1,12 @@
|
||||
################################################################################
|
||||
# Repo
|
||||
|
||||
.circleci/
|
||||
.devcontainer/
|
||||
.dockerignore
|
||||
.git/
|
||||
.github/
|
||||
.gitignore
|
||||
**.Dockerfile
|
||||
**Dockerfile
|
||||
doc/
|
||||
+45
@@ -0,0 +1,45 @@
|
||||
<!--
|
||||
For general questions, please ask on ROS answers: https://answers.ros.org, make sure to include at least the `ros2` tag and the rosdistro version you are running, e.g. `ardent`.
|
||||
For general design discussions, please post on discourse: https://discourse.ros.org/c/ng-ros
|
||||
Not sure if this is the right repository? Open an issue on https://github.com/ros-planning/navigation2
|
||||
For Bug report or feature requests, please fill out the relevant category below
|
||||
-->
|
||||
|
||||
## Bug report
|
||||
|
||||
**Required Info:**
|
||||
|
||||
- Operating System:
|
||||
- <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
|
||||
- ROS2 Version:
|
||||
- <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
|
||||
- Version or commit hash:
|
||||
- <!-- from source: output of `git -C navigation2 rev-parse HEAD
|
||||
apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"
|
||||
or: dpkg-query --show "ros-$ROS_DISTRO-nav2-*" -->
|
||||
- DDS implementation:
|
||||
- <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc.) -->
|
||||
|
||||
#### Steps to reproduce issue
|
||||
<!-- Detailed instructions on how to reliably reproduce this issue http://sscce.org/
|
||||
``` code that can be copy-pasted is preferred ``` -->
|
||||
```
|
||||
|
||||
```
|
||||
|
||||
#### Expected behavior
|
||||
|
||||
#### Actual behavior
|
||||
|
||||
#### Additional information
|
||||
|
||||
<!-- If you are reporting a bug delete everything below
|
||||
If you are requesting a feature deleted everything above this line -->
|
||||
----
|
||||
## Feature request
|
||||
|
||||
#### Feature description
|
||||
<!-- Description in a few sentences what the feature consists of and what problem it will solve -->
|
||||
|
||||
#### Implementation considerations
|
||||
<!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->
|
||||
+46
@@ -0,0 +1,46 @@
|
||||
<!-- Please fill out the following pull request template for non-trivial changes to help us process your PR faster and more efficiently.-->
|
||||
|
||||
---
|
||||
|
||||
## Basic Info
|
||||
|
||||
| Info | Please fill out this column |
|
||||
| ------ | ----------- |
|
||||
| Ticket(s) this addresses | (add tickets here #1) |
|
||||
| Primary OS tested on | (Ubuntu, MacOS, Windows) |
|
||||
| Robotic platform tested on | (Steve's Robot, gazebo simulation of Tally, hardware turtlebot) |
|
||||
|
||||
---
|
||||
|
||||
## Description of contribution in a few bullet points
|
||||
|
||||
<!--
|
||||
* I added this neat new feature
|
||||
* Also fixed a typo in a parameter name in nav2_costmap_2d
|
||||
-->
|
||||
|
||||
## Description of documentation updates required from your changes
|
||||
|
||||
<!--
|
||||
* Added new parameter, so need to add that to default configs and documentation page
|
||||
* I added some capabilities, need to document them
|
||||
-->
|
||||
|
||||
---
|
||||
|
||||
## Future work that may be required in bullet points
|
||||
|
||||
<!--
|
||||
* I think there might be some optimizations to be made from STL vector
|
||||
* I see alot of redundancy in this package, we might want to add a function `bool XYZ()` to reduce clutter
|
||||
* I tested on a differential drive robot, but there might be issues turning near corners on an omnidirectional platform
|
||||
-->
|
||||
|
||||
#### For Maintainers: <!-- DO NOT EDIT OR REMOVE -->
|
||||
- [ ] Check that any new parameters added are updated in navigation.ros.org
|
||||
- [ ] Check that any significant change is added to the migration guide
|
||||
- [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide
|
||||
- [ ] Check that any new functions have Doxygen added
|
||||
- [ ] Check that any new features have test coverage
|
||||
- [ ] Check that any new plugins is added to the plugins page
|
||||
- [ ] If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
|
||||
Vendored
+14
@@ -0,0 +1,14 @@
|
||||
version: 2
|
||||
updates:
|
||||
- package-ecosystem: "docker"
|
||||
directory: "/"
|
||||
schedule:
|
||||
interval: "daily"
|
||||
commit-message:
|
||||
prefix: "🐳"
|
||||
- package-ecosystem: "github-actions"
|
||||
directory: "/"
|
||||
schedule:
|
||||
interval: "daily"
|
||||
commit-message:
|
||||
prefix: "🛠️"
|
||||
Vendored
+73
@@ -0,0 +1,73 @@
|
||||
pull_request_rules:
|
||||
- name: backport to galactic at reviewers discretion
|
||||
conditions:
|
||||
- base=main
|
||||
- "label=backport-galactic"
|
||||
actions:
|
||||
backport:
|
||||
branches:
|
||||
- galactic
|
||||
|
||||
- name: backport to foxy at reviewers discretion
|
||||
conditions:
|
||||
- base=main
|
||||
- "label=backport-foxy"
|
||||
actions:
|
||||
backport:
|
||||
branches:
|
||||
- foxy-devel
|
||||
|
||||
- name: delete head branch after merge
|
||||
conditions:
|
||||
- merged
|
||||
actions:
|
||||
delete_head_branch:
|
||||
|
||||
- name: ask to resolve conflict
|
||||
conditions:
|
||||
- conflict
|
||||
- author!=mergify
|
||||
actions:
|
||||
comment:
|
||||
message: This pull request is in conflict. Could you fix it @{{author}}?
|
||||
|
||||
- name: development targets main branch
|
||||
conditions:
|
||||
- base!=main
|
||||
- author!=SteveMacenski
|
||||
- author!=mergify
|
||||
actions:
|
||||
comment:
|
||||
message: |
|
||||
@{{author}}, all pull requests must be targeted towards the `main` development branch.
|
||||
Once merged into `main`, it is possible to backport to @{{base}}, but it must be in `main`
|
||||
to have these changes reflected into new distributions.
|
||||
|
||||
- name: Main build failures
|
||||
conditions:
|
||||
- base=main
|
||||
- or:
|
||||
- "check-failure=ci/circleci: debug_build"
|
||||
- "check-failure=ci/circleci: release_build"
|
||||
actions:
|
||||
comment:
|
||||
message: |
|
||||
@{{author}}, your PR has failed to build. Please check CI outputs and resolve issues.
|
||||
You may need to rebase or pull in `main` due to API changes (or your contribution genuinely fails).
|
||||
|
||||
- name: Removed maintainer checklist
|
||||
conditions:
|
||||
- "-body~=^.*#### For Maintainers: <!-- DO NOT EDIT OR REMOVE -->.*$"
|
||||
- author!=SteveMacenski
|
||||
- author!=mergify
|
||||
actions:
|
||||
comment:
|
||||
message: |
|
||||
@{{author}}, please properly fill in PR template in the future. @stevemacenski, use this instead.
|
||||
- [ ] Check that any new parameters added are updated in navigation.ros.org
|
||||
- [ ] Check that any significant change is added to the migration guide
|
||||
- [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide
|
||||
- [ ] Check that any new functions have Doxygen added
|
||||
- [ ] Check that any new features have test coverage
|
||||
- [ ] Check that any new plugins is added to the plugins page
|
||||
- [ ] If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
|
||||
+132
@@ -0,0 +1,132 @@
|
||||
---
|
||||
name: Update CI Image
|
||||
|
||||
on:
|
||||
schedule:
|
||||
# 7am UTC, 12am PDT
|
||||
- cron: '0 7 * * *'
|
||||
push:
|
||||
branches:
|
||||
- main
|
||||
- jazzy
|
||||
- humble
|
||||
paths:
|
||||
- '**/package.xml'
|
||||
- '**/*.repos'
|
||||
- 'Dockerfile'
|
||||
- '.github/workflows/update_ci_image.yaml'
|
||||
|
||||
jobs:
|
||||
check_ci_files:
|
||||
name: Check CI Files
|
||||
runs-on: ubuntu-latest
|
||||
outputs:
|
||||
trigger: ${{ steps.check.outputs.trigger }}
|
||||
no_cache: ${{ steps.check.outputs.no_cache }}
|
||||
steps:
|
||||
- name: "Check package updates"
|
||||
id: check
|
||||
if: github.event_name == 'push'
|
||||
run: |
|
||||
echo "trigger=true" >> $GITHUB_OUTPUT
|
||||
echo "no_cache=false" >> $GITHUB_OUTPUT
|
||||
check_ci_image:
|
||||
name: Check CI Image
|
||||
if: github.event_name == 'schedule'
|
||||
needs: check_ci_files
|
||||
runs-on: ubuntu-latest
|
||||
outputs:
|
||||
trigger: ${{ steps.check.outputs.trigger }}
|
||||
no_cache: ${{ steps.check.outputs.no_cache }}
|
||||
container:
|
||||
image: ghcr.io/${{ github.repository }}:${{ github.ref_name }}
|
||||
steps:
|
||||
- name: "Check apt updates"
|
||||
id: check
|
||||
env:
|
||||
SOURCELIST: sources.list.d/ros2.list
|
||||
run: |
|
||||
apt-get update \
|
||||
-o Dir::Etc::sourcelist="${SOURCELIST}"
|
||||
apt-get --simulate upgrade \
|
||||
-o Dir::Etc::sourcelist="${SOURCELIST}" \
|
||||
> upgrade.log
|
||||
cat upgrade.log
|
||||
cat upgrade.log \
|
||||
| grep "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" \
|
||||
&& echo "trigger=false" >> $GITHUB_OUTPUT \
|
||||
|| echo "trigger=true" >> $GITHUB_OUTPUT
|
||||
echo "no_cache=true" >> $GITHUB_OUTPUT
|
||||
rebuild_ci_image:
|
||||
name: Rebuild CI Image
|
||||
if: always()
|
||||
needs:
|
||||
- check_ci_files
|
||||
- check_ci_image
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v3
|
||||
- name: Set up Docker Buildx
|
||||
uses: docker/setup-buildx-action@v2
|
||||
- name: Login to Docker Hub
|
||||
uses: docker/login-action@v2
|
||||
with:
|
||||
registry: ghcr.io
|
||||
username: ${{ github.repository_owner }}
|
||||
password: ${{ secrets.GITHUB_TOKEN }}
|
||||
- name: Set build config
|
||||
id: config
|
||||
run: |
|
||||
created=$(date -u +'%Y-%m-%dT%H:%M:%SZ')
|
||||
echo "created=${created}" >> $GITHUB_OUTPUT
|
||||
|
||||
version=$(grep -oP '(?<=<version>).*?(?=</version>)' navigation2/package.xml)
|
||||
echo "version=${version}" >> $GITHUB_OUTPUT
|
||||
|
||||
no_cache=false
|
||||
if [ "${{needs.check_ci_files.outputs.no_cache}}" == 'true' ] || \
|
||||
[ "${{needs.check_ci_image.outputs.no_cache}}" == 'true' ]
|
||||
then
|
||||
no_cache=true
|
||||
fi
|
||||
echo "no_cache=${no_cache}" >> $GITHUB_OUTPUT
|
||||
|
||||
trigger=false
|
||||
if [ "${{needs.check_ci_files.outputs.trigger}}" == 'true' ] || \
|
||||
[ "${{needs.check_ci_image.outputs.trigger}}" == 'true' ]
|
||||
then
|
||||
trigger=true
|
||||
fi
|
||||
echo "trigger=${trigger}" >> $GITHUB_OUTPUT
|
||||
- name: Build and push ${{ github.ref_name }}
|
||||
if: steps.config.outputs.trigger == 'true'
|
||||
id: docker_build
|
||||
uses: docker/build-push-action@v3
|
||||
with:
|
||||
context: .
|
||||
pull: true
|
||||
push: true
|
||||
provenance: false
|
||||
no-cache: ${{ steps.config.outputs.no_cache }}
|
||||
cache-from: type=registry,ref=ghcr.io/${{ github.repository }}:${{ github.ref_name }}
|
||||
cache-to: type=inline
|
||||
target: builder
|
||||
tags: |
|
||||
ghcr.io/${{ github.repository }}:${{ github.ref_name }}
|
||||
ghcr.io/${{ github.repository }}:${{ github.ref_name }}-${{ steps.config.outputs.version }}
|
||||
labels: |
|
||||
org.opencontainers.image.authors=${{ github.event.repository.owner.html_url }}
|
||||
org.opencontainers.image.created=${{ steps.config.outputs.created }}
|
||||
org.opencontainers.image.description=${{ github.event.repository.description }}
|
||||
org.opencontainers.image.documentation=${{ github.event.repository.homepage }}
|
||||
org.opencontainers.image.licenses=${{ github.event.repository.license.spdx_id }}
|
||||
org.opencontainers.image.ref.name=${{ github.ref }}
|
||||
org.opencontainers.image.revision=${{ github.sha }}
|
||||
org.opencontainers.image.source=${{ github.event.repository.clone_url }}
|
||||
org.opencontainers.image.title=${{ github.event.repository.name }}
|
||||
org.opencontainers.image.url=${{ github.event.repository.html_url }}
|
||||
org.opencontainers.image.vendor=${{ github.event.repository.owner.login }}
|
||||
org.opencontainers.image.version=${{ steps.config.outputs.version }}
|
||||
- name: Image digest
|
||||
if: steps.config.outputs.trigger == 'true'
|
||||
run: echo ${{ steps.docker_build.outputs.digest }}
|
||||
@@ -0,0 +1,56 @@
|
||||
# Compiled Object files
|
||||
*.slo
|
||||
*.lo
|
||||
*.o
|
||||
*.obj
|
||||
|
||||
# Precompiled Headers
|
||||
*.gch
|
||||
*.pch
|
||||
|
||||
# Compiled Dynamic libraries
|
||||
*.so
|
||||
*.dylib
|
||||
*.dll
|
||||
|
||||
# Fortran module files
|
||||
*.mod
|
||||
*.smod
|
||||
|
||||
# Compiled Static libraries
|
||||
*.lai
|
||||
*.la
|
||||
*.a
|
||||
*.lib
|
||||
|
||||
# Executables
|
||||
*.exe
|
||||
*.out
|
||||
*.app
|
||||
|
||||
# Colcon output
|
||||
build
|
||||
log
|
||||
install
|
||||
|
||||
# Visual Studio Code files
|
||||
.vscode
|
||||
|
||||
# Eclipse project files
|
||||
.cproject
|
||||
.project
|
||||
.pydevproject
|
||||
|
||||
# Python artifacts
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.ipynb_checkpoints
|
||||
|
||||
sphinx_doc/_build
|
||||
|
||||
# CLion artifacts
|
||||
.idea
|
||||
cmake-build-debug/
|
||||
|
||||
# doxygen docs
|
||||
doc/html/
|
||||
@@ -0,0 +1,227 @@
|
||||
# This dockerfile can be configured via --build-arg
|
||||
# Build context must be the /navigation2 root folder for COPY.
|
||||
# Example build command:
|
||||
# export UNDERLAY_MIXINS="debug ccache lld"
|
||||
# export OVERLAY_MIXINS="debug ccache coverage-gcc lld"
|
||||
# docker build -t nav2:latest \
|
||||
# --build-arg UNDERLAY_MIXINS \
|
||||
# --build-arg OVERLAY_MIXINS ./
|
||||
ARG FROM_IMAGE=ros:humble
|
||||
ARG UNDERLAY_WS=/opt/underlay_ws
|
||||
ARG OVERLAY_WS=/opt/overlay_ws
|
||||
|
||||
# multi-stage for caching
|
||||
FROM $FROM_IMAGE AS cacher
|
||||
|
||||
# clone underlay source
|
||||
ARG UNDERLAY_WS
|
||||
WORKDIR $UNDERLAY_WS/src
|
||||
COPY ./tools/underlay.repos ../
|
||||
RUN vcs import ./ < ../underlay.repos
|
||||
|
||||
# copy overlay source
|
||||
ARG OVERLAY_WS
|
||||
WORKDIR $OVERLAY_WS/src
|
||||
COPY ./ ./navigation2
|
||||
|
||||
# copy manifests for caching
|
||||
WORKDIR /opt
|
||||
RUN find . -name "src" -type d \
|
||||
-mindepth 1 -maxdepth 2 -printf '%P\n' \
|
||||
| xargs -I % mkdir -p /tmp/opt/% && \
|
||||
find . -name "package.xml" \
|
||||
| xargs cp --parents -t /tmp/opt && \
|
||||
find . -name "COLCON_IGNORE" \
|
||||
| xargs cp --parents -t /tmp/opt || true
|
||||
|
||||
# multi-stage for building
|
||||
FROM $FROM_IMAGE AS builder
|
||||
|
||||
# config dependencies install
|
||||
ARG DEBIAN_FRONTEND=noninteractive
|
||||
RUN echo '\
|
||||
APT::Install-Recommends "0";\n\
|
||||
APT::Install-Suggests "0";\n\
|
||||
' > /etc/apt/apt.conf.d/01norecommend
|
||||
ENV PYTHONUNBUFFERED 1
|
||||
|
||||
# install CI dependencies
|
||||
ARG RTI_NC_LICENSE_ACCEPTED=yes
|
||||
RUN apt-get update && \
|
||||
apt-get upgrade -y --with-new-pkgs && \
|
||||
apt-get install -y \
|
||||
ccache \
|
||||
lcov \
|
||||
lld \
|
||||
python3-pip \
|
||||
ros-$ROS_DISTRO-rmw-fastrtps-cpp \
|
||||
ros-$ROS_DISTRO-rmw-connextdds \
|
||||
ros-$ROS_DISTRO-rmw-cyclonedds-cpp \
|
||||
&& pip3 install \
|
||||
fastcov \
|
||||
git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \
|
||||
git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \
|
||||
&& rosdep update \
|
||||
&& colcon mixin update \
|
||||
&& colcon metadata update \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# install underlay dependencies
|
||||
ARG UNDERLAY_WS
|
||||
ENV UNDERLAY_WS $UNDERLAY_WS
|
||||
WORKDIR $UNDERLAY_WS
|
||||
COPY --from=cacher /tmp/$UNDERLAY_WS ./
|
||||
RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
|
||||
apt-get update && rosdep install -q -y \
|
||||
--from-paths src \
|
||||
--skip-keys " \
|
||||
slam_toolbox \
|
||||
" \
|
||||
--ignore-src \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# build underlay source
|
||||
COPY --from=cacher $UNDERLAY_WS ./
|
||||
ARG UNDERLAY_MIXINS="release ccache lld"
|
||||
ARG CCACHE_DIR="$UNDERLAY_WS/.ccache"
|
||||
RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
|
||||
colcon cache lock && \
|
||||
colcon build \
|
||||
--symlink-install \
|
||||
--mixin $UNDERLAY_MIXINS \
|
||||
--event-handlers console_direct+
|
||||
|
||||
# install overlay dependencies
|
||||
ARG OVERLAY_WS
|
||||
ENV OVERLAY_WS $OVERLAY_WS
|
||||
WORKDIR $OVERLAY_WS
|
||||
COPY --from=cacher /tmp/$OVERLAY_WS ./
|
||||
RUN . $UNDERLAY_WS/install/setup.sh && \
|
||||
apt-get update && rosdep install -q -y \
|
||||
--from-paths src \
|
||||
--skip-keys " \
|
||||
slam_toolbox \
|
||||
"\
|
||||
--ignore-src \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# multi-stage for testing
|
||||
FROM builder AS tester
|
||||
|
||||
# build overlay source
|
||||
COPY --from=cacher $OVERLAY_WS ./
|
||||
ARG OVERLAY_MIXINS="release ccache lld"
|
||||
ARG CCACHE_DIR="$OVERLAY_WS/.ccache"
|
||||
RUN . $UNDERLAY_WS/install/setup.sh && \
|
||||
colcon cache lock && \
|
||||
colcon build \
|
||||
--symlink-install \
|
||||
--mixin $OVERLAY_MIXINS
|
||||
|
||||
# source overlay from entrypoint
|
||||
RUN sed --in-place \
|
||||
's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
|
||||
/ros_entrypoint.sh
|
||||
|
||||
# test overlay build
|
||||
ARG RUN_TESTS
|
||||
ARG FAIL_ON_TEST_FAILURE
|
||||
RUN if [ -n "$RUN_TESTS" ]; then \
|
||||
. install/setup.sh && \
|
||||
colcon test && \
|
||||
colcon test-result \
|
||||
|| ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \
|
||||
fi
|
||||
|
||||
# multi-stage for developing
|
||||
FROM builder AS dever
|
||||
|
||||
# edit apt for caching
|
||||
RUN mv /etc/apt/apt.conf.d/docker-clean /etc/apt/
|
||||
|
||||
# install developer dependencies
|
||||
RUN apt-get update && \
|
||||
apt-get install -y \
|
||||
bash-completion \
|
||||
gdb \
|
||||
wget && \
|
||||
pip3 install \
|
||||
bottle \
|
||||
glances
|
||||
|
||||
# source underlay for shell
|
||||
RUN echo 'source "$UNDERLAY_WS/install/setup.bash"' >> /etc/bash.bashrc
|
||||
|
||||
# multi-stage for caddy
|
||||
FROM caddy:builder AS caddyer
|
||||
|
||||
# build custom modules
|
||||
RUN xcaddy build \
|
||||
--with github.com/caddyserver/replace-response
|
||||
|
||||
# multi-stage for visualizing
|
||||
FROM dever AS visualizer
|
||||
|
||||
ENV ROOT_SRV /srv
|
||||
RUN mkdir -p $ROOT_SRV
|
||||
|
||||
# install demo dependencies
|
||||
RUN apt-get update && apt-get install -y \
|
||||
ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \
|
||||
ros-$ROS_DISTRO-rviz2 \
|
||||
ros-$ROS_DISTRO-turtlebot3-simulations
|
||||
|
||||
# install gzweb dependacies
|
||||
RUN apt-get install -y --no-install-recommends \
|
||||
imagemagick \
|
||||
libboost-all-dev \
|
||||
libgazebo-dev \
|
||||
libgts-dev \
|
||||
libjansson-dev \
|
||||
libtinyxml-dev \
|
||||
nodejs \
|
||||
npm \
|
||||
psmisc \
|
||||
xvfb
|
||||
|
||||
# clone gzweb
|
||||
ENV GZWEB_WS /opt/gzweb
|
||||
RUN git clone https://github.com/osrf/gzweb.git $GZWEB_WS
|
||||
|
||||
# setup gzweb
|
||||
RUN cd $GZWEB_WS && . /usr/share/gazebo/setup.sh && \
|
||||
GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(find /opt/ros/$ROS_DISTRO/share \
|
||||
-mindepth 1 -maxdepth 2 -type d -name "models" | paste -s -d: -) && \
|
||||
sed -i "s|var modelList =|var modelList = []; var oldModelList =|g" gz3d/src/gzgui.js && \
|
||||
xvfb-run -s "-screen 0 1280x1024x24" ./deploy.sh -m local && \
|
||||
ln -s $GZWEB_WS/http/client/assets http/client/assets/models && \
|
||||
ln -s $GZWEB_WS/http/client $ROOT_SRV/gzweb
|
||||
|
||||
# patch gzsever
|
||||
RUN GZSERVER=$(which gzserver) && \
|
||||
mv $GZSERVER $GZSERVER.orig && \
|
||||
echo '#!/bin/bash' > $GZSERVER && \
|
||||
echo 'exec xvfb-run -s "-screen 0 1280x1024x24" gzserver.orig "$@"' >> $GZSERVER && \
|
||||
chmod +x $GZSERVER
|
||||
|
||||
# install foxglove dependacies
|
||||
RUN apt-get install -y --no-install-recommends \
|
||||
ros-$ROS_DISTRO-foxglove-bridge
|
||||
|
||||
# setup foxglove
|
||||
# Use custom fork until PR is merged:
|
||||
# https://github.com/foxglove/studio/pull/5987
|
||||
# COPY --from=ghcr.io/foxglove/studio /src $ROOT_SRV/foxglove
|
||||
COPY --from=ghcr.io/ruffsl/foxglove_studio@sha256:8a2f2be0a95f24b76b0d7aa536f1c34f3e224022eed607cbf7a164928488332e /src $ROOT_SRV/foxglove
|
||||
|
||||
# install web server
|
||||
COPY --from=caddyer /usr/bin/caddy /usr/bin/caddy
|
||||
|
||||
# download media files
|
||||
RUN mkdir -p $ROOT_SRV/media && cd /tmp && \
|
||||
export ICONS="icons.tar.gz" && wget https://github.com/ros-planning/navigation2/files/11506823/$ICONS && \
|
||||
echo "cae5e2a5230f87b004c8232b579781edb4a72a7431405381403c6f9e9f5f7d41 $ICONS" | sha256sum -c && \
|
||||
tar xvz -C $ROOT_SRV/media -f $ICONS && rm $ICONS
|
||||
|
||||
# multi-stage for exporting
|
||||
FROM tester AS exporter
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,17 @@
|
||||
Portions of this repository are available under one of the following licenses
|
||||
|
||||
SPDX-ID:
|
||||
* LGPL-2.1-or-later
|
||||
* Apache-2.0
|
||||
* BSD-3-Clause
|
||||
* Apache-2.0 AND BSD-3-Clause
|
||||
|
||||
Please see the package.xml file for each package to see the specific license for
|
||||
that package.
|
||||
|
||||
Contributions to existing files should be made under the license of that file.
|
||||
New files should be made under the first license listed in the appropriate
|
||||
package.xml file
|
||||
|
||||
For files that are not otherwise marked, they are provided under the Apache-2.0
|
||||
license.
|
||||
@@ -0,0 +1,111 @@
|
||||
# Nav2
|
||||
[](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml)
|
||||
[](https://codecov.io/gh/ros-planning/navigation2)
|
||||
|
||||
<p align="center">
|
||||
<img height="300" src="doc/nav2_logo.png" />
|
||||
</p>
|
||||
|
||||
For detailed instructions on how to:
|
||||
- [Getting Started](https://navigation.ros.org/getting_started/index.html)
|
||||
- [Concepts](https://navigation.ros.org/concepts/index.html)
|
||||
- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build)
|
||||
- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install)
|
||||
- [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html)
|
||||
- [Configure](https://navigation.ros.org/configuration/index.html)
|
||||
- [Navigation Plugins](https://navigation.ros.org/plugins/index.html)
|
||||
- [Migration Guides](https://navigation.ros.org/migration/index.html)
|
||||
- [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2)
|
||||
- [Contribute](https://navigation.ros.org/development_guides/involvement_docs/index.html)
|
||||
|
||||
Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
|
||||
|
||||
If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org.
|
||||
|
||||
## Our Sponsors
|
||||
|
||||
Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.
|
||||
|
||||
<p align="center">
|
||||
<img src="doc/sponsors_may_2023.png" />
|
||||
</p>
|
||||
|
||||
### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights.
|
||||
|
||||
### [Polymath Robotics](https://www.polymathrobotics.com/) creates safety-critical navigation systems for industrial vehicles that are radically simple to enable and deploy.
|
||||
|
||||
### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more.
|
||||
|
||||
### Confidential is just happy to support Nav2's mission!
|
||||
|
||||
|
||||
## Citation
|
||||
|
||||
If you use the navigation framework, an algorithm from this repository, or ideas from it
|
||||
please cite this work in your papers!
|
||||
|
||||
- S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
|
||||
|
||||
```bibtex
|
||||
@InProceedings{macenski2020marathon2,
|
||||
title = {The Marathon 2: A Navigation System},
|
||||
author = {Macenski, Steve and Martín, Francisco and White, Ruffin and Ginés Clavero, Jonatan},
|
||||
year = {2020},
|
||||
booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
|
||||
url = {https://github.com/ros-planning/navigation2},
|
||||
pdf = {https://arxiv.org/abs/2003.00368}
|
||||
}
|
||||
```
|
||||
|
||||
If you use our work on VSLAM and formal comparisons for service robot needs, please cite the paper:
|
||||
|
||||
- A. Merzlyakov, S. Macenski. [**A Comparison of Modern General-Purpose Visual SLAM Approaches**](https://arxiv.org/abs/2107.07589). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2021.
|
||||
|
||||
```bibtex
|
||||
@InProceedings{vslamComparison2021,
|
||||
title = {A Comparison of Modern General-Purpose Visual SLAM Approaches},
|
||||
author = {Merzlyakov, Alexey and Macenski, Steven},
|
||||
year = {2021},
|
||||
booktitle = {2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
|
||||
pdf = {https://arxiv.org/abs/2107.07589}
|
||||
}
|
||||
```
|
||||
|
||||
## Build Status
|
||||
|
||||
| Service | Foxy | Humble | Main |
|
||||
| :---: | :---: | :---: | :---: |
|
||||
| ROS Build Farm | [](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | [](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | N/A |
|
||||
| Circle CI | N/A | N/A | [](https://circleci.com/gh/ros-planning/navigation2/tree/main) |
|
||||
|
||||
|
||||
| Package | Foxy Source | Foxy Debian | Humble Source | Humble Debian |
|
||||
| :---: | :---: | :---: | :---: | :---: |
|
||||
| Navigation2 | [](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_amcl | [](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_behavior_tree | [](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_{recoveries, behaviors} | [](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_bringup | [](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_bt_navigator | [](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_common | [](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_constrained_smoother | N/A | N/A | N/A | N/A | [](https://build.ros2.org/job/Hsrc_uJ__nav2_constrained_smoother__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_controller | [](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_core | [](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_costmap_2d | [](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_dwb_controller | [](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_lifecycle_manager | [](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_map_server | [](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_msgs | [](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_navfn_planner | [](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_planner | [](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_regulated_pure_pursuit | [](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [](https://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_rotation_shim_controller | N/A | N/A | N/A | N/A | [](https://build.ros2.org/job/Hsrc_uJ__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_rviz_plugins | [](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_simple_commander | N/A | N/A | [](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_smac_planner | [](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_smoother | N/A | N/A | N/A | N/A | [](https://build.ros2.org/job/Hsrc_uJ__nav2_smoother__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_smoother__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_system_tests | [](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_theta_star_planner | N/A | N/A | [](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_util | [](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_voxel_grid | [](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/) | [](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) |
|
||||
| nav2_waypoint_follower | [](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | [](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/) | [](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) |
|
||||
@@ -0,0 +1,13 @@
|
||||
fixes:
|
||||
- "src/navigation2/::"
|
||||
- "install/::"
|
||||
|
||||
ignore:
|
||||
- "*/**/test/*" # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests
|
||||
- "*/test/**/*" # ignore package test directories, e.g. nav2_costmap_2d/tests
|
||||
- "**/test_*.*" # ignore files starting with test_ e.g. nav2_map_server/test/test_constants.cpp
|
||||
- "**/*_tests.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp
|
||||
- "*/**/benchmark/*" # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests
|
||||
- "*/benchmark/**/*" # ignore package test directories, e.g. nav2_costmap_2d/tests
|
||||
- "**/benchmark_*.*" # ignore files starting with test_ e.g. nav2_map_server/test/test_constants.cpp
|
||||
- "**/*_benchmark.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp
|
||||
@@ -0,0 +1,17 @@
|
||||
# ROS2 Navigation System Documentation
|
||||
This is where the ROS2 Navigation System documentation is being collected and vetted.
|
||||
|
||||
# Use Cases
|
||||
See the [Use Cases README](use_cases/README.md) for info on our target use cases.
|
||||
|
||||
# Requirements
|
||||
See the [Requirements document](requirements/requirements.md) for the current list of requirements.
|
||||
|
||||
# Design Overview
|
||||
See the [Navigation 2 Overview](design/Navigation_2_Overview.pdf) file for the current design / architecture
|
||||
|
||||
# Differences from ROS Navigation
|
||||
See the [ROS_COMPARISON](design/ROS_COMPARISON.md) file for an overview of the differences between this design and ROS1 Navigation (move_base)
|
||||
|
||||
# Contributing
|
||||
To propose additions or changes to the design or requirements, please file an issue to initiate a discussion of the topic. Then, once the discussion has completed and the group has agreed to move forward on the item, you can submit a pull request and link to the issue.
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 376 KiB |
Binary file not shown.
@@ -0,0 +1,23 @@
|
||||
# Codespaces
|
||||
|
||||
TODO: welcome and introduction
|
||||
|
||||
# Overview
|
||||
|
||||
TODO: document devcontainer
|
||||
TODO: reference extensions
|
||||
TODO: use of dockercompose and services
|
||||
|
||||
# Terminal
|
||||
|
||||
TODO: link to vscode terminal
|
||||
|
||||
# Graphics and Simulations
|
||||
|
||||
TODO: vnc options
|
||||
TODO: foxglove example
|
||||
TODO: gazebo example with gzweb
|
||||
|
||||
# References
|
||||
|
||||
TODO: links to more info
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 36 KiB |
@@ -0,0 +1,65 @@
|
||||
# Pre Release Checklist
|
||||
|
||||
This documents the steps to be taken prior to making a new release of the
|
||||
nav2 stack.
|
||||
|
||||
## Summary
|
||||
1. `Ensure all dependencies are listed in the package.xml files` by doing a
|
||||
build of all of ROS2, dependencies, and navigation 2 in one workspace.
|
||||
|
||||
2. `Ensure all dependencies are released.` by using rosdep to pull in dependencies instead of building them ourselves.
|
||||
|
||||
3. `Ensure the test suite passes`
|
||||
|
||||
## Detailed Steps
|
||||
|
||||
### Ensure all dependencies are listed in the package.xml files
|
||||
|
||||
We want to ensure that every package has a complete list of its dependencies
|
||||
in the `package.xml` file. This can be done by not sourcing any ros `setup.bash` files. Instead we need to build everything as one big repo.
|
||||
|
||||
There is a docker file to do that, so run
|
||||
|
||||
```bash
|
||||
sudo docker build -t nav2:full_ros_build --build-arg ROS2_BRANCH=dashing --build-arg http_proxy=http://myproxy.example.com:80 --build-arg https_proxy=http://myproxy.example.com:80 -f Dockerfile.full_ros_build ./
|
||||
```
|
||||
|
||||
ROS2_BRANCH should be the release you are targeting or just `main` if you want
|
||||
to compare against ROS2 main.
|
||||
|
||||
### Ensure all dependencies are released.
|
||||
|
||||
We want to ensure the correct version of all our dependencies have been released
|
||||
to the branch we are targeting. To do that, we skip the
|
||||
`underlay.repos` install step and rely solely on rosdep to install
|
||||
everything.
|
||||
|
||||
There is a dockerfile to do that as well, so run
|
||||
|
||||
```bash
|
||||
sudo docker build -t nav2:rosdep_only_build --build-arg ROS2_BRANCH=dashing --build-arg http_proxy=http://myproxy.example.com:80 --build-arg https_proxy=http://myproxy.example.com:80 -f Dockerfile.release_branch ./
|
||||
```
|
||||
|
||||
As before, ROS2_BRANCH is the branch you are targeting. In this case, there is
|
||||
no main option. We can only run this dockerfile against a set of released
|
||||
packages.
|
||||
|
||||
### Ensure the test suite passes
|
||||
|
||||
Ensure the test suite passes in one of the docker images you just built.
|
||||
|
||||
#### Crystal
|
||||
|
||||
For the `crystal` release, run
|
||||
|
||||
```bash
|
||||
sudo docker run nav2:crystal colcon test
|
||||
```
|
||||
|
||||
#### Dashing and newer
|
||||
|
||||
For newer releases, run
|
||||
|
||||
```bash
|
||||
sudo docker run nav2:crystal src/navigation2/tools/run_test_suite.bash
|
||||
```
|
||||
@@ -0,0 +1,19 @@
|
||||
# Requirement Title
|
||||
The \<navigation system> should be able to \<shall> \<do something>
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- What is the expected user interaction?
|
||||
- What use case does this map to?
|
||||
- Are there any non-functional requirements (build system, tools, performance, etc)
|
||||
|
||||
|
||||
# Example:
|
||||
|
||||
# Warehouse Navigation
|
||||
The navigation system should include a modular collision avoidance algorithm that can be replaced with a new algorithm at run time
|
||||
|
||||
## More details
|
||||
- I want to be able to write or use my own collision avoidance algorithm without having to re-compile the entire stack from source
|
||||
- Ideally I can just change out a node using a custom launch file
|
||||
- This maps to the "Collision Avoidance" use case
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 61 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 33 KiB |
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 29 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 67 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 35 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 37 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 13 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 45 KiB |
@@ -0,0 +1,332 @@
|
||||
# ROS 2 Navigation System Requirements
|
||||
|
||||
The ROS 2 Navigation System ("Navigation System") is the control system that enables a robot to autonomously reach a goal state, such as a specific position and orientation relative to a given map. Provided with a navigation command to execute, the Navigation System generates a plan to achieve the desired result and outputs control commands to autonomously direct the robot, respecting any safety constraints and avoiding obstacles encountered along the way.
|
||||
|
||||
This document lists the requirements for the ROS 2 Navigation System. The ROS 2 Navigation System is intended to be a generalization of the ROS 1 navigation stack and will address some of its known limitations.
|
||||
|
||||
# 1. Introduction
|
||||
|
||||
This section describes the format of each requirement, the keywords available for use in the definition of each requirement, and the basic concepts needed to define and understand the requirements.
|
||||
|
||||
## 1.1 Requirement Fields
|
||||
|
||||
Each requirement is presented in tabular form with the following fields:
|
||||
|
||||
* **Id** - A unique identifier for the requirement
|
||||
* **Handle** - A short, scoped, description summarizing the essence of the requirement
|
||||
* **Priority** - An associated priority level: **1** (high), **2** (medium), and **3** (low)
|
||||
* **Requirement** - The requirement itself, stated in clear, concise requirements language
|
||||
* **Notes** - Elaboration and related information for the requirement
|
||||
|
||||
## 1.2 Requirement Language Keywords
|
||||
|
||||
In the requirements specified below, certain keywords have a specific meaning as they appear in the text. These keywords are defined as follows and must be capitalized whenever used in a manner intended to specify a behavior or requirement.
|
||||
|
||||
1. **MUST**: This word, or the terms "REQUIRED" or "SHALL", mean that the definition is an absolute requirement of the specification.
|
||||
|
||||
2. **MUST NOT**: This phrase, or the phrase "SHALL NOT", mean that the definition is an absolute prohibition of the specification.
|
||||
|
||||
3. **SHOULD**: This word, or the adjective "RECOMMENDED", mean that there may exist valid reasons in particular circumstances to ignore a particular item, but the full implications must be understood and carefully weighed before choosing a different course.
|
||||
|
||||
4. **SHOULD NOT**: This phrase, or the phrase "NOT RECOMMENDED" mean that there may exist valid reasons in particular circumstances when the particular behavior is acceptable or even useful, but the full implications should be understood and the case carefully weighed before implementing any behavior described with this label.
|
||||
|
||||
5. **MAY**: This word, or the adjective "OPTIONAL", mean that an item is truly optional. An implementation which does not include a particular option MUST be prepared to interoperate with another implementation which does include the option, though perhaps with reduced functionality. In the same vein an implementation which does include a particular option MUST be prepared to interoperate with another implementation which does not include the option (except, of course, for the feature the option provides).
|
||||
|
||||
These definitions are derived from the [IETF Best Current Practices Document 14](https://tools.ietf.org/html/bcp14).
|
||||
|
||||
## 1.3 Terminology
|
||||
|
||||
This section defines some common terminology as used in this document.
|
||||
|
||||
Term | Definition
|
||||
---- | ----------
|
||||
Path | A *Path* is an ordered sequence of points in space.
|
||||
Route | A *Route* is a synonym for Path.
|
||||
Trajectory | A *Trajectory* is a path parameterized by time.
|
||||
Path Planning | *Path Planning* refers to the process of finding an optimal path between multiple locations. Path planning is typically characterized as a graph traversal problem and algorithms such as A*, D*, and RRT are common choices for implementation.
|
||||
Motion Planning | *Motion Planning* refers to the process of specifying the motion of the robot over time to follow a specific path.
|
||||
|
||||
## 1.4 Use Cases
|
||||
|
||||
The Navigation System is part of a larger system that includes a person or automated system ("the user") directing the operation of one or more robots. To provide context for the Navigation System, this section lists the expected interactions between the user and the robot system.
|
||||
|
||||
## 1.4.1 Mapping Use Cases
|
||||
|
||||
The user will typically create a map of the area in which the robot is to navigate, either manually or using the SLAM algorithm. This map identifies significant features of the environment, including fixed features, such as walls and fixed obstacles, and virtual features, such as navigation lanes and safety zones. While the creation of the map itself is outside the scope of the Navigation System, the system is dependent on the map format(s). The map will need to be rich enough to support the Navigation System requirements listed in this document.
|
||||
|
||||
The following use case diagram shows an example of the kinds of operations provided by a mapping interface.
|
||||
|
||||
|
||||
|
||||
## 1.4.2 Mission Planning Use Cases
|
||||
|
||||
Another area in which the user interacts with the system is in the creation of a mission plan for the robot. The user composes a sequence of primitive navigation commands, such as **Navigate to Pose**, **Navigate to Area**, **Maintain Pose**, etc., into an overall plan. While mission planning is also outside the scope of the Navigation System, the mission plan format should be sufficient to meet the Navigation System requirements listed in this document.
|
||||
|
||||
|
||||
|
||||
## 1.4.3 Mission Execution Use Cases
|
||||
|
||||
The user will be able to initiate the execution of specific mission plans ("missions") and should also be able to view the status of the mission in progress, as well as cancel the mission that is currently in progress. In addition, the user may be required to provide the robot with its initial pose if the robot is not able to determine it automatically.
|
||||
|
||||
|
||||
|
||||
## 1.5 Architectural Components
|
||||
|
||||
The Navigation System is part of a larger software system. This document does not specify the architecture for the complete system, but simply gives a conceptual overview for the purpose of requirements definition.
|
||||
|
||||
The Navigation System has a *command chain*, where each level refines its command input into successively more specific operations for the lext level down, and *support modules* which are used by modules in the command chain.
|
||||
|
||||
## 1.5.1 Command Chain
|
||||
|
||||
The command chain is the sequence of modules that comprise the chain of command from the user, at the top, to the robot, at the bottom.
|
||||
|
||||
* **Mission Planning** - Mission Planning provides an interface to the user to allow the user to create mission plans and assign them to robots for execution. A *Mission Plan* is a sequence of *Navigation Commands* along with associated information about how the commands should be carried out.
|
||||
* **Mission Execution** - Mission Execution receives the Mission Plan and is responsible to execute the plan and report progress on its execution.
|
||||
* **Navigation System** - The Navigation System receives a segment of an overall plan to execute (a *Navigation Command*) and generates the control commands to the robot to carry it out.
|
||||
* **Robot Interface** - The Robot Interface is an abstraction of the robot platform, providing the means for the Navigation System to control the robot, learn about its capabilities, and receive feedback from the robot.
|
||||
|
||||
The following diagram shows the modules in the command chain and the successive refinement of the control commands:
|
||||
|
||||
|
||||
|
||||
The Navigation System itself can be decomposed into two general responsibilities, *Planning*, and *Execution*.
|
||||
|
||||
* **Planning** - The Planning Module is responsible to execute Navigation Commands. To do so, this module can evaluate input maps and continually assess the robot's environment to plan motion and provide the path for the robot to follow to achieve completion of the Navigation Command.
|
||||
* **Execution** - The Execution Module is responsible to execute the path provided by Planning, generating the control commands required to follow the path.
|
||||
|
||||
|
||||
|
||||
Decomposing the Navigation System, the overall command chain is as follows:
|
||||
|
||||
|
||||
|
||||
## 1.5.2 Support Modules
|
||||
|
||||
In addition to the command chain, there are several supporting modules and subsystems required for a complete system. The implementation of these modules is outside the scope of the Navigation System. However, the interface to these components is in scope and the associated requirements should be defined. Together, the support modules provide the robot with a full picture of the robot's environment.
|
||||
|
||||
* **Mapping** - The Mapping Subsystem generates maps that can be used by the Navigation System to plan the robot's motion. Maps are typically created in advance and are available to the Navigation System. A map can be updated to reflect changes in the environment. The frequency of these updates will vary among implementations.
|
||||
* **Perception** - The Perception Subsystem utilizes sensors to develop an understanding of the dynamic environment around the robot. This information is available to the Navigation System, such as when avoiding obstacles in the robot's path.
|
||||
* **Prediction** - The Prediction Subsystem anticipates future motion trajectories of any perceived objects.
|
||||
* **Localization** - The Localization Subsystem provides the current location of the robot.
|
||||
|
||||
In a complete robot system these modules are available to the core navigation modules (the command chain), as shown in the following diagram:
|
||||
|
||||
|
||||
|
||||
To facilitate error recovery, each module in the command chain, if it is unable to carry out its task, should be able to propagate error information to its predecessor in the chain.
|
||||
|
||||
## 1.6 Design Goals
|
||||
|
||||
The Navigation System should meet the following high-level design goals:
|
||||
|
||||
* **Extensibility** - The Navigation System should be a *pluggable framework* to allow for other developers to easily extend the capabilities of the Navigation System, such as adding the ability to handle new navigation commands.
|
||||
* **Modularity** - The Navigation System should allow developers to *easily replace components* with alternative implementations.
|
||||
* **Generality** - The Navigation System should not introduce inherent limitations in the architectural blocks. For example, it should support multiple kinds of robots, not making assumptions about robot capabilities and limitations and should support various map types and orientations.
|
||||
* **Performance** - *TODO: What are the performance goals?*
|
||||
* **Scalability** - *TODO: What are the scalability goals?*
|
||||
* *TODO: Other important design goals to call out?*
|
||||
|
||||
# 2.0 Requirements
|
||||
|
||||
This section lists the requirements for the Navigation System.
|
||||
|
||||
## 2.1 Implementation Constraints
|
||||
|
||||
There are various constraints on the development of the ROS 2 Navigation stack.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
IC001 | Developer's Guide | 1 | The Navigation System SHOULD be developed in accordance with the ROS 2 Developer's Guide | [ROS 2 Developer's Guide](https://github.com/ros2/ros2/wiki/Developer-Guide)
|
||||
IC002 | Implementation Language.C++.Version | 1 | Developers SHALL assume the availability of C++14 language features | Per the ROS 2 Developer's Guide
|
||||
IC003 | Implementation Language.C++.API Preference | 1 | Developers SHOULD prefer standard C++, then Boost, then custom code, in that order. | Boost may be used if equivalent functionality is not already available in the C++ standard library
|
||||
IC004 | Implementation Language.C++.Supported Compilers.g++ | 1 | The Navigation System code SHALL compile with gcc 5.4 or newer
|
||||
IC005 | Implementation Language.C++.Supported Compilers.Clang | 1 | The Navigation System code SHALL compile with Clang, version *x*
|
||||
IC006 | Implementation Language.C++.Supported Compilers.Intel C++ Compiler | 1 | The Navigation System code SHOULD compile with the Intel C++ Compiler, version *x* | Could be useful for optimization purposes
|
||||
IC007 | Implementation Language.Python.Version | 1 | Any Python code developed for the Navigation System MUST use Python 3
|
||||
IC008 | Implementation Language.GUI | 1 | Any GUIs developed as part of the Navigation System SHOULD use the Qt library, via C++ or Python (PyQt) | *Which version?*
|
||||
IC009 | Implementation Language.GUI.QML | 1 | Any GUIs developed as part of the Navigation System MAY use QML
|
||||
IC010 | ROS2.Version | 1 | The first revision of the Navigation System WILL target the Crystal Clemmys release of ROS2. | We should develop against the latest ROS2 code whenever possible.
|
||||
|
||||
## 2.2 Target Platforms
|
||||
|
||||
Navigation System will run on the latest versions of the operating systems supported by the core ROS 2 code.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
TP001 | Target Platforms.Operating Systems.Ubuntu | 1 | The Navigation System MUST support Ubuntu Desktop 16.04 and Ubuntu Desktop 18.04
|
||||
TP002 | Target Platforms.Operating Systems.MacOS | 1 | The Navigation System MUST support MacOS 10.13 (High Sierra) and MacOS 10.14 (Mohave)
|
||||
TP003 | Target Platforms.Operating Systems.Windows | 1 | The Navigation System MUST support Windows 10 Professional
|
||||
TP004 | Target Platforms.Operating Systems.Clear Linux | 1 | The Navigation System SHOULD support the Intel's Clear Linux distribution | Clear Linux uses a continuous deployment model.
|
||||
TP005 | Target Platforms.CPU.Word Size | 1 | The Navigation System SHALL support 64-bit processors | Don't assume a specific pointer size
|
||||
TP006 | Target Platforms.Minimum Platform | 1 | *TODO: Should we specify a minimum target platform? Or, should this be expressed as minimum platform requirements?*
|
||||
|
||||
## 2.3 Command Chain Modules
|
||||
|
||||
This section lists the requirements for the core command chain modules in the Navigation System.
|
||||
|
||||
### 2.3.1 Mission Planning
|
||||
|
||||
A complete system should have some kind of Mission Planning subsystem to convey the user's intentions to the robot. The User interacts with this Mission Planning subsystem to generate a Mission Plan for the robot. The Mission Plan is defined as a sequence of Navigation Commands, along with any associated information about how and when the plan should be carried out. The design and implementation of a Mission Planning subsystem is outside the scope of the Navigation System. However, in order to understand the larger system context and how Mission Planning interacts with the Navigation System, this section will consider the nature of a mission plan and the kinds of operations it may contain.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
MP001 | Mission Planning.Navigation Commands | 1 | The Mission Plan MUST be able to express the plan as a coordinated sequence of Navigation Commands. | Could include time and policy aspects (*when* and *how*, not just *what*)
|
||||
MP002 | Mission Planning.Navigation Commands.Composition | 1 | The Mission Plan SHOULD allow for the composition and naming of new Navigation Commands from a sequence of previously-defined Navigation Commands. | Build up levels of abstraction. For example, Enter-Elevator could be expressed as Navigate-to-Pose (right outside of elevator), Wait (for door to open), Navigate-to-Pose (inside the elevator).
|
||||
MP003 | Mission Planning.Navigation Commands.Navigate to Pose | 1 | The Mission Plan MUST be able to convey the information required for a robot to navigate from its current location to a specific destination pose.
|
||||
MP004 | Mission Planning.Navigation Commands.Navigate to Area | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to navigate from its current location to a specific area. | An "area" could be a rectangular region or a more complex shape. It may be defined as tolerance to a goal (ie. within +/- 1 meter distance).
|
||||
MP005 | Mission Planning.Navigation Commands.Enqueue | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to navigate from its current location to a position behind another specified robot.
|
||||
MP006 | Mission Planning.Navigation Commands.Follow | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to be able to follow another specified robot. | This one doesn't have a completion state (reaching the goal), unless it specifies additional information such as "follow until destination reached."
|
||||
MP007 | Mission Planning.Navigation Commands.Maintain Pose | 1 | The Mission Plan SHOULD be able to convey the information required for a robot to maintain its current pose. | Could be indefinite or time-based.
|
||||
MP008 | Mission Planning.Navigation Commands.Park | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to park itself. | The implementation of the parking command could interact with the robot to cause it, for example, to shut down or enter a low-power state.
|
||||
MP009 | Mission Planning.Navigation Commands.Dock to Charger | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to dock to a specific charging station.
|
||||
MP010 | Mission Planning.Policy | 1 | The Mission Plan SHOULD be able to express information about how and when the navigation commands are to be carried out. | Time and safety constraints.
|
||||
MP011 | Mission Planning.Policy.Time.Initiation | 1 | The Mission Plan SHOULD be able to convey when a mission should begin.
|
||||
MP012 | Mission Planning.Policy.Time.Completion | 1 | The Mission Plan SHOULD be able to convey by when a mission should end.
|
||||
MP013 | Mission Planning.Policy.Safety.Maximum Speed | 1 | The Mission Plan SHOULD be able to convey a maximum speed for the robot. | The robot would respect this value in carrying out the plan. This could be site-specific policy.
|
||||
MP014 | Mission Planning.Policy.Safety.Minimum Safety Buffer | 1 | The Mission Plan SHOULD be able to convey a minimum safety buffer distance. | The robot would respect this value and maintain the distance from other objects at all times. Should vary with relative velocities.
|
||||
|
||||
### 2.3.2 Mission Execution
|
||||
|
||||
The Mission Execution module has the responsibility to execute a provided mission. It provides each successive Navigation Command to the Navigation Subsystem, monitoring and reporting progress towards completion of the plan.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
ME001 | Mission Execution.Inputs.Mission Plan | 1 | The Mission Execution module MUST accept the Mission Plan to execute.
|
||||
ME002 | Mission Execution.Inputs.Commands.Execute Mission | 1 | When commanded to do so, the Mission Execution module MUST execute the provided Mission Plan, respecting any specified constraints.
|
||||
ME003 | Mission Execution.Inputs.Commands.Cancel Mission | 1 | When commanded to do so, the Mission Execution module MUST interrupt the Robot's navigation and cancel the current mission.
|
||||
ME004 | Mission Execution.Outputs.Navigation Command | 1 | Upon completion of each Navigation Command, the Mission Execution module SHALL output the next Navigation Command to execute.
|
||||
ME005 | Mission Execution.Command Sequencing | 1 | The Mission Execution module MUST monitor for completion of each Navigation Command before sending the next command.
|
||||
ME006 | Mission Execution.Logging | 1 | The Mission Execution module SHOULD log its activity. | In case of forensic analysis of a safety event, for example.
|
||||
ME007 | Mission Execution.Feedback.Inputs.Error Recovery | 1 | Upon receipt of a downstream failure (unable to execute the Navigation Command), the Mission Execution module SHOULD attempt to recover and continue execution of the mission.
|
||||
ME008 | Mission Execution.Feedback.Outputs.Progress Notification | 1 | The Mission Execution module SHALL provide progress notifications on the execution of the mission. | Intermediate steps of interest.
|
||||
ME009 | Mission Execution.Feedback.Outputs.Mission Completed | 1 | Upon successfull completion of the mission, the Mission Execution module SHALL output a corresponding notification.
|
||||
ME010 | Mission Execution.Feedback.Outputs.Mission Canceled | 1 | Upon receiving a cancellation command and cancelling the mission, the Mission Execution module SHALL output a corresponding notification.
|
||||
ME011 | Mission Execution.Feedback.Outputs.Mission Failure | 1 | If the Mission Execution module is unable to execute the mission, it MUST output a failure notification. | This would be received by the user-level interface and could necessitate user intervention, such as having a remote operating center where the remote operator "rescues" the robot.
|
||||
ME012 | Mission Execution.Safe State Upon Failure | 1 | If the Mission Execution module is unable to execute the mission, it MUST direct the robot to a safe state. | The failure could be for a variety of reasons - sensor failures, algorithmic failure, a collision, etc.
|
||||
ME013 | Mission Execution.Selection of Planners | 1 | The Navigation System SHOULD allow the association and use of specific Planning and Execution Modules for a given Navigation Command. | For example, a user may want to have components for classic point-A-to-point-B travel, but upon reaching point B, have specialized components that control a series of maneuvers such as docking to a charging station or a conveyor belt.
|
||||
|
||||
### 2.3.3 Navigation System.Planning
|
||||
|
||||
The Navigation System's Planning Module receives the Navigation Command from the Mission Execution module and is responsible to implement that command. To do so, the Planning Module can use information from the Mapping Subsystem to plan a route and use input from the Perception Subsystem to evaluate the dynamic environment and avoid collisions with objects crossing its path.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
PLN001 | Planning | 1 | The Navigation System SHOULD have a Planning Module that generates the Path for the robot to follow to implement a specified Navigation Command.
|
||||
PLN002 | Planning.Inputs.Navigation Command | 1 | The Planning Module SHALL receive the Navigation Command to execute.
|
||||
PLN003 | Planning.Inputs.Policy | 1 | The Planning Module SHALL receive policy information associated with the Navigation Command to execute. | This could be global policy and/or per-command policy. Policy could contain, for example, a list of conventions for the robot to follow (navigate on the right side of a path, for example).
|
||||
PLN004 | Planning.Inputs.Mapping.Maps | 1 | The Planning Module MUST have access to one or more maps available that describe the robot's environment.
|
||||
PLN005 | Planning.Inputs.Perception.Sensory Input | 1 | The Planning Module MUST have access to data from the Perception Subsystem.
|
||||
PLN006 | Planning.Inputs.Prediction.Predicted Trajectories | 1 | The Planning Module MAY have access to predicted trajectories of objects detected by the Perception Subsystem. | In simple planners, there is no prediction of moving objects, but in more complex planners, this may be considered.
|
||||
PLN007 | Planning.Inputs.Localization.Current Pose | 1 | The Planning Module MUST have access to the robot's current pose. | The pose could be be provided manually or automatically determined (outside of this module).
|
||||
PLN008 | Planning.Outputs.Path | 1 | The Planning Module SHOULD output the Path for the robot to follow to execute the input Navigation Command and MUST respect any associated policy.
|
||||
PLN009 | Planning.Feedback.Inputs | 1 | The Planning Module MAY receive error input from the downstream Execution Module. | So that it can attempt to recover from execution failures.
|
||||
PLN010 | Planning.Feedback.Inputs.Error Recovery | 1 | Upon receipt of a downstream failure, the Planning Module SHOULD attempt to automatically recover from the error. | Handling a robot that gets stuck or handling a collision, for example.
|
||||
PLN011 | Planning.Feedback.Outputs.Command Completed | 1 | Upon completing the provided Navigation Command, the Planning Module MUST report this event on its feedback output.
|
||||
PLN012 | Planning.Feedback.Outputs.Unable to Execute Command | 1 | If the Planning Module is unable to execute the Navigation Command, it SHALL report the error on its feedback output. | It should handle errors if possible, but report back if it can't.
|
||||
PLN013 | Planning.Feedback.Outputs.Error Propagation | 1 | The Planning Module SHOULD propagate errors that it can't handle.
|
||||
PLN014 | Planning.Logging | 1 | The Planning Module SHOULD log significant events. | Commands completed, failures, recoveries, etc.
|
||||
PLN015 | Planning.Documentation | 1 | The Navigation System SHOULD provided detailed documentation on how to develop a Planning Module.
|
||||
PLN016 | Planning.Simple Example | 1 | The Navigation System SHOULD provided a simple example of a Planning Module. | An easily understood module that developers and students could use as a starting point.
|
||||
|
||||
### 2.3.4 Navigation System.Execution
|
||||
|
||||
The Navigation System's Execution Module is responsible to execute the Path specified by the Planning Module. It has available to it all of the information from the support modules and must respect any policy guidance.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
EXE001 | Execution | 1 | The Navigation System SHOULD have an Execution Module that generates commands to the robot to achieve a specific Path.
|
||||
EXE002 | Execution.Inputs.Path | 1 | The Execution Module SHALL receive a Path that the robot is to follow.
|
||||
EXE003 | Execution.Inputs.Policy | 1 | The Execution Module SHALL receive policy information associated with the Path to follow. | Could filter down from higher-level policy specification.
|
||||
EXE004 | Execution.Collision Avoidance.Avoid Stationary Objects | 1 | The Execution Module MUST direct the robot such that it avoids colliding into stationary objects in its environment.
|
||||
EXE005 | Execution.Collision Avoidance.Avoid Moving Objects | 1 | The Execution Module MUST direct the robot such that it avoids colliding into moving objects that intercept its path.
|
||||
EXE006 | Execution.Collision Detection | 1 | The Execution Module SHOULD detect if a collision has occurred.
|
||||
EXE007 | Execution.Collision Detection.Latency | 1 | The Execution Module SHOULD detect collisions within 50ms. | *TODO: What is the right value?*
|
||||
EXE008 | Execution.Feedback.Inputs.Robot Malfunction | 1 | The Execution Module SHOULD receive notifications of any robot malfunctions from the downstream robot interface. | A sensor failure, for example.
|
||||
EXE009 | Execution.Feedback.Outputs.Collision Detected | 1 | The Execution Module SHOULD report the detection of a collision. | So that the Planning Module can attempt recovery or otherwise respond.
|
||||
EXE010 | Execution.Feedback.Outputs.Error Propagation | 1 | The Execution Module SHOULD propagate errors that it can't handle.
|
||||
EXE011 | Execution.Documentation | 1 | The Navigation System SHOULD provided detailed documentation on how to develop an Execution Module.
|
||||
EXE012 | Execution.Simple Example | 1 | The Navigation System SHOULD provided a simple example of an Execution Module| An easily understood module that developers and students could use as a starting point.
|
||||
|
||||
### 2.3.5 Robot Interface
|
||||
|
||||
There should be a uniform interface to the various supported robots. The abstraction of different robots may be handled by current mechanisms, such as UDRF, Twist commands, etc. We should consider if anything else is needed here. The user should be able to specify different types of robot drive types, such as Ackerman (automobile) steering, and robot shapes.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
RI001 | Robot Interface.Attributes | 1 | Holonomicity, max/min speeds and accelerations, etc.
|
||||
RI002 | Robot Interface.Dynamic Switching | 1 | Can the robot dynamically change attributes?
|
||||
RI003 | Robot Interface.Safety.Limited Parameters | 1 | A list of parameters used to limit certain circumstances and provide the hooks for users to set those values if they want
|
||||
RI004 | Robot Interface.Safety.Speed Limiting | 1 | *TODO*
|
||||
RI005 | Robot Interface.Safety.Force Limiting | 1 | *TODO*
|
||||
RI006 | Robot Interface.EMO Button | 1 | *TODO*
|
||||
RI007 | Robot Interface.Feedback.Outputs | 1 | *TODO*
|
||||
|
||||
## 2.4 Support Modules
|
||||
|
||||
There are a few support modules and subsystems that are not part of the Navigation System proper, but are necessary components in a complete system. The Navigation System depends on the data interfaces to these components. This section describes the requirements and assumptions of these interface.
|
||||
|
||||
### 2.4.1 Mapping
|
||||
|
||||
The map data format should be capable of describing typical indoor and outdoor environments encoutered by the robots.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
MAP001 | Mapping | 1 | The Mapping System SHALL provide map information to the Navigation System.
|
||||
MAP002 | Mapping.Data Model.Obstacles | 1 | Maps provided by Mapping Subsystem MUST indicate the location of known obstacles.
|
||||
MAP003 | Mapping.Data Model.Confidence Metric | 1 | Each known obstacle in a map SHALL have a confidence metric associated with it.
|
||||
MAP004 | Mapping.Data Model.Unknown Space | 1 | Maps provided by the Mapping Subsystem MUST indicate unmapped/unknown space. | Such as areas beyond the edge of the map, or areas within the map for which we didn't have any observations during map building.
|
||||
MAP005 | Mapping.Data Model.Surface Planarity | 1 | The map data format SHALL be capable of describing the planarity of traversable surfaces. | Can describe uneven ground.
|
||||
MAP006 | Mapping.Data Model.Safety Zone | 1 | The map data format SHALL be capable of defining regions where the robot may have to adjust its operations according to specified constraints.
|
||||
MAP007 | Mapping.Data Model.Safety Zone.Name | 1 | The map data format SHOULD allow for naming each safety zone. | *TODO: Does it need to be a unique name?*
|
||||
MAP008 | Mapping.Data Model.Safety Zone.Type | 1 | The map data format SHOULD allow for defining types of safety zones. | To allow for re-use of a safety zone type without redefining policy. Could be an "intersection" type, for example. May want to slot down at all intersections, for example.
|
||||
MAP009 | Mapping.Data Model.Safety Zone.Policy | 1 | The map data format SHALL be capable of expressing policy associated with each safety zone and safety zone type. | Maximum speed, (increased) distance to people, etc.
|
||||
MAP010 | Mapping.Data Model.Safety Zone.Policy.Keep Out Zone | 1 | The map data format SHALL be capable of expressing that a robot must not navigate through this zone.
|
||||
MAP011 | Mapping.Data Model.Lanes | 1 | The map data format SHALL be able to specify virtual lanes. | May prefer specified lanes in a warehouse, for example.
|
||||
MAP012 | Mapping.Data Model.Building Levels | 1 | The map data format SHALL be able to specify single and multi-level buildings.
|
||||
MAP013 | Mapping.Data Model.Building Levels.Level Connecting Features | 1 | The map data format SHALL be able to specify level-connecting features, such as elevators, stairways, and ramps.
|
||||
MAP014 | Mapping.Multiple Maps Per Environment | 1 | The Mapping System MAY provide multiple maps of the same environment. | Such as for different scales and elevations.
|
||||
MAP015 | Mapping.Data Model.Extensibility | 1 | The Mapping System SHOULD be extensible, to allow for the description of additional entities in the environment.
|
||||
MAP016 | Mapping.Dimensionality.2D | 1 | The Mapping System MUST provide 2D map information.
|
||||
MAP017 | Mapping.Dimensionality.2D+ | 1 | The Mapping System MAY provide 2D+ map information.
|
||||
MAP018 | Mapping.Dimensionality.3D | 1 | The Mapping System MAY provide 3D map information.
|
||||
MAP019 | Mapping.Dynamic Updates | 1 | The Mapping System SHOULD provide real-time updates of map information. | Allow for updates to the map to be pushed to clients.
|
||||
MAP020 | Mapping.Memory Optimization.Tiling | 1 | The Mapping System MAY provide the Navigation System with local map regions, sufficient for navigation. | Could provide relevant map tiles, for example, saving memory in the planners.
|
||||
|
||||
### 2.4.2 Perception
|
||||
|
||||
The Perception Subsystem provides information about objects detected in the robot's environment. This information would typically be generated from a fusion of sensor input.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
PER001 | Perception | 1 | The Perception Subsystem SHALL provide information about the dynamic environment of the robot. | Info sufficient to carry out the Navigation System requirements.
|
||||
PER002 | Perception.Latency | 1 | *TODO*
|
||||
|
||||
### 2.4.3 Prediction
|
||||
|
||||
The Prediction Subsystem uses input from the Perception Subsystem and predicts the trajectories of the detected objects over time.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
PRE001 | Prediction.Object Prediction | 1 | The Prediction Subsystem SHOULD predict the trajectories of detected objects. | One of the biggest shortcomings of the current system is the inability to model/predict where obstacles will be in the future. This leads to collisions with other moving objects
|
||||
PRE002 | Prediction.Object Prediction.Time Horizon | 1 | *TODO: How far into the future should the object prediction work?*
|
||||
|
||||
### 2.4.4 Localization
|
||||
|
||||
The Navigation System requires the Robot's current pose, provided by an external Localization module. This section lists the requirements for the information provided by the the Localization Module.
|
||||
|
||||
Id | Handle | Priority | Description | Notes
|
||||
-- | ------ | -------- | ----------- | -----
|
||||
LOC001 | Localization.Robot Pose | 1 | The Localization module MUST provide the robot's current pose to the Navigation System. | This could be manual or as a result of automatic localization; the Navigation System wouldn't know either way.
|
||||
LOC002 | Localization.Robot Pose.Accuracy | 1 | The Localization Module MUST provide the estimated accuracy of the pose. | So that Planning modules can determine if a particular Localization module has sufficient accuracy. Could use PoseWithCovariance message.
|
||||
|
||||
## 2.5 Open Issues
|
||||
|
||||
* What are the performance goals for the ROS2 Navigation System?
|
||||
* What are the scalability for the ROS2 Navigaton System?
|
||||
* Any other important design goals to call out?
|
||||
* Should we specify a minimum target platform? Or, should this be expressed as minimum platform requirements?
|
||||
* What is the right latency value for detecting a collision?
|
||||
* Should we add any safety-related functionality at the robot interface level?
|
||||
* Do safety zones need unique names?
|
||||
* What is the target latency for the perception subsystem?
|
||||
* How far into the future should the object prediction work?
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 157 KiB |
@@ -0,0 +1,16 @@
|
||||
|
||||
|
||||
# Target Use Cases
|
||||
The Nav2 system is targeting the following use cases:
|
||||
|
||||
[2D Indoor Navigation](indoor_navigation_use_case.md) - example: Warehouse / Logistics robot
|
||||
|
||||
2D Navigation with Elevation - example: rescue robot navigating building with ramps and stairways
|
||||
|
||||
2D Navigation with Elevators - example: Room service robot
|
||||
|
||||
[Outdoor Navigation](outdoor_navigation_use_case.md)
|
||||
|
||||
## Stretch Target
|
||||
3D Navigation - Drones
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
# Use Case Title
|
||||
As a \<Developer, Researcher, Technician, etc.> I want the robot to \<action> so that \<I, the robot, etc.> can \<do something important>
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- What is the expected user interaction?
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
# Example:
|
||||
|
||||
# Collision Avoidance
|
||||
As a robot user, I want the robot to navigate without colliding into people or objects so that it doesn't hurt anyone or damage anything
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- I want this so that I know the robot won't damage itself, damage property or hurt anyone
|
||||
- Example: a logistics robot in a warehouse must avoid shelves, people, forklifts, and other robots
|
||||
- What is the expected user interaction?
|
||||
- I shouldn't have to interact with the robot to prevent it from crashing into people or things
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
- The performance needs to be fast enough to avoid moving objects such as people walking or other moving robots
|
||||
@@ -0,0 +1,14 @@
|
||||
# Collision Avoidance
|
||||
As a Robot user I want to my robot to avoid colliding with people or objects so that it won't damage anything or hurt anyone
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- This is needed for indoor and outdoor robot navigation in most (all?) cases
|
||||
- Example: a robot in a warehouse should avoid colliding into the walls or shelving, and dynamically avoid people that cross its path
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to walk in front of a robot and it should avoid crashing into that person
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
# Indoor Localization
|
||||
As a Robot user I want my robot to know its location on a given map of an indoor area so that it can move around the area
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- This is needed for indoor robot navigation in most (all?) cases
|
||||
- Example: a courier robot in a logistics warehouse
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to specify a map to use and a location on that map for the robot
|
||||
- The robot should be able to deduce it's own position on a map autonomously
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
# Indoor Navigation
|
||||
As a Robot user I want my robot to autonomously navigate to a given location on a given map so that it can help me at that location
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- This is needed for indoor robot navigation in most (all?) cases
|
||||
- Example: a courier robot in a logistics warehouse
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to specify a map to use and a location on that map for the robot to move to.
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
# Keep Out Zones
|
||||
As a Robot user I want to be able to designate keep-out zones or areas on a map so that my robot will go around those areas instead of through them
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- This is needed for indoor/outdoor robot navigation in areas where there may be safety issues or hazards
|
||||
- Example: a courier robot in a logistics warehouse may be required to avoid areas where the forklift is unloading pallets from trucks
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to specify keep out zones for the robot to avoid
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
# Multi-story Building Navigation (2D+)
|
||||
As a Robot user I want my robot to be able to navigate stairways, ramps or elevators to move to another portion of the multi-story building so that it can do something useful
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- Example: a delivery robot in an office building
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to specify stairways, ramps and elevators on a map for a robot to use or not use
|
||||
- via a GUI
|
||||
- via a config file or API so that it can be done by another program
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
# Outdoor Localization
|
||||
As a Robot user I want my robot to know its location on a given map of an outdoor area, such as a street or college campus, so that it can move around the area
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- This is needed for outdoor robot navigation in most (all?) cases
|
||||
- Example: a delivery robot on a college campus
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to specify a map to use and a location on that map for the robot
|
||||
- The robot should be able to deduce it's own position on a map autonomously
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
# Outdoor Navigation
|
||||
As a Robot user I want my robot to autonomously navigate to a given location on a given outdoor map, such as a college campus or street, so that it can do something useful at that location
|
||||
|
||||
## More details
|
||||
- Why is this needed?
|
||||
- This is needed for outdoor robot navigation in most (all?) cases
|
||||
- Example: a delivery robot on a college campus
|
||||
|
||||
- What is the expected user interaction?
|
||||
- The user should be able to specify a map to use and a location on that map for the robot to move to.
|
||||
|
||||
- Are there any non-functional requirements? (build system, tools, performance, etc)
|
||||
|
||||
|
||||
@@ -0,0 +1,114 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_amcl)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
find_package(message_filters REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
include(CheckSymbolExists)
|
||||
check_symbol_exists(drand48 stdlib.h HAVE_DRAND48)
|
||||
|
||||
add_subdirectory(src/pf)
|
||||
add_subdirectory(src/map)
|
||||
add_subdirectory(src/motion_model)
|
||||
add_subdirectory(src/sensors)
|
||||
|
||||
set(executable_name amcl)
|
||||
|
||||
add_executable(${executable_name}
|
||||
src/main.cpp
|
||||
)
|
||||
|
||||
set(library_name ${executable_name}_core)
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/amcl_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${library_name} PRIVATE src/include)
|
||||
|
||||
if(HAVE_DRAND48)
|
||||
target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48")
|
||||
endif()
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
rclcpp_components
|
||||
message_filters
|
||||
tf2_geometry_msgs
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
sensor_msgs
|
||||
std_srvs
|
||||
tf2_ros
|
||||
tf2
|
||||
nav2_util
|
||||
nav2_msgs
|
||||
pluginlib
|
||||
)
|
||||
|
||||
ament_target_dependencies(${executable_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_link_libraries(${executable_name}
|
||||
${library_name}
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_link_libraries(${library_name}
|
||||
map_lib pf_lib sensors_lib
|
||||
)
|
||||
|
||||
rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(TARGETS ${executable_name}
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib)
|
||||
ament_export_dependencies(${dependencies})
|
||||
pluginlib_export_plugin_description_file(nav2_amcl plugins.xml)
|
||||
ament_package()
|
||||
@@ -0,0 +1,4 @@
|
||||
# AMCL
|
||||
Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes.
|
||||
|
||||
See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings.
|
||||
@@ -0,0 +1,398 @@
|
||||
/*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAV2_AMCL__AMCL_NODE_HPP_
|
||||
#define NAV2_AMCL__AMCL_NODE_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "message_filters/subscriber.h"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_amcl/motion_model/motion_model.hpp"
|
||||
#include "nav2_amcl/sensors/laser/laser.hpp"
|
||||
#include "nav2_msgs/msg/particle.hpp"
|
||||
#include "nav2_msgs/msg/particle_cloud.hpp"
|
||||
#include "nav2_msgs/srv/set_initial_pose.hpp"
|
||||
#include "nav_msgs/srv/set_map.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "std_srvs/srv/empty.hpp"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "pluginlib/class_loader.hpp"
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-parameter"
|
||||
#pragma GCC diagnostic ignored "-Wreorder"
|
||||
#include "tf2_ros/message_filter.h"
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
#define NEW_UNIFORM_SAMPLING 1
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
/*
|
||||
* @class AmclNode
|
||||
* @brief ROS wrapper for AMCL
|
||||
*/
|
||||
class AmclNode : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* @brief AMCL constructor
|
||||
* @param options Additional options to control creation of the node.
|
||||
*/
|
||||
explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
/*
|
||||
* @brief AMCL destructor
|
||||
*/
|
||||
~AmclNode();
|
||||
|
||||
protected:
|
||||
/*
|
||||
* @brief Lifecycle configure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
|
||||
/*
|
||||
* @brief Lifecycle activate
|
||||
*/
|
||||
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
|
||||
/*
|
||||
* @brief Lifecycle deactivate
|
||||
*/
|
||||
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
|
||||
/*
|
||||
* @brief Lifecycle cleanup
|
||||
*/
|
||||
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
|
||||
/*
|
||||
* @brief Lifecycle shutdown
|
||||
*/
|
||||
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a parameter change is detected
|
||||
* @param event ParameterEvent message
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
|
||||
// Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
|
||||
// respond until we're in the active state
|
||||
std::atomic<bool> active_{false};
|
||||
|
||||
// Dedicated callback group and executor for services and subscriptions in AmclNode,
|
||||
// in order to isolate TF timer used in message filter.
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
std::unique_ptr<nav2_util::NodeThread> executor_thread_;
|
||||
|
||||
// Pose hypothesis
|
||||
typedef struct
|
||||
{
|
||||
double weight; // Total weight (weights sum to 1)
|
||||
pf_vector_t pf_pose_mean; // Mean of pose esimate
|
||||
pf_matrix_t pf_pose_cov; // Covariance of pose estimate
|
||||
} amcl_hyp_t;
|
||||
|
||||
// Map-related
|
||||
/*
|
||||
* @brief Get new map from ROS topic to localize in
|
||||
* @param msg Map message
|
||||
*/
|
||||
void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
|
||||
/*
|
||||
* @brief Handle a new map message
|
||||
* @param msg Map message
|
||||
*/
|
||||
void handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg);
|
||||
/*
|
||||
* @brief Creates lookup table of free cells in map
|
||||
*/
|
||||
void createFreeSpaceVector();
|
||||
/*
|
||||
* @brief Frees allocated map related memory
|
||||
*/
|
||||
void freeMapDependentMemory();
|
||||
map_t * map_{nullptr};
|
||||
/*
|
||||
* @brief Convert an occupancy grid map to an AMCL map
|
||||
* @param map_msg Map message
|
||||
* @return pointer to map for AMCL to use
|
||||
*/
|
||||
map_t * convertMap(const nav_msgs::msg::OccupancyGrid & map_msg);
|
||||
bool first_map_only_{true};
|
||||
std::atomic<bool> first_map_received_{false};
|
||||
amcl_hyp_t * initial_pose_hyp_;
|
||||
std::recursive_mutex mutex_;
|
||||
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
|
||||
#if NEW_UNIFORM_SAMPLING
|
||||
static std::vector<std::pair<int, int>> free_space_indices;
|
||||
#endif
|
||||
|
||||
// Transforms
|
||||
/*
|
||||
* @brief Initialize required ROS transformations
|
||||
*/
|
||||
void initTransforms();
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
bool sent_first_transform_{false};
|
||||
bool latest_tf_valid_{false};
|
||||
tf2::Transform latest_tf_;
|
||||
|
||||
// Message filters
|
||||
/*
|
||||
* @brief Initialize incoming data message subscribers and filters
|
||||
*/
|
||||
void initMessageFilters();
|
||||
std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
|
||||
rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
|
||||
std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
|
||||
message_filters::Connection laser_scan_connection_;
|
||||
|
||||
// Publishers and subscribers
|
||||
/*
|
||||
* @brief Initialize pub subs of AMCL
|
||||
*/
|
||||
void initPubSub();
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
|
||||
initial_pose_sub_;
|
||||
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
|
||||
pose_pub_;
|
||||
rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
|
||||
particle_cloud_pub_;
|
||||
/*
|
||||
* @brief Handle with an initial pose estimate is received
|
||||
*/
|
||||
void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
|
||||
/*
|
||||
* @brief Handle when a laser scan is received
|
||||
*/
|
||||
void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
|
||||
|
||||
// Services and service callbacks
|
||||
/*
|
||||
* @brief Initialize state services
|
||||
*/
|
||||
void initServices();
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
|
||||
/*
|
||||
* @brief Service callback for a global relocalization request
|
||||
*/
|
||||
void globalLocalizationCallback(
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response> response);
|
||||
|
||||
// service server for providing an initial pose guess
|
||||
rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
|
||||
/*
|
||||
* @brief Service callback for an initial pose guess request
|
||||
*/
|
||||
void initialPoseReceivedSrv(
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
|
||||
std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
|
||||
|
||||
// Let amcl update samples without requiring motion
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
|
||||
/*
|
||||
* @brief Request an AMCL update even though the robot hasn't moved
|
||||
*/
|
||||
void nomotionUpdateCallback(
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response> response);
|
||||
|
||||
// Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs
|
||||
std::atomic<bool> force_update_{false};
|
||||
|
||||
// Odometry
|
||||
/*
|
||||
* @brief Initialize odometry
|
||||
*/
|
||||
void initOdometry();
|
||||
std::shared_ptr<nav2_amcl::MotionModel> motion_model_;
|
||||
geometry_msgs::msg::PoseStamped latest_odom_pose_;
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_;
|
||||
double init_pose_[3]; // Initial robot pose
|
||||
double init_cov_[3];
|
||||
pluginlib::ClassLoader<nav2_amcl::MotionModel> plugin_loader_{"nav2_amcl",
|
||||
"nav2_amcl::MotionModel"};
|
||||
/*
|
||||
* @brief Get robot pose in odom frame using TF
|
||||
*/
|
||||
bool getOdomPose(
|
||||
// Helper to get odometric pose from transform system
|
||||
geometry_msgs::msg::PoseStamped & pose,
|
||||
double & x, double & y, double & yaw,
|
||||
const rclcpp::Time & sensor_timestamp, const std::string & frame_id);
|
||||
std::atomic<bool> first_pose_sent_;
|
||||
|
||||
// Particle filter
|
||||
/*
|
||||
* @brief Initialize particle filter
|
||||
*/
|
||||
void initParticleFilter();
|
||||
/*
|
||||
* @brief Pose-generating function used to uniformly distribute particles over the map
|
||||
*/
|
||||
static pf_vector_t uniformPoseGenerator(void * arg);
|
||||
pf_t * pf_{nullptr};
|
||||
bool pf_init_;
|
||||
pf_vector_t pf_odom_pose_;
|
||||
int resample_count_{0};
|
||||
|
||||
// Laser scan related
|
||||
/*
|
||||
* @brief Initialize laser scan
|
||||
*/
|
||||
void initLaserScan();
|
||||
/*
|
||||
* @brief Create a laser object
|
||||
*/
|
||||
nav2_amcl::Laser * createLaserObject();
|
||||
int scan_error_count_{0};
|
||||
std::vector<nav2_amcl::Laser *> lasers_;
|
||||
std::vector<bool> lasers_update_;
|
||||
std::map<std::string, int> frame_to_laser_;
|
||||
rclcpp::Time last_laser_received_ts_;
|
||||
|
||||
/*
|
||||
* @brief Check if sufficient time has elapsed to get an update
|
||||
*/
|
||||
bool checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time);
|
||||
rclcpp::Time last_time_printed_msg_;
|
||||
/*
|
||||
* @brief Add a new laser scanner if a new one is received in the laser scallbacks
|
||||
*/
|
||||
bool addNewScanner(
|
||||
int & laser_index,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
|
||||
const std::string & laser_scan_frame_id,
|
||||
geometry_msgs::msg::PoseStamped & laser_pose);
|
||||
/*
|
||||
* @brief Whether the pf needs to be updated
|
||||
*/
|
||||
bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta);
|
||||
/*
|
||||
* @brief Update the PF
|
||||
*/
|
||||
bool updateFilter(
|
||||
const int & laser_index,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
|
||||
const pf_vector_t & pose);
|
||||
/*
|
||||
* @brief Publish particle cloud
|
||||
*/
|
||||
void publishParticleCloud(const pf_sample_set_t * set);
|
||||
/*
|
||||
* @brief Get the current state estimat hypothesis from the particle cloud
|
||||
*/
|
||||
bool getMaxWeightHyp(
|
||||
std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
|
||||
int & max_weight_hyp);
|
||||
/*
|
||||
* @brief Publish robot pose in map frame from AMCL
|
||||
*/
|
||||
void publishAmclPose(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
|
||||
const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp);
|
||||
/*
|
||||
* @brief Determine TF transformation from map to odom
|
||||
*/
|
||||
void calculateMaptoOdomTransform(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
|
||||
const std::vector<amcl_hyp_t> & hyps,
|
||||
const int & max_weight_hyp);
|
||||
/*
|
||||
* @brief Publish TF transformation from map to odom
|
||||
*/
|
||||
void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration);
|
||||
/*
|
||||
* @brief Handle a new pose estimate callback
|
||||
*/
|
||||
void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg);
|
||||
bool init_pose_received_on_inactive{false};
|
||||
bool initial_pose_is_known_{false};
|
||||
bool set_initial_pose_{false};
|
||||
bool always_reset_initial_pose_;
|
||||
double initial_pose_x_;
|
||||
double initial_pose_y_;
|
||||
double initial_pose_z_;
|
||||
double initial_pose_yaw_;
|
||||
|
||||
/*
|
||||
* @brief Get ROS parameters for node
|
||||
*/
|
||||
void initParameters();
|
||||
double alpha1_;
|
||||
double alpha2_;
|
||||
double alpha3_;
|
||||
double alpha4_;
|
||||
double alpha5_;
|
||||
std::string base_frame_id_;
|
||||
double beam_skip_distance_;
|
||||
double beam_skip_error_threshold_;
|
||||
double beam_skip_threshold_;
|
||||
bool do_beamskip_;
|
||||
std::string global_frame_id_;
|
||||
double lambda_short_;
|
||||
double laser_likelihood_max_dist_;
|
||||
double laser_max_range_;
|
||||
double laser_min_range_;
|
||||
std::string sensor_model_type_;
|
||||
int max_beams_;
|
||||
int max_particles_;
|
||||
int min_particles_;
|
||||
std::string odom_frame_id_;
|
||||
double pf_err_;
|
||||
double pf_z_;
|
||||
double alpha_fast_;
|
||||
double alpha_slow_;
|
||||
int resample_interval_;
|
||||
std::string robot_model_type_;
|
||||
tf2::Duration save_pose_period_;
|
||||
double sigma_hit_;
|
||||
bool tf_broadcast_;
|
||||
tf2::Duration transform_tolerance_;
|
||||
double a_thresh_;
|
||||
double d_thresh_;
|
||||
double z_hit_;
|
||||
double z_max_;
|
||||
double z_short_;
|
||||
double z_rand_;
|
||||
std::string scan_topic_{"scan"};
|
||||
std::string map_topic_{"map"};
|
||||
};
|
||||
|
||||
} // namespace nav2_amcl
|
||||
|
||||
#endif // NAV2_AMCL__AMCL_NODE_HPP_
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAV2_AMCL__ANGLEUTILS_HPP_
|
||||
#define NAV2_AMCL__ANGLEUTILS_HPP_
|
||||
|
||||
#include <math.h>
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
/*
|
||||
* @class angleutils
|
||||
* @brief Some utilities for working with angles
|
||||
*/
|
||||
class angleutils
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* @brief Normalize angles
|
||||
* @brief z Angle to normalize
|
||||
* @return normalized angle
|
||||
*/
|
||||
static double normalize(double z);
|
||||
|
||||
/*
|
||||
* @brief Find minimum distance between 2 angles
|
||||
* @brief a Angle 1
|
||||
* @brief b Angle 2
|
||||
* @return normalized angle difference
|
||||
*/
|
||||
static double angle_diff(double a, double b);
|
||||
};
|
||||
|
||||
inline double
|
||||
angleutils::normalize(double z)
|
||||
{
|
||||
return atan2(sin(z), cos(z));
|
||||
}
|
||||
|
||||
inline double
|
||||
angleutils::angle_diff(double a, double b)
|
||||
{
|
||||
a = normalize(a);
|
||||
b = normalize(b);
|
||||
double d1 = a - b;
|
||||
double d2 = 2 * M_PI - fabs(d1);
|
||||
if (d1 > 0) {
|
||||
d2 *= -1.0;
|
||||
}
|
||||
if (fabs(d1) < fabs(d2)) {
|
||||
return d1;
|
||||
} else {
|
||||
return d2;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
|
||||
#endif // NAV2_AMCL__ANGLEUTILS_HPP_
|
||||
@@ -0,0 +1,138 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Global map (grid-based)
|
||||
* Author: Andrew Howard
|
||||
* Date: 6 Feb 2003
|
||||
* CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
|
||||
**************************************************************************/
|
||||
|
||||
#ifndef NAV2_AMCL__MAP__MAP_HPP_
|
||||
#define NAV2_AMCL__MAP__MAP_HPP_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// Forward declarations
|
||||
struct _rtk_fig_t;
|
||||
|
||||
|
||||
// Limits
|
||||
#define MAP_WIFI_MAX_LEVELS 8
|
||||
|
||||
|
||||
// Description for a single map cell.
|
||||
typedef struct
|
||||
{
|
||||
// Occupancy state (-1 = free, 0 = unknown, +1 = occ)
|
||||
int occ_state;
|
||||
|
||||
// Distance to the nearest occupied cell
|
||||
double occ_dist;
|
||||
|
||||
// Wifi levels
|
||||
// int wifi_levels[MAP_WIFI_MAX_LEVELS];
|
||||
} map_cell_t;
|
||||
|
||||
|
||||
// Description for a map
|
||||
typedef struct
|
||||
{
|
||||
// Map origin; the map is a viewport onto a conceptual larger map.
|
||||
double origin_x, origin_y;
|
||||
|
||||
// Map scale (m/cell)
|
||||
double scale;
|
||||
|
||||
// Map dimensions (number of cells)
|
||||
int size_x, size_y;
|
||||
|
||||
// The map data, stored as a grid
|
||||
map_cell_t * cells;
|
||||
|
||||
// Max distance at which we care about obstacles, for constructing
|
||||
// likelihood field
|
||||
double max_occ_dist;
|
||||
} map_t;
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Basic map functions
|
||||
**************************************************************************/
|
||||
|
||||
// Create a new (empty) map
|
||||
map_t * map_alloc(void);
|
||||
|
||||
// Destroy a map
|
||||
void map_free(map_t * map);
|
||||
|
||||
// Update the cspace distances
|
||||
void map_update_cspace(map_t * map, double max_occ_dist);
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Range functions
|
||||
**************************************************************************/
|
||||
|
||||
// Extract a single range reading from the map
|
||||
double map_calc_range(map_t * map, double ox, double oy, double oa, double max_range);
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* GUI/diagnostic functions
|
||||
**************************************************************************/
|
||||
|
||||
// Draw the occupancy grid
|
||||
void map_draw_occ(map_t * map, struct _rtk_fig_t * fig);
|
||||
|
||||
// Draw the cspace map
|
||||
void map_draw_cspace(map_t * map, struct _rtk_fig_t * fig);
|
||||
|
||||
// Draw a wifi map
|
||||
void map_draw_wifi(map_t * map, struct _rtk_fig_t * fig, int index);
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Map manipulation macros
|
||||
**************************************************************************/
|
||||
|
||||
// Convert from map index to world coords
|
||||
#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
|
||||
#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
|
||||
|
||||
// Convert from world coords to map coords
|
||||
#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
|
||||
#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
|
||||
|
||||
// Test to see if the given map coords lie within the absolute map bounds.
|
||||
#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
|
||||
|
||||
// Compute the cell index for the given map coords.
|
||||
#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NAV2_AMCL__MAP__MAP_HPP_
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
|
||||
#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include "nav2_amcl/motion_model/motion_model.hpp"
|
||||
#include "nav2_amcl/angleutils.hpp"
|
||||
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
class DifferentialMotionModel : public nav2_amcl::MotionModel
|
||||
{
|
||||
public:
|
||||
virtual void initialize(
|
||||
double alpha1, double alpha2, double alpha3, double alpha4,
|
||||
double alpha5);
|
||||
virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
|
||||
|
||||
private:
|
||||
double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
|
||||
};
|
||||
} // namespace nav2_amcl
|
||||
#endif // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// This library is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU Lesser General Public
|
||||
// License as published by the Free Software Foundation; either
|
||||
// version 2.1 of the License, or (at your option) any later version.
|
||||
//
|
||||
// This library is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
// Lesser General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU Lesser General Public
|
||||
// License along with this library; if not, write to the Free Software
|
||||
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
|
||||
#ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
|
||||
#define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_amcl/pf/pf.hpp"
|
||||
#include "nav2_amcl/pf/pf_pdf.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
/**
|
||||
* @class nav2_amcl::MotionModel
|
||||
* @brief An abstract motion model class
|
||||
*/
|
||||
class MotionModel
|
||||
{
|
||||
public:
|
||||
virtual ~MotionModel() = default;
|
||||
|
||||
/**
|
||||
* @brief An factory to create motion models
|
||||
* @param type Type of motion model to create in factory
|
||||
* @param alpha1 error parameters, see documentation
|
||||
* @param alpha2 error parameters, see documentation
|
||||
* @param alpha3 error parameters, see documentation
|
||||
* @param alpha4 error parameters, see documentation
|
||||
* @param alpha5 error parameters, see documentation
|
||||
* @return MotionModel A pointer to the motion model it created
|
||||
*/
|
||||
virtual void initialize(
|
||||
double alpha1, double alpha2, double alpha3, double alpha4,
|
||||
double alpha5) = 0;
|
||||
|
||||
/**
|
||||
* @brief Update on new odometry data
|
||||
* @param pf The particle filter to update
|
||||
* @param pose pose of robot in odometry update
|
||||
* @param delta change in pose in odometry update
|
||||
*/
|
||||
virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0;
|
||||
};
|
||||
} // namespace nav2_amcl
|
||||
|
||||
#endif // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
|
||||
#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include "nav2_amcl/motion_model/motion_model.hpp"
|
||||
#include "nav2_amcl/angleutils.hpp"
|
||||
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
class OmniMotionModel : public nav2_amcl::MotionModel
|
||||
{
|
||||
public:
|
||||
virtual void initialize(
|
||||
double alpha1, double alpha2, double alpha3, double alpha4,
|
||||
double alpha5);
|
||||
virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
|
||||
|
||||
private:
|
||||
double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
|
||||
};
|
||||
} // namespace nav2_amcl
|
||||
#endif // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/* Eigen-decomposition for symmetric 3x3 real matrices.
|
||||
Public domain, copied from the public domain Java library JAMA. */
|
||||
|
||||
#ifndef NAV2_AMCL__PF__EIG3_HPP_
|
||||
#define NAV2_AMCL__PF__EIG3_HPP_
|
||||
|
||||
/* Symmetric matrix A => eigenvectors in columns of V, corresponding
|
||||
eigenvalues in d. */
|
||||
void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
|
||||
|
||||
#endif // NAV2_AMCL__PF__EIG3_HPP_
|
||||
@@ -0,0 +1,200 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Simple particle filter for localization.
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef NAV2_AMCL__PF__PF_HPP_
|
||||
#define NAV2_AMCL__PF__PF_HPP_
|
||||
|
||||
#include "nav2_amcl/pf/pf_vector.hpp"
|
||||
#include "nav2_amcl/pf/pf_kdtree.hpp"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// Forward declarations
|
||||
struct _pf_t;
|
||||
struct _rtk_fig_t;
|
||||
struct _pf_sample_set_t;
|
||||
|
||||
// Function prototype for the initialization model; generates a sample pose from
|
||||
// an appropriate distribution.
|
||||
typedef pf_vector_t (* pf_init_model_fn_t) (void * init_data);
|
||||
|
||||
// Function prototype for the action model; generates a sample pose from
|
||||
// an appropriate distribution
|
||||
typedef void (* pf_action_model_fn_t) (
|
||||
void * action_data,
|
||||
struct _pf_sample_set_t * set);
|
||||
|
||||
// Function prototype for the sensor model; determines the probability
|
||||
// for the given set of sample poses.
|
||||
typedef double (* pf_sensor_model_fn_t) (
|
||||
void * sensor_data,
|
||||
struct _pf_sample_set_t * set);
|
||||
|
||||
|
||||
// Information for a single sample
|
||||
typedef struct
|
||||
{
|
||||
// Pose represented by this sample
|
||||
pf_vector_t pose;
|
||||
|
||||
// Weight for this pose
|
||||
double weight;
|
||||
} pf_sample_t;
|
||||
|
||||
|
||||
// Information for a cluster of samples
|
||||
typedef struct
|
||||
{
|
||||
// Number of samples
|
||||
int count;
|
||||
|
||||
// Total weight of samples in this cluster
|
||||
double weight;
|
||||
|
||||
// Cluster statistics
|
||||
pf_vector_t mean;
|
||||
pf_matrix_t cov;
|
||||
|
||||
// Workspace
|
||||
double m[4], c[2][2];
|
||||
} pf_cluster_t;
|
||||
|
||||
|
||||
// Information for a set of samples
|
||||
typedef struct _pf_sample_set_t
|
||||
{
|
||||
// The samples
|
||||
int sample_count;
|
||||
pf_sample_t * samples;
|
||||
|
||||
// A kdtree encoding the histogram
|
||||
pf_kdtree_t * kdtree;
|
||||
|
||||
// Clusters
|
||||
int cluster_count, cluster_max_count;
|
||||
pf_cluster_t * clusters;
|
||||
|
||||
// Filter statistics
|
||||
pf_vector_t mean;
|
||||
pf_matrix_t cov;
|
||||
int converged;
|
||||
} pf_sample_set_t;
|
||||
|
||||
|
||||
// Information for an entire filter
|
||||
typedef struct _pf_t
|
||||
{
|
||||
// This min and max number of samples
|
||||
int min_samples, max_samples;
|
||||
|
||||
// Population size parameters
|
||||
double pop_err, pop_z;
|
||||
|
||||
// The sample sets. We keep two sets and use [current_set]
|
||||
// to identify the active set.
|
||||
int current_set;
|
||||
pf_sample_set_t sets[2];
|
||||
|
||||
// Running averages, slow and fast, of likelihood
|
||||
double w_slow, w_fast;
|
||||
|
||||
// Decay rates for running averages
|
||||
double alpha_slow, alpha_fast;
|
||||
|
||||
// Function used to draw random pose samples
|
||||
pf_init_model_fn_t random_pose_fn;
|
||||
|
||||
double dist_threshold; // distance threshold in each axis over which the pf is considered to not
|
||||
// be converged
|
||||
int converged;
|
||||
} pf_t;
|
||||
|
||||
|
||||
// Create a new filter
|
||||
pf_t * pf_alloc(
|
||||
int min_samples, int max_samples,
|
||||
double alpha_slow, double alpha_fast,
|
||||
pf_init_model_fn_t random_pose_fn);
|
||||
|
||||
// Free an existing filter
|
||||
void pf_free(pf_t * pf);
|
||||
|
||||
// Initialize the filter using a guassian
|
||||
void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov);
|
||||
|
||||
// Initialize the filter using some model
|
||||
void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data);
|
||||
|
||||
// Update the filter with some new action
|
||||
// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data);
|
||||
|
||||
// Update the filter with some new sensor observation
|
||||
void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data);
|
||||
|
||||
// Resample the distribution
|
||||
void pf_update_resample(pf_t * pf, void * random_pose_data);
|
||||
|
||||
// Compute the CEP statistics (mean and variance).
|
||||
// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var);
|
||||
|
||||
// Compute the statistics for a particular cluster. Returns 0 if
|
||||
// there is no such cluster.
|
||||
int pf_get_cluster_stats(
|
||||
pf_t * pf, int cluster, double * weight,
|
||||
pf_vector_t * mean, pf_matrix_t * cov);
|
||||
|
||||
// Re-compute the cluster statistics for a sample set
|
||||
void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set);
|
||||
|
||||
|
||||
// Display the sample set
|
||||
void pf_draw_samples(pf_t * pf, struct _rtk_fig_t * fig, int max_samples);
|
||||
|
||||
// Draw the histogram (kdtree)
|
||||
void pf_draw_hist(pf_t * pf, struct _rtk_fig_t * fig);
|
||||
|
||||
// Draw the CEP statistics
|
||||
// void pf_draw_cep_stats(pf_t * pf, struct _rtk_fig_t * fig);
|
||||
|
||||
// Draw the cluster statistics
|
||||
void pf_draw_cluster_stats(pf_t * pf, struct _rtk_fig_t * fig);
|
||||
|
||||
// calculate if the particle filter has converged -
|
||||
// and sets the converged flag in the current set and the pf
|
||||
int pf_update_converged(pf_t * pf);
|
||||
|
||||
// sets the current set and pf converged values to zero
|
||||
void pf_init_converged(pf_t * pf);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif // NAV2_AMCL__PF__PF_HPP_
|
||||
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: KD tree functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 18 Dec 2002
|
||||
* CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef NAV2_AMCL__PF__PF_KDTREE_HPP_
|
||||
#define NAV2_AMCL__PF__PF_KDTREE_HPP_
|
||||
|
||||
#ifdef INCLUDE_RTKGUI
|
||||
#include <rtk.h>
|
||||
#endif
|
||||
|
||||
|
||||
// Info for a node in the tree
|
||||
typedef struct pf_kdtree_node
|
||||
{
|
||||
// Depth in the tree
|
||||
int leaf, depth;
|
||||
|
||||
// Pivot dimension and value
|
||||
int pivot_dim;
|
||||
double pivot_value;
|
||||
|
||||
// The key for this node
|
||||
int key[3];
|
||||
|
||||
// The value for this node
|
||||
double value;
|
||||
|
||||
// The cluster label (leaf nodes)
|
||||
int cluster;
|
||||
|
||||
// Child nodes
|
||||
struct pf_kdtree_node * children[2];
|
||||
} pf_kdtree_node_t;
|
||||
|
||||
|
||||
// A kd tree
|
||||
typedef struct
|
||||
{
|
||||
// Cell size
|
||||
double size[3];
|
||||
|
||||
// The root node of the tree
|
||||
pf_kdtree_node_t * root;
|
||||
|
||||
// The number of nodes in the tree
|
||||
int node_count, node_max_count;
|
||||
pf_kdtree_node_t * nodes;
|
||||
|
||||
// The number of leaf nodes in the tree
|
||||
int leaf_count;
|
||||
} pf_kdtree_t;
|
||||
|
||||
|
||||
// Create a tree
|
||||
extern pf_kdtree_t * pf_kdtree_alloc(int max_size);
|
||||
|
||||
// Destroy a tree
|
||||
extern void pf_kdtree_free(pf_kdtree_t * self);
|
||||
|
||||
// Clear all entries from the tree
|
||||
extern void pf_kdtree_clear(pf_kdtree_t * self);
|
||||
|
||||
// Insert a pose into the tree
|
||||
extern void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value);
|
||||
|
||||
// Cluster the leaves in the tree
|
||||
extern void pf_kdtree_cluster(pf_kdtree_t * self);
|
||||
|
||||
// Determine the probability estimate for the given pose
|
||||
// extern double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose);
|
||||
|
||||
// Determine the cluster label for the given pose
|
||||
extern int pf_kdtree_get_cluster(pf_kdtree_t * self, pf_vector_t pose);
|
||||
|
||||
|
||||
#ifdef INCLUDE_RTKGUI
|
||||
|
||||
// Draw the tree
|
||||
extern void pf_kdtree_draw(pf_kdtree_t * self, rtk_fig_t * fig);
|
||||
|
||||
#endif
|
||||
|
||||
#endif // NAV2_AMCL__PF__PF_KDTREE_HPP_
|
||||
@@ -0,0 +1,84 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Useful pdf functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef NAV2_AMCL__PF__PF_PDF_HPP_
|
||||
#define NAV2_AMCL__PF__PF_PDF_HPP_
|
||||
|
||||
#include "nav2_amcl/pf/pf_vector.hpp"
|
||||
|
||||
// #include <gsl/gsl_rng.h>
|
||||
// #include <gsl/gsl_randist.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**************************************************************************
|
||||
* Gaussian
|
||||
*************************************************************************/
|
||||
|
||||
// Gaussian PDF info
|
||||
typedef struct
|
||||
{
|
||||
// Mean, covariance and inverse covariance
|
||||
pf_vector_t x;
|
||||
pf_matrix_t cx;
|
||||
// pf_matrix_t cxi;
|
||||
double cxdet;
|
||||
|
||||
// Decomposed covariance matrix (rotation * diagonal)
|
||||
pf_matrix_t cr;
|
||||
pf_vector_t cd;
|
||||
|
||||
// A random number generator
|
||||
// gsl_rng *rng;
|
||||
} pf_pdf_gaussian_t;
|
||||
|
||||
|
||||
// Create a gaussian pdf
|
||||
pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
|
||||
|
||||
// Destroy the pdf
|
||||
void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf);
|
||||
|
||||
// Compute the value of the pdf at some point [z].
|
||||
// double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
|
||||
|
||||
// Draw randomly from a zero-mean Gaussian distribution, with standard
|
||||
// deviation sigma.
|
||||
// We use the polar form of the Box-Muller transformation, explained here:
|
||||
// http://www.taygeta.com/random/gaussian.html
|
||||
double pf_ran_gaussian(double sigma);
|
||||
|
||||
// Generate a sample from the pdf.
|
||||
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t * pdf);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NAV2_AMCL__PF__PF_PDF_HPP_
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Vector functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef NAV2_AMCL__PF__PF_VECTOR_HPP_
|
||||
#define NAV2_AMCL__PF__PF_VECTOR_HPP_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
// The basic vector
|
||||
typedef struct
|
||||
{
|
||||
double v[3];
|
||||
} pf_vector_t;
|
||||
|
||||
|
||||
// The basic matrix
|
||||
typedef struct
|
||||
{
|
||||
double m[3][3];
|
||||
} pf_matrix_t;
|
||||
|
||||
|
||||
// Return a zero vector
|
||||
pf_vector_t pf_vector_zero(void);
|
||||
|
||||
// Check for NAN or INF in any component
|
||||
// int pf_vector_finite(pf_vector_t a);
|
||||
|
||||
// Print a vector
|
||||
// void pf_vector_fprintf(pf_vector_t s, FILE * file, const char * fmt);
|
||||
|
||||
// Simple vector addition
|
||||
// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
|
||||
|
||||
// Simple vector subtraction
|
||||
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
|
||||
|
||||
// Transform from local to global coords (a + b)
|
||||
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
|
||||
|
||||
// Transform from global to local coords (a - b)
|
||||
// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
|
||||
|
||||
|
||||
// Return a zero matrix
|
||||
pf_matrix_t pf_matrix_zero(void);
|
||||
|
||||
// Check for NAN or INF in any component
|
||||
// int pf_matrix_finite(pf_matrix_t a);
|
||||
|
||||
// Print a matrix
|
||||
// void pf_matrix_fprintf(pf_matrix_t s, FILE * file, const char * fmt);
|
||||
|
||||
// Compute the matrix inverse. Will also return the determinant,
|
||||
// which should be checked for underflow (indicated singular matrix).
|
||||
// pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
|
||||
|
||||
// Decompose a covariance matrix [a] into a rotation matrix [r] and a
|
||||
// diagonal matrix [d] such that a = r * d * r^T.
|
||||
void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NAV2_AMCL__PF__PF_VECTOR_HPP_
|
||||
@@ -0,0 +1,41 @@
|
||||
// Copyright (c) Samsung Research America
|
||||
//
|
||||
// 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 NAV2_AMCL__PORTABLE_UTILS_HPP_
|
||||
#define NAV2_AMCL__PORTABLE_UTILS_HPP_
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef HAVE_DRAND48
|
||||
// Some system (e.g., Windows) doesn't come with drand48(), srand48().
|
||||
// Use rand, and srand for such system.
|
||||
static double drand48(void)
|
||||
{
|
||||
return ((double)rand()) / RAND_MAX;// NOLINT
|
||||
}
|
||||
|
||||
static void srand48(long int seedval)// NOLINT
|
||||
{
|
||||
srand(seedval);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NAV2_AMCL__PORTABLE_UTILS_HPP_
|
||||
@@ -0,0 +1,210 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// This library is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU Lesser General Public
|
||||
// License as published by the Free Software Foundation; either
|
||||
// version 2.1 of the License, or (at your option) any later version.
|
||||
//
|
||||
// This library is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
// Lesser General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU Lesser General Public
|
||||
// License along with this library; if not, write to the Free Software
|
||||
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
|
||||
|
||||
#ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_
|
||||
#define NAV2_AMCL__SENSORS__LASER__LASER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include "nav2_amcl/pf/pf.hpp"
|
||||
#include "nav2_amcl/pf/pf_pdf.hpp"
|
||||
#include "nav2_amcl/map/map.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
// Forward declarations
|
||||
class LaserData;
|
||||
|
||||
/*
|
||||
* @class Laser
|
||||
* @brief Base class for laser sensor models
|
||||
*/
|
||||
class Laser
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A Laser constructor
|
||||
* @param max_beams number of beams to use
|
||||
* @param map Map pointer to use
|
||||
*/
|
||||
Laser(size_t max_beams, map_t * map);
|
||||
|
||||
/*
|
||||
* @brief Laser destructor
|
||||
*/
|
||||
virtual ~Laser();
|
||||
|
||||
/*
|
||||
* @brief Run a sensor update on laser
|
||||
* @param pf Particle filter to use
|
||||
* @param data Laser data to use
|
||||
* @return if it was succesful
|
||||
*/
|
||||
virtual bool sensorUpdate(pf_t * pf, LaserData * data) = 0;
|
||||
|
||||
/*
|
||||
* @brief Set the laser pose from an update
|
||||
* @param laser_pose Pose of the laser
|
||||
*/
|
||||
void SetLaserPose(pf_vector_t & laser_pose);
|
||||
|
||||
protected:
|
||||
double z_hit_;
|
||||
double z_rand_;
|
||||
double sigma_hit_;
|
||||
|
||||
/*
|
||||
* @brief Reallocate weights
|
||||
* @param max_samples Max number of samples
|
||||
* @param max_obs number of observations
|
||||
*/
|
||||
void reallocTempData(int max_samples, int max_obs);
|
||||
map_t * map_;
|
||||
pf_vector_t laser_pose_;
|
||||
int max_beams_;
|
||||
int max_samples_;
|
||||
int max_obs_;
|
||||
double ** temp_obs_;
|
||||
};
|
||||
|
||||
/*
|
||||
* @class LaserData
|
||||
* @brief Class of laser data to process
|
||||
*/
|
||||
class LaserData
|
||||
{
|
||||
public:
|
||||
Laser * laser;
|
||||
|
||||
/*
|
||||
* @brief LaserData constructor
|
||||
*/
|
||||
LaserData() {ranges = NULL;}
|
||||
/*
|
||||
* @brief LaserData destructor
|
||||
*/
|
||||
virtual ~LaserData() {delete[] ranges;}
|
||||
|
||||
public:
|
||||
int range_count;
|
||||
double range_max;
|
||||
double(*ranges)[2];
|
||||
};
|
||||
|
||||
/*
|
||||
* @class BeamModel
|
||||
* @brief Beam model laser sensor
|
||||
*/
|
||||
class BeamModel : public Laser
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* @brief BeamModel constructor
|
||||
*/
|
||||
BeamModel(
|
||||
double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
|
||||
double lambda_short, double chi_outlier, size_t max_beams, map_t * map);
|
||||
|
||||
/*
|
||||
* @brief Run a sensor update on laser
|
||||
* @param pf Particle filter to use
|
||||
* @param data Laser data to use
|
||||
* @return if it was succesful
|
||||
*/
|
||||
bool sensorUpdate(pf_t * pf, LaserData * data);
|
||||
|
||||
private:
|
||||
static double sensorFunction(LaserData * data, pf_sample_set_t * set);
|
||||
double z_short_;
|
||||
double z_max_;
|
||||
double lambda_short_;
|
||||
double chi_outlier_;
|
||||
};
|
||||
|
||||
/*
|
||||
* @class LikelihoodFieldModel
|
||||
* @brief likelihood field model laser sensor
|
||||
*/
|
||||
class LikelihoodFieldModel : public Laser
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* @brief BeamModel constructor
|
||||
*/
|
||||
LikelihoodFieldModel(
|
||||
double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
|
||||
size_t max_beams, map_t * map);
|
||||
|
||||
/*
|
||||
* @brief Run a sensor update on laser
|
||||
* @param pf Particle filter to use
|
||||
* @param data Laser data to use
|
||||
* @return if it was succesful
|
||||
*/
|
||||
bool sensorUpdate(pf_t * pf, LaserData * data);
|
||||
|
||||
private:
|
||||
/*
|
||||
* @brief Perform the update function
|
||||
* @param data Laser data to use
|
||||
* @param pf Particle filter to use
|
||||
* @return if it was succesful
|
||||
*/
|
||||
static double sensorFunction(LaserData * data, pf_sample_set_t * set);
|
||||
};
|
||||
|
||||
/*
|
||||
* @class LikelihoodFieldModelProb
|
||||
* @brief likelihood prob model laser sensor
|
||||
*/
|
||||
class LikelihoodFieldModelProb : public Laser
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* @brief BeamModel constructor
|
||||
*/
|
||||
LikelihoodFieldModelProb(
|
||||
double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
|
||||
bool do_beamskip, double beam_skip_distance,
|
||||
double beam_skip_threshold, double beam_skip_error_threshold,
|
||||
size_t max_beams, map_t * map);
|
||||
|
||||
/*
|
||||
* @brief Run a sensor update on laser
|
||||
* @param pf Particle filter to use
|
||||
* @param data Laser data to use
|
||||
* @return if it was succesful
|
||||
*/
|
||||
bool sensorUpdate(pf_t * pf, LaserData * data);
|
||||
|
||||
private:
|
||||
/*
|
||||
* @brief Perform the update function
|
||||
* @param data Laser data to use
|
||||
* @param pf Particle filter to use
|
||||
* @return if it was succesful
|
||||
*/
|
||||
static double sensorFunction(LaserData * data, pf_sample_set_t * set);
|
||||
bool do_beamskip_;
|
||||
double beam_skip_distance_;
|
||||
double beam_skip_threshold_;
|
||||
double beam_skip_error_threshold_;
|
||||
};
|
||||
|
||||
} // namespace nav2_amcl
|
||||
|
||||
#endif // NAV2_AMCL__SENSORS__LASER__LASER_HPP_
|
||||
@@ -0,0 +1,45 @@
|
||||
<?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>nav2_amcl</name>
|
||||
<version>1.1.18</version>
|
||||
<description>
|
||||
<p>
|
||||
amcl is a probabilistic localization system for a robot moving in
|
||||
2D. It implements the adaptive (or KLD-sampling) Monte Carlo
|
||||
localization approach (as described by Dieter Fox), which uses a
|
||||
particle filter to track the pose of a robot against a known map.
|
||||
</p>
|
||||
<p>
|
||||
This node is derived, with thanks, from Andrew Howard's excellent
|
||||
'amcl' Player driver.
|
||||
</p>
|
||||
</description>
|
||||
<!-- <author>Brian P. Gerkey</author>
|
||||
<author>contradict@gmail.com</author> -->
|
||||
<maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
|
||||
<license>LGPL-2.1-or-later</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>launch_ros</depend>
|
||||
<depend>launch_testing</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,8 @@
|
||||
<library path="motions_lib">
|
||||
<class type="nav2_amcl::DifferentialMotionModel" base_class_type="nav2_amcl::MotionModel">
|
||||
<description>This is a diff plugin.</description>
|
||||
</class>
|
||||
<class type="nav2_amcl::OmniMotionModel" base_class_type="nav2_amcl::MotionModel">
|
||||
<description>This is a omni plugin.</description>
|
||||
</class>
|
||||
</library>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,30 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// This library is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU Lesser General Public
|
||||
// License as published by the Free Software Foundation; either
|
||||
// version 2.1 of the License, or (at your option) any later version.
|
||||
//
|
||||
// This library is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
// Lesser General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU Lesser General Public
|
||||
// License along with this library; if not, write to the Free Software
|
||||
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_amcl/amcl_node.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<nav2_amcl::AmclNode>();
|
||||
rclcpp::spin(node->get_node_base_interface());
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
add_library(map_lib SHARED
|
||||
map.c
|
||||
map_range.c
|
||||
map_draw.c
|
||||
map_cspace.cpp
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
map_lib
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
@@ -0,0 +1,65 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Global map (grid-based)
|
||||
* Author: Andrew Howard
|
||||
* Date: 6 Feb 2003
|
||||
* CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $
|
||||
**************************************************************************/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "nav2_amcl/map/map.hpp"
|
||||
|
||||
|
||||
// Create a new map
|
||||
map_t * map_alloc(void)
|
||||
{
|
||||
map_t * map;
|
||||
|
||||
map = (map_t *) malloc(sizeof(map_t));
|
||||
|
||||
// Assume we start at (0, 0)
|
||||
map->origin_x = 0;
|
||||
map->origin_y = 0;
|
||||
|
||||
// Make the size odd
|
||||
map->size_x = 0;
|
||||
map->size_y = 0;
|
||||
map->scale = 0;
|
||||
|
||||
// Allocate storage for main map
|
||||
map->cells = (map_cell_t *) NULL;
|
||||
|
||||
return map;
|
||||
}
|
||||
|
||||
|
||||
// Destroy a map
|
||||
void map_free(map_t * map)
|
||||
{
|
||||
free(map->cells);
|
||||
free(map);
|
||||
}
|
||||
@@ -0,0 +1,213 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <queue>
|
||||
#include "nav2_amcl/map/map.hpp"
|
||||
|
||||
/*
|
||||
* @class CellData
|
||||
* @brief Data about map cells
|
||||
*/
|
||||
class CellData
|
||||
{
|
||||
public:
|
||||
map_t * map_;
|
||||
unsigned int i_, j_;
|
||||
unsigned int src_i_, src_j_;
|
||||
};
|
||||
|
||||
/*
|
||||
* @class CachedDistanceMap
|
||||
* @brief Cached map with distances
|
||||
*/
|
||||
class CachedDistanceMap
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* @brief CachedDistanceMap constructor
|
||||
*/
|
||||
CachedDistanceMap(double scale, double max_dist)
|
||||
: distances_(NULL), scale_(scale), max_dist_(max_dist)
|
||||
{
|
||||
cell_radius_ = max_dist / scale;
|
||||
distances_ = new double *[cell_radius_ + 2];
|
||||
for (int i = 0; i <= cell_radius_ + 1; i++) {
|
||||
distances_[i] = new double[cell_radius_ + 2];
|
||||
for (int j = 0; j <= cell_radius_ + 1; j++) {
|
||||
distances_[i][j] = sqrt(i * i + j * j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief CachedDistanceMap destructor
|
||||
*/
|
||||
~CachedDistanceMap()
|
||||
{
|
||||
if (distances_) {
|
||||
for (int i = 0; i <= cell_radius_ + 1; i++) {
|
||||
delete[] distances_[i];
|
||||
}
|
||||
delete[] distances_;
|
||||
}
|
||||
}
|
||||
double ** distances_;
|
||||
double scale_;
|
||||
double max_dist_;
|
||||
int cell_radius_;
|
||||
};
|
||||
|
||||
/*
|
||||
* @brief operator<
|
||||
*/
|
||||
bool operator<(const CellData & a, const CellData & b)
|
||||
{
|
||||
return a.map_->cells[MAP_INDEX(
|
||||
a.map_, a.i_,
|
||||
a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief get_distance_map
|
||||
* @param scale of cost information wrt distance
|
||||
* @param max_dist Maximum distance to cache from occupied information
|
||||
* @return Pointer to cached distance map
|
||||
*/
|
||||
CachedDistanceMap *
|
||||
get_distance_map(double scale, double max_dist)
|
||||
{
|
||||
static CachedDistanceMap * cdm = NULL;
|
||||
|
||||
if (!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist)) {
|
||||
if (cdm) {
|
||||
delete cdm;
|
||||
}
|
||||
cdm = new CachedDistanceMap(scale, max_dist);
|
||||
}
|
||||
|
||||
return cdm;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief enqueue cell data for caching
|
||||
*/
|
||||
void enqueue(
|
||||
map_t * map, int i, int j,
|
||||
int src_i, int src_j,
|
||||
std::priority_queue<CellData> & Q,
|
||||
CachedDistanceMap * cdm,
|
||||
unsigned char * marked)
|
||||
{
|
||||
if (marked[MAP_INDEX(map, i, j)]) {
|
||||
return;
|
||||
}
|
||||
|
||||
int di = abs(i - src_i);
|
||||
int dj = abs(j - src_j);
|
||||
double distance = cdm->distances_[di][dj];
|
||||
|
||||
if (distance > cdm->cell_radius_) {
|
||||
return;
|
||||
}
|
||||
|
||||
map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
|
||||
|
||||
CellData cell;
|
||||
cell.map_ = map;
|
||||
cell.i_ = i;
|
||||
cell.j_ = j;
|
||||
cell.src_i_ = src_i;
|
||||
cell.src_j_ = src_j;
|
||||
|
||||
Q.push(cell);
|
||||
|
||||
marked[MAP_INDEX(map, i, j)] = 1;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Update the cspace distance values
|
||||
* @param map Map to update
|
||||
* @param max_occ_distance Maximum distance for occpuancy interest
|
||||
*/
|
||||
void map_update_cspace(map_t * map, double max_occ_dist)
|
||||
{
|
||||
unsigned char * marked;
|
||||
std::priority_queue<CellData> Q;
|
||||
|
||||
marked = new unsigned char[map->size_x * map->size_y];
|
||||
memset(marked, 0, sizeof(unsigned char) * map->size_x * map->size_y);
|
||||
|
||||
map->max_occ_dist = max_occ_dist;
|
||||
|
||||
CachedDistanceMap * cdm = get_distance_map(map->scale, map->max_occ_dist);
|
||||
|
||||
// Enqueue all the obstacle cells
|
||||
CellData cell;
|
||||
cell.map_ = map;
|
||||
for (int i = 0; i < map->size_x; i++) {
|
||||
cell.src_i_ = cell.i_ = i;
|
||||
for (int j = 0; j < map->size_y; j++) {
|
||||
if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
|
||||
map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
|
||||
cell.src_j_ = cell.j_ = j;
|
||||
marked[MAP_INDEX(map, i, j)] = 1;
|
||||
Q.push(cell);
|
||||
} else {
|
||||
map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
while (!Q.empty()) {
|
||||
CellData current_cell = Q.top();
|
||||
if (current_cell.i_ > 0) {
|
||||
enqueue(
|
||||
map, current_cell.i_ - 1, current_cell.j_,
|
||||
current_cell.src_i_, current_cell.src_j_,
|
||||
Q, cdm, marked);
|
||||
}
|
||||
if (current_cell.j_ > 0) {
|
||||
enqueue(
|
||||
map, current_cell.i_, current_cell.j_ - 1,
|
||||
current_cell.src_i_, current_cell.src_j_,
|
||||
Q, cdm, marked);
|
||||
}
|
||||
if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
|
||||
enqueue(
|
||||
map, current_cell.i_ + 1, current_cell.j_,
|
||||
current_cell.src_i_, current_cell.src_j_,
|
||||
Q, cdm, marked);
|
||||
}
|
||||
if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
|
||||
enqueue(
|
||||
map, current_cell.i_, current_cell.j_ + 1,
|
||||
current_cell.src_i_, current_cell.src_j_,
|
||||
Q, cdm, marked);
|
||||
}
|
||||
|
||||
Q.pop();
|
||||
}
|
||||
|
||||
delete[] marked;
|
||||
}
|
||||
@@ -0,0 +1,147 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Local map GUI functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 18 Jan 2003
|
||||
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
|
||||
**************************************************************************/
|
||||
|
||||
#pragma GCC diagnostic ignored "-Wpedantic"
|
||||
#ifdef INCLUDE_RTKGUI
|
||||
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <rtk.h>
|
||||
#include "nav2_amcl/map/map.hpp"
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
// Draw the occupancy map
|
||||
void map_draw_occ(map_t * map, rtk_fig_t * fig)
|
||||
{
|
||||
int i, j;
|
||||
int col;
|
||||
map_cell_t * cell;
|
||||
uint16_t * image;
|
||||
uint16_t * pixel;
|
||||
|
||||
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
|
||||
|
||||
// Draw occupancy
|
||||
for (j = 0; j < map->size_y; j++) {
|
||||
for (i = 0; i < map->size_x; i++) {
|
||||
cell = map->cells + MAP_INDEX(map, i, j);
|
||||
pixel = image + (j * map->size_x + i);
|
||||
|
||||
col = 127 - 127 * cell->occ_state;
|
||||
*pixel = RTK_RGB16(col, col, col);
|
||||
}
|
||||
}
|
||||
|
||||
// Draw the entire occupancy map as an image
|
||||
rtk_fig_image(
|
||||
fig, map->origin_x, map->origin_y, 0,
|
||||
map->scale, map->size_x, map->size_y, 16, image, NULL);
|
||||
|
||||
free(image);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
// Draw the cspace map
|
||||
void map_draw_cspace(map_t * map, rtk_fig_t * fig)
|
||||
{
|
||||
int i, j;
|
||||
int col;
|
||||
map_cell_t * cell;
|
||||
uint16_t * image;
|
||||
uint16_t * pixel;
|
||||
|
||||
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
|
||||
|
||||
// Draw occupancy
|
||||
for (j = 0; j < map->size_y; j++) {
|
||||
for (i = 0; i < map->size_x; i++) {
|
||||
cell = map->cells + MAP_INDEX(map, i, j);
|
||||
pixel = image + (j * map->size_x + i);
|
||||
|
||||
col = 255 * cell->occ_dist / map->max_occ_dist;
|
||||
|
||||
*pixel = RTK_RGB16(col, col, col);
|
||||
}
|
||||
}
|
||||
|
||||
// Draw the entire occupancy map as an image
|
||||
rtk_fig_image(
|
||||
fig, map->origin_x, map->origin_y, 0,
|
||||
map->scale, map->size_x, map->size_y, 16, image, NULL);
|
||||
|
||||
free(image);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
// Draw a wifi map
|
||||
void map_draw_wifi(map_t * map, rtk_fig_t * fig, int index)
|
||||
{
|
||||
int i, j;
|
||||
int level, col;
|
||||
map_cell_t * cell;
|
||||
uint16_t * image, * mask;
|
||||
uint16_t * ipix, * mpix;
|
||||
|
||||
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
|
||||
mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
|
||||
|
||||
// Draw wifi levels
|
||||
for (j = 0; j < map->size_y; j++) {
|
||||
for (i = 0; i < map->size_x; i++) {
|
||||
cell = map->cells + MAP_INDEX(map, i, j);
|
||||
ipix = image + (j * map->size_x + i);
|
||||
mpix = mask + (j * map->size_x + i);
|
||||
|
||||
level = cell->wifi_levels[index];
|
||||
|
||||
if (cell->occ_state == -1 && level != 0) {
|
||||
col = 255 * (100 + level) / 100;
|
||||
*ipix = RTK_RGB16(col, col, col);
|
||||
*mpix = 1;
|
||||
} else {
|
||||
*mpix = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Draw the entire occupancy map as an image
|
||||
rtk_fig_image(
|
||||
fig, map->origin_x, map->origin_y, 0,
|
||||
map->scale, map->size_x, map->size_y, 16, image, mask);
|
||||
|
||||
free(mask);
|
||||
free(image);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,118 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Range routines
|
||||
* Author: Andrew Howard
|
||||
* Date: 18 Jan 2003
|
||||
* CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
|
||||
**************************************************************************/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "nav2_amcl/map/map.hpp"
|
||||
|
||||
// Extract a single range reading from the map. Unknown cells and/or
|
||||
// out-of-bound cells are treated as occupied, which makes it easy to
|
||||
// use Stage bitmap files.
|
||||
double map_calc_range(map_t * map, double ox, double oy, double oa, double max_range)
|
||||
{
|
||||
// Bresenham raytracing
|
||||
int x0, x1, y0, y1;
|
||||
int x, y;
|
||||
int xstep, ystep;
|
||||
char steep;
|
||||
int tmp;
|
||||
int deltax, deltay, error, deltaerr;
|
||||
|
||||
x0 = MAP_GXWX(map, ox);
|
||||
y0 = MAP_GYWY(map, oy);
|
||||
|
||||
x1 = MAP_GXWX(map, ox + max_range * cos(oa));
|
||||
y1 = MAP_GYWY(map, oy + max_range * sin(oa));
|
||||
|
||||
if (abs(y1 - y0) > abs(x1 - x0)) {
|
||||
steep = 1;
|
||||
} else {
|
||||
steep = 0;
|
||||
}
|
||||
|
||||
if (steep) {
|
||||
tmp = x0;
|
||||
x0 = y0;
|
||||
y0 = tmp;
|
||||
|
||||
tmp = x1;
|
||||
x1 = y1;
|
||||
y1 = tmp;
|
||||
}
|
||||
|
||||
deltax = abs(x1 - x0);
|
||||
deltay = abs(y1 - y0);
|
||||
error = 0;
|
||||
deltaerr = deltay;
|
||||
|
||||
x = x0;
|
||||
y = y0;
|
||||
|
||||
if (x0 < x1) {
|
||||
xstep = 1;
|
||||
} else {
|
||||
xstep = -1;
|
||||
}
|
||||
if (y0 < y1) {
|
||||
ystep = 1;
|
||||
} else {
|
||||
ystep = -1;
|
||||
}
|
||||
|
||||
if (steep) {
|
||||
if (!MAP_VALID(map, y, x) || map->cells[MAP_INDEX(map, y, x)].occ_state > -1) {
|
||||
return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
|
||||
}
|
||||
} else {
|
||||
if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1) {
|
||||
return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
|
||||
}
|
||||
}
|
||||
|
||||
while (x != (x1 + xstep * 1)) {
|
||||
x += xstep;
|
||||
error += deltaerr;
|
||||
if (2 * error >= deltax) {
|
||||
y += ystep;
|
||||
error -= deltax;
|
||||
}
|
||||
|
||||
if (steep) {
|
||||
if (!MAP_VALID(map, y, x) || map->cells[MAP_INDEX(map, y, x)].occ_state > -1) {
|
||||
return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
|
||||
}
|
||||
} else {
|
||||
if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1) {
|
||||
return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
|
||||
}
|
||||
}
|
||||
}
|
||||
return max_range;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
add_library(motions_lib SHARED
|
||||
omni_motion_model.cpp
|
||||
differential_motion_model.cpp
|
||||
)
|
||||
target_link_libraries(motions_lib pf_lib)
|
||||
ament_target_dependencies(motions_lib
|
||||
pluginlib
|
||||
nav2_util
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
motions_lib
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
@@ -0,0 +1,117 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#include "nav2_amcl/motion_model/differential_motion_model.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
void
|
||||
DifferentialMotionModel::initialize(
|
||||
double alpha1, double alpha2, double alpha3, double alpha4,
|
||||
double alpha5)
|
||||
{
|
||||
alpha1_ = alpha1;
|
||||
alpha2_ = alpha2;
|
||||
alpha3_ = alpha3;
|
||||
alpha4_ = alpha4;
|
||||
alpha5_ = alpha5;
|
||||
}
|
||||
|
||||
void
|
||||
DifferentialMotionModel::odometryUpdate(
|
||||
pf_t * pf, const pf_vector_t & pose,
|
||||
const pf_vector_t & delta)
|
||||
{
|
||||
// Compute the new sample poses
|
||||
pf_sample_set_t * set;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
pf_vector_t old_pose = pf_vector_sub(pose, delta);
|
||||
|
||||
// Implement sample_motion_odometry (Prob Rob p 136)
|
||||
double delta_rot1, delta_trans, delta_rot2;
|
||||
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
|
||||
double delta_rot1_noise, delta_rot2_noise;
|
||||
|
||||
// Avoid computing a bearing from two poses that are extremely near each
|
||||
// other (happens on in-place rotation).
|
||||
if (sqrt(
|
||||
delta.v[1] * delta.v[1] +
|
||||
delta.v[0] * delta.v[0]) < 0.01)
|
||||
{
|
||||
delta_rot1 = 0.0;
|
||||
} else {
|
||||
delta_rot1 = angleutils::angle_diff(
|
||||
atan2(delta.v[1], delta.v[0]),
|
||||
old_pose.v[2]);
|
||||
}
|
||||
delta_trans = sqrt(
|
||||
delta.v[0] * delta.v[0] +
|
||||
delta.v[1] * delta.v[1]);
|
||||
delta_rot2 = angleutils::angle_diff(delta.v[2], delta_rot1);
|
||||
|
||||
// We want to treat backward and forward motion symmetrically for the
|
||||
// noise model to be applied below. The standard model seems to assume
|
||||
// forward motion.
|
||||
delta_rot1_noise = std::min(
|
||||
fabs(angleutils::angle_diff(delta_rot1, 0.0)),
|
||||
fabs(angleutils::angle_diff(delta_rot1, M_PI)));
|
||||
delta_rot2_noise = std::min(
|
||||
fabs(angleutils::angle_diff(delta_rot2, 0.0)),
|
||||
fabs(angleutils::angle_diff(delta_rot2, M_PI)));
|
||||
|
||||
for (int i = 0; i < set->sample_count; i++) {
|
||||
pf_sample_t * sample = set->samples + i;
|
||||
|
||||
// Sample pose differences
|
||||
delta_rot1_hat = angleutils::angle_diff(
|
||||
delta_rot1,
|
||||
pf_ran_gaussian(
|
||||
sqrt(
|
||||
alpha1_ * delta_rot1_noise * delta_rot1_noise +
|
||||
alpha2_ * delta_trans * delta_trans)));
|
||||
delta_trans_hat = delta_trans -
|
||||
pf_ran_gaussian(
|
||||
sqrt(
|
||||
alpha3_ * delta_trans * delta_trans +
|
||||
alpha4_ * delta_rot1_noise * delta_rot1_noise +
|
||||
alpha4_ * delta_rot2_noise * delta_rot2_noise));
|
||||
delta_rot2_hat = angleutils::angle_diff(
|
||||
delta_rot2,
|
||||
pf_ran_gaussian(
|
||||
sqrt(
|
||||
alpha1_ * delta_rot2_noise * delta_rot2_noise +
|
||||
alpha2_ * delta_trans * delta_trans)));
|
||||
|
||||
// Apply sampled update to particle pose
|
||||
sample->pose.v[0] += delta_trans_hat *
|
||||
cos(sample->pose.v[2] + delta_rot1_hat);
|
||||
sample->pose.v[1] += delta_trans_hat *
|
||||
sin(sample->pose.v[2] + delta_rot1_hat);
|
||||
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_amcl::DifferentialMotionModel, nav2_amcl::MotionModel)
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#include "nav2_amcl/motion_model/omni_motion_model.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
void
|
||||
OmniMotionModel::initialize(
|
||||
double alpha1, double alpha2, double alpha3, double alpha4,
|
||||
double alpha5)
|
||||
{
|
||||
alpha1_ = alpha1;
|
||||
alpha2_ = alpha2;
|
||||
alpha3_ = alpha3;
|
||||
alpha4_ = alpha4;
|
||||
alpha5_ = alpha5;
|
||||
}
|
||||
|
||||
void
|
||||
OmniMotionModel::odometryUpdate(
|
||||
pf_t * pf, const pf_vector_t & pose,
|
||||
const pf_vector_t & delta)
|
||||
{
|
||||
// Compute the new sample poses
|
||||
pf_sample_set_t * set;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
pf_vector_t old_pose = pf_vector_sub(pose, delta);
|
||||
|
||||
double delta_trans, delta_rot, delta_bearing;
|
||||
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
|
||||
|
||||
delta_trans = sqrt(
|
||||
delta.v[0] * delta.v[0] +
|
||||
delta.v[1] * delta.v[1]);
|
||||
delta_rot = delta.v[2];
|
||||
|
||||
// Precompute a couple of things
|
||||
double trans_hat_stddev = sqrt(
|
||||
alpha3_ * (delta_trans * delta_trans) +
|
||||
alpha4_ * (delta_rot * delta_rot) );
|
||||
double rot_hat_stddev = sqrt(
|
||||
alpha1_ * (delta_rot * delta_rot) +
|
||||
alpha2_ * (delta_trans * delta_trans) );
|
||||
double strafe_hat_stddev = sqrt(
|
||||
alpha4_ * (delta_rot * delta_rot) +
|
||||
alpha5_ * (delta_trans * delta_trans) );
|
||||
|
||||
for (int i = 0; i < set->sample_count; i++) {
|
||||
pf_sample_t * sample = set->samples + i;
|
||||
|
||||
delta_bearing = angleutils::angle_diff(
|
||||
atan2(delta.v[1], delta.v[0]),
|
||||
old_pose.v[2]) + sample->pose.v[2];
|
||||
double cs_bearing = cos(delta_bearing);
|
||||
double sn_bearing = sin(delta_bearing);
|
||||
|
||||
// Sample pose differences
|
||||
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
|
||||
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
|
||||
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
|
||||
// Apply sampled update to particle pose
|
||||
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
|
||||
delta_strafe_hat * sn_bearing);
|
||||
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
|
||||
delta_strafe_hat * cs_bearing);
|
||||
sample->pose.v[2] += delta_rot_hat;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_amcl::OmniMotionModel, nav2_amcl::MotionModel)
|
||||
@@ -0,0 +1,25 @@
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wno-gnu-folding-constant)
|
||||
endif()
|
||||
|
||||
add_library(pf_lib SHARED
|
||||
pf.c
|
||||
pf_kdtree.c
|
||||
pf_pdf.c
|
||||
pf_vector.c
|
||||
eig3.c
|
||||
pf_draw.c
|
||||
)
|
||||
|
||||
target_include_directories(pf_lib PRIVATE ../include)
|
||||
if(HAVE_DRAND48)
|
||||
target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
|
||||
endif()
|
||||
target_link_libraries(pf_lib m)
|
||||
|
||||
install(TARGETS
|
||||
pf_lib
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
@@ -0,0 +1,282 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
|
||||
domain Java Matrix library JAMA. */
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef MAX
|
||||
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||
#endif
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define n 3
|
||||
#else
|
||||
static int n = 3;
|
||||
#endif
|
||||
|
||||
// Symmetric Householder reduction to tridiagonal form.
|
||||
|
||||
static void tred2(double V[n][n], double d[n], double e[n])
|
||||
{
|
||||
// This is derived from the Algol procedures tred2 by
|
||||
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
|
||||
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
|
||||
// Fortran subroutine in EISPACK.
|
||||
|
||||
int i, j, k;
|
||||
double f, g, h, hh;
|
||||
for (j = 0; j < n; j++) {
|
||||
d[j] = V[n - 1][j];
|
||||
}
|
||||
|
||||
// Householder reduction to tridiagonal form.
|
||||
|
||||
for (i = n - 1; i > 0; i--) {
|
||||
// Scale to avoid under/overflow.
|
||||
|
||||
double scale = 0.0;
|
||||
double h = 0.0;
|
||||
for (k = 0; k < i; k++) {
|
||||
scale = scale + fabs(d[k]);
|
||||
}
|
||||
if (scale == 0.0) {
|
||||
e[i] = d[i - 1];
|
||||
for (j = 0; j < i; j++) {
|
||||
d[j] = V[i - 1][j];
|
||||
V[i][j] = 0.0;
|
||||
V[j][i] = 0.0;
|
||||
}
|
||||
} else {
|
||||
// Generate Householder vector.
|
||||
for (k = 0; k < i; k++) {
|
||||
d[k] /= scale;
|
||||
h += d[k] * d[k];
|
||||
}
|
||||
f = d[i - 1];
|
||||
g = sqrt(h);
|
||||
if (f > 0) {
|
||||
g = -g;
|
||||
}
|
||||
e[i] = scale * g;
|
||||
h = h - f * g;
|
||||
d[i - 1] = f - g;
|
||||
for (j = 0; j < i; j++) {
|
||||
e[j] = 0.0;
|
||||
}
|
||||
|
||||
// Apply similarity transformation to remaining columns.
|
||||
|
||||
for (j = 0; j < i; j++) {
|
||||
f = d[j];
|
||||
V[j][i] = f;
|
||||
g = e[j] + V[j][j] * f;
|
||||
for (k = j + 1; k <= i - 1; k++) {
|
||||
g += V[k][j] * d[k];
|
||||
e[k] += V[k][j] * f;
|
||||
}
|
||||
e[j] = g;
|
||||
}
|
||||
f = 0.0;
|
||||
for (j = 0; j < i; j++) {
|
||||
e[j] /= h;
|
||||
f += e[j] * d[j];
|
||||
}
|
||||
hh = f / (h + h);
|
||||
for (j = 0; j < i; j++) {
|
||||
e[j] -= hh * d[j];
|
||||
}
|
||||
for (j = 0; j < i; j++) {
|
||||
f = d[j];
|
||||
g = e[j];
|
||||
for (k = j; k <= i - 1; k++) {
|
||||
V[k][j] -= (f * e[k] + g * d[k]);
|
||||
}
|
||||
d[j] = V[i - 1][j];
|
||||
V[i][j] = 0.0;
|
||||
}
|
||||
}
|
||||
d[i] = h;
|
||||
}
|
||||
|
||||
// Accumulate transformations.
|
||||
|
||||
for (i = 0; i < n - 1; i++) {
|
||||
V[n - 1][i] = V[i][i];
|
||||
V[i][i] = 1.0;
|
||||
h = d[i + 1];
|
||||
if (h != 0.0) {
|
||||
for (k = 0; k <= i; k++) {
|
||||
d[k] = V[k][i + 1] / h;
|
||||
}
|
||||
for (j = 0; j <= i; j++) {
|
||||
g = 0.0;
|
||||
for (k = 0; k <= i; k++) {
|
||||
g += V[k][i + 1] * V[k][j];
|
||||
}
|
||||
for (k = 0; k <= i; k++) {
|
||||
V[k][j] -= g * d[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (k = 0; k <= i; k++) {
|
||||
V[k][i + 1] = 0.0;
|
||||
}
|
||||
}
|
||||
for (j = 0; j < n; j++) {
|
||||
d[j] = V[n - 1][j];
|
||||
V[n - 1][j] = 0.0;
|
||||
}
|
||||
V[n - 1][n - 1] = 1.0;
|
||||
e[0] = 0.0;
|
||||
}
|
||||
|
||||
// Symmetric tridiagonal QL algorithm.
|
||||
|
||||
static void tql2(double V[n][n], double d[n], double e[n])
|
||||
{
|
||||
// This is derived from the Algol procedures tql2, by
|
||||
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
|
||||
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
|
||||
// Fortran subroutine in EISPACK.
|
||||
int i, j, m, l, k;
|
||||
double g, p, r, dl1, h, f, tst1, eps;
|
||||
double c, c2, c3, el1, s, s2;
|
||||
|
||||
for (i = 1; i < n; i++) {
|
||||
e[i - 1] = e[i];
|
||||
}
|
||||
e[n - 1] = 0.0;
|
||||
|
||||
f = 0.0;
|
||||
tst1 = 0.0;
|
||||
eps = pow(2.0, -52.0);
|
||||
for (l = 0; l < n; l++) {
|
||||
// Find small subdiagonal element
|
||||
tst1 = MAX(tst1, fabs(d[l]) + fabs(e[l]));
|
||||
m = l;
|
||||
while (m < n) {
|
||||
if (fabs(e[m]) <= eps * tst1) {
|
||||
break;
|
||||
}
|
||||
m++;
|
||||
}
|
||||
|
||||
// If m == l, d[l] is an eigenvalue,
|
||||
// otherwise, iterate.
|
||||
|
||||
if (m > l) {
|
||||
int iter = 0;
|
||||
do {
|
||||
iter = iter + 1; // (Could check iteration count here.)
|
||||
|
||||
// Compute implicit shift
|
||||
|
||||
g = d[l];
|
||||
p = (d[l + 1] - g) / (2.0 * e[l]);
|
||||
r = hypot(p, 1.0);
|
||||
if (p < 0) {
|
||||
r = -r;
|
||||
}
|
||||
d[l] = e[l] / (p + r);
|
||||
d[l + 1] = e[l] * (p + r);
|
||||
dl1 = d[l + 1];
|
||||
h = g - d[l];
|
||||
for (i = l + 2; i < n; i++) {
|
||||
d[i] -= h;
|
||||
}
|
||||
f = f + h;
|
||||
|
||||
// Implicit QL transformation.
|
||||
|
||||
p = d[m];
|
||||
c = 1.0;
|
||||
c2 = c;
|
||||
c3 = c;
|
||||
el1 = e[l + 1];
|
||||
s = 0.0;
|
||||
s2 = 0.0;
|
||||
for (i = m - 1; i >= l; i--) {
|
||||
c3 = c2;
|
||||
c2 = c;
|
||||
s2 = s;
|
||||
g = c * e[i];
|
||||
h = c * p;
|
||||
r = hypot(p, e[i]);
|
||||
e[i + 1] = s * r;
|
||||
s = e[i] / r;
|
||||
c = p / r;
|
||||
p = c * d[i] - s * g;
|
||||
d[i + 1] = h + s * (c * g + s * d[i]);
|
||||
|
||||
// Accumulate transformation.
|
||||
|
||||
for (k = 0; k < n; k++) {
|
||||
h = V[k][i + 1];
|
||||
V[k][i + 1] = s * V[k][i] + c * h;
|
||||
V[k][i] = c * V[k][i] - s * h;
|
||||
}
|
||||
}
|
||||
p = -s * s2 * c3 * el1 * e[l] / dl1;
|
||||
e[l] = s * p;
|
||||
d[l] = c * p;
|
||||
|
||||
// Check for convergence.
|
||||
} while (fabs(e[l]) > eps * tst1);
|
||||
}
|
||||
d[l] = d[l] + f;
|
||||
e[l] = 0.0;
|
||||
}
|
||||
// Sort eigenvalues and corresponding vectors.
|
||||
|
||||
for (i = 0; i < n - 1; i++) {
|
||||
k = i;
|
||||
p = d[i];
|
||||
for (j = i + 1; j < n; j++) {
|
||||
if (d[j] < p) {
|
||||
k = j;
|
||||
p = d[j];
|
||||
}
|
||||
}
|
||||
if (k != i) {
|
||||
d[k] = d[i];
|
||||
d[i] = p;
|
||||
for (j = 0; j < n; j++) {
|
||||
p = V[j][i];
|
||||
V[j][i] = V[j][k];
|
||||
V[j][k] = p;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void eigen_decomposition(double A[n][n], double V[n][n], double d[n])
|
||||
{
|
||||
int i, j;
|
||||
double e[n]; // NOLINT
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = 0; j < n; j++) {
|
||||
V[i][j] = A[i][j];
|
||||
}
|
||||
}
|
||||
tred2(V, d, e);
|
||||
tql2(V, d, e);
|
||||
}
|
||||
@@ -0,0 +1,646 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Simple particle filter for localization.
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
|
||||
*************************************************************************/
|
||||
|
||||
#include <float.h>
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "nav2_amcl/pf/pf.hpp"
|
||||
#include "nav2_amcl/pf/pf_pdf.hpp"
|
||||
#include "nav2_amcl/pf/pf_kdtree.hpp"
|
||||
|
||||
#include "nav2_amcl/portable_utils.hpp"
|
||||
|
||||
|
||||
// Compute the required number of samples, given that there are k bins
|
||||
// with samples in them.
|
||||
static int pf_resample_limit(pf_t * pf, int k);
|
||||
|
||||
|
||||
// Create a new filter
|
||||
pf_t * pf_alloc(
|
||||
int min_samples, int max_samples,
|
||||
double alpha_slow, double alpha_fast,
|
||||
pf_init_model_fn_t random_pose_fn)
|
||||
{
|
||||
int i, j;
|
||||
pf_t * pf;
|
||||
pf_sample_set_t * set;
|
||||
pf_sample_t * sample;
|
||||
|
||||
srand48(time(NULL));
|
||||
|
||||
pf = calloc(1, sizeof(pf_t));
|
||||
|
||||
pf->random_pose_fn = random_pose_fn;
|
||||
|
||||
pf->min_samples = min_samples;
|
||||
pf->max_samples = max_samples;
|
||||
|
||||
// Control parameters for the population size calculation. [err] is
|
||||
// the max error between the true distribution and the estimated
|
||||
// distribution. [z] is the upper standard normal quantile for (1 -
|
||||
// p), where p is the probability that the error on the estimated
|
||||
// distrubition will be less than [err].
|
||||
pf->pop_err = 0.01;
|
||||
pf->pop_z = 3;
|
||||
pf->dist_threshold = 0.5;
|
||||
|
||||
pf->current_set = 0;
|
||||
for (j = 0; j < 2; j++) {
|
||||
set = pf->sets + j;
|
||||
|
||||
set->sample_count = max_samples;
|
||||
set->samples = calloc(max_samples, sizeof(pf_sample_t));
|
||||
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
sample->pose.v[0] = 0.0;
|
||||
sample->pose.v[1] = 0.0;
|
||||
sample->pose.v[2] = 0.0;
|
||||
sample->weight = 1.0 / max_samples;
|
||||
}
|
||||
|
||||
// HACK: is 3 times max_samples enough?
|
||||
set->kdtree = pf_kdtree_alloc(3 * max_samples);
|
||||
|
||||
set->cluster_count = 0;
|
||||
set->cluster_max_count = max_samples;
|
||||
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
|
||||
|
||||
set->mean = pf_vector_zero();
|
||||
set->cov = pf_matrix_zero();
|
||||
}
|
||||
|
||||
pf->w_slow = 0.0;
|
||||
pf->w_fast = 0.0;
|
||||
|
||||
pf->alpha_slow = alpha_slow;
|
||||
pf->alpha_fast = alpha_fast;
|
||||
|
||||
// set converged to 0
|
||||
pf_init_converged(pf);
|
||||
|
||||
return pf;
|
||||
}
|
||||
|
||||
// Free an existing filter
|
||||
void pf_free(pf_t * pf)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < 2; i++) {
|
||||
free(pf->sets[i].clusters);
|
||||
pf_kdtree_free(pf->sets[i].kdtree);
|
||||
free(pf->sets[i].samples);
|
||||
}
|
||||
free(pf);
|
||||
}
|
||||
|
||||
// Initialize the filter using a guassian
|
||||
void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov)
|
||||
{
|
||||
int i;
|
||||
pf_sample_set_t * set;
|
||||
pf_sample_t * sample;
|
||||
pf_pdf_gaussian_t * pdf;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
|
||||
// Create the kd tree for adaptive sampling
|
||||
pf_kdtree_clear(set->kdtree);
|
||||
|
||||
set->sample_count = pf->max_samples;
|
||||
|
||||
pdf = pf_pdf_gaussian_alloc(mean, cov);
|
||||
|
||||
// Compute the new sample poses
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
sample->weight = 1.0 / pf->max_samples;
|
||||
sample->pose = pf_pdf_gaussian_sample(pdf);
|
||||
|
||||
// Add sample to histogram
|
||||
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
|
||||
}
|
||||
|
||||
pf->w_slow = pf->w_fast = 0.0;
|
||||
|
||||
pf_pdf_gaussian_free(pdf);
|
||||
|
||||
// Re-compute cluster statistics
|
||||
pf_cluster_stats(pf, set);
|
||||
|
||||
// set converged to 0
|
||||
pf_init_converged(pf);
|
||||
}
|
||||
|
||||
|
||||
// Initialize the filter using some model
|
||||
void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data)
|
||||
{
|
||||
int i;
|
||||
pf_sample_set_t * set;
|
||||
pf_sample_t * sample;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
|
||||
// Create the kd tree for adaptive sampling
|
||||
pf_kdtree_clear(set->kdtree);
|
||||
|
||||
set->sample_count = pf->max_samples;
|
||||
|
||||
// Compute the new sample poses
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
sample->weight = 1.0 / pf->max_samples;
|
||||
sample->pose = (*init_fn)(init_data);
|
||||
|
||||
// Add sample to histogram
|
||||
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
|
||||
}
|
||||
|
||||
pf->w_slow = pf->w_fast = 0.0;
|
||||
|
||||
// Re-compute cluster statistics
|
||||
pf_cluster_stats(pf, set);
|
||||
|
||||
// set converged to 0
|
||||
pf_init_converged(pf);
|
||||
}
|
||||
|
||||
void pf_init_converged(pf_t * pf)
|
||||
{
|
||||
pf_sample_set_t * set;
|
||||
set = pf->sets + pf->current_set;
|
||||
set->converged = 0;
|
||||
pf->converged = 0;
|
||||
}
|
||||
|
||||
int pf_update_converged(pf_t * pf)
|
||||
{
|
||||
int i;
|
||||
pf_sample_set_t * set;
|
||||
pf_sample_t * sample;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
double mean_x = 0, mean_y = 0;
|
||||
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
|
||||
mean_x += sample->pose.v[0];
|
||||
mean_y += sample->pose.v[1];
|
||||
}
|
||||
mean_x /= set->sample_count;
|
||||
mean_y /= set->sample_count;
|
||||
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
if (fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold ||
|
||||
fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold)
|
||||
{
|
||||
set->converged = 0;
|
||||
pf->converged = 0;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
set->converged = 1;
|
||||
pf->converged = 1;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Update the filter with some new action
|
||||
// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data)
|
||||
// {
|
||||
// pf_sample_set_t * set;
|
||||
|
||||
// set = pf->sets + pf->current_set;
|
||||
|
||||
// (*action_fn)(action_data, set);
|
||||
// }
|
||||
|
||||
// Update the filter with some new sensor observation
|
||||
void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data)
|
||||
{
|
||||
int i;
|
||||
pf_sample_set_t * set;
|
||||
pf_sample_t * sample;
|
||||
double total;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
|
||||
// Compute the sample weights
|
||||
total = (*sensor_fn)(sensor_data, set);
|
||||
|
||||
if (total > 0.0) {
|
||||
// Normalize weights
|
||||
double w_avg = 0.0;
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
w_avg += sample->weight;
|
||||
sample->weight /= total;
|
||||
}
|
||||
// Update running averages of likelihood of samples (Prob Rob p258)
|
||||
w_avg /= set->sample_count;
|
||||
if (pf->w_slow == 0.0) {
|
||||
pf->w_slow = w_avg;
|
||||
} else {
|
||||
pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
|
||||
}
|
||||
if (pf->w_fast == 0.0) {
|
||||
pf->w_fast = w_avg;
|
||||
} else {
|
||||
pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
|
||||
}
|
||||
} else {
|
||||
// Handle zero total
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
sample->weight = 1.0 / set->sample_count;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Resample the distribution
|
||||
void pf_update_resample(pf_t * pf, void * random_pose_data)
|
||||
{
|
||||
int i;
|
||||
double total;
|
||||
pf_sample_set_t * set_a, * set_b;
|
||||
pf_sample_t * sample_a, * sample_b;
|
||||
|
||||
// double r,c,U;
|
||||
// int m;
|
||||
// double count_inv;
|
||||
double * c;
|
||||
|
||||
double w_diff;
|
||||
|
||||
set_a = pf->sets + pf->current_set;
|
||||
set_b = pf->sets + (pf->current_set + 1) % 2;
|
||||
|
||||
// Build up cumulative probability table for resampling.
|
||||
// TODO(?): Replace this with a more efficient procedure
|
||||
// (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
|
||||
c = (double *)malloc(sizeof(double) * (set_a->sample_count + 1));
|
||||
c[0] = 0.0;
|
||||
for (i = 0; i < set_a->sample_count; i++) {
|
||||
c[i + 1] = c[i] + set_a->samples[i].weight;
|
||||
}
|
||||
|
||||
// Create the kd tree for adaptive sampling
|
||||
pf_kdtree_clear(set_b->kdtree);
|
||||
|
||||
// Draw samples from set a to create set b.
|
||||
total = 0;
|
||||
set_b->sample_count = 0;
|
||||
|
||||
w_diff = 1.0 - pf->w_fast / pf->w_slow;
|
||||
if (w_diff < 0.0) {
|
||||
w_diff = 0.0;
|
||||
}
|
||||
// printf("w_diff: %9.6f\n", w_diff);
|
||||
|
||||
// Can't (easily) combine low-variance sampler with KLD adaptive
|
||||
// sampling, so we'll take the more traditional route.
|
||||
/*
|
||||
// Low-variance resampler, taken from Probabilistic Robotics, p110
|
||||
count_inv = 1.0/set_a->sample_count;
|
||||
r = drand48() * count_inv;
|
||||
c = set_a->samples[0].weight;
|
||||
i = 0;
|
||||
m = 0;
|
||||
*/
|
||||
while (set_b->sample_count < pf->max_samples) {
|
||||
sample_b = set_b->samples + set_b->sample_count++;
|
||||
|
||||
if (drand48() < w_diff) {
|
||||
sample_b->pose = (pf->random_pose_fn)(random_pose_data);
|
||||
} else {
|
||||
// Can't (easily) combine low-variance sampler with KLD adaptive
|
||||
// sampling, so we'll take the more traditional route.
|
||||
/*
|
||||
// Low-variance resampler, taken from Probabilistic Robotics, p110
|
||||
U = r + m * count_inv;
|
||||
while(U>c)
|
||||
{
|
||||
i++;
|
||||
// Handle wrap-around by resetting counters and picking a new random
|
||||
// number
|
||||
if(i >= set_a->sample_count)
|
||||
{
|
||||
r = drand48() * count_inv;
|
||||
c = set_a->samples[0].weight;
|
||||
i = 0;
|
||||
m = 0;
|
||||
U = r + m * count_inv;
|
||||
continue;
|
||||
}
|
||||
c += set_a->samples[i].weight;
|
||||
}
|
||||
m++;
|
||||
*/
|
||||
|
||||
// Naive discrete event sampler
|
||||
double r;
|
||||
r = drand48();
|
||||
for (i = 0; i < set_a->sample_count; i++) {
|
||||
if ((c[i] <= r) && (r < c[i + 1])) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert(i < set_a->sample_count);
|
||||
|
||||
sample_a = set_a->samples + i;
|
||||
|
||||
assert(sample_a->weight > 0);
|
||||
|
||||
// Add sample to list
|
||||
sample_b->pose = sample_a->pose;
|
||||
}
|
||||
|
||||
sample_b->weight = 1.0;
|
||||
total += sample_b->weight;
|
||||
|
||||
// Add sample to histogram
|
||||
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
|
||||
|
||||
// See if we have enough samples yet
|
||||
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset averages, to avoid spiraling off into complete randomness.
|
||||
if (w_diff > 0.0) {
|
||||
pf->w_slow = pf->w_fast = 0.0;
|
||||
}
|
||||
|
||||
// fprintf(stderr, "\n\n");
|
||||
|
||||
// Normalize weights
|
||||
for (i = 0; i < set_b->sample_count; i++) {
|
||||
sample_b = set_b->samples + i;
|
||||
sample_b->weight /= total;
|
||||
}
|
||||
|
||||
// Re-compute cluster statistics
|
||||
pf_cluster_stats(pf, set_b);
|
||||
|
||||
// Use the newly created sample set
|
||||
pf->current_set = (pf->current_set + 1) % 2;
|
||||
|
||||
pf_update_converged(pf);
|
||||
|
||||
free(c);
|
||||
}
|
||||
|
||||
|
||||
// Compute the required number of samples, given that there are k bins
|
||||
// with samples in them. This is taken directly from Fox et al.
|
||||
int pf_resample_limit(pf_t * pf, int k)
|
||||
{
|
||||
double a, b, c, x;
|
||||
int n;
|
||||
|
||||
if (k <= 1) {
|
||||
return pf->max_samples;
|
||||
}
|
||||
|
||||
a = 1;
|
||||
b = 2 / (9 * ((double) k - 1));
|
||||
c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
|
||||
x = a - b + c;
|
||||
|
||||
n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
|
||||
|
||||
if (n < pf->min_samples) {
|
||||
return pf->min_samples;
|
||||
}
|
||||
if (n > pf->max_samples) {
|
||||
return pf->max_samples;
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
|
||||
// Re-compute the cluster statistics for a sample set
|
||||
void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
|
||||
{
|
||||
(void)pf;
|
||||
int i, j, k, cidx;
|
||||
pf_sample_t * sample;
|
||||
pf_cluster_t * cluster;
|
||||
|
||||
// Workspace
|
||||
double m[4], c[2][2];
|
||||
double weight;
|
||||
|
||||
// Cluster the samples
|
||||
pf_kdtree_cluster(set->kdtree);
|
||||
|
||||
// Initialize cluster stats
|
||||
set->cluster_count = 0;
|
||||
|
||||
for (i = 0; i < set->cluster_max_count; i++) {
|
||||
cluster = set->clusters + i;
|
||||
cluster->weight = 0;
|
||||
cluster->mean = pf_vector_zero();
|
||||
cluster->cov = pf_matrix_zero();
|
||||
|
||||
for (j = 0; j < 4; j++) {
|
||||
cluster->m[j] = 0.0;
|
||||
}
|
||||
for (j = 0; j < 2; j++) {
|
||||
for (k = 0; k < 2; k++) {
|
||||
cluster->c[j][k] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize overall filter stats
|
||||
weight = 0.0;
|
||||
set->mean = pf_vector_zero();
|
||||
set->cov = pf_matrix_zero();
|
||||
for (j = 0; j < 4; j++) {
|
||||
m[j] = 0.0;
|
||||
}
|
||||
for (j = 0; j < 2; j++) {
|
||||
for (k = 0; k < 2; k++) {
|
||||
c[j][k] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Compute cluster stats
|
||||
for (i = 0; i < set->sample_count; i++) {
|
||||
sample = set->samples + i;
|
||||
|
||||
// printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
|
||||
|
||||
// Get the cluster label for this sample
|
||||
cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
|
||||
assert(cidx >= 0);
|
||||
if (cidx >= set->cluster_max_count) {
|
||||
continue;
|
||||
}
|
||||
if (cidx + 1 > set->cluster_count) {
|
||||
set->cluster_count = cidx + 1;
|
||||
}
|
||||
|
||||
cluster = set->clusters + cidx;
|
||||
|
||||
cluster->weight += sample->weight;
|
||||
|
||||
weight += sample->weight;
|
||||
|
||||
// Compute mean
|
||||
cluster->m[0] += sample->weight * sample->pose.v[0];
|
||||
cluster->m[1] += sample->weight * sample->pose.v[1];
|
||||
cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
|
||||
cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
|
||||
|
||||
m[0] += sample->weight * sample->pose.v[0];
|
||||
m[1] += sample->weight * sample->pose.v[1];
|
||||
m[2] += sample->weight * cos(sample->pose.v[2]);
|
||||
m[3] += sample->weight * sin(sample->pose.v[2]);
|
||||
|
||||
// Compute covariance in linear components
|
||||
for (j = 0; j < 2; j++) {
|
||||
for (k = 0; k < 2; k++) {
|
||||
cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
|
||||
c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize
|
||||
for (i = 0; i < set->cluster_count; i++) {
|
||||
cluster = set->clusters + i;
|
||||
|
||||
cluster->mean.v[0] = cluster->m[0] / cluster->weight;
|
||||
cluster->mean.v[1] = cluster->m[1] / cluster->weight;
|
||||
cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
|
||||
|
||||
cluster->cov = pf_matrix_zero();
|
||||
|
||||
// Covariance in linear components
|
||||
for (j = 0; j < 2; j++) {
|
||||
for (k = 0; k < 2; k++) {
|
||||
cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
|
||||
cluster->mean.v[j] * cluster->mean.v[k];
|
||||
}
|
||||
}
|
||||
|
||||
// Covariance in angular components; I think this is the correct
|
||||
// formula for circular statistics.
|
||||
cluster->cov.m[2][2] = -2 * log(
|
||||
sqrt(
|
||||
cluster->m[2] * cluster->m[2] +
|
||||
cluster->m[3] * cluster->m[3]));
|
||||
|
||||
// printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
|
||||
// cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
|
||||
// pf_matrix_fprintf(cluster->cov, stdout, "%e");
|
||||
}
|
||||
|
||||
// Compute overall filter stats
|
||||
set->mean.v[0] = m[0] / weight;
|
||||
set->mean.v[1] = m[1] / weight;
|
||||
set->mean.v[2] = atan2(m[3], m[2]);
|
||||
|
||||
// Covariance in linear components
|
||||
for (j = 0; j < 2; j++) {
|
||||
for (k = 0; k < 2; k++) {
|
||||
set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
|
||||
}
|
||||
}
|
||||
|
||||
// Covariance in angular components; I think this is the correct
|
||||
// formula for circular statistics.
|
||||
set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
|
||||
}
|
||||
|
||||
|
||||
// Compute the CEP statistics (mean and variance).
|
||||
// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var)
|
||||
// {
|
||||
// int i;
|
||||
// double mn, mx, my, mrr;
|
||||
// pf_sample_set_t * set;
|
||||
// pf_sample_t * sample;
|
||||
|
||||
// set = pf->sets + pf->current_set;
|
||||
|
||||
// mn = 0.0;
|
||||
// mx = 0.0;
|
||||
// my = 0.0;
|
||||
// mrr = 0.0;
|
||||
|
||||
// for (i = 0; i < set->sample_count; i++) {
|
||||
// sample = set->samples + i;
|
||||
|
||||
// mn += sample->weight;
|
||||
// mx += sample->weight * sample->pose.v[0];
|
||||
// my += sample->weight * sample->pose.v[1];
|
||||
// mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
|
||||
// mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
|
||||
// }
|
||||
|
||||
// mean->v[0] = mx / mn;
|
||||
// mean->v[1] = my / mn;
|
||||
// mean->v[2] = 0.0;
|
||||
|
||||
// *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
|
||||
// }
|
||||
|
||||
|
||||
// Get the statistics for a particular cluster.
|
||||
int pf_get_cluster_stats(
|
||||
pf_t * pf, int clabel, double * weight,
|
||||
pf_vector_t * mean, pf_matrix_t * cov)
|
||||
{
|
||||
pf_sample_set_t * set;
|
||||
pf_cluster_t * cluster;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
|
||||
if (clabel >= set->cluster_count) {
|
||||
return 0;
|
||||
}
|
||||
cluster = set->clusters + clabel;
|
||||
|
||||
*weight = cluster->weight;
|
||||
*mean = cluster->mean;
|
||||
*cov = cluster->cov;
|
||||
|
||||
return 1;
|
||||
}
|
||||
@@ -0,0 +1,150 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Particle filter; drawing routines
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
|
||||
*************************************************************************/
|
||||
|
||||
#pragma GCC diagnostic ignored "-Wpedantic"
|
||||
#ifdef INCLUDE_RTKGUI
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
#include <rtk.h>
|
||||
|
||||
#include "nav2_amcl/pf/pf.hpp"
|
||||
#include "nav2_amcl/pf/pf_pdf.hpp"
|
||||
#include "nav2_amcl/pf/pf_kdtree.hpp"
|
||||
|
||||
// Draw the statistics
|
||||
void pf_draw_statistics(pf_t * pf, rtk_fig_t * fig);
|
||||
|
||||
|
||||
// Draw the sample set
|
||||
void pf_draw_samples(pf_t * pf, rtk_fig_t * fig, int max_samples)
|
||||
{
|
||||
int i;
|
||||
double px, py, pa;
|
||||
pf_sample_set_t * set;
|
||||
pf_sample_t * sample;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
max_samples = MIN(max_samples, set->sample_count);
|
||||
|
||||
for (i = 0; i < max_samples; i++) {
|
||||
sample = set->samples + i;
|
||||
|
||||
px = sample->pose.v[0];
|
||||
py = sample->pose.v[1];
|
||||
pa = sample->pose.v[2];
|
||||
|
||||
// printf("%f %f\n", px, py);
|
||||
|
||||
rtk_fig_point(fig, px, py);
|
||||
rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
|
||||
// rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Draw the hitogram (kd tree)
|
||||
void pf_draw_hist(pf_t * pf, rtk_fig_t * fig)
|
||||
{
|
||||
pf_sample_set_t * set;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
|
||||
rtk_fig_color(fig, 0.0, 0.0, 1.0);
|
||||
pf_kdtree_draw(set->kdtree, fig);
|
||||
}
|
||||
|
||||
|
||||
// Draw the CEP statistics
|
||||
// void pf_draw_cep_stats(pf_t * pf, rtk_fig_t * fig)
|
||||
// {
|
||||
// pf_vector_t mean;
|
||||
// double var;
|
||||
|
||||
// pf_get_cep_stats(pf, &mean, &var);
|
||||
// var = sqrt(var);
|
||||
|
||||
// rtk_fig_color(fig, 0, 0, 1);
|
||||
// rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
|
||||
// }
|
||||
|
||||
// Draw the cluster statistics
|
||||
void pf_draw_cluster_stats(pf_t * pf, rtk_fig_t * fig)
|
||||
{
|
||||
int i;
|
||||
pf_cluster_t * cluster;
|
||||
pf_sample_set_t * set;
|
||||
pf_vector_t mean;
|
||||
pf_matrix_t cov;
|
||||
pf_matrix_t r, d;
|
||||
double weight, o, d1, d2;
|
||||
|
||||
set = pf->sets + pf->current_set;
|
||||
|
||||
for (i = 0; i < set->cluster_count; i++) {
|
||||
cluster = set->clusters + i;
|
||||
|
||||
weight = cluster->weight;
|
||||
mean = cluster->mean;
|
||||
cov = cluster->cov;
|
||||
|
||||
// Compute unitary representation S = R D R^T
|
||||
pf_matrix_unitary(&r, &d, cov);
|
||||
|
||||
/* Debugging
|
||||
printf("mean = \n");
|
||||
pf_vector_fprintf(mean, stdout, "%e");
|
||||
printf("cov = \n");
|
||||
pf_matrix_fprintf(cov, stdout, "%e");
|
||||
printf("r = \n");
|
||||
pf_matrix_fprintf(r, stdout, "%e");
|
||||
printf("d = \n");
|
||||
pf_matrix_fprintf(d, stdout, "%e");
|
||||
*/
|
||||
|
||||
// Compute the orientation of the error ellipse (first eigenvector)
|
||||
o = atan2(r.m[1][0], r.m[0][0]);
|
||||
d1 = 6 * sqrt(d.m[0][0]);
|
||||
d2 = 6 * sqrt(d.m[1][1]);
|
||||
|
||||
if (d1 > 1e-3 && d2 > 1e-3) {
|
||||
// Draw the error ellipse
|
||||
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
|
||||
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
|
||||
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
|
||||
}
|
||||
|
||||
// Draw a direction indicator
|
||||
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
|
||||
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
|
||||
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,462 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: kd-tree functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 18 Dec 2002
|
||||
* CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
|
||||
*************************************************************************/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#include "nav2_amcl/pf/pf_vector.hpp"
|
||||
#include "nav2_amcl/pf/pf_kdtree.hpp"
|
||||
|
||||
|
||||
// Compare keys to see if they are equal
|
||||
static int pf_kdtree_equal(pf_kdtree_t * self, int key_a[], int key_b[]);
|
||||
|
||||
// Insert a node into the tree
|
||||
static pf_kdtree_node_t * pf_kdtree_insert_node(
|
||||
pf_kdtree_t * self, pf_kdtree_node_t * parent,
|
||||
pf_kdtree_node_t * node, int key[], double value);
|
||||
|
||||
// Recursive node search
|
||||
static pf_kdtree_node_t * pf_kdtree_find_node(
|
||||
pf_kdtree_t * self, pf_kdtree_node_t * node,
|
||||
int key[]);
|
||||
|
||||
// Recursively label nodes in this cluster
|
||||
static void pf_kdtree_cluster_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int depth);
|
||||
|
||||
// Recursive node printing
|
||||
// static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node);
|
||||
|
||||
|
||||
#ifdef INCLUDE_RTKGUI
|
||||
|
||||
// Recursively draw nodes
|
||||
static void pf_kdtree_draw_node(pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t * fig);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Create a tree
|
||||
pf_kdtree_t * pf_kdtree_alloc(int max_size)
|
||||
{
|
||||
pf_kdtree_t * self;
|
||||
|
||||
self = calloc(1, sizeof(pf_kdtree_t));
|
||||
|
||||
self->size[0] = 0.50;
|
||||
self->size[1] = 0.50;
|
||||
self->size[2] = (10 * M_PI / 180);
|
||||
|
||||
self->root = NULL;
|
||||
|
||||
self->node_count = 0;
|
||||
self->node_max_count = max_size;
|
||||
self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));
|
||||
|
||||
self->leaf_count = 0;
|
||||
|
||||
return self;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Destroy a tree
|
||||
void pf_kdtree_free(pf_kdtree_t * self)
|
||||
{
|
||||
free(self->nodes);
|
||||
free(self);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Clear all entries from the tree
|
||||
void pf_kdtree_clear(pf_kdtree_t * self)
|
||||
{
|
||||
self->root = NULL;
|
||||
self->leaf_count = 0;
|
||||
self->node_count = 0;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Insert a pose into the tree.
|
||||
void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value)
|
||||
{
|
||||
int key[3];
|
||||
|
||||
key[0] = floor(pose.v[0] / self->size[0]);
|
||||
key[1] = floor(pose.v[1] / self->size[1]);
|
||||
key[2] = floor(pose.v[2] / self->size[2]);
|
||||
|
||||
self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
|
||||
|
||||
// Test code
|
||||
/*
|
||||
printf("find %d %d %d\n", key[0], key[1], key[2]);
|
||||
assert(pf_kdtree_find_node(self, self->root, key) != NULL);
|
||||
|
||||
pf_kdtree_print_node(self, self->root);
|
||||
|
||||
printf("\n");
|
||||
|
||||
for (i = 0; i < self->node_count; i++)
|
||||
{
|
||||
node = self->nodes + i;
|
||||
if (node->leaf)
|
||||
{
|
||||
printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]);
|
||||
assert(pf_kdtree_find_node(self, self->root, node->key) == node);
|
||||
}
|
||||
}
|
||||
printf("\n\n");
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Determine the probability estimate for the given pose. TODO: this
|
||||
// should do a kernel density estimate rather than a simple histogram.
|
||||
// double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose)
|
||||
// {
|
||||
// int key[3];
|
||||
// pf_kdtree_node_t * node;
|
||||
|
||||
// key[0] = floor(pose.v[0] / self->size[0]);
|
||||
// key[1] = floor(pose.v[1] / self->size[1]);
|
||||
// key[2] = floor(pose.v[2] / self->size[2]);
|
||||
|
||||
// node = pf_kdtree_find_node(self, self->root, key);
|
||||
// if (node == NULL) {
|
||||
// return 0.0;
|
||||
// }
|
||||
// return node->value;
|
||||
// }
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Determine the cluster label for the given pose
|
||||
int pf_kdtree_get_cluster(pf_kdtree_t * self, pf_vector_t pose)
|
||||
{
|
||||
int key[3];
|
||||
pf_kdtree_node_t * node;
|
||||
|
||||
key[0] = floor(pose.v[0] / self->size[0]);
|
||||
key[1] = floor(pose.v[1] / self->size[1]);
|
||||
key[2] = floor(pose.v[2] / self->size[2]);
|
||||
|
||||
node = pf_kdtree_find_node(self, self->root, key);
|
||||
if (node == NULL) {
|
||||
return -1;
|
||||
}
|
||||
return node->cluster;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Compare keys to see if they are equal
|
||||
int pf_kdtree_equal(pf_kdtree_t * self, int key_a[], int key_b[])
|
||||
{
|
||||
(void)self;
|
||||
// double a, b;
|
||||
|
||||
if (key_a[0] != key_b[0]) {
|
||||
return 0;
|
||||
}
|
||||
if (key_a[1] != key_b[1]) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (key_a[2] != key_b[2]) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* TODO: make this work (pivot selection needs fixing, too)
|
||||
// Normalize angles
|
||||
a = key_a[2] * self->size[2];
|
||||
a = atan2(sin(a), cos(a)) / self->size[2];
|
||||
b = key_b[2] * self->size[2];
|
||||
b = atan2(sin(b), cos(b)) / self->size[2];
|
||||
|
||||
if ((int) a != (int) b)
|
||||
return 0;
|
||||
*/
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Insert a node into the tree
|
||||
pf_kdtree_node_t * pf_kdtree_insert_node(
|
||||
pf_kdtree_t * self, pf_kdtree_node_t * parent,
|
||||
pf_kdtree_node_t * node, int key[], double value)
|
||||
{
|
||||
int i;
|
||||
int split, max_split;
|
||||
|
||||
// If the node doesnt exist yet...
|
||||
if (node == NULL) {
|
||||
assert(self->node_count < self->node_max_count);
|
||||
node = self->nodes + self->node_count++;
|
||||
memset(node, 0, sizeof(pf_kdtree_node_t));
|
||||
|
||||
node->leaf = 1;
|
||||
|
||||
if (parent == NULL) {
|
||||
node->depth = 0;
|
||||
} else {
|
||||
node->depth = parent->depth + 1;
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
node->key[i] = key[i];
|
||||
}
|
||||
|
||||
node->value = value;
|
||||
self->leaf_count += 1;
|
||||
} else if (node->leaf) { // If the node exists, and it is a leaf node...
|
||||
// If the keys are equal, increment the value
|
||||
if (pf_kdtree_equal(self, key, node->key)) {
|
||||
node->value += value;
|
||||
} else { // The keys are not equal, so split this node
|
||||
// Find the dimension with the largest variance and do a mean
|
||||
// split
|
||||
max_split = 0;
|
||||
node->pivot_dim = -1;
|
||||
for (i = 0; i < 3; i++) {
|
||||
split = abs(key[i] - node->key[i]);
|
||||
if (split > max_split) {
|
||||
max_split = split;
|
||||
node->pivot_dim = i;
|
||||
}
|
||||
}
|
||||
assert(node->pivot_dim >= 0);
|
||||
|
||||
node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0;
|
||||
|
||||
if (key[node->pivot_dim] < node->pivot_value) {
|
||||
node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value);
|
||||
node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
|
||||
} else {
|
||||
node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
|
||||
node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value);
|
||||
}
|
||||
|
||||
node->leaf = 0;
|
||||
self->leaf_count -= 1;
|
||||
}
|
||||
} else { // If the node exists, and it has children...
|
||||
assert(node->children[0] != NULL);
|
||||
assert(node->children[1] != NULL);
|
||||
|
||||
if (key[node->pivot_dim] < node->pivot_value) {
|
||||
pf_kdtree_insert_node(self, node, node->children[0], key, value);
|
||||
} else {
|
||||
pf_kdtree_insert_node(self, node, node->children[1], key, value);
|
||||
}
|
||||
}
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Recursive node search
|
||||
pf_kdtree_node_t * pf_kdtree_find_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int key[])
|
||||
{
|
||||
if (node->leaf) {
|
||||
// printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]);
|
||||
|
||||
// If the keys are the same...
|
||||
if (pf_kdtree_equal(self, key, node->key)) {
|
||||
return node;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
} else {
|
||||
// printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value);
|
||||
|
||||
assert(node->children[0] != NULL);
|
||||
assert(node->children[1] != NULL);
|
||||
|
||||
// If the keys are different...
|
||||
if (key[node->pivot_dim] < node->pivot_value) {
|
||||
return pf_kdtree_find_node(self, node->children[0], key);
|
||||
} else {
|
||||
return pf_kdtree_find_node(self, node->children[1], key);
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Recursive node printing
|
||||
/*
|
||||
void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node)
|
||||
{
|
||||
if (node->leaf)
|
||||
{
|
||||
printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]);
|
||||
printf("%*s", node->depth * 11, "");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]);
|
||||
pf_kdtree_print_node(self, node->children[0]);
|
||||
pf_kdtree_print_node(self, node->children[1]);
|
||||
}
|
||||
return;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Cluster the leaves in the tree
|
||||
void pf_kdtree_cluster(pf_kdtree_t * self)
|
||||
{
|
||||
int i;
|
||||
int queue_count, cluster_count;
|
||||
pf_kdtree_node_t ** queue, * node;
|
||||
|
||||
queue_count = 0;
|
||||
queue = calloc(self->node_count, sizeof(queue[0]));
|
||||
|
||||
// Put all the leaves in a queue
|
||||
for (i = 0; i < self->node_count; i++) {
|
||||
node = self->nodes + i;
|
||||
if (node->leaf) {
|
||||
node->cluster = -1;
|
||||
assert(queue_count < self->node_count);
|
||||
queue[queue_count++] = node;
|
||||
|
||||
// TESTING; remove
|
||||
assert(node == pf_kdtree_find_node(self, self->root, node->key));
|
||||
}
|
||||
}
|
||||
|
||||
cluster_count = 0;
|
||||
|
||||
// Do connected components for each node
|
||||
while (queue_count > 0) {
|
||||
node = queue[--queue_count];
|
||||
|
||||
// If this node has already been labelled, skip it
|
||||
if (node->cluster >= 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Assign a label to this cluster
|
||||
node->cluster = cluster_count++;
|
||||
|
||||
// Recursively label nodes in this cluster
|
||||
pf_kdtree_cluster_node(self, node, 0);
|
||||
}
|
||||
|
||||
free(queue);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Recursively label nodes in this cluster
|
||||
void pf_kdtree_cluster_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int depth)
|
||||
{
|
||||
int i;
|
||||
int nkey[3];
|
||||
pf_kdtree_node_t * nnode;
|
||||
|
||||
for (i = 0; i < 3 * 3 * 3; i++) {
|
||||
nkey[0] = node->key[0] + (i / 9) - 1;
|
||||
nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
|
||||
nkey[2] = node->key[2] + ((i % 9) % 3) - 1;
|
||||
|
||||
nnode = pf_kdtree_find_node(self, self->root, nkey);
|
||||
if (nnode == NULL) {
|
||||
continue;
|
||||
}
|
||||
|
||||
assert(nnode->leaf);
|
||||
|
||||
// This node already has a label; skip it. The label should be
|
||||
// consistent, however.
|
||||
if (nnode->cluster >= 0) {
|
||||
assert(nnode->cluster == node->cluster);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Label this node and recurse
|
||||
nnode->cluster = node->cluster;
|
||||
|
||||
pf_kdtree_cluster_node(self, nnode, depth + 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#ifdef INCLUDE_RTKGUI
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Draw the tree
|
||||
void pf_kdtree_draw(pf_kdtree_t * self, rtk_fig_t * fig)
|
||||
{
|
||||
if (self->root != NULL) {
|
||||
pf_kdtree_draw_node(self, self->root, fig);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Recursively draw nodes
|
||||
void pf_kdtree_draw_node(pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t * fig)
|
||||
{
|
||||
double ox, oy;
|
||||
char text[64];
|
||||
|
||||
if (node->leaf) {
|
||||
ox = (node->key[0] + 0.5) * self->size[0];
|
||||
oy = (node->key[1] + 0.5) * self->size[1];
|
||||
|
||||
rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0);
|
||||
|
||||
// snprintf(text, sizeof(text), "%0.3f", node->value);
|
||||
// rtk_fig_text(fig, ox, oy, 0.0, text);
|
||||
|
||||
snprintf(text, sizeof(text), "%d", node->cluster);
|
||||
rtk_fig_text(fig, ox, oy, 0.0, text);
|
||||
} else {
|
||||
assert(node->children[0] != NULL);
|
||||
assert(node->children[1] != NULL);
|
||||
pf_kdtree_draw_node(self, node->children[0], fig);
|
||||
pf_kdtree_draw_node(self, node->children[1], fig);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Useful pdf functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
|
||||
*************************************************************************/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
// #include <gsl/gsl_rng.h>
|
||||
// #include <gsl/gsl_randist.h>
|
||||
|
||||
#include "nav2_amcl/pf/pf_pdf.hpp"
|
||||
|
||||
#include "nav2_amcl/portable_utils.hpp"
|
||||
|
||||
// Random number generator seed value
|
||||
static unsigned int pf_pdf_seed;
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Gaussian
|
||||
*************************************************************************/
|
||||
|
||||
// Create a gaussian pdf
|
||||
pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
|
||||
{
|
||||
pf_matrix_t cd;
|
||||
pf_pdf_gaussian_t * pdf;
|
||||
|
||||
pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
|
||||
|
||||
pdf->x = x;
|
||||
pdf->cx = cx;
|
||||
// pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
|
||||
|
||||
// Decompose the convariance matrix into a rotation
|
||||
// matrix and a diagonal matrix.
|
||||
pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
|
||||
pdf->cd.v[0] = sqrt(cd.m[0][0]);
|
||||
pdf->cd.v[1] = sqrt(cd.m[1][1]);
|
||||
pdf->cd.v[2] = sqrt(cd.m[2][2]);
|
||||
|
||||
// Initialize the random number generator
|
||||
// pdf->rng = gsl_rng_alloc(gsl_rng_taus);
|
||||
// gsl_rng_set(pdf->rng, ++pf_pdf_seed);
|
||||
srand48(++pf_pdf_seed);
|
||||
|
||||
return pdf;
|
||||
}
|
||||
|
||||
|
||||
// Destroy the pdf
|
||||
void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf)
|
||||
{
|
||||
// gsl_rng_free(pdf->rng);
|
||||
free(pdf);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
// Compute the value of the pdf at some point [x].
|
||||
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
|
||||
{
|
||||
int i, j;
|
||||
pf_vector_t z;
|
||||
double zz, p;
|
||||
|
||||
z = pf_vector_sub(x, pdf->x);
|
||||
|
||||
zz = 0;
|
||||
for (i = 0; i < 3; i++)
|
||||
for (j = 0; j < 3; j++)
|
||||
zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
|
||||
|
||||
p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
|
||||
|
||||
return p;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
// Generate a sample from the pdf.
|
||||
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t * pdf)
|
||||
{
|
||||
int i, j;
|
||||
pf_vector_t r;
|
||||
pf_vector_t x;
|
||||
|
||||
// Generate a random vector
|
||||
for (i = 0; i < 3; i++) {
|
||||
// r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
|
||||
r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
x.v[i] = pdf->x.v[i];
|
||||
for (j = 0; j < 3; j++) {
|
||||
x.v[i] += pdf->cr.m[i][j] * r.v[j];
|
||||
}
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
// Draw randomly from a zero-mean Gaussian distribution, with standard
|
||||
// deviation sigma.
|
||||
// We use the polar form of the Box-Muller transformation, explained here:
|
||||
// http://www.taygeta.com/random/gaussian.html
|
||||
double pf_ran_gaussian(double sigma)
|
||||
{
|
||||
double x1, x2, w, r;
|
||||
|
||||
do {
|
||||
do {
|
||||
r = drand48();
|
||||
} while (r == 0.0);
|
||||
x1 = 2.0 * r - 1.0;
|
||||
do {
|
||||
r = drand48();
|
||||
} while (r == 0.0);
|
||||
x2 = 2.0 * r - 1.0;
|
||||
w = x1 * x1 + x2 * x2;
|
||||
} while (w > 1.0 || w == 0.0);
|
||||
|
||||
return sigma * x2 * sqrt(-2.0 * log(w) / w);
|
||||
}
|
||||
@@ -0,0 +1,270 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
/**************************************************************************
|
||||
* Desc: Vector functions
|
||||
* Author: Andrew Howard
|
||||
* Date: 10 Dec 2002
|
||||
* CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
|
||||
*************************************************************************/
|
||||
|
||||
#include <math.h>
|
||||
// #include <gsl/gsl_matrix.h>
|
||||
// #include <gsl/gsl_eigen.h>
|
||||
// #include <gsl/gsl_linalg.h>
|
||||
|
||||
#include "nav2_amcl/pf/pf_vector.hpp"
|
||||
#include "nav2_amcl/pf/eig3.hpp"
|
||||
|
||||
|
||||
// Return a zero vector
|
||||
pf_vector_t pf_vector_zero(void)
|
||||
{
|
||||
pf_vector_t c;
|
||||
|
||||
c.v[0] = 0.0;
|
||||
c.v[1] = 0.0;
|
||||
c.v[2] = 0.0;
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
// // Check for NAN or INF in any component
|
||||
// int pf_vector_finite(pf_vector_t a)
|
||||
// {
|
||||
// int i;
|
||||
|
||||
// for (i = 0; i < 3; i++) {
|
||||
// if (!isfinite(a.v[i])) {
|
||||
// return 0;
|
||||
// }
|
||||
// }
|
||||
|
||||
// return 1;
|
||||
// }
|
||||
|
||||
|
||||
// Print a vector
|
||||
// void pf_vector_fprintf(pf_vector_t a, FILE * file, const char * fmt)
|
||||
// {
|
||||
// int i;
|
||||
|
||||
// for (i = 0; i < 3; i++) {
|
||||
// fprintf(file, fmt, a.v[i]);
|
||||
// fprintf(file, " ");
|
||||
// }
|
||||
// fprintf(file, "\n");
|
||||
// }
|
||||
|
||||
|
||||
// // Simple vector addition
|
||||
// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
|
||||
// {
|
||||
// pf_vector_t c;
|
||||
|
||||
// c.v[0] = a.v[0] + b.v[0];
|
||||
// c.v[1] = a.v[1] + b.v[1];
|
||||
// c.v[2] = a.v[2] + b.v[2];
|
||||
|
||||
// return c;
|
||||
// }
|
||||
|
||||
|
||||
// Simple vector subtraction
|
||||
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
|
||||
{
|
||||
pf_vector_t c;
|
||||
|
||||
c.v[0] = a.v[0] - b.v[0];
|
||||
c.v[1] = a.v[1] - b.v[1];
|
||||
c.v[2] = a.v[2] - b.v[2];
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
// Transform from local to global coords (a + b)
|
||||
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
|
||||
{
|
||||
pf_vector_t c;
|
||||
|
||||
c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
|
||||
c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
|
||||
c.v[2] = b.v[2] + a.v[2];
|
||||
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
// // Transform from global to local coords (a - b)
|
||||
// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
|
||||
// {
|
||||
// pf_vector_t c;
|
||||
|
||||
// c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
|
||||
// c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
|
||||
// c.v[2] = a.v[2] - b.v[2];
|
||||
// c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
|
||||
|
||||
// return c;
|
||||
// }
|
||||
|
||||
|
||||
// Return a zero matrix
|
||||
pf_matrix_t pf_matrix_zero(void)
|
||||
{
|
||||
int i, j;
|
||||
pf_matrix_t c;
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < 3; j++) {
|
||||
c.m[i][j] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
// // Check for NAN or INF in any component
|
||||
// int pf_matrix_finite(pf_matrix_t a)
|
||||
// {
|
||||
// int i, j;
|
||||
|
||||
// for (i = 0; i < 3; i++) {
|
||||
// for (j = 0; j < 3; j++) {
|
||||
// if (!isfinite(a.m[i][j])) {
|
||||
// return 0;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// return 1;
|
||||
// }
|
||||
|
||||
|
||||
// Print a matrix
|
||||
// void pf_matrix_fprintf(pf_matrix_t a, FILE * file, const char * fmt)
|
||||
// {
|
||||
// int i, j;
|
||||
|
||||
// for (i = 0; i < 3; i++) {
|
||||
// for (j = 0; j < 3; j++) {
|
||||
// fprintf(file, fmt, a.m[i][j]);
|
||||
// fprintf(file, " ");
|
||||
// }
|
||||
// fprintf(file, "\n");
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
/*
|
||||
// Compute the matrix inverse
|
||||
pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
|
||||
{
|
||||
double lndet;
|
||||
int signum;
|
||||
gsl_permutation *p;
|
||||
gsl_matrix_view A, Ai;
|
||||
|
||||
pf_matrix_t ai;
|
||||
|
||||
A = gsl_matrix_view_array((double*) a.m, 3, 3);
|
||||
Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
|
||||
|
||||
// Do LU decomposition
|
||||
p = gsl_permutation_alloc(3);
|
||||
gsl_linalg_LU_decomp(&A.matrix, p, &signum);
|
||||
|
||||
// Check for underflow
|
||||
lndet = gsl_linalg_LU_lndet(&A.matrix);
|
||||
if (lndet < -1000)
|
||||
{
|
||||
//printf("underflow in matrix inverse lndet = %f", lndet);
|
||||
gsl_matrix_set_zero(&Ai.matrix);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Compute inverse
|
||||
gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
|
||||
}
|
||||
|
||||
gsl_permutation_free(p);
|
||||
|
||||
if (det)
|
||||
*det = exp(lndet);
|
||||
|
||||
return ai;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
|
||||
// matrix [d] such that a = r d r^T.
|
||||
void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a)
|
||||
{
|
||||
int i, j;
|
||||
/*
|
||||
gsl_matrix *aa;
|
||||
gsl_vector *eval;
|
||||
gsl_matrix *evec;
|
||||
gsl_eigen_symmv_workspace *w;
|
||||
|
||||
aa = gsl_matrix_alloc(3, 3);
|
||||
eval = gsl_vector_alloc(3);
|
||||
evec = gsl_matrix_alloc(3, 3);
|
||||
*/
|
||||
|
||||
double aa[3][3];
|
||||
double eval[3];
|
||||
double evec[3][3];
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < 3; j++) {
|
||||
// gsl_matrix_set(aa, i, j, a.m[i][j]);
|
||||
aa[i][j] = a.m[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// Compute eigenvectors/values
|
||||
/*
|
||||
w = gsl_eigen_symmv_alloc(3);
|
||||
gsl_eigen_symmv(aa, eval, evec, w);
|
||||
gsl_eigen_symmv_free(w);
|
||||
*/
|
||||
|
||||
eigen_decomposition(aa, evec, eval);
|
||||
|
||||
*d = pf_matrix_zero();
|
||||
for (i = 0; i < 3; i++) {
|
||||
// d->m[i][i] = gsl_vector_get(eval, i);
|
||||
d->m[i][i] = eval[i];
|
||||
for (j = 0; j < 3; j++) {
|
||||
// r->m[i][j] = gsl_matrix_get(evec, i, j);
|
||||
r->m[i][j] = evec[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// gsl_matrix_free(evec);
|
||||
// gsl_vector_free(eval);
|
||||
// gsl_matrix_free(aa);
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
add_library(sensors_lib SHARED
|
||||
laser/laser.cpp
|
||||
laser/beam_model.cpp
|
||||
laser/likelihood_field_model.cpp
|
||||
laser/likelihood_field_model_prob.cpp
|
||||
)
|
||||
# map_update_cspace
|
||||
target_link_libraries(sensors_lib pf_lib map_lib)
|
||||
|
||||
install(TARGETS
|
||||
sensors_lib
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
@@ -0,0 +1,136 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "nav2_amcl/sensors/laser/laser.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
BeamModel::BeamModel(
|
||||
double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
|
||||
double lambda_short, double chi_outlier, size_t max_beams, map_t * map)
|
||||
: Laser(max_beams, map)
|
||||
{
|
||||
z_hit_ = z_hit;
|
||||
z_rand_ = z_rand;
|
||||
sigma_hit_ = sigma_hit;
|
||||
z_short_ = z_short;
|
||||
z_max_ = z_max;
|
||||
lambda_short_ = lambda_short;
|
||||
chi_outlier_ = chi_outlier;
|
||||
}
|
||||
|
||||
// Determine the probability for the given pose
|
||||
double
|
||||
BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
|
||||
{
|
||||
BeamModel * self;
|
||||
int i, j, step;
|
||||
double z, pz;
|
||||
double p;
|
||||
double map_range;
|
||||
double obs_range, obs_bearing;
|
||||
double total_weight;
|
||||
pf_sample_t * sample;
|
||||
pf_vector_t pose;
|
||||
|
||||
self = reinterpret_cast<BeamModel *>(data->laser);
|
||||
|
||||
total_weight = 0.0;
|
||||
|
||||
// Compute the sample weights
|
||||
for (j = 0; j < set->sample_count; j++) {
|
||||
sample = set->samples + j;
|
||||
pose = sample->pose;
|
||||
|
||||
// Take account of the laser pose relative to the robot
|
||||
pose = pf_vector_coord_add(self->laser_pose_, pose);
|
||||
|
||||
p = 1.0;
|
||||
|
||||
step = (data->range_count - 1) / (self->max_beams_ - 1);
|
||||
for (i = 0; i < data->range_count; i += step) {
|
||||
obs_range = data->ranges[i][0];
|
||||
|
||||
// Check for NaN
|
||||
if (isnan(obs_range)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
obs_bearing = data->ranges[i][1];
|
||||
|
||||
// Compute the range according to the map
|
||||
map_range = map_calc_range(
|
||||
self->map_, pose.v[0], pose.v[1],
|
||||
pose.v[2] + obs_bearing, data->range_max);
|
||||
pz = 0.0;
|
||||
|
||||
// Part 1: good, but noisy, hit
|
||||
z = obs_range - map_range;
|
||||
pz += self->z_hit_ * exp(-(z * z) / (2 * self->sigma_hit_ * self->sigma_hit_));
|
||||
|
||||
// Part 2: short reading from unexpected obstacle (e.g., a person)
|
||||
if (z < 0) {
|
||||
pz += self->z_short_ * self->lambda_short_ * exp(-self->lambda_short_ * obs_range);
|
||||
}
|
||||
|
||||
// Part 3: Failure to detect obstacle, reported as max-range
|
||||
if (obs_range == data->range_max) {
|
||||
pz += self->z_max_ * 1.0;
|
||||
}
|
||||
|
||||
// Part 4: Random measurements
|
||||
if (obs_range < data->range_max) {
|
||||
pz += self->z_rand_ * 1.0 / data->range_max;
|
||||
}
|
||||
|
||||
// TODO(?): outlier rejection for short readings
|
||||
|
||||
assert(pz <= 1.0);
|
||||
assert(pz >= 0.0);
|
||||
// p *= pz;
|
||||
// here we have an ad-hoc weighting scheme for combining beam probs
|
||||
// works well, though...
|
||||
p += pz * pz * pz;
|
||||
}
|
||||
|
||||
sample->weight *= p;
|
||||
total_weight += sample->weight;
|
||||
}
|
||||
|
||||
return total_weight;
|
||||
}
|
||||
|
||||
bool
|
||||
BeamModel::sensorUpdate(pf_t * pf, LaserData * data)
|
||||
{
|
||||
if (max_beams_ < 2) {
|
||||
return false;
|
||||
}
|
||||
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "nav2_amcl/sensors/laser/laser.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
Laser::Laser(size_t max_beams, map_t * map)
|
||||
: max_samples_(0), max_obs_(0), temp_obs_(NULL)
|
||||
{
|
||||
max_beams_ = max_beams;
|
||||
map_ = map;
|
||||
}
|
||||
|
||||
Laser::~Laser()
|
||||
{
|
||||
if (temp_obs_) {
|
||||
for (int k = 0; k < max_samples_; k++) {
|
||||
delete[] temp_obs_[k];
|
||||
}
|
||||
delete[] temp_obs_;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Laser::reallocTempData(int new_max_samples, int new_max_obs)
|
||||
{
|
||||
if (temp_obs_) {
|
||||
for (int k = 0; k < max_samples_; k++) {
|
||||
delete[] temp_obs_[k];
|
||||
}
|
||||
delete[] temp_obs_;
|
||||
}
|
||||
max_obs_ = new_max_obs;
|
||||
max_samples_ = fmax(max_samples_, new_max_samples);
|
||||
|
||||
temp_obs_ = new double *[max_samples_]();
|
||||
for (int k = 0; k < max_samples_; k++) {
|
||||
temp_obs_[k] = new double[max_obs_]();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Laser::SetLaserPose(pf_vector_t & laser_pose)
|
||||
{
|
||||
laser_pose_ = laser_pose;
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "nav2_amcl/sensors/laser/laser.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
LikelihoodFieldModel::LikelihoodFieldModel(
|
||||
double z_hit, double z_rand, double sigma_hit,
|
||||
double max_occ_dist, size_t max_beams, map_t * map)
|
||||
: Laser(max_beams, map)
|
||||
{
|
||||
z_hit_ = z_hit;
|
||||
z_rand_ = z_rand;
|
||||
sigma_hit_ = sigma_hit;
|
||||
map_update_cspace(map, max_occ_dist);
|
||||
}
|
||||
|
||||
double
|
||||
LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
|
||||
{
|
||||
LikelihoodFieldModel * self;
|
||||
int i, j, step;
|
||||
double z, pz;
|
||||
double p;
|
||||
double obs_range, obs_bearing;
|
||||
double total_weight;
|
||||
pf_sample_t * sample;
|
||||
pf_vector_t pose;
|
||||
pf_vector_t hit;
|
||||
|
||||
self = reinterpret_cast<LikelihoodFieldModel *>(data->laser);
|
||||
|
||||
// Pre-compute a couple of things
|
||||
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
|
||||
double z_rand_mult = 1.0 / data->range_max;
|
||||
|
||||
step = (data->range_count - 1) / (self->max_beams_ - 1);
|
||||
|
||||
// Step size must be at least 1
|
||||
if (step < 1) {
|
||||
step = 1;
|
||||
}
|
||||
|
||||
total_weight = 0.0;
|
||||
|
||||
// Compute the sample weights
|
||||
for (j = 0; j < set->sample_count; j++) {
|
||||
sample = set->samples + j;
|
||||
pose = sample->pose;
|
||||
|
||||
// Take account of the laser pose relative to the robot
|
||||
pose = pf_vector_coord_add(self->laser_pose_, pose);
|
||||
|
||||
p = 1.0;
|
||||
|
||||
for (i = 0; i < data->range_count; i += step) {
|
||||
obs_range = data->ranges[i][0];
|
||||
obs_bearing = data->ranges[i][1];
|
||||
|
||||
// This model ignores max range readings
|
||||
if (obs_range >= data->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check for NaN
|
||||
if (obs_range != obs_range) {
|
||||
continue;
|
||||
}
|
||||
|
||||
pz = 0.0;
|
||||
|
||||
// Compute the endpoint of the beam
|
||||
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
|
||||
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
|
||||
|
||||
// Convert to map grid coords.
|
||||
int mi, mj;
|
||||
mi = MAP_GXWX(self->map_, hit.v[0]);
|
||||
mj = MAP_GYWY(self->map_, hit.v[1]);
|
||||
|
||||
// Part 1: Get distance from the hit to closest obstacle.
|
||||
// Off-map penalized as max distance
|
||||
if (!MAP_VALID(self->map_, mi, mj)) {
|
||||
z = self->map_->max_occ_dist;
|
||||
} else {
|
||||
z = self->map_->cells[MAP_INDEX(self->map_, mi, mj)].occ_dist;
|
||||
}
|
||||
// Gaussian model
|
||||
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
|
||||
pz += self->z_hit_ * exp(-(z * z) / z_hit_denom);
|
||||
// Part 2: random measurements
|
||||
pz += self->z_rand_ * z_rand_mult;
|
||||
|
||||
// TODO(?): outlier rejection for short readings
|
||||
|
||||
assert(pz <= 1.0);
|
||||
assert(pz >= 0.0);
|
||||
// p *= pz;
|
||||
// here we have an ad-hoc weighting scheme for combining beam probs
|
||||
// works well, though...
|
||||
p += pz * pz * pz;
|
||||
}
|
||||
|
||||
sample->weight *= p;
|
||||
total_weight += sample->weight;
|
||||
}
|
||||
|
||||
return total_weight;
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
LikelihoodFieldModel::sensorUpdate(pf_t * pf, LaserData * data)
|
||||
{
|
||||
if (max_beams_ < 2) {
|
||||
return false;
|
||||
}
|
||||
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
@@ -0,0 +1,254 @@
|
||||
/*
|
||||
* Player - One Hell of a Robot Server
|
||||
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
||||
* gerkey@usc.edu kaspers@robotics.usc.edu
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "nav2_amcl/sensors/laser/laser.hpp"
|
||||
|
||||
namespace nav2_amcl
|
||||
{
|
||||
|
||||
LikelihoodFieldModelProb::LikelihoodFieldModelProb(
|
||||
double z_hit, double z_rand, double sigma_hit,
|
||||
double max_occ_dist, bool do_beamskip,
|
||||
double beam_skip_distance,
|
||||
double beam_skip_threshold,
|
||||
double beam_skip_error_threshold,
|
||||
size_t max_beams, map_t * map)
|
||||
: Laser(max_beams, map)
|
||||
{
|
||||
z_hit_ = z_hit;
|
||||
z_rand_ = z_rand;
|
||||
sigma_hit_ = sigma_hit;
|
||||
do_beamskip_ = do_beamskip;
|
||||
beam_skip_distance_ = beam_skip_distance;
|
||||
beam_skip_threshold_ = beam_skip_threshold;
|
||||
beam_skip_error_threshold_ = beam_skip_error_threshold;
|
||||
map_update_cspace(map, max_occ_dist);
|
||||
}
|
||||
|
||||
// Determine the probability for the given pose
|
||||
double
|
||||
LikelihoodFieldModelProb::sensorFunction(LaserData * data, pf_sample_set_t * set)
|
||||
{
|
||||
LikelihoodFieldModelProb * self;
|
||||
int i, j, step;
|
||||
double z, pz;
|
||||
double log_p;
|
||||
double obs_range, obs_bearing;
|
||||
double total_weight;
|
||||
pf_sample_t * sample;
|
||||
pf_vector_t pose;
|
||||
pf_vector_t hit;
|
||||
|
||||
self = reinterpret_cast<LikelihoodFieldModelProb *>(data->laser);
|
||||
|
||||
total_weight = 0.0;
|
||||
|
||||
step = ceil((data->range_count) / static_cast<double>(self->max_beams_));
|
||||
|
||||
// Step size must be at least 1
|
||||
if (step < 1) {
|
||||
step = 1;
|
||||
}
|
||||
|
||||
// Pre-compute a couple of things
|
||||
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
|
||||
double z_rand_mult = 1.0 / data->range_max;
|
||||
|
||||
double max_dist_prob = exp(-(self->map_->max_occ_dist * self->map_->max_occ_dist) / z_hit_denom);
|
||||
|
||||
// Beam skipping - ignores beams for which a majoirty of particles do not agree with the map
|
||||
// prevents correct particles from getting down weighted because of unexpected obstacles
|
||||
// such as humans
|
||||
|
||||
bool do_beamskip = self->do_beamskip_;
|
||||
double beam_skip_distance = self->beam_skip_distance_;
|
||||
double beam_skip_threshold = self->beam_skip_threshold_;
|
||||
|
||||
// we only do beam skipping if the filter has converged
|
||||
if (do_beamskip && !set->converged) {
|
||||
do_beamskip = false;
|
||||
}
|
||||
|
||||
// we need a count the no of particles for which the beam agreed with the map
|
||||
int * obs_count = new int[self->max_beams_]();
|
||||
|
||||
// we also need a mask of which observations to integrate (to decide which beams to integrate to
|
||||
// all particles)
|
||||
bool * obs_mask = new bool[self->max_beams_]();
|
||||
|
||||
int beam_ind = 0;
|
||||
|
||||
// realloc indicates if we need to reallocate the temp data structure needed to do beamskipping
|
||||
bool realloc = false;
|
||||
|
||||
if (do_beamskip) {
|
||||
if (self->max_obs_ < self->max_beams_) {
|
||||
realloc = true;
|
||||
}
|
||||
|
||||
if (self->max_samples_ < set->sample_count) {
|
||||
realloc = true;
|
||||
}
|
||||
|
||||
if (realloc) {
|
||||
self->reallocTempData(set->sample_count, self->max_beams_);
|
||||
fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples_, self->max_obs_);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the sample weights
|
||||
for (j = 0; j < set->sample_count; j++) {
|
||||
sample = set->samples + j;
|
||||
pose = sample->pose;
|
||||
|
||||
// Take account of the laser pose relative to the robot
|
||||
pose = pf_vector_coord_add(self->laser_pose_, pose);
|
||||
|
||||
log_p = 0;
|
||||
|
||||
beam_ind = 0;
|
||||
|
||||
for (i = 0; i < data->range_count; i += step, beam_ind++) {
|
||||
obs_range = data->ranges[i][0];
|
||||
obs_bearing = data->ranges[i][1];
|
||||
|
||||
// This model ignores max range readings
|
||||
if (obs_range >= data->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check for NaN
|
||||
if (obs_range != obs_range) {
|
||||
continue;
|
||||
}
|
||||
|
||||
pz = 0.0;
|
||||
|
||||
// Compute the endpoint of the beam
|
||||
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
|
||||
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
|
||||
|
||||
// Convert to map grid coords.
|
||||
int mi, mj;
|
||||
mi = MAP_GXWX(self->map_, hit.v[0]);
|
||||
mj = MAP_GYWY(self->map_, hit.v[1]);
|
||||
|
||||
// Part 1: Get distance from the hit to closest obstacle.
|
||||
// Off-map penalized as max distance
|
||||
|
||||
if (!MAP_VALID(self->map_, mi, mj)) {
|
||||
pz += self->z_hit_ * max_dist_prob;
|
||||
} else {
|
||||
z = self->map_->cells[MAP_INDEX(self->map_, mi, mj)].occ_dist;
|
||||
if (z < beam_skip_distance) {
|
||||
obs_count[beam_ind] += 1;
|
||||
}
|
||||
pz += self->z_hit_ * exp(-(z * z) / z_hit_denom);
|
||||
}
|
||||
|
||||
// Gaussian model
|
||||
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
|
||||
|
||||
// Part 2: random measurements
|
||||
pz += self->z_rand_ * z_rand_mult;
|
||||
|
||||
assert(pz <= 1.0);
|
||||
assert(pz >= 0.0);
|
||||
|
||||
// TODO(?): outlier rejection for short readings
|
||||
|
||||
if (!do_beamskip) {
|
||||
log_p += log(pz);
|
||||
} else {
|
||||
self->temp_obs_[j][beam_ind] = pz;
|
||||
}
|
||||
}
|
||||
if (!do_beamskip) {
|
||||
sample->weight *= exp(log_p);
|
||||
total_weight += sample->weight;
|
||||
}
|
||||
}
|
||||
|
||||
if (do_beamskip) {
|
||||
int skipped_beam_count = 0;
|
||||
for (beam_ind = 0; beam_ind < self->max_beams_; beam_ind++) {
|
||||
if ((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold) {
|
||||
obs_mask[beam_ind] = true;
|
||||
} else {
|
||||
obs_mask[beam_ind] = false;
|
||||
skipped_beam_count++;
|
||||
}
|
||||
}
|
||||
|
||||
// we check if there is at least a critical number of beams that agreed with the map
|
||||
// otherwise it probably indicates that the filter converged to a wrong solution
|
||||
// if that's the case we integrate all the beams and hope the filter might converge to
|
||||
// the right solution
|
||||
bool error = false;
|
||||
|
||||
if (skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold_)) {
|
||||
fprintf(
|
||||
stderr,
|
||||
"Over %f%% of the observations were not in the map - pf may have converged to wrong pose -"
|
||||
" integrating all observations\n",
|
||||
(100 * self->beam_skip_error_threshold_));
|
||||
error = true;
|
||||
}
|
||||
|
||||
for (j = 0; j < set->sample_count; j++) {
|
||||
sample = set->samples + j;
|
||||
pose = sample->pose;
|
||||
|
||||
log_p = 0;
|
||||
|
||||
for (beam_ind = 0; beam_ind < self->max_beams_; beam_ind++) {
|
||||
if (error || obs_mask[beam_ind]) {
|
||||
log_p += log(self->temp_obs_[j][beam_ind]);
|
||||
}
|
||||
}
|
||||
|
||||
sample->weight *= exp(log_p);
|
||||
|
||||
total_weight += sample->weight;
|
||||
}
|
||||
}
|
||||
|
||||
delete[] obs_count;
|
||||
delete[] obs_mask;
|
||||
return total_weight;
|
||||
}
|
||||
|
||||
bool
|
||||
LikelihoodFieldModelProb::sensorUpdate(pf_t * pf, LaserData * data)
|
||||
{
|
||||
if (max_beams_ < 2) {
|
||||
return false;
|
||||
}
|
||||
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace nav2_amcl
|
||||
@@ -0,0 +1,240 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_behavior_tree CXX)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(library_name ${PROJECT_NAME})
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_lifecycle
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
behaviortree_cpp_v3
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_geometry_msgs
|
||||
std_msgs
|
||||
std_srvs
|
||||
nav2_util
|
||||
)
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/behavior_tree_engine.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
add_library(nav2_compute_path_to_pose_action_bt_node SHARED plugins/action/compute_path_to_pose_action.cpp)
|
||||
list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node)
|
||||
|
||||
add_library(nav2_compute_path_through_poses_action_bt_node SHARED plugins/action/compute_path_through_poses_action.cpp)
|
||||
list(APPEND plugin_libs nav2_compute_path_through_poses_action_bt_node)
|
||||
|
||||
add_library(nav2_controller_cancel_bt_node SHARED plugins/action/controller_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_controller_cancel_bt_node)
|
||||
|
||||
add_library(nav2_wait_cancel_bt_node SHARED plugins/action/wait_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_wait_cancel_bt_node)
|
||||
|
||||
add_library(nav2_spin_cancel_bt_node SHARED plugins/action/spin_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_spin_cancel_bt_node)
|
||||
|
||||
add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_back_up_cancel_bt_node)
|
||||
|
||||
add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node)
|
||||
|
||||
add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node)
|
||||
|
||||
add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_smooth_path_action_bt_node)
|
||||
|
||||
add_library(nav2_follow_path_action_bt_node SHARED plugins/action/follow_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_follow_path_action_bt_node)
|
||||
|
||||
add_library(nav2_back_up_action_bt_node SHARED plugins/action/back_up_action.cpp)
|
||||
list(APPEND plugin_libs nav2_back_up_action_bt_node)
|
||||
|
||||
add_library(nav2_drive_on_heading_bt_node SHARED plugins/action/drive_on_heading_action.cpp)
|
||||
list(APPEND plugin_libs nav2_drive_on_heading_bt_node)
|
||||
|
||||
add_library(nav2_spin_action_bt_node SHARED plugins/action/spin_action.cpp)
|
||||
list(APPEND plugin_libs nav2_spin_action_bt_node)
|
||||
|
||||
add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp)
|
||||
list(APPEND plugin_libs nav2_wait_action_bt_node)
|
||||
|
||||
add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp)
|
||||
list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node)
|
||||
|
||||
add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp)
|
||||
list(APPEND plugin_libs nav2_clear_costmap_service_bt_node)
|
||||
|
||||
add_library(nav2_is_stuck_condition_bt_node SHARED plugins/condition/is_stuck_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_stuck_condition_bt_node)
|
||||
|
||||
add_library(nav2_transform_available_condition_bt_node SHARED plugins/condition/transform_available_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_transform_available_condition_bt_node)
|
||||
|
||||
add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
|
||||
|
||||
add_library(nav2_globally_updated_goal_condition_bt_node SHARED plugins/condition/globally_updated_goal_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_globally_updated_goal_condition_bt_node)
|
||||
|
||||
add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
|
||||
|
||||
add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
|
||||
|
||||
add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
|
||||
|
||||
add_library(nav2_path_expiring_timer_condition SHARED plugins/condition/path_expiring_timer_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_path_expiring_timer_condition)
|
||||
|
||||
add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
|
||||
|
||||
add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
|
||||
|
||||
add_library(nav2_is_battery_charging_condition_bt_node SHARED plugins/condition/is_battery_charging_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_battery_charging_condition_bt_node)
|
||||
|
||||
add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
|
||||
|
||||
add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
|
||||
list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
|
||||
|
||||
add_library(nav2_rate_controller_bt_node SHARED plugins/decorator/rate_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_rate_controller_bt_node)
|
||||
|
||||
add_library(nav2_distance_controller_bt_node SHARED plugins/decorator/distance_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_distance_controller_bt_node)
|
||||
|
||||
add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_speed_controller_bt_node)
|
||||
|
||||
add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_truncate_path_action_bt_node)
|
||||
|
||||
add_library(nav2_truncate_path_local_action_bt_node SHARED plugins/action/truncate_path_local_action.cpp)
|
||||
list(APPEND plugin_libs nav2_truncate_path_local_action_bt_node)
|
||||
|
||||
add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_updater_node_bt_node)
|
||||
|
||||
add_library(nav2_path_longer_on_approach_bt_node SHARED plugins/decorator/path_longer_on_approach.cpp)
|
||||
list(APPEND plugin_libs nav2_path_longer_on_approach_bt_node)
|
||||
|
||||
add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
|
||||
list(APPEND plugin_libs nav2_recovery_node_bt_node)
|
||||
|
||||
add_library(nav2_navigate_to_pose_action_bt_node SHARED plugins/action/navigate_to_pose_action.cpp)
|
||||
list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node)
|
||||
|
||||
add_library(nav2_navigate_through_poses_action_bt_node SHARED plugins/action/navigate_through_poses_action.cpp)
|
||||
list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
|
||||
|
||||
add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
|
||||
list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
|
||||
|
||||
add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
|
||||
|
||||
add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
|
||||
list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
|
||||
|
||||
add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
|
||||
list(APPEND plugin_libs nav2_round_robin_node_bt_node)
|
||||
|
||||
add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp)
|
||||
list(APPEND plugin_libs nav2_single_trigger_bt_node)
|
||||
|
||||
add_library(nav2_planner_selector_bt_node SHARED plugins/action/planner_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_planner_selector_bt_node)
|
||||
|
||||
add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_controller_selector_bt_node)
|
||||
|
||||
add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_smoother_selector_bt_node)
|
||||
|
||||
add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)
|
||||
|
||||
add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
|
||||
|
||||
add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
|
||||
|
||||
foreach(bt_plugin ${plugin_libs})
|
||||
ament_target_dependencies(${bt_plugin} ${dependencies})
|
||||
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
|
||||
endforeach()
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
${plugin_libs}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
ament_export_libraries(
|
||||
${library_name}
|
||||
${plugin_libs}
|
||||
)
|
||||
|
||||
ament_export_dependencies(${dependencies})
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,66 @@
|
||||
# nav2_behavior_tree
|
||||
|
||||
This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees for either navigation or autonomy systems. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing.
|
||||
|
||||
The nav2_behavior_tree module provides:
|
||||
* A C++ template class for easily integrating ROS2 actions and services into Behavior Trees,
|
||||
* Navigation-specific behavior tree nodes, and
|
||||
* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications.
|
||||
|
||||
See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin.
|
||||
|
||||
See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.
|
||||
|
||||
## The bt_action_node Template and the Behavior Tree Engine
|
||||
|
||||
The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) allows one to easily integrate a ROS2 action into a BehaviorTree. To do so, one derives from the BtActionNode template, providing the action message type. For example,
|
||||
|
||||
```C++
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
|
||||
{
|
||||
...
|
||||
};
|
||||
```
|
||||
|
||||
The resulting node must be registered with the factory in the Behavior Tree engine in order to be available for use in Behavior Trees executed by this engine.
|
||||
|
||||
```C++
|
||||
BehaviorTreeEngine::BehaviorTreeEngine()
|
||||
{
|
||||
...
|
||||
|
||||
factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath");
|
||||
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the FollowPath node in use:
|
||||
|
||||
```XML
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence name="root">
|
||||
<ComputePathToPose goal="${goal}"/>
|
||||
<FollowPath path="${path}" controller_property="FollowPath"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
```
|
||||
The BehaviorTree engine has a run method that accepts an XML description of a BT for execution:
|
||||
|
||||
```C++
|
||||
BtStatus run(
|
||||
BT::Blackboard::Ptr & blackboard,
|
||||
const std::string & behavior_tree_xml,
|
||||
std::function<void()> onLoop,
|
||||
std::function<bool()> cancelRequested,
|
||||
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
|
||||
```
|
||||
|
||||
See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
|
||||
|
||||
For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics
|
||||
Binary file not shown.
@@ -0,0 +1,167 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
|
||||
<svg version="1.2" width="139.7mm" height="139.7mm" viewBox="0 0 13970 13970" preserveAspectRatio="xMidYMid" fill-rule="evenodd" stroke-width="28.222" stroke-linejoin="round" xmlns="http://www.w3.org/2000/svg" xmlns:ooo="http://xml.openoffice.org/svg/export" xmlns:xlink="http://www.w3.org/1999/xlink" xmlns:presentation="http://sun.com/xmlns/staroffice/presentation" xmlns:smil="http://www.w3.org/2001/SMIL20/" xmlns:anim="urn:oasis:names:tc:opendocument:xmlns:animation:1.0" xml:space="preserve">
|
||||
<defs class="ClipPathGroup">
|
||||
<clipPath id="presentation_clip_path" clipPathUnits="userSpaceOnUse">
|
||||
<rect x="0" y="0" width="13970" height="13970"/>
|
||||
</clipPath>
|
||||
</defs>
|
||||
<defs>
|
||||
<font id="EmbeddedFont_1" horiz-adv-x="2048">
|
||||
<font-face font-family="Verdana embedded" units-per-em="2048" font-weight="bold" font-style="normal" ascent="2037" descent="423"/>
|
||||
<missing-glyph horiz-adv-x="2048" d="M 0,0 L 2047,0 2047,2047 0,2047 0,0 Z"/>
|
||||
<glyph unicode="x" horiz-adv-x="1350" d="M 1344,0 L 924,0 684,336 438,0 26,0 469,561 33,1123 453,1123 689,793 926,1123 1339,1123 902,570 1344,0 Z"/>
|
||||
<glyph unicode="w" horiz-adv-x="1933" d="M 1961,1123 L 1613,0 1231,0 1002,757 777,0 390,0 45,1123 423,1123 615,349 855,1123 1174,1123 1402,349 1591,1123 1961,1123 Z"/>
|
||||
<glyph unicode="v" horiz-adv-x="1271" d="M 1301,1123 L 868,0 460,0 30,1123 411,1123 671,351 928,1123 1301,1123 Z"/>
|
||||
<glyph unicode="u" horiz-adv-x="1138" d="M 1289,0 L 929,0 929,124 C 862,73 801,35 746,9 691,-18 625,-31 548,-31 424,-31 329,5 262,76 195,147 161,253 161,392 L 161,1123 523,1123 523,566 C 523,509 525,462 529,425 532,387 541,356 554,331 567,306 586,288 612,277 637,266 673,260 719,260 750,260 784,266 821,277 858,288 894,305 929,327 L 929,1123 1289,1123 1289,0 Z"/>
|
||||
<glyph unicode="t" horiz-adv-x="875" d="M 888,10 C 849,0 808,-8 765,-14 722,-19 669,-22 606,-22 466,-22 362,6 294,63 225,120 191,217 191,354 L 191,879 43,879 43,1123 191,1123 191,1444 551,1444 551,1123 888,1123 888,879 551,879 551,481 C 551,442 551,407 552,378 553,349 558,322 568,299 577,276 594,257 618,244 641,230 676,223 721,223 740,223 764,227 795,235 825,243 846,250 858,257 L 888,257 888,10 Z"/>
|
||||
<glyph unicode="s" horiz-adv-x="1086" d="M 1146,356 C 1146,240 1094,147 990,76 886,5 744,-31 563,-31 463,-31 372,-22 291,-3 210,16 144,36 95,59 L 95,355 128,355 C 146,342 167,329 191,314 214,299 248,282 291,265 328,250 371,237 418,226 465,215 516,209 570,209 641,209 693,217 727,233 761,248 778,271 778,302 778,329 768,349 748,362 728,374 690,386 634,397 607,403 571,410 526,417 480,424 438,432 401,442 299,469 223,510 174,567 125,623 100,693 100,777 100,885 151,975 254,1047 356,1118 496,1154 673,1154 757,1154 838,1146 915,1129 992,1112 1052,1093 1095,1074 L 1095,790 1064,790 C 1011,827 950,857 882,880 813,903 744,914 674,914 616,914 567,906 528,890 488,874 468,851 468,822 468,795 477,775 495,761 513,747 555,733 622,719 659,712 698,705 741,698 783,691 825,682 868,671 963,646 1033,608 1078,555 1123,502 1146,435 1146,356 Z"/>
|
||||
<glyph unicode="p" horiz-adv-x="1191" d="M 1342,577 C 1342,484 1328,400 1300,324 1271,248 1234,185 1188,136 1140,84 1086,45 1025,18 964,-9 899,-22 830,-22 766,-22 712,-15 667,-2 622,12 577,31 530,56 L 530,-412 170,-412 170,1123 530,1123 530,1006 C 585,1050 642,1086 700,1113 757,1140 824,1154 899,1154 1039,1154 1148,1103 1226,1000 1303,897 1342,756 1342,577 Z M 971,570 C 971,679 952,759 915,808 878,857 819,882 738,882 704,882 669,877 633,867 597,856 563,841 530,822 L 530,257 C 554,248 580,242 607,239 634,236 661,235 688,235 783,235 854,263 901,319 948,374 971,458 971,570 Z"/>
|
||||
<glyph unicode="o" horiz-adv-x="1245" d="M 1318,561 C 1318,376 1264,231 1157,125 1049,18 898,-35 703,-35 508,-35 357,18 250,125 142,231 88,376 88,561 88,747 142,893 251,999 359,1105 510,1158 703,1158 899,1158 1051,1105 1158,998 1265,891 1318,746 1318,561 Z M 875,288 C 898,317 916,351 928,392 939,432 945,488 945,559 945,625 939,680 927,725 915,770 898,805 877,832 856,859 830,879 800,890 770,901 738,907 703,907 668,907 638,902 611,893 584,884 558,865 533,837 511,811 494,775 481,730 468,685 461,628 461,559 461,498 467,445 478,400 489,355 506,318 528,291 549,265 575,246 605,234 634,222 668,216 706,216 739,216 770,222 800,233 829,244 854,262 875,288 Z"/>
|
||||
<glyph unicode="n" horiz-adv-x="1138" d="M 1298,0 L 936,0 936,557 C 936,602 934,648 929,693 924,738 916,771 905,792 892,817 872,835 847,846 821,857 785,863 739,863 706,863 673,858 640,847 606,836 569,819 530,796 L 530,0 170,0 170,1123 530,1123 530,999 C 594,1049 656,1087 715,1114 774,1141 839,1154 911,1154 1032,1154 1127,1119 1196,1048 1264,977 1298,872 1298,731 L 1298,0 Z"/>
|
||||
<glyph unicode="m" horiz-adv-x="1853" d="M 1268,796 L 1268,0 906,0 906,561 C 906,616 905,662 902,701 899,739 892,770 880,794 868,818 850,836 826,847 801,858 767,863 723,863 688,863 653,856 620,842 587,827 557,812 530,796 L 530,0 170,0 170,1123 530,1123 530,999 C 592,1048 651,1086 708,1113 765,1140 827,1154 896,1154 970,1154 1035,1138 1092,1105 1149,1072 1193,1023 1225,959 1297,1020 1367,1067 1435,1102 1503,1137 1570,1154 1635,1154 1756,1154 1849,1118 1912,1045 1975,972 2006,868 2006,731 L 2006,0 1644,0 1644,561 C 1644,616 1643,663 1641,701 1638,739 1631,770 1619,794 1608,818 1590,836 1565,847 1540,858 1506,863 1461,863 1431,863 1402,858 1373,848 1344,837 1309,820 1268,796 Z"/>
|
||||
<glyph unicode="l" horiz-adv-x="371" d="M 530,0 L 170,0 170,1556 530,1556 530,0 Z"/>
|
||||
<glyph unicode="i" horiz-adv-x="398" d="M 530,0 L 170,0 170,1123 530,1123 530,0 Z M 540,1283 L 160,1283 160,1556 540,1556 540,1283 Z"/>
|
||||
<glyph unicode="h" horiz-adv-x="1138" d="M 1298,0 L 936,0 936,557 C 936,602 934,648 929,693 924,738 916,771 905,792 892,817 872,835 847,846 821,857 785,863 739,863 706,863 673,858 640,847 606,836 569,819 530,796 L 530,0 170,0 170,1556 530,1556 530,999 C 594,1049 656,1087 715,1114 774,1141 839,1154 911,1154 1032,1154 1127,1119 1196,1048 1264,977 1298,872 1298,731 L 1298,0 Z"/>
|
||||
<glyph unicode="g" horiz-adv-x="1192" d="M 1262,129 C 1262,24 1247,-65 1217,-137 1187,-209 1145,-265 1091,-306 1037,-347 972,-377 896,-396 819,-414 733,-423 637,-423 559,-423 482,-418 407,-409 331,-400 265,-388 210,-375 L 210,-94 254,-94 C 298,-111 352,-127 415,-142 478,-156 535,-163 585,-163 652,-163 706,-157 748,-145 789,-132 821,-115 843,-92 864,-71 879,-43 888,-10 897,23 902,63 902,110 L 902,131 C 859,96 811,68 758,47 705,26 647,16 582,16 425,16 303,63 218,158 133,253 90,397 90,590 90,683 103,763 129,830 155,897 192,956 239,1006 283,1053 337,1089 402,1115 466,1141 532,1154 599,1154 660,1154 715,1147 765,1133 814,1118 859,1098 900,1073 L 913,1123 1262,1123 1262,129 Z M 902,354 L 902,863 C 881,872 856,879 826,884 796,889 769,891 745,891 650,891 579,864 532,810 485,755 461,679 461,582 461,474 481,399 522,356 562,313 622,292 701,292 737,292 772,298 807,309 842,320 873,335 902,354 Z"/>
|
||||
<glyph unicode="e" horiz-adv-x="1192" d="M 1276,495 L 452,495 C 457,407 491,340 553,293 614,246 705,223 825,223 901,223 975,237 1046,264 1117,291 1174,321 1215,352 L 1255,352 1255,63 C 1174,30 1097,7 1025,-8 953,-23 873,-30 786,-30 561,-30 388,21 268,122 148,223 88,368 88,555 88,740 145,887 259,996 372,1104 528,1158 726,1158 909,1158 1046,1112 1138,1020 1230,927 1276,794 1276,621 L 1276,495 Z M 918,706 C 916,781 897,838 862,876 827,914 772,933 697,933 628,933 571,915 526,879 481,843 456,785 451,706 L 918,706 Z"/>
|
||||
<glyph unicode="c" horiz-adv-x="1060" d="M 755,-30 C 657,-30 568,-18 487,5 406,28 335,64 275,113 216,162 170,223 137,297 104,371 88,458 88,557 88,662 106,752 141,828 176,904 224,967 287,1017 348,1064 418,1099 497,1121 576,1143 659,1154 744,1154 821,1154 891,1146 956,1129 1021,1112 1081,1091 1137,1064 L 1137,757 1086,757 C 1072,769 1055,783 1036,799 1016,815 992,831 963,846 936,861 906,873 873,883 840,892 802,897 759,897 663,897 589,867 538,806 486,745 460,662 460,557 460,449 487,367 540,311 593,255 668,227 765,227 810,227 851,232 888,243 924,253 954,265 978,279 1001,292 1021,306 1038,321 1055,336 1071,350 1086,364 L 1137,364 1137,57 C 1080,30 1021,9 960,-7 898,-22 830,-30 755,-30 Z"/>
|
||||
<glyph unicode="a" horiz-adv-x="1139" d="M 850,293 L 850,527 C 801,523 749,518 692,511 635,504 592,495 563,486 527,475 500,458 481,437 462,415 452,386 452,351 452,328 454,309 458,294 462,279 472,265 488,252 503,239 522,229 543,223 564,216 598,213 643,213 679,213 716,220 753,235 790,250 822,269 850,293 Z M 850,119 C 831,104 807,87 778,66 749,45 722,29 697,17 662,1 625,-11 587,-19 549,-26 507,-30 462,-30 355,-30 266,3 194,70 122,136 86,221 86,324 86,406 104,474 141,526 178,578 230,619 297,650 364,680 446,701 545,714 644,727 746,733 852,733 L 852,739 C 852,803 827,847 776,871 725,896 651,908 552,908 493,908 429,898 362,877 295,856 246,839 217,828 L 184,828 184,1099 C 222,1109 284,1121 370,1135 455,1148 541,1155 627,1155 832,1155 980,1124 1071,1061 1162,998 1207,899 1207,764 L 1207,0 850,0 850,119 Z"/>
|
||||
<glyph unicode="T" horiz-adv-x="1324" d="M 1355,1201 L 890,1201 890,0 506,0 506,1201 41,1201 41,1489 1355,1489 1355,1201 Z"/>
|
||||
<glyph unicode="P" horiz-adv-x="1245" d="M 1419,1019 C 1419,952 1407,887 1384,824 1361,760 1327,706 1284,663 1225,604 1159,560 1086,530 1013,500 922,485 813,485 L 574,485 574,0 190,0 190,1489 822,1489 C 917,1489 997,1481 1062,1465 1127,1448 1184,1424 1234,1391 1294,1352 1340,1301 1372,1240 1403,1179 1419,1105 1419,1019 Z M 1022,1011 C 1022,1053 1011,1090 988,1120 965,1151 939,1172 909,1184 869,1200 830,1208 792,1208 754,1208 703,1208 640,1208 L 574,1208 574,765 684,765 C 749,765 803,769 846,777 888,785 923,801 952,825 977,847 995,872 1006,902 1017,932 1022,968 1022,1011 Z"/>
|
||||
<glyph unicode="N" horiz-adv-x="1377" d="M 1544,0 L 1174,0 542,1022 542,0 190,0 190,1489 649,1489 1192,636 1192,1489 1544,1489 1544,0 Z"/>
|
||||
<glyph unicode="M" horiz-adv-x="1562" d="M 1751,0 L 1369,0 1369,997 1093,350 828,350 552,997 552,0 190,0 190,1489 636,1489 971,742 1305,1489 1751,1489 1751,0 Z"/>
|
||||
<glyph unicode="F" horiz-adv-x="1086" d="M 1257,1201 L 572,1201 572,924 1207,924 1207,636 572,636 572,0 190,0 190,1489 1257,1489 1257,1201 Z"/>
|
||||
<glyph unicode="E" horiz-adv-x="1086" d="M 1267,0 L 190,0 190,1489 1267,1489 1267,1201 572,1201 572,944 1217,944 1217,656 572,656 572,288 1267,288 1267,0 Z"/>
|
||||
<glyph unicode="C" horiz-adv-x="1324" d="M 863,-29 C 752,-29 650,-13 557,20 463,53 382,101 315,166 248,231 196,311 159,408 122,505 103,616 103,743 103,861 121,968 156,1064 191,1160 243,1242 310,1311 375,1377 455,1428 551,1464 646,1500 751,1518 864,1518 927,1518 983,1515 1034,1508 1084,1501 1130,1491 1173,1480 1218,1467 1258,1453 1295,1438 1331,1422 1363,1407 1390,1394 L 1390,1033 1346,1033 C 1327,1049 1304,1068 1276,1090 1247,1112 1215,1134 1179,1155 1142,1176 1103,1194 1060,1209 1017,1224 972,1231 923,1231 869,1231 818,1223 769,1206 720,1189 675,1160 634,1121 595,1083 563,1033 539,970 514,907 502,831 502,742 502,649 515,571 542,508 568,445 601,396 641,360 682,323 727,297 777,282 827,266 876,258 925,258 972,258 1018,265 1064,279 1109,293 1151,312 1190,336 1223,355 1253,376 1281,398 1309,420 1332,439 1350,455 L 1390,455 1390,99 C 1353,82 1317,67 1283,52 1249,37 1213,25 1176,14 1127,0 1082,-11 1039,-18 996,-25 938,-29 863,-29 Z"/>
|
||||
</font>
|
||||
</defs>
|
||||
<defs class="TextShapeIndex">
|
||||
<g ooo:slide="id1" ooo:id-list="id3 id4 id5 id6 id7 id8 id9"/>
|
||||
</defs>
|
||||
<defs class="EmbeddedBulletChars">
|
||||
<g id="bullet-char-template(57356)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 580,1141 L 1163,571 580,0 -4,571 580,1141 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(57354)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 8,1128 L 1137,1128 1137,0 8,0 8,1128 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10146)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 174,0 L 602,739 174,1481 1456,739 174,0 Z M 1358,739 L 309,1346 659,739 1358,739 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10132)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 2015,739 L 1276,0 717,0 1260,543 174,543 174,936 1260,936 717,1481 1274,1481 2015,739 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10007)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 0,-2 C -7,14 -16,27 -25,37 L 356,567 C 262,823 215,952 215,954 215,979 228,992 255,992 264,992 276,990 289,987 310,991 331,999 354,1012 L 381,999 492,748 772,1049 836,1024 860,1049 C 881,1039 901,1025 922,1006 886,937 835,863 770,784 769,783 710,716 594,584 L 774,223 C 774,196 753,168 711,139 L 727,119 C 717,90 699,76 672,76 641,76 570,178 457,381 L 164,-76 C 142,-110 111,-127 72,-127 30,-127 9,-110 8,-76 1,-67 -2,-52 -2,-32 -2,-23 -1,-13 0,-2 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10004)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 285,-33 C 182,-33 111,30 74,156 52,228 41,333 41,471 41,549 55,616 82,672 116,743 169,778 240,778 293,778 328,747 346,684 L 369,508 C 377,444 397,411 428,410 L 1163,1116 C 1174,1127 1196,1133 1229,1133 1271,1133 1292,1118 1292,1087 L 1292,965 C 1292,929 1282,901 1262,881 L 442,47 C 390,-6 338,-33 285,-33 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(9679)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 813,0 C 632,0 489,54 383,161 276,268 223,411 223,592 223,773 276,916 383,1023 489,1130 632,1184 813,1184 992,1184 1136,1130 1245,1023 1353,916 1407,772 1407,592 1407,412 1353,268 1245,161 1136,54 992,0 813,0 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(8226)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 346,457 C 273,457 209,483 155,535 101,586 74,649 74,723 74,796 101,859 155,911 209,963 273,989 346,989 419,989 480,963 531,910 582,859 608,796 608,723 608,648 583,586 532,535 482,483 420,457 346,457 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(8211)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M -4,459 L 1135,459 1135,606 -4,606 -4,459 Z"/>
|
||||
</g>
|
||||
</defs>
|
||||
<defs class="TextEmbeddedBitmaps"/>
|
||||
<g>
|
||||
<g id="id2" class="Master_Slide">
|
||||
<g id="bg-id2" class="Background"/>
|
||||
<g id="bo-id2" class="BackgroundObjects"/>
|
||||
</g>
|
||||
</g>
|
||||
<g class="SlideGroup">
|
||||
<g>
|
||||
<g id="id1" class="Slide" clip-path="url(#presentation_clip_path)">
|
||||
<g class="Page">
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id3">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="4528" y="1533" width="4776" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 5089,1734 C 4909,1734 4729,1914 4729,2094 L 4729,3534 C 4729,3714 4909,3894 5089,3894 L 8942,3894 C 9122,3894 9302,3714 9302,3534 L 9302,2094 C 9302,1914 9122,1734 8942,1734 L 5089,1734 Z M 4729,1734 L 4729,1734 Z M 9302,3894 L 9302,3894 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 5089,1734 C 4909,1734 4729,1914 4729,2094 L 4729,3534 C 4729,3714 4909,3894 5089,3894 L 8942,3894 C 9122,3894 9302,3714 9302,3534 L 9302,2094 C 9302,1914 9122,1734 8942,1734 L 5089,1734 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 4729,1734 L 4729,1734 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 9302,3894 L 9302,3894 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="5186" y="2983"><tspan fill="rgb(128,128,128)" stroke="none">ExecuteMission</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 4889,1534 C 4709,1534 4529,1714 4529,1894 L 4529,3334 C 4529,3514 4709,3694 4889,3694 L 8742,3694 C 8922,3694 9102,3514 9102,3334 L 9102,1894 C 9102,1714 8922,1534 8742,1534 L 4889,1534 Z M 4529,1534 L 4529,1534 Z M 9102,3694 L 9102,3694 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 4889,1534 C 4709,1534 4529,1714 4529,1894 L 4529,3334 C 4529,3514 4709,3694 4889,3694 L 8742,3694 C 8922,3694 9102,3514 9102,3334 L 9102,1894 C 9102,1714 8922,1534 8742,1534 L 4889,1534 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 4529,1534 L 4529,1534 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 9102,3694 L 9102,3694 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="4986" y="2783"><tspan fill="rgb(255,255,255)" stroke="none">ExecuteMission</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id4">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="4655" y="5470" width="4776" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 5216,5671 C 5036,5671 4856,5851 4856,6031 L 4856,7471 C 4856,7651 5036,7831 5216,7831 L 9069,7831 C 9249,7831 9429,7651 9429,7471 L 9429,6031 C 9429,5851 9249,5671 9069,5671 L 5216,5671 Z M 4856,5671 L 4856,5671 Z M 9429,7831 L 9429,7831 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 5216,5671 C 5036,5671 4856,5851 4856,6031 L 4856,7471 C 4856,7651 5036,7831 5216,7831 L 9069,7831 C 9249,7831 9429,7651 9429,7471 L 9429,6031 C 9429,5851 9249,5671 9069,5671 L 5216,5671 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 4856,5671 L 4856,5671 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 9429,7831 L 9429,7831 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="5229" y="6920"><tspan fill="rgb(128,128,128)" stroke="none">NavigateToPose</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 5016,5471 C 4836,5471 4656,5651 4656,5831 L 4656,7271 C 4656,7451 4836,7631 5016,7631 L 8869,7631 C 9049,7631 9229,7451 9229,7271 L 9229,5831 C 9229,5651 9049,5471 8869,5471 L 5016,5471 Z M 4656,5471 L 4656,5471 Z M 9229,7631 L 9229,7631 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 5016,5471 C 4836,5471 4656,5651 4656,5831 L 4656,7271 C 4656,7451 4836,7631 5016,7631 L 8869,7631 C 9049,7631 9229,7451 9229,7271 L 9229,5831 C 9229,5651 9049,5471 8869,5471 L 5016,5471 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 4656,5471 L 4656,5471 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 9229,7631 L 9229,7631 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="5029" y="6720"><tspan fill="rgb(255,255,255)" stroke="none">NavigateToPose</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id5">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="1352" y="9788" width="5665" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 1912,9989 C 1732,9989 1553,10169 1553,10349 L 1553,11789 C 1553,11969 1732,12149 1912,12149 L 6655,12149 C 6835,12149 7015,11969 7015,11789 L 7015,10349 C 7015,10169 6835,9989 6655,9989 L 1912,9989 Z M 1553,9989 L 1553,9989 Z M 7015,12149 L 7015,12149 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 1912,9989 C 1732,9989 1553,10169 1553,10349 L 1553,11789 C 1553,11969 1732,12149 1912,12149 L 6655,12149 C 6835,12149 7015,11969 7015,11789 L 7015,10349 C 7015,10169 6835,9989 6655,9989 L 1912,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 1553,9989 L 1553,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 7015,12149 L 7015,12149 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="1826" y="11238"><tspan fill="rgb(128,128,128)" stroke="none">ComputePathToPose</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 1712,9789 C 1532,9789 1353,9969 1353,10149 L 1353,11589 C 1353,11769 1532,11949 1712,11949 L 6455,11949 C 6635,11949 6815,11769 6815,11589 L 6815,10149 C 6815,9969 6635,9789 6455,9789 L 1712,9789 Z M 1353,9789 L 1353,9789 Z M 6815,11949 L 6815,11949 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 1712,9789 C 1532,9789 1353,9969 1353,10149 L 1353,11589 C 1353,11769 1532,11949 1712,11949 L 6455,11949 C 6635,11949 6815,11769 6815,11589 L 6815,10149 C 6815,9969 6635,9789 6455,9789 L 1712,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 1353,9789 L 1353,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 6815,11949 L 6815,11949 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="1626" y="11038"><tspan fill="rgb(255,255,255)" stroke="none">ComputePathToPose</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id6">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="7703" y="9788" width="4776" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 8264,9989 C 8084,9989 7904,10169 7904,10349 L 7904,11789 C 7904,11969 8084,12149 8264,12149 L 12117,12149 C 12297,12149 12477,11969 12477,11789 L 12477,10349 C 12477,10169 12297,9989 12117,9989 L 8264,9989 Z M 7904,9989 L 7904,9989 Z M 12477,12149 L 12477,12149 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 8264,9989 C 8084,9989 7904,10169 7904,10349 L 7904,11789 C 7904,11969 8084,12149 8264,12149 L 12117,12149 C 12297,12149 12477,11969 12477,11789 L 12477,10349 C 12477,10169 12297,9989 12117,9989 L 8264,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 7904,9989 L 7904,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 12477,12149 L 12477,12149 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8865" y="11238"><tspan fill="rgb(128,128,128)" stroke="none">FollowPath</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 8064,9789 C 7884,9789 7704,9969 7704,10149 L 7704,11589 C 7704,11769 7884,11949 8064,11949 L 11917,11949 C 12097,11949 12277,11769 12277,11589 L 12277,10149 C 12277,9969 12097,9789 11917,9789 L 8064,9789 Z M 7704,9789 L 7704,9789 Z M 12277,11949 L 12277,11949 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 8064,9789 C 7884,9789 7704,9969 7704,10149 L 7704,11589 C 7704,11769 7884,11949 8064,11949 L 11917,11949 C 12097,11949 12277,11769 12277,11589 L 12277,10149 C 12277,9969 12097,9789 11917,9789 L 8064,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 7704,9789 L 7704,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 12277,11949 L 12277,11949 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8665" y="11038"><tspan fill="rgb(255,255,255)" stroke="none">FollowPath</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.LineShape">
|
||||
<g id="id7">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="6792" y="3693" width="301" height="1780"/>
|
||||
<path fill="none" stroke="rgb(0,0,0)" d="M 6942,3694 L 6942,5042"/>
|
||||
<path fill="rgb(0,0,0)" stroke="none" d="M 6942,5472 L 7092,5022 6792,5022 6942,5472 Z"/>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.LineShape">
|
||||
<g id="id8">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="6814" y="7630" width="3050" height="2161"/>
|
||||
<path fill="none" stroke="rgb(0,0,0)" d="M 6815,7631 L 9512,9541"/>
|
||||
<path fill="rgb(0,0,0)" stroke="none" d="M 9863,9790 L 9582,9407 9409,9652 9863,9790 Z"/>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.LineShape">
|
||||
<g id="id9">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="4021" y="7630" width="2796" height="2161"/>
|
||||
<path fill="none" stroke="rgb(0,0,0)" d="M 6815,7631 L 4361,9527"/>
|
||||
<path fill="rgb(0,0,0)" stroke="none" d="M 4021,9790 L 4469,9634 4285,9396 4021,9790 Z"/>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 24 KiB |
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user