Merge branch 'main' into pr-fix_race_condition

.docker/ci-testing/Dockerfile

@@ -1,10 +1,8 @@
 # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing
 # CI image using the ROS testing repository
 
-ARG ROS_DISTRO=foxy
+ARG ROS_DISTRO=rolling
 FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci
-
-MAINTAINER Dave Coleman dave@picknik.ai
 MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
 # Switch to ros-testing

.docker/ci/Dockerfile

@@ -1,34 +1,34 @@
 # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci
 # ROS base image augmented with all MoveIt dependencies to use for CI
 
-ARG ROS_DISTRO=foxy
+ARG ROS_DISTRO=rolling
 FROM ros:${ROS_DISTRO}-ros-base-focal
-
 MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
-MAINTAINER Dave Coleman dave@picknik.ai
 
 ENV TERM xterm
 
+# Setup (temporary) ROS workspace
 WORKDIR /root/ws_moveit
 
+# Copy MoveIt sources from docker context
+COPY . src/moveit2
+
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
 # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers
 RUN \
     # Update apt package list as previous containers clear the cache
-    apt-get -qq update && \
-    apt-get -qq dist-upgrade && \
+    apt-get -q update && \
+    apt-get -q -y dist-upgrade && \
     #
     # Install some base dependencies
-    apt-get -qq install --no-install-recommends -y \
+    apt-get -q install --no-install-recommends -y \
         # Some basic requirements
         wget git sudo curl \
         # Preferred build tools
         clang clang-format-10 clang-tidy clang-tools \
         ccache && \
     #
-    # Download MoveIt source, so that we can fetch all necessary dependencies
-    mkdir src && \
-    git clone https://github.com/ros-planning/moveit2.git src/moveit2 -b main && \
+    # Fetch all dependencies from moveit2.repos
     vcs import src < src/moveit2/moveit2.repos && \
     if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \
     #

.docker/release/Dockerfile

@@ -1,11 +1,12 @@
 # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-release
 # Full debian-based install of MoveIt using apt-get
 
-ARG ROS_DISTRO=foxy
+ARG ROS_DISTRO=rolling
 FROM ros:${ROS_DISTRO}-ros-base-focal
-MAINTAINER Dave Coleman dave@picknik.ai
+MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
-RUN apt-get update && \
+RUN apt-get update -q && \
+    apt-get dist-upgrade -q -y && \
     apt-get install -y ros-${ROS_DISTRO}-moveit-* && \
     rm -rf /var/lib/apt/lists/*

.docker/source/Dockerfile

@@ -1,35 +1,34 @@
+# syntax = docker/dockerfile:1.3
+
 # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-source
 # Downloads the moveit source code and install remaining debian dependencies
 
-ARG ROS_DISTRO=foxy
+ARG ROS_DISTRO=rolling
 FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing
-
 MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
-MAINTAINER Dave Coleman dave@picknik.ai
 
+# Export ROS_UNDERLAY for downstream docker containers
 ENV ROS_UNDERLAY /root/ws_moveit/install
-WORKDIR $ROS_UNDERLAY/../src
+WORKDIR $ROS_UNDERLAY/..
+
+# Copy MoveIt sources from docker context
+COPY . src/moveit2
 
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
 # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers
-RUN \
-    # Download moveit source so that we can get necessary dependencies
-    git clone https://github.com/ros-planning/moveit2.git -b main && \
-    vcs import < moveit2/moveit2.repos && \
-    if [ -r moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import < moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \
+RUN --mount=type=cache,target=/root/.ccache/ \
+    # Enable ccache
+    PATH=/usr/lib/ccache:$PATH && \
+    # Fetch required upstream sources for building
+    vcs import src < src/moveit2/moveit2.repos && \
+    if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \
     #
-    # Update apt package list as cache is cleared in previous container
-    # Usually upgrading involves a few packages only (if container builds became out-of-sync)
-    apt-get -qq update && \
-    apt-get -qq dist-upgrade && \
-    #
-    rosdep update && \
-    rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \
-    rm -rf /var/lib/apt/lists/*
-
-# Build the workspace
-RUN cd $ROS_UNDERLAY/.. && . /opt/ros/${ROS_DISTRO}/setup.sh &&\
+    . /opt/ros/${ROS_DISTRO}/setup.sh &&\
     colcon build \
             --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \
-              --ament-cmake-args -DCMAKE_BUILD_TYPE=Release \
-            --event-handlers desktop_notification- status-
+            --ament-cmake-args -DCMAKE_BUILD_TYPE=Release \
+            --event-handlers desktop_notification- status- && \
+    ccache -s && \
+    #
+    # Update /ros_entrypoint.sh to source our new workspace
+    sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.sh#g" /ros_entrypoint.sh

.dockerignore

@@ -0,0 +1,41 @@
+# ignore everything
+*
+# but include these:
+!moveit2*.repos
+!**/package.xml
+!**/COLCON_IGNORE
+
+# https://github.com/moby/moby/issues/42788
+!moveit_plugins/moveit_plugins/package.xml
+!moveit_plugins/moveit_ros_control_interface/package.xml
+!moveit_plugins/moveit_simple_controller_manager/package.xml
+!moveit_kinematics/package.xml
+!moveit_common/package.xml
+!moveit_setup_assistant/package.xml
+!moveit_core/package.xml
+!moveit_commander/package.xml
+!moveit_demo_nodes/run_move_group/package.xml
+!moveit_demo_nodes/run_ompl_constrained_planning/package.xml
+!moveit_demo_nodes/run_moveit_cpp/package.xml
+!moveit_planners/ompl/package.xml
+!moveit_planners/chomp/chomp_motion_planner/package.xml
+!moveit_planners/chomp/chomp_interface/package.xml
+!moveit_planners/chomp/chomp_optimizer_adapter/package.xml
+!moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
+!moveit_planners/pilz_industrial_motion_planner/package.xml
+!moveit_planners/moveit_planners/package.xml
+!moveit_planners/trajopt/package.xml
+!moveit_runtime/package.xml
+!moveit/package.xml
+!moveit_ros/warehouse/package.xml
+!moveit_ros/moveit_servo/package.xml
+!moveit_ros/occupancy_map_monitor/package.xml
+!moveit_ros/perception/package.xml
+!moveit_ros/move_group/package.xml
+!moveit_ros/robot_interaction/package.xml
+!moveit_ros/visualization/package.xml
+!moveit_ros/manipulation/package.xml
+!moveit_ros/planning/package.xml
+!moveit_ros/planning_interface/package.xml
+!moveit_ros/benchmarks/package.xml
+!moveit_ros/moveit_ros/package.xml

