diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile deleted file mode 100644 index 45c18e07d6..0000000000 --- a/.docker/ci-testing/Dockerfile +++ /dev/null @@ -1,68 +0,0 @@ -# ghcr.io/ros-planning/moveit2:${OUR_ROS_DISTRO}-ci-testing -# CI image using the ROS testing repository - -FROM osrf/ros2:testing -LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de - -ENV TERM xterm - -# Overwrite the ROS_DISTRO set in osrf/ros2:testing to the distro tied to this Dockerfile (OUR_ROS_DISTRO). -# In case ROS_DISTRO is now different from what was set in osrf/ros2:testing, run `rosdep update` again -# to get any missing dependencies. -# https://docs.docker.com/engine/reference/builder/#using-arg-variables explains why ARG and ENV can't have -# the same name (ROS_DISTRO is an ENV in the osrf/ros2:testing image). -ARG OUR_ROS_DISTRO=rolling -ENV ROS_DISTRO=${OUR_ROS_DISTRO} -RUN rosdep update --rosdistro $ROS_DISTRO - -# Install ROS 2 base packages and build tools -# We are installing ros--ros-base here to mimic the behavior of the ros:-ros-base images. -# This step is split into a separate layer so that we can rely on cached dependencies instead of having -# to install them with every new build. The testing image and packages will only update every couple weeks. -RUN \ - # Update apt package list as previous containers clear the cache - apt-get -q update && \ - apt-get -q -y upgrade && \ - # - # Install base dependencies - apt-get -q install --no-install-recommends -y \ - # Some basic requirements - wget git sudo curl \ - # Preferred build tools - clang clang-format-14 clang-tidy clang-tools \ - ccache \ - ros-"$ROS_DISTRO"-ros-base && \ - # - # Clear apt-cache to reduce image size - rm -rf /var/lib/apt/lists/* - -# 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 -q update && \ - apt-get -q -y upgrade && \ - # - # Globally disable git security - # https://github.blog/2022-04-12-git-security-vulnerability-announced - git config --global --add safe.directory "*" && \ - # - # 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 && \ - # - # Download all dependencies of MoveIt - rosdep update && \ - DEBIAN_FRONTEND=noninteractive \ - rosdep install -y --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" --as-root=apt:false && \ - # Remove the source code from this container - rm -rf src && \ - # - # Clear apt-cache to reduce image size - rm -rf /var/lib/apt/lists/* diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index c6aa5ee42c..994d1bfe89 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -6,6 +6,8 @@ FROM ros:${ROS_DISTRO}-ros-base LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de ENV TERM xterm +# Allow non-interactive installation of ros-humble-rmw-connextdds +ENV RTI_NC_LICENSE_ACCEPTED yes # Setup (temporary) ROS workspace WORKDIR /root/ws_moveit diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 8a1b6992c4..f95c3cc86a 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -5,6 +5,9 @@ ARG ROS_DISTRO=rolling FROM ros:${ROS_DISTRO}-ros-base LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de +# Allow non-interactive installation of ros-humble-rmw-connextdds +ENV RTI_NC_LICENSE_ACCEPTED yes + # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size RUN apt-get update -q && \ apt-get upgrade -q -y && \ diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 2b9082885e..d665d02ab2 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -4,7 +4,7 @@ # Downloads the moveit source code and install remaining debian dependencies ARG ROS_DISTRO=rolling -FROM moveit/moveit2:${ROS_DISTRO}-ci-testing +FROM moveit/moveit2:${ROS_DISTRO}-ci LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de # Export ROS_UNDERLAY for downstream docker containers diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile new file mode 100644 index 0000000000..11c0ae738e --- /dev/null +++ b/.docker/tutorial-source/Dockerfile @@ -0,0 +1,43 @@ +# syntax = docker/dockerfile:1.3 + +# ghcr.io/moveit/moveit2:main-${ROS_DISTRO}-tutorial-source +# Source build of the repos file from the tutorial site + +ARG ROS_DISTRO=humble + +FROM moveit/moveit2:${ROS_DISTRO}-ci +LABEL maintainer Tyler Weaver tyler@picknik.ai + +# Export ROS_UNDERLAY for downstream docker containers +ENV ROS_UNDERLAY /root/ws_moveit/install +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 --mount=type=cache,target=/root/.ccache/,sharing=locked \ + # Enable ccache + PATH=/usr/lib/ccache:$PATH && \ + # Checkout the tutorial repo + git clone -b ${ROS_DISTRO} https://github.com/moveit/moveit2_tutorials src/moveit2_tutorials && \ + # Fetch required upstream sources for building + vcs import --skip-existing src < src/moveit2_tutorials/moveit2_tutorials.repos && \ + # Source ROS install + . "/opt/ros/${ROS_DISTRO}/setup.sh" &&\ + # Install dependencies from rosdep + apt-get -q update && \ + rosdep update && \ + DEBIAN_FRONTEND=noninteractive \ + rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ + rm -rf /var/lib/apt/lists/* && \ + # Build the workspace + colcon build \ + --cmake-args "-DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --no-warn-unused-cli" \ + --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 diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8eb1919ebe..a8fb537b51 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -6,6 +6,7 @@ name: CI on: workflow_dispatch: pull_request: + merge_group: push: branches: - humble @@ -19,7 +20,7 @@ jobs: - IMAGE: humble-ci CCOV: true ROS_DISTRO: humble - - IMAGE: humble-ci-testing + - IMAGE: humble-ci ROS_DISTRO: humble IKFAST_TEST: true CLANG_TIDY: pedantic @@ -37,9 +38,6 @@ jobs: $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f) # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src - # Uninstall binaries that are duplicated in the .repos file - # TODO(andyz): remove this once a sync containing 35b93c8 happens - AFTER_SETUP_UPSTREAM_WORKSPACE_EMBED: sudo apt remove ros-${{ matrix.env.ROS_DISTRO }}-moveit-msgs -y AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src # Clear the ccache stats before and log the stats after the build AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G @@ -52,7 +50,7 @@ jobs: -DCMAKE_SHARED_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_MODULE_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} - -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" + -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer' || ''}}" UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''" DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache @@ -63,6 +61,7 @@ jobs: (cd $TARGET_REPO_PATH; clang-tidy --list-checks) # Disable clang-tidy for ikfast plugins as we cannot fix the generated code find $BASEDIR/target_ws/build -iwholename "*_ikfast_plugin/compile_commands.json" -exec rm {} \; + find $BASEDIR/target_ws/build -iwholename "*_ikfast_manipulator_plugin/compile_commands.json" -exec rm {} \; CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} ADDITIONAL_DEBS: lld @@ -73,7 +72,7 @@ jobs: - name: "Free up disk space" if: matrix.env.CCOV run: | - sudo apt-get -qq purge build-essential "ghc*" + sudo apt-get -qq purge "ghc*" sudo apt-get clean # cleanup docker images not used by us docker system prune -af @@ -84,11 +83,13 @@ jobs: # free up a lot of stuff from /usr/local sudo rm -rf /usr/local df -h - - uses: actions/checkout@v3 - - uses: testspace-com/setup-testspace@v1 - if: github.repository == 'ros-planning/moveit2' - with: - domain: ros-planning + - uses: actions/checkout@v4 +# NOTE: Testspace is temporarily disabled and needs to be installed for the MoveIt org +# See: https://github.com/moveit/moveit2/issues/2852 +# - uses: testspace-com/setup-testspace@v1 +# if: github.repository == 'moveit/moveit2' +# with: +# domain: moveit - name: Get latest release date for rosdistro id: rosdistro_release_date uses: JafarAbdi/latest-rosdistro-release-date-action@main @@ -100,26 +101,28 @@ jobs: with: file: moveit2.repos - name: Cache upstream workspace - uses: pat-s/always-upload-cache@v3.0.11 + uses: rhaschke/cache@main with: path: ${{ env.BASEDIR }}/upstream_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} env: + GHA_CACHE_SAVE: always CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-${{ 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 workspace if: "!matrix.env.CCOV" - uses: pat-s/always-upload-cache@v3.0.11 + uses: rhaschke/cache@main with: path: ${{ env.BASEDIR }}/target_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} env: + GHA_CACHE_SAVE: always CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }} - name: Cache ccache - uses: pat-s/always-upload-cache@v3.0.11 + uses: rhaschke/cache@main with: path: ${{ env.CCACHE_DIR }} key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} @@ -127,6 +130,7 @@ jobs: ${{ env.CACHE_PREFIX }}-${{ github.sha }} ${{ env.CACHE_PREFIX }} env: + GHA_CACHE_SAVE: always CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }} - name: Configure ccache run: | @@ -139,12 +143,14 @@ jobs: name: Run industrial_ci uses: ros-industrial/industrial_ci@master env: ${{ matrix.env }} - - name: Push result to Testspace - if: always() && (github.repository == 'ros-planning/moveit2') - run: | - testspace "[ ${{ matrix.env.IMAGE }} ]${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml" +# NOTE: Testspace is temporarily disabled and needs to be installed for the MoveIt org +# See: https://github.com/moveit/moveit2/issues/2852 +# - name: Push result to Testspace +# if: always() && (github.repository == 'moveit/moveit2') +# run: | +# testspace "[ ${{ matrix.env.IMAGE }} ]${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml" - name: Upload test artifacts (on failure) - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: name: test-results-${{ matrix.env.IMAGE }} @@ -157,7 +163,7 @@ jobs: workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v3 + uses: codecov/codecov-action@v5 if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 55e382cac3..18c1c2588e 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -25,33 +25,45 @@ jobs: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: + - uses: rhaschke/docker-run-action@v5 + name: Check for apt updates + continue-on-error: true + id: apt + with: + image: ${{ env.IMAGE }} + run: | + apt-get update + have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) + echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Login to Github Container Registry if: env.PUSH == 'true' - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ghcr.io username: ${{ github.repository_owner }} password: ${{ secrets.GITHUB_TOKEN }} - name: Login to DockerHub if: env.PUSH == 'true' - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v6 with: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} - no-cache: true + no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} + cache-from: type=registry,ref=${{ env.GH_IMAGE }} + cache-to: type=inline tags: | ${{ env.GH_IMAGE }} ${{ env.DH_IMAGE }} @@ -66,80 +78,53 @@ jobs: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v2 + - uses: rhaschke/docker-run-action@v5 + name: Check for apt updates + continue-on-error: true + id: apt with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v2 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build and Push - uses: docker/build-push-action@v4 - with: - file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - push: ${{ env.PUSH }} - no-cache: true - tags: | - ${{ env.GH_IMAGE }} - ${{ env.DH_IMAGE }} - - ci-testing: - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] - runs-on: ubuntu-latest - permissions: - packages: write - contents: read - env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} - - steps: + image: ${{ env.IMAGE }} + run: | + apt-get update + have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) + echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Login to Github Container Registry if: env.PUSH == 'true' - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ghcr.io username: ${{ github.repository_owner }} password: ${{ secrets.GITHUB_TOKEN }} - name: Login to DockerHub if: env.PUSH == 'true' - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v6 with: file: .docker/${{ github.job }}/Dockerfile - build-args: OUR_ROS_DISTRO=${{ matrix.ROS_DISTRO }} + build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} - no-cache: true + no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} + cache-from: type=registry,ref=${{ env.GH_IMAGE }} + cache-to: type=inline tags: | ${{ env.GH_IMAGE }} + ${{ env.GH_IMAGE }}-testing ${{ env.DH_IMAGE }} + ${{ env.DH_IMAGE }}-testing source: - needs: ci-testing + needs: ci strategy: fail-fast: false matrix: @@ -149,37 +134,38 @@ jobs: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Login to Github Container Registry if: env.PUSH == 'true' - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ghcr.io username: ${{ github.repository_owner }} password: ${{ secrets.GITHUB_TOKEN }} - name: Login to DockerHub if: env.PUSH == 'true' - uses: docker/login-action@v2 + uses: docker/login-action@v3 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@v4 + uses: docker/build-push-action@v6 with: context: . file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} - no-cache: true + cache-from: type=registry,ref=${{ env.GH_IMAGE }} + cache-to: type=inline tags: | ${{ env.GH_IMAGE }} ${{ env.DH_IMAGE }} @@ -191,8 +177,8 @@ jobs: - source steps: - name: Delete Untagged Images - if: (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') - uses: actions/github-script@v6 + if: (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') + uses: actions/github-script@v7 with: github-token: ${{ secrets.DELETE_PACKAGES_TOKEN }} script: | @@ -207,6 +193,6 @@ jobs: } } env: - OWNER: ros-planning + OWNER: moveit PACKAGE_NAME: moveit2 PER_PAGE: 100 diff --git a/.github/workflows/docker_lint.yaml b/.github/workflows/docker_lint.yaml index 92a1f169db..14c1cfc363 100644 --- a/.github/workflows/docker_lint.yaml +++ b/.github/workflows/docker_lint.yaml @@ -16,13 +16,13 @@ jobs: strategy: fail-fast: false matrix: - DOCKERFILE_PATH: [ci, ci-testing, release, source] + DOCKERFILE_PATH: [ci, release, source] name: Lint Dockerfiles runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 - - uses: hadolint/hadolint-action@v3.0.0 + - uses: actions/checkout@v4 + - uses: hadolint/hadolint-action@v3.1.0 with: dockerfile: .docker/${{ matrix.DOCKERFILE_PATH }}/Dockerfile config: .docker/.hadolint.yaml diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 5cda0c0bf1..44537cde76 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -15,8 +15,8 @@ jobs: name: Format runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v4 + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 with: python-version: '3.10' - name: Install clang-format-12 diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index c4d3c37c13..9eb0dbf3f4 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -21,6 +21,6 @@ jobs: name: "${{ matrix.distro }}" runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: industrial_ci uses: ros-industrial/industrial_ci@master diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml new file mode 100644 index 0000000000..c922f1a995 --- /dev/null +++ b/.github/workflows/tutorial_docker.yaml @@ -0,0 +1,95 @@ +name: "Tutorial Docker Images" + +on: + schedule: + # 5 PM UTC every Sunday + - cron: '0 17 * * 6' + workflow_dispatch: + pull_request: + merge_group: + push: + branches: + - humble + +jobs: + tutorial-source: + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + runs-on: ubuntu-latest + permissions: + packages: write + contents: read + env: + GH_IMAGE: ghcr.io/moveit/moveit2:humble-${{ matrix.ROS_DISTRO }}-${{ github.job }} + DH_IMAGE: moveit/moveit2:humble-${{ matrix.ROS_DISTRO }}-${{ github.job }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} + + steps: + - uses: actions/checkout@v4 + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + - name: Login to Github Container Registry + if: env.PUSH == 'true' + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Login to DockerHub + if: env.PUSH == 'true' + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: "Remove .dockerignore" + run: rm .dockerignore # enforce full source context + - name: Cache ccache + uses: actions/cache@v4 + with: + path: .ccache + key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }} + - name: inject ccache into docker + uses: reproducible-containers/buildkit-cache-dance@v3.1.2 + with: + cache-source: .ccache + cache-target: /root/.ccache/ + - name: Build and Push + uses: docker/build-push-action@v6 + with: + context: . + file: .docker/${{ github.job }}/Dockerfile + build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} + push: ${{ env.PUSH }} + cache-from: type=gha + cache-to: type=gha,mode=max + tags: | + ${{ env.GH_IMAGE }} + ${{ env.DH_IMAGE }} + + delete_untagged: + runs-on: ubuntu-latest + needs: + - tutorial-source + steps: + - name: Delete Untagged Images + if: (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') + uses: actions/github-script@v7 + with: + github-token: ${{ secrets.DELETE_PACKAGES_TOKEN }} + script: | + const response = await github.request("GET /orgs/${{ env.OWNER }}/packages/container/${{ env.PACKAGE_NAME }}/versions", { + per_page: ${{ env.PER_PAGE }} + }); + for(version of response.data) { + if (version.metadata.container.tags.length == 0) { + console.log("delete " + version.id) + const deleteResponse = await github.request("DELETE /orgs/${{ env.OWNER }}/packages/container/${{ env.PACKAGE_NAME }}/versions/" + version.id, { }); + console.log("status " + deleteResponse.status) + } + } + env: + OWNER: moveit + PACKAGE_NAME: moveit2 + PER_PAGE: 100 diff --git a/.gitignore b/.gitignore index c67054706a..e960e3115b 100644 --- a/.gitignore +++ b/.gitignore @@ -51,7 +51,7 @@ qtcreator-* # Vim *.swp -# Continous Integration +# Continuous Integration .moveit_ci *.pyc diff --git a/README.md b/README.md index 83ada97bd8..aa521f6ac9 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ## Getting Started -See our extensive [Tutorials and Documentation](https://moveit.picknik.ai/) +See our extensive [Tutorials and Documentation](https://moveit.picknik.ai/). ## Install @@ -36,7 +36,7 @@ This open source project is maintained by supporters from around the world — s [PickNik Inc](https://picknik.ai/) is leading the development of MoveIt. -If you would like to support this project, please contact hello@picknik.ai +If you would like to support this project, please contact hello@picknik.ai. The port to ROS 2 was supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. -More information: rosin-project.eu +More information: rosin-project.eu. eu_flag @@ -53,7 +53,7 @@ This project has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement no. 732287. ## Generate API Doxygen Documentation -See [How To Generate API Doxygen Reference Locally](https://moveit.picknik.ai/main/doc/how_to_guides/how_to_generate_api_doxygen_locally.html) +See [How To Generate API Doxygen Reference Locally](https://moveit.picknik.ai/main/doc/how_to_guides/how_to_generate_api_doxygen_locally.html). # Buildfarm | MoveIt Package | Foxy Binary | Galactic Binary | Rolling Binary | Humble Binary | diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index e94e625a3d..2010521b8c 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit/package.xml b/moveit/package.xml index 50f0e8b4a1..d00371f961 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -2,7 +2,7 @@ moveit - 2.5.4 + 2.5.6 Meta package that contains all essential packages of MoveIt 2 Henning Kayser Tyler Weaver diff --git a/moveit2.repos b/moveit2.repos index 0d855cfcf2..6c9f2668e4 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -9,7 +9,3 @@ repositories: type: git url: https://github.com/ros-planning/moveit_resources.git version: humble - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: 2.13.0 diff --git a/moveit_common/CHANGELOG.rst b/moveit_common/CHANGELOG.rst index ed31514812..a8502cb254 100644 --- a/moveit_common/CHANGELOG.rst +++ b/moveit_common/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Add `-Wunused-parameter` (`#1756 `_) (`#1757 `_) + (cherry picked from commit be474ec5ba6d0210379d009d518bdd631cc46ad9) + Co-authored-by: Chris Thrasher +* Add `-Wunused-function` (`#1754 `_) (`#1755 `_) + (cherry picked from commit ed9c3317bc1335b66afb0b2e7478b95ddb5c4b33) + Co-authored-by: Chris Thrasher +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_common/package.xml b/moveit_common/package.xml index 809c1346c4..72cc2d2ff8 100644 --- a/moveit_common/package.xml +++ b/moveit_common/package.xml @@ -2,7 +2,7 @@ moveit_common - 2.5.4 + 2.5.6 Common support functionality used throughout MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_configs_utils/CHANGELOG.rst b/moveit_configs_utils/CHANGELOG.rst index 9b4d28b3b0..3f74abfacd 100644 --- a/moveit_configs_utils/CHANGELOG.rst +++ b/moveit_configs_utils/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package moveit_configs_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* fix move_group_capabilities usage (`#3018 `_) (`#3033 `_) +* Backport of `#2172 `_ and `#2684 `_ into Humble (`#2779 `_) +* Use different packages for launch and config packages in generate_demo_launch (backport `#2647 `_) (`#2650 `_) +* Pass along move_group_capabilities parameters (`#2587 `_) (`#2696 `_) +* Use $DISPLAY rather than assuming :0 (`#2049 `_) (`#2365 `_) +* Contributors: Michael Ferguson, Anthony Baker, Alex Navarro, Forrest Rogers-Marcovitz, Stephanie Eng, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Do not add Pilz parameters to MoveIt Configs Utils if Pilz is not used (`#1583 `_) (`#2174 `_) + (cherry picked from commit 1c7fa52edeef08bf8eb1e9cc73c1b0835aaf17e7) + Co-authored-by: Stephanie Eng +* Update default planning configs to use AddTimeOptimalParameterization (`#2167 `_) (`#2170 `_) + (cherry picked from commit 895e9268bd5d9337bebdede07a7f68a99055a1df) + Co-authored-by: Anthony Baker +* Add xacro subsititution class and use it for loading urdf & srdf (backport `#1805 `_) (`#1937 `_) + * Add xacro subsititution class and use it for loading urdf & srdf (`#1805 `_) + * Add Xacro substitution type + * Use Xacro substitution for robot description and robot description semantic + * Install subsititution folder + * Default to load_xacro if there's no launch substitution specified in the mappings + (cherry picked from commit 4bc83c3c9e6bfa9efea8c431794a630fbf27dddc) + # Conflicts: + # moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py + * Fix merge conflicts + --------- + Co-authored-by: Jafar + Co-authored-by: Tyler Weaver +* Add support for multiple MoveItConfigBuilder instaces (`#1807 `_) (`#1808 `_) + (cherry picked from commit 25d086cee9a7cf1c95a15ea12a27e5b7cbe50a1f) + Co-authored-by: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Use MoveItConfigsBuilder in Pilz test launch file (`#1571 `_) (`#1662 `_) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index 40b08d8f19..17ec72eb02 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -1,3 +1,5 @@ +import os + from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -57,6 +59,7 @@ def generate_moveit_rviz_launch(moveit_config): rviz_parameters = [ moveit_config.planning_pipelines, moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ] add_debuggable_node( @@ -195,9 +198,19 @@ def generate_move_group_launch(moveit_config): DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True) ) # load non-default MoveGroup capabilities (space separated) - ld.add_action(DeclareLaunchArgument("capabilities", default_value="")) + ld.add_action( + DeclareLaunchArgument( + "capabilities", + default_value=moveit_config.move_group_capabilities["capabilities"], + ) + ) # inhibit these default MoveGroup capabilities (space separated) - ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value="")) + ld.add_action( + DeclareLaunchArgument( + "disable_capabilities", + default_value=moveit_config.move_group_capabilities["disable_capabilities"], + ) + ) # do not copy dynamics information from /joint_states to internal robot monitoring # default to false, because almost nothing in move_group relies on this information @@ -237,15 +250,17 @@ def generate_move_group_launch(moveit_config): parameters=move_group_params, extra_debug_args=["--debug"], # Set the display variable, in case OpenGL code is used internally - additional_env={"DISPLAY": ":0"}, + additional_env={"DISPLAY": os.environ["DISPLAY"]}, ) return ld -def generate_demo_launch(moveit_config): +def generate_demo_launch(moveit_config, launch_package_path=None): """ Launches a self contained demo + launch_package_path is optional to use different launch and config packages + Includes * static_virtual_joint_tfs * robot_state_publisher @@ -254,6 +269,9 @@ def generate_demo_launch(moveit_config): * warehouse_db (optional) * ros2_control_node + controller spawners """ + if launch_package_path == None: + launch_package_path = moveit_config.package_path + ld = LaunchDescription() ld.add_action( DeclareBooleanLaunchArg( @@ -270,11 +288,11 @@ def generate_demo_launch(moveit_config): ) ) ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True)) - # If there are virtual joints, broadcast static tf by including virtual_joints launch virtual_joints_launch = ( - moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" + launch_package_path / "launch/static_virtual_joint_tfs.launch.py" ) + if virtual_joints_launch.exists(): ld.add_action( IncludeLaunchDescription( @@ -286,7 +304,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/rsp.launch.py") + str(launch_package_path / "launch/rsp.launch.py") ), ) ) @@ -294,7 +312,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/move_group.launch.py") + str(launch_package_path / "launch/move_group.launch.py") ), ) ) @@ -303,7 +321,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/moveit_rviz.launch.py") + str(launch_package_path / "launch/moveit_rviz.launch.py") ), condition=IfCondition(LaunchConfiguration("use_rviz")), ) @@ -313,7 +331,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/warehouse_db.launch.py") + str(launch_package_path / "launch/warehouse_db.launch.py") ), condition=IfCondition(LaunchConfiguration("db")), ) @@ -334,7 +352,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/spawn_controllers.launch.py") + str(launch_package_path / "launch/spawn_controllers.launch.py") ), ) ) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 9c3636fb34..2778c916e0 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -108,7 +108,9 @@ class MoveItConfigs: # A dictionary that has the sensor 3d configuration parameters. sensors_3d: Dict = field(default_factory=dict) # A dictionary containing move_group's non-default capabilities. - move_group_capabilities: Dict = field(default_factory=dict) + move_group_capabilities: Dict = field( + default_factory=lambda: {"capabilities": "", "disable_capabilities": ""} + ) # A dictionary containing the overridden position/velocity/acceleration limits. joint_limits: Dict = field(default_factory=dict) # A dictionary containing MoveItCpp related parameters. @@ -162,6 +164,7 @@ def __init__( self.__urdf_package = None self.__urdf_file_path = None + self.__urdf_xacro_args = None self.__srdf_file_path = None modified_urdf_path = Path("config") / (self.__robot_name + ".urdf.xacro") @@ -172,15 +175,20 @@ def __init__( if setup_assistant_file.exists(): setup_assistant_yaml = load_yaml(setup_assistant_file) config = setup_assistant_yaml.get("moveit_setup_assistant_config", {}) - urdf_config = config.get("urdf", config.get("URDF")) - if urdf_config and self.__urdf_package is None: - self.__urdf_package = Path( - get_package_share_directory(urdf_config["package"]) - ) - self.__urdf_file_path = Path(urdf_config["relative_path"]) - srdf_config = config.get("srdf", config.get("SRDF")) - if srdf_config: + if urdf_config := config.get("urdf", config.get("URDF")): + if self.__urdf_package is None: + self.__urdf_package = Path( + get_package_share_directory(urdf_config["package"]) + ) + self.__urdf_file_path = Path(urdf_config["relative_path"]) + + if xacro_args := urdf_config.get("xacro_args"): + self.__urdf_xacro_args = dict( + arg.split(":=") for arg in xacro_args.split(" ") if arg + ) + + if srdf_config := config.get("srdf", config.get("SRDF")): self.__srdf_file_path = Path(srdf_config["relative_path"]) if not self.__urdf_package or not self.__urdf_file_path: @@ -224,7 +232,8 @@ def robot_description( try: self.__moveit_configs.robot_description = { self.__robot_description: load_xacro( - robot_description_file_path, mappings=mappings + robot_description_file_path, + mappings=mappings or self.__urdf_xacro_args, ) } except ParameterBuilderFileNotFoundError as e: diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index a42ba2c6b4..bd0e0f88b7 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -2,7 +2,7 @@ moveit_configs_utils - 2.5.4 + 2.5.6 Python library for loading moveit config parameters in launch files MoveIt Release Team BSD diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 60188f14cc..06f9f5ea16 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version="2.5.3", + version="2.5.6", packages=find_packages(), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index 41b7bf9c4d..07d1cdc625 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,94 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Allow RobotState::setFromIK to work with subframes (backport `#3077 `_) (`#3085 `_) +* Fixes flaky RobotState test (backport `#3105 `_) (`#3107 `_) +* Fix flipped comments in `joint_model.h` (`#3047 `_) (`#3049 `_) +* Fix RobotState::getRigidlyConnectedParentLinkModel() (`#2985 `_, `#2993 `_) +* Backport to Humble: Pass more distance information out from FCL collision check `#2572 `_ (`#2979 `_) +* Fix doc reference to non-existent function (`#2765 `_) (`#2766 `_) +* Update moveit::core::error_msg_to_string (`#2689 `_) +* [TOTG] Exit loop when position can't change (backport `#2307 `_) (`#2646 `_) +* Fix angular distance calculation in floating joint model (backport `#2538 `_) (`#2543 `_) +* Change `collision_detection_bullet` install path back to `include/moveit` (`#2403 `_) +* Use find_package for fcl (backport `#2399 `_) (`#2400 `_) +* Contributors: Chris Schindlbeck, Gabriel Pacheco, Nacho Mellado, Tom Noble, Sebastian Castro, reidchristopher, Robert Haschke, Henning Kayser, Tyler Weaver, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Don't use ament_export_targets from package sub folder (backport `#1889 `_) (`#1893 `_) +* Fix comment formatting (`#2276 `_) (`#2278 `_) + (cherry picked from commit 83892d6a7cb2f84485ebd96d41adb3acd8c44bee) + Co-authored-by: Stephanie Eng +* fix for kinematic constraints parsing (`#2267 `_) (`#2268 `_) + (cherry picked from commit b0f0f680c3f86b8074d208a1e78c92cfa75cf5ca) + Co-authored-by: Jorge Nicho +* Added butterworth_filter_coeff parameter (`#2129 `_) + * Added butterworth_filter_coeff parameter + * Added formating like in original PR `#2091 `_ + * Update moveit_core/online_signal_smoothing/src/butterworth_filter.cpp + Co-authored-by: AndyZe + * Update moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h + Co-authored-by: AndyZe + * Alphabetized dependencies + * Update moveit_core/package.xml + Co-authored-by: AndyZe + --------- + Co-authored-by: andrey <> + Co-authored-by: AndyZe +* Fix ci-testing build issues (backport `#1998 `_) (`#2002 `_) +* Fix invalid case style for private member in RobotTrajectory + (cherry picked from commit 31e07d3d6a6c1d59bca5876cc0acc51abb960997) +* Fix unreachable child logger instance + (cherry picked from commit 1323d05c89a8815450f8f4edf7a1d7b520871d18) +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Switch to clang-format-14 (`#1877 `_) (`#1880 `_) + * Switch to clang-format-14 + * Fix clang-format-14 + (cherry picked from commit 7fa5eaf1ac21ab8a99c5adae53bd0a2d4abf98f6) + Co-authored-by: Henning Kayser +* Cleanup msg includes: Use C++ instead of C header (backport `#1844 `_) + * Cleanup msg includes: Use C++ instead of C header + * Remove obsolete include: moveit_msgs/srv/execute_known_trajectory.hpp +* Fix moveit_core dependency on tf2_kdl (`#1817 `_) (`#1823 `_) + This is a proper dependency, and not only a test dependency. It is still + needed when building moveit_core with -DBUILD_TESTING=OFF. + (cherry picked from commit 9f7d6df9cac9b55d10f6fee6c29e41ff1d1bf44c) + Co-authored-by: Scott K Logan +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Add `-Wunused-function` (`#1754 `_) (`#1755 `_) + (cherry picked from commit ed9c3317bc1335b66afb0b2e7478b95ddb5c4b33) + Co-authored-by: Chris Thrasher +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, Henning Kayser, Robert Haschke, andrey-pr, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Free functions for calculating properties of trajectories (`#1503 `_) (`#1657 `_) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 93a479fd7d..7a27cda45a 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -6,47 +6,36 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(LIBFCL REQUIRED "fcl>=0.5.0") -# replace LIBFCL_LIBRARIES with full paths to the libraries -set(LIBFCL_LIBRARIES_FULL "") -foreach(LIBFCL_LIBRARY ${LIBFCL_LIBRARIES}) - find_library(${LIBFCL_LIBRARY}_LIB ${LIBFCL_LIBRARY} ${LIBFCL_LIBRARY_DIRS}) - list(APPEND LIBFCL_LIBRARIES_FULL ${${LIBFCL_LIBRARY}_LIB}) -endforeach() -set(LIBFCL_LIBRARIES "${LIBFCL_LIBRARIES_FULL}") - -find_package(Bullet 2.87 REQUIRED) find_package(angles REQUIRED) -find_package(OCTOMAP REQUIRED) -find_package(urdfdom REQUIRED) -find_package(urdf REQUIRED) -find_package(urdfdom_headers REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(tf2_kdl REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) +find_package(Bullet 2.87 REQUIRED) +find_package(common_interfaces REQUIRED) find_package(eigen_stl_containers REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(fcl REQUIRED) find_package(generate_parameter_library REQUIRED) find_package(geometric_shapes REQUIRED) find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) find_package(moveit_msgs REQUIRED) +find_package(OCTOMAP REQUIRED) find_package(octomap_msgs REQUIRED) +find_package(pluginlib REQUIRED) find_package(random_numbers REQUIRED) +find_package(rclcpp REQUIRED) find_package(ruckig REQUIRED) find_package(sensor_msgs REQUIRED) find_package(shape_msgs REQUIRED) find_package(srdfdom REQUIRED) find_package(std_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_kdl REQUIRED) find_package(trajectory_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) +find_package(urdfdom_headers REQUIRED) find_package(visualization_msgs REQUIRED) -find_package(common_interfaces REQUIRED) -find_package(pluginlib REQUIRED) # TODO: Port python bindings # find_package(pybind11 REQUIRED) @@ -85,60 +74,6 @@ set(THIS_PACKAGE_INCLUDE_DIRS utils/include ) -set(THIS_PACKAGE_LIBRARIES - moveit_butterworth_filter - moveit_butterworth_parameters - 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_planning_request_adapter - # 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_transforms - moveit_utils -) - -set(THIS_PACKAGE_INCLUDE_DEPENDS - angles - eigen_stl_containers - generate_parameter_library - geometric_shapes - geometry_msgs - kdl_parser - moveit_msgs - octomap_msgs - random_numbers - sensor_msgs - shape_msgs - srdfdom - std_msgs - tf2_eigen - tf2_geometry_msgs - trajectory_msgs - visualization_msgs - Eigen3 - eigen3_cmake_module - OCTOMAP - Bullet - ruckig -) - include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} ) @@ -212,7 +147,34 @@ add_subdirectory(collision_detection_fcl) install( - TARGETS ${THIS_PACKAGE_LIBRARIES} + TARGETS + collision_detector_bullet_plugin + moveit_butterworth_filter + moveit_butterworth_parameters + 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_planning_request_adapter + # 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_transforms + moveit_utils EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -221,7 +183,39 @@ install( ) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} orocos_kdl_vendor) +ament_export_dependencies( + angles + Bullet + common_interfaces + eigen_stl_containers + Eigen3 + eigen3_cmake_module + fcl + generate_parameter_library + geometric_shapes + geometry_msgs + kdl_parser + moveit_msgs + OCTOMAP + octomap_msgs + pluginlib + random_numbers + rclcpp + ruckig + sensor_msgs + shape_msgs + srdfdom + std_msgs + tf2_eigen + tf2_geometry_msgs + tf2_kdl + trajectory_msgs + urdf + urdfdom + urdfdom_headers + visualization_msgs + orocos_kdl_vendor +) # Plugin exports pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 8c14ea803b..319273fd63 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -141,91 +141,6 @@ struct CostSource } }; -/** \brief Representation of a collision checking result */ -struct CollisionResult -{ - CollisionResult() : collision(false), distance(std::numeric_limits::max()), contact_count(0) - { - } - using ContactMap = std::map, std::vector >; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Clear a previously stored result */ - void clear() - { - collision = false; - distance = std::numeric_limits::max(); - contact_count = 0; - contacts.clear(); - cost_sources.clear(); - } - - /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ - void print() const; - - /** \brief True if collision was found, false otherwise */ - bool collision; - - /** \brief Closest distance between two bodies */ - double distance; - - /** \brief Number of contacts returned */ - std::size_t contact_count; - - /** \brief A map returning the pairs of body ids in contact, plus their contact details */ - ContactMap contacts; - - /** \brief These are the individual cost sources when costs are computed */ - std::set cost_sources; -}; - -/** \brief Representation of a collision checking request */ -struct CollisionRequest -{ - CollisionRequest() - : distance(false) - , cost(false) - , contacts(false) - , max_contacts(1) - , max_contacts_per_pair(1) - , max_cost_sources(1) - , verbose(false) - { - } - virtual ~CollisionRequest() - { - } - - /** \brief The group name to check collisions for (optional; if empty, assume the complete robot) */ - std::string group_name; - - /** \brief If true, compute proximity distance */ - bool distance; - - /** \brief If true, a collision cost is computed */ - bool cost; - - /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */ - bool contacts; - - /** \brief Overall maximum number of contacts to compute */ - std::size_t max_contacts; - - /** \brief Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at different - * configurations) */ - std::size_t max_contacts_per_pair; - - /** \brief When costs are computed, this value defines how many of the top cost sources should be returned */ - std::size_t max_cost_sources; - - /** \brief Function call that decides whether collision detection should stop. */ - std::function is_done; - - /** \brief Flag indicating whether information about detected collisions should be reported */ - bool verbose; -}; - namespace DistanceRequestTypes { enum DistanceRequestType @@ -381,4 +296,93 @@ struct DistanceResult distances.clear(); } }; + +/** \brief Representation of a collision checking result */ +struct CollisionResult +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Clear a previously stored result */ + void clear() + { + collision = false; + distance = std::numeric_limits::max(); + distance_result.clear(); + contact_count = 0; + contacts.clear(); + cost_sources.clear(); + } + + /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ + void print() const; + + /** \brief True if collision was found, false otherwise */ + bool collision = false; + + /** \brief Closest distance between two bodies */ + double distance = std::numeric_limits::max(); + + /** \brief Distance data for each link */ + DistanceResult distance_result; + + /** \brief Number of contacts returned */ + std::size_t contact_count = 0; + + /** \brief A map returning the pairs of body ids in contact, plus their contact details */ + using ContactMap = std::map, std::vector >; + ContactMap contacts; + + /** \brief These are the individual cost sources when costs are computed */ + std::set cost_sources; +}; + +/** \brief Representation of a collision checking request */ +struct CollisionRequest +{ + CollisionRequest() + : distance(false) + , cost(false) + , contacts(false) + , max_contacts(1) + , max_contacts_per_pair(1) + , max_cost_sources(1) + , verbose(false) + { + } + virtual ~CollisionRequest() + { + } + + /** \brief The group name to check collisions for (optional; if empty, assume the complete robot) */ + std::string group_name; + + /** \brief If true, compute proximity distance */ + bool distance; + + /** \brief If true, return detailed distance information. Distance must be set to true as well */ + bool detailed_distance = false; + + /** \brief If true, a collision cost is computed */ + bool cost; + + /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */ + bool contacts; + + /** \brief Overall maximum number of contacts to compute */ + std::size_t max_contacts; + + /** \brief Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at different + * configurations) */ + std::size_t max_contacts_per_pair; + + /** \brief When costs are computed, this value defines how many of the top cost sources should be returned */ + std::size_t max_cost_sources; + + /** \brief Function call that decides whether collision detection should stop. */ + std::function is_done; + + /** \brief Flag indicating whether information about detected collisions should be reported */ + bool verbose; +}; + } // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 15fb835a69..11e796d3fe 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -48,13 +48,7 @@ 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} 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) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_bullet_export.h DESTINATION include) if(BUILD_TESTING) if(WIN32) diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 382dc41c70..1a6b984c64 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -14,11 +14,11 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom urdfdom_headers - LIBFCL visualization_msgs ) target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection + fcl ) add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp) diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index eb0394ca4f..1ce152dcff 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -277,6 +277,10 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll dreq.enableGroup(getRobotModel()); distanceSelf(dreq, dres, state); res.distance = dres.minimum_distance.distance; + if (req.detailed_distance) + { + res.distance_result = dres; + } } } @@ -330,6 +334,10 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col dreq.enableGroup(getRobotModel()); distanceRobot(dreq, dres, state); res.distance = dres.minimum_distance.distance; + if (req.detailed_distance) + { + res.distance_result = dres; + } } } diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 0d3cb224da..59289def58 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -2,7 +2,7 @@ moveit_core - 2.5.4 + 2.5.6 Core libraries used by MoveIt Henning Kayser diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 349dfa4146..3d77c15b77 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -496,10 +496,10 @@ class JointModel /** \brief The joint this one mimics (nullptr for joints that do not mimic) */ const JointModel* mimic_; - /** \brief The offset to the mimic joint */ + /** \brief The multiplier to the mimic joint */ double mimic_factor_; - /** \brief The multiplier to the mimic joint */ + /** \brief The offset to the mimic joint */ double mimic_offset_; /** \brief The set of joints that should get a value copied to them when this joint changes */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 1b9fe18b77..b7cf3efb12 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -406,7 +406,7 @@ class JointModelGroup void interpolate(const double* from, const double* to, double t, double* state) const; /** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic - joints, so will always be >= the number of items returned by getActiveVariableNames() */ + joints, so will always be >= getActiveVariableCount() */ unsigned int getVariableCount() const { return variable_count_; diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index dfe6ba5e55..873d348afe 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -121,12 +122,10 @@ double FloatingJointModel::distanceTranslation(const double* values1, const doub double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const { - double dq = - fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]); - if (dq + std::numeric_limits::epsilon() >= 1.0) - return 0.0; - else - return acos(dq); + // The values are in "xyzw" order but Eigen expects "wxyz". + const auto q1 = Eigen::Quaterniond(values1[6], values1[3], values1[4], values1[5]).normalized(); + const auto q2 = Eigen::Quaterniond(values2[6], values2[3], values2[4], values2[5]).normalized(); + return q2.angularDistance(q1); } void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 91810f6f0e..78614304a0 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -801,26 +801,11 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const { - const moveit::core::LinkModel* link{ nullptr }; - - size_t idx = 0; - if ((idx = frame.find('/')) != std::string::npos) - { // resolve sub frame - std::string object{ frame.substr(0, idx) }; - if (!hasAttachedBody(object)) - return nullptr; - auto body{ getAttachedBody(object) }; - if (!body->hasSubframeTransform(frame)) - return nullptr; - link = body->getAttachedLink(); - } - else if (hasAttachedBody(frame)) - { - link = getAttachedBody(frame)->getAttachedLink(); - } - else if (getRobotModel()->hasLinkModel(frame)) - link = getLinkModel(frame); - + bool found; + const LinkModel* link{ nullptr }; + getFrameInfo(frame, link, found); + if (!found) + RCLCPP_ERROR(LOGGER, "Unable to find link for frame '%s'", frame.c_str()); return getRobotModel()->getRigidlyConnectedParentLinkModel(link); } @@ -1683,40 +1668,34 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (pose_frame != solver_tip_frame) { - if (hasAttachedBody(pose_frame)) + auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame); + if (!pose_parent) { - const AttachedBody* body = getAttachedBody(pose_frame); - pose_frame = body->getAttachedLinkName(); - pose = pose * body->getPose().inverse(); + RCLCPP_ERROR_STREAM(LOGGER, "The following Pose Frame does not exist: " << pose_frame); + return false; } - if (pose_frame != solver_tip_frame) + Eigen::Isometry3d pose_parent_to_frame = getFrameTransform(pose_frame); + auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame); + if (!tip_parent) { - const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); - if (!link_model) - { - RCLCPP_ERROR(LOGGER, "The following Pose Frame does not exist: %s", pose_frame.c_str()); - return false; - } - const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); - for (const std::pair& fixed_link : fixed_links) - if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame)) - { - pose_frame = solver_tip_frame; - pose = pose * fixed_link.second; - break; - } + RCLCPP_ERROR_STREAM(LOGGER, "The following Solver Tip Frame does not exist: " << solver_tip_frame); + return false; } - - } // end if pose_frame - - // Check if this pose frame works - if (pose_frame == solver_tip_frame) + Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame); + if (pose_parent == tip_parent) + { + // transform goal pose as target for solver_tip_frame (instead of pose_frame) + pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip; + found_valid_frame = true; + break; + } + } + else { found_valid_frame = true; break; - } - - } // end for solver_tip_frames + } // end if pose_frame + } // end for solver_tip_frames // Make sure one of the tip frames worked if (!found_valid_frame) diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index a102e49ee2..cdc49bbf62 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -691,6 +691,8 @@ TEST_F(OneRobot, rigidlyConnectedParent) EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a); moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + state.updateLinkTransforms(); EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 5eefa8e898..283f328b85 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -54,7 +54,7 @@ class RobotTrajectoryTestFixture : public testing::Test { robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); robot_state_ = std::make_shared(robot_model_); - robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_); + robot_state_->setToDefaultValues(); robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0); robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1); robot_state_->update(); @@ -66,7 +66,7 @@ class RobotTrajectoryTestFixture : public testing::Test void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) { - // Init a traj + // Init a trajectory ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: " << arm_jmg_name_; @@ -78,7 +78,10 @@ class RobotTrajectoryTestFixture : public testing::Test double duration_from_previous = 0.1; std::size_t waypoint_count = 5; for (std::size_t ix = 0; ix < waypoint_count; ++ix) + { trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous); + } + // Quick check that getDuration is working correctly EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) << "Generated trajectory duration incorrect"; @@ -323,6 +326,17 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) { robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + EXPECT_FLOAT_EQ(robot_trajectory::path_length(*trajectory), 0.0); + + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0); } @@ -331,6 +345,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + const auto smoothness = robot_trajectory::smoothness(*trajectory); ASSERT_TRUE(smoothness.has_value()); EXPECT_GT(smoothness.value(), 0.0); @@ -345,13 +369,28 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); - const auto density = robot_trajectory::waypoint_density(*trajectory); + // If trajectory has all equal state, and length zero, density should be null. + auto density = robot_trajectory::waypoint_density(*trajectory); + ASSERT_FALSE(density.has_value()); + + // modify joint values so the density is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + + density = robot_trajectory::waypoint_density(*trajectory); ASSERT_TRUE(density.has_value()); EXPECT_GT(density.value(), 0.0); // Check for empty trajectory trajectory->clear(); - EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value()); + density = robot_trajectory::waypoint_density(*trajectory); + EXPECT_FALSE(density.has_value()); } int main(int argc, char** argv) diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index ba0374adb5..cc5de33234 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -324,6 +324,12 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co , time_step_(time_step) , cached_time_(std::numeric_limits::max()) { + if (time_step_ == 0) + { + valid_ = false; + RCLCPP_ERROR(LOGGER, "The trajectory is invalid because the time step is 0."); + return; + } trajectory_.push_back(TrajectoryStep(0.0, 0.0)); double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true); while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_) @@ -565,6 +571,16 @@ bool Trajectory::integrateForward(std::list& trajectory, double trajectory.push_back(TrajectoryStep(path_pos, path_vel)); acceleration = getMinMaxPathAcceleration(path_pos, path_vel, true); + if (path_vel == 0 && acceleration == 0) + { + // The position will never change if velocity and acceleration are zero. + // The loop will spin indefinitely as no exit condition is met. + valid_ = false; + RCLCPP_ERROR(LOGGER, "Error while integrating forward: zero acceleration and velocity. Are any relevant " + "acceleration components limited to zero?"); + return true; + } + if (path_vel > getAccelerationMaxPathVelocity(path_pos) || path_vel > getVelocityMaxPathVelocity(path_pos)) { // Find more accurate intersection with max-velocity curve using bisection diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index fa4a909d14..8863f80b92 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -496,6 +496,36 @@ TEST(time_optimal_trajectory_generation, testPluginAPI) ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end); } +TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory) +{ + const Eigen::Vector2d max_velocity(1, 1); + const Path path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }); + + EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 1)).isValid()); + EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(1, 0)).isValid()); + EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 0)).isValid()); +} + +TEST(time_optimal_trajectory_generation, testIrrelevantZeroMaxAccelerationsDontInvalidateTrajectory) +{ + const Eigen::Vector2d max_velocity(1, 1); + + EXPECT_TRUE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(0, 1)) + .isValid()); + EXPECT_TRUE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(1, 0)) + .isValid()); +} + +TEST(time_optimal_trajectory_generation, testTimeStepZeroMakesTrajectoryInvalid) +{ + EXPECT_FALSE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }), + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/Eigen::Vector2d(1, 1), + /*time_step=*/0) + .isValid()); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index e42008f59a..b948cf3201 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -47,11 +47,7 @@ namespace core class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes { public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) + MoveItErrorCode(int code = 0) { val = code; } @@ -104,12 +100,18 @@ inline std::string error_code_to_string(MoveItErrorCode error_code) return std::string("START_STATE_IN_COLLISION"); case moveit::core::MoveItErrorCode::START_STATE_VIOLATES_PATH_CONSTRAINTS: return std::string("START_STATE_VIOLATES_PATH_CONSTRAINTS"); + case moveit::core::MoveItErrorCode::START_STATE_INVALID: + return std::string("START_STATE_INVALID"); case moveit::core::MoveItErrorCode::GOAL_IN_COLLISION: return std::string("GOAL_IN_COLLISION"); case moveit::core::MoveItErrorCode::GOAL_VIOLATES_PATH_CONSTRAINTS: return std::string("GOAL_VIOLATES_PATH_CONSTRAINTS"); case moveit::core::MoveItErrorCode::GOAL_CONSTRAINTS_VIOLATED: return std::string("GOAL_CONSTRAINTS_VIOLATED"); + case moveit::core::MoveItErrorCode::GOAL_STATE_INVALID: + return std::string("GOAL_STATE_INVALID"); + case moveit::core::MoveItErrorCode::UNRECOGNIZED_GOAL_TYPE: + return std::string("UNRECOGNIZED_GOAL_TYPE"); case moveit::core::MoveItErrorCode::INVALID_GROUP_NAME: return std::string("INVALID_GROUP_NAME"); case moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS: @@ -130,9 +132,14 @@ inline std::string error_code_to_string(MoveItErrorCode error_code) return std::string("SENSOR_INFO_STALE"); case moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE: return std::string("COMMUNICATION_FAILURE"); + case moveit::core::MoveItErrorCode::CRASH: + return std::string("CRASH"); + case moveit::core::MoveItErrorCode::ABORT: + return std::string("ABORT"); case moveit::core::MoveItErrorCode::NO_IK_SOLUTION: return std::string("NO_IK_SOLUTION"); } + return std::string("Unrecognized MoveItErrorCode. This should never happen!"); } } // namespace core diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 3794f81fc4..0033cf742a 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* CI: Fix building of ikfast plugins (`#2791 `_) +* Contributors: Robert Haschke, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Fix ikfast package template (`#2195 `_) (`#2199 `_) + (cherry picked from commit 21036b58e99876928b46e3cc4603a9eb9b85e11d) + Co-authored-by: Jafar +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Contributors: Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Backport to Humble (`#1642 `_) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh index e819fd91fe..02bb5cd1d0 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh @@ -120,7 +120,7 @@ FROM personalrobotics/ros-openrave RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && \ apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && \ apt-get update && \ - apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ + apt-get install -y --force-yes --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ apt-get clean && rm -rf /var/lib/apt/lists/* # enforce a specific version of sympy, which is known to work with OpenRave RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1 diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index dfebba6209..68003ee148 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -2,7 +2,7 @@ moveit_kinematics - 2.5.4 + 2.5.6 Package for all inverse kinematics solvers in MoveIt Henning Kayser diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 0f4609aadc..e9f6142298 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Add planner configurations to CHOMP and PILZ (`#1522 `_) (`#1656 `_) diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index 9e426ac978..68e1314db8 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp - 2.5.4 + 2.5.6 The interface for using CHOMP within MoveIt Chittaranjan Srinivas Swaminathan diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index 70e96200a9..a84d591695 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Switch to clang-format-14 (`#1877 `_) (`#1880 `_) + * Switch to clang-format-14 + * Fix clang-format-14 + (cherry picked from commit 7fa5eaf1ac21ab8a99c5adae53bd0a2d4abf98f6) + Co-authored-by: Henning Kayser +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * updated comment formatting for correct doxygen generation (`#1582 `_) (`#1664 `_) diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 42075c665f..a8ec276dda 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner - 2.5.4 + 2.5.6 chomp_motion_planner Chittaranjan Srinivas Swaminathan diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index d89f74f2aa..6b05f3bdff 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index 471100b9c6..4c7b44fc1d 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -2,7 +2,7 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 2.5.4 + 2.5.6 Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 9557f00eab..3de3f878ca 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 847ee8f210..c4033841be 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -2,7 +2,7 @@ moveit_planners - 2.5.4 + 2.5.6 Meta package that installs all available planners for MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index d260987b8d..6bd4389c1b 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Invoke OMPL debug print only when debug logging is enabled (backport `#2608 `_) (`#2762 `_) +* Map ompl's APPROXIMATE_SOLUTION -> TIMED_OUT / PLANNING_FAILED (`#2455 `_) (`#2459 `_) +* Contributors: Igor Medvedev, Robert Hashcke, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Fix Constraint Planning Segfault (`#2130 `_) (`#2173 `_) + * Fix Constraint Planning Segfault + * Reuse planner data + * apply clang formatting + * apply clang formatting round 2 + * add FIXME note and verbose output of planning graph size + --------- + Co-authored-by: Sebastian Jahr + (cherry picked from commit 92a7951f74baaf26d07356612a2f5dca0bac5065) + Co-authored-by: Marq Rasmussen +* Cleanup msg includes: Use C++ instead of C header (backport `#1844 `_) + * Cleanup msg includes: Use C++ instead of C header + * Remove obsolete include: moveit_msgs/srv/execute_known_trajectory.hpp +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * simplify_solution per planning context (`#1437 `_) (`#1646 `_) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 7283f0bc1b..6ffb553b11 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -755,9 +755,13 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() RCLCPP_DEBUG(LOGGER, "There were %d valid motions and %d invalid motions.", v, iv); // Debug OMPL setup and solution - std::stringstream debug_out; - ompl_simple_setup_->print(debug_out); - RCLCPP_DEBUG(LOGGER, "%s", rclcpp::get_c_string(debug_out.str())); + RCLCPP_DEBUG(LOGGER, "%s", + [&] { + std::stringstream debug_out; + ompl_simple_setup_->print(debug_out); + return debug_out.str(); + }() + .c_str()); } bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) @@ -857,7 +861,7 @@ const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningConte RCLCPP_DEBUG(LOGGER, "%s: Solving the planning problem once...", name_.c_str()); ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); - result.val = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION; + std::ignore = ompl_simple_setup_->solve(ptc); last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime(); unregisterTerminationCondition(); // fill the result status code @@ -971,7 +975,7 @@ void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition() int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup) { auto result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus(); + const ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus(); switch (ompl::base::PlannerStatus::StatusType(ompl_status)) { case ompl::base::PlannerStatus::UNKNOWN: @@ -991,12 +995,23 @@ int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::Si result = moveit_msgs::msg::MoveItErrorCodes::UNRECOGNIZED_GOAL_TYPE; break; case ompl::base::PlannerStatus::TIMEOUT: - RCLCPP_WARN(LOGGER, "Timed out"); + RCLCPP_WARN(LOGGER, "Timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(), + request_.allowed_planning_time); result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT; break; case ompl::base::PlannerStatus::APPROXIMATE_SOLUTION: - RCLCPP_WARN(LOGGER, "Solution is approximate"); - result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + // timeout is a common reason for APPROXIMATE_SOLUTION + if (ompl_simple_setup->getLastPlanComputationTime() > request_.allowed_planning_time) + { + RCLCPP_WARN(LOGGER, "Planning timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(), + request_.allowed_planning_time); + result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT; + } + else + { + RCLCPP_WARN(LOGGER, "Solution is approximate"); + result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + } break; case ompl::base::PlannerStatus::EXACT_SOLUTION: result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 6a082b461c..cff2f629f2 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -2,7 +2,7 @@ moveit_planners_ompl - 2.5.4 + 2.5.6 MoveIt interface to OMPL Henning Kayser Tyler Weaver diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst index 6586375ccf..686390999d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,43 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Allow RobotState::setFromIK to work with subframes (backport `#3077 `_) (`#3085 `_) +* Enhancement/ports moveit 3522 (backport `#3070 `_) (`#3074 `_) +* Ports moveit/moveit/pull/3519 to ros2 (backport `#3055 `_) (`#3061 `_) +* Fix Pilz blending times (backport `#2961 `_) (`#3000 `_) +* PILZ: Throw if IK solver doesn't exist (`#2082 `_) (`#2921 `_) +* Contributors: Tom Noble, Sebastian Castro, Sebastian Jahr, Robert Haschke, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Pilz multi-group incompatibility (`#1856 `_) (`#2306 `_) + (cherry picked from commit 5bbe21b3574d3e3ef2a2c681b3962f70c3db7e78) + Co-authored-by: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Fx class_loader warnings in PILZ unittests (`#2296 `_) (`#2303 `_) + (cherry picked from commit d4dcea36d9767605fc1921716a67a0c6e9be2a2e) + Co-authored-by: Yang Lin +* fix: resolve bugs in MoveGroupSequenceAction class (main branch) (`#1797 `_) (`#1809 `_) + * fix: resolve bugs in MoveGroupSequenceAction class + * style: adopt .clang-format + Co-authored-by: Marco Magri + (cherry picked from commit fca8e9bd3f41e6bff5aadbf75c494b5cc3fa25ee) + Co-authored-by: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Backport to Humble (`#1642 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index e81abbeae7..08b32424a6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -121,17 +121,7 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve(plan { if (!terminated_) { - // Use current state as start state if not set - if (request_.start_state.joint_state.name.empty()) - { - moveit_msgs::msg::RobotState current_state; - moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state); - request_.start_state = current_state; - } - bool result = generator_.generate(getPlanningScene(), request_, res); - return result; - // res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; - // return false; // TODO + return generator_.generate(getPlanningScene(), request_, res); } RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index 863a12fe70..fc398e5f0b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -45,6 +45,7 @@ #include #include +#include namespace pilz_industrial_motion_planner { @@ -74,17 +75,17 @@ bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std bool check_self_collision = true, const double timeout = 0.0); /** - * @brief compute the pose of a link at give robot state - * @param robot_model: kinematic model of the robot + * @brief compute the pose of a link at a given robot state + * @param robot_state: an arbitrary robot state (with collision objects attached) * @param link_name: target link name * @param joint_state: joint positions of this group * @param pose: pose of the link in base frame of robot model * @return true if succeed */ -bool computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, +bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, const std::map& joint_state, Eigen::Isometry3d& pose); -bool computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, +bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, const std::vector& joint_names, const std::vector& joint_positions, Eigen::Isometry3d& pose); @@ -212,9 +213,8 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p * @param ik_solution * @return */ -bool isStateColliding(const bool test_for_self_collision, const planning_scene::PlanningSceneConstPtr& scene, - moveit::core::RobotState* state, const moveit::core::JointModelGroup* const group, - const double* const ik_solution); +bool isStateColliding(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* const group, const double* const ik_solution); } // namespace pilz_industrial_motion_planner void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index a24a7395c6..fbde978692 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -113,12 +113,13 @@ class TrajectoryGenerator protected: /** - * @brief This class is used to extract needed information from motion plan - * request. + * @brief This class is used to extract needed information from motion plan request. */ class MotionPlanInfo { public: + MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req); + std::string group_name; std::string link_name; Eigen::Isometry3d start_pose; @@ -126,6 +127,7 @@ class TrajectoryGenerator std::map start_joint_position; std::map goal_joint_position; std::pair circ_path_point; + planning_scene::PlanningSceneConstPtr start_scene; // scene with updated start state }; /** @@ -201,7 +203,7 @@ class TrajectoryGenerator * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure * @param req: motion plan request */ - void validateRequest(const planning_interface::MotionPlanRequest& req) const; + void validateRequest(const planning_interface::MotionPlanRequest& req, const moveit::core::RobotState& rstate) const; /** * @brief set MotionPlanResponse from joint trajectory @@ -228,14 +230,13 @@ class TrajectoryGenerator void checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const; void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, - const std::vector& expected_joint_names, const std::string& group_name) const; + const std::string& group_name, const moveit::core::RobotState& rstate) const; - void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, - const std::vector& expected_joint_names, - const std::string& group_name) const; + void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::string& group_name) const; void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, - const std::string& group_name) const; + const moveit::core::RobotState& robot_state, + const moveit::core::JointModelGroup* const jmg) const; private: /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index b79eafa2b2..87d9c0ead9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -56,8 +56,6 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState, - moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index f1a2dfcf7e..f586918959 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -48,7 +48,6 @@ namespace pilz_industrial_motion_planner CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); /** diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 509d0cc48a..26d2a09d9c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -2,7 +2,7 @@ pilz_industrial_motion_planner - 2.5.4 + 2.5.6 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Christian Henkel diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index a75751d77f..b3c0dceb28 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -173,7 +173,8 @@ void CommandListManager::setStartState(const MotionResponseCont& motion_plan_res RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) }; if (rob_state_op) { - moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state); + moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state, false); + start_state.is_diff = true; } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index 3c58f8978a..a6711117ee 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -66,8 +66,10 @@ void MoveGroupSequenceAction::initialize() { // start the move action server RCLCPP_INFO_STREAM(LOGGER, "initialize move group sequence action"); + // Use MutuallyExclusiveCallbackGroup to prevent race conditions in callbacks. + // See: https://github.com/moveit/moveit2/issues/3117 for details. action_callback_group_ = - context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); move_action_server_ = rclcpp_action::create_server( context_->moveit_cpp_->getNode(), "sequence_move_group", [](const rclcpp_action::GoalUUID& /* unused */, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index 39c9d1e06c..0513d22b49 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -54,13 +54,17 @@ std::vector PlanComponentsBuilder::build() void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, const robot_trajectory::RobotTrajectory& source) { - if (result.empty() || - !pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), - result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + if (result.empty()) { result.append(source, 0.0); return; } + if (!pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), + result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + { + result.append(source, source.getWayPointDurationFromStart(0)); + return; + } for (size_t i = 1; i < source.getWayPointCount(); ++i) { @@ -94,7 +98,8 @@ void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& p // Append the new trajectory elements appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory); - traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); + appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory); + // Store the last new trajectory element for future processing traj_tail_ = blend_response.second_trajectory; // first for next blending segment } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 920c43530a..17e87b8f13 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -67,7 +67,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { - RCLCPP_ERROR(LOGGER, "Blend radius to large."); + RCLCPP_ERROR(LOGGER, "Blend radius too large."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -97,7 +97,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian, req.group_name, req.link_name, initial_joint_position, initial_joint_velocity, - blend_joint_trajectory, error_code, true)) + blend_joint_trajectory, error_code)) { // LCOV_EXCL_START RCLCPP_INFO(LOGGER, "Failed to generate joint trajectory for blending trajectory."); @@ -124,6 +124,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( // append the blend trajectory res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory); + // copy the points [second_intersection_index, len] from the second trajectory for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index d7f15c34da..1f8d871385 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -59,12 +59,6 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin return false; } - if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name)) - { - RCLCPP_ERROR_STREAM(LOGGER, "No valid IK solver exists for " << link_name << " in planning group " << group_name); - return false; - } - if (frame_id != robot_model->getModelFrame()) { RCLCPP_ERROR_STREAM(LOGGER, "Given frame (" << frame_id << ") is unequal to model frame(" @@ -72,25 +66,26 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin return false; } - moveit::core::RobotState rstate(robot_model); - // By setting the robot state to default values, we basically allow - // the user of this function to supply an incomplete or even empty seed. - rstate.setToDefaultValues(); + moveit::core::RobotState rstate{ scene->getCurrentState() }; rstate.setVariablePositions(seed); moveit::core::GroupStateValidityCallbackFn ik_constraint_function; - ik_constraint_function = [check_self_collision, scene](moveit::core::RobotState* robot_state, - const moveit::core::JointModelGroup* joint_group, - const double* joint_group_variable_values) { - return pilz_industrial_motion_planner::isStateColliding(check_self_collision, scene, robot_state, joint_group, - joint_group_variable_values); - }; + if (check_self_collision) + { + ik_constraint_function = [scene](moveit::core::RobotState* robot_state, + const moveit::core::JointModelGroup* joint_group, + const double* joint_group_variable_values) { + return pilz_industrial_motion_planner::isStateColliding(scene, robot_state, joint_group, + joint_group_variable_values); + }; + } // call ik - if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function)) + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name); + if (rstate.setFromIK(jmg, pose, link_name, timeout, ik_constraint_function)) { // copy the solution - for (const auto& joint_name : robot_model->getJointModelGroup(group_name)->getActiveJointModelNames()) + for (const auto& joint_name : jmg->getActiveJointModelNames()) { solution[joint_name] = rstate.getVariablePosition(joint_name); } @@ -118,27 +113,22 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin timeout); } -bool pilz_industrial_motion_planner::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, - const std::string& link_name, +bool pilz_industrial_motion_planner::computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, const std::map& joint_state, Eigen::Isometry3d& pose) -{ // create robot state - moveit::core::RobotState rstate(robot_model); - +{ // check the reference frame of the target pose - if (!rstate.knowsFrameTransform(link_name)) + if (!robot_state.knowsFrameTransform(link_name)) { RCLCPP_ERROR_STREAM(LOGGER, "The target link " << link_name << " is not known by robot."); return false; } - // set the joint positions - rstate.setToDefaultValues(); - rstate.setVariablePositions(joint_state); + robot_state.setVariablePositions(joint_state); // update the frame - rstate.update(); - pose = rstate.getFrameTransform(link_name); + robot_state.update(); + pose = robot_state.getFrameTransform(link_name); return true; } @@ -576,17 +566,11 @@ bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_ return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r); } -bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision, - const planning_scene::PlanningSceneConstPtr& scene, +bool pilz_industrial_motion_planner::isStateColliding(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState* rstate, const moveit::core::JointModelGroup* const group, const double* const ik_solution) { - if (!test_for_self_collision) - { - return true; - } - rstate->setJointGroupPositions(group, ik_solution); rstate->update(); collision_detection::CollisionRequest collision_req; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 5a8c9753e3..18da41f112 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -114,11 +114,6 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name) void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const { - if (start_state.joint_state.name.empty()) - { - throw NoJointNamesInStartState("No joint names for state state given"); - } - if (start_state.joint_state.name.size() != start_state.joint_state.position.size()) { throw SizeMismatchInStartState("Joint state name and position do not match in start state"); @@ -151,19 +146,11 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& st } void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, - const std::vector& expected_joint_names, const std::string& group_name) const { for (auto const& joint_constraint : constraint.joint_constraints) { const std::string& curr_joint_name{ joint_constraint.joint_name }; - if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) == - expected_joint_names.cend()) - { - std::ostringstream os; - os << "Cannot find joint \"" << curr_joint_name << "\" from start state in goal constraint"; - throw StartStateGoalStateMismatch(os.str()); - } if (!robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name)) { @@ -182,7 +169,8 @@ void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Const } void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, - const std::string& group_name) const + const moveit::core::RobotState& robot_state, + const moveit::core::JointModelGroup* const jmg) const { assert(constraint.position_constraints.size() == 1); assert(constraint.orientation_constraints.size() == 1); @@ -208,7 +196,8 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::C throw PositionOrientationConstraintNameMismatch(os.str()); } - if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name)) + const auto& lm = robot_state.getRigidlyConnectedParentLinkModel(pos_constraint.link_name); + if (!lm || !jmg->canSetStateFromIK(lm->getName())) { std::ostringstream os; os << "No IK solver available for link: \"" << pos_constraint.link_name << "\""; @@ -222,8 +211,8 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::C } void TrajectoryGenerator::checkGoalConstraints( - const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, - const std::vector& expected_joint_names, const std::string& group_name) const + const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::string& group_name, + const moveit::core::RobotState& rstate) const { if (goal_constraints.size() != 1) { @@ -240,21 +229,22 @@ void TrajectoryGenerator::checkGoalConstraints( if (isJointGoalGiven(goal_con)) { - checkJointGoalConstraint(goal_con, expected_joint_names, group_name); + checkJointGoalConstraint(goal_con, group_name); } else { - checkCartesianGoalConstraint(goal_con, group_name); + checkCartesianGoalConstraint(goal_con, rstate, robot_model_->getJointModelGroup(group_name)); } } -void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req) const +void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req, + const moveit::core::RobotState& rstate) const { checkVelocityScaling(req.max_velocity_scaling_factor); checkAccelerationScaling(req.max_acceleration_scaling_factor); checkForValidGroupName(req.group_name); checkStartState(req.start_state, req.group_name); - checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name); + checkGoalConstraints(req.goal_constraints, req.group_name, rstate); } void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& start_state, const std::string& group_name, @@ -309,7 +299,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& try { - validateRequest(req); + validateRequest(req, scene->getCurrentState()); } catch (const MoveItErrorCodeException& ex) { @@ -331,7 +321,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& return false; } - MotionPlanInfo plan_info; + MotionPlanInfo plan_info(scene, req); try { extractMotionPlanInfo(scene, req, plan_info); @@ -347,7 +337,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& trajectory_msgs::msg::JointTrajectory joint_trajectory; try { - plan(scene, req, plan_info, sampling_time, joint_trajectory); + plan(plan_info.start_scene, req, plan_info, sampling_time, joint_trajectory); } catch (const MoveItErrorCodeException& ex) { @@ -357,10 +347,30 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& return false; } - moveit::core::RobotState start_state(scene->getCurrentState()); - moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true); - setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res); + setSuccessResponse(plan_info.start_scene->getCurrentState(), req.group_name, joint_trajectory, planning_begin, res); return true; } +TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req) +{ + auto ps = scene->diff(); + auto& start_state = ps->getCurrentStateNonConst(); + // update start state from req + moveit::core::robotStateMsgToRobotState(scene->getTransforms(), req.start_state, start_state); + start_state.update(); + start_scene = std::move(ps); + + // initialize info.start_joint_position with active joint values from start_state + const double* positions = start_state.getVariablePositions(); + for (const auto* jm : start_state.getRobotModel()->getJointModelGroup(req.group_name)->getActiveJointModels()) + { + const auto& names = jm->getVariableNames(); + for (std::size_t i = 0, j = jm->getFirstVariableIndex(); i < jm->getVariableCount(); ++i, ++j) + { + start_joint_position[names[i]] = positions[j]; + } + } +} + } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index abe54a0afe..d3a145ca8f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -95,7 +95,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); info.group_name = req.group_name; - std::string frame_id{ robot_model_->getModelFrame() }; + moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -117,22 +117,17 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni throw NumberOfConstraintsMismatch(os.str()); } - // initializing all joints of the model - for (const auto& joint_name : robot_model_->getVariableNames()) - { - info.goal_joint_position[joint_name] = 0; - } - for (const auto& joint_item : req.goal_constraints.front().joint_constraints) { info.goal_joint_position[joint_item.joint_name] = joint_item.position; } - computeLinkFK(robot_model_, info.link_name, info.goal_joint_position, info.goal_pose); + computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose); } // goal given in Cartesian space else { + std::string frame_id; info.link_name = req.goal_constraints.front().position_constraints.front().link_name; if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) @@ -145,54 +140,59 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); - } - assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); - for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) - { - auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; - if (it == req.start_state.joint_state.name.cend()) + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) { + // LCOV_EXCL_START std::ostringstream os; - os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name - << "\" in start state of request"; - throw CircJointMissingInStartState(os.str()); + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw CircInverseForGoalIncalculable(os.str()); + // LCOV_EXCL_STOP // not able to trigger here since lots of checks before + // are in place } - size_t index = it - req.start_state.joint_state.name.cbegin(); - info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; } - computeLinkFK(robot_model_, info.link_name, info.start_joint_position, info.start_pose); + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); - // check goal pose ik before Cartesian motion plan starts - std::map ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) + // center point with wrt. the planning frame + std::string center_point_frame_id; + + info.circ_path_point.first = req.path_constraints.name; + if (req.path_constraints.position_constraints.front().header.frame_id.empty()) { - // LCOV_EXCL_START - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw CircInverseForGoalIncalculable(os.str()); - // LCOV_EXCL_STOP // not able to trigger here since lots of checks before - // are in place + RCLCPP_WARN(LOGGER, "Frame id is not set in position constraints of " + "path. Use model frame as default"); + center_point_frame_id = robot_model_->getModelFrame(); } - info.circ_path_point.first = req.path_constraints.name; + else + { + center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id; + } + + Eigen::Isometry3d center_point_pose; + tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(), + center_point_pose); + + center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose; + if (!req.goal_constraints.front().position_constraints.empty()) { const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); - info.circ_path_point.second = - getConstraintPose( - req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset) - .translation(); + geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation())); + info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation, + goal.position_constraints.front().target_point_offset) + .translation(); } else { - Eigen::Vector3d circ_path_point; - tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - circ_path_point); - info.circ_path_point.second = circ_path_point; + info.circ_path_point.second = center_point_pose.translation(); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 7dbf234bfe..0caf4b6327 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -34,6 +34,8 @@ #include +#include + #include #include #include @@ -70,12 +72,12 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); info.group_name = req.group_name; - std::string frame_id{ robot_model_->getModelFrame() }; + moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) { - info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name)); if (req.goal_constraints.front().joint_constraints.size() != robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) @@ -87,24 +89,19 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() << ")"; throw JointNumberMismatch(os.str()); } - // initializing all joints of the model - for (const auto& joint_name : robot_model_->getVariableNames()) - { - info.goal_joint_position[joint_name] = 0; - } for (const auto& joint_item : req.goal_constraints.front().joint_constraints) { info.goal_joint_position[joint_item.joint_name] = joint_item.position; } - // Ignored return value because at this point the function should always - // return 'true'. - computeLinkFK(robot_model_, info.link_name, info.goal_joint_position, info.goal_pose); + computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose); } // goal given in Cartesian space else { + std::string frame_id; + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) @@ -117,37 +114,25 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); - } - assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); - for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) - { - auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; - if (it == req.start_state.joint_state.name.cend()) + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) { std::ostringstream os; - os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name - << "\" in start state of request"; - throw LinJointMissingInStartState(os.str()); + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); } - size_t index = it - req.start_state.joint_state.name.cbegin(); - info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; } // Ignored return value because at this point the function should always // return 'true'. - computeLinkFK(robot_model_, info.link_name, info.start_joint_position, info.start_pose); - - // check goal pose ik before Cartesian motion plan starts - std::map ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) - { - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw LinInverseForGoalIncalculable(os.str()); - } + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index bb665bca53..fe6b096c5d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -215,13 +215,6 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin { info.group_name = req.group_name; - // extract start state information - info.start_joint_position.clear(); - for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i) - { - info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i]; - } - // extract goal info.goal_joint_position.clear(); if (!req.goal_constraints.at(0).joint_constraints.empty()) @@ -234,11 +227,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin // solve the ik else { - Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front()); - if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, - goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + std::string frame_id; + + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan start + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + info.goal_joint_position)) { - throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw PtpNoIkSolutionForGoalPose(os.str()); } } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp index 8d51e51110..cdf7002625 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp @@ -56,14 +56,20 @@ class JointLimitsAggregator : public ::testing::Test node_ = rclcpp::Node::make_shared("unittest_joint_limits_aggregator", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; } + void TearDown() override + { + robot_model_.reset(); + } + protected: rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; }; /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index 17f37347ec..6b2e78654d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -69,8 +69,8 @@ class CommandPlannerTest : public testing::Test void createPlannerInstance() { // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; // Load planner name from node parameters @@ -105,12 +105,14 @@ class CommandPlannerTest : public testing::Test void TearDown() override { planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_); + robot_model_.reset(); } protected: // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; std::string planner_plugin_name_; std::unique_ptr> planner_plugin_loader_; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 16e581a724..30d5d3dab8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -94,8 +94,8 @@ class PlanningContextTest : public ::testing::Test node_ = rclcpp::Node::make_shared("unittest_planning_context", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; // get parameters @@ -129,6 +129,11 @@ class PlanningContextTest : public ::testing::Test planning_context_->setPlanningScene(scene); // TODO Check what happens if this is missing } + void TearDown() override + { + robot_model_.reset(); + } + /** * @brief Generate a valid fully defined request */ @@ -184,6 +189,7 @@ class PlanningContextTest : public ::testing::Test // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; std::unique_ptr planning_context_; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp index db2b62f468..25eaea7564 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp @@ -63,8 +63,8 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; // Load the plugin @@ -98,11 +98,13 @@ class PlanningContextLoadersTest : public ::testing::TestWithParamunloadLibraryForClass(GetParam().front()); } + robot_model_.reset(); } protected: rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; // Load the plugin std::unique_ptr> diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 5db1847f63..a75992bcbc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -74,8 +74,8 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test node_ = rclcpp::Node::make_shared("unittest_trajectory_blender_transition_window", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; planning_scene_ = std::make_shared(robot_model_); @@ -123,6 +123,11 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; } + void TearDown() override + { + robot_model_.reset(); + } + /** * @brief Generate lin trajectories for blend sequences */ @@ -153,6 +158,7 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; std::unique_ptr lin_generator_; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index a789615d2b..3a389fae1a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -92,9 +92,10 @@ class TrajectoryFunctionsTestBase : public testing::Test node_ = rclcpp::Node::make_shared("unittest_trajectory_functions", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + robot_state_ = std::make_shared(robot_model_); planning_scene_ = std::make_shared(robot_model_); // get parameters @@ -120,6 +121,11 @@ class TrajectoryFunctionsTestBase : public testing::Test } } + void TearDown() override + { + robot_model_.reset(); + } + /** * @brief check if two transformations are close * @param pose1 @@ -129,10 +135,41 @@ class TrajectoryFunctionsTestBase : public testing::Test */ bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon); + /** + * @brief check if two sets of joint positions are close + * @param joints1 the first set of joint positions to compare + * @param joints2 the second set of joint positions to compare + * @param epsilon the tolerance a all joint position diffs must satisfy + * @return false if any joint diff exceeds tolerance. true otherwise + */ + bool jointsNear(const std::vector& joints1, const std::vector& joints2, double epsilon); + + /** + * @brief get the current joint values of the robot state + * @param jmg the joint model group whose joints we are interested in + * @param state the robot state to fetch the current joint positions for + * @return the joint positions for joints from jmg, set to the positions determined from state + */ + std::vector getJoints(const moveit::core::JointModelGroup* jmg, const moveit::core::RobotState& state); + + /** + * @brief attach a collision object and subframes to a link + * @param state the state we are updating + * @param link the link we are attaching the collision object to + * @param object_name a unique name for the collision object + * @param object_pose the pose of the object relative to the parent link + * @param subframes subframe names and poses relative to the object they attach to + */ + void attachToLink(moveit::core::RobotState& state, const moveit::core::LinkModel* link, + const std::string& object_name, const Eigen::Isometry3d& object_pose, + const moveit::core::FixedTransformsMap& subframes); + protected: // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; // test parameters from parameter server @@ -158,6 +195,43 @@ bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const E return true; } +bool TrajectoryFunctionsTestBase::jointsNear(const std::vector& joints1, const std::vector& joints2, + double epsilon) +{ + if (joints1.size() != joints2.size()) + { + return false; + } + for (std::size_t i = 0; i < joints1.size(); ++i) + { + if (fabs(joints1.at(i) - joints2.at(i)) > fabs(epsilon)) + { + return false; + } + } + return true; +} + +std::vector TrajectoryFunctionsTestBase::getJoints(const moveit::core::JointModelGroup* jmg, + const moveit::core::RobotState& state) +{ + std::vector joints; + for (const auto& name : jmg->getActiveJointModelNames()) + { + joints.push_back(state.getVariablePosition(name)); + } + return joints; +} + +void TrajectoryFunctionsTestBase::attachToLink(moveit::core::RobotState& state, const moveit::core::LinkModel* link, + const std::string& object_name, const Eigen::Isometry3d& object_pose, + const moveit::core::FixedTransformsMap& subframes) +{ + state.attachBody(std::make_unique( + link, object_name, object_pose, std::vector{}, EigenSTL::vector_Isometry3d{}, + std::set{}, trajectory_msgs::msg::JointTrajectory{}, subframes)); +} + /** * @brief Parametrized class for tests with and without gripper. */ @@ -182,27 +256,27 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) { Eigen::Isometry3d tip_pose; std::map test_state = zero_state_; - EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, group_tip_link_, test_state, tip_pose)); EXPECT_NEAR(tip_pose(0, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3, EPSILON); test_state[joint_names_.at(1)] = M_PI_2; - EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, group_tip_link_, test_state, tip_pose)); EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3, EPSILON); EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(2, 3), L0, EPSILON); test_state[joint_names_.at(1)] = -M_PI_2; test_state[joint_names_.at(2)] = M_PI_2; - EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, group_tip_link_, test_state, tip_pose)); EXPECT_NEAR(tip_pose(0, 3), -L1, EPSILON); EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3, EPSILON); // wrong link name std::string link_name = "wrong_link_name"; - EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, link_name, test_state, tip_pose)); + EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, link_name, test_state, tip_pose)); } /** @@ -214,6 +288,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); + if (!solver) + { + throw("No IK solver configured for group '" + planning_group_ + "'"); + } // robot state moveit::core::RobotState rstate(robot_model_); @@ -317,6 +395,120 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) } } +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityCollisionObject) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object with an ignored subframe, and no transform + Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity(); + moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use an object pose to set the joints + Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip; + bool success = state.setFromIK(jmg, object_pose_in_base, "object"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the object, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedCollisionObject) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object with no subframes, and a non-trivial transform + Eigen::Isometry3d object_pose_in_tip; + object_pose_in_tip = Eigen::Translation3d(1, 2, 3); + object_pose_in_tip *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitX()); + moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use an object pose to set the joints + Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip; + bool success = state.setFromIK(jmg, object_pose_in_base, "object"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the object, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentitySubframe) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object and subframe with no transforms + Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d subframe_pose_in_object = Eigen::Isometry3d::Identity(); + moveit::core::FixedTransformsMap subframes({ { "subframe", subframe_pose_in_object } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use a subframe pose to set the joints + Eigen::Isometry3d subframe_pose_in_base = tip_pose_in_base * object_pose_in_tip * subframe_pose_in_object; + bool success = state.setFromIK(jmg, subframe_pose_in_base, "object/subframe"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the subframe, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedSubframe) +{ + // Set up a default robot + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + std::vector default_joints = getJoints(jmg, state); + const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); + Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); + + // Attach an object and subframe with non-trivial transforms + Eigen::Isometry3d object_pose_in_tip; + object_pose_in_tip = Eigen::Translation3d(1, 2, 3); + object_pose_in_tip *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitX()); + + Eigen::Isometry3d subframe_pose_in_object; + subframe_pose_in_object = Eigen::Translation3d(4, 5, 6); + subframe_pose_in_object *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitY()); + + moveit::core::FixedTransformsMap subframes({ { "subframe", subframe_pose_in_object } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); + + // The RobotState should be able to use a subframe pose to set the joints + Eigen::Isometry3d subframe_pose_in_base = tip_pose_in_base * object_pose_in_tip * subframe_pose_in_object; + bool success = state.setFromIK(jmg, subframe_pose_in_base, "object/subframe"); + EXPECT_TRUE(success); + + // Given the target pose is the default pose of the subframe, the joints should be unchanged + std::vector ik_joints = getJoints(jmg, state); + EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET)); +} + /** * @brief Test the wrapper function to compute inverse kinematics using robot * model diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index 100270497a..5e540e4fbb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -70,8 +70,8 @@ class TrajectoryGeneratorCIRCTest : public testing::Test node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_circ", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; planning_scene_ = std::make_shared(robot_model_); @@ -160,6 +160,11 @@ class TrajectoryGeneratorCIRCTest : public testing::Test } } + void TearDown() override + { + robot_model_.reset(); + } + void getCircCenter(const planning_interface::MotionPlanRequest& req, const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center) { @@ -192,6 +197,7 @@ class TrajectoryGeneratorCIRCTest : public testing::Test // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; std::unique_ptr circ_; @@ -257,11 +263,6 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } - { - std::shared_ptr cjmiss_ex{ new CircJointMissingInStartState("") }; - EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - } - { std::shared_ptr cifgi_ex{ new CircInverseForGoalIncalculable("") }; EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); @@ -278,24 +279,6 @@ TEST_F(TrajectoryGeneratorCIRCTest, noLimits) TrajectoryGeneratorInvalidLimitsException); } -/** - * @brief test invalid motion plan request with incomplete start state and - * cartesian goal - */ -TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState) -{ - auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - - planning_interface::MotionPlanRequest req{ circ.toRequest() }; - EXPECT_GT(req.start_state.joint_state.name.size(), 1u); - req.start_state.joint_state.name.resize(1); - req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes - - planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -} - /** * @brief test invalid motion plan request with non zero start velocity */ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 7657f90849..1da2435882 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -88,8 +88,8 @@ class TrajectoryGeneratorLINTest : public testing::Test node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_lin", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; planning_scene_ = std::make_shared(robot_model_); @@ -131,6 +131,11 @@ class TrajectoryGeneratorLINTest : public testing::Test ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; } + void TearDown() override + { + robot_model_.reset(); + } + bool checkLinResponse(const planning_interface::MotionPlanRequest& req, const planning_interface::MotionPlanResponse& res) { @@ -159,6 +164,7 @@ class TrajectoryGeneratorLINTest : public testing::Test // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; // lin trajectory generator using model without gripper @@ -190,11 +196,6 @@ TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } - { - std::shared_ptr ljmiss_ex{ new LinJointMissingInStartState("") }; - EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - } - { std::shared_ptr lifgi_ex{ new LinInverseForGoalIncalculable("") }; EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); @@ -422,24 +423,6 @@ TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber) EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } -/** - * @brief test invalid motion plan request with incomplete start state and - * cartesian goal - */ -TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) -{ - // construct motion plan request - moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; - EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u); - lin_cart_req.start_state.joint_state.name.resize(1); - lin_cart_req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes - - // generate lin trajectory - planning_interface::MotionPlanResponse res; - EXPECT_FALSE(lin_->generate(planning_scene_, lin_cart_req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -} - /** * @brief Set a frame id in goal constraint with cartesian goal on both position * and orientation constraints diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index bb5c0ddd34..766d7ffd63 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -71,8 +71,8 @@ class TrajectoryGeneratorPTPTest : public testing::Test node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_ptp", node_options); // load robot model - robot_model_loader::RobotModelLoader rm_loader(node_); - robot_model_ = rm_loader.getModel(); + rm_loader_ = std::make_unique(node_); + robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; planning_scene_ = std::make_shared(robot_model_); @@ -118,6 +118,11 @@ class TrajectoryGeneratorPTPTest : public testing::Test ASSERT_NE(nullptr, ptp_); } + void TearDown() override + { + robot_model_.reset(); + } + /** * @brief check the resulted joint trajectory * @param trajectory @@ -140,6 +145,7 @@ class TrajectoryGeneratorPTPTest : public testing::Test // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; // trajectory generator diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index 8a2f8d0f1d..42dc0568c0 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Contributors: Chris Thrasher, Robert Haschke + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index 8fb710c58c..3728ddca31 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -2,7 +2,7 @@ pilz_industrial_motion_planner_testutils - 2.5.4 + 2.5.6 Helper scripts and functionality to test industrial motion generation Christian Henkel diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst index 57ea7ad150..ebe788733a 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package moveit_resources_prbt_ikfast_manipulator_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* PRBT IkFast patch from robostack (`#2395 `_) (`#2397 `_) +* Contributors: Tyler Weaver, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Used C++ style casting for int type (backport `#1627 `_) (`#1819 `_) + (cherry picked from commit 1f32ab0e43f488e9c5bd1957c7677e302c406df0) + Co-authored-by: Abhijeet Das Gupta <75399048+abhijelly@users.noreply.github.com> +* Add `-Wunused-parameter` (`#1756 `_) (`#1757 `_) + (cherry picked from commit be474ec5ba6d0210379d009d518bdd631cc46ad9) + Co-authored-by: Chris Thrasher +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Use pragma once as header include guard (`#1525 `_) (`#1652 `_) diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml index a15c77d6a9..a5722b14a4 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml @@ -1,7 +1,7 @@ moveit_resources_prbt_ikfast_manipulator_plugin - 2.5.4 + 2.5.6 The prbt_ikfast_manipulator_plugin package Alexander Gutenkunst Christian Henkel diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp index a0143f8fa1..ce2497a571 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp @@ -804,12 +804,12 @@ bool IKFastKinematicsPlugin::getPositionFK(const std::vector& link_ return false; } - IkReal angles[num_joints_]; + std::vector angles(num_joints_, 0); for (unsigned char i = 0; i < num_joints_; i++) angles[i] = joint_angles[i]; // IKFast56/61 - ComputeFk(angles, eetrans, eerot); + ComputeFk(angles.data(), eetrans, eerot); for (int i = 0; i < 3; ++i) p_out.p.data[i] = eetrans[i]; diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp index b22d0f73d8..53cb3bfda7 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp @@ -26,8 +26,7 @@ using namespace ikfast; // check if the included ikfast version matches what this file was compiled with -#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[static_cast(x)] -IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a); +static_assert(IKFAST_VERSION==61); // version found in ikfast.h #include #include diff --git a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst index 0858488cf7..b2cb5f9950 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_resources_prbt_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_planners/test_configs/prbt_moveit_config/package.xml b/moveit_planners/test_configs/prbt_moveit_config/package.xml index e8aea7c684..91052a9e43 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/package.xml +++ b/moveit_planners/test_configs/prbt_moveit_config/package.xml @@ -1,6 +1,6 @@ moveit_resources_prbt_moveit_config - 2.5.4 + 2.5.6

