Dashing 0.2.5 Candidate

.circleci/config.yml

@@ -66,20 +66,19 @@ references:
               cat << parameters.underlay >>/checksum.txt > checksum.txt
               vcs export --exact src | \
                 (echo vcs_export && cat) >> checksum.txt
-              sha256sum checksum.txt >> checksum.txt
+              sha256sum $PWD/checksum.txt >> checksum.txt
               apt-get update
               dependencies=$(
                 rosdep install -q -y \
-                  --from-paths \
-                    $UNDERLAY_WS/src \
+                  --from-paths src \
                   --ignore-src \
                   --verbose | \
                 awk '$1 ~ /^resolution\:/' | \
                 awk -F'[][]' '{print $2}' | \
                 tr -d \, | xargs -n1 | sort -u | xargs)
               dpkg -s $dependencies | \
                 (echo workspace_dependencies && cat) >> checksum.txt
-              sha256sum checksum.txt >> checksum.txt
+              sha256sum $PWD/checksum.txt >> checksum.txt
     setup_workspace:
       description: "Setup Workspace"
       parameters:
@@ -177,14 +176,14 @@ references:
     run:
       name: Pre Checkout
       command: |
-        mkdir -p $ROS_WS
-        ln -s /opt/ros/$ROS_DISTRO $ROS_WS/install
+        mkdir -p $ROS_WS && cd $ROS_WS
+        ln -s /opt/ros/$ROS_DISTRO install
         echo $CACHE_NONCE | \
-          (echo cache_nonce && cat) >> $ROS_WS/checksum.txt
-        sha256sum $ROS_WS/checksum.txt >> $ROS_WS/checksum.txt
+          (echo cache_nonce && cat) >> checksum.txt
+        sha256sum $PWD/checksum.txt >> checksum.txt
         TZ=utc stat -c '%y' /ros_entrypoint.sh | \