.github/workflows/backport.yaml

@@ -0,0 +1,21 @@
+# Backporting Github Actions that ports a given PR to specified branch (via backport branch_name label) if the following conditions are met:
+# 1) The PR is merged
+# 2) The PR has a label of "backport branch_name" and branch_name is an existing branch.
+# The label can be specified before or after the original PR is merged. Once these two conditions are met the bot will open a backport PR.
+name: Backport
+on:
+  pull_request:
+    types:
+      - closed
+      - labeled
+
+jobs:
+  backport:
+    if: github.event.pull_request.merged == true
+    runs-on: ubuntu-latest
+    name: Backport
+    steps:
+      - name: Backport
+        uses: tibdex/backport@v1
+        with:
+          github_token: ${{ secrets.GITHUB_TOKEN }}

.github/workflows/docker.yaml

@@ -26,6 +26,7 @@ jobs:
     steps:
       - name: Check for apt updates
         uses: addnab/docker-run-action@v3
+        continue-on-error: true
         id: apt
         with:
           image: ${{ env.GH_IMAGE }}
@@ -77,6 +78,7 @@ jobs:
     steps:
       - name: Check for apt updates
         uses: addnab/docker-run-action@v3
+        continue-on-error: true
         id: apt
         with:
           image: ${{ env.GH_IMAGE }}
@@ -129,6 +131,7 @@ jobs:
     steps:
       - name: Check for apt updates
         uses: addnab/docker-run-action@v3
+        continue-on-error: true
         id: apt
         with:
           image: ${{ env.GH_IMAGE }}
@@ -179,6 +182,7 @@ jobs:
       DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }}
 
     steps:
+      - uses: actions/checkout@v2
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v1
       - name: Login to Github Containter Registry
@@ -192,15 +196,17 @@ jobs:
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: "Remove .dockerignore"
+        run: rm .dockerignore  # enforce full source context
       - name: Build and Push
         uses: docker/build-push-action@v2
         with:
+          context: .
           file: .docker/${{ github.job }}/Dockerfile
           build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }}
           push: true
           cache-from: type=registry,ref=${{ env.GH_IMAGE }}
           cache-to: type=inline
-          no-cache: true
           tags: |
             ${{ env.GH_IMAGE }}
             ${{ env.DH_IMAGE }}

moveit2_galactic.repos

@@ -3,8 +3,8 @@ repositories:
   ros2_control:
     type: git
     url: https://github.com/ros-controls/ros2_control
-    version: master
+    version: 0.8.0
   ros2_controllers:
     type: git
     url: https://github.com/ros-controls/ros2_controllers
-    version: master
+    version: 0.5.0

moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h → moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.h

