Merge branch 'main' into andyz/ruckig_improvements

moveit · Oct 30, 2021 · 18aa636 · 18aa636
2 parents fb303e6 + 7e3fdcf
commit 18aa636
Show file tree

Hide file tree

Showing 44 changed files with 1,075 additions and 546 deletions.
diff --git a/.ci.prepare_codecov b/.ci.prepare_codecov
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -30,23 +30,21 @@ jobs:
       # Pull any updates to the upstream workspace (after restoring it from cache)
       AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
       AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src
-      AFTER_RUN_TARGET_TEST: ${{ matrix.env.CCOV && './.ci.prepare_codecov' || '' }}
       TARGET_CMAKE_ARGS: >
-        -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'RelWithDebInfo' || 'Release'}}
-        -DCMAKE_CXX_FLAGS="$CXXFLAGS ${{ matrix.env.CCOV && '--coverage'}}"
-        --no-warn-unused-cli
+        -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}
+        -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
       CCACHE_DIR: ${{ github.workspace }}/.ccache
       BASEDIR: ${{ github.workspace }}/.work
-      CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }}
+      CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }}
       BEFORE_CLANG_TIDY_CHECKS: (cd $TARGET_REPO_PATH; clang-tidy --list-checks)
       CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
       CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }}
 
-    name: ${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || '' }}
+    name: ${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' + clang-tidy (delta)' || ' + clang-tidy (all)') || '' }}
     runs-on: ubuntu-latest
     steps:
       - name: "Free up disk space"
-        if: ${{ matrix.env.CCOV }}
+        if: matrix.env.CCOV
         run: |
           sudo apt-get -qq purge build-essential "ghc*"
           sudo apt-get clean
@@ -60,26 +58,26 @@ jobs:
           sudo rm -rf /usr/local
           df -h
       - uses: actions/checkout@v2
-      - name: cache upstream_ws
+      - name: Cache upstream workspace
         uses: pat-s/always-upload-cache@v2.1.5
         with:
           path: ${{ env.BASEDIR }}/upstream_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
         env:
-          CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles(env.UPSTREAM_WORKSPACE, '.github/workflows/ci.yaml') }}
+          CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }}
       # The target directory cache doesn't include the source directory because
       # that comes from the checkout.  See "prepare target_ws for cache" task below
-      - name: cache target_ws
-        if: ${{ ! matrix.env.CCOV }}
+      - name: Cache target workspace
+        if: "!matrix.env.CCOV"
         uses: pat-s/always-upload-cache@v2.1.5
         with:
           path: ${{ env.BASEDIR }}/target_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
         env:
           CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }}
-      - name: cache ccache
+      - name: Cache ccache
         uses: pat-s/always-upload-cache@v2.1.5
         with:
           path: ${{ env.CCACHE_DIR }}
@@ -90,27 +88,40 @@ jobs:
         env:
           CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }}
 
-      - name: generate ikfast packages
-        if: ${{ matrix.env.IKFAST_TEST }}
-        run: |
-          moveit_kinematics/test/test_ikfast_plugins.sh
-      - id: industrial_ci
+      - name: Generate ikfast packages
+        if: matrix.env.IKFAST_TEST
+        run: moveit_kinematics/test/test_ikfast_plugins.sh
+      - id: ici
+        name: Run industrial_ci
         uses: ros-industrial/industrial_ci@master
         env: ${{ matrix.env }}
 
-      - name: upload test artifacts (on failure)
+      - name: Upload test artifacts (on failure)
         uses: actions/upload-artifact@v2
-        if: steps.industrial_ci.outcome != 'success'
+        if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
         with:
           name: test-results-${{ matrix.env.IMAGE }}
           path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
-      - name: upload codecov report
-        uses: codecov/codecov-action@v1
-        if: ${{ matrix.env.CCOV }}
+      - name: Generate codecov report
+        uses: rhaschke/lcov-action@main
+        if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
+        with:
+          docker: $DOCKER_IMAGE
+          workdir: ${{ env.BASEDIR }}/target_ws
+          ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
+      - name: Upload codecov report
+        uses: codecov/codecov-action@v2
+        if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
+        with:
+          files: ${{ env.BASEDIR }}/target_ws/coverage.info
+      - name: Upload clang-tidy changes
+        uses: rhaschke/upload-git-patch-action@main
+        if: matrix.env.CLANG_TIDY
         with:
-          files: ${{ env.BASEDIR }}/coverage.info
-      - name: prepare target_ws for cache
-        if: ${{ always() && ! matrix.env.CCOV }}
+          name: clang-tidy
+          path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd))
+      - name: Prepare target_ws for cache
+        if: "!matrix.env.CCOV"
         run: |
           du -sh ${{ env.BASEDIR }}/target_ws
           sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete

diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml
@@ -19,4 +19,10 @@ jobs:
     - uses: actions/setup-python@v2
     - name: Install clang-format-10
       run: sudo apt-get install clang-format-10
-    - uses: pre-commit/action@v2.0.0
+    - uses: pre-commit/action@v2.0.3
+      id: precommit
+    - name: Upload pre-commit changes
+      if: failure() && steps.precommit.outcome == 'failure'
+      uses: rhaschke/upload-git-patch-action@main
+      with:
+        name: pre-commit
diff --git a/moveit2.repos b/moveit2.repos
@@ -15,3 +15,11 @@ repositories:
     type: git
     url: https://github.com/ros-planning/warehouse_ros_mongo
     version: ros2
+  ros2_control:
+    type: git
+    url: https://github.com/ros-controls/ros2_control
+    version: 1.1.0
+  ros2_controllers:
+    type: git
+    url: https://github.com/ros-controls/ros2_controllers
+    version: 1.1.0
diff --git a/moveit2_galactic.repos b/moveit2_galactic.repos
diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
@@ -75,6 +75,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     robot_trajectory/include
     kinematic_constraints/include
     macros/include
+    online_signal_smoothing/include
     planning_interface/include
     planning_request_adapter/include
     planning_scene/include
@@ -87,31 +88,33 @@ set(THIS_PACKAGE_INCLUDE_DIRS
 )
 
 set(THIS_PACKAGE_LIBRARIES
-    moveit_exceptions
     moveit_background_processing
-    moveit_kinematics_base
-    moveit_robot_model
-    moveit_transforms
-    moveit_robot_state
-    moveit_robot_trajectory
-    moveit_planning_interface
+    moveit_butterworth_filter
+    moveit_collision_distance_field
     moveit_collision_detection
     moveit_collision_detection_fcl
     moveit_collision_detection_bullet
+    moveit_dynamics_solver
+    moveit_constraint_samplers
+    moveit_distance_field
+    moveit_exceptions
+    moveit_kinematics_base
     moveit_kinematic_constraints
+    moveit_kinematics_metrics
+    moveit_planning_interface
     moveit_planning_scene
-    moveit_constraint_samplers
     moveit_planning_request_adapter
     moveit_profiler
     # TODO: Port python bindings
     # moveit_python_tools
+    moveit_robot_model
+    moveit_robot_state
+    moveit_robot_trajectory
+    moveit_smoothing_base
+    moveit_test_utils
     moveit_trajectory_processing
-    moveit_distance_field
-    moveit_collision_distance_field
-    moveit_kinematics_metrics
-    moveit_dynamics_solver
+    moveit_transforms
     moveit_utils
-    moveit_test_utils
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -138,9 +141,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   ruckig
 )
 
-pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml)
-pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml)
-
 include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}
   ${LIBFCL_INCLUDE_DIRS}
 )
@@ -158,35 +158,35 @@ message(STATUS " *** Building MoveIt ${MOVEIT_VERSION} ***")
 configure_file("version/version.h.in" "${VERSION_FILE_PATH}/moveit/version.h")
 install(FILES "${VERSION_FILE_PATH}/moveit/version.h" DESTINATION include/moveit)
 
-add_subdirectory(version)
-add_subdirectory(macros)
+add_subdirectory(background_processing)
 add_subdirectory(backtrace)
+add_subdirectory(collision_distance_field)
+add_subdirectory(constraint_samplers)
+add_subdirectory(controller_manager)
+add_subdirectory(distance_field)
+add_subdirectory(dynamics_solver)
 add_subdirectory(exceptions)
+add_subdirectory(kinematics_base)
+add_subdirectory(kinematic_constraints)
+add_subdirectory(kinematics_metrics)
+add_subdirectory(macros)
+add_subdirectory(online_signal_smoothing)
+add_subdirectory(planning_interface)
+add_subdirectory(planning_request_adapter)
+add_subdirectory(planning_scene)
 add_subdirectory(profiler)
-add_subdirectory(utils)
-add_subdirectory(background_processing)
 add_subdirectory(robot_model)
-add_subdirectory(collision_detection)
-add_subdirectory(kinematics_base)
-add_subdirectory(transforms)
 add_subdirectory(robot_state)
-add_subdirectory(collision_detection_fcl)
 add_subdirectory(robot_trajectory)
-add_subdirectory(kinematic_constraints)
-add_subdirectory(trajectory_processing)
-add_subdirectory(planning_scene)
-add_subdirectory(controller_manager)
 add_subdirectory(sensor_manager)
-add_subdirectory(constraint_samplers)
-add_subdirectory(planning_interface)
-add_subdirectory(planning_request_adapter)
-add_subdirectory(distance_field)
-add_subdirectory(collision_distance_field)
-add_subdirectory(kinematics_metrics)
-add_subdirectory(dynamics_solver)
-add_subdirectory(collision_detection_bullet)
+add_subdirectory(trajectory_processing)
+add_subdirectory(transforms)
+add_subdirectory(utils)
+add_subdirectory(version)
 
-pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml)
+add_subdirectory(collision_detection)
+add_subdirectory(collision_detection_bullet)
+add_subdirectory(collision_detection_fcl)
 
 # TODO: Port python bindings
 # add_subdirectory(python)
@@ -228,6 +228,11 @@ install(
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+# Plugin exports
+pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml)
+pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml)
+pluginlib_export_plugin_description_file(moveit_core filter_plugin_butterworth.xml)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
 

diff --git a/moveit_core/filter_plugin_butterworth.xml b/moveit_core/filter_plugin_butterworth.xml
@@ -0,0 +1,7 @@
+<library path="moveit_butterworth_filter">
+  <class type="online_signal_smoothing::ButterworthFilterPlugin" base_class_type="online_signal_smoothing::SmoothingBaseClass">
+    <description>
+    Simple Butterworth 1st order lowpass filter that only has one tuneable parameter and does not overshoot.
+    </description>
+  </class>
+</library>
diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt
@@ -0,0 +1,54 @@
+# Base class
+set(SMOOTHING_BASE_LIB moveit_smoothing_base)
+add_library(${SMOOTHING_BASE_LIB} SHARED
+  src/smoothing_base_class.cpp
+)
+include(GenerateExportHeader)
+generate_export_header(${SMOOTHING_BASE_LIB})
+target_include_directories(${SMOOTHING_BASE_LIB} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
+)
+set_target_properties(${SMOOTHING_BASE_LIB} PROPERTIES VERSION
+  "${${PROJECT_NAME}_VERSION}"
+)
+ament_target_dependencies(${SMOOTHING_BASE_LIB}
+  rclcpp
+)
+
+# Plugin implementations
+set(BUTTERWORTH_FILTER_LIB moveit_butterworth_filter)
+add_library(${BUTTERWORTH_FILTER_LIB} SHARED
+  src/butterworth_filter.cpp
+)
+generate_export_header(${BUTTERWORTH_FILTER_LIB})
+target_include_directories(${BUTTERWORTH_FILTER_LIB} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
+)
+set_target_properties(${BUTTERWORTH_FILTER_LIB} PROPERTIES VERSION
+  "${${PROJECT_NAME}_VERSION}"
+)
+target_link_libraries(${BUTTERWORTH_FILTER_LIB}
+  ${SMOOTHING_BASE_LIB}
+  moveit_robot_model
+)
+ament_target_dependencies(${BUTTERWORTH_FILTER_LIB}
+  srdfdom  # include dependency from moveit_robot_model
+)
+
+# Installation
+
+install(DIRECTORY include/ DESTINATION include)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${SMOOTHING_BASE_LIB}_export.h DESTINATION include)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${BUTTERWORTH_FILTER_LIB}_export.h DESTINATION include)
+
+# Testing
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ros_testing REQUIRED)
+
+  # Lowpass filter unit test
+  ament_add_gtest(test_butterworth_filter test/test_butterworth_filter.cpp)
+  target_link_libraries(test_butterworth_filter ${BUTTERWORTH_FILTER_LIB})
+endif()