Merge branch 'main' into pr-update-maintainers

.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/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 }}

MIGRATION.md

@@ -2,6 +2,9 @@
 
 API changes in MoveIt releases
 
+## ROS Rolling
+- ServoServer was renamed to ServoNode
+
 ## ROS Noetic
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.
 - Planned trajectories are *slow* by default.

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_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h

@@ -160,7 +160,8 @@ class PosedDistanceField : public distance_field::PropagationDistanceField
   double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
                              bool& in_bounds) const
   {
-    Eigen::Vector3d rel_pos = pose_.inverse() * Eigen::Vector3d(x, y, z);
+    // Transpose of a rotation matrix equals its inverse, but computationally cheaper
+    Eigen::Vector3d rel_pos = pose_.linear().transpose() * Eigen::Vector3d(x, y, z);
     double gx, gy, gz;
     double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(),
                                                                                gx, gy, gz, in_bounds);

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -273,11 +273,30 @@ class RobotTrajectory
    */
   bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;
 
+  /** @brief Print information about the trajectory
+   * @param out              Stream to print to
+   * @param variable_indexes The indexes of the variables to print.
+   *                         If empty/not specified and the group is defined, then uses the indexes for the group
+   *                         If empty and the group is not defined, uses ALL variables in robot_model
+   *
+   * e.g.
+   * Trajectory has 13 points over 2.965 seconds
+   *   waypoint   0 time 0.000 pos  0.000 vel  0.000 acc  0.000
+   *   waypoint   1 time 0.067 pos  0.001 vel  0.033 acc  1.000
+   *   waypoint   2 time 0.665 pos  0.200 vel  0.632 acc  1.000
+   *   ...
+   */
+  void print(std::ostream& out, std::vector<int> variable_indexes = std::vector<int>()) const;
+
 private:
   moveit::core::RobotModelConstPtr robot_model_;
   const moveit::core::JointModelGroup* group_;
   std::deque<moveit::core::RobotStatePtr> waypoints_;
   std::deque<double> duration_from_previous_;
   rclcpp::Clock clock_ros_;
 };
+
+/** @brief Operator overload for printing trajectory to a stream */
+std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory);
+
 }  // namespace robot_trajectory

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -504,4 +504,81 @@ bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
   return true;
 }
 
+void RobotTrajectory::print(std::ostream& out, std::vector<int> variable_indexes) const
+{
+  size_t num_points = getWayPointCount();
+  if (num_points == 0)
+  {
+    out << "Empty trajectory.";
+    return;
+  }
+
+  std::ios::fmtflags old_settings = out.flags();
+  int old_precision = out.precision();
+  out << std::fixed << std::setprecision(3);
+
+  out << "Trajectory has " << num_points << " points over " << getDuration() << " seconds\n";
+
+  if (variable_indexes.empty())
+  {
+    if (group_)
+    {
+      variable_indexes = group_->getVariableIndexList();
+    }
+    else
+    {
+      // use all variables
+      variable_indexes.resize(robot_model_->getVariableCount());
+      std::iota(variable_indexes.begin(), variable_indexes.end(), 0);
+    }
+  }
+
+  for (size_t p_i = 0; p_i < num_points; ++p_i)
+  {
+    const moveit::core::RobotState& point = getWayPoint(p_i);
+    out << "  waypoint " << std::setw(3) << p_i;
+    out << " time " << std::setw(5) << getWayPointDurationFromStart(p_i);
+    out << " pos ";
+    for (int index : variable_indexes)
+    {
+      out << std::setw(6) << point.getVariablePosition(index) << " ";
+    }
+    if (point.hasVelocities())
+    {
+      out << "vel ";
+      for (int index : variable_indexes)
+      {
+        out << std::setw(6) << point.getVariableVelocity(index) << " ";
+      }
+    }
+    if (point.hasAccelerations())
+    {
+      out << "acc ";
+      for (int index : variable_indexes)
+      {
+        out << std::setw(6) << point.getVariableAcceleration(index) << " ";
+      }
+    }
+    if (point.hasEffort())
+    {
+      out << "eff ";
+      for (int index : variable_indexes)
+      {
+        out << std::setw(6) << point.getVariableEffort(index) << " ";
+      }
+    }
+    out << "\n";
+  }
+
+  out.flags(old_settings);
+  out.precision(old_precision);
+  out.flush();
+}
+
+std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory)
+{
+  trajectory.print(out);
+  return out;
+}
+
 }  // end of namespace robot_trajectory

moveit_core/trajectory_processing/test/test_time_parameterization.cpp

@@ -115,26 +115,7 @@ void printTrajectory(robot_trajectory::RobotTrajectory& trajectory)
 {
   const moveit::core::JointModelGroup* group = trajectory.getGroup();
   const std::vector<int>& idx = group->getVariableIndexList();
-  unsigned int count = trajectory.getWayPointCount();
-
-  std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds."
-            << std::endl;
-  std::cout << "  Trajectory Points" << std::endl;
-  for (unsigned i = 0; i < count; i++)
-  {
-    moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i);
-    printf("  waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i),
-           point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]),
-           point->getVariableAcceleration(idx[0]));
-    if (i > 0)
-    {
-      moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1);
-      printf("jrk %6.2f",
-             (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) /
-                 (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1)));
-    }
-    printf("\n");
-  }
+  trajectory.print(std::cout, { idx[0] });
 }
 
 TEST(TestTimeParameterization, TestIterativeParabolic)