-          (echo ros_entrypoint && cat) >> $ROS_WS/checksum.txt
-        sha256sum $ROS_WS/checksum.txt >> $ROS_WS/checksum.txt
+          (echo ros_entrypoint && cat) >> checksum.txt
+        sha256sum $PWD/checksum.txt >> checksum.txt
         rm -rf $OVERLAY_WS/*
   on_checkout: &on_checkout
     checkout:
@@ -193,7 +192,6 @@ references:
     run:
       name: Post Checkout
       command: |
-        rm $OVERLAY_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE
         if ! cmp \
           $OVERLAY_WS/src/navigation2/tools/ros2_dependencies.repos \
           $UNDERLAY_WS/ros2_dependencies.repos >/dev/null 2>&1
@@ -234,7 +232,7 @@ references:
       build: false
   install_overlay_dependencies: &install_overlay_dependencies
     install_dependencies:
-      underlay: /opt/ros_ws
+      underlay: /opt/underlay_ws
       workspace: /opt/overlay_ws
   setup_overlay_workspace: &setup_overlay_workspace
     setup_workspace: &setup_workspace_overlay
@@ -322,7 +320,7 @@ executors:
       <<: *common_environment
       CACHE_NONCE: "Debug"
       OVERLAY_MIXINS: "debug ccache coverage-gcc"
-      UNDERLAY_MIXINS: "debug ccache"
+      UNDERLAY_MIXINS: "release ccache"
   release_exec:
     docker:
       - image: rosplanning/navigation2:master.release
@@ -360,6 +358,10 @@ jobs:
     <<: *release_test
     environment:
       RMW_IMPLEMENTATION: "rmw_connext_cpp"
+  test_rmw_cyclonedds_cpp:
+    <<: *release_test
+    environment:
+      RMW_IMPLEMENTATION: "rmw_cyclonedds_cpp"
   test_rmw_fastrtps_cpp:
     <<: *release_test
     environment:
@@ -391,6 +393,9 @@ workflows:
       - test_rmw_connext_cpp:
           requires:
             - release_build
+      - test_rmw_cyclonedds_cpp:
+          requires:
+            - release_build
       - test_rmw_fastrtps_cpp:
           requires:
             - release_build

.dockerhub/debug/hooks/build

@@ -3,7 +3,7 @@ set -ex
 
 export FROM_IMAGE=osrf/ros2:nightly
 export FAIL_ON_BUILD_FAILURE=""
-export UNDERLAY_MIXINS="debug ccache"
+export UNDERLAY_MIXINS="release ccache"
 export OVERLAY_MIXINS="debug ccache coverage-gcc"
 
 docker build \

Dockerfile

@@ -7,23 +7,9 @@
 #   --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
+# multi-stage for caching
+FROM $FROM_IMAGE AS cache
 
 # clone underlay source
 ENV UNDERLAY_WS /opt/underlay_ws
@@ -32,15 +18,43 @@ 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 CI dependencies	
+RUN apt-get update && apt-get install -q -y \	
+      ccache \
+      lcov \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy underlay manifests
+ENV UNDERLAY_WS /opt/underlay_ws
+COPY --from=cache /tmp/underlay_ws $UNDERLAY_WS
+WORKDIR $UNDERLAY_WS
+
 # install underlay dependencies
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
-    apt-get update && \
-    rosdep install -q -y \
-      --from-paths \
-        src \
+    apt-get update && rosdep install -q -y \
+      --from-paths src \
       --ignore-src \
     && rm -rf /var/lib/apt/lists/*
 
+# copy underlay source
+COPY --from=cache $UNDERLAY_WS ./
+
 # build underlay source
 ARG UNDERLAY_MIXINS="release ccache"
 ARG FAIL_ON_BUILD_FAILURE=True
@@ -54,25 +68,23 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
       exit 1; \
     fi
 
-# copy overlay source
+# copy overlay manifests
 ENV OVERLAY_WS /opt/overlay_ws
-RUN mkdir -p $OVERLAY_WS/src
+COPY --from=cache /tmp/overlay_ws $OVERLAY_WS
 WORKDIR $OVERLAY_WS
-COPY ./ src/navigation2/
 
 # install overlay dependencies
 RUN . $UNDERLAY_WS/install/setup.sh && \
-    apt-get update && \
-    rosdep install -q -y \
-      --from-paths \
-        $UNDERLAY_WS/src \
-        src \
+    apt-get update && rosdep install -q -y \
+      --from-paths src \
       --ignore-src \
     && rm -rf /var/lib/apt/lists/*
 
+# copy overlay source
+COPY --from=cache $OVERLAY_WS ./
+
 # 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 \

Dockerfile.full_ros_build

@@ -3,7 +3,6 @@
 #
 # Example build command:
 # export CMAKE_BUILD_TYPE=Debug
-# export BUILD_SYSTEM_TESTS=False
 #
 # This determines which version of the ROS2 code base to pull
 # export ROS2_BRANCH=master
@@ -13,15 +12,16 @@
 # 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 BUILD_SYSTEM_TESTS
-#   --build-arg CMAKE_BUILD_TYPE -f Dockerfile.full_ros_build ./
+# 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 ROS2_DOCKER_BRANCH=osrf/ros2:nightly
-FROM $ROS2_DOCKER_BRANCH
+ARG FROM_IMAGE=osrf/ros2:nightly
+FROM $FROM_IMAGE
 
 # install ROS2 dependencies
 RUN apt-get update && apt-get install -q -y \
@@ -49,15 +49,6 @@ 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
 
-# delete nav2_system_tests/COLCON_IGNORE before running rosdep, otherwise it
-# doesn't install nav2_system_tests dependencies.
-ARG BUILD_SYSTEM_TESTS=True
-RUN if [ "$BUILD_SYSTEM_TESTS" = "True" ] ; then \
-        rm $NAV2_DIR/nav2_system_tests/COLCON_IGNORE ; \
-        echo "Building of system tests enabled" ; \
-    fi
-
-
 # install package dependencies
 RUN apt-get update && \
     rosdep install -q -y \

Dockerfile.release_branch

@@ -3,29 +3,20 @@
 #
 # Example build command:
 # export CMAKE_BUILD_TYPE=Debug
-# export BUILD_SYSTEM_TESTS=False
-# docker build -t nav2:release_branch --build-arg ROS2_BRANCH=dashing \
-#  --build-arg BUILD_SYSTEM_TESTS #   --build-arg CMAKE_BUILD_TYPE \
-#  -f Dockerfile.release_branch ./
+# docker build -t nav2:release_branch \
+#   --build-arg FROM_IMAGE=dashing \
+#   --build-arg CMAKE_BUILD_TYPE \
+#   -f Dockerfile.release_branch ./
 
-ARG ROS2_BRANCH=dashing
-
-FROM ros:$ROS2_BRANCH
+ARG FROM_IMAGE=dashing
+FROM ros:$FROM_IMAGE
 
 # copy ros package repo
 ENV NAV2_WS /opt/nav2_ws
 RUN mkdir -p $NAV2_WS/src
 WORKDIR $NAV2_WS/src
 COPY ./ navigation2/
 
-# delete nav2_system_tests/COLCON_IGNORE before running rosdep, otherwise it
-# doesn't install nav2_system_tests dependencies.
-ARG BUILD_SYSTEM_TESTS=True
-RUN if [ "$BUILD_SYSTEM_TESTS" = "True" ] ; then \
-        rm $NAV2_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE ; \
-        echo "Building of system tests enabled" ; \
-    fi
-
 # install navigation2 package dependencies
 WORKDIR $NAV2_WS
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \

nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp

@@ -22,7 +22,7 @@
 #include "behaviortree_cpp/condition_node.h"
 #include "nav2_util/robot_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree
 {
@@ -58,8 +58,7 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
     node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
-    tf_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+    tf_ = blackboard()->template get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
     initialized_ = true;
   }
@@ -69,7 +68,6 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     geometry_msgs::msg::PoseStamped current_pose;
 
-    rclcpp::spin_some(node_);
     if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return false;
@@ -94,7 +92,6 @@ class GoalReachedCondition : public BT::ConditionNode
 private:
   rclcpp::Node::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   geometry_msgs::msg::PoseStamped::SharedPtr goal_;
 
   bool initialized_;

nav2_bt_navigator/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(nav2_msgs REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
@@ -45,6 +46,7 @@ set(dependencies
   behaviortree_cpp
   std_srvs
   nav2_util
+  tf2_ros
 )
 
 ament_target_dependencies(${executable_name}

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -26,6 +26,7 @@
 #include "nav2_msgs/msg/path.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
+#include "tf2_ros/transform_listener.h"
 
 namespace nav2_bt_navigator
 {
@@ -83,6 +84,10 @@ class BtNavigator : public nav2_util::LifecycleNode
 
   // A regular, non-spinning ROS node that we can use for calls to the action client
   rclcpp::Node::SharedPtr client_node_;
+
+  // Spinning transform that can be used by the BT nodes
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/package.xml

@@ -7,6 +7,8 @@
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>
 
+  <depend>tf2_ros</depend>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
   <build_depend>rclcpp</build_depend>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -55,6 +55,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   self_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
     client_node_, "NavigateToPose");
 
+  tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, rclcpp_node_, false);
+
   goal_sub_ = rclcpp_node_->create_subscription<geometry_msgs::msg::PoseStamped>(
     "/move_base_simple/goal",
     rclcpp::SystemDefaultsQoS(),
@@ -78,6 +81,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   blackboard_->set<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_);  // NOLINT
   blackboard_->set<nav2_msgs::msg::Path::SharedPtr>("path", path_);  // NOLINT
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
+  blackboard_->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10));  // NOLINT
   blackboard_->set<bool>("path_updated", false);  // NOLINT
   blackboard_->set<bool>("initial_pose_received", false);  // NOLINT
@@ -141,6 +145,8 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   goal_sub_.reset();
   client_node_.reset();
   self_client_.reset();
+  tf_.reset();
+  tf_listener_.reset();
   action_server_.reset();
   path_.reset();
   xml_string_.clear();