Skip to content

Commit

Permalink
0.2.4 sync - Ready for testing (#1096)
Browse files Browse the repository at this point in the history
* remove nav2_robot package

* remove CMakeList and package.xml dependence on nav2_robot

* robot_util classes for publishing and states

* changing all users of robot_ to their appropriate eq.

* changing the controller to use the nav2_utils velocity publisher wrapper as well

* adding shared locks properly to share get resources

* correct namespace nav2_robot -> nav2_util

* remove sharedptr references

* resolve merge conflicts

* Setting default to use AMCL topic to true

* fix rebase/merge conflicts

deleting robot package

* removing old maintainers from navigation1

* Switch from map service to topic (#885)

* Add parameter to disable map server

* Move parameter check to getParameters

* Uncrustify

* Don't mark static layer as current until we have a map

* Make sure static layer gets initialized

Make sure static layer gets initialized, even with an empty layer, so it doesn't crash if you disable the map service and enable the obstacle layer

* remove map service from static layer

Only use map topic instead
Subscribe to map topic with default transient local volatility
Add parameter "map_subscribe_transient_local" (default=true) for backwards compatibility with volatile publishers

* Change AMCL to use only the map topic, not map service

* remove now-unused MapServiceClient

* uncrustify

* only set current_=true when first map received

* Fix nav2_costmap inflation tests.

* Fixing collision tester test.

* Revert "uncrustify"

This reverts commit 82f88a7.
This uncrustify failure is enforced differently depending on whether
we are on the dashing branch or on the master branch. This particular
commit is only appropriate on the dashing branch, but is already present
there.

* change primitives to recoveries (#705)

* change primitives to recoveries

* changing all over new bringups with nav2_recoveries

* renaming primitive_name -> recovery_name

* stop robot on cancel in recovery

* fixed recovery node halt bug

* removing nav2_robot package

* adding lifecycle publisher for dwb

* Removing unneeded tf initialization calls

* Remove dedicated thread from costmap

* Add support for new createTimerInterface API

* Fix system test launch file.

* updated the build scripts for ROS2 Dashing (#924)

* run build from anywhere in tree

* renaming dependencies because its driving me up a wall autocomplete for nav doesnt work

* remove extra files re-introduced in PR #704

* set align critics scale to 0 (#936)

* fix typo in build tool updates

* Nav2-rviz-plugin changes. (#937)

* I added a timeout to spin_until_future_complete, so rviz doesn't get frozen if something goes wrong in the navigation stack. I did some changes in the start navigation; - changed the funtion type to boolean - added returns to the funtion, so it will return false if something goes wrong. I changed goal_tool added a if statement to check if start_navigation doesn't return false before calling any other functions in goal_tool

* made the code cleaner and changed the error messages. Reduced the timeout value to 3 seconds. The timeouts will be parametrized in the future.

* added timeout parameter

* fix linter failures in Rviz plugin (#979)

* I added a timeout to spin_until_future_complete, so rviz doesn't get frozen if something goes wrong in the navigation stack. I did some changes in the start navigation; - changed the funtion type to boolean - added returns to the funtion, so it will return false if something goes wrong. I changed goal_tool added a if statement to check if start_navigation doesn't return false before calling any other functions in goal_tool

* made the code cleaner and changed the error messages. Reduced the timeout value to 3 seconds. The timeouts will be parametrized in the future.

* added timeout parameter

* Update navigation_dialog.cpp

* Update navigation_dialog.hpp

* Update navigation_dialog.cpp

* Update navigation_dialog.hpp

* Fixing typo in dependencies (#982)

* Fixing typo in dependencies

* Also remove some duplicate dependencies

* Fixing robot_state_publisher dependency

* Add support to change the local controller rate. (#970)

* Add back parameter to configure DWA rate

* Report out if we missed our control loop time.

* Fix the dashing build on OSRF build farm. (#935)

All our builds have boost installed due to other dependencies, so
we've never run into this. The build farm must build with all other
dependencies removed somehow.

* bump for release to 0.2.2

* Fix for TEB local planner buildable (#981)

Signed-off-by: vinnamkim <vinnam.kim@gmail.com>

* adding rate loops where applicable (#743)

* adding rate loops where applicable

* throttle while loops with sleep_for

* sleep_dir -> sleep_dur

* enabling closed-loop control in spin (#983)

* implemented controlled spin

* remove files reintroduced (#991)

* collision checkers in recoveries now look sufficiently far ahead in the local coordinate frame

* removing whitespace

* revert to global: need PR for TF positioning merged before can fix that

* Creating a simple action server to be able to invoke Random Crawl from BT (#945)

added random crawl action to bt

* amcl particlecloud is best effort (#1003)

Signed-off-by: Mikael Arguedas <mikael.arguedas@gmail.com>

* Disable displaying RobotModel in Rviz by default (#1001)

* returning failed if recovery cannot be completed due to collision

* backing out collision detection into a new fuction

* removing extraneous visualization from tests that have been removed

* revert my own config

* make ye old linter happy

* Footprint subscriber const topic

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Costmap subscriber const topic

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* fixing PR feedback to stopRobot() in better location and parameterization

* removing unnecessary smart pointers that dont effect lifecycle

* lifecycle executor doesnt need to be a pointer

* removing extra PR template field in application no one uses

* Reorganize some of the BehaviorTree-related code (#957)

* Reorganize some of the BehaviorTree-related code.

Eliminate the NavigateToPoseBehaviorTree class, merging it with the BehaviorTreeEngine.
Put clear_entirely service client with the others in nav2_util.
Move RecoveryNode to nav2_behavior_tree with the other BT nodes.

* Remove method signature that isn't implemented yet

* Add a missing dependency

* Address a couple linter issues

* Tf based positioning (#993)

* remove topic based positioning for TF and removing extraneous build fields

* robot utils complete for getting positioning from TF

* readding the custom logger for getting current pose

* remove extra TODO

* fixing fun crustyness

* pass by ref not shared ptr

* wrapping costmap 2d's get pose with util

* revert costmap 2d plugin initialization swap

* fixing more uncrustified issues while I wait

* adding transform listener to buffers

* adding tf listeners and missing dependencies

* editing test cases

* trying no exception

* first stab at unit tests with TF2 for costmap 2D integration test

* adding the create timer interface

* adding updated pose info for testing

* com'n crusty code, why are you different in CI?

* fixing merge conflicts p2

* remove unnecessary CMake code to remove warning (#1005)

Signed-off-by: Mikael Arguedas <mikael.arguedas@gmail.com>

* disabling by default the debug markers for local planner (#1007)

* adding deprecation warning for topic-based goal navigation

* fixing spin recovery

* incorrect topic names

* Provide correct path to default map (#987)

* Extend the costmap's namespace with the hosting node's namespace (#999)

* Parameter dumping utility (#902)

* Add a parameter dumping utility to nav2_utils

* Add some convenience functions to simplify declaring parameters

* Add some examples for AMCL

* Remove stray example file

* Default to dumping all nodes; throw/catch exceptions

* Address some review feedback

* Fix node name collisions (#1020)

* Fix node name collisions

Make sure service clients have a unique node name (this was a problem in recoveries nodes)
Delimit ros arguments with new `--ros-args`,`--`

* Uncrustify

* Fixing linter failures introduced in master (#1029)

* Fixing linter failures introduced in master

* Sort of fix test_occ_grid_cpp test but disable since it's flaky

* Disable failing test_collision_checker test

* Recovery using odometric poses and costmaps (#1023)

* recovery behaviors using the odometric positioning for pose estimation

* use local costmap information in recoveries

* fixing costmap namespacing for collision detector

* odometry based recoveries with collision checker global frame model

* on download, rosdep all necessary items, calling on each build is redundant

* doing rosdep install in a single call

* removing extraneous cd

* adding recoveries XML to simulation launch file (#1031)

* adding recoveries XML to simulation launch file

* fix typo from name vs frame odom

* refactor goal handle into panel

clean up

remove extra log info

* remove nav2 goal tool and address review feedback

* fix lint issue

* send pose via qt signal through goal tool

* remove navigation dialog

minor clean up

* use enum for action states

fix compile issues

remove extra semicolons

fix uncrustify

* add QBasicTimer

cleanup timer

* remove more unnecessary headers

* Add parallel testing jobs for each rmw vendor

* Fix yaml

* Fix yaml

* Use custom version of nightly instead

* Extend workflow for rmw testing on release

* Fix typo

* Try speeding up build by omitting job limit

* Ensure checksum.txt exist for restoring cache

* Set --load-average +1 from -j
For a 2CPU/4096MB CI resource

* Reorganized executor and add caching nonce

Improve env DRY'ness by using separate executors

Enable debug builds

Add nonce to caching to help avoid caching key collisions
between debug and release workflow paths

* Fix awk to print env to checksum

* Add .dockerhub build hooks
and add FROM_IMAGE to arguments

* Switch to custom docker repo

* Use symlinks to place Dockerfile with hooks
Tweak build hook to compensate for dockhub assumptions
DockerHub repo build rules:

source: master
tag: master
Dockerfile: /.dockerhub/nightly/Dockerfile

source: master
tag: master-rmw
Dockerfile: /.dockerhub/nightly-rmw/Dockerfile

source: /^.*-devel$
tag: {sourceref}
Dockerfile: /.dockerhub/devel/Dockerfile

* Have CI Dockerfile use ccache

* Add .dockerignore to avoid braking build cache
So tweaking unrelated files doesn't invalidate:
COPY ./ navigation2/

* Word wrap long command

* Combine ccache with Circle CI caching

* Correct terms for underlay and overlay
upstream -> underlay
navigation2,build,source -> overlay

* Change setup_underlay to setup_dependencies

* Share ccache across overlay and underlay

* Correct typo

* Add new line

* Always save ccache
avoid unnecessary checking step

* Hack to avoid breaking docker build cache
for COPY when changing Dockerfile itself.
This shouldn't be needed,
but does not seem to work without **
¯\_(ツ)_/¯

* Add adjustable build failure behavior
When FAIL_ON_BUILD_FAILURE is unset
failures from colcon build will be supressed
allowing for docker build to succeed.
This prevents DockerHub CI images from becoming stale
when branch build brakes, previously blocking DockerHub CI image builds.

* Enable nightly release tests
now that docker CI builds unset FAIL_ON_BUILD_FAILURE
thus its status is less indicative of master build breaking

* Avoid cache collisions for PRs with same branch name
by including unique PR number into cache key

CircleCI branch name substitution using {{ .Branch }}
does not prefix names with the repo name.
Thus PRs from separate forks using the same branch name could collide.

https://circleci.com/docs/2.0/env-vars/#built-in-environment-variables
https://circleci.com/docs/2.0/configuration-reference/#save_cache

* Do not share underlay cache across branchs/PRs

* Abridge lengthening cache key names

* Account for failed underlay builds
if we don't happen to clean the underlay
when we keep the underlay from the docker image

Copy logic to overlay for consistency in behavior

* Add epoch to cache key given caches are immutable
and use spaceless string concatenation in yaml
to split long key names into multiple lines.

Given the immutability of caches,
and that cache key matching is prefix based,
{{ epoch }} must be the last part of the cache-save key.

https://discuss.circleci.com/t/add-mechanism-to-update-existing-cache-key/9014/13
https://circleci.com/docs/2.0/caching/#restoring-cache
https://circleci.com/docs/2.0/configuration-reference/#save_cache
https://stackoverflow.com/questions/3790454/how-do-i-break-a-string-over-multiple-lines/21699210#comment65667554_21699210

* Clean workspace incase build_failed

* Improve underlay build cache
By only copying .repo file for vcs import of underlay
and move overlay copy just before rosdep install for overlay

* Remove duplicate copy of overlay .repos file
given the dockerfile now copies this directly into the overlay wrokspace

* Rename workspaces to underlay and overlay
To complete the standardization of terminology
ros_ws -> underlay_ws
nav2_ws -> overlay_ws

* Add on_checkout anchor
to make config more DRY

* Target nonfree tag to include connext

* Add ccache stats command
to print and zero ccache statistics
as well as ccache version and config

after restore of ccache cache
and after each underlay/overlay build

* Cleanup code coverage pipline
Use offical Codecov orb for uploading
make the coverage script more DRY

* Remove unnecessary indents

* Split debuild build and test
and make release tests more DRY

* Shorten config

* Print out lcov command when runing script

* Use  to avoid infinite recursion

* Add common_environment refrence
and make jobs more DRY

* Store ccache logs for CI debugging

* Simplify code coverage script
to prevent untented shell glob expansion

* Rename fields

* Store test result

* Copy test results into expected directory structure
See directory structure example in
https://circleci.com/docs/2.0/configuration-reference/#store_test_results

* Store test artifacts for debug uploaded results

* Support parallel testing

* Fix xargs line splitting

* Run colcon test-results to fail on test errors
Needed to use return status from test-results

* Lookup historical timing data by classname for split-by
E.g. "costmap_queue.CostmapQueue" in `costmap_queue/utest.gtest.xml` :

<testsuites tests="5" failures="0" disabled="0" errors="0" timestamp="2019-08-12T06:57:26" time="0.499" name="AllTests">
<script/>
<testsuite name="CostmapQueue" tests="5" failures="0" disabled="0" errors="0" time="0.499">
<testcase name="basicQueue" status="run" time="0" classname="costmap_queue.CostmapQueue"/>
<testcase name="bigTest" status="run" time="0.496" classname="costmap_queue.CostmapQueue"/>
<testcase name="linearQueue" status="run" time="0" classname="costmap_queue.CostmapQueue"/>
<testcase name="crossQueue" status="run" time="0.001" classname="costmap_queue.CostmapQueue"/>
<testcase name="limitedQueue" status="run" time="0.002" classname="costmap_queue.CostmapQueue"/>
</testsuite>
</testsuites>

* Show split count and omit unneeded xargs

* Simplify build using colcon mixins
and add build args for underlay and overlay mixins

* Override mixing in build hook

* Rename build hooks and tags

* Build overlay with same settings to maximize ccache hit rate

* Correct mixin typo

* Make checksum.txt more informative
by piping message into directly into file for debugging
and simplify digests inclusion for spotting hash diffs

* Fixing up paths for codecov.io report (#1036)

* Update .dockerignore

Co-Authored-By: Carl Delsey <carl.r.delsey@intel.com>

* Move rwm jobs back into nightly workflow

* Set genhtml as default and add case for ci

* Refactor config using commands

* Use paths instead of env

* Revert ccache anchor name

* Fix install linking

* Avoid using paths as keys
prefix matching fails when key includes slashes

* Rename yaml anchor

* Make sure lcov is installed for CI

* Fix the build due to access specifiers (#1040)

No idea why this builds under GCC but these changes were needed to build under clang.

* Mark timerEvent override (#1037)

* Add dummy Dockerfiles for setting repository links on Docker Hub

* Set exit and print trace in script

* Tweek tag names

* nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds (#1048)

* nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds

* Update nav2_costmap_2d/plugins/static_layer.cpp

oops!

Co-Authored-By: Carl Delsey <carl.r.delsey@intel.com>

* Rewrite nav2_map_server (#989)

Add GraphicsMagick API and use it to implement map_saver, map_loader
Removes dependency on SDL, creates installation dependency on libgraphicsmagick++; apt package libgraphicsmagick++1-dev
Allows saving in trinary, raw, or scale mode.
Added configurable image format pgm, png, etc.
Add warning when using scale mode with opaque format.
Remove SDL from map server
Extract repeated MapMode to shared enum class
Switch writing YAML to same library that reads it (yaml-cpp)
Add --help alias to map saver -h
Extract map yaml loading to its own function, with better error messages. Also fixed crash due to misuse of std::string constructor.

* Migrate image tag to rosplanning repo

* Enable AMCL to initialize at the origin, under control of a parameter

* Make sure everything shuts down cleanly

* Some cleanup and linting

* Address review feedback

* Also have to update the YAML file

* Remove the connection object for now. Needs further investigation

* Find all uses of yaw to quaternion and use new function

* Simplify a quaternion copy

* Restore COLCON_IGNORE file

* Oops. Missed a package dependency

* Fixing a rebase error

* Add missing dependencies to system tests (#1059)

Signed-off-by: Louise Poubel <louise@openrobotics.org>

* add tf2_geometry_msgs dependency

* Fix the updown test. (#1061)

* Fixing linter errors, accidentally commited.

* Export envs for arg in build hook

* Remove the setSimTime() method - not needed anymore

* Remove ros2param package dependency

* Fix indentation (#1065)

* BT Service node & implementations (#1055)

* generic ros service caller BT node

* BT specialty service callers using BT node service

* rename to reinitialize global localization

* uncrustify/lint

* changes per PR requests

* changing pose topics to goal_pose

* Removing deprecation warning

* Removing an accidentally re-introduced file

* use seconds in Duration

* Enable global planner tester (#927)

* use duration from seconds

* Add ctest loop script to system tests

* Set line buffering to 1 in system test launch

* Update README

* Update README for using ctest_loop script

* Recombine AMCL library components with AMCL to have consistent license (#1074)

* Move AMCL library files back to nav2_amcl package

* Fixing up naming so things compile again

* Transition copyright disable check to follow eig3 files.

* Transition amcl files license to LGPL 2.1

* Adding missing copyright headers to nav2_bringup package

* Adding missing copyright headers to nav2_common

* Fixing up include guard to match coding style

* Reformatting license to pass the copyright check

The change here is only whitespace changes - line length and paragraph
spacing, as well as the addition of the license title

* Relicensing file to better match the existing package

* Fixing readme to account for the new change

* Begin updating nav2_behavior_tree README

* Continue update of nav2_behavior_tree readme

* Add link to behavior tree basic nodes to README

Adding a link to the upstream documentation on behaviortree.dev

* Clarify Licensing throughout the repo (#1080)

* Add proper SPDX license IDs and repo LICENSE file

* Fixing a minor linter typo

* Use python yaml safe loader (#1085)

* Null-terminate string to dirname function (#1093)

possibly responsible for heisenbug #1079 (comment)

* Update to latest v2 BT Library (#1088)

* Revert CreateTimer change

* Revert "nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds (#1048)"

This reverts commit 7c473cd.

* Fix dependency exports

* Fixing missing boost dependency
  • Loading branch information
Carl Delsey committed Sep 9, 2019
1 parent 9cbda50 commit ec6ae7c
Show file tree
Hide file tree
Showing 212 changed files with 4,128 additions and 2,969 deletions.
556 changes: 361 additions & 195 deletions .circleci/config.yml

Large diffs are not rendered by default.

1 change: 1 addition & 0 deletions .dockerhub/debug/Dockerfile
12 changes: 12 additions & 0 deletions .dockerhub/debug/dummy.Dockerfile
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
# This is a dummy Dockerfile for setting repository links on Docker Hub.
# Build rules on Docker Hub can trigger whenever the base image updates.
# Base images are specified in the FROM: directive in the tracked Dockerfile.
# However, build args are used by the build hooks to adjust the base image
# so a single Dockerfile can be reused for multiple CI jobs, reducing maintenance.
# To re-enable repository linking when using build args in the FROM: directive,
# this dummy Dockerfile explicitly conveys the base image/repo to link against
# while build rules that target this still use the same hook in this directory.
# Note: This only works for non-official images.

FROM osrf/ros2:nightly
RUN echo "This is a dummy Dockerfile."
15 changes: 15 additions & 0 deletions .dockerhub/debug/hooks/build
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
#!/bin/bash
set -ex

export FROM_IMAGE=osrf/ros2:nightly
export FAIL_ON_BUILD_FAILURE=""
export UNDERLAY_MIXINS="debug ccache"
export OVERLAY_MIXINS="debug ccache coverage-gcc"

docker build \
--tag ${IMAGE_NAME} \
--build-arg FROM_IMAGE \
--build-arg FAIL_ON_BUILD_FAILURE \
--build-arg UNDERLAY_MIXINS \
--build-arg OVERLAY_MIXINS \
--file ./Dockerfile ../../.
1 change: 1 addition & 0 deletions .dockerhub/devel/Dockerfile
11 changes: 11 additions & 0 deletions .dockerhub/devel/hooks/build
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
#!/bin/bash
set -ex

export ROS_DISTRO=${SOURCE_BRANCH%"-devel"}
export FROM_IMAGE=ros:${ROS_DISTRO}

docker build \
--tag ${IMAGE_NAME} \
--build-arg FROM_IMAGE \
--build-arg FAIL_ON_BUILD_FAILURE="" \
--file ./Dockerfile ../../.
1 change: 1 addition & 0 deletions .dockerhub/release/Dockerfile
12 changes: 12 additions & 0 deletions .dockerhub/release/dummy.Dockerfile
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
# This is a dummy Dockerfile for repository links on Docker Hub.
# Build rules on Docker Hub can trigger whenever the base image updates.
# Base images are specified in the FROM: directive in the tracked Dockerfile.
# However, build args are used by the build hooks to adjust the base image
# so a single Dockerfile can be reused for multiple CI jobs, reducing maintenance.
# To re-enable repository linking when using build args in the FROM: directive,
# this dummy Dockerfile explicitly conveys the base image/repo to link against
# while build rules that target this still use the same hook in this directory.
# Note: This only works for non-official images.

FROM osrf/ros2:nightly-rmw-nonfree
RUN echo "This is a dummy Dockerfile."
15 changes: 15 additions & 0 deletions .dockerhub/release/hooks/build
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
#!/bin/bash
set -ex

export FROM_IMAGE=osrf/ros2:nightly-rmw-nonfree
export FAIL_ON_BUILD_FAILURE=""
export UNDERLAY_MIXINS="release ccache"
export OVERLAY_MIXINS="release ccache"

docker build \
--tag ${IMAGE_NAME} \
--build-arg FROM_IMAGE \
--build-arg FAIL_ON_BUILD_FAILURE \
--build-arg UNDERLAY_MIXINS \
--build-arg OVERLAY_MIXINS \
--file ./Dockerfile ../../.
9 changes: 9 additions & 0 deletions .dockerignore
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
################################################################################
# Repo

.circleci/*
.git/*
.dockerignore
.gitignore
**Dockerfile
codecov.yml
6 changes: 0 additions & 6 deletions .github/PULL_REQUEST_TEMPLATE.md
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,3 @@
* 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
-->

---

<!-- OPTIONAL -->

I'd like to request maintainer: <blank> to review this PR.
107 changes: 67 additions & 40 deletions Dockerfile
Original file line number Diff line number Diff line change
@@ -1,23 +1,38 @@
# This dockerfile expects proxies to be set via --build-arg if needed
# It also expects to be contained in the /navigation2 root folder for file copy
# This dockerfile can be configured via --build-arg
# Build context must be the /navigation2 root folder for COPY.
# Example build command:
# export CMAKE_BUILD_TYPE=Debug
# docker build -t nav2:latest --build-arg CMAKE_BUILD_TYPE ./
FROM osrf/ros2:nightly

# copy ros package repo
ENV NAV2_WS /opt/nav2_ws
RUN mkdir -p $NAV2_WS/src
WORKDIR $NAV2_WS/src
COPY ./ navigation2/

# clone dependency package repos
ENV ROS_WS /opt/ros_ws
RUN mkdir -p $ROS_WS/src
WORKDIR $ROS_WS
RUN vcs import src < $NAV2_WS/src/navigation2/tools/ros2_dependencies.repos

# install dependency package dependencies
# export UNDERLAY_MIXINS="debug ccache"
# export OVERLAY_MIXINS="debug ccache coverage"
# docker build -t nav2:latest \
# --build-arg UNDERLAY_MIXINS \
# --build-arg OVERLAY_MIXINS ./
ARG FROM_IMAGE=osrf/ros2:nightly
FROM $FROM_IMAGE

# install CI dependencies
RUN apt-get update && \
apt-get install -q -y \
ccache \
lcov \
python3-colcon-mixin \
&& rm -rf /var/lib/apt/lists/*

# setup colcon mixin / meta
RUN colcon mixin add upstream \
https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \
colcon mixin update && \
colcon metadata add upstream \
https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml && \
colcon metadata update

# clone underlay source
ENV UNDERLAY_WS /opt/underlay_ws
RUN mkdir -p $UNDERLAY_WS/src
WORKDIR $UNDERLAY_WS
COPY ./tools/ros2_dependencies.repos ./
RUN vcs import src < ros2_dependencies.repos

# install underlay dependencies
RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
apt-get update && \
rosdep install -q -y \
Expand All @@ -26,37 +41,49 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
--ignore-src \
&& rm -rf /var/lib/apt/lists/*

# build dependency package source
ARG CMAKE_BUILD_TYPE=Release

# build underlay source
ARG UNDERLAY_MIXINS="release ccache"
ARG FAIL_ON_BUILD_FAILURE=True
RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
colcon build \
--symlink-install \
--cmake-args \
-DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE
--mixin \
$UNDERLAY_MIXINS \
|| touch build_failed && \
if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \
exit 1; \
fi

# copy overlay source
ENV OVERLAY_WS /opt/overlay_ws
RUN mkdir -p $OVERLAY_WS/src
WORKDIR $OVERLAY_WS
COPY ./ src/navigation2/

# install navigation2 package dependencies
WORKDIR $NAV2_WS
RUN . $ROS_WS/install/setup.sh && \
# install overlay dependencies
RUN . $UNDERLAY_WS/install/setup.sh && \
apt-get update && \
rosdep install -q -y \
--from-paths \
$ROS_WS/src \
$UNDERLAY_WS/src \
src \
--ignore-src \
&& rm -rf /var/lib/apt/lists/*

# build navigation2 package source
RUN rm $NAV2_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE
ARG COVERAGE_ENABLED=False
RUN . $ROS_WS/install/setup.sh && \
colcon build \
--symlink-install \
--cmake-args \
-DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
-DCOVERAGE_ENABLED=$COVERAGE_ENABLED

# source navigation2 workspace from entrypoint
# build overlay source
ARG OVERLAY_MIXINS="release ccache"
RUN rm $OVERLAY_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE
RUN . $UNDERLAY_WS/install/setup.sh && \
colcon build \
--symlink-install \
--mixin \
$OVERLAY_MIXINS \
|| touch build_failed && \
if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \
exit 1; \
fi

# source overlay from entrypoint
RUN sed --in-place \
's|^source .*|source "$NAV2_WS/install/setup.bash"|' \
's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
/ros_entrypoint.sh
17 changes: 17 additions & 0 deletions LICENSE
Original file line number Diff line number Diff line change
@@ -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.
2 changes: 2 additions & 0 deletions codecov.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
fixes:
- "src/navigation2/::"
12 changes: 11 additions & 1 deletion nav2_amcl/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,11 @@ include_directories(
include
)

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}
Expand Down Expand Up @@ -59,6 +64,10 @@ ament_target_dependencies(${library_name}
${dependencies}
)

target_link_libraries(${library_name}
map_lib motions_lib sensors_lib
)

install(TARGETS ${executable_name} ${library_name}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
Expand All @@ -73,6 +82,7 @@ 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()

find_package(ament_cmake_pytest REQUIRED)
Expand All @@ -81,7 +91,7 @@ if(BUILD_TESTING)
endif()

ament_export_include_directories(include)
ament_export_libraries(${library_name})
ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib)
ament_export_dependencies(${dependencies})

ament_package()
7 changes: 4 additions & 3 deletions nav2_amcl/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -2,22 +2,23 @@
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.

## Overview
Currently, the AMCL module in ROS 2 Navigation System is a direct port from [ROS1 AMCL](http://wiki.ros.org/amcl) package with some minor code re-factoring. The direct port includes all of ROS1 functionalities except running from Bag files. The code re-factoring includes moving Sensor, Particle Filter, and Map from nav2_amcl directory to nav2_util directory. The main logic behind this is code re-usability. However, given the way in which these classes are currently written, these codes are highly bounded to AMCL. Therefore, to make use of code re-usability, these classes need to be substantially modified.
Currently, the AMCL module in ROS 2 Navigation System is a direct port from [ROS1 AMCL](http://wiki.ros.org/amcl) package with some minor code re-factoring. The direct port includes all of ROS1 functionalities except running from Bag files.

## Added Feature
AutoLocalization - is implemented by utilizing AMCL's `global_localization` service request and Behavior Tree (BT). This enables initial pose estimation capability on differential type robot.

On startup of the navigation stack, the initial robot pose needs to be sent to AMCL otherwise AMCL initializes its filter state to default values with a particle cloud centered around (0,0,0). If the initial pose of the robot is not known and the robot is outside of default particle cloud, particles may not converge when robot moves.

With the AutoLocalization branch of BT, first the `global_localization` service of AMCL is called to randomly disperse all particles through the free space in the map. Once particles are dispersed, the robot rotates, backs up, and, if necessary, rotates again to localize itself. The full implementation is described in the AutoLocalization section of [BT Navigator](https://github.com/ros-planning/navigation2/blob/master/nav2_bt_navigator/README.md)

**Warning**: AutoLocalization actuates robot; currently, obstacle avoidance has not been integrated into this feature. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations.
**Warning**: AutoLocalization actuates robot; currently, obstacle avoidance has not been integrated into this feature. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations.

## Current Plan
* Polishing AMCL core code, especially the `laserReceived` callback [Issue 211](https://github.com/ros-planning/navigation2/issues/211)
* Using generic Particle Filter library [Issue 206](https://github.com/ros-planning/navigation2/issues/206)
* Creating a generic library to pull out the algorithms that are from Probabilistic Robotics textbook. These algorithms could potentially be used on other modules [Issue 207](https://github.com/ros-planning/navigation2/issues/207)

## Future Plan
* Running from Ros bag
* Running from Ros bag
* Extending AMCL to work with different type of Sensors
* Improving overall localization performance with fusing data from different sensors such as IMU, Sonar, Radar, Laser, Depth camera, and etc.
17 changes: 11 additions & 6 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "message_filters/subscriber.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/motion_model/motion_model.hpp"
#include "nav2_util/sensors/laser/laser.hpp"
#include "nav2_amcl/motion_model/motion_model.hpp"
#include "nav2_amcl/sensors/laser/laser.hpp"
#include "nav_msgs/srv/set_map.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
Expand Down Expand Up @@ -140,7 +140,7 @@ class AmclNode : public nav2_util::LifecycleNode

// Odometry
void initOdometry();
std::unique_ptr<nav2_util::MotionModel> motion_model_;
std::unique_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
Expand All @@ -164,9 +164,9 @@ class AmclNode : public nav2_util::LifecycleNode
// Laser scan related
void initLaserScan();
const char * scan_topic_{"scan"};
nav2_util::Laser * createLaserObject();
nav2_amcl::Laser * createLaserObject();
int scan_error_count_{0};
std::vector<nav2_util::Laser *> lasers_;
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_;
Expand Down Expand Up @@ -198,9 +198,14 @@ class AmclNode : public nav2_util::LifecycleNode
const std::vector<amcl_hyp_t> & hyps,
const int & max_weight_hyp);
void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration);
void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
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};
double initial_pose_x_;
double initial_pose_y_;
double initial_pose_z_;
double initial_pose_yaw_;

// Node parameters (initialized via initParameters)
void initParameters();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,12 @@
*
*/

#ifndef NAV2_UTIL__ANGLEUTILS_HPP_
#define NAV2_UTIL__ANGLEUTILS_HPP_
#ifndef NAV2_AMCL__ANGLEUTILS_HPP_
#define NAV2_AMCL__ANGLEUTILS_HPP_

#include <math.h>

namespace nav2_util
namespace nav2_amcl
{

class angleutils
Expand Down Expand Up @@ -57,6 +57,6 @@ angleutils::angle_diff(double a, double b)
}
}

} // namespace nav2_util
} // namespace nav2_amcl

#endif // NAV2_UTIL__ANGLEUTILS_HPP_
#endif // NAV2_AMCL__ANGLEUTILS_HPP_
Loading

0 comments on commit ec6ae7c

Please sign in to comment.