@@ -45,7 +45,7 @@
 #include <memory>
 #include <string>
 
-namespace occupancy_map_monitor
+namespace collision_detection
 {
 typedef octomap::OcTreeNode OccMapNode;
 
@@ -118,4 +118,4 @@ class OccMapTree : public octomap::OcTree
 
 using OccMapTreePtr = std::shared_ptr<OccMapTree>;
 using OccMapTreeConstPtr = std::shared_ptr<const OccMapTree>;
-}  // namespace occupancy_map_monitor
+}  // namespace collision_detection

moveit_core/collision_detection_bullet/CMakeLists.txt

@@ -48,12 +48,12 @@ target_link_libraries(collision_detector_bullet_plugin
 
 install(DIRECTORY include/ DESTINATION include)
 install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${MOVEIT_LIB_NAME}_export.h DESTINATION include)
-install(TARGETS ${MOVEIT_LIB_NAME} EXPORT ${MOVEIT_LIB_NAME}
-  TARGETS collision_detector_bullet_plugin EXPORT collision_detector_bullet_plugin
+install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bullet_plugin EXPORT export_${MOVEIT_LIB_NAME}
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin
 )
+ament_export_targets(export_${MOVEIT_LIB_NAME} HAS_LIBRARY_TARGET)
 
 if(BUILD_TESTING)
   if(WIN32)

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

@@ -56,12 +56,12 @@ namespace
 void checkFCLCapabilities(const DistanceRequest& req)
 {
 #if MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)
-  static rclcpp::Clock steady_clock(RCL_STEADY_TIME);
   if (req.enable_nearest_points)
   {
     // Known issues:
     //   https://github.com/flexible-collision-library/fcl/issues/171,
     //   https://github.com/flexible-collision-library/fcl/pull/288
+    rclcpp::Clock steady_clock(RCL_STEADY_TIME);
     RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 2000,
                           "You requested a distance check with enable_nearest_points=true, "
                           "but the FCL version MoveIt was compiled against (%d.%d.%d) "

moveit_core/planning_scene/src/planning_scene.cpp

@@ -36,6 +36,7 @@
 
 #include <boost/algorithm/string.hpp>
 #include <moveit/planning_scene/planning_scene.h>
+#include <moveit/collision_detection/occupancy_map.h>
 #include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
 #include <geometric_shapes/shape_operations.h>
 #include <moveit/collision_detection/collision_tools.h>
@@ -379,14 +380,7 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest&
                                    collision_detection::CollisionResult& res,
                                    const moveit::core::RobotState& robot_state) const
 {
-  // check collision with the world using the padded version
-  getCollisionEnv()->checkRobotCollision(req, res, robot_state, getAllowedCollisionMatrix());
-
-  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
-  {
-    // do self-collision checking with the unpadded version of the robot
-    getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
-  }
+  checkCollision(req, res, robot_state, getAllowedCollisionMatrix());
 }
 
 void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req,
@@ -1159,6 +1153,26 @@ bool PlanningScene::usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene& s
     return setPlanningSceneMsg(scene_msg);
 }
 
+collision_detection::OccMapTreePtr createOctomap(const octomap_msgs::msg::Octomap& map)
+{
+  std::shared_ptr<collision_detection::OccMapTree> om =
+      std::make_shared<collision_detection::OccMapTree>(map.resolution);
+  if (map.binary)
+  {
+    octomap_msgs::readTree(om.get(), map);
+  }
+  else
+  {
+    std::stringstream datastream;
+    if (map.data.size() > 0)
+    {
+      datastream.write((const char*)&map.data[0], map.data.size());
+      om->readData(datastream);
+    }
+  }
+  return om;
+}
+
 void PlanningScene::processOctomapMsg(const octomap_msgs::msg::Octomap& map)
 {
   // each octomap replaces any previous one
@@ -1173,7 +1187,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::Octomap& map)
     return;
   }
 
-  std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map)));
+  std::shared_ptr<collision_detection::OccMapTree> om = createOctomap(map);
   if (!map.header.frame_id.empty())
   {
     const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
@@ -1211,7 +1225,8 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose&
     return;
   }
 
-  std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
+  std::shared_ptr<collision_detection::OccMapTree> om = createOctomap(map.octomap);
+
   const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
   Eigen::Isometry3d p;
   tf2::fromMsg(map.origin, p);

moveit_core/profiler/src/profiler.cpp

@@ -173,7 +173,7 @@ void Profiler::console()
   std::stringstream ss;
   ss << std::endl;
   status(ss, true);
-  RCLCPP_INFO(LOGGER, ss.str().c_str());
+  RCLCPP_INFO(LOGGER, "%s", ss.str().c_str());
 }
 
 /// @cond IGNORE