MoveIt Resources for testing: Pilz PRBT 6 diff --git a/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst b/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst index 3d750eac77..88c1dc6a80 100644 --- a/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_resources_prbt_pg70_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_planners/test_configs/prbt_pg70_support/package.xml b/moveit_planners/test_configs/prbt_pg70_support/package.xml index e654275965..c07a3a3bff 100644 --- a/moveit_planners/test_configs/prbt_pg70_support/package.xml +++ b/moveit_planners/test_configs/prbt_pg70_support/package.xml @@ -1,7 +1,7 @@ moveit_resources_prbt_pg70_support - 2.5.4 + 2.5.6 PRBT support for Schunk pg70 gripper. Alexander Gutenkunst diff --git a/moveit_planners/test_configs/prbt_support/CHANGELOG.rst b/moveit_planners/test_configs/prbt_support/CHANGELOG.rst index a7ffa1c2f2..191dd12729 100644 --- a/moveit_planners/test_configs/prbt_support/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_support/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package prbt_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_planners/test_configs/prbt_support/package.xml b/moveit_planners/test_configs/prbt_support/package.xml index 538307ce6e..d239a7e228 100644 --- a/moveit_planners/test_configs/prbt_support/package.xml +++ b/moveit_planners/test_configs/prbt_support/package.xml @@ -1,6 +1,6 @@ moveit_resources_prbt_support - 2.5.4 + 2.5.6 Mechanical, kinematic and visual description of the Pilz light weight arm PRBT. diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index fc191d2b42..04b455ec55 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index c8121afc39..47ce7c0ceb 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -2,7 +2,7 @@ moveit_plugins - 2.5.4 + 2.5.6 Metapackage for MoveIt plugins. Henning Kayser diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index e2b95f2e36..86239139b7 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Use the non-deprecated service fields for switching controllers (`#2927 `_) +* Contributors: Sai Kishor Kothakota + +2.5.5 (2023-09-10) +------------------ +* Fix parameters for ros2_control namespaces (`#1833 `_) (`#1897 `_) + Co-authored-by: AndyZe + Co-authored-by: Henning Kayser + (cherry picked from commit 5838ce890975e3a058cdc9ab699b27941374c3a2) + Co-authored-by: Pablo Iñigo Blasco +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Rename MoveItControllerManager. Add deprecation warning (`#1601 `_) (`#1666 `_) diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index cd9195f89a..305b597a26 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -2,7 +2,7 @@ moveit_ros_control_interface - 2.5.4 + 2.5.6 ros_control controller manager interface for MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index f57cae4d1c..6c8319c5fc 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -418,7 +418,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan ControllersMap::iterator c = managed_controllers_.find(it); if (c != managed_controllers_.end()) { // controller belongs to this manager - request->stop_controllers.push_back(c->second.name); + request->deactivate_controllers.push_back(c->second.name); claimed_resources.right.erase(c->second.name); // remove resources } } @@ -430,16 +430,16 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan ControllersMap::iterator c = managed_controllers_.find(it); if (c != managed_controllers_.end()) { // controller belongs to this manager - request->start_controllers.push_back(c->second.name); + request->activate_controllers.push_back(c->second.name); for (const auto& required_resource : c->second.required_command_interfaces) { resources_bimap::right_const_iterator res = claimed_resources.right.find(required_resource); if (res != claimed_resources.right.end()) { // resource is claimed - if (std::find(request->stop_controllers.begin(), request->stop_controllers.end(), res->second) == - request->stop_controllers.end()) + if (std::find(request->deactivate_controllers.begin(), request->deactivate_controllers.end(), + res->second) == request->deactivate_controllers.end()) { - request->stop_controllers.push_back(res->second); // add claiming controller to stop list + request->deactivate_controllers.push_back(res->second); // add claiming controller to stop list } claimed_resources.left.erase(res->second); // remove claimed resources } @@ -451,7 +451,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan // successfully activated or deactivated. request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; - if (!request->start_controllers.empty() || !request->stop_controllers.empty()) + if (!request->activate_controllers.empty() || !request->deactivate_controllers.empty()) { // something to switch? auto result_future = switch_controller_service_->async_send_request(request); if (result_future.wait_for(std::chrono::duration(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout) diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index 4efbe07fa4..e71c2f9757 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Become standard-compatible (`#2895 `_) +* Contributors: Tobias Fischer + +2.5.5 (2023-09-10) +------------------ +* Use emulated time in action-based controller (`#899 `_) (`#1743 `_) + (cherry picked from commit b6fcac8055f54012dd9e698be6e06f70613a5abf) + Co-authored-by: Gaël Écorchard +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Support chained controllers (backport `#1482 `_) (`#1623 `_) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index 311d51f7a5..e6e21efe97 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -145,7 +145,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase do { status = result_future.wait_for(50ms); - if ((status == std::future_status::timeout) and ((node_->now() - start) > timeout)) + if ((status == std::future_status::timeout) && ((node_->now() - start) > timeout)) { RCLCPP_WARN(LOGGER, "waitForExecution timed out"); return false; diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index 68b7e86597..7877d7dcad 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -2,7 +2,7 @@ moveit_simple_controller_manager - 2.5.4 + 2.5.6 A generic, simple controller manager plugin for MoveIt. Michael Görner Henning Kayser diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index e98ec85471..84536990bd 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Free functions for calculating properties of trajectories (`#1503 `_) (`#1657 `_) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 414102e5c5..b9ad0655fd 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -2,7 +2,7 @@ moveit_ros_benchmarks - 2.5.4 + 2.5.6 Enhanced tools for benchmarks in MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_ros/hybrid_planning/CHANGELOG.rst b/moveit_ros/hybrid_planning/CHANGELOG.rst index b0c8ebcec4..e5371d27a1 100644 --- a/moveit_ros/hybrid_planning/CHANGELOG.rst +++ b/moveit_ros/hybrid_planning/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package moveit_hybrid_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Cleanup msg includes: Use C++ instead of C header (backport `#1844 `_) + * Cleanup msg includes: Use C++ instead of C header + * Remove obsolete include: moveit_msgs/srv/execute_known_trajectory.hpp +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml index a65a26b1bd..3d87af0936 100644 --- a/moveit_ros/hybrid_planning/package.xml +++ b/moveit_ros/hybrid_planning/package.xml @@ -1,6 +1,6 @@ moveit_hybrid_planning - 2.5.4 + 2.5.6 Hybrid planning components of MoveIt 2 Sebastian Jahr diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 9f1db90c0c..3e24441408 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Removed plan_with_sensing (`#1142 `_) (`#1647 `_) diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index b6abf73fe1..4a4778e967 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -2,7 +2,7 @@ moveit_ros_move_group - 2.5.4 + 2.5.6 The move_group node for MoveIt Michael Görner Henning Kayser diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index 68939ccee7..e08ac4c6fc 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 866ad9f354..eaddd3e010 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -2,7 +2,7 @@ moveit_ros - 2.5.4 + 2.5.6 Components of MoveIt that use ROS Michael Görner Henning Kayser diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst index a2a4f3f2f9..e06a2f9e86 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Make `moveit_servo` listen to Octomap updates (backport `#2601 `_) (`#2606 `_) +* Use ACM in all MoveIt Servo collision checks (`#2643 `_) +* Contributors: Amal Nanavati, Sebastian Castro, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* [Servo] CI simplification (backport `#1556 `_) (`#1980 `_) +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Cleanup msg includes: Use C++ instead of C header (backport `#1844 `_) + * Cleanup msg includes: Use C++ instead of C header + * Remove obsolete include: moveit_msgs/srv/execute_known_trajectory.hpp +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: AndyZe, Chris Thrasher, Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * [Servo] Remove the option for "stop distance"-based collision checking (`#1574 `_) (`#1663 `_) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index b76ae98b40..94ecc53635 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -80,9 +80,6 @@ class CollisionCheck /** \brief Run one iteration of collision checking */ void run(); - /** \brief Get a read-only copy of the planning scene */ - planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - // Pointer to the ROS node const std::shared_ptr node_; diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index f8d3c57c1b..f8c4c92efc 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -2,7 +2,7 @@ moveit_servo - 2.5.4 + 2.5.6 Provides real-time manipulator Cartesian and joint servoing. Blake Anderson Andy Zelenak diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 14de0cb8ca..69ae157b32 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -77,11 +77,6 @@ CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, const ServoP current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); } -planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const -{ - return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); -} - void CollisionCheck::start() { timer_ = node_->create_wall_timer(std::chrono::duration(period_), [this]() { return run(); }); @@ -101,16 +96,17 @@ void CollisionCheck::run() // Do a timer-safe distance-based collision detection collision_result_.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, - *current_state_); + auto locked_scene = planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); + locked_scene->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, *current_state_, + locked_scene->getAllowedCollisionMatrix()); scene_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; collision_result_.print(); collision_result_.clear(); // Self-collisions and scene collisions are checked separately so different thresholds can be used - getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision( - collision_request_, collision_result_, *current_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix()); + locked_scene->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_, + locked_scene->getAllowedCollisionMatrix()); self_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; collision_result_.print(); diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index e93aaee9d8..f7b77aa8c8 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -90,6 +90,7 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) node_, robot_description_name, "planning_scene_monitor"); planning_scene_monitor_->startStateMonitor(servo_parameters->joint_topic); planning_scene_monitor_->startSceneMonitor(servo_parameters->monitored_planning_scene_topic); + planning_scene_monitor_->startWorldGeometryMonitor(); planning_scene_monitor_->setPlanningScenePublishingFrequency(25); planning_scene_monitor_->getStateMonitor()->enableCopyDynamics(true); planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index d66c0d8c7f..d2eebc7b27 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Switch to clang-format-14 (`#1877 `_) (`#1880 `_) + * Switch to clang-format-14 + * Fix clang-format-14 + (cherry picked from commit 7fa5eaf1ac21ab8a99c5adae53bd0a2d4abf98f6) + Co-authored-by: Henning Kayser +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 7732c5bec0..d4c82d375b 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -2,7 +2,7 @@ moveit_ros_occupancy_map_monitor - 2.5.4 + 2.5.6 Components of MoveIt connecting to occupancy map Henning Kayser Tyler Weaver diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index adf7599b5b..0675313fac 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 0122c740e6..8327b316c1 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -2,7 +2,7 @@ moveit_ros_perception - 2.5.4 + 2.5.6 Components of MoveIt connecting to perception Henning Kayser Tyler Weaver diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 52512d56a3..1189e25ad6 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* PSM: keep references to scene\_ valid upon receiving full scenes (`#2850 `_) +* Protect against zero frequency in TrajectoryMonitorMiddlewareHandler (`#2423 `_) (`#2424 `_) +* Re-enable waiting for current state in MoveItCpp (`#2419 `_) (`#2426 `_) +* Contributors: Robert Haschke, Sebastian Castro, Henning Kayser, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* Update default planning configs to use AddTimeOptimalParameterization (`#2167 `_) (`#2170 `_) + (cherry picked from commit 895e9268bd5d9337bebdede07a7f68a99055a1df) + Co-authored-by: Anthony Baker +* Fix timeout in waitForCurrentState (backport `#1899 `_) (`#1938 `_) + (cherry picked from commit 2c48478ac9b4ccfa6784f0e3a9cbf92ef1ecd363) + Co-authored-by: Carlo Rizzardo + Co-authored-by: Henning Kayser +* Switch to clang-format-14 (`#1877 `_) (`#1880 `_) + * Switch to clang-format-14 + * Fix clang-format-14 + (cherry picked from commit 7fa5eaf1ac21ab8a99c5adae53bd0a2d4abf98f6) + Co-authored-by: Henning Kayser +* Cleanup msg includes: Use C++ instead of C header (backport `#1844 `_) + * Cleanup msg includes: Use C++ instead of C header + * Remove obsolete include: moveit_msgs/srv/execute_known_trajectory.hpp +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Backport to Humble (`#1642 `_) diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 1c8eb04534..c8dd4ea04b 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -116,12 +116,11 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti } // Wait for complete state to be received - // TODO(henningkayser): Fix segfault in waitForCurrentState() - // if (options.wait_for_initial_state_timeout > 0.0) - // { - // return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(node_->now(), - // options.wait_for_initial_state_timeout); - // } + if (options.wait_for_initial_state_timeout > 0.0) + { + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(node_->now(), + options.wait_for_initial_state_timeout); + } return true; } diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index b06277fec0..8d16c607d7 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning - 2.5.4 + 2.5.6 Planning components of MoveIt that use ROS Henning Kayser Tyler Weaver @@ -45,6 +45,7 @@ ament_lint_common ament_cmake_gmock ament_cmake_gtest + moveit_configs_utils ros_testing moveit_resources_panda_moveit_config diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index 35e7bc1b63..2419e5d852 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -46,6 +46,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${MOVEIT_LIB_NAME}_export.h DESTINATIO if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) ament_add_gmock(current_state_monitor_tests test/current_state_monitor_tests.cpp @@ -59,4 +60,10 @@ if(BUILD_TESTING) target_link_libraries(trajectory_monitor_tests ${MOVEIT_LIB_NAME} ) + + ament_add_gtest_executable(planning_scene_monitor_test test/planning_scene_monitor_test.cpp) + target_link_libraries(planning_scene_monitor_test ${MOVEIT_LIB_NAME}) + ament_target_dependencies(planning_scene_monitor_test moveit_core rclcpp moveit_msgs) + add_ros_test(test/launch/planning_scene_monitor.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 27e6d796a1..3a9f84cfdc 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -666,7 +666,21 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann RCLCPP_DEBUG(LOGGER, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.), fmod(last_robot_motion_time_.seconds(), 10.)); old_scene_name = scene_->getName(); - result = scene_->usePlanningSceneMsg(scene); + + if (!scene.is_diff && parent_scene_) + { + // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead + scene_->clearDiffs(); + result = parent_scene_->setPlanningSceneMsg(scene); + // There were no callbacks for individual object changes, so rebuild the octree masks + excludeAttachedBodiesFromOctree(); + excludeWorldObjectsFromOctree(); + } + else + { + result = scene_->setPlanningSceneDiffMsg(scene); + } + if (octomap_monitor_) { if (!scene.is_diff && scene.world.octomap.octomap.data.empty()) @@ -678,23 +692,6 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann } robot_model_ = scene_->getRobotModel(); - // if we just reset the scene completely but we were maintaining diffs, we need to fix that - if (!scene.is_diff && parent_scene_) - { - // the scene is now decoupled from the parent, since we just reset it - scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); - scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); - parent_scene_ = scene_; - scene_ = parent_scene_->diff(); - scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) { - currentStateAttachedBodyUpdateCallback(body, attached); - }); - scene_->setCollisionObjectUpdateCallback( - [this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) { - currentWorldObjectUpdateCallback(object, action); - }); - } if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor_middleware_handle.cpp index 67b75c1006..83fc8a7bb4 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor_middleware_handle.cpp @@ -35,21 +35,28 @@ /* Author: Abishalini Sivaraman */ #include +#include namespace planning_scene_monitor { planning_scene_monitor::TrajectoryMonitorMiddlewareHandle::TrajectoryMonitorMiddlewareHandle(double sampling_frequency) - : rate_{ std::make_unique(sampling_frequency) } { + setRate(sampling_frequency); } void planning_scene_monitor::TrajectoryMonitorMiddlewareHandle::setRate(double sampling_frequency) { - rate_ = std::make_unique(sampling_frequency); + if (sampling_frequency > std::numeric_limits::epsilon()) + { + rate_ = std::make_unique(sampling_frequency); + } } void planning_scene_monitor::TrajectoryMonitorMiddlewareHandle::sleep() { - rate_->sleep(); + if (rate_) + { + rate_->sleep(); + } } } // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py new file mode 100644 index 0000000000..0e9aa717ae --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py @@ -0,0 +1,92 @@ +import os +import launch +import unittest +import launch_ros +import launch_testing +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + output="screen", + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + psm_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "planning_scene_monitor_test", + ] + ), + parameters=[ + moveit_config.to_dict(), + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + launch.actions.TimerAction(period=2.0, actions=[ros2_control_node]), + launch.actions.TimerAction( + period=4.0, actions=[joint_state_broadcaster_spawner] + ), + launch.actions.TimerAction( + period=6.0, actions=[panda_arm_controller_spawner] + ), + launch.actions.TimerAction(period=9.0, actions=[psm_gtest]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "psm_gtest": psm_gtest, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, psm_gtest): + self.proc_info.assertWaitForShutdown(psm_gtest, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, psm_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=psm_gtest) diff --git a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp new file mode 100644 index 0000000000..0547216437 --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, University of Hamburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Michael 'v4hn' Goerner + Desc: Tests for PlanningSceneMonitor +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include + +class PlanningSceneMonitorTest : public ::testing::Test +{ +public: + void SetUp() override + { + test_node_ = std::make_shared("moveit_planning_scene_monitor_test"); + executor_ = std::make_shared(); + planning_scene_monitor_ = std::make_unique( + test_node_, "robot_description", "planning_scene_monitor"); + planning_scene_monitor_->monitorDiffs(true); + scene_ = planning_scene_monitor_->getPlanningScene(); + executor_->add_node(test_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + + // Needed to avoid race conditions on high-load CPUs. + std::this_thread::sleep_for(std::chrono::seconds{ 1 }); + } + + void TearDown() override + { + scene_.reset(); + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + std::shared_ptr test_node_; + + // Executor and a thread to run the executor. + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + planning_scene::PlanningScenePtr scene_; +}; + +// various code expects the monitored scene to remain the same +TEST_F(PlanningSceneMonitorTest, TestPersistentScene) +{ + auto scene{ planning_scene_monitor_->getPlanningScene() }; + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = msg.robot_state.is_diff = true; + planning_scene_monitor_->newPlanningSceneMessage(msg); + EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene()); + msg.is_diff = msg.robot_state.is_diff = false; + planning_scene_monitor_->newPlanningSceneMessage(msg); + EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene()); +} + +using UpdateType = planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType; + +#define TRIGGERS_UPDATE(msg, expected_update_type) \ + { \ + planning_scene_monitor_->clearUpdateCallbacks(); \ + auto received_update_type{ UpdateType::UPDATE_NONE }; \ + planning_scene_monitor_->addUpdateCallback([&](auto type) { received_update_type = type; }); \ + planning_scene_monitor_->newPlanningSceneMessage(msg); \ + EXPECT_EQ(received_update_type, expected_update_type); \ + } + +TEST_F(PlanningSceneMonitorTest, UpdateTypes) +{ + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = msg.robot_state.is_diff = true; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_NONE); + + msg.fixed_frame_transforms.emplace_back(); + msg.fixed_frame_transforms.back().header.frame_id = "base_link"; + msg.fixed_frame_transforms.back().child_frame_id = "object"; + msg.fixed_frame_transforms.back().transform.rotation.w = 1.0; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_TRANSFORMS); + msg.fixed_frame_transforms.clear(); + moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state, false); + msg.robot_state.is_diff = true; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE); + + msg.robot_state.is_diff = false; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY); + + msg.robot_state = moveit_msgs::msg::RobotState{}; + msg.robot_state.is_diff = true; + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = "base_link"; + collision_object.id = "object"; + collision_object.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_object.pose.orientation.w = 1.0; + collision_object.primitives.emplace_back(); + collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE; + collision_object.primitives.back().dimensions = { 1.0 }; + msg.world.collision_objects.emplace_back(collision_object); + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_GEOMETRY); + + msg.world.collision_objects.clear(); + msg.is_diff = false; + + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_SCENE); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 18d8434d51..c83a225111 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* 400% speed up to move group interface (`#1865 `_) (`#1867 `_) + (cherry picked from commit 0642ef064df512566ffb171f5a889f4c7886110d) + Co-authored-by: azalutsky +* Cleanup msg includes: Use C++ instead of C header (backport `#1844 `_) + * Cleanup msg includes: Use C++ instead of C header + * Remove obsolete include: moveit_msgs/srv/execute_known_trajectory.hpp +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index f722681ca1..6c8d94c701 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -117,6 +117,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl throw std::runtime_error(error); } + setStartStateToCurrentState(); joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); joint_state_target_ = std::make_shared(getRobotModel()); @@ -384,26 +385,30 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return *joint_state_target_; } + void setStartState(const moveit_msgs::msg::RobotState& start_state) + { + considered_start_state_ = start_state; + } + void setStartState(const moveit::core::RobotState& start_state) { - considered_start_state_ = std::make_shared(start_state); + considered_start_state_ = moveit_msgs::msg::RobotState(); + moveit::core::robotStateToRobotStateMsg(start_state, considered_start_state_, true); } void setStartStateToCurrentState() { - considered_start_state_.reset(); + // set message to empty diff + considered_start_state_ = moveit_msgs::msg::RobotState(); + considered_start_state_.is_diff = true; } moveit::core::RobotStatePtr getStartState() { - if (considered_start_state_) - return considered_start_state_; - else - { - moveit::core::RobotStatePtr s; - getCurrentState(s); - return s; - } + moveit::core::RobotStatePtr s; + getCurrentState(s); + moveit::core::robotStateMsgToRobotState(considered_start_state_, *s, true); + return s; } bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link, @@ -955,11 +960,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; - if (considered_start_state_) - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state); - else - req->start_state.is_diff = true; - + req->start_state = considered_start_state_; req->group_name = opt_.group_name_; req->header.frame_id = getPoseReferenceFrame(); req->header.stamp = getClock()->now(); @@ -1093,6 +1094,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } + void constructRobotState(moveit_msgs::msg::RobotState& state) const + { + state = considered_start_state_; + } + void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name_; @@ -1103,11 +1109,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.pipeline_id = planning_pipeline_id_; request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; - - if (considered_start_state_) - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - else - request.start_state.is_diff = true; + request.start_state = considered_start_state_; if (active_target_ == JOINT) { @@ -1341,7 +1343,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::shared_ptr> execute_action_client_; // general planning params - moveit::core::RobotStatePtr considered_start_state_; + moveit_msgs::msg::RobotState considered_start_state_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; double allowed_planning_time_; std::string planning_pipeline_id_; @@ -1626,16 +1628,7 @@ void MoveGroupInterface::stop() void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start_state) { - moveit::core::RobotStatePtr rs; - if (start_state.is_diff) - impl_->getCurrentState(rs); - else - { - rs = std::make_shared(getRobotModel()); - rs->setToDefaultValues(); // initialize robot state values for conversion - } - moveit::core::robotStateMsgToRobotState(start_state, *rs); - setStartState(*rs); + impl_->setStartState(start_state); } void MoveGroupInterface::setStartState(const moveit::core::RobotState& start_state) diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 553004a1a9..3b9150ec08 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning_interface - 2.5.4 + 2.5.6 Components of MoveIt that offer simpler interfaces to planning and execution Henning Kayser Tyler Weaver diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 7cf62af488..b62da5d58e 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Add moveit_core dependency to robot_interaction (`#1617 `_) (`#1618 `_) diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index d9cd91f15d..69dc2fe91c 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -2,7 +2,7 @@ moveit_ros_robot_interaction - 2.5.4 + 2.5.6 Components of MoveIt that offer interaction via interactive markers Henning Kayser Tyler Weaver diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 7ca9e241ec..f1d36e3e50 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,37 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Attached Collision Object Transparency (`#3093 `_) (`#3099 `_) +* fix for not having transparency in collision scenes on rviz, backporting the fix to humble (`#2929 `_) +* Check valid interactive marker pointer before trying to update pose (`#1581 `_) (`#2366 `_) +* Contributors: Sami Alperen Akgün, Aiden Neale, Sebastian Castro, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* Doxygen tag (backport `#1955 `_) (`#1958 `_) + * Generate Doxygen Tag + * Install tagfile in output directory + * Fix problematic override for Doxygen linking + (cherry picked from commit 752571e9ff027b3137b9720227681ed6b57e42d6) + Co-authored-by: Henning Kayser +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Use pragma once as header include guard (`#1525 `_) (`#1652 `_) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index e3b915fd1e..2caade8e88 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1422,8 +1422,9 @@ void MotionPlanningDisplay::fixedFrameChanged() if (int_marker_display_) int_marker_display_->setFixedFrame(fixed_frame_); // When the fixed frame changes we need to tell RViz to update the rendered interactive marker display - frame_->scene_marker_->requestPoseUpdate(frame_->scene_marker_->getPosition(), - frame_->scene_marker_->getOrientation()); + if (frame_ && frame_->scene_marker_) + frame_->scene_marker_->requestPoseUpdate(frame_->scene_marker_->getPosition(), + frame_->scene_marker_->getOrientation()); changedPlanningGroup(); } diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 0ed4afe680..7fed67a50c 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -2,7 +2,7 @@ moveit_ros_visualization - 2.5.4 + 2.5.6 Components of MoveIt that offer visualization Henning Kayser Tyler Weaver diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index a9d6419730..f336213066 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -347,10 +347,10 @@ void PlanningSceneDisplay::changedSceneName() void PlanningSceneDisplay::renderPlanningScene() { QColor color = scene_color_property_->getColor(); - Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF(), scene_alpha_property_->getFloat()); if (attached_body_color_property_) color = attached_body_color_property_->getColor(); - Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF(), robot_alpha_property_->getFloat()); try { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp index 40afa630c6..194584f88e 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp @@ -140,7 +140,11 @@ void MeshShape::clear() if (entity_) { entity_->detachFromParent(); - Ogre::MeshManager::getSingleton().remove(entity_->getMesh()->getName()); + const auto& mesh_name = entity_->getMesh()->getName(); + if (Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().getByName(mesh_name)) + { + Ogre::MeshManager::getSingleton().remove(mesh); + } scene_manager_->destroyEntity(entity_); entity_ = nullptr; } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 30b93ce190..9d2f396c0b 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -91,7 +91,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen color.r = default_attached_color.r; color.g = default_attached_color.g; color.b = default_attached_color.b; - color.a = 1.0f; + color.a = default_attached_color.a; planning_scene::ObjectColorMap color_map; scene->getKnownObjectColors(color_map); scene_robot_->update(moveit::core::RobotStateConstPtr(rs), color, color_map); @@ -109,6 +109,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen color.r = c.r; color.g = c.g; color.b = c.b; + color.a = c.a; alpha = c.a; } for (std::size_t j = 0; j < object->shapes_.size(); ++j) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 95cc5ca8d7..88bfa66be8 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -137,7 +137,7 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt RCLCPP_ERROR_STREAM(LOGGER, "Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); continue; } - Ogre::ColourValue rcolor(color.r, color.g, color.b); + Ogre::ColourValue rcolor(color.r, color.g, color.b, color.a); const EigenSTL::vector_Isometry3d& ab_t = attached_body->getShapePosesInLinkFrame(); const std::vector& ab_shapes = attached_body->getShapes(); for (std::size_t j = 0; j < ab_shapes.size(); ++j) diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 6ba18e9d11..43e265a337 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Use node logging in warehouse broadcast (`#2432 `_) (`#2443 `_) +* Contributors: Tyler Weaver, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) (`#2277 `_) + (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) + Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Restructure moveit warehouse package (`#1551 `_) (`#1661 `_) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 4454ce8dbb..db61bbaa54 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -2,7 +2,7 @@ moveit_ros_warehouse - 2.5.4 + 2.5.6 Components of MoveIt connecting to MongoDB Henning Kayser Tyler Weaver diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index d5d1c01a2a..8afbd22254 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -59,8 +59,6 @@ static const std::string CONSTRAINTS_TOPIC = "constraints"; static const std::string STATES_TOPIC = "robot_states"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.broadcast"); - using namespace std::chrono_literals; int main(int argc, char** argv) @@ -70,6 +68,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("publish_warehouse_data", node_options); + const auto logger = node->get_logger(); // time to wait in between publishing messages double delay = 0.001; @@ -136,7 +135,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(LOGGER, "Publishing scene '%s'", + RCLCPP_INFO(logger, "Publishing scene '%s'", pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str()); pub_scene->publish(static_cast(*pswm)); executor.spin_once(0ns); @@ -149,14 +148,14 @@ int main(int argc, char** argv) std::vector planning_queries; std::vector query_names; pss.getPlanningQueries(planning_queries, query_names, pswm->name); - RCLCPP_INFO(LOGGER, "There are %d planning queries associated to the scene", + RCLCPP_INFO(logger, "There are %d planning queries associated to the scene", static_cast(planning_queries.size())); rclcpp::sleep_for(500ms); for (std::size_t i = 0; i < planning_queries.size(); ++i) { if (req) { - RCLCPP_INFO(LOGGER, "Publishing query '%s'", query_names[i].c_str()); + RCLCPP_INFO(logger, "Publishing query '%s'", query_names[i].c_str()); pub_req->publish(static_cast(*planning_queries[i])); executor.spin_once(0ns); } @@ -190,7 +189,7 @@ int main(int argc, char** argv) moveit_warehouse::ConstraintsWithMetadata cwm; if (cs.getConstraints(cwm, cname)) { - RCLCPP_INFO(LOGGER, "Publishing constraints '%s'", + RCLCPP_INFO(logger, "Publishing constraints '%s'", cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str()); pub_constr->publish(static_cast(*cwm)); executor.spin_once(0ns); @@ -212,7 +211,7 @@ int main(int argc, char** argv) moveit_warehouse::RobotStateWithMetadata rswm; if (rs.getRobotState(rswm, rname)) { - RCLCPP_INFO(LOGGER, "Publishing state '%s'", + RCLCPP_INFO(logger, "Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str()); pub_state->publish(static_cast(*rswm)); executor.spin_once(0ns); @@ -222,7 +221,7 @@ int main(int argc, char** argv) } rclcpp::sleep_for(1s); - RCLCPP_INFO(LOGGER, "Done."); + RCLCPP_INFO(logger, "Done."); return 0; } diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index 5ecc7f8c04..1193e18b2c 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Uncomment moveit_ros_perception dependency (`#1463 `_) (`#1644 `_) diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 27bf6d80df..4ea8994649 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -2,7 +2,7 @@ moveit_runtime - 2.5.4 + 2.5.6 moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Henning Kayser Tyler Weaver diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst index 08af8a09df..f336b9f4de 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package moveit_setup_app_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* add missing dependencies on config utils (backport `#1962 `_) (`#2206 `_) + when installing ros-humble-moveit-setup-assistant from debs, + the package cannot currently run due to this missing depend + (cherry picked from commit cc635471aadfb9446398ece319ae31c6b72bec86) + Co-authored-by: Michael Ferguson +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Contributors: Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml index 111550cbd3..82f4eefa41 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml @@ -2,7 +2,7 @@ moveit_setup_app_plugins - 2.5.4 + 2.5.6 Various specialty plugins for MoveIt Setup Assistant David V. Lu!! BSD diff --git a/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst index f70791e4f3..1ad8c34f0e 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_setup_assistant/moveit_setup_assistant/package.xml b/moveit_setup_assistant/moveit_setup_assistant/package.xml index 391aa81b3a..7ace434e29 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/moveit_setup_assistant/package.xml @@ -2,7 +2,7 @@ moveit_setup_assistant - 2.5.4 + 2.5.6 Generates a configuration package that makes it easy to use MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst index 02fe04519d..79300cd043 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst @@ -2,6 +2,50 @@ Changelog for package moveit_setup_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Don't assume gripper controller for single joint control in MoveIt Setup Assistant (backport `#2555 `_) (`#2559 `_) + * For single joint controllers which are not gripper controllers, still output joints list + * Use OR + * Only check for GripperActionController + Co-authored-by: Sebastian Jahr + --------- + Co-authored-by: Sebastian Jahr + (cherry picked from commit 81094a63898ace7829687d2d6aa3ccb3cdd81b58) + Co-authored-by: Forrest Rogers-Marcovitz <39061824+forrest-rm@users.noreply.github.com> +* Contributors: Forrest Rogers-Marcovitz, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* add missing dependencies on config utils (backport `#1962 `_) (`#2206 `_) + when installing ros-humble-moveit-setup-assistant from debs, + the package cannot currently run due to this missing depend + (cherry picked from commit cc635471aadfb9446398ece319ae31c6b72bec86) + Co-authored-by: Michael Ferguson +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * MSA: write the default controller namespace (`#1515 `_) (`#1651 `_) diff --git a/moveit_setup_assistant/moveit_setup_controllers/package.xml b/moveit_setup_assistant/moveit_setup_controllers/package.xml index f84640d2f9..ba71394c5e 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/package.xml +++ b/moveit_setup_assistant/moveit_setup_controllers/package.xml @@ -2,7 +2,7 @@ moveit_setup_controllers - 2.5.4 + 2.5.6 MoveIt Setup Steps for ROS 2 Control David V. Lu!! BSD diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp index 3ce21bffe6..97f1907211 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp @@ -192,7 +192,7 @@ bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter& emitter << YAML::Value; emitter << YAML::BeginMap; { - if (ci.joints_.size() != 1) + if (ci.type_ != "position_controllers/GripperActionController") { emitter << YAML::Key << "joints" << YAML::Value << ci.joints_; } diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst index e75e8a731b..03d9d51830 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_setup_core_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Contributors: Chris Thrasher + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml index 6de3e2a8a4..1db940cea3 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml @@ -2,7 +2,7 @@ moveit_setup_core_plugins - 2.5.4 + 2.5.6 Core (meta) plugins for MoveIt Setup Assistant David V. Lu!! BSD diff --git a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst index db60081d65..380600107b 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package moveit_setup_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ +* Cast of "max_velocity" and "max_acceleration" values to double (`#2803 `_) (`#3038 `_) +* Fix `#1971 `_ (`#2428 `_) (`#2430 `_) +* Contributors: Michael Ferguson, Jorge Pérez Ramos, David V. Lu!!, mergify[bot] + +2.5.5 (2023-09-10) +------------------ +* Fix clang compiler warnings (backport of `#1712 `_) (`#1896 `_) + - Fix warning: definition of implicit copy assignment operator is deprecated + - Fix warning: expression with side effects will be evaluated + - Fix warning: passing by value + - Enable -Werror + - Fix -Wdelete-non-abstract-non-virtual-dtor + - Fix more clang warnings + - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE + - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite + - Add default copy/move constructors/assignment operators + As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, + we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. + - Explicitly declare overrides + - Add default constructors as they are not implicitly declared anymore + - Declare selected classes as final + - Add noexcept specifier to constructors + - Fixup gmock/gtest warnings +* Use <> for non-local headers (`#1765 `_) + Unless a header lives in the same or a child directory of the file + including it, it's recommended to use <> for the #include statement. + For more information, see the C++ Core Guidelines item SF.12 + https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else + (cherry picked from commit 7a1f2a101f9aeb8557e8a31656bbe1a6d53b430e) +* Re-enable clang-tidy check `performance-unnecessary-value-param` (backport `#1703 `_) + * Re-enable clang-tidy check performance-unnecessary-value-param (`#1703 `_) + * Fix clang-tidy issues (`#1706 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Robert Haschke +* Contributors: Chris Thrasher, Robert Haschke, mergify[bot] + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_setup_assistant/moveit_setup_framework/package.xml b/moveit_setup_assistant/moveit_setup_framework/package.xml index 89b7b9d773..dff74a6e3e 100644 --- a/moveit_setup_assistant/moveit_setup_framework/package.xml +++ b/moveit_setup_assistant/moveit_setup_framework/package.xml @@ -2,7 +2,7 @@ moveit_setup_framework - 2.5.4 + 2.5.6 C++ Interface for defining setup steps for MoveIt Setup Assistant David V. Lu!! BSD diff --git a/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp b/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp index 1bcf89645c..97ebfb3aa7 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp @@ -38,6 +38,7 @@ #include #include #include +#include namespace moveit_setup { @@ -74,6 +75,9 @@ void RVizPanel::initialize() rviz_manager_->initialize(); rviz_manager_->startUpdate(); + auto tm = rviz_manager_->getToolManager(); + tm->addTool("rviz_default_plugins/MoveCamera"); + // Create the MoveIt Rviz Plugin and attach to display robot_state_display_ = new moveit_rviz_plugin::RobotStateDisplay(); robot_state_display_->setName("Robot State"); diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp index 9058aea1c0..716c12da96 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -296,7 +296,7 @@ bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) // Output property emitter << YAML::Key << "max_velocity"; - emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); + emitter << YAML::Value << static_cast(std::min(fabs(b.max_velocity_), fabs(b.min_velocity_))); // Output property emitter << YAML::Key << "has_acceleration_limits"; @@ -307,7 +307,7 @@ bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) // Output property emitter << YAML::Key << "max_acceleration"; - emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); + emitter << YAML::Value << static_cast(std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_))); emitter << YAML::EndMap; } diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst index 29b12c3b99..45adf855ff 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_setup_srdf_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.6 (2024-11-17) +------------------ + +2.5.5 (2023-09-10) +------------------ + 2.5.4 (2022-11-04) ------------------ * Improve CMake usage (`#1550 `_) (`#1555 `_) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml index 25780235ca..45a82eee64 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml @@ -2,7 +2,7 @@ moveit_setup_srdf_plugins - 2.5.4 + 2.5.6 SRDF-based plugins for MoveIt Setup Assistant David V. Lu!! BSD