Merge branch 'ros2' into pr-add-traj-exec-info-property

moveit · Oct 18, 2023 · 67075c8 · 67075c8
2 parents 883b49a + b25d2ba
commit 67075c8
Show file tree

Hide file tree

Showing 183 changed files with 7,744 additions and 1,688 deletions.
diff --git a/.clang-tidy b/.clang-tidy
@@ -1,5 +1,7 @@
 ---
 Checks:          'performance-*,
+                  -performance-noexcept-move-constructor,
+                  -clang-analyzer-core.CallAndMessage,
                   llvm-namespace-comment,
                   modernize-redundant-void-arg,
                   modernize-use-nullptr,

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -8,42 +8,40 @@ on:
   pull_request:
   push:
 
+permissions:
+  contents: read
+  pages: write
+  id-token: write
+
 jobs:
   default:
     strategy:
       fail-fast: false
       matrix:
         env:
-          # TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers
-          - IMAGE: galactic-source
+          - IMAGE: rolling-source
             NAME: ccov
             TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
           - IMAGE: rolling-source
-            CXXFLAGS: >-
-             -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls
-             -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter
-          - IMAGE: rolling-source
-            CXX: clang++
-            CLANG_TIDY: true
-            CXXFLAGS: >-
-             -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls
-             -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy
-          # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
-          # see https://github.com/google/sanitizers/wiki/AddressSanitizer
+            CLANG_TIDY: pedantic
           - IMAGE: rolling-source
             NAME: asan
+            # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
+            # see https://github.com/google/sanitizers/wiki/AddressSanitizer
+            # Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324
             DOCKER_RUN_OPTS: >-
               -e PRELOAD=libasan.so.5
               -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
-            TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
+              -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0"
+            TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
 
     env:
-      CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
-      DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
+      CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
+      DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UNDERLAY: /root/ws_moveit/install
       # TODO: Port to ROS2
       # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
-      UPSTREAM_WORKSPACE: .repos
+      TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release
       CCACHE_DIR: ${{ github.workspace }}/.ccache
       BASEDIR: ${{ github.workspace }}/.work
       CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
@@ -55,28 +53,52 @@ jobs:
     name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
+        with:
+          submodules: recursive
 
       - name: Cache ccache
-        uses: pat-s/always-upload-cache@v2.1.5
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
           restore-keys: |
             ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
             ccache-${{ env.CACHE_PREFIX }}
+        env:
+          GHA_CACHE_SAVE: always
 
       - id: ici
         name: Run industrial_ci
-        uses: ros-industrial/industrial_ci@master
+        uses: rhaschke/industrial_ci@clang-tidy
         env: ${{ matrix.env }}
 
+      - name: Upload ici's target_ws/install folder
+        uses: rhaschke/upload-ici-workspace@main
+        if: success() && matrix.env.CLANG_TIDY
+        with:
+          subdir: target_ws/install
+
       - name: Upload test artifacts (on failure)
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
         with:
-          name: test-results-${{ matrix.env.IMAGE }}
+          name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && '-clang-tidy' || '' }}
           path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
+
+      - name: Upload clang-tidy fixes (on failure)
+        uses: actions/upload-artifact@v3
+        if: failure() && steps.ici.outputs.clang_tidy_checks
+        with:
+          name: clang-tidy-fixes.yaml
+          path: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml
+
+      - name: Show clang-tidy warnings
+        if: always() && matrix.env.CLANG_TIDY
+        uses: asarium/clang-tidy-action@v1
+        with:
+          fixesFile: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml
+
       - name: Generate codecov report
         uses: rhaschke/lcov-action@main
         if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
@@ -85,7 +107,7 @@ jobs:
           workdir: ${{ env.BASEDIR }}/target_ws
           ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
       - name: Upload codecov report
-        uses: codecov/codecov-action@v2
+        uses: codecov/codecov-action@v3
         if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
         with:
           files: ${{ env.BASEDIR }}/target_ws/coverage.info
diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml
@@ -13,11 +13,12 @@ jobs:
     name: pre-commit
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
-      - uses: actions/setup-python@v2
-      - name: Install clang-format-10
-        run: sudo apt-get install clang-format-10
-      - uses: pre-commit/[email protected]
+      - uses: actions/checkout@v3
+        with:
+          submodules: recursive
+      - name: Install clang-format-12
+        run: sudo apt-get install clang-format-12
+      - uses: pre-commit/[email protected]
         id: precommit
       - name: Upload pre-commit changes
         if: failure() && steps.precommit.outcome == 'failure'

diff --git a/.github/workflows/lsan.suppressions b/.github/workflows/lsan.suppressions
@@ -1 +1,3 @@
 leak:class_loader
+leak:rviz_default_plugins
+leak:static_transform_broadcaster_node
diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml
@@ -0,0 +1,41 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: pre-release
+
+on:
+  workflow_dispatch:
+
+permissions:
+  contents: read # to fetch code (actions/checkout)
+
+jobs:
+  default:
+    strategy:
+      matrix:
+        distro: [noetic]
+
+    env:
+      # https://github.com/ros-industrial/industrial_ci/issues/666
+      BUILDER: catkin_make_isolated
+      ROS_DISTRO: ${{ matrix.distro }}
+      PRERELEASE: true
+      BASEDIR: ${{ github.workspace }}/.work
+
+    name: "${{ matrix.distro }}"
+    runs-on: ubuntu-latest
+    steps:
+      - name: "Free up disk space"
+        run: |
+          sudo apt-get -qq purge build-essential "ghc*"
+          sudo apt-get clean
+          # cleanup docker images not used by us
+          docker system prune -af
+          # free up a lot of stuff from /usr/local
+          sudo rm -rf /usr/local
+          df -h
+      - uses: actions/checkout@v3
+        with:
+          submodules: recursive
+      - name: industrial_ci
+        uses: ros-industrial/industrial_ci@master
diff --git a/.gitignore b/.gitignore
@@ -1 +1,3 @@
 *.swp
+*.pyc
+__pycache__/
diff --git a/.gitmodules b/.gitmodules
@@ -0,0 +1,5 @@
+[submodule "core/python/pybind11"]
+	path = core/python/pybind11
+	url = https://github.com/pybind/pybind11
+	branch = smart_holder
+	shallow = true
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -15,7 +15,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v3.4.0
+    rev: v4.0.1
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
@@ -32,13 +32,14 @@ repos:
     rev: 22.3.0
     hooks:
       - id: black
+        args: ["--line-length", "100"]
 
   - repo: local
     hooks:
       - id: clang-format
         name: clang-format
         description: Format files with ClangFormat.
-        entry: clang-format-10
+        entry: clang-format-12
         language: system
         files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
-        args: ['-fallback-style=none', '-i']
+        args: ["-fallback-style=none", "-i"]
diff --git a/.repos b/.repos
diff --git a/README.md b/README.md
@@ -5,9 +5,14 @@ It draws on the planning capabilities of [MoveIt](https://moveit.ros.org/) to so
 A common interface, based on MoveIt's PlanningScene is used to pass solution hypotheses between stages.
 The framework enables the hierarchical organization of basic stages using *containers*, allowing for sequential as well as parallel compositions.
 
-## Video
+## Videos
 
-[![Video associated with ICRA 2019 paper](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg )](https://www.youtube.com/watch?v=fCORKVYsdDI)
+- Demo video associated with [ICRA 2019 paper](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf)
+
+  [![](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg)](https://www.youtube.com/watch?v=fCORKVYsdDI)
+
+- [Presentation @ ROSCon 2018 (Madrid)](https://vimeo.com/293432325)
+- [Presentation @ MoveIt workshop 2019 (Macau)](https://www.youtube.com/watch?v=a8r7O2bs1Mc)
 
 ## Tutorial
 
@@ -32,10 +37,9 @@ Ideas and requests for other interesting/useful features are welcome.
 
 If you use this framework in your project, please cite the associated paper:
 
-
 Michael Görner*, Robert Haschke*, Helge Ritter, and Jianwei Zhang,
-MoveIt! Task Constructor for Task-Level Motion Planning,
-International Conference on Robotics and Automation, ICRA 2019, Montreal, Canada.
+"MoveIt! Task Constructor for Task-Level Motion Planning",
+_International Conference on Robotics and Automation (ICRA)_, 2019, Montreal, Canada.
 [[DOI]](https://doi.org/10.1109/ICRA.2019.8793898) [[PDF]](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf).
 
 

diff --git a/capabilities/CHANGELOG.rst b/capabilities/CHANGELOG.rst
@@ -0,0 +1,17 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package moveit_task_constructor_capabilities
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.3 (2023-03-06)
+------------------
+
+0.1.2 (2023-02-24)
+------------------
+
+0.1.1 (2023-02-15)
+------------------
+
+0.1.0 (2023-02-02)
+------------------
+* Initial release
+* Contributors: Michael Görner, Robert Haschke
diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt
@@ -1,16 +1,16 @@
 cmake_minimum_required(VERSION 3.5)
 project(moveit_task_constructor_capabilities)
 
-add_compile_options(-std=c++17)
-
 find_package(ament_cmake REQUIRED)
 
 find_package(Boost REQUIRED)
-find_package(rclcpp_action REQUIRED)
+find_package(moveit_common REQUIRED)
+moveit_package()
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_move_group REQUIRED)
 find_package(moveit_task_constructor_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
@@ -29,10 +29,10 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
-ament_export_dependencies(rclcpp_action)
 ament_export_dependencies(moveit_core)
 ament_export_dependencies(moveit_ros_move_group)
 ament_export_dependencies(moveit_task_constructor_msgs)
 ament_export_dependencies(pluginlib)
+ament_export_dependencies(rclcpp_action)
 ament_export_dependencies(std_msgs)
 ament_package()
diff --git a/capabilities/package.xml b/capabilities/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
 	<name>moveit_task_constructor_capabilities</name>
-	<version>0.0.0</version>
+	<version>0.1.3</version>
 	<description>
 	MoveGroupCapabilites to interact with MoveIt
 	</description>
@@ -15,11 +15,11 @@
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_move_group</depend>
 	<depend>moveit_ros_planning</depend>
-	<depend>rclcpp_action</depend>
-	<depend>pluginlib</depend>
-	<depend>std_msgs</depend>
 	<depend>moveit_task_constructor_core</depend>
 	<depend>moveit_task_constructor_msgs</depend>
+	<depend>pluginlib</depend>
+	<depend>rclcpp_action</depend>
+	<depend>std_msgs</depend>
 
 	<export>
 		<build_type>ament_cmake</build_type>

diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -99,7 +99,7 @@ void ExecuteTaskSolutionCapability::initialize() {
 }
 
 void ExecuteTaskSolutionCapability::goalCallback(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
 	auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();
 
 	const auto& goal = goal_handle->get_goal();
@@ -126,7 +126,7 @@ void ExecuteTaskSolutionCapability::goalCallback(
 }
 
 rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& /*goal_handle*/) {
 	if (context_->plan_execution_)
 		context_->plan_execution_->stop();
 	return rclcpp_action::CancelResponse::ACCEPT;
@@ -142,16 +142,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		state = scene->getCurrentState();
 	}
 
-	plan.plan_components_.reserve(solution.sub_trajectory.size());
+	plan.plan_components.reserve(solution.sub_trajectory.size());
 	for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) {
 		const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i];
 
-		plan.plan_components_.emplace_back();
-		plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
+		plan.plan_components.emplace_back();
+		plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components.back();
 
 		// define individual variable for use in closure below
 		const std::string description = std::to_string(i + 1) + "/" + std::to_string(solution.sub_trajectory.size());
-		exec_traj.description_ = description;
+		exec_traj.description = description;
 
 		const moveit::core::JointModelGroup* group = nullptr;
 		{
@@ -173,8 +173,8 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		exec_traj.controller_names_ = sub_traj.execution_info.controller_names;
 
 		/* TODO add action feedback and markers */
-		exec_traj.effect_on_success_ = [this, sub_traj,
-		                                description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
+		exec_traj.effect_on_success = [this, sub_traj,
+		                               description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
 			if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
 				RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
 				return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
@@ -194,5 +194,5 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 
 }  // namespace move_group
 
-#include <class_loader/class_loader.hpp>
-CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)