Merge branch 'master' into get-footprint-time

.dockerignore

@@ -6,4 +6,5 @@
 .dockerignore
 .gitignore
 **Dockerfile
+**.Dockerfile
 codecov.yml

Dockerfile.full_ros_build

-# 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
-#
-# Example build command:
-# export CMAKE_BUILD_TYPE=Debug
-#
-# This determines which version of the ROS2 code base to pull
-# export ROS2_BRANCH=master
-#
-# This determines which ros2 docker image to use as a base for getting ros2_dependencies
-# The latest (osrf/ros2:nightly) is usually the best choice unless a dependency
-# has been removed that an older codebase depends on. In those cases, using
-# ros:DISTRO-ros-core would be a good choice where distro is crystal, dashing, etc.
-# export ROS2_DOCKER_IMAGE=osrf/ros2:nightly
-# docker build -t nav2:full_ros_build
-#   --build-arg CMAKE_BUILD_TYPE \
-#   -f Dockerfile.full_ros_build ./
-
-# We're only building on top of a ROS docker image to get the basics
-# prerequisites installed such as the apt source, rosdep, etc. We don't want to
-# actually use any of the ros dashing packages. Instead we are going to build
-# everything from source in one big workspace.
-ARG FROM_IMAGE=osrf/ros2:nightly
-FROM $FROM_IMAGE
-
-# install ROS2 dependencies
-RUN apt-get update && apt-get install -q -y \
-      wget \
-      libasio-dev \
-    && rm -rf /var/lib/apt/lists/*
-
-# setup directory structure
-ENV ROS2_WS /opt/ros2_ws
-RUN mkdir -p $ROS2_WS/src
-WORKDIR $ROS2_WS
-
-# clone ROS 2 packages
-
-ARG ROS2_BRANCH=master
-RUN wget https://raw.githubusercontent.com/ros2/ros2/$ROS2_BRANCH/ros2.repos
-RUN vcs import src < ros2.repos
-
-# copy nav 2 source code
-ENV NAV2_DIR $ROS2_WS/src/navigation2
-COPY ./ src/navigation2/
-
-# clone dependency package repos
-ENV NAV2_DEPS_DIR $ROS2_WS/src/navigation2_dependencies
-RUN mkdir -p $NAV2_DEPS_DIR
-RUN vcs import src < $NAV2_DIR/tools/ros2_dependencies.repos
-
-# install package dependencies
-RUN apt-get update && \
-    rosdep install -q -y \
-      --from-paths \
-        $ROS2_WS/src \
-        src \
-      --ignore-src --skip-keys rti-connext-dds-5.3.1 \
-    && rm -rf /var/lib/apt/lists/*
-
-# build entire workspace
-ARG CMAKE_BUILD_TYPE=Debug
-RUN colcon build \
-      --symlink-install \
-      --cmake-args \
-        -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \

doc/BUILD.md

 Build Instructions
 ==================
 
-Quickstart
+## 1. Platform
+* Ubuntu 18.04
+
+## 2. Manual Build Steps
+* The instructions are to build **Navigation2** package and its dependency packages.
+* To build **Navigation2** package from script, kindly refer [QuickStart](BUILD.md#quickstart-using-initial-setup-script).
+* To build **Navigation2** package for `master` branch, select the latest ROS2 distro in place of `ros2-distro-name` during `rosdep install`.
+* To build **Navigation2** package for specific distro branch (e.g- dashing-devel), select the matching ROS2 distro in place of `ros2-distro-name` during `rosdep install`.
+
+
+### 2.1 ROS2
+* Install ROS2 with dependencies from the ROS2 Installation Page: https://index.ros.org/doc/ros2/Installation/
+* Install the latest distribution of ROS2 to support Navigation2 `master`.
+* ROS2 can be installed either from binary packages or build from source. Choose to build ROS2 from source to support Navigation2 `master`.
+
+### 2.2 Navigation2 Dependencies
+If ROS2 installed from source (Ignore if installed from binary packages)
+```console
+$ source ~/ros2_ws/install/setup.bash
+```
+Fetch, build and install navigation2 dependencies:
+```console
+$ mkdir -p ~/ros2_nav_dependencies_ws/src
+$ cd ~/ros2_nav_dependencies_ws
+$ wget https://raw.githubusercontent.com/ros-planning/navigation2/master/tools/ros2_dependencies.repos
+$ vcs import src < ros2_dependencies.repos
+$ rosdep install -y -r -q --from-paths src --ignore-src --rosdistro <ros2-distro-name>
+e.g- ros2-distro-name = dashing
+$ colcon build --symlink-install
+```
+
+### 2.3 Navigation2
+
+Fetch, build and install navigation2 stack:
+```console
+$ source ~/ros2_nav_dependencies_ws/install/setup.bash
+$ mkdir -p ~/navigation2_ws/src
+$ cd ~/navigation2_ws/src
+$ git clone https://github.com/ros-planning/navigation2.git -b <nav2-branch-name>
+e.g- nav2-branch-name = master
+$ cd ~/navigation2_ws
+$ rosdep install -y -r -q --from-paths src --ignore-src --rosdistro <ros2-distro-name>
+e.g- ros2-distro-name = dashing
+$ colcon build --symlink-install
+```
+### 2.4 Turtlebot3
+Turtlebot3 is one of the target robot platform to run navigation2. Ignore building Turtlebot3 packages if turtlebot3 is not your target.
+
+If ROS2 installed from source (Ignore if installed from binary packages)
+```console
+$ source ~/ros2_ws/install/setup.bash
+```
+Fetch, build and install turtlebot3 packages:
+```console
+$ mkdir -p ~/turtlebot3_ws/src
+$ cd ~/turtlebot3_ws
+$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/ros2/turtlebot3.repos
+$ vcs import src < turtlebot3.repos
+$ rosdep install -y -r -q --from-paths src --ignore-src --rosdistro <ros2-distro-name>
+e.g- ros2-distro-name = dashing
+$ colcon build --symlink-install
+```
+
+## 3. Conclusion
+After installation of **Navigation2** and required dependencies, test **Navigation2** by following steps in [nav2_bringup README](../nav2_bringup/README.md)
+
+## 4. Reporting Issue
+If run into any issue, feel free to submit pull request or report issue in this project.
+
+Quickstart using initial setup script
 ----------
 
 ### Steps

doc/use_cases/navigation_with_cartographer.md → doc/use_cases/navigation_with_slam.md

 
-# Navigation 2 with Cartographer
+# Navigation 2 with SLAM
 
 This document explains how to use Navigation 2 with SLAM. The following steps show ROS 2 users how to generate occupancy grid maps and use Navigation 2 to move their robot around.
 

full_ros_build.Dockerfile

+# This dockerfile expects to be contained in the /navigation2 root folder for file copy
+#
+# Example build command:
+# This determines which version of the ROS2 code base to pull
+# export ROS2_BRANCH=master
+# docker build \
+#   --no-cache \
+#   --tag nav2:full_ros_build \
+#   --file full_ros_build.Dockerfile ./
+#
+# Omit the `--no-cache` if you know you don't need to break the cache.
+# We're only building on top of a ros2 devel image to get the basics
+# prerequisites installed such as the apt source, rosdep, etc. We don't want to
+# actually use any of the ros release packages. Instead we are going to build
+# everything from source in one big workspace.
+
+ARG FROM_IMAGE=osrf/ros2:devel
+
+# multi-stage for caching
+FROM $FROM_IMAGE AS cache
+
+# 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
+
+# copy overlay source
+ENV OVERLAY_WS /opt/overlay_ws
+RUN mkdir -p $OVERLAY_WS/src
+WORKDIR $OVERLAY_WS
+COPY ./ src/navigation2
+
+# copy manifests for caching
+WORKDIR /opt
+RUN find ./ -name "package.xml" | \
+      xargs cp --parents -t /tmp && \
+    find ./ -name "COLCON_IGNORE" | \
+      xargs cp --parents -t /tmp
+
+# multi-stage for building
+FROM $FROM_IMAGE AS build
+
+# install packages
+RUN apt-get update && apt-get install -q -y \
+      libasio-dev \
+      libtinyxml2-dev \
+      wget \
+    && rm -rf /var/lib/apt/lists/*
+
+ARG ROS2_BRANCH=master
+ENV ROS2_BRANCH=$ROS2_BRANCH
+ENV ROS_VERSION=2 \
+    ROS_PYTHON_VERSION=3
+
+WORKDIR $ROS2_WS
+
+# get ros2 source code
+RUN wget https://raw.githubusercontent.com/ros2/ros2/$ROS2_BRANCH/ros2.repos \
+    && vcs import src < ros2.repos
+
+# get skip keys
+COPY ./tools/skip_keys.txt ./
+
+# copy underlay manifests
+COPY --from=cache /tmp/underlay_ws src/underlay
+RUN cd src/underlay && colcon list --names-only | \
+      cat > packages.txt && \
+    cd ../../ && colcon list --names-only \
+      --packages-up-to \
+        $(cat src/underlay/packages.txt | xargs) | \
+          cat > packages.txt
+
+# install underlay dependencies
+RUN apt-get update && rosdep install -y \
+      --from-paths src \
+      --ignore-src \
+      --skip-keys \
+        "$(cat skip_keys.txt | xargs)" \
+      src/underlay \
+    && rm -rf /var/lib/apt/lists/*
+
+# build ros2 source
+ARG ROS2_MIXINS="release"
+RUN colcon build \
+      --symlink-install \
+      --mixin \
+        $ROS2_MIXINS \
+      --packages-up-to \
+        $(cat src/underlay/packages.txt | xargs) \
+      --packages-skip \
+        $(cat src/underlay/packages.txt | xargs) \
+      --cmake-args --no-warn-unused-cli
+
+# copy underlay source
+COPY --from=cache /opt/underlay_ws src/underlay
+
+# build underlay source
+ARG UNDERLAY_MIXINS="release"
+RUN colcon build \
+      --symlink-install \
+      --mixin \
+        $UNDERLAY_MIXINS \
+      --packages-up-to \
+        $(cat src/underlay/packages.txt | xargs) \
+      --packages-skip-build-finished \
+      --cmake-args --no-warn-unused-cli
+
+# copy overlay manifests
+COPY --from=cache /tmp/overlay_ws src/overlay
+RUN cd src/overlay && colcon list --names-only | \
+      cat > packages.txt && \
+    cd ../../ && colcon list --names-only \
+      --packages-up-to \
+        $(cat src/overlay/packages.txt | xargs) | \
+          cat > packages.txt
+
+# install overlay dependencies
+RUN apt-get update && rosdep install -y \
+      --from-paths src \
+      --ignore-src \
+      --skip-keys \
+        "$(cat skip_keys.txt | xargs)" \
+      src/overlay \
+    && rm -rf /var/lib/apt/lists/*
+
+# build ros2 source
+RUN colcon build \
+      --symlink-install \
+      --mixin \
+        $ROS2_MIXINS \
+      --packages-up-to \
+        $(cat src/overlay/packages.txt | xargs) \
+      --packages-skip \
+        $(cat src/overlay/packages.txt | xargs) \
+      --packages-skip-build-finished \
+      --cmake-args --no-warn-unused-cli
+
+# copy overlay source
+COPY --from=cache /opt/overlay_ws src/overlay
+
+# build overlay source
+ARG OVERLAY_MIXINS="build-testing-on release"
+RUN colcon build \
+      --symlink-install \
+      --mixin \
+        $OVERLAY_MIXINS \
+      --packages-up-to \
+        $(cat src/overlay/packages.txt | xargs) \
+      --packages-skip-build-finished \
+      --cmake-args --no-warn-unused-cli
+
+# test overlay source
+ARG RUN_TESTS
+ARG FAIL_ON_TEST_FAILURE
+RUN if [ ! -z "$RUN_TESTS" ]; then \
+        colcon test \
+          --packages-select \
+            $(cat src/overlay/packages.txt | xargs); \
+        if [ ! -z "$FAIL_ON_TEST_FAILURE" ]; then \
+            colcon test-result; \
+        else \
+            colcon test-result || true; \
+        fi \
+    fi

nav2_amcl/CMakeLists.txt

@@ -84,10 +84,6 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
-
-  find_package(ament_cmake_pytest REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

nav2_amcl/README.md

@@ -13,11 +13,6 @@ With the AutoLocalization branch of BT, first the `global_localization` service
 
 **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
 * Extending AMCL to work with different type of Sensors

nav2_amcl/package.xml

@@ -37,10 +37,6 @@
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_cmake_pytest</test_depend>
-  <test_depend>launch</test_depend>
-  <test_depend>launch_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nav2_amcl/src/amcl_node.cpp

@@ -1214,7 +1214,7 @@ AmclNode::initOdometry()
   // When pausing and resuming, remember the last robot pose so we don't start at 0:0 again
   init_pose_[0] = last_published_pose_.pose.pose.position.x;
   init_pose_[1] = last_published_pose_.pose.pose.position.y;
-  init_pose_[2] = last_published_pose_.pose.pose.position.z;
+  init_pose_[2] = tf2::getYaw(last_published_pose_.pose.pose.orientation);
 
   if (!initial_pose_is_known_) {
     init_cov_[0] = 0.5 * 0.5;

nav2_amcl/test/CMakeLists.txt

-set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files)
-add_subdirectory(unit)

nav2_amcl/test/maps/test_map.yaml

-image: test_map.pgm
-resolution: 0.050000
-origin: [-15.400000, -12.200000, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196

nav2_amcl/test/test_launch_files/localization_node.launch.py

-#!/usr/bin/env python3
-
-# Copyright (c) 2018 Intel Corporation
-#
-# 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.
-
-import os
-
-from launch import LaunchDescription
-import launch_ros.actions
-
-
-def generate_launch_description():
-    mapFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), '../maps/test_map.yaml')
-    run_amcl = launch_ros.actions.Node(
-        package='nav2_amcl',
-        node_executable='amcl',
-        output='screen')
-    run_map_server = launch_ros.actions.Node(
-        package='nav2_map_server',
-        node_executable='map_server',
-        output='screen',
-        arguments=[[mapFile], 'occupancy'])
-    return LaunchDescription([run_amcl, run_map_server])

nav2_amcl/test/unit/CMakeLists.txt

-include_directories( ${PROJECT_SOURCE_DIR}/src )
-
-ament_add_gtest(test_localization_amcl test_localization_amcl.cpp)
-ament_target_dependencies( test_localization_amcl
-  ${dependencies}
-)
-

nav2_amcl/test/unit/test_localization_amcl.cpp

-// Copyright (c) 2018 Intel Corporation
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-TEST(SimplePassingTest, SimplePassingTest)
-{
-  SUCCEED();
-}

nav2_behavior_tree/CMakeLists.txt

@@ -63,8 +63,6 @@ install(DIRECTORY include/
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-
   ament_lint_auto_find_test_dependencies()
 endif()
 

nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp

@@ -28,17 +28,37 @@ namespace nav2_behavior_tree
 class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
 {
 public:
-  explicit BackUpAction(const std::string & action_name)
-  : BtActionNode<nav2_msgs::action::BackUp>(action_name)
+  explicit BackUpAction(
+    const std::string & action_name,
+    const BT::NodeParameters & params)
+  : BtActionNode<nav2_msgs::action::BackUp>(action_name, params)
   {
   }
 
   void on_init() override
   {
+    double dist;
+    getParam<double>("backup_dist", dist);
+    double speed;
+    getParam<double>("backup_speed", speed);
+
+    // silently fix, vector direction determined by distance sign
+    if (speed < 0.0) {
+      speed *= -1.0;
+    }
+
     // Populate the input message
-    goal_.target.x = -0.15;
+    goal_.target.x = dist;
     goal_.target.y = 0.0;
     goal_.target.z = 0.0;
+    goal_.speed = speed;
+  }
+
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {
+      {"backup_dist", "-0.15"}, {"backup_speed", "0.025"}};
+    return params;
   }
 };
 

nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp

@@ -31,14 +31,24 @@ namespace nav2_behavior_tree
 class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 {
 public:
-  explicit SpinAction(const std::string & action_name)
-  : BtActionNode<nav2_msgs::action::Spin>(action_name)
+  explicit SpinAction(
+    const std::string & action_name,
+    const BT::NodeParameters & params)
+  : BtActionNode<nav2_msgs::action::Spin>(action_name, params)
   {
   }
 
   void on_init() override
   {
-    goal_.target_yaw = M_PI / 2;
+    double dist;
+    getParam<double>("spin_dist", dist);
+    goal_.target_yaw = dist;
+  }
+
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"spin_dist", "1.57"}};
+    return params;
   }
 };
 

nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp

@@ -28,8 +28,8 @@ namespace nav2_behavior_tree
 class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
 {
 public:
-  explicit WaitAction(const std::string & action_name)
-  : BtActionNode<nav2_msgs::action::Wait>(action_name)
+  explicit WaitAction(const std::string & action_name, const BT::NodeParameters & params)
+  : BtActionNode<nav2_msgs::action::Wait>(action_name, params)
   {
   }
 
@@ -45,6 +45,13 @@ public:
 
     goal_.time.sec = duration;
   }
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"wait_duration", "1"}};
+    return params;
+  }
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/package.xml

@@ -49,10 +49,6 @@
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_cmake_pytest</test_depend>
-  <test_depend>launch</test_depend>
-  <test_depend>launch_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>