diff --git a/.clang-format b/.clang-format index 9a6ec509a4..13ed70e95b 100644 --- a/.clang-format +++ b/.clang-format @@ -59,6 +59,10 @@ SpaceBeforeAssignmentOperators: true # Configure each individual brace in BraceWrapping BreakBeforeBraces: Custom +# Qualifiers (const, volatile, static, etc) +QualifierAlignment: Custom +QualifierOrder: ['static', 'inline', 'constexpr', 'const', 'volatile', 'type'] + # Control of individual brace wrapping cases BraceWrapping: AfterCaseLabel: true diff --git a/.codespell_words b/.codespell_words index e16efbf946..33d567ff91 100644 --- a/.codespell_words +++ b/.codespell_words @@ -15,3 +15,5 @@ uint whis sinic padd +brin +aas diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile new file mode 100644 index 0000000000..325949b2ac --- /dev/null +++ b/.docker/tutorial-source/Dockerfile @@ -0,0 +1,42 @@ +# syntax = docker/dockerfile:1.3 + +# ghcr.io/ros-planning/moveit2:main-${ROS_DISTRO}-tutorial-source +# Source build of the repos file from the tutorail site + +ARG ROS_DISTRO=rolling +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 https://github.com/ros-planning/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/mergify.yml b/.github/mergify.yml index 9c1dfcdcab..7d5bc4a548 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -26,6 +26,15 @@ pull_request_rules: branches: - humble + - name: backport to iron at reviewers discretion + conditions: + - base=main + - "label=backport-iron" + actions: + backport: + branches: + - iron + - name: ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8a58adcb94..a5977f405b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -23,10 +23,6 @@ jobs: ROS_DISTRO: rolling IKFAST_TEST: true CLANG_TIDY: pedantic - # Silent gmock/gtest warnings: https://github.com/ament/googletest/pull/21 - AFTER_BUILD_UPSTREAM_WORKSPACE: | - git clone --quiet --branch clalancette/update-to-gtest-1.11 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest" - builder_run_build "/opt/ros/${ROS_DISTRO}" "${BASEDIR}/upstream_ws" --packages-select gtest_vendor gmock_vendor - IMAGE: humble-ci ROS_DISTRO: humble - IMAGE: humble-ci-testing @@ -85,7 +81,7 @@ jobs: # free up a lot of stuff from /usr/local sudo rm -rf /usr/local df -h - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: testspace-com/setup-testspace@v1 if: github.repository == 'ros-planning/moveit2' with: diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 68ac0ba0e0..3a1bed88fd 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -19,7 +19,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [humble, rolling] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -31,22 +31,22 @@ jobs: steps: - 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@v5 with: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} @@ -60,7 +60,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [humble, rolling] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -72,22 +72,22 @@ jobs: steps: - 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@v5 with: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} @@ -101,7 +101,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [humble, rolling] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -113,22 +113,22 @@ jobs: steps: - 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@v5 with: file: .docker/${{ github.job }}/Dockerfile build-args: OUR_ROS_DISTRO=${{ matrix.ROS_DISTRO }} @@ -143,7 +143,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [humble, rolling] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -154,26 +154,26 @@ jobs: PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/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@v5 with: context: . file: .docker/${{ github.job }}/Dockerfile diff --git a/.github/workflows/docker_lint.yaml b/.github/workflows/docker_lint.yaml index 4aef41010a..2806d563ad 100644 --- a/.github/workflows/docker_lint.yaml +++ b/.github/workflows/docker_lint.yaml @@ -21,7 +21,7 @@ jobs: name: Lint Dockerfiles runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: hadolint/hadolint-action@v3.1.0 with: dockerfile: .docker/${{ matrix.DOCKERFILE_PATH }}/Dockerfile diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 75a0bf6c9f..0be2212627 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -15,7 +15,7 @@ jobs: name: Format runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v4 with: python-version: '3.10' diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index 9f76ded945..28e0eed4fc 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -11,7 +11,7 @@ jobs: strategy: fail-fast: false matrix: - distro: [humble, rolling] + distro: [rolling] env: ROS_DISTRO: ${{ matrix.distro }} @@ -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/sonar.yaml b/.github/workflows/sonar.yaml index d50f260e98..281894af23 100644 --- a/.github/workflows/sonar.yaml +++ b/.github/workflows/sonar.yaml @@ -55,7 +55,7 @@ jobs: # free up a lot of stuff from /usr/local sudo rm -rf /usr/local df -h - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: testspace-com/setup-testspace@v1 if: github.repository == 'ros-planning/moveit2' with: @@ -107,7 +107,7 @@ jobs: run: cat ${{ github.workspace }}/.work/target_ws/coverage.info - name: Install sonar-scanner if: steps.ici.outputs.target_test_results == '0' - uses: SonarSource/sonarcloud-github-c-cpp@v1 + uses: SonarSource/sonarcloud-github-c-cpp@v2 - name: Run sonar-scanner if: steps.ici.outputs.target_test_results == '0' env: diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml new file mode 100644 index 0000000000..4eb2156397 --- /dev/null +++ b/.github/workflows/tutorial_docker.yaml @@ -0,0 +1,94 @@ +name: "Tutorial Docker Images" + +on: + schedule: + # 5 PM UTC every Sunday + - cron: '0 17 * * 6' + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + tutorial-source: + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling, humble, iron] + runs-on: ubuntu-latest + permissions: + packages: write + contents: read + env: + GH_IMAGE: ghcr.io/ros-planning/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }} + DH_IMAGE: moveit/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/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@v3 + 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@v2.1.2 + with: + cache-source: .ccache + cache-target: /root/.ccache/ + - name: Build and Push + uses: docker/build-push-action@v5 + 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 == 'ros-planning/moveit2') + uses: actions/github-script@v6 + 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: ros-planning + PACKAGE_NAME: moveit2 + PER_PAGE: 100 diff --git a/.gitignore b/.gitignore index fe1224a00f..82f4adc900 100644 --- a/.gitignore +++ b/.gitignore @@ -56,7 +56,7 @@ qtcreator-* # Vim *.swp -# Continous Integration +# Continuous Integration .moveit_ci *.pyc diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e046b741aa..b18718e8cc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-case-conflict @@ -33,7 +33,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 23.3.0 + rev: 23.10.0 hooks: - id: black @@ -47,7 +47,7 @@ repos: files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 + rev: v2.2.6 hooks: - id: codespell args: ['--write-changes', '--ignore-words=.codespell_words', '--skip="*.eps"'] diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 2958875f19..a6d3f541fb 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit/package.xml b/moveit/package.xml index 3fadb67820..dd3b317079 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -2,7 +2,7 @@ moveit - 2.7.4 + 2.8.0 Meta package that contains all essential packages of MoveIt 2 Henning Kayser Tyler Weaver diff --git a/moveit_common/CHANGELOG.rst b/moveit_common/CHANGELOG.rst index 4ee4dae7da..69d12df673 100644 --- a/moveit_common/CHANGELOG.rst +++ b/moveit_common/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_common/package.xml b/moveit_common/package.xml index 5379c26aa8..ac80e85bd2 100644 --- a/moveit_common/package.xml +++ b/moveit_common/package.xml @@ -2,7 +2,7 @@ moveit_common - 2.7.4 + 2.8.0 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 152a1f7745..5faf3d4a25 100644 --- a/moveit_configs_utils/CHANGELOG.rst +++ b/moveit_configs_utils/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_configs_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Add stomp default config to moveit_config_utils (`#2238 `_) +* Contributors: Sebastian Jahr + 2.7.4 (2023-05-18) ------------------ * Parse xacro args from .setup_assistant config in MoveIt Configs Builder (`#2172 `_) diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index 6a29b05f5c..1d734d2f98 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -2,7 +2,7 @@ moveit_configs_utils - 2.7.4 + 2.8.0 Python library for loading moveit config parameters in launch files MoveIt Release Team BSD-3-Clause diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 03406fa217..e117b65a36 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version="2.7.4", + version="2.8.0", 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 5d1576f473..5d84457c6b 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,37 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Add a benchmark for 'getJacobian' (`#2326 `_) +* [TOTG] Exit loop when position can't change (`#2307 `_) +* Remove added path index from planner adapter function signature (`#2285 `_) +* Fix typo in error message (`#2286 `_) +* Fix comment formatting (`#2276 `_) +* Cleanup planning request adapter interface (`#2266 `_) + * Use default arguments instead of additional functions + * Use generate param lib for default plan request adapters + * Small cleanup of ResolveConstraintFrames + * Remove dublicate yaml file entry + * Move list_planning_adapter_plugins into own directory + * Apply suggestions from code review + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + * Fix copy& paste error + * Update parameter descriptions + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + * Apply suggestions from code review + Co-authored-by: Kyle Cesare + * EMPTY_PATH_INDEX_VECTOR -> empty_path_index_vector + * Update parameter yaml + * Make param listener unique + * Fix build error + * Use gt_eq instead of deprecated lower_bounds + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Kyle Cesare +* fix for kinematic constraints parsing (`#2267 `_) +* Contributors: Jorge Nicho, Mario Prats, Nacho Mellado, Sebastian Jahr, Stephanie Eng + 2.7.4 (2023-05-18) ------------------ * Add documentation and cleanups for PlanningRequestAdapter and PlanningRequestAdapterChain classes (`#2142 `_) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 834744b246..5f9f358d84 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -5,16 +5,6 @@ project(moveit_core LANGUAGES CXX) find_package(moveit_common REQUIRED) moveit_package() -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(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(Bullet 2.87 REQUIRED) @@ -22,6 +12,7 @@ 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) @@ -148,6 +139,7 @@ ament_export_dependencies( eigen_stl_containers Eigen3 eigen3_cmake_module + fcl generate_parameter_library geometric_shapes geometry_msgs diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index b2c9ad1043..034f3bf537 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -64,10 +64,10 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf) for (const std::string& name : srdf.getNoDefaultCollisionLinks()) setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); // re-enable specific collision pairs - for (auto const& collision : srdf.getEnabledCollisionPairs()) + for (const auto& collision : srdf.getEnabledCollisionPairs()) setEntry(collision.link1_, collision.link2_, false); // *finally* disable selected collision pairs - for (auto const& collision : srdf.getDisabledCollisionPairs()) + for (const auto& collision : srdf.getDisabledCollisionPairs()) setEntry(collision.link1_, collision.link2_, true); } diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index 23a523aaf4..3840497943 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -48,21 +48,19 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_octomap_filter"); // forward declarations -bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value, - const double& r_multiple, const octomath::Vector3& contact_point, - octomath::Vector3& normal, double& depth, bool estimate_depth); +bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value, + double r_multiple, const octomath::Vector3& contact_point, octomath::Vector3& normal, + double& depth, bool estimate_depth); -bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value, - const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point, - octomath::Vector3& normal); +bool findSurface(const octomap::point3d_list& cloud, double spacing, double iso_value, double r_multiple, + const octomath::Vector3& seed, octomath::Vector3& surface_point, octomath::Vector3& normal); -bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple, +bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_multiple, const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient); int collision_detection::refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res, - const double cell_bbx_search_distance, - const double allowed_angle_divergence, const bool estimate_depth, - const double iso_value, const double metaball_radius_multiple) + double cell_bbx_search_distance, double allowed_angle_divergence, + bool estimate_depth, double iso_value, double metaball_radius_multiple) { if (!object) { @@ -98,13 +96,13 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec continue; } - double cell_size = 0; if (!object->shapes_.empty()) { const shapes::ShapeConstPtr& shape = object->shapes_[0]; const std::shared_ptr shape_octree = std::dynamic_pointer_cast(shape); if (shape_octree) { + double cell_size = 0; std::shared_ptr octree = shape_octree->octree; cell_size = octree->getResolution(); for (auto& contact_info : contact_vector) @@ -170,11 +168,10 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec return modified; } -bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value, - const double& r_multiple, const octomath::Vector3& contact_point, - octomath::Vector3& normal, double& depth, const bool estimate_depth) +bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value, + double r_multiple, const octomath::Vector3& contact_point, octomath::Vector3& normal, + double& depth, bool estimate_depth) { - double intensity; if (estimate_depth) { octomath::Vector3 surface_point; @@ -191,6 +188,7 @@ bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const doub else // just get normals, no depth { octomath::Vector3 gradient; + double intensity = 0; if (sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient)) { normal = gradient.normalized(); @@ -207,15 +205,13 @@ bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const doub // This algorithm is from Salisbury & Tarr's 1997 paper. It will find the // closest point on the surface starting from a seed point that is close by // following the direction of the field gradient. -bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value, - const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point, - octomath::Vector3& normal) +bool findSurface(const octomap::point3d_list& cloud, double spacing, double iso_value, double r_multiple, + const octomath::Vector3& seed, octomath::Vector3& surface_point, octomath::Vector3& normal) { - const double epsilon = 1e-10; + octomath::Vector3 p = seed, dp, gs; const int iterations = 10; + const double epsilon = 1e-10; double intensity = 0; - - octomath::Vector3 p = seed, dp, gs; for (int i = 0; i < iterations; ++i) { if (!sampleCloud(cloud, spacing, r_multiple, p, intensity, gs)) @@ -234,7 +230,7 @@ bool findSurface(const octomap::point3d_list& cloud, const double& spacing, cons // return p; } -bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple, +bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_multiple, const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient) { intensity = 0.f; diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h index 10a4814508..946fd81125 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h @@ -76,7 +76,7 @@ class BulletBVHManager * @return true if successfully disabled, otherwise false. */ bool disableCollisionObject(const std::string& name); - /**@brief Set a single static collision object's tansform + /**@brief Set a single static collision object's transform * @param name The name of the object * @param pose The transformation in world */ void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h index e440420a9b..5435c6419d 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h @@ -57,7 +57,7 @@ class BulletCastBVHManager : public BulletBVHManager * This is to be used for multi threaded applications. A user should make a clone for each thread. */ BulletCastBVHManagerPtr clone() const; - /**@brief Set a single cast (moving) collision object's tansforms + /**@brief Set a single cast (moving) collision object's transforms * * This should only be used for moving objects. * diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h index 8d75d3da4f..7854022874 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -96,8 +96,8 @@ class CollisionEnvBullet : public CollisionEnv void updatedPaddingOrScaling(const std::vector& links) override; /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ - void addAttachedOjects(const moveit::core::RobotState& state, - std::vector& cows) const; + void addAttachedObjects(const moveit::core::RobotState& state, + std::vector& cows) const; /** \brief Bundles the different checkSelfCollision functions into a single function */ void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index 36c7894d27..ad1d8cf7d3 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -119,7 +119,7 @@ void CollisionEnvBullet::checkSelfCollisionHelper(const CollisionRequest& req, C std::lock_guard guard(collision_env_mutex_); std::vector cows; - addAttachedOjects(state, cows); + addAttachedObjects(state, cows); if (req.distance) { @@ -187,7 +187,7 @@ void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req, } std::vector attached_cows; - addAttachedOjects(state, attached_cows); + addAttachedObjects(state, attached_cows); updateTransformsFromState(state, manager_); for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) @@ -213,7 +213,7 @@ void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& re std::lock_guard guard(collision_env_mutex_); std::vector attached_cows; - addAttachedOjects(state1, attached_cows); + addAttachedObjects(state1, attached_cows); for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) { @@ -331,8 +331,8 @@ void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Ac } } -void CollisionEnvBullet::addAttachedOjects(const moveit::core::RobotState& state, - std::vector& cows) const +void CollisionEnvBullet::addAttachedObjects(const moveit::core::RobotState& state, + std::vector& cows) const { std::vector attached_bodies; state.getAttachedBodies(attached_bodies); diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index ad85cb8105..ac1179c93c 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -16,11 +16,11 @@ ament_target_dependencies(moveit_collision_detection_fcl urdf urdfdom urdfdom_headers - LIBFCL visualization_msgs ) target_link_libraries(moveit_collision_detection_fcl moveit_collision_detection + fcl ) add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp) diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 6ceaa96ccd..f89864803d 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -43,7 +43,7 @@ #include #include -const static double EPSILON{ 0.0001 }; +static const double EPSILON{ 0.0001 }; namespace collision_detection { diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index a5ea115b27..1df9d8c7af 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -198,7 +198,7 @@ void PR2ArmIK::getSolverInfo(moveit_msgs::msg::KinematicSolverInfo& info) info = solver_info_; } -void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& t1_in, +void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, double t1_in, std::vector >& solution) const { // t1 = shoulder/turret pan is specified @@ -463,7 +463,7 @@ void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& } } -void PR2ArmIK::computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double& t3, +void PR2ArmIK::computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double t3, std::vector >& solution) const { std::vector solution_ik(NUM_JOINTS_ARM7DOF, 0.0); @@ -779,7 +779,7 @@ bool PR2ArmIK::checkJointLimits(const std::vector& joint_values) const return true; } -bool PR2ArmIK::checkJointLimits(const double& joint_value, const int& joint_num) const +bool PR2ArmIK::checkJointLimits(const double joint_value, const int joint_num) const { double jv; if (continuous_joint_[joint_num]) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index c1c11b4da7..a7633c6ef4 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -58,7 +58,7 @@ inline double distance(const urdf::Pose& transform) transform.position.z * transform.position.z); } -inline bool solveQuadratic(const double& a, const double& b, const double& c, double* x1, double* x2) +inline bool solveQuadratic(double a, double b, double c, double* x1, double* x2) { double discriminant = b * b - 4 * a * c; if (fabs(a) < IK_EPS) @@ -88,7 +88,7 @@ inline bool solveQuadratic(const double& a, const double& b, const double& c, do } } -inline bool solveCosineEqn(const double& a, const double& b, const double& c, double& soln1, double& soln2) +inline bool solveCosineEqn(double a, double b, double c, double& soln1, double& soln2) { double theta1 = atan2(b, a); double denom = sqrt(a * a + b * b); @@ -140,7 +140,7 @@ class PR2ArmIK @param Input pose for end-effector @param Initial guess for shoulder pan angle */ - void computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& shoulder_pan_initial_guess, + void computeIKShoulderPan(const Eigen::Isometry3f& g_in, double shoulder_pan_initial_guess, std::vector >& solution) const; /** @@ -148,7 +148,7 @@ class PR2ArmIK h @param Input pose for end-effector @param Initial guess for shoulder roll angle */ - void computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double& shoulder_roll_initial_guess, + void computeIKShoulderRoll(const Eigen::Isometry3f& g_in, double shoulder_roll_initial_guess, std::vector >& solution) const; // std::vector > solution_ik_;/// a vector of ik solutions @@ -172,7 +172,7 @@ class PR2ArmIK bool checkJointLimits(const std::vector& joint_values) const; - bool checkJointLimits(const double& joint_value, const int& joint_num) const; + bool checkJointLimits(double joint_value, int joint_num) const; Eigen::Isometry3f grhs_, gf_, home_inv_; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 707461470c..eec415406c 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -50,7 +50,7 @@ namespace pr2_arm_kinematics { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin"); -bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count) +bool PR2ArmIKSolver::getCount(int& count, int max_count, int min_count) { if (count > 0) { @@ -87,8 +87,7 @@ bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_c } PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name, - const std::string& tip_frame_name, const double& search_discretization_angle, - const int& free_angle) + const std::string& tip_frame_name, double search_discretization_angle, int free_angle) : ChainIkSolverPos() { search_discretization_angle_ = search_discretization_angle; @@ -160,7 +159,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i } int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, - const double& timeout) + double timeout) { const bool verbose = false; KDL::JntArray q_init = q_in; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index 7ef142ea75..1d231fc932 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -80,7 +80,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos * to return multiple solutions from an inverse kinematics computation. */ PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name, - const std::string& tip_frame_name, const double& search_discretization_angle, const int& free_angle); + const std::string& tip_frame_name, double search_discretization_angle, int free_angle); ~PR2ArmIKSolver() override{}; @@ -98,7 +98,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out) override; - int cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, const double& timeout); + int cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, double timeout); void getSolverInfo(moveit_msgs::msg::KinematicSolverInfo& response) { @@ -106,7 +106,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos } private: - bool getCount(int& count, const int& max_count, const int& min_count); + bool getCount(int& count, int max_count, int min_count); double search_discretization_angle_; diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index 99384f4139..cfbe2113d4 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -188,9 +188,6 @@ class DistanceField */ void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); - // DEPRECATED form - [[deprecated]] void addShapeToField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose); - /** * \brief Adds an octree to the distance field. Cells that are * occupied in the octree that lie within the voxel grid are added @@ -227,10 +224,6 @@ class DistanceField void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose, const Eigen::Isometry3d& new_pose); - // DEPRECATED form - [[deprecated]] void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& old_pose, - const geometry_msgs::msg::Pose& new_pose); - /** * \brief All points corresponding to the shape are removed from the * distance field. @@ -242,9 +235,6 @@ class DistanceField */ void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); - // DEPRECATED form - [[deprecated]] void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose); - /** * \brief Resets all points in the distance field to an uninitialize * value. diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index 79f067e367..c441333d18 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -202,9 +202,6 @@ class VoxelGrid */ double getResolution() const; - /** \brief deprecated. Use the version with no arguments. */ - double getResolution(Dimension dim) const; - /** * \brief Gets the origin (minimum point) of the indicated dimension * @@ -440,12 +437,6 @@ inline double VoxelGrid::getResolution() const return resolution_; } -template -inline double VoxelGrid::getResolution(Dimension /*dim*/) const -{ - return resolution_; -} - template inline double VoxelGrid::getOrigin(Dimension dim) const { diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index c5590ce14e..b455f5d002 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -231,14 +231,6 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Iso addPointsToField(point_vec); } -// DEPRECATED -void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose) -{ - Eigen::Isometry3d pose_e; - tf2::fromMsg(pose, pose_e); - addShapeToField(shape, pose_e); -} - void DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points) { // lower extent @@ -313,16 +305,6 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is updatePointsInField(old_point_vec, new_point_vec); } -// DEPRECATED -void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& old_pose, - const geometry_msgs::msg::Pose& new_pose) -{ - Eigen::Isometry3d old_pose_e, new_pose_e; - tf2::fromMsg(old_pose, old_pose_e); - tf2::fromMsg(new_pose, new_pose_e); - moveShapeInField(shape, old_pose_e, new_pose_e); -} - void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose) { bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); @@ -335,14 +317,6 @@ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen removePointsFromField(point_vec); } -// DEPRECATED -void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose) -{ - Eigen::Isometry3d pose_e; - tf2::fromMsg(pose, pose_e); - removeShapeFromField(shape, pose_e); -} - void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, const Eigen::Vector3d& origin, const std::string& frame_id, const rclcpp::Time& stamp, visualization_msgs::msg::Marker& plane_marker) const diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index f5a7406707..d2db990b96 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -557,7 +557,7 @@ static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::v const auto constraint_param = "constraints." + constraint_id; if (!node->has_parameter(constraint_param + ".type")) { - RCLCPP_ERROR(LOGGER, "constraint parameter does not specify its type"); + RCLCPP_ERROR(LOGGER, "constraint parameter \"%s\" does not specify its type", constraint_param.c_str()); return false; } std::string constraint_type; @@ -607,7 +607,7 @@ bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string return false; for (auto& constraint_id : constraint_ids) - constraint_id.insert(0, constraints_param); + constraint_id.insert(0, constraints_param + std::string(".")); return collectConstraints(node, constraint_ids, constraints); } diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 5c2641d49d..691ba11983 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -156,7 +156,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase /** @brief Signature for a cost function used to evaluate IK solutions. */ using IKCostFn = std::function&)>; + const moveit::core::JointModelGroup*, const std::vector&)>; /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it @@ -592,51 +592,6 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase std::map redundant_joint_discretization_; std::vector supported_methods_; - /** - * @brief Enables kinematics plugins access to parameters that are defined - * for the private namespace and inside 'robot_description_kinematics'. - * Parameters are searched in the following locations and order - * - * ~// - * ~/ - * robot_description_kinematics// - * robot_description_kinematics/ - * - * This order maintains default behavior by keeping the private namespace - * as the predominant configuration but also allows groupwise specifications. - */ - template - [[deprecated("Use generate_parameter_library instead")]] inline bool - lookupParam(const rclcpp::Node::SharedPtr& node, const std::string& param, T& val, const T& default_val) const - { - if (node->has_parameter({ group_name_ + "." + param })) - { - node->get_parameter(group_name_ + "." + param, val); - return true; - } - - if (node->has_parameter({ param })) - { - node->get_parameter(param, val); - return true; - } - - if (node->has_parameter({ "robot_description_kinematics." + group_name_ + "." + param })) - { - node->get_parameter("robot_description_kinematics." + group_name_ + "." + param, val); - return true; - } - - if (node->has_parameter("robot_description_kinematics." + param)) - { - node->get_parameter("robot_description_kinematics." + param, val); - return true; - } - - val = default_val; - return false; - } - /** Store some core variables passed via initialize(). * * @param robot_model RobotModel, this kinematics solver should act on. diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index d64056f903..75811a382c 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -95,7 +95,7 @@ bool KinematicsBase::initialize(const rclcpp::Node::SharedPtr& /*node*/, bool KinematicsBase::setRedundantJoints(const std::vector& redundant_joint_indices) { - for (const unsigned int& redundant_joint_index : redundant_joint_indices) + for (unsigned int redundant_joint_index : redundant_joint_indices) { if (redundant_joint_index >= getJointNames().size()) { diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index ab532a166f..daabe12ccb 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -129,7 +129,7 @@ class KinematicsMetrics penalty_multiplier_ = fabs(multiplier); } - const double& getPenaltyMultiplier() const + double getPenaltyMultiplier() const { return penalty_multiplier_; } diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index abd58fa3c2..dc79a7a843 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -7,7 +7,10 @@ target_include_directories(moveit_smoothing_base PUBLIC $ $ ) -target_link_libraries(moveit_smoothing_base moveit_macros) +target_link_libraries(moveit_smoothing_base + ${Eigen_LIBRARIES} + moveit_macros +) include(GenerateExportHeader) generate_export_header(moveit_smoothing_base) set_target_properties(moveit_smoothing_base PROPERTIES VERSION @@ -15,6 +18,7 @@ set_target_properties(moveit_smoothing_base PROPERTIES VERSION ) ament_target_dependencies(moveit_smoothing_base rclcpp + tf2_eigen ) # Plugin implementations diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index e5da3bc7d1..bdcf6a7e5b 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -104,17 +104,22 @@ class ButterworthFilterPlugin : public SmoothingBaseClass /** * Smooth the command signals for all DOF - * @param position_vector array of joint position commands + * @param positions array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands * @return True if initialization was successful */ - bool doSmoothing(std::vector& position_vector) override; + bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; /** * Reset to a given joint state - * @param joint_positions reset the filters to these joint positions + * @param positions reset the filters to these joint positions + * @param velocities (unused) + * @param accelerations (unused) * @return True if reset was successful */ - bool reset(const std::vector& joint_positions) override; + bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) override; private: rclcpp::Node::SharedPtr node_; diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h index 0f5736c8b6..e870e493f0 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -39,10 +39,12 @@ #pragma once #include -#include +#include #include +#include + namespace moveit { namespace core @@ -71,16 +73,22 @@ class SmoothingBaseClass /** * Smooth an array of joint position deltas - * @param position_vector array of joint position commands + * @param positiona array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands * @return True if initialization was successful */ - virtual bool doSmoothing(std::vector& position_vector) = 0; + virtual bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) = 0; /** * Reset to a given joint state - * @param joint_positions reset the filters to these joint positions + * @param positions reset the filters to these joint positions + * @param velocities reset the filters to these joint velocities (if applicable) + * @param accelerations reset the filters to these joint accelerations (if applicable) * @return True if reset was successful */ - virtual bool reset(const std::vector& joint_positions) = 0; + virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) = 0; + ; }; } // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp index 53d23a926c..d0d533799b 100644 --- a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp @@ -40,6 +40,9 @@ #include #include +// Disable -Wold-style-cast because all _THROTTLE macros trigger this +#pragma GCC diagnostic ignored "-Wold-style-cast" + namespace online_signal_smoothing { namespace @@ -111,39 +114,37 @@ bool ButterworthFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::c return true; }; -bool ButterworthFilterPlugin::doSmoothing(std::vector& position_vector) +bool ButterworthFilterPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& /* unused */, + Eigen::VectorXd& /* unused */) { - if (position_vector.size() != position_filters_.size()) + const size_t num_positions = positions.size(); + if (num_positions != position_filters_.size()) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Position vector to be smoothed does not have the right length."); -#pragma GCC diagnostic pop return false; } - for (size_t i = 0; i < position_vector.size(); ++i) + for (size_t i = 0; i < num_positions; ++i) { // Lowpass filter the position command - position_vector[i] = position_filters_.at(i).filter(position_vector[i]); + positions[i] = position_filters_.at(i).filter(positions[i]); } return true; }; -bool ButterworthFilterPlugin::reset(const std::vector& joint_positions) +bool ButterworthFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& /* unused */, + const Eigen::VectorXd& /* unused */) { - if (joint_positions.size() != position_filters_.size()) + const size_t num_positions = positions.size(); + if (num_positions != position_filters_.size()) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Position vector to be reset does not have the right length."); -#pragma GCC diagnostic pop return false; } - for (size_t joint_idx = 0; joint_idx < joint_positions.size(); ++joint_idx) + for (size_t joint_idx = 0; joint_idx < num_positions; ++joint_idx) { - position_filters_.at(joint_idx).reset(joint_positions.at(joint_idx)); + position_filters_.at(joint_idx).reset(positions[joint_idx]); } return true; }; diff --git a/moveit_core/package.xml b/moveit_core/package.xml index ca0fe4cb45..1cbe79cfb7 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -2,7 +2,7 @@ moveit_core - 2.7.4 + 2.8.0 Core libraries used by MoveIt Henning Kayser @@ -36,6 +36,7 @@ generate_parameter_library geometric_shapes geometry_msgs + google_benchmark_vendor kdl_parser libfcl-dev moveit_common @@ -67,6 +68,7 @@ moveit_resources_pr2_description angles orocos_kdl_vendor + ament_cmake_google_benchmark ament_cmake_gtest ament_cmake_gmock ament_index_cpp diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 7f8bbadcb6..3b4db78f08 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -65,6 +65,11 @@ struct MotionPlanResponse moveit_msgs::msg::RobotState start_state; std::string planner_id; + /// Sometimes planning request adapters may add states on the solution path (e.g., add the current state of the robot + /// as prefix, when the robot started to plan only from near that state, as the current state itself appears to touch + /// obstacles). This is helpful because the added states should not be considered invalid in all situations. + std::vector added_path_index; // This won't be included into the MotionPlanResponse ROS 2 message! + // \brief Enable checking of query success or failure, for example if(response) ... explicit operator bool() const { diff --git a/moveit_core/planning_request_adapter/CMakeLists.txt b/moveit_core/planning_request_adapter/CMakeLists.txt index 981cc7b69f..f3b589ed6c 100644 --- a/moveit_core/planning_request_adapter/CMakeLists.txt +++ b/moveit_core/planning_request_adapter/CMakeLists.txt @@ -18,3 +18,11 @@ target_link_libraries(moveit_planning_request_adapter ) install(DIRECTORY include/ DESTINATION include/moveit_core) + +if(BUILD_TESTING) + ament_add_gmock(test_planning_request_adapter_chain test/test_planning_request_adapter_chain.cpp) + target_link_libraries(test_planning_request_adapter_chain + moveit_test_utils + moveit_planning_interface + moveit_planning_request_adapter) +endif() diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 9569edc8e1..4c0a346747 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -60,8 +60,6 @@ class PlanningRequestAdapter std::function; - virtual ~PlanningRequestAdapter() = default; - /** \brief Initialize parameters using the passed Node and parameter namespace. * @param node Node instance used by the adapter * @param parameter_namespace Parameter namespace for adapter @@ -71,23 +69,7 @@ class PlanningRequestAdapter /** \brief Get a description of this adapter * @return Returns a short string that identifies the planning request adapter */ - virtual std::string getDescription() const - { - return ""; - } - - /** \brief Adapt the planning request if needed, call the planner - function \e planner and update the planning response if - needed. - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; + [[nodiscard]] virtual std::string getDescription() const = 0; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -98,15 +80,11 @@ class PlanningRequestAdapter * @param planning_scene Representation of the environment for the planning * @param req Motion planning request with a set of constraints * @param res Reference to which the generated motion plan response is written to - * @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be considered - invalid in all situations. * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const; + [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -117,36 +95,11 @@ class PlanningRequestAdapter * @param planning_scene Representation of the environment for the planning * @param req Motion planning request with a set of constraints * @param res Reference to which the generated motion plan response is written to - * @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be - considered invalid in all situations. * @return True if response got generated correctly */ - virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const = 0; - -protected: - /** \brief Helper param for getting a parameter using a namespace **/ - template - T getParam(const rclcpp::Node::SharedPtr& node, const rclcpp::Logger& logger, const std::string& parameter_namespace, - const std::string& parameter_name, T default_value = {}) const - { - std::string full_name = parameter_namespace.empty() ? parameter_name : parameter_namespace + "." + parameter_name; - T value; - if (!node->get_parameter(full_name, value)) - { - RCLCPP_INFO(logger, "Param '%s' was not set. Using default value: %s", full_name.c_str(), - std::to_string(default_value).c_str()); - return default_value; - } - else - { - RCLCPP_INFO(logger, "Param '%s' was set to %s", full_name.c_str(), std::to_string(value).c_str()); - return value; - } - } + [[nodiscard]] virtual bool adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const = 0; }; /** \brief Apply a sequence of adapters to a motion plan */ @@ -163,25 +116,10 @@ class PlanningRequestAdapterChain * @param req Motion planning request with a set of constraints * @param res Reference to which the generated motion plan response is written to * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; - - /** \brief Iterate through the chain and call all adapters and planners in the correct order - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be - considered invalid in all situations. - * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const; + [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const; private: std::vector adapters_; diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index 5144964e03..c5ddc0841f 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -69,7 +69,9 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda { try { - bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index); + std::swap(res.added_path_index, added_path_index); + bool result = adapter.adaptAndPlan(planner, planning_scene, req, res); + std::swap(res.added_path_index, added_path_index); RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code)); return result; } @@ -77,7 +79,6 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda { RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\nSkipping adapter instead.", adapter.getDescription().c_str(), ex.what()); - added_path_index.clear(); return planner(planning_scene, req, res); } } @@ -87,24 +88,14 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const + planning_interface::MotionPlanResponse& res) const { return adaptAndPlan( [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) { return callPlannerInterfaceSolve(*planner, scene, req, res); }, - planning_scene, req, res, added_path_index); -} - -bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - std::vector empty_added_path_index; - return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index); + planning_scene, req, res); } void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter) @@ -117,24 +108,13 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const { - std::vector empty_added_path_index; - return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index); -} + res.added_path_index.clear(); -bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const -{ // if there are no adapters, run the planner directly if (adapters_.empty()) { - added_path_index.clear(); return callPlannerInterfaceSolve(*planner, planning_scene, req, res); } - // the index values added by each adapter - std::vector> added_path_index_each(adapters_.size()); // if there are adapters, construct a function for each, in order, // so that in the end we have a nested sequence of functions that calls all adapters @@ -145,32 +125,34 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner return callPlannerInterfaceSolve(planner, scene, req, res); }; + std::vector> added_path_index_each(adapters_.size()); for (int i = adapters_.size() - 1; i >= 0; --i) { - fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]]( - const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) { + fn = [&adapter = *adapters_[i], &added_path_index = added_path_index_each[i], + fn](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) { return callAdapter(adapter, fn, scene, req, res, added_path_index); }; } bool result = fn(planning_scene, req, res); - added_path_index.clear(); - // merge the index values from each adapter - for (std::vector& added_states_by_each_adapter : added_path_index_each) + // merge the index values from each adapter (in reverse order) + assert(res.added_path_index.empty()); + for (auto added_state_by_each_adapter_it = added_path_index_each.rbegin(); + added_state_by_each_adapter_it != added_path_index_each.rend(); ++added_state_by_each_adapter_it) { - for (std::size_t& added_index : added_states_by_each_adapter) + for (std::size_t added_index : *added_state_by_each_adapter_it) { - for (std::size_t& index_in_path : added_path_index) + for (std::size_t& index_in_path : res.added_path_index) { if (added_index <= index_in_path) index_in_path++; } - added_path_index.push_back(added_index); + res.added_path_index.push_back(added_index); } } - std::sort(added_path_index.begin(), added_path_index.end()); + std::sort(res.added_path_index.begin(), res.added_path_index.end()); return result; } diff --git a/moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp b/moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp new file mode 100644 index 0000000000..277b4a43c7 --- /dev/null +++ b/moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp @@ -0,0 +1,204 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Willow Garage, Inc. + * 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 Willow Garage 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: Hugo Laloge */ + +#include +#include +#include + +class AppendingPlanningRequestAdapter final : public planning_request_adapter::PlanningRequestAdapter +{ +public: + void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override + { + } + bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override + { + bool success = planner(planning_scene, req, res); + if (success) + { + res.trajectory->addSuffixWayPoint(res.trajectory->getLastWayPoint(), 1); + res.added_path_index.push_back(res.trajectory->getWayPointCount() - 1); + } + return success; + } + std::string getDescription() const override + { + return "AppendingPlanningRequestAdapter"; + } +}; + +class PrependingPlanningRequestAdapter final : public planning_request_adapter::PlanningRequestAdapter +{ +public: + void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override + { + } + bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override + { + bool success = planner(planning_scene, req, res); + if (success) + { + res.trajectory->addPrefixWayPoint(res.trajectory->getFirstWayPoint(), 0); + res.added_path_index.push_back(0); + } + return success; + } + std::string getDescription() const override + { + return "PrependingPlanningRequestAdapter"; + } +}; + +class PlannerManagerStub : public planning_interface::PlannerManager +{ +public: + PlannerManagerStub(planning_interface::PlanningContextPtr planningContext) + : m_planningContext_{ std::move(planningContext) } + { + } + planning_interface::PlanningContextPtr + getPlanningContext(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& /*req*/, + moveit_msgs::msg::MoveItErrorCodes& /*error*/) const override + { + return m_planningContext_; + } + + bool canServiceRequest(const planning_interface::MotionPlanRequest& /*req*/) const override + { + return true; + } + +private: + planning_interface::PlanningContextPtr m_planningContext_; +}; + +class PlanningContextStub : public planning_interface::PlanningContext +{ +public: + using planning_interface::PlanningContext::PlanningContext; + + void setTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory) + { + m_trajectory_ = std::move(trajectory); + } + + bool solve(planning_interface::MotionPlanResponse& res) override + { + if (!m_trajectory_) + return false; + + res.trajectory = m_trajectory_; + return true; + } + + bool solve(planning_interface::MotionPlanDetailedResponse& res) override + { + if (!m_trajectory_) + return false; + + res.trajectory.push_back(m_trajectory_); + return true; + } + + bool terminate() override + { + return true; + } + + void clear() override + { + } + +private: + robot_trajectory::RobotTrajectoryPtr m_trajectory_; +}; + +class PlanningRequestAdapterTests : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + group_name_ = "panda_arm"; + planning_scene_ = std::make_shared(robot_model_); + planning_context_ = std::make_shared("stub", group_name_); + planner_manager_ = std::make_shared(planning_context_); + } + + robot_trajectory::RobotTrajectoryPtr createRandomTrajectory(std::size_t waypointCount) + { + robot_trajectory::RobotTrajectoryPtr robot_trajectory = + std::make_shared(robot_model_, group_name_); + for (std::size_t i = 0; i < waypointCount; ++i) + robot_trajectory->addSuffixWayPoint(std::make_shared(robot_model_), 1.); + return robot_trajectory; + } + +protected: + std::string group_name_; + planning_request_adapter::PlanningRequestAdapterChain sut_; + moveit::core::RobotModelPtr robot_model_; + planning_scene::PlanningScenePtr planning_scene_; + std::shared_ptr planning_context_; + std::shared_ptr planner_manager_; +}; + +TEST_F(PlanningRequestAdapterTests, testMergeAddedPathIndex) +{ + sut_.addAdapter(std::make_shared()); + sut_.addAdapter(std::make_shared()); + sut_.addAdapter(std::make_shared()); + + planning_context_->setTrajectory(createRandomTrajectory(4)); + + planning_interface::MotionPlanRequest req; + req.group_name = group_name_; + planning_interface::MotionPlanResponse res; + std::ignore = sut_.adaptAndPlan(planner_manager_, planning_scene_, req, res); + + EXPECT_THAT(res.added_path_index, testing::ElementsAre(0, 5, 6)); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index b7f81e375d..472c63d68a 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -255,7 +255,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro return world_const_; } - // brief Get the representation of the world + /** \brief Get the representation of the world */ const collision_detection::WorldPtr& getWorldNonConst() { // we always have a world representation @@ -925,20 +925,6 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Outputs debug information about the planning scene contents */ void printKnownObjects(std::ostream& out = std::cout) const; - /** \brief Check if a message includes any information about a planning scene, or it is just a default, empty message. - */ - [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool - isEmpty(const moveit_msgs::msg::PlanningScene& msg); - - /** \brief Check if a message includes any information about a planning scene world, or it is just a default, empty - * message. */ - [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool - isEmpty(const moveit_msgs::msg::PlanningSceneWorld& msg); - - /** \brief Check if a message includes any information about a robot state, or it is just a default, empty message. */ - [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool - isEmpty(const moveit_msgs::msg::RobotState& msg); - /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */ static PlanningScenePtr clone(const PlanningSceneConstPtr& scene); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index d51217216b..f223db5e83 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -144,21 +144,6 @@ class SceneTransforms : public moveit::core::Transforms const PlanningScene* scene_; }; -bool PlanningScene::isEmpty(const moveit_msgs::msg::PlanningScene& msg) -{ - return moveit::core::isEmpty(msg); -} - -bool PlanningScene::isEmpty(const moveit_msgs::msg::RobotState& msg) -{ - return moveit::core::isEmpty(msg); -} - -bool PlanningScene::isEmpty(const moveit_msgs::msg::PlanningSceneWorld& msg) -{ - return moveit::core::isEmpty(msg); -} - PlanningScene::PlanningScene(const moveit::core::RobotModelConstPtr& robot_model, const collision_detection::WorldPtr& world) @@ -1058,7 +1043,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet RCLCPP_ERROR(LOGGER, "Failed to read pose from scene file"); return false; } - float r, g, b, a; + double r, g, b, a; if (!(in >> r >> g >> b >> a)) { RCLCPP_ERROR(LOGGER, "Improperly formatted color in scene geometry file"); diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 1cd5204808..f234728ec6 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -173,7 +173,7 @@ bool FloatingJointModel::satisfiesPositionBounds(const double* values, const Bou if (values[2] < bounds[2].min_position_ - margin || values[2] > bounds[2].max_position_ + margin) return false; double norm_sqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6]; - return fabs(norm_sqr - 1.0) <= std::numeric_limits::epsilon() * 10.0; + return fabs(norm_sqr - 1.0) <= std::numeric_limits::epsilon() * 10.0; } bool FloatingJointModel::normalizeRotation(double* values) const diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 609224965d..4fa903a626 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -412,7 +412,7 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) } else { - RCLCPP_ERROR(LOGGER, "Join '%s' cannot mimic joint '%s' because they have different number of DOF", + RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic joint '%s' because they have different number of DOF", joint_model->getName().c_str(), jm->mimic->joint_name.c_str()); } } diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 4a4f41c7de..90dec83a73 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -26,7 +26,11 @@ install(DIRECTORY include/ DESTINATION include/moveit_core) # Unit tests if(BUILD_TESTING) + find_package(ament_cmake_google_benchmark REQUIRED) find_package(ament_cmake_gtest REQUIRED) + find_package(benchmark REQUIRED) + find_package(kdl_parser REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) find_package(resource_retriever REQUIRED) @@ -77,4 +81,16 @@ if(BUILD_TESTING) moveit_robot_state moveit_kinematics_base ) + + ament_add_google_benchmark( + robot_state_jacobian_benchmark + test/robot_state_jacobian_benchmark.cpp) + ament_target_dependencies(robot_state_jacobian_benchmark + kdl_parser + ) + target_link_libraries(robot_state_jacobian_benchmark + moveit_robot_model + moveit_test_utils + moveit_robot_state + ) endif() diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 33aa9e535d..a7c74d0905 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -133,14 +133,6 @@ class AttachedBody return shape_poses_in_link_frame_; } - /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link). The returned - * transforms are guaranteed to be valid isometries. - * Deprecated. Use getShapePosesInLinkFrame instead. */ - [[deprecated]] const EigenSTL::vector_Isometry3d& getFixedTransforms() const - { - return shape_poses_in_link_frame_; - } - /** \brief Get subframes of this object (relative to the object pose). The returned transforms are guaranteed to be * valid isometries. */ const moveit::core::FixedTransformsMap& getSubframes() const diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index ba338b4e4c..3bcb85d897 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1067,83 +1067,11 @@ class RobotState bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group. - - The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin - The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin - of a robot link (\e link). The direction is assumed to be either in a global reference frame or in the local - reference frame of the link. In the latter case (\e global_reference_frame is false) the \e direction is rotated - accordingly. The link needs to move in a straight line, following the specified direction, for the desired \e - distance. The resulting joint values are stored in the vector \e traj, one by one. The maximum distance in - Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which - provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal - call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to - the distance that was computed and for which corresponding states were added to the path. At the end of the - function call, the state of the group corresponds to the last attempted Cartesian pose. - - During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a - large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold struct is provided, which comprises three fields: - \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. - If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. - If \e jump_threshold_factor is non-zero, we test for relative jumps. Otherwise (all params are zero), jump - detection is disabled. - - For relative jump detection, the average joint-space distance between consecutive points in the trajectory is - computed. If any individual joint-space motion delta is larger then this average distance by a factor of - \e jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just - before the jump. - - For absolute jump thresholds, if any individual joint-space motion delta is larger then \e revolute_jump_threshold - for revolute joints or \e prismatic_jump_threshold for prismatic joints then this step is considered a failure and - the returned path is truncated up to just before the jump. - - NOTE: As of ROS-Melodic these are deprecated and should not be used - */ - [[deprecated]] double - computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, - const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step, - double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group. - - In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) - for the origin of a robot link (\e link). The target frame is assumed to be either in a global reference frame or - in the local reference frame of the link. In the latter case (\e global_reference_frame is false) the \e target is - rotated accordingly. All other comments from the previous function apply. - - NOTE: As of ROS-Melodic these are deprecated and should not be used - */ - [[deprecated]] double - computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, - const Eigen::Isometry3d& target, bool global_reference_frame, double max_step, - double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - - /** \brief Compute the sequence of joint values that perform a general Cartesian path. - - In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially - reached for the origin of a robot link (\e link). The waypoints are transforms given either in a global reference - frame or in the local reference frame of the link at the immediately preceding waypoint. The link needs to move - in a straight line between two consecutive waypoints. All other comments apply. - - NOTE: As of ROS-Melodic these are deprecated and should not be used - */ - [[deprecated]] double - computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, - const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step, - double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. * \param group The group to compute the Jacobian for * \param link The link model to compute the Jacobian for * \param reference_point_position The reference point position (with respect to the link specified in link) - * \param jacobian The resultant jacobian + * \param jacobian The resultant jacobian, with the origin at the group root link. * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation * (default is false) * \return True if jacobian was successfully computed, false otherwise @@ -1155,7 +1083,7 @@ class RobotState * \param group The group to compute the Jacobian for * \param link The link model to compute the Jacobian for * \param reference_point_position The reference point position (with respect to the link specified in link) - * \param jacobian The resultant jacobian + * \param jacobian The resultant jacobian, with the origin at the group root link. * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation * (default is false) * \return True if jacobian was successfully computed, false otherwise @@ -1168,8 +1096,8 @@ class RobotState use_quaternion_representation); } - /** \brief Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an - * exception is thrown. + /** \brief Compute the Jacobian with reference to the last link of a specified group, and origin at the group root + * link. If the group is not a chain, an exception is thrown. * \param group The group to compute the Jacobian for * \param reference_point_position The reference point position (with respect to the link specified in link_name) * \return The computed Jacobian. @@ -1177,8 +1105,8 @@ class RobotState Eigen::MatrixXd getJacobian(const JointModelGroup* group, const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const; - /** \brief Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an - * exception is thrown. + /** \brief Compute the Jacobian with reference to the last link of a specified group, and origin at the group root + * link. If the group is not a chain, an exception is thrown. * \param group The group to compute the Jacobian for * \param reference_point_position The reference point position (with respect to the link specified in link_name) * \return The computed Jacobian. @@ -1258,14 +1186,14 @@ class RobotState /** \brief Set all joints in \e group to random values near the value in \e seed. * \e distance is the maximum amount each joint value will vary from the * corresponding value in \e seed. \distance represents meters for - * prismatic/postitional joints and radians for revolute/orientation joints. + * prismatic/positional joints and radians for revolute/orientation joints. * Resulting values are clamped within default bounds. */ void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance); /** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number generator. * \e distance is the maximum amount each joint value will vary from the * corresponding value in \e seed. \distance represents meters for - * prismatic/postitional joints and radians for revolute/orientation joints. + * prismatic/positional joints and radians for revolute/orientation joints. * Resulting values are clamped within default bounds. */ void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance, random_numbers::RandomNumberGenerator& rng); @@ -1275,7 +1203,7 @@ class RobotState * group.getActiveJointModels(). Each value in \e distances is the maximum * amount the corresponding active joint in \e group will vary from the * corresponding value in \e seed. \distance represents meters for - * prismatic/postitional joints and radians for revolute/orientation joints. + * prismatic/positional joints and radians for revolute/orientation joints. * Resulting values are clamped within default bounds. */ void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, const std::vector& distances); @@ -1285,7 +1213,7 @@ class RobotState * group.getActiveJointModels(). Each value in \e distances is the maximum * amount the corresponding active joint in \e group will vary from the * corresponding value in \e seed. \distance represents meters for - * prismatic/postitional joints and radians for revolute/orientation joints. + * prismatic/positional joints and radians for revolute/orientation joints. * Resulting values are clamped within default bounds. */ void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, const std::vector& distances, random_numbers::RandomNumberGenerator& rng); @@ -1599,27 +1527,6 @@ class RobotState **/ void attachBody(std::unique_ptr attached_body); - /** \brief Add an attached body to this state. Ownership of the - * memory for the attached body is assumed by the state. - * - * This only adds the given body to this RobotState - * instance. It does not change anything about other - * representations of the object elsewhere in the system. So if the - * body represents an object in a collision_detection::World (like - * from a planning_scene::PlanningScene), you will likely need to remove the - * corresponding object from that world to avoid having collisions - * detected against it. - * - * \note This version of the function (taking an AttachedBody - * pointer) does not copy the AttachedBody object, it just uses it - * directly. The AttachedBody object stores its position data - * internally. This means you should never attach a single - * AttachedBody instance to multiple RobotState instances, or - * the body positions will get corrupted. You need to make a fresh - * copy of the AttachedBody object for each RobotState you attach it - * to.*/ - [[deprecated("Deprecated. Pass a unique_ptr instead")]] void attachBody(AttachedBody* attached_body); - /** @brief Add an attached body to a link * @param id The string id associated with the attached body * @param pose The pose associated with the attached body @@ -1857,21 +1764,6 @@ class RobotState } } - /** \brief Update a set of joints that are certain to be mimicking other joints */ - /* use updateMimicJoints() instead, which also marks joints dirty */ - [[deprecated]] void updateMimicJoint(const std::vector& mim) - { - for (const JointModel* jm : mim) - { - const int fvi = jm->getFirstVariableIndex(); - position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); - // Only mark joint transform dirty, but not the associated link transform - // as this function is always used in combination of - // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); - dirty_joint_transforms_[jm->getJointIndex()] = 1; - } - } - /** \brief Update all mimic joints within group */ void updateMimicJoints(const JointModelGroup* group) { diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 36424f9098..01702c0cfc 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -851,7 +851,7 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const { - const moveit::core::LinkModel* link{ nullptr }; + const LinkModel* link{ nullptr }; size_t idx = 0; if ((idx = frame.find('/')) != std::string::npos) @@ -1005,7 +1005,7 @@ double RobotState::distance(const RobotState& other, const JointModelGroup* join void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const { - moveit::core::checkInterpolationParamBounds(LOGGER, t); + checkInterpolationParamBounds(LOGGER, t); robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions()); memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char)); @@ -1014,7 +1014,7 @@ void RobotState::interpolate(const RobotState& to, double t, RobotState& state) void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const { - moveit::core::checkInterpolationParamBounds(LOGGER, t); + checkInterpolationParamBounds(LOGGER, t); const std::vector& jm = joint_group->getActiveJointModels(); for (const JointModel* joint : jm) { @@ -1057,16 +1057,11 @@ void RobotState::attachBody(std::unique_ptr attached_body) attached_body_map_[attached_body->getName()] = std::move(attached_body); } -void RobotState::attachBody(AttachedBody* attached_body) -{ - attachBody(std::unique_ptr(attached_body)); -} - void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose, const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, const std::set& touch_links, const std::string& link, const trajectory_msgs::msg::JointTrajectory& detach_posture, - const moveit::core::FixedTransformsMap& subframe_poses) + const FixedTransformsMap& subframe_poses) { attachBody(std::make_unique(robot_model_->getLinkModel(link), id, pose, shapes, shape_poses, touch_links, detach_posture, subframe_poses)); @@ -1287,7 +1282,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark)) { // if the object is invisible (0 volume) we skip it - if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits::epsilon()) + if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits::epsilon()) continue; att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]); arr.markers.push_back(att_mark); @@ -1313,7 +1308,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons if (!shapes::constructMarkerFromShape(link_model->getShapes()[j].get(), mark)) continue; // if the object is invisible (0 volume) we skip it - if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits::epsilon()) + if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits::epsilon()) continue; mark.pose = tf2::toMsg(global_collision_body_transforms_[link_model->getFirstCollisionBodyTransformIndex() + j]); @@ -1351,89 +1346,85 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link { assert(checkLinkTransforms()); + // Check that the group is a chain, contains 'link' and has joint models. if (!group->isChain()) { RCLCPP_ERROR(LOGGER, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); return false; } - if (!group->isLinkUpdated(link->getName())) + const std::set& descendant_links = group->getUpdatedLinkModelsSet(); + if (descendant_links.find(link) == descendant_links.end()) { RCLCPP_ERROR(LOGGER, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", link->getName().c_str(), group->getName().c_str()); return false; } - const moveit::core::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; - const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); - // getGlobalLinkTransform() returns a valid isometry by contract - Eigen::Isometry3d reference_transform = - root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity(); - int rows = use_quaternion_representation ? 7 : 6; - int columns = group->getVariableCount(); - jacobian = Eigen::MatrixXd::Zero(rows, columns); - - // getGlobalLinkTransform() returns a valid isometry by contract - Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry - Eigen::Vector3d point_transform = link_transform * reference_point_position; - - // RCLCPP_DEBUG(LOGGER, "Point from reference origin expressed in world coordinates: %f %f %f", - // point_transform.x(), - // point_transform.y(), - // point_transform.z()); - - Eigen::Vector3d joint_axis; - Eigen::Isometry3d joint_transform; - - while (link) + const std::vector& joint_models = group->getActiveJointModels(); + if (joint_models.empty()) { - // RCLCPP_DEBUG(LOGGER, "Link: %s, %f %f %f",link_state->getName().c_str(), - // link_state->getGlobalLinkTransform().translation().x(), - // link_state->getGlobalLinkTransform().translation().y(), - // link_state->getGlobalLinkTransform().translation().z()); - // RCLCPP_DEBUG(LOGGER, "Joint: %s",link_state->getParentJointState()->getName().c_str()); + RCLCPP_ERROR(LOGGER, "The group '%s' doesn't contain any joint models. Cannot compute Jacobian.", + group->getName().c_str()); + return false; + } - const JointModel* pjm = link->getParentJointModel(); - if (pjm->getVariableCount() > 0) + // Get the link model of the group root link, and its inverted pose with respect to the RobotModel (URDF) root, + // 'root_pose_world'. + const JointModel* root_joint_model = group->getJointModels().front(); + const LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + const Eigen::Isometry3d root_pose_world = + root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity(); + const int rows = use_quaternion_representation ? 7 : 6; + const int columns = group->getVariableCount(); + jacobian.resize(rows, columns); + + // Get the tip pose with respect to the group root link. Append the user-requested offset 'reference_point_position'. + const Eigen::Isometry3d root_pose_tip = root_pose_world * getGlobalLinkTransform(link); + const Eigen::Vector3d tip_point = root_pose_tip * reference_point_position; + + // Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian + // displacement at the tip. So we build the Jacobian incrementally joint by joint. + std::size_t active_joints = group->getActiveJointModels().size(); + int i = 0; + for (std::size_t joint = 0; joint < active_joints; ++joint) + { + // Get the child link for the current joint, and its pose with respect to the group root link. + const JointModel* joint_model = joint_models[joint]; + const LinkModel* child_link_model = joint_model->getChildLinkModel(); + const Eigen::Isometry3d& root_pose_link = root_pose_world * getGlobalLinkTransform(child_link_model); + + // Compute the Jacobian for the specific joint model, given with respect to the group root link. + if (joint_model->getType() == JointModel::REVOLUTE) { - if (!group->hasJointModel(pjm->getName())) - { - link = pjm->getParentLinkModel(); - continue; - } - unsigned int joint_index = group->getVariableGroupIndex(pjm->getName()); - // getGlobalLinkTransform() returns a valid isometry by contract - joint_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry - if (pjm->getType() == moveit::core::JointModel::REVOLUTE) - { - joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); - jacobian.block<3, 1>(0, joint_index) = - jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); - jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis; - } - else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) - { - joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); - jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; - } - else if (pjm->getType() == moveit::core::JointModel::PLANAR) - { - joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0); - jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; - joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0); - jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis; - joint_axis = joint_transform * Eigen::Vector3d(0.0, 0.0, 1.0); - jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) + - joint_axis.cross(point_transform - joint_transform.translation()); - jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis; - } - else - RCLCPP_ERROR(LOGGER, "Unknown type of joint in Jacobian computation"); + const Eigen::Vector3d axis_wrt_origin = + root_pose_link.linear() * static_cast(joint_model)->getAxis(); + jacobian.block<3, 1>(0, i) = axis_wrt_origin.cross(tip_point - root_pose_link.translation()); + jacobian.block<3, 1>(3, i) = axis_wrt_origin; + } + else if (joint_model->getType() == JointModel::PRISMATIC) + { + const Eigen::Vector3d axis_wrt_origin = + root_pose_link.linear() * static_cast(joint_model)->getAxis(); + jacobian.block<3, 1>(0, i) = axis_wrt_origin; + jacobian.block<3, 1>(3, i) = Eigen::Vector3d::Zero(); + } + else if (joint_model->getType() == JointModel::PLANAR) + { + jacobian.block<3, 1>(0, i) = root_pose_link.linear() * Eigen::Vector3d(1.0, 0.0, 0.0); + jacobian.block<3, 1>(0, i + 1) = root_pose_link.linear() * Eigen::Vector3d(0.0, 1.0, 0.0); + jacobian.block<3, 1>(0, i + 2) = + (root_pose_link.linear() * Eigen::Vector3d(0.0, 0.0, 1.0)).cross(tip_point - root_pose_link.translation()); + jacobian.block<3, 1>(3, i + 2) = root_pose_link.linear() * Eigen::Vector3d(0.0, 0.0, 1.0); } - if (pjm == root_joint_model) - break; - link = pjm->getParentLinkModel(); + else + { + RCLCPP_ERROR(LOGGER, "Unknown type of joint in Jacobian computation"); + return false; + } + i += joint_model->getVariableCount(); } + if (use_quaternion_representation) { // Quaternion representation // From "Advanced Dynamics and Motion Simulation" by Paul Mitiguy @@ -1441,7 +1432,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link // [x] [ w -z y ] [ omega_2 ] // [y] [ z w -x ] [ omega_3 ] // [z] [ -y x w ] - Eigen::Quaterniond q(link_transform.linear()); + Eigen::Quaterniond q(root_pose_tip.linear()); double w = q.w(), x = q.x(), y = q.y(), z = q.z(); Eigen::MatrixXd quaternion_update_matrix(4, 3); quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; @@ -1488,7 +1479,7 @@ void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::Vect const Eigen::VectorXd& s = svd_of_j.singularValues(); Eigen::VectorXd sinv = s; - static const double PINVTOLER = std::numeric_limits::epsilon(); + static const double PINVTOLER = std::numeric_limits::epsilon(); double maxsv = 0.0; for (std::size_t i = 0; i < static_cast(s.rows()); ++i) { @@ -1769,13 +1760,13 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } if (pose_frame != solver_tip_frame) { - const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); + const 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(); + const LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) { if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame)) @@ -1983,11 +1974,11 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: } if (pose_frame != solver_tip_frame) { - const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); + const LinkModel* link_model = getLinkModel(pose_frame); if (!link_model) return false; // getAssociatedFixedTransforms() returns valid isometries by contract - const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); + const LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) { if (fixed_link.first->getName() == solver_tip_frame) @@ -2102,39 +2093,6 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: return false; } -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Vector3d& direction, - bool global_reference_frame, double distance, double max_step, - double jump_threshold_factor, const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, direction, global_reference_frame, - distance, MaxEEFStep(max_step), - JumpThreshold(jump_threshold_factor), validCallback, options); -} - -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Isometry3d& target, - bool global_reference_frame, double max_step, double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, target, global_reference_frame, - MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor), - validCallback, options); -} - -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, - bool global_reference_frame, double max_step, double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, waypoints, global_reference_frame, - MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor), - validCallback, options); -} - void RobotState::computeAABB(std::vector& aabb) const { assert(checkLinkTransforms()); @@ -2177,12 +2135,12 @@ void RobotState::printStatePositions(std::ostream& out) const out << nm[i] << '=' << position_[i] << '\n'; } -void RobotState::printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out) const +void RobotState::printStatePositionsWithJointLimits(const JointModelGroup* jmg, std::ostream& out) const { // TODO(davetcoleman): support joints with multiple variables / multiple DOFs such as floating joints // TODO(davetcoleman): support unbounded joints - const std::vector& joints = jmg->getActiveJointModels(); + const std::vector& joints = jmg->getActiveJointModels(); // Loop through joints for (const JointModel* joint : joints) @@ -2196,7 +2154,7 @@ void RobotState::printStatePositionsWithJointLimits(const moveit::core::JointMod // check if joint is beyond limits bool out_of_bounds = !satisfiesBounds(joint); - const moveit::core::VariableBounds& bound = joint->getVariableBounds()[0]; + const VariableBounds& bound = joint->getVariableBounds()[0]; if (out_of_bounds) out << MOVEIT_CONSOLE_COLOR_RED; diff --git a/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp new file mode 100644 index 0000000000..82c96a20d8 --- /dev/null +++ b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp @@ -0,0 +1,133 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Robotics. + * 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 Willow Garage 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: Mario Prats */ + +// To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary. + +#include +#include +#include +#include +#include +#include +#include + +// Robot and planning group for which the Jacobian will be benchmarked. +constexpr char TEST_ROBOT[] = "panda"; +constexpr char TEST_GROUP[] = "panda_arm"; + +static void BM_MoveItJacobian(benchmark::State& st) +{ + // Load a test robot model. + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) + { + st.SkipWithError("The planning group doesn't exist."); + return; + } + + // Robot state. + moveit::core::RobotState kinematic_state(robot_model); + const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); + + // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint + // configurations. + random_numbers::RandomNumberGenerator rng(0); + + for (auto _ : st) + { + // Time only the jacobian computation, not the forward kinematics. + st.PauseTiming(); + kinematic_state.setToRandomPositions(jmg, rng); + kinematic_state.updateLinkTransforms(); + st.ResumeTiming(); + kinematic_state.getJacobian(jmg); + } +} + +static void BM_KDLJacobian(benchmark::State& st) +{ + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) + { + st.SkipWithError("The planning group doesn't exist."); + return; + } + + // Robot state. + moveit::core::RobotState kinematic_state(robot_model); + const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); + + // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint + // configurations. + random_numbers::RandomNumberGenerator rng(0); + + KDL::Tree kdl_tree; + if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree)) + { + st.SkipWithError("Can't create KDL tree."); + return; + } + + KDL::Chain kdl_chain; + if (!kdl_tree.getChain(jmg->getJointModels().front()->getParentLinkModel()->getName(), + jmg->getLinkModelNames().back(), kdl_chain)) + { + st.SkipWithError("Can't create KDL Chain."); + return; + } + + KDL::ChainJntToJacSolver jacobian_solver(kdl_chain); + + for (auto _ : st) + { + st.PauseTiming(); + kinematic_state.setToRandomPositions(jmg, rng); + kinematic_state.updateLinkTransforms(); + KDL::Jacobian jacobian(kdl_chain.getNrOfJoints()); + KDL::JntArray kdl_q; + kdl_q.resize(kdl_chain.getNrOfJoints()); + kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]); + st.ResumeTiming(); + jacobian_solver.JntToJac(kdl_q, jacobian); + } +} + +BENCHMARK(BM_MoveItJacobian); +BENCHMARK(BM_KDLJacobian); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index e6e7c2ee54..fb481b828b 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -46,34 +46,58 @@ namespace { constexpr double EPSILON{ 1.e-9 }; -} -#if 0 // unused function -static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2) +// Helper to create an Eigen::VectorXd from an initializer list, e.g.: +// auto vector = makeVector({1.0, 2.0, 3.0}); +Eigen::VectorXd makeVector(const std::vector& values) { - unsigned int i1 = 0; - unsigned int i2 = 0; - while (i1 < s1.size() && isspace(s1[i1])) - i1++; - while (i2 < s2.size() && isspace(s2[i2])) - i2++; - while (i1 < s1.size() && i2 < s2.size()) + Eigen::VectorXd vector = Eigen::VectorXd::Zero(values.size()); + for (std::size_t i = 0; i < values.size(); i++) { - if (i1 < s1.size() && i2 < s2.size()) - { - if (s1[i1] != s2[i2]) - return false; - i1++; - i2++; - } - while (i1 < s1.size() && isspace(s1[i1])) - i1++; - while (i2 < s2.size() && isspace(s2[i2])) - i2++; + vector[i] = values[i]; } - return i1 == s1.size() && i2 == s2.size(); + return vector; } -#endif + +// Checks the validity of state.getJacobian() at the given 'joint_values' and 'joint_velocities'. +void CheckJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group, + const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities) +{ + // Using the Jacobian, compute the Cartesian velocity vector at which the end-effector would move, with the given + // joint velocities. + state.setToDefaultValues(); + state.setJointGroupPositions(&joint_model_group, joint_values); + state.updateLinkTransforms(); + + const moveit::core::JointModel* root_joint_model = joint_model_group.getJointModels().front(); + const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + const Eigen::Isometry3d root_pose_world = state.getGlobalLinkTransform(root_link_model).inverse(); + + const Eigen::Isometry3d tip_pose_initial = + root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back()); + const Eigen::MatrixXd jacobian = state.getJacobian(&joint_model_group); + const Eigen::VectorXd cartesian_velocity = jacobian * joint_velocities; + + // Compute the instantaneous displacement that the end-effector would achieve if the given joint + // velocities were applied for a small amount of time. + constexpr double time_step = 1e-5; + const Eigen::VectorXd delta_joint_angles = time_step * joint_velocities; + state.setJointGroupPositions(&joint_model_group, joint_values + delta_joint_angles); + state.updateLinkTransforms(); + const Eigen::Isometry3d tip_pose_after_delta = + root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back()); + const Eigen::Vector3d displacement = tip_pose_after_delta.translation() - tip_pose_initial.translation(); + + // The Cartesian velocity vector obtained via the Jacobian should be aligned with the instantaneous robot motion, i.e. + // the angle between the vectors should be close to zero. + double angle = std::acos(displacement.dot(cartesian_velocity.head<3>()) / + (displacement.norm() * cartesian_velocity.head<3>().norm())); + EXPECT_NEAR(angle, 0.0, 1e-05) << "Angle between Cartesian velocity and Cartesian displacement larger than expected. " + "Angle: " + << angle << ". displacement: " << displacement.transpose() + << ". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() << std::endl; +} +} // namespace static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y, double eps = std::numeric_limits::epsilon()) @@ -195,179 +219,181 @@ class OneRobot : public testing::Test protected: void SetUp() override { - static const std::string MODEL2 = - "" - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - ""; - - static const std::string SMODEL2 = - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - ""; + static const std::string MODEL2 = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + static const std::string SMODEL2 = R"xml( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )xml"; urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2); auto srdf_model = std::make_shared(); @@ -587,13 +613,15 @@ TEST_F(OneRobot, testInterpolation) { state_a.interpolate(state_b, static_cast(i) / 10., interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e")); - EXPECT_NEAR(state_a.distance(state_b), 0, EPSILON) - << "Interpolation between identical states yielded a different state."; + EXPECT_NEAR(state_a.distance(state_b), 0, EPSILON) << "Interpolation between identical states yielded a different " + "state."; for (const auto& link_name : robot_model_->getLinkModelNames()) { - EXPECT_FALSE(interpolated_state.getCollisionBodyTransform(link_name, 0).matrix().hasNaN()) - << "Interpolation between identical states yielded NaN value."; + EXPECT_FALSE(interpolated_state.getCollisionBodyTransform(link_name, 0).matrix().hasNaN()) << "Interpolation " + "between identical " + "states yielded " + "NaN value."; } } @@ -608,17 +636,20 @@ TEST_F(OneRobot, testInterpolation) EXPECT_NEAR(3 * std::sqrt(2), state_a.distance(state_b), EPSILON) << "Simple interpolation of base_joint failed."; state_a.interpolate(state_b, 0.5, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e")); - EXPECT_NEAR(0., state_a.distance(interpolated_state) - state_b.distance(interpolated_state), EPSILON) - << "Simple interpolation of base_joint failed."; - EXPECT_NEAR(0.5, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) - << "Simple interpolation of base_joint failed."; - EXPECT_NEAR(1.5, interpolated_state.getVariablePosition("base_joint/y"), EPSILON) - << "Simple interpolation of base_joint failed."; + EXPECT_NEAR(0., state_a.distance(interpolated_state) - state_b.distance(interpolated_state), EPSILON) << "Simple " + "interpolati" + "on of " + "base_joint " + "failed."; + EXPECT_NEAR(0.5, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) << "Simple interpolation of " + "base_joint failed."; + EXPECT_NEAR(1.5, interpolated_state.getVariablePosition("base_joint/y"), EPSILON) << "Simple interpolation of " + "base_joint failed."; state_a.interpolate(state_b, 0.1, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e")); - EXPECT_NEAR(0.9, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) - << "Simple interpolation of base_joint failed."; - EXPECT_NEAR(1.1, interpolated_state.getVariablePosition("base_joint/y"), EPSILON) - << "Simple interpolation of base_joint failed."; + EXPECT_NEAR(0.9, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) << "Simple interpolation of " + "base_joint failed."; + EXPECT_NEAR(1.1, interpolated_state.getVariablePosition("base_joint/y"), EPSILON) << "Simple interpolation of " + "base_joint failed."; // Interpolate all the joints joint_values["base_joint/x"] = 0.0; @@ -641,10 +672,11 @@ TEST_F(OneRobot, testInterpolation) { double t = static_cast(i) / 5.; state_a.interpolate(state_b, t, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e")); - EXPECT_NEAR(10.0 * t, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) - << "Base joint interpolation failed."; - EXPECT_NEAR(20.0 * (1 - t), interpolated_state.getVariablePosition("base_joint/y"), EPSILON) - << "Base joint interpolation failed."; + EXPECT_NEAR(10.0 * t, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) << "Base joint " + "interpolation failed."; + EXPECT_NEAR(20.0 * (1 - t), interpolated_state.getVariablePosition("base_joint/y"), EPSILON) << "Base joint " + "interpolation " + "failed."; if (t < 0.5) { EXPECT_NEAR(3 * M_PI / 4 + (M_PI / 2) * t, interpolated_state.getVariablePosition("base_joint/theta"), EPSILON) @@ -660,12 +692,13 @@ TEST_F(OneRobot, testInterpolation) EXPECT_NEAR(4 * M_PI / 5 + (2 * M_PI / 5) * (1 - t), interpolated_state.getVariablePosition("joint_a"), EPSILON) << "Continuous joint interpolation failed."; } - EXPECT_NEAR(0.07 * t, interpolated_state.getVariablePosition("joint_c"), EPSILON) - << "Interpolation of joint_c failed."; - EXPECT_NEAR(1 - t, interpolated_state.getVariablePosition("joint_f"), EPSILON) - << "Interpolation of joint_f failed."; - EXPECT_NEAR(1.5 * (1 - t) + 0.1, interpolated_state.getVariablePosition("mim_f"), EPSILON) - << "Interpolation of mimic joint mim_f failed."; + EXPECT_NEAR(0.07 * t, interpolated_state.getVariablePosition("joint_c"), EPSILON) << "Interpolation of joint_c " + "failed."; + EXPECT_NEAR(1 - t, interpolated_state.getVariablePosition("joint_f"), EPSILON) << "Interpolation of joint_f " + "failed."; + EXPECT_NEAR(1.5 * (1 - t) + 0.1, interpolated_state.getVariablePosition("mim_f"), EPSILON) << "Interpolation of " + "mimic joint mim_f " + "failed."; } bool nan_exception = false; @@ -714,6 +747,336 @@ TEST_F(OneRobot, rigidlyConnectedParent) EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/")); } +TEST(getJacobian, RevoluteJoints) +{ + // Robot URDF with four revolute joints. + constexpr char robot_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(urdf_model, srdf_model); + + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); + + // Some made-up numbers, at zero and non-zero robot configurations. + CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 })); + CheckJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); +} + +TEST(getJacobian, RevoluteAndPrismaticJoints) +{ + // Robot URDF with three revolute joints and one prismatic joint. + constexpr char robot_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(urdf_model, srdf_model); + + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); + + // Some made-up numbers, at zero and non-zero robot configurations. + CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 })); + CheckJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); +} + +TEST(getJacobian, RevoluteAndFixedJoints) +{ + // Robot URDF with two revolute joints and two fixed joints. + constexpr char robot_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(urdf_model, srdf_model); + + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); + + // Some made-up numbers, at zero and non-zero robot configurations. + CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0 }), makeVector({ 0.1, 0.4 })); + CheckJacobian(state, *jmg, makeVector({ 0.1, 0.4 }), makeVector({ 0.5, 0.1 })); +} + +TEST(getJacobian, RevolutePlanarAndPrismaticJoints) +{ + // Robot URDF with two revolute joints, one planar joint and one prismatic. + constexpr char robot_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(urdf_model, srdf_model); + + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); + + CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }), + makeVector({ 0.2, 0.05, 0.1, 0.2, 0.3, 0.4 })); +} + +TEST(getJacobian, GroupNotAtOrigin) +{ + // URDF with a 3 DOF robot not at the URDF origin. + constexpr char robot_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(urdf_model, srdf_model); + + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); + + // Some made-up numbers, at zero and non-zero robot configurations. + CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.4, 0.2 })); + CheckJacobian(state, *jmg, makeVector({ 0.1, 0.4, 0.3 }), makeVector({ 0.5, 0.1, 0.2 })); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 109a48863d..26e7f9da4f 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -155,8 +155,6 @@ class RobotTrajectory */ double getWayPointDurationFromStart(std::size_t index) const; - [[deprecated]] double getWaypointDurationFromStart(std::size_t index) const; - double getWayPointDurationFromPrevious(std::size_t index) const { if (duration_from_previous_.size() > index) @@ -302,7 +300,7 @@ class RobotTrajectory * @param The waypoint index after (or equal to) the supplied duration. * @param The progress (0 to 1) between the two waypoints, based on time (not based on joint distances). */ - void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double& blend) const; + void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const; // TODO support visitor function for interpolation, or at least different types. /** @brief Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time @@ -398,18 +396,18 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); /// active joint distances between the two states (L1 norm). /// \param[in] trajectory Given robot trajectory /// \return Length of the robot trajectory [rad] -[[nodiscard]] double path_length(RobotTrajectory const& trajectory); +[[nodiscard]] double path_length(const RobotTrajectory& trajectory); /// \brief Calculate the smoothness of a given trajectory /// \param[in] trajectory Given robot trajectory /// \return Smoothness of the given trajectory /// or nullopt if it is not possible to calculate the smoothness -[[nodiscard]] std::optional smoothness(RobotTrajectory const& trajectory); +[[nodiscard]] std::optional smoothness(const RobotTrajectory& trajectory); /// \brief Calculate the waypoint density of a trajectory /// \param[in] trajectory Given robot trajectory /// \return Waypoint density of the given trajectory /// or nullopt if it is not possible to calculate the density -[[nodiscard]] std::optional waypoint_density(RobotTrajectory const& trajectory); +[[nodiscard]] std::optional waypoint_density(const RobotTrajectory& trajectory); } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index f2663deb50..8709ed35e2 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -490,7 +490,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo return setRobotTrajectoryMsg(st, trajectory); } -void RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, +void RobotTrajectory::findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const { if (duration < 0.0) @@ -538,11 +538,6 @@ double RobotTrajectory::getWayPointDurationFromStart(std::size_t index) const return time; } -double RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const -{ - return getWayPointDurationFromStart(index); -} - bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const { @@ -636,19 +631,19 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory) return out; } -double path_length(RobotTrajectory const& trajectory) +double path_length(const RobotTrajectory& trajectory) { auto trajectory_length = 0.0; for (std::size_t index = 1; index < trajectory.getWayPointCount(); ++index) { - auto const& first = trajectory.getWayPoint(index - 1); - auto const& second = trajectory.getWayPoint(index); + const auto& first = trajectory.getWayPoint(index - 1); + const auto& second = trajectory.getWayPoint(index); trajectory_length += first.distance(second); } return trajectory_length; } -std::optional smoothness(RobotTrajectory const& trajectory) +std::optional smoothness(const RobotTrajectory& trajectory) { if (trajectory.getWayPointCount() > 2) { @@ -686,13 +681,13 @@ std::optional smoothness(RobotTrajectory const& trajectory) return std::nullopt; } -std::optional waypoint_density(RobotTrajectory const& trajectory) +std::optional waypoint_density(const RobotTrajectory& trajectory) { // Only calculate density if more than one waypoint exists if (trajectory.getWayPointCount() > 1) { // Calculate path length - auto const length = path_length(trajectory); + const auto length = path_length(trajectory); if (length > 0.0) { auto density = static_cast(trajectory.getWayPointCount()) / length; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index 8d333e4b98..26038d1bbc 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h @@ -108,7 +108,7 @@ class RuckigSmoothing */ [[nodiscard]] static bool getRobotModelBounds(const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, - moveit::core::JointModelGroup const* const group, + const moveit::core::JointModelGroup* const group, ruckig::InputParameter& ruckig_input); /** diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 9f77ca81e0..2e8d600cb3 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -37,9 +37,46 @@ #pragma once #include +#include namespace trajectory_processing { +/** + * \brief Checks if a robot trajectory is empty. + * \param [in] trajectory The robot trajectory to check. + * \return True if the trajectory is empty, false otherwise. + */ bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory); +/** + * \brief Returns the number of waypoints in a robot trajectory. + * \param [in] trajectory The robot trajectory to count waypoints in. + * \return The number of waypoints in the trajectory. + */ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& trajectory); +/** + * \brief Applies time parameterization to a robot trajectory using the Time-Optimal Trajectory Generation (TOTG) + * algorithm. + * \param [in,out] trajectory The robot trajectory to be time parameterized. + * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory. + * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory. + * \param [in] path_tolerance The path tolerance to use for time parameterization (default: 0.1). + * \param [in] resample_dt The time step to use for time parameterization (default: 0.1). + * \param [in] min_angle_change The minimum angle change to use for time parameterization (default: 0.001). + * \return True if time parameterization was successful, false otherwise. + */ +bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, double path_tolerance = 0.1, + double resample_dt = 0.1, double min_angle_change = 0.001); +/** + * \brief Applies Ruckig smoothing to a robot trajectory. + * \param [in,out] trajectory The robot trajectory to be smoothed. + * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory. + * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory. + * \param [in] mitigate_overshoot Whether to mitigate overshoot during smoothing (default: false). + * \param [in] overshoot_threshold The maximum allowed overshoot during smoothing (default: 0.01). + * \return True if smoothing was successful, false otherwise. + */ +bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, bool mitigate_overshoot = false, + double overshoot_threshold = 0.01); } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index be966d2677..dfd3b6d029 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -75,7 +75,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } // Kinematic limits (vels/accels/jerks) from RobotModel - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::InputParameter ruckig_input{ num_dof }; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) @@ -109,7 +109,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } // Set default kinematic limits (vels/accels/jerks) - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::InputParameter ruckig_input{ num_dof }; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) @@ -176,7 +176,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory) { - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); if (!group) { RCLCPP_ERROR(LOGGER, "The planner did not set the group the plan was computed for"); @@ -187,7 +187,7 @@ bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& tra bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, - moveit::core::JointModelGroup const* const group, + const moveit::core::JointModelGroup* const group, ruckig::InputParameter& ruckig_input) { const size_t num_dof = group->getVariableCount(); @@ -244,7 +244,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, const bool mitigate_overshoot, const double overshoot_threshold) { const size_t num_waypoints = trajectory.getWayPointCount(); - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::OutputParameter ruckig_output{ num_dof }; 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 1c4da3dee2..70a6272df1 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -325,6 +325,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_) @@ -566,6 +572,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/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index 04b8f8b594..6ce5a412e6 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -35,7 +35,8 @@ /* Author: Ioan Sucan */ #include - +#include +#include namespace trajectory_processing { bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory) @@ -47,4 +48,18 @@ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& tra { return std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size()); } +bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, double path_tolerance, double resample_dt, + double min_angle_change) +{ + TimeOptimalTrajectoryGeneration totg(path_tolerance, resample_dt, min_angle_change); + return totg.computeTimeStamps(trajectory, velocity_scaling_factor, acceleration_scaling_factor); +} +bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, bool mitigate_overshoot, double overshoot_threshold) +{ + RuckigSmoothing time_param; + return time_param.applySmoothing(trajectory, velocity_scaling_factor, acceleration_scaling_factor, mitigate_overshoot, + overshoot_threshold); +} } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp index 3094e511a8..e7d2ef6737 100644 --- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp @@ -158,9 +158,9 @@ TEST_F(RuckigTests, single_waypoint) EXPECT_TRUE( smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */)); // And the waypoint did not change - auto const new_first_waypoint = trajectory_->getFirstWayPointPtr(); - auto const& variable_names = new_first_waypoint->getVariableNames(); - for (std::string const& variable_name : variable_names) + const auto new_first_waypoint = trajectory_->getFirstWayPointPtr(); + const auto& variable_names = new_first_waypoint->getVariableNames(); + for (const std::string& variable_name : variable_names) { EXPECT_EQ(first_waypoint_input.getVariablePosition(variable_name), new_first_waypoint->getVariablePosition(variable_name)); 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 5d067c7c82..71a6b9c147 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 @@ -538,6 +538,36 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) } } +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_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index afe54a010c..a497b89d59 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Fix linking error with cached_ik_kinematics_plugin (`#2292 `_) +* Fix ikfast package template (`#2195 `_) +* Make loggers static or move into anonymous namespace (`#2184 `_) + * Make loggers static or move into anonymous namespace + * Update moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp + * Update moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp + * Move LOGGER out of class template +* Contributors: Jafar, Sebastian Jahr, Shane Loretz + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt index e968516f62..f8240f409c 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt @@ -4,12 +4,11 @@ find_package(ur_kinematics QUIET) add_library(moveit_cached_ik_kinematics_base SHARED src/ik_cache.cpp) set_target_properties(moveit_cached_ik_kinematics_base PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_cached_ik_kinematics_base +ament_target_dependencies(moveit_cached_ik_kinematics_base PUBLIC rclcpp moveit_core moveit_msgs ) -target_link_libraries(moveit_cached_ik_kinematics_base moveit_cached_ik_kinematics_plugin) if(trac_ik_kinematics_plugin_FOUND) include_directories(${trac_ik_kinematics_plugin_INCLUDE_DIRS}) @@ -20,20 +19,25 @@ generate_parameter_library( include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_parameters.yaml # path to input yaml file ) +target_link_libraries(moveit_cached_ik_kinematics_base PUBLIC + cached_ik_kinematics_parameters + moveit_kdl_kinematics_plugin) + add_library(moveit_cached_ik_kinematics_plugin SHARED src/cached_ik_kinematics_plugin.cpp) set_target_properties(moveit_cached_ik_kinematics_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_cached_ik_kinematics_plugin +ament_target_dependencies(moveit_cached_ik_kinematics_plugin PUBLIC rclcpp moveit_core moveit_msgs rsl ) -target_link_libraries(moveit_cached_ik_kinematics_plugin +target_link_libraries(moveit_cached_ik_kinematics_plugin PRIVATE cached_ik_kinematics_parameters moveit_kdl_kinematics_plugin - moveit_srv_kinematics_plugin) + moveit_srv_kinematics_plugin + moveit_cached_ik_kinematics_base) if(trac_ik_kinematics_plugin_FOUND) - target_link_libraries(moveit_cached_ik_kinematics_plugin ${trac_ik_kinematics_plugin_LIBRARIES}) + target_link_libraries(moveit_cached_ik_kinematics_plugin PRIVATE ${trac_ik_kinematics_plugin_LIBRARIES}) set_target_properties(moveit_cached_ik_kinematics_plugin PROPERTIES COMPILE_DEFINITIONS "CACHED_IK_KINEMATICS_TRAC_IK") endif() diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h index 6e83dd6e85..79ad631c02 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h @@ -267,7 +267,7 @@ class IkSolution : public IkSolutionBase } } - std::vector > _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector > _vbasesol; ///< solution and their offsets if joints are mimicked std::vector _vfree; }; diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index 4066109c33..d0c71b113e 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -353,7 +353,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase double enforceLimits(double val, double min, double max) const; void fillFreeParams(int count, int* array); - bool getCount(int& count, const int& max_count, const int& min_count) const; + bool getCount(int& count, int max_count, int min_count) const; /** * @brief samples the designated redundant joint using the chosen discretization method @@ -728,7 +728,7 @@ void IKFastKinematicsPlugin::fillFreeParams(int count, int* array) free_params_.push_back(array[i]); } -bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const +bool IKFastKinematicsPlugin::getCount(int& count, int max_count, int min_count) const { if (count > 0) { diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 374c031ce9..fb7b102296 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -2,7 +2,7 @@ moveit_kinematics - 2.7.4 + 2.8.0 Package for all inverse kinematics solvers in MoveIt Henning Kayser diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index 02cc0b1707..cbce29dcc1 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -154,7 +154,7 @@ bool SrvKinematicsPlugin::setRedundantJoints(const std::vector& re bool SrvKinematicsPlugin::isRedundantJoint(unsigned int index) const { - for (const unsigned int& redundant_joint_indice : redundant_joint_indices_) + for (unsigned int redundant_joint_indice : redundant_joint_indices_) { if (redundant_joint_indice == index) return true; @@ -312,7 +312,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector moveit_planners_chomp - 2.7.4 + 2.8.0 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 c1abfe81f3..35f3f91ccb 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Always set response planner id and warn if it is not set (`#2236 `_) +* Contributors: Sebastian Jahr + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index b28f3ad5df..2186dbf729 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.7.4 + 2.8.0 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 2de5dd9dfc..9b0ea98d17 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Remove added path index from planner adapter function signature (`#2285 `_) +* Contributors: Sebastian Jahr + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index 0f6ef1309e..a0da33eaae 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.7.4 + 2.8.0 Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index a21402dafb..e8f8b8cbe6 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -53,7 +53,7 @@ namespace chomp { -static rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter { @@ -173,8 +173,8 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ..."); diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index e806e3daa6..0584bf63a5 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ * Migrate STOMP from ros-planning/stomp_moveit (`#2158 `_) diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 2755e0b5e9..4998321c8a 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -2,7 +2,7 @@ moveit_planners - 2.7.4 + 2.8.0 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 213d08c3d0..63c2c9f7a1 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Fix typo in model_based_planning_context.h (`#2243 `_) +* Warn if optimization objective does not match expected values (`#2213 `_) + * Warn if optimization objective does not match expected values + * Update moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp + Co-authored-by: Sebastian Jahr + * Format + --------- +* Contributors: Stephanie Eng + 2.7.4 (2023-05-18) ------------------ * Fix Constraint Planning Segfault (`#2130 `_) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index 8dea8329f3..2f533778c2 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -57,7 +57,7 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples private: bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); - bool stateValidityCallback(ompl::base::State* new_goal, moveit::core::RobotState const* state, + bool stateValidityCallback(ompl::base::State* new_goal, const moveit::core::RobotState* state, const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, bool verbose = false) const; bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h index ebbaa85276..93b2cc8bc2 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h @@ -446,7 +446,7 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf * Eq. 2.107 * */ -inline Eigen::Matrix3d angularVelocityToAngleAxis(const double& angle, const Eigen::Ref& axis) +inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref& axis) { double t{ std::abs(angle) }; Eigen::Matrix3d r_skew; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index 86b4b2affe..bf4376da49 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -77,7 +77,7 @@ bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_g } bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, - moveit::core::RobotState const* state, + const moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* jpos, bool verbose) const { 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 58b1e06c53..2a03ade24f 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 @@ -860,7 +860,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 @@ -976,7 +976,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: @@ -996,12 +996,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/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index cfb3234a44..6e2223da26 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -126,7 +126,7 @@ void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destinati void ompl_interface::PoseModelStateSpace::sanityChecks() const { - ModelBasedStateSpace::sanityChecks(std::numeric_limits::epsilon(), std::numeric_limits::epsilon(), + ModelBasedStateSpace::sanityChecks(std::numeric_limits::epsilon(), std::numeric_limits::epsilon(), ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY); } diff --git a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h index 1a0c6b3c73..6ae825d265 100644 --- a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h +++ b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h @@ -44,7 +44,7 @@ namespace ompl_interface_testing { /** \brief Robot independent test class setup * - * The desired robot tests can be impelmented in a derived class in a generic way. + * The desired robot tests can be implemented in a derived class in a generic way. * * It is implemented this way to avoid the ros specific test framework * outside moveit_ros. diff --git a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp index bcca9890e5..483d43a5ef 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp @@ -68,10 +68,10 @@ class TestThreadSafeStateStorage : public ompl_interface_testing::LoadTestRobot, auto robot_state_stored = tss.getStateStorage(); // Check if robot_state_stored's joint angles matches with what we set - for (auto const& joint_name : robot_state_->getVariableNames()) + for (const auto& joint_name : robot_state_->getVariableNames()) { - auto const expected_value = robot_state_->getVariablePosition(joint_name); - auto const actual_value = robot_state_stored->getVariablePosition(joint_name); + const auto expected_value = robot_state_->getVariablePosition(joint_name); + const auto actual_value = robot_state_stored->getVariablePosition(joint_name); EXPECT_EQ(actual_value, expected_value) << "Expecting joint value for " << joint_name << " to match."; } } diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 47064b59e0..da4030445a 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -2,7 +2,7 @@ moveit_planners_ompl - 2.7.4 + 2.8.0 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 a0b537c9f3..1e28741455 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Fx class_loader warnings in PILZ unittests (`#2296 `_) +* Always set response planner id and warn if it is not set (`#2236 `_) +* Pilz multi-group incompatibility (`#1856 `_) +* Enhance PILZ service request checks (`#2087 `_) +* Contributors: Marco Magri, Sebastian Jahr, Yang Lin + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h index d91b12469f..97b3be3936 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -67,12 +67,25 @@ inline bool getJointLimits(const std::string& joint_name, const std::string& par { // Deceleration limits const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; - - limits.has_deceleration_limits = node->declare_parameter(param_base_name + ".has_deceleration_limits", false); + if (node->has_parameter(param_base_name + ".has_deceleration_limits")) + { + limits.has_deceleration_limits = node->get_parameter(param_base_name + ".has_deceleration_limits").as_bool(); + } + else + { + limits.has_deceleration_limits = node->declare_parameter(param_base_name + ".has_deceleration_limits", false); + } if (limits.has_deceleration_limits) { - limits.max_deceleration = - node->declare_parameter(param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); + if (node->has_parameter(param_base_name + ".max_deceleration")) + { + limits.max_deceleration = node->get_parameter(param_base_name + ".max_deceleration").as_double(); + } + else + { + limits.max_deceleration = + node->declare_parameter(param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); + } } } catch (const std::exception& ex) 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 7a81fa9eea..1dd242b703 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 @@ -126,7 +126,7 @@ bool verifySampleJointLimits(const std::map& position_last, bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, const double& sampling_time, + const std::map& initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); @@ -195,12 +195,12 @@ bool isRobotStateStationary(const moveit::core::RobotState& state, const std::st * smallest index of trajectroy. * @param index The intersection index which has to be determined. */ -bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, - const double& r, const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, +bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, const double r, + const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, std::size_t& index); bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, - const double& r); + double r); /** * @brief Checks if current robot state is in self collision. 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..27f976bf69 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 @@ -136,8 +136,8 @@ class TrajectoryGenerator * The trap profile returns uses the longer distance of translational and * rotational motion. */ - std::unique_ptr cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, - const double& max_acceleration_scaling_factor, + std::unique_ptr cartesianTrapVelocityProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, const std::unique_ptr& path) const; private: @@ -157,7 +157,7 @@ class TrajectoryGenerator virtual void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; private: /** @@ -247,9 +247,9 @@ class TrajectoryGenerator /** * @return True if scaling factor is valid, otherwise false. */ - static bool isScalingFactorValid(const double& scaling_factor); - static void checkVelocityScaling(const double& scaling_factor); - static void checkAccelerationScaling(const double& scaling_factor); + static bool isScalingFactorValid(double scaling_factor); + static void checkVelocityScaling(double scaling_factor); + static void checkAccelerationScaling(double scaling_factor); /** * @return True if ONE position + ONE orientation constraint given, @@ -277,7 +277,7 @@ class TrajectoryGenerator const std::unique_ptr clock_; }; -inline bool TrajectoryGenerator::isScalingFactorValid(const double& scaling_factor) +inline bool TrajectoryGenerator::isScalingFactorValid(double scaling_factor) { return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); } 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..2246b2c3d6 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 @@ -91,7 +91,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, const double& sampling_time, + const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** 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..19e48dac38 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 @@ -74,7 +74,7 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, const double& sampling_time, + const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index f4a916b230..cc73ff4839 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -79,11 +79,11 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param sampling_time */ void planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, const double& velocity_scaling_factor, - const double& acceleration_scaling_factor, const double& sampling_time); + trajectory_msgs::msg::JointTrajectory& joint_trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, double sampling_time); void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, const double& sampling_time, + const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; private: diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 4f437c6abd..5239dbefac 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.7.4 + 2.8.0 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 5f84806869..bdbf31f662 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 @@ -239,7 +239,11 @@ CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstP setStartState(motion_plan_responses, req.group_name, req.start_state); planning_interface::MotionPlanResponse res; - planning_pipeline->generatePlan(planning_scene, req, res); + if (!planning_pipeline->generatePlan(planning_scene, req, res)) + { + RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed."); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + } if (res.error_code.val != res.error_code.SUCCESS) { std::ostringstream os; 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 13fc0243e3..97f2d6380e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -201,7 +201,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( const planning_scene::PlanningSceneConstPtr& scene, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, const double& sampling_time, + const std::map& initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { @@ -525,8 +525,7 @@ bool pilz_industrial_motion_planner::isRobotStateStationary(const moveit::core:: } bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::string& link_name, - const Eigen::Vector3d& center_position, - const double& r, + const Eigen::Vector3d& center_position, double r, const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, std::size_t& index) { @@ -564,7 +563,7 @@ bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::st bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, - const double& r) + double r) { return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r); } 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 154eac5aa1..75bb14d7ac 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -79,7 +79,7 @@ void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface: // to provide a command specific request validation. } -void TrajectoryGenerator::checkVelocityScaling(const double& scaling_factor) +void TrajectoryGenerator::checkVelocityScaling(double scaling_factor) { if (!isScalingFactorValid(scaling_factor)) { @@ -90,7 +90,7 @@ void TrajectoryGenerator::checkVelocityScaling(const double& scaling_factor) } } -void TrajectoryGenerator::checkAccelerationScaling(const double& scaling_factor) +void TrajectoryGenerator::checkAccelerationScaling(double scaling_factor) { if (!isScalingFactorValid(scaling_factor)) { @@ -154,7 +154,7 @@ void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Const const std::vector& expected_joint_names, const std::string& group_name) const { - for (auto const& joint_constraint : constraint.joint_constraints) + for (const auto& 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) == @@ -281,8 +281,8 @@ void TrajectoryGenerator::setFailureResponse(const rclcpp::Time& planning_start, } std::unique_ptr -TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, - const double& max_acceleration_scaling_factor, +TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, const std::unique_ptr& path) const { std::unique_ptr vp_trans = std::make_unique( 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 512497380f..2748e11fe6 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 @@ -188,7 +188,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { std::unique_ptr cart_path(setPathCIRC(plan_info)); std::unique_ptr vel_profile( 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 8dbed6dfa2..50e25a8f7e 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 @@ -145,7 +145,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // create Cartesian path for lin std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); 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..95623762d1 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 @@ -91,8 +91,8 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelCon void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, const std::map& goal_pos, trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const double& velocity_scaling_factor, const double& acceleration_scaling_factor, - const double& sampling_time) + double velocity_scaling_factor, double acceleration_scaling_factor, + double sampling_time) { // initialize joint names for (const auto& item : goal_pos) @@ -102,7 +102,7 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ // check if goal already reached bool goal_reached = true; - for (auto const& goal : goal_pos) + for (const auto& goal : goal_pos) { if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT) { @@ -245,7 +245,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // plan the ptp trajectory planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp index 7c1c5195d8..76d0354cf0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp @@ -217,7 +217,7 @@ bool VelocityProfileATrap::setProfileStartVelocity(double pos1, double pos2, dou if (s * vel1 <= 0) { - // TODO initial velocity is in opposite derection of start-end vector + // TODO initial velocity is in opposite direction of start-end vector return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index d95f8f9d80..ad3a4934b0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -926,7 +926,7 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl return true; } -bool testutils::checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose, +bool testutils::checkThatPointsInRadius(const std::string& link_name, double r, Eigen::Isometry3d& circ_pose, const pilz_industrial_motion_planner::TrajectoryBlendResponse& res) { bool result = true; @@ -1052,9 +1052,9 @@ bool testutils::getBlendTestData(const rclcpp::Node::SharedPtr& node, const size bool testutils::generateTrajFromBlendTestData( const planning_scene::PlanningSceneConstPtr& scene, const std::shared_ptr& tg, const std::string& group_name, - const std::string& link_name, const testutils::BlendTestData& data, const double& sampling_time_1, - const double& sampling_time_2, planning_interface::MotionPlanResponse& res_1, - planning_interface::MotionPlanResponse& res_2, double& dis_1, double& dis_2) + const std::string& link_name, const testutils::BlendTestData& data, double sampling_time_1, double sampling_time_2, + planning_interface::MotionPlanResponse& res_1, planning_interface::MotionPlanResponse& res_2, double& dis_1, + double& dis_2) { const moveit::core::RobotModelConstPtr robot_model = scene->getRobotModel(); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h index 45bf3e64d0..625f70fd23 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -72,7 +72,7 @@ static constexpr double DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE{ 1e-8 }; /** * @brief Convert degree to rad. */ -inline static constexpr double deg2Rad(double angle) +static inline constexpr double deg2Rad(double angle) { return (angle / 180.0) * M_PI; } @@ -88,7 +88,7 @@ inline std::string getJointName(size_t joint_number, const std::string& joint_pr */ pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); -inline std::string demangle(char const* name) +inline std::string demangle(const char* name) { return boost::core::demangle(name); } @@ -169,7 +169,7 @@ void createPTPRequest(const std::string& planning_group, const moveit::core::Rob /** * @brief check if the goal given in joint space is reached - * Only the last point in the trajectory is veryfied. + * Only the last point in the trajectory is verified. * @param trajectory: generated trajectory * @param goal: goal in joint space * @param joint_position_tolerance @@ -182,7 +182,7 @@ bool isGoalReached(const trajectory_msgs::msg::JointTrajectory& trajectory, /** * @brief check if the goal given in cartesian space is reached - * Only the last point in the trajectory is veryfied. + * Only the last point in the trajectory is verified. * @param robot_model * @param trajectory * @param req @@ -344,7 +344,7 @@ bool checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::Traj * @brief Checks if all points of the blending trajectory lie within the * blending radius. */ -bool checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose, +bool checkThatPointsInRadius(const std::string& link_name, double r, Eigen::Isometry3d& circ_pose, const pilz_industrial_motion_planner::TrajectoryBlendResponse& res); /** @@ -434,8 +434,8 @@ bool checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendReque bool generateTrajFromBlendTestData(const planning_scene::PlanningSceneConstPtr& scene, const std::shared_ptr& tg, const std::string& group_name, const std::string& link_name, - const BlendTestData& data, const double& sampling_time_1, - const double& sampling_time_2, planning_interface::MotionPlanResponse& res_lin_1, + const BlendTestData& data, double sampling_time_1, double sampling_time_2, + planning_interface::MotionPlanResponse& res_lin_1, planning_interface::MotionPlanResponse& res_lin_2, double& dis_lin_1, double& dis_lin_2); @@ -474,7 +474,7 @@ checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& tr const double rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); -inline bool isMonotonouslyDecreasing(const std::vector& vec, const double& tol) +inline bool isMonotonouslyDecreasing(const std::vector& vec, double tol) { return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) { return !(std::abs(a - b) < tol || a < b); // true -> a is ordered before b -> list is not sorted diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp index cad029206a..110f67b418 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp @@ -59,7 +59,7 @@ class SolverMock class JointModelGroupMock { public: - MOCK_CONST_METHOD0(getSolverInstance, SolverMock const*()); + MOCK_CONST_METHOD0(getSolverInstance, const SolverMock*()); MOCK_CONST_METHOD0(getName, const std::string&()); }; 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 209b77aa1b..aecedafed3 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 d07dd1ce6a..e48b26d719 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 @@ -130,6 +130,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 */ @@ -185,6 +190,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 fc7525821a..0f937f99ca 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 1c7386b250..8fd48ab9e7 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_); @@ -125,6 +125,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 */ @@ -155,6 +160,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 2368c129c2..e6c0675cb5 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,8 +92,8 @@ 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"; planning_scene_ = std::make_shared(robot_model_); @@ -120,6 +120,11 @@ class TrajectoryFunctionsTestBase : public testing::Test } } + void TearDown() override + { + robot_model_.reset(); + } + /** * @brief check if two transformations are close * @param pose1 @@ -127,12 +132,13 @@ class TrajectoryFunctionsTestBase : public testing::Test * @param epsilon * @return */ - bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon); + bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double epsilon); protected: // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; // test parameters from parameter server @@ -146,8 +152,7 @@ class TrajectoryFunctionsTestBase : public testing::Test random_numbers::RandomNumberGenerator rng_{ random_seed_ }; }; -bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, - const double& epsilon) +bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double epsilon) { for (std::size_t i = 0; i < 3; ++i) { 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 a1b3b7ad0e..df2f5ad69f 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_); @@ -161,6 +161,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) { @@ -193,6 +198,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_; 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 98b8b02b78..4d093276e5 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_); @@ -133,6 +133,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) { @@ -161,6 +166,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 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 aca597f3dc..f0d4e06da7 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 2be30c4c36..59cf54835e 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Make loggers static or move into anonymous namespace (`#2184 `_) + * Make loggers static or move into anonymous namespace + * Update moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp + * Update moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp + * Move LOGGER out of class template +* Contributors: Sebastian Jahr + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h index c523df53d1..213c11a321 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h @@ -173,7 +173,7 @@ class XmlTestdataLoader : public TestdataLoader /** * @brief Converts string vector to double vector. */ - inline static std::vector strVec2doubleVec(std::vector& strVec); + static inline std::vector strVec2doubleVec(std::vector& strVec); public: /** diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index 180c4c881f..eddadec302 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.7.4 + 2.8.0 Helper scripts and functionality to test industrial motion generation Christian Henkel diff --git a/moveit_planners/stomp/CHANGELOG.rst b/moveit_planners/stomp/CHANGELOG.rst index aeffaa5467..628e31a4ac 100644 --- a/moveit_planners/stomp/CHANGELOG.rst +++ b/moveit_planners/stomp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_planners_stomp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Remove added path index from planner adapter function signature (`#2285 `_) +* Always set response planner id and warn if it is not set (`#2236 `_) +* Contributors: Sebastian Jahr + 2.7.4 (2023-05-18) ------------------ * Migrate STOMP from ros-planning/stomp_moveit (`#2158 `_) diff --git a/moveit_planners/stomp/CMakeLists.txt b/moveit_planners/stomp/CMakeLists.txt index e91a6607e8..ef600cf98d 100644 --- a/moveit_planners/stomp/CMakeLists.txt +++ b/moveit_planners/stomp/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(std_msgs REQUIRED) find_package(stomp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(rsl REQUIRED) generate_parameter_library(stomp_moveit_parameters res/stomp_moveit.yaml) @@ -29,6 +30,7 @@ ament_target_dependencies(stomp_moveit_plugin std_msgs tf2_eigen visualization_msgs + rsl ) target_link_libraries(stomp_moveit_plugin stomp::stomp stomp_moveit_parameters) diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp index b7113ca3a2..e41a7cac5b 100644 --- a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp @@ -49,15 +49,15 @@ namespace stomp_moveit { -// Decides if the given state position vector is valid or not - example use cases are collision or constraint checking -using StateValidatorFn = std::function; +// Validates a given state and produces a scalar cost penalty - example use cases are collision or constraint checking +using StateValidatorFn = std::function; namespace costs { // Interpolation step size for collision checking (joint space, L2 norm) constexpr double COL_CHECK_DISTANCE = 0.05; -constexpr double CONSTRAINT_CHECK_DISTANCE = 0.1; +constexpr double CONSTRAINT_CHECK_DISTANCE = 0.05; /** * Creates a cost function from a binary robot state validation function. @@ -69,12 +69,10 @@ constexpr double CONSTRAINT_CHECK_DISTANCE = 0.1; * * @param state_validator_fn The validator function that tests for binary conditions * @param interpolation_step_size The L2 norm distance step used for interpolation - * @param penalty The penalty cost value applied to invalid states * * @return Cost function that computes smooth costs for binary validity conditions */ -CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_validator_fn, - double interpolation_step_size, double penalty) +CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_validator_fn, double interpolation_step_size) { CostFn cost_fn = [=](const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity) { costs.setZero(values.cols()); @@ -96,11 +94,13 @@ CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_vali double interpolation_fraction = 0.0; const double interpolation_step = std::min(0.5, interpolation_step_size / segment_distance); bool found_invalid_state = false; + double penalty = 0.0; while (!found_invalid_state && interpolation_fraction < 1.0) { Eigen::VectorXd sample_vec = (1 - interpolation_fraction) * current + interpolation_fraction * next; - found_invalid_state = !state_validator_fn(sample_vec); + penalty = state_validator_fn(sample_vec); + found_invalid_state = penalty > 0.0; interpolation_fraction += interpolation_step; } @@ -178,10 +178,10 @@ CostFn get_collision_cost_function(const std::shared_ptrisStateColliding(state, group_name); + return planning_scene->isStateColliding(state, group_name) ? collision_penalty : 0.0; }; - return get_cost_function_from_state_validator(collision_validator_fn, COL_CHECK_DISTANCE, collision_penalty); + return get_cost_function_from_state_validator(collision_validator_fn, COL_CHECK_DISTANCE); } /** @@ -192,13 +192,13 @@ CostFn get_collision_cost_function(const std::shared_ptr& planning_scene, const moveit::core::JointModelGroup* group, - const moveit_msgs::msg::Constraints& constraints_msg, double constraints_penalty) + const moveit_msgs::msg::Constraints& constraints_msg, double cost_scale) { const auto& joints = group ? group->getActiveJointModels() : planning_scene->getRobotModel()->getActiveJointModels(); @@ -212,13 +212,10 @@ CostFn get_constraints_cost_function(const std::shared_ptr #include -// TODO(#2166): Replace with std types -#include -#include -#include -#include +#include #include namespace stomp_moveit @@ -79,9 +75,7 @@ class MultivariateGaussian Eigen::MatrixXd covariance_cholesky_; /**< Cholesky decomposition (LL^T) of the covariance */ int size_; - boost::mt19937 rng_; - boost::normal_distribution<> normal_dist_; - std::shared_ptr > > gaussian_; + std::normal_distribution normal_dist_; }; //////////////////////// template function definitions follow ////////////////////////////// @@ -91,16 +85,14 @@ MultivariateGaussian::MultivariateGaussian(const Eigen::MatrixBase& me const Eigen::MatrixBase& covariance) : mean_(mean), covariance_(covariance), covariance_cholesky_(covariance_.llt().matrixL()), normal_dist_(0.0, 1.0) { - rng_.seed(rand()); size_ = mean.rows(); - gaussian_.reset(new boost::variate_generator >(rng_, normal_dist_)); } template void MultivariateGaussian::sample(Eigen::MatrixBase& output, bool use_covariance) { for (int i = 0; i < size_; ++i) - output(i) = (*gaussian_)(); + output(i) = normal_dist_(rsl::rng()); if (use_covariance) { diff --git a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp index 0500e7fc13..537898b108 100644 --- a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp @@ -56,7 +56,7 @@ namespace visualization namespace { -const auto GREEN = [](const double& a) { +const auto GREEN = [](double a) { std_msgs::msg::ColorRGBA color; color.r = 0.1; color.g = 0.8; diff --git a/moveit_planners/stomp/package.xml b/moveit_planners/stomp/package.xml index f289323989..9d9082971b 100644 --- a/moveit_planners/stomp/package.xml +++ b/moveit_planners/stomp/package.xml @@ -2,7 +2,7 @@ moveit_planners_stomp - 2.7.4 + 2.8.0 STOMP Motion Planner for MoveIt Henning Kayser BSD-3-Clause @@ -21,6 +21,7 @@ std_msgs tf2_eigen visualization_msgs + rsl ament_lint_auto ament_lint_common diff --git a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index da474c223e..e32528f2db 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp @@ -87,7 +87,7 @@ class StompPlannerManager : public PlannerManager return nullptr; } - auto const params = param_listener_->get_params(); + const auto params = param_listener_->get_params(); std::shared_ptr planning_context = std::make_shared("STOMP", req.group_name, params); diff --git a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp b/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp index 4f6a912fc4..c09d35635b 100644 --- a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp @@ -71,8 +71,8 @@ class StompSmoothingAdapter : public planning_request_adapter::PlanningRequestAd } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { // Following call to planner() calls the motion planner defined for the pipeline and stores the trajectory inside // the MotionPlanResponse res variable which is then passed to STOMP for optimization 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 41cbc0c786..770f10e2fc 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,9 @@ Changelog for package moveit_resources_prbt_ikfast_manipulator_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h index 33109eb370..0ecaae21eb 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h @@ -263,7 +263,7 @@ class IkSolution : public IkSolutionBase } } - std::vector > _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector > _vbasesol; ///< solution and their offsets if joints are mimicked std::vector _vfree; }; 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 77e7866db2..3cd25f540d 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.7.4 + 2.8.0 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 dbeaed0a38..d035de8c15 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 @@ -358,7 +358,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase double enforceLimits(double val, double min, double max) const; void fillFreeParams(int count, int* array); - bool getCount(int& count, const int& max_count, const int& min_count) const; + bool getCount(int& count, int max_count, int min_count) const; /** * @brief samples the designated redundant joint using the chosen discretization method @@ -732,7 +732,7 @@ void IKFastKinematicsPlugin::fillFreeParams(int count, int* array) free_params_.push_back(array[i]); } -bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const +bool IKFastKinematicsPlugin::getCount(int& count, int max_count, int min_count) const { if (count > 0) { @@ -805,12 +805,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 bda20bc5c8..8741aa7cce 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 @@ -106,7 +105,7 @@ inline double IKlog(double f) { return log(f); } #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) #endif -// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate +// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold specifies by how much they can deviate #ifndef IKFAST_EVALCOND_THRESH #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001) #endif diff --git a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst index ffa8c84211..644bca41d7 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_resources_prbt_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/test_configs/prbt_moveit_config/package.xml b/moveit_planners/test_configs/prbt_moveit_config/package.xml index 603b37635a..ba72aba81b 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.7.4 + 2.8.0

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 27820f9d30..3d40c7b6ab 100644 --- a/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_resources_prbt_pg70_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/test_configs/prbt_pg70_support/package.xml b/moveit_planners/test_configs/prbt_pg70_support/package.xml index efee049dc3..93bee91ca6 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.7.4 + 2.8.0 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 bd116fd585..f729df1a5c 100644 --- a/moveit_planners/test_configs/prbt_support/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_support/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package prbt_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_planners/test_configs/prbt_support/package.xml b/moveit_planners/test_configs/prbt_support/package.xml index 2a037921b6..5a6649d093 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.7.4 + 2.8.0 Mechanical, kinematic and visual description of the Pilz light weight arm PRBT. diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp index 905765e869..9fd3803ad0 100644 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ b/moveit_planners/trajopt/src/problem_description.cpp @@ -57,8 +57,8 @@ * @param name The name to use when printing an error or warning * @param apply_first If true and only one value is given, broadcast value to length of expected_size */ -void checkParameterSize(trajopt::DblVec& parameter, const unsigned int& expected_size, const std::string& name, - const bool& apply_first = true) +void checkParameterSize(trajopt::DblVec& parameter, unsigned int expected_size, const std::string& name, + bool apply_first = true) { if (apply_first == true && parameter.size() == 1) { @@ -242,7 +242,7 @@ TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) // Apply constraint to each fixed dof to its initial value for all timesteps (freeze that column) if (!bi.dofs_fixed.empty()) { - for (const int& dof_ind : bi.dofs_fixed) + for (int dof_ind : bi.dofs_fixed) { for (int i = 1; i < prob->GetNumSteps(); ++i) { diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index 71fd318c16..0b82fbb97c 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index f80b19645e..3cd8451ade 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -2,7 +2,7 @@ moveit_plugins - 2.7.4 + 2.8.0 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 69adc187ff..2b118c9e27 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Simplify controller manager namespacing (`#2210 `_) +* Minor cleanup to ros_control_interface and trajectory execution (`#2208 `_) +* Contributors: Stephanie Eng + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 62ab73601b..ece1032e87 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.7.4 + 2.8.0 ros_control controller manager interface for MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index 8328d1aef1..c59c0d9aea 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index a3611867b0..efa6058759 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.7.4 + 2.8.0 A generic, simple controller manager plugin for MoveIt. Michael Görner Henning Kayser diff --git a/moveit_py/CHANGELOG.rst b/moveit_py/CHANGELOG.rst index 468bf28630..ee48db850e 100644 --- a/moveit_py/CHANGELOG.rst +++ b/moveit_py/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_py ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Fix moveit_py rclcpp::init() (`#2223 `_) + * Fix moveit_py rclcpp::init() + Rclcpp has been initialized without args which was problematic + for some use cases like clock simulation. Parameters like + use_sim_time:=true need to be passed to rclcpp, also + NodeOptions access the global rcl state on construction. + Co-authored-by: Jafar Uruç +* Export moveit_py_utils' cmake target (`#2207 `_) +* fix typo in name +* Contributors: Henning Kayser, Michael Görner, Robert Haschke + 2.7.4 (2023-05-18) ------------------ * Rename named_target_state_values to get_named_target_state_values (`#2181 `_) diff --git a/moveit_py/moveit/__init__.py b/moveit_py/moveit/__init__.py index 0f44d4a088..d7c49b2af2 100644 --- a/moveit_py/moveit/__init__.py +++ b/moveit_py/moveit/__init__.py @@ -1,3 +1,3 @@ from moveit.core import * from moveit.planning import * -from moveit.utils import get_launch_params_filepath +from moveit.utils import get_launch_params_filepaths diff --git a/moveit_py/moveit/utils.py b/moveit_py/moveit/utils.py index a40cd12284..18bc7ae553 100644 --- a/moveit_py/moveit/utils.py +++ b/moveit_py/moveit/utils.py @@ -32,6 +32,8 @@ # # Author: Peter David Fagan +from typing import List, Optional + import sys import traceback import yaml @@ -47,12 +49,17 @@ def create_params_file_from_dict(params, node_name): return param_file_path -def get_launch_params_filepath(): +def get_launch_params_filepaths(cli_args: Optional[List[str]] = None) -> List[str]: """ - A utility that returns the path value after argument --params-file. + A utility that returns the path value after the --params-file arguments. """ - try: + if cli_args is None: cli_args = sys.argv - return sys.argv[sys.argv.index("--params-file") + 1] - except ValueError: - return "Failed to parse params file path from command line arguments. Check that --params-file command line argument is specified." + + try: + indexes = [i for i, v in enumerate(cli_args) if v == "--params-file"] + return [cli_args[i + 1] for i in indexes] + except IndexError: + return [ + "Failed to parse params file paths from command line arguments. Check that --params-file command line argument is specified." + ] diff --git a/moveit_py/moveit/utils.pyi b/moveit_py/moveit/utils.pyi index 1b43712276..d840a3ffed 100644 --- a/moveit_py/moveit/utils.pyi +++ b/moveit_py/moveit/utils.pyi @@ -1,2 +1,4 @@ +from typing import List, Optional + def create_params_file_from_dict(params, node_name): ... -def get_launch_params_filepath(): ... +def get_launch_params_filepaths(cli_args: Optional[List[str]] = ...) -> List[str]: ... diff --git a/moveit_py/package.xml b/moveit_py/package.xml index 62b7c593b3..f278985fce 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -2,7 +2,7 @@ moveit_py - 2.7.4 + 2.8.0 Python binding for MoveIt 2 Peter David Fagan diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index cbba1f2d30..fc6eb6f314 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -36,6 +36,7 @@ #include "robot_trajectory.h" #include +#include namespace moveit_py { @@ -134,12 +135,40 @@ void init_robot_trajectory(py::module& m) Returns: list of float: The duration from previous of each waypoint in the trajectory. )") + .def("apply_totg_time_parameterization", &trajectory_processing::applyTOTGTimeParameterization, + py::arg("velocity_scaling_factor"), py::arg("acceleration_scaling_factor"), py::kw_only(), + py::arg("path_tolerance") = 0.1, py::arg("resample_dt") = 0.1, py::arg("min_angle_change") = 0.001, + R"( + Adds time parameterization to the trajectory using the Time-Optimal Trajectory Generation (TOTG) algorithm. + Args: + velocity_scaling_factor (float): The velocity scaling factor. + acceleration_scaling_factor (float): The acceleration scaling factor. + path_tolerance (float): The path tolerance to use for time parameterization (default: 0.1). + resample_dt (float): The time step to use for time parameterization (default: 0.1). + min_angle_change (float): The minimum angle change to use for time parameterization (default: 0.001). + ) + Returns: + bool: True if the trajectory was successfully retimed, false otherwise. + )") + .def("apply_ruckig_smoothing", &trajectory_processing::applyRuckigSmoothing, py::arg("velocity_scaling_factor"), + py::arg("acceleration_scaling_factor"), py::kw_only(), py::arg("mitigate_overshoot") = false, + py::arg("overshoot_threshold") = 0.01, + R"( + Applies Ruckig smoothing to the trajectory. + Args: + velocity_scaling_factor (float): The velocity scaling factor. + acceleration_scaling_factor (float): The acceleration scaling factor. + mitigate_overshoot (bool): Whether to mitigate overshoot during smoothing (default: false). + overshoot_threshold (float): The maximum allowed overshoot during smoothing (default: 0.01 + ) + Returns: + bool: True if the trajectory was successfully retimed, false otherwise. + )") .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::get_robot_trajectory_msg, py::arg("joint_filter") = std::vector(), R"( Get the trajectory as a moveit_msgs.msg.RobotTrajectory message. - - Returns: + Returns: moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. )") .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg, diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h index 6b8f3b7e8b..29eb51b3cd 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index b18afa9ec3..ed43b05e24 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -54,44 +54,53 @@ void init_moveit_py(py::module& m) The MoveItPy class is the main interface to the MoveIt Python API. It is a wrapper around the MoveIt C++ API. )") - .def(py::init([](const std::string& node_name, const std::string& launch_params_filepath, + .def(py::init([](const std::string& node_name, const std::vector& launch_params_filepaths, const py::object& config_dict, bool provide_planning_service) { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_initializer"); - if (!rclcpp::ok()) - { - RCLCPP_INFO(LOGGER, "Initialize rclcpp"); - rclcpp::init(0, nullptr); - } - - RCLCPP_INFO(LOGGER, "Initialize node parameters"); - rclcpp::NodeOptions node_options; - // This section is used to load the appropriate node parameters before spinning a moveit_cpp instance // Priority is given to parameters supplied directly via a config_dict, followed by launch parameters // and finally no supplied parameters. + std::vector launch_arguments; if (!config_dict.is(py::none())) { auto utils = py::module::import("moveit.utils"); // TODO (peterdavidfagan): replace python method with C++ method std::string params_filepath = - utils.attr("create_params_file_from_dict")(config_dict, "moveit_py").cast(); - RCLCPP_INFO(LOGGER, "params_filepath: %s", params_filepath.c_str()); - node_options.allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true) - .arguments({ "--ros-args", "--params-file", params_filepath }); + utils.attr("create_params_file_from_dict")(config_dict, node_name).cast(); + launch_arguments = { "--ros-args", "--params-file", params_filepath }; } - else if (!launch_params_filepath.empty()) + else if (!launch_params_filepaths.empty()) { - node_options.allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true) - .arguments({ "--ros-args", "--params-file", launch_params_filepath }); + launch_arguments = { "--ros-args" }; + for (const auto& launch_params_filepath : launch_params_filepaths) + { + launch_arguments.push_back("--params-file"); + launch_arguments.push_back(launch_params_filepath); + } } - else + + // Initialize ROS, pass launch arguments with rclcpp::init() + if (!rclcpp::ok()) { - // TODO (peterdavidfagan): consider failing initialization if no params file is provided - node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); + RCLCPP_INFO(LOGGER, "Initialize rclcpp"); + std::vector chars; + chars.reserve(launch_arguments.size()); + for (const auto& arg : launch_arguments) + { + chars.push_back(arg.c_str()); + } + + rclcpp::init(launch_arguments.size(), chars.data()); } + + // Build NodeOptions + RCLCPP_INFO(LOGGER, "Initialize node parameters"); + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true) + .arguments(launch_arguments); + RCLCPP_INFO(LOGGER, "Initialize node and executor"); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options); std::shared_ptr executor = @@ -121,7 +130,8 @@ void init_moveit_py(py::module& m) return moveit_cpp_ptr; }), py::arg("node_name") = "moveit_py", - py::arg("launch_params_filepath") = utils.attr("get_launch_params_filepath")().cast(), + py::arg("launch_params_filepaths") = + utils.attr("get_launch_params_filepaths")().cast>(), py::arg("config_dict") = py::none(), py::arg("provide_planning_service") = true, py::return_value_policy::take_ownership, R"( diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index cd4a01c576..8bfb4c5d76 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index f52a5c330b..235c9bec45 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -2,7 +2,7 @@ moveit_ros_benchmarks - 2.7.4 + 2.8.0 Enhanced tools for benchmarks in MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 7308854c6c..e55267689c 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -112,17 +112,13 @@ BenchmarkExecutor::~BenchmarkExecutor() return false; } - auto const& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name); + const auto& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name); // Verify the pipeline has successfully initialized a planner if (!pipeline->getPlannerManager()) { RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); continue; } - - // Disable visualizations - pipeline->displayComputedMotionPlans(false); - pipeline->checkSolutionPaths(false); } // Error check @@ -861,7 +857,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); // Planning pipeline benchmark - auto const response = planning_component->plan(plan_req_params, planning_scene_); + const auto response = planning_component->plan(plan_req_params, planning_scene_); solved[j] = bool(response.error_code); @@ -924,7 +920,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request // Create multi-pipeline request moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request; - for (auto const& pipeline_planner_id_pair : parallel_pipeline_entry.second) + for (const auto& pipeline_planner_id_pair : parallel_pipeline_entry.second) { moveit_cpp::PlanningComponent::PlanRequestParameters plan_req_params = { .planner_id = pipeline_planner_id_pair.second, @@ -959,11 +955,11 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request // Solve problem std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); - auto const t1 = std::chrono::system_clock::now(); - auto const response = planning_component->plan(multi_pipeline_plan_request, + const auto t1 = std::chrono::system_clock::now(); + const auto response = planning_component->plan(multi_pipeline_plan_request, &moveit::planning_pipeline_interfaces::getShortestSolution, nullptr, planning_scene_); - auto const t2 = std::chrono::system_clock::now(); + const auto t2 = std::chrono::system_clock::now(); solved[j] = bool(response.error_code); diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 3e76ac485e..e348324540 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -170,8 +170,7 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::vector pipelines; if (!node->get_parameter(ns + ".pipelines", pipelines)) { - RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", (ns + ".pipelines").c_str()); - return false; + RCLCPP_WARN(LOGGER, "No single planning pipeline namespace `%s` configured.", (ns + ".pipelines").c_str()); } for (const std::string& pipeline : pipelines) @@ -210,8 +209,8 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::vector parallel_pipelines; if (!node->get_parameter(ns + ".parallel_pipelines", parallel_pipelines)) { - RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", (ns + ".parallel_pipelines").c_str()); - return false; + RCLCPP_WARN(LOGGER, "No parallel planning pipeline namespace `%s` configured.", + (ns + ".parallel_pipelines").c_str()); } for (const std::string& parallel_pipeline : parallel_pipelines) @@ -250,20 +249,25 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) std::vector> pipeline_planner_id_pairs; for (size_t i = 0; i < pipelines.size(); ++i) { - pipeline_planner_id_pairs.push_back(std::pair(pipelines[i], planner_ids[i])); + pipeline_planner_id_pairs.push_back(std::pair(pipelines.at(i), planner_ids.at(i))); } parallel_planning_pipelines[parallel_pipeline] = pipeline_planner_id_pairs; - for (auto const& entry : parallel_planning_pipelines) + for (const auto& entry : parallel_planning_pipelines) { RCLCPP_INFO(LOGGER, "Parallel planning pipeline '%s'", entry.first.c_str()); - for (auto const& pair : entry.second) + for (const auto& pair : entry.second) { RCLCPP_INFO(LOGGER, " '%s': '%s'", pair.first.c_str(), pair.second.c_str()); } } } } + if (pipelines.empty() && parallel_pipelines.empty()) + { + RCLCPP_ERROR(LOGGER, "No single or parallel planning pipelines configured for benchmarking."); + return false; + } return true; } diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index c718622765..d12952b168 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -97,7 +97,7 @@ int main(int argc, char** argv) return 1; } // Running benchmarks - for (auto const& name : scene_names) + for (const auto& name : scene_names) { options.scene_name = name; if (!server.runBenchmarks(options)) diff --git a/moveit_ros/hybrid_planning/CHANGELOG.rst b/moveit_ros/hybrid_planning/CHANGELOG.rst index 410200f735..694065be1d 100644 --- a/moveit_ros/hybrid_planning/CHANGELOG.rst +++ b/moveit_ros/hybrid_planning/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_hybrid_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* Contributors: Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h index f27c1c4650..7c3fc6b124 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h @@ -108,7 +108,7 @@ class TrajectoryOperatorInterface virtual bool reset() = 0; protected: - // Reference trajectory to be precessed + // Reference trajectory to be processed robot_trajectory::RobotTrajectoryPtr reference_trajectory_; std::string group_; }; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index 0edc36e341..bd1173b5a1 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -51,7 +51,7 @@ const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); const auto JOIN_THREAD_TIMEOUT = std::chrono::seconds(1); // If the trajectory progress reaches more than 0.X the global goal state is considered as reached -constexpr float PROGRESS_THRESHOLD = 0.995; +constexpr double PROGRESS_THRESHOLD = 0.995; } // namespace LocalPlannerComponent::LocalPlannerComponent(const rclcpp::NodeOptions& options) @@ -85,7 +85,7 @@ bool LocalPlannerComponent::initialize() // Configure planning scene monitor planning_scene_monitor_ = std::make_shared( - node_, "robot_description", tf_buffer_, "local_planner/planning_scene_monitor"); + node_, "robot_description", "local_planner/planning_scene_monitor"); if (!planning_scene_monitor_->getPlanningScene()) { RCLCPP_ERROR(LOGGER, "Unable to configure planning scene monitor"); @@ -203,7 +203,8 @@ bool LocalPlannerComponent::initialize() // Initialize global trajectory listener global_solution_subscriber_ = node_->create_subscription( - config_.global_solution_topic, 1, [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { + config_.global_solution_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { // Add received trajectory to internal reference trajectory robot_trajectory::RobotTrajectory new_trajectory(planning_scene_monitor_->getRobotModel(), msg->group_name); moveit::core::RobotState start_state(planning_scene_monitor_->getRobotModel()); diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml index 09105a2231..f79e349898 100644 --- a/moveit_ros/hybrid_planning/package.xml +++ b/moveit_ros/hybrid_planning/package.xml @@ -1,6 +1,6 @@ moveit_hybrid_planning - 2.7.4 + 2.8.0 Hybrid planning components of MoveIt 2 Sebastian Jahr diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp index e309fbef13..5e7b375e0a 100644 --- a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp +++ b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp @@ -150,8 +150,8 @@ class HybridPlanningDemo RCLCPP_INFO(LOGGER, "Initialize Planning Scene Monitor"); tf_buffer_ = std::make_shared(node_->get_clock()); - planning_scene_monitor_ = std::make_shared( - node_, "robot_description", tf_buffer_, "planning_scene_monitor"); + planning_scene_monitor_ = std::make_shared(node_, "robot_description", + "planning_scene_monitor"); if (!planning_scene_monitor_->getPlanningScene()) { RCLCPP_ERROR(LOGGER, "The planning scene was not retrieved!"); diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index c256d04b83..deb9f83c30 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced boost::algorithm::join with fmt::join (`#2273 `_) + * Replaced boost::algorithm::join with fmt::join + * Made changes in CMakeLists.txt to accomodate fmt + * Updated package.xml files + * removed redundant boost dependencies + * Rename variables -> variable + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Specify controller name in MGI execution (`#2257 `_) + * Specify controller name in MGI execute + * Finish comment + --------- + Co-authored-by: Sebastian Jahr +* fix move_group capabilities loading (`#2270 `_) + * fix move_group capabilities loading + * clang-format +* Cleanup move_group CMake (`#2226 `_) +* Contributors: Shobuj Paul, Stephanie Eng, Tyler Weaver, Yang Lin + 2.7.4 (2023-05-18) ------------------ * Fix MoveGroup action cancel callback (`#2118 `_) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index b59830d91e..5e52563b90 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -9,6 +9,7 @@ moveit_package() include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DEPENDS + fmt ament_cmake moveit_core moveit_ros_occupancy_map_monitor @@ -64,8 +65,8 @@ ament_target_dependencies(move_group ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) target_link_libraries(move_group moveit_move_group_capabilities_base) add_executable(list_move_group_capabilities src/list_capabilities.cpp) -ament_target_dependencies(list_move_group_capabilities ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -target_link_libraries(list_move_group_capabilities moveit_move_group_capabilities_base) +ament_target_dependencies(list_move_group_capabilities ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(list_move_group_capabilities moveit_move_group_capabilities_base fmt) install( TARGETS diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index e2ede13aa5..2d80ac41eb 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -2,7 +2,7 @@ moveit_ros_move_group - 2.7.4 + 2.8.0 The move_group node for MoveIt Michael Görner Henning Kayser @@ -31,6 +31,7 @@ tf2 tf2_geometry_msgs tf2_ros + fmt moveit_kinematics diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 3e159f3fb9..ff1bd1dac1 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -108,7 +108,7 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptrtrajectory_execution_manager_->clear(); - if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory)) + if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names)) { setExecuteTrajectoryState(MONITOR, goal); context_->trajectory_execution_manager_->execute(); diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 84e2d52a25..b199bebc26 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -46,8 +46,13 @@ namespace move_group { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_move_group_default_capabilities.move_action_capability"); + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.move_action_capability"); +constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true; +constexpr bool CHECK_SOLUTION_PATHS = true; +} // namespace MoveGroupMoveAction::MoveGroupMoveAction() : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false } @@ -69,8 +74,9 @@ void MoveGroupMoveAction::initialize() preemptMoveCallback(); return rclcpp_action::CancelResponse::ACCEPT; }, - [this](std::shared_ptr goal) { - std::thread{ std::bind(&MoveGroupMoveAction::executeMoveCallback, this, std::placeholders::_1), goal }.detach(); + [this](const std::shared_ptr& goal) { + std::thread{ [this](const std::shared_ptr& goal) { executeMoveCallback(goal); }, goal } + .detach(); }); } @@ -211,7 +217,12 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptrgeneratePlan(the_scene, goal->get_goal()->request, res); + if (!planning_pipeline->generatePlan(the_scene, goal->get_goal()->request, res, context_->debug_, + CHECK_SOLUTION_PATHS, DISPLAY_COMPUTED_MOTION_PLANS)) + { + RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed."); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + } } catch (std::exception& ex) { @@ -243,7 +254,8 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor); try { - solved = planning_pipeline->generatePlan(plan.planning_scene, req, res); + solved = planning_pipeline->generatePlan(plan.planning_scene, req, res, context_->debug_, CHECK_SOLUTION_PATHS, + DISPLAY_COMPUTED_MOTION_PLANS); } catch (std::exception& ex) { diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index 00c2198453..39cb03e05d 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp @@ -42,8 +42,12 @@ namespace move_group { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_move_group_default_capabilities.plan_service_capability"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.plan_service_capability"); +constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true; +constexpr bool CHECK_SOLUTION_PATHS = true; +} // namespace MoveGroupPlanService::MoveGroupPlanService() : MoveGroupCapability("MotionPlanService") { @@ -82,7 +86,12 @@ bool MoveGroupPlanService::computePlanService(const std::shared_ptrgeneratePlan(ps, req->motion_plan_request, mp_res); + if (!planning_pipeline->generatePlan(ps, req->motion_plan_request, mp_res, context_->debug_, CHECK_SOLUTION_PATHS, + DISPLAY_COMPUTED_MOTION_PLANS)) + { + RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed."); + mp_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + } mp_res.getMessage(res->motion_plan_response); } catch (std::exception& ex) diff --git a/moveit_ros/move_group/src/list_capabilities.cpp b/moveit_ros/move_group/src/list_capabilities.cpp index 455217d506..78ff926dd5 100644 --- a/moveit_ros/move_group/src/list_capabilities.cpp +++ b/moveit_ros/move_group/src/list_capabilities.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include int main(int /*argc*/, char** /*argv*/) { @@ -45,7 +45,7 @@ int main(int /*argc*/, char** /*argv*/) pluginlib::ClassLoader capability_plugin_loader("moveit_ros_move_group", "move_group::MoveGroupCapability"); std::cout << "Available capabilities:\n" - << boost::algorithm::join(capability_plugin_loader.getDeclaredClasses(), "\n") << '\n'; + << fmt::format("{}", fmt::join(capability_plugin_loader.getDeclaredClasses(), "\n")) << '\n'; } catch (pluginlib::PluginlibException& ex) { diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 5fe4a52262..429168efe0 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -152,8 +152,7 @@ class MoveGroupExe { const auto& pipeline_name = pipeline_entry.first; std::string pipeline_capabilities; - if (context_->moveit_cpp_->getNode()->get_parameter("planning_pipelines/" + pipeline_name + "/capabilities", - pipeline_capabilities)) + if (context_->moveit_cpp_->getNode()->get_parameter(pipeline_name + ".capabilities", pipeline_capabilities)) { boost::char_separator sep(" "); boost::tokenizer> tok(pipeline_capabilities, sep); @@ -276,8 +275,7 @@ int main(int argc, char** argv) } // Initialize MoveItCpp - const auto tf_buffer = std::make_shared(nh->get_clock(), tf2::durationFromSec(10.0)); - const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options, tf_buffer); + const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options); const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst(); if (planning_scene_monitor->getPlanningScene()) diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index efeb36ad0a..6c595893dd 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -56,13 +56,6 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m if (default_pipeline_it != pipelines.end()) { planning_pipeline_ = default_pipeline_it->second; - - // configure the planning pipeline - planning_pipeline_->displayComputedMotionPlans(true); - planning_pipeline_->checkSolutionPaths(true); - - if (debug_) - planning_pipeline_->publishReceivedRequests(true); } else { diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index 5723ee3855..25085f9e83 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 8019ab6e23..a3e1bf4ccf 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -2,7 +2,7 @@ moveit_ros - 2.7.4 + 2.8.0 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 f7696ca5c7..0814ff918c 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* [Servo] Fix Twist transformation (`#2311 `_) +* [Servo] Add additional info about twist frame conversion (`#2295 `_) + * Update docstring + warning for twist frame conversion + * Apply suggestions from code review + Co-authored-by: AndyZe + * Suppress old-style-cast warnings + --------- + Co-authored-by: AndyZe +* [Servo] Refactoring servo (`#2224 `_) +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* Fix Servo suddenHalt() to halt at previous state, not current (`#2229 `_) +* Fix the launching of Servo as a node component (`#2194 `_) + * Fix the launching of Servo as a node component + * Comment improvement + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + * Add launch argument + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> +* Revert central differencing calculation in servo (`#2203 `_) + * Revert central differencing calculation in servo + * current_joint_state\_ to internal_joint_state\_ +* Fix servo speed scaling YAML parameters (`#2211 `_) +* Reset Servo filters when starting (`#2186 `_) +* [Servo] Move `enforcePositionLimits` and `enforceVelocityLimits` to utilities (`#2180 `_) + * Move limit enforcing functions to utilities + * Fix comments + * Make clock const + * Remove clock from enforcePositionLimit + * Remove clock usage from transformTwistToPlanningFrame and applyJointUpdates + * Remove clock from vvelocityScalingFactorForSingularity + * Fix tests + * Cleanups + clang-tidy + * Minor cleanups + * Log output formatting +* Change servo collision checking parameters to dynamically update (`#2183 `_) +* Contributors: AndyZe, Sebastian Castro, Shobuj Paul, V Mohammed Ibrahim + 2.7.4 (2023-05-18) ------------------ * [Servo] Remove soon-to-be obsolete functions (`#2175 `_) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index dc9fccbf96..e95461540e 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -7,7 +7,6 @@ moveit_package() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs - control_toolbox geometry_msgs moveit_core moveit_msgs @@ -24,8 +23,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) find_package(generate_parameter_library REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -42,60 +39,59 @@ include_directories( # This library provides a way of loading parameters for servo generate_parameter_library( moveit_servo_lib_parameters - src/servo_parameters.yaml + config/servo_parameters.yaml ) # This library provides a C++ interface for sending realtime twist or joint commands to a robot -add_library(moveit_servo_lib SHARED - src/collision_check.cpp +add_library(moveit_servo_lib_cpp SHARED + src/collision_monitor.cpp src/servo.cpp - src/servo_calcs.cpp - src/utilities.cpp -) -set_target_properties(moveit_servo_lib PROPERTIES VERSION "${moveit_servo_VERSION}") -ament_target_dependencies(moveit_servo_lib ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(moveit_servo_lib moveit_servo_lib_parameters) + src/utils/common.cpp + src/utils/command.cpp -add_library(pose_tracking SHARED src/pose_tracking.cpp) -ament_target_dependencies(pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(pose_tracking moveit_servo_lib) +) +set_target_properties(moveit_servo_lib_cpp PROPERTIES VERSION "${moveit_servo_VERSION}") +target_link_libraries(moveit_servo_lib_cpp moveit_servo_lib_parameters) +ament_target_dependencies(moveit_servo_lib_cpp ${THIS_PACKAGE_INCLUDE_DEPENDS}) -##################### -## Component Nodes ## -##################### +add_library(moveit_servo_lib_ros SHARED src/servo_node.cpp) +set_target_properties(moveit_servo_lib_ros PROPERTIES VERSION "${moveit_servo_VERSION}") +target_link_libraries(moveit_servo_lib_ros moveit_servo_lib_cpp) +ament_target_dependencies(moveit_servo_lib_ros ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Add and export library to run as a ROS node component, and receive commands via topics -add_library(servo_node SHARED src/servo_node.cpp) -ament_target_dependencies(servo_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(servo_node moveit_servo_lib) -rclcpp_components_register_nodes(servo_node "moveit_servo::ServoNode") +###################### +## Components ## +###################### -# Add executable for using a controller -add_library(servo_controller_input SHARED src/teleop_demo/joystick_servo_example.cpp) -ament_target_dependencies(servo_controller_input PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -rclcpp_components_register_nodes(servo_controller_input "moveit_servo::JoyToServoPub") +rclcpp_components_register_node( + moveit_servo_lib_ros + PLUGIN "moveit_servo::ServoNode" + EXECUTABLE servo_node +) ###################### ## Executable Nodes ## ###################### -# An executable node for the servo server -add_executable(servo_node_main src/servo_node_main.cpp) -target_link_libraries(servo_node_main servo_node) -ament_target_dependencies(servo_node_main ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Executable node for the joint jog demo +add_executable(demo_joint_jog demos/cpp_interface/demo_joint_jog.cpp ) +target_link_libraries(demo_joint_jog moveit_servo_lib_cpp) +ament_target_dependencies(demo_joint_jog ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# An example of pose tracking -add_executable(servo_pose_tracking_demo src/cpp_interface_demo/pose_tracking_demo.cpp) -target_link_libraries(servo_pose_tracking_demo pose_tracking) -ament_target_dependencies(servo_pose_tracking_demo ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Executable node for the twist demo +add_executable(demo_twist demos/cpp_interface/demo_twist.cpp ) +target_link_libraries(demo_twist moveit_servo_lib_cpp) +ament_target_dependencies(demo_twist ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Add executable to publish fake servo commands for testing/demo purposes -add_executable(fake_command_publisher test/publish_fake_jog_commands.cpp) -ament_target_dependencies(fake_command_publisher - rclcpp - geometry_msgs - std_srvs -) +# Executable node for the pose demo +add_executable(demo_pose demos/cpp_interface/demo_pose.cpp ) +target_link_libraries(demo_pose moveit_servo_lib_cpp) +ament_target_dependencies(demo_pose ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Keyboard control example for servo +add_executable(servo_keyboard_input demos/servo_keyboard_input.cpp) +target_include_directories(servo_keyboard_input PUBLIC include) +ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS}) ############# ## Install ## @@ -104,11 +100,9 @@ ament_target_dependencies(fake_command_publisher # Install Libraries install( TARGETS - moveit_servo_lib + moveit_servo_lib_cpp + moveit_servo_lib_ros moveit_servo_lib_parameters - pose_tracking - servo_node - servo_controller_input EXPORT moveit_servoTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -119,9 +113,11 @@ install( # Install Binaries install( TARGETS - servo_node_main - servo_pose_tracking_demo - fake_command_publisher + demo_joint_jog + demo_twist + demo_pose + servo_node + servo_keyboard_input ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/moveit_servo @@ -135,56 +131,25 @@ install(DIRECTORY config DESTINATION share/moveit_servo) ament_export_targets(moveit_servoTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -############# -## TESTING ## -############# if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - find_package(ros_testing REQUIRED) - find_package(Boost REQUIRED COMPONENTS filesystem) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() - - # Servo integration launch test - ament_add_gtest_executable(test_servo_integration - test/test_servo_interface.cpp - test/servo_launch_test_common.hpp - ) - target_link_libraries(test_servo_integration moveit_servo_lib_parameters) - ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # Servo collision checking integration test - ament_add_gtest_executable(test_servo_collision - test/test_servo_collision.cpp - test/servo_launch_test_common.hpp - ) - target_link_libraries(test_servo_collision moveit_servo_lib_parameters) - ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # pose_tracking - ament_add_gtest_executable(test_servo_pose_tracking - test/pose_tracking_test.cpp - ) - ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) - target_link_libraries(test_servo_pose_tracking pose_tracking) - add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # Unit tests - ament_add_gtest(servo_calcs_unit_tests - test/servo_calcs_unit_tests.cpp - ) - target_link_libraries(servo_calcs_unit_tests moveit_servo_lib) + + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + + ament_add_gtest_executable(moveit_servo_utils_test tests/test_utils.cpp) + target_link_libraries(moveit_servo_utils_test moveit_servo_lib_cpp) + ament_target_dependencies(moveit_servo_utils_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_utils.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + ament_add_gtest_executable(moveit_servo_cpp_integration_test tests/test_integration.cpp tests/servo_cpp_fixture.hpp) + target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp) + ament_target_dependencies(moveit_servo_cpp_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + ament_add_gtest_executable(moveit_servo_ros_integration_test tests/test_ros_integration.cpp tests/servo_ros_fixture.hpp) + ament_target_dependencies(moveit_servo_ros_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/moveit_servo/config/demo_rviz_config.rviz b/moveit_ros/moveit_servo/config/demo_rviz_config.rviz index 8bbbaccbc4..ad183c48c5 100644 --- a/moveit_ros/moveit_servo/config/demo_rviz_config.rviz +++ b/moveit_ros/moveit_servo/config/demo_rviz_config.rviz @@ -46,7 +46,7 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /servo_node/publish_planning_scene + Planning Scene Topic: /moveit_servo_demo/publish_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 diff --git a/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz b/moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz similarity index 99% rename from moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz rename to moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz index d93be94cde..8bbbaccbc4 100644 --- a/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz +++ b/moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz @@ -46,7 +46,7 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /monitored_planning_scene + Planning Scene Topic: /servo_node/publish_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index e65036135a..d20a9e4570 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -2,8 +2,13 @@ # Modify all parameters related to servoing here ############################################### -## Properties of incoming commands -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] + +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. @@ -11,13 +16,6 @@ scale: # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 -# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) -# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] - -## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] -low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) - # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory command_out_type: trajectory_msgs/JointTrajectory @@ -28,6 +26,7 @@ publish_joint_velocities: true publish_joint_accelerations: false ## Plugins for smoothing outgoing commands +use_smoothing: true smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, @@ -41,11 +40,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: panda_link8 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +ee_frame: panda_link8 # The name of the IK tip link ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml b/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml deleted file mode 100644 index e4b803774d..0000000000 --- a/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml +++ /dev/null @@ -1,21 +0,0 @@ -################################# -# PID parameters for pose seeking -################################# - -# Maximum value of error integral for all PID controllers -windup_limit: 0.05 - -# PID gains -x_proportional_gain: 1.5 -y_proportional_gain: 1.5 -z_proportional_gain: 1.5 -x_integral_gain: 0.0 -y_integral_gain: 0.0 -z_integral_gain: 0.0 -x_derivative_gain: 0.0 -y_derivative_gain: 0.0 -z_derivative_gain: 0.0 - -angular_proportional_gain: 0.5 -angular_integral_gain: 0.0 -angular_derivative_gain: 0.0 diff --git a/moveit_ros/moveit_servo/src/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml similarity index 64% rename from moveit_ros/moveit_servo/src/servo_parameters.yaml rename to moveit_ros/moveit_servo/config/servo_parameters.yaml index 774a085991..f5dc614552 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -1,7 +1,31 @@ servo: -################################ ROBOT SPECIFIC CONFIG ############################# +################################ GENERAL CONFIG ################################ + + thread_priority: { + type: int, + read_only: true, + default_value: 40, + description: "This value is used when configuring the servo loop thread to use SCHED_FIFO scheduling \ + We use a slightly lower priority than the ros2_control default in order to reduce jitter \ + Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html", + validation: { + gt_eq<>: 0 + } + } + + publish_period: { + type: double, + read_only: true, + default_value: 0.034, + description: " 1 / (Nominal publish rate) [seconds]", + validation: { + gt<>: 0.0 + } + } + move_group_name: { type: string, + read_only: true, description: "The name of the moveit move_group of your robot \ This parameter does not have a default value and \ must be passed to the node during launch time." @@ -14,59 +38,39 @@ servo: must be passed to the node during launch time." } - ee_frame_name: { + ee_frame: { type: string, - description: "The name of end-effector frame of your robot \ + description: "The name of end effector frame of your robot, \ + this should also be the IK tip frame of your IK solver. \ This parameter does not have a default value and \ must be passed to the node during launch time." } - robot_link_command_frame: { - type: string, - description: "The frame of incoming command, change this to your own value when using a different robot \ - This parameter does not have a default value and \ - must be passed to the node during launch time." - } - - monitored_planning_scene_topic: { - type: string, - default_value: "/planning_scene", - description: "The name of the planning scene topic. \ - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC" - } - -################################ OUTPUTS ######################################### - status_topic: { - type: string, - default_value: "~/status", - description: "The topic to which the status will be published" - } - - command_out_topic: { + active_subgroup: { type: string, - default_value: "/panda_arm_controller/joint_trajectory", - description: "The topic on which servo will publish the joint trajectory \ - Change this to the topic your controller requires." + default_value: "", + description: "This parameter can be used to switch online to actuating a subgroup of the move group. \ + If it is empty, the full move group is actuated." } - command_out_type: { +############################# INCOMING COMMAND SETTINGS ######################## + pose_command_in_topic: { type: string, - default_value: "trajectory_msgs/JointTrajectory", - description: "The type of command that servo will publish", - validation: { - one_of<>: [["trajectory_msgs/JointTrajectory", "std_msgs/Float64MultiArray"]] - } + read_only: true, + default_value: "~/pose_target_cmds", + description: "The topic on which servo will receive the pose commands" } -################################ INPUTS ############################# cartesian_command_in_topic: { type: string, + read_only: true, default_value: "~/delta_twist_cmds", description: "The topic on which servo will receive the twist commands" } joint_command_in_topic: { type: string, + read_only: true, default_value: "~/delta_joint_cmds", description: "The topic on which servo will receive the joint jog commands" } @@ -74,28 +78,14 @@ servo: command_in_type: { type: string, default_value: "unitless", - description: "The unit of the incoming command.", + description: "The unit of the incoming command. \ + unitless commands are in the range [-1:1] as if from joystick \ + speed_units are in m/s and rad/s", validation: { one_of<>: [["unitless", "speed_units"]] } } - joint_topic: { - type: string, - default_value: "/joint_states", - description: "The topic on which joint states can be monitored" - } - -################################ GENERAL CONFIG ############################# - enable_parameter_update: { - type: bool, - default_value: false, - description: "If true, continuous check for parameter update is enabled \ - Currently only the following parameters are updated: - 1. override_velocity_scaling_factor - 2. robot_link_command_frame" - } - scale: linear: { type: double, @@ -113,18 +103,69 @@ servo: description: "Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic." } - override_velocity_scaling_factor: { + + incoming_command_timeout: { type: double, - default_value: 0.0, - description: "Scaling factor when servo overrides the velocity (eg: near singularities)" + default_value: 0.1, + description: "Commands will be discarded if it is older than the timeout.\ + Important because ROS may drop some messages." } - publish_period: { - type: double, - default_value: 0.034, - description: " 1 / (Nominal publish rate) [seconds]", +################################ TWIST SETTINGS ################################# + + apply_twist_commands_about_ee_frame: { + type: bool, + default_value: true, + description: "If true, the angular velocity specified in the twist command is applied about the ee frame axes \ + if false, the twist computed will include the linear component from rotation of ee frame about the planning frame, \ + due to the existence of a lever between the two frames." + } + +############################ POSE TRACKING SETTINGS ############################# + + pose_tracking: + linear_tolerance: { + type: double, + default_value: 0.001, + description: "The allowable linear error when tracking a pose.", + validation: { + gt<>: 0.0 + } + } + + angular_tolerance: { + type: double, + default_value: 0.01, + description: "The allowable angular error when tracking a pose.", + validation: { + gt<>: 0.0 + } + } + +############################## OUTGOING COMMAND SETTINGS ####################### + + status_topic: { + type: string, + read_only: true, + default_value: "~/status", + description: "The topic to which the status will be published" + } + + command_out_topic: { + type: string, + read_only: true, + default_value: "/panda_arm_controller/joint_trajectory", + description: "The topic on which servo will publish the joint trajectory \ + Change this to the topic your controller requires." + } + + command_out_type: { + type: string, + read_only: true, + default_value: "trajectory_msgs/JointTrajectory", + description: "The type of command that servo will publish", validation: { - gt<>: 0.0 + one_of<>: [["trajectory_msgs/JointTrajectory", "std_msgs/Float64MultiArray"]] } } @@ -146,14 +187,26 @@ servo: description: "If true servo will publish joint accelerations in the output command" } - smoothing_filter_plugin_name: { +############################## PLANNING SCENE MONITOR ########################## + + monitored_planning_scene_topic: { type: string, - default_value: "online_signal_smoothing::ButterworthFilterPlugin", - description: "The name of the smoothing plugin to be used" + read_only: true, + default_value: "/planning_scene", + description: "The name of the planning scene topic. \ + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC" + } + + joint_topic: { + type: string, + read_only: true, + default_value: "/joint_states", + description: "The topic on which joint states can be monitored" } is_primary_planning_scene_monitor: { type: bool, + read_only: true, default_value: true, description: "If is_primary_planning_scene_monitor is set to true, \ the Servo server's PlanningScene advertises the /get_planning_scene service, \ @@ -162,77 +215,48 @@ servo: this should be set to false" } - incoming_command_timeout: { - type: double, - default_value: 0.1, - description: "Stop servoing if X seconds elapse without a new command. \ - If 0, republish commands forever even if the robot is stationary. \ - Otherwise, specify num. to publish. Important because ROS may drop \ - some messages and we need the robot to halt reliably." - } +############################### SMOOTHING PLUGIN ############################### - halt_all_joints_in_joint_mode: { + use_smoothing: { type: bool, + read_only: true, default_value: true, - description: "Halt all joints in joint mode" + description: "Enables the use of smoothing plugins for joint trajectory smoothing" } - halt_all_joints_in_cartesian_mode: { - type: bool, - default_value: true, - description: "Halt all joints in cartesian mode" + smoothing_filter_plugin_name: { + type: string, + read_only: true, + default_value: "online_signal_smoothing::ButterworthFilterPlugin", + description: "The name of the smoothing plugin to be used" } - lower_singularity_threshold: { - type: double, - default_value: 17.0, - description: "Start decelerating when the condition number hits this (close to singularity)", - validation: { - gt<>: 0.0 - } - } +############################# COLLISION MONITOR ################################ - hard_stop_singularity_threshold: { - type: double, - default_value: 30.0, - description: "Stop when the condition number hits this", - validation: { - gt<>: 0.0, - } + check_collisions: { + type: bool, + default_value: true, + description: "If true, servo will check for collision using the planning scene monitor." } - leaving_singularity_threshold_multiplier: { + self_collision_proximity_threshold: { type: double, - default_value: 2.0, - description: "When 'lower_singularity_threshold' is triggered, \ - but we are moving away from singularity, move this many times faster \ - than if we were moving further into singularity", + default_value: 0.01, + description: "Start decelerating when a self-collision is this far [m]", validation: { gt<>: 0.0 } } - joint_limit_margin: { + scene_collision_proximity_threshold: { type: double, - default_value: 0.1, - description: "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.", + default_value: 0.02, + description: "Start decelerating when a collision is this far [m]", validation: { gt<>: 0.0 } } - low_latency_mode: { - type: bool, - default_value: false, - description: "If true , low latency mode is enabled." - } - - check_collisions: { - type: bool, - default_value: true, - description: "If true, servo will check for collision using the planning scene monitor." - } - collision_check_rate: { type: double, default_value: 10.0, @@ -243,100 +267,71 @@ servo: } } - self_collision_proximity_threshold: { +############################# SINGULARITY CHECKING ############################# + + lower_singularity_threshold: { type: double, - default_value: 0.01, - description: "Start decelerating when a self-collision is this far [m]", + default_value: 17.0, + description: "Start decelerating when the condition number hits this (close to singularity)", validation: { gt<>: 0.0 } } - scene_collision_proximity_threshold: { + hard_stop_singularity_threshold: { type: double, - default_value: 0.02, - description: "Start decelerating when a collision is this far [m]", + default_value: 30.0, + description: "Stop when the condition number hits this", validation: { - gt<>: 0.0 + gt<>: 0.0, } } -############### POSE TRACKING PARAMETERS ######################### - - windup_limit: { - type: double, - default_value: 0.05, - description: "Maximum value of error integral for all PID controllers" - } - - x_proportional_gain: { - type: double, - default_value: 1.5, - description: "Proportional gain value for the controller in x direction" - } - - y_proportional_gain: { - type: double, - default_value: 1.5, - description: "Proportional gain value for the controller in y direction" - } - - z_proportional_gain: { - type: double, - default_value: 1.5, - description: "Proportional gain value for the controller in z direction" - } - - x_integral_gain: { - type: double, - default_value: 0.0, - description: "Integral gain value for the controller in x direction" - } - - y_integral_gain: { - type: double, - default_value: 0.0, - description: "Integral gain value for the controller in y direction" - } - - z_integral_gain: { + leaving_singularity_threshold_multiplier: { type: double, - default_value: 0.0, - description: "Integral gain value for the controller in z direction" + default_value: 2.0, + description: "When 'lower_singularity_threshold' is triggered, \ + but we are moving away from singularity, move this many times faster \ + than if we were moving further into singularity", + validation: { + gt<>: 0.0 + } } - x_derivative_gain: { + singularity_step_scale: { type: double, - default_value: 0.0, - description: "Derivative gain value for the controller in x direction" + default_value: 0.01, + description: "The test vector towards singularity is scaled by this factor during singularity check", + validation: { + gt<>: 0.0 + } } - y_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for the controller in y direction" - } +############################### JOINT LIMITING ################################# - z_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for the controller in z direction" + halt_all_joints_in_joint_mode: { + type: bool, + default_value: true, + description: "Halt all joints in joint mode, else halt only the joints at their limit" } - angular_proportional_gain: { - type: double, - default_value: 0.5, - description: "Proportional gain value for angular control" + halt_all_joints_in_cartesian_mode: { + type: bool, + default_value: true, + description: "Halt all joints in cartesian mode, else halt only the joints at their limit" } - angular_integral_gain: { + joint_limit_margin: { type: double, - default_value: 0.0, - description: "Integral gain value for angular control" + default_value: 0.1, + description: "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.", + validation: { + gt<>: 0.0 + } } - angular_derivative_gain: { + override_velocity_scaling_factor: { type: double, default_value: 0.0, - description: "Derivative gain value for angular control" + description: "Scaling factor when servo overrides the velocity (eg: near singularities)" } diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml similarity index 74% rename from moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml rename to moveit_ros/moveit_servo/config/test_config_panda.yaml index d0ee8d6b40..59dc407f61 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -2,20 +2,20 @@ # Modify all parameters related to servoing here ############################################### -## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. - joint: 0.01 - # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] ## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] +publish_period: 0.02 # 1/Nominal publish rate [seconds] + +incoming_command_timeout: 0.5 # seconds +command_in_type: "unitless" # "unitless" in the range [-1:1], as if from joystick. "speed_units" cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 1.0 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 1.0 # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory @@ -27,18 +27,21 @@ publish_joint_velocities: true publish_joint_accelerations: false ## Plugins for smoothing outgoing commands +use_smoothing: false smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: panda_hand # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_hand # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +ee_frame: panda_link8 # The name of the IK tip link ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp new file mode 100644 index 0000000000..237f3f715a --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -0,0 +1,115 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2023, PickNik Inc. + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : demo_joint_jog.cpp + * Project : moveit_servo + * Created : 05/27/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of controlling a robot through joint jog commands via the C++ API. + */ + +#include +#include +#include +#include + +using namespace moveit_servo; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.joint_jog_demo"); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + // The servo object expects to get a ROS node. + const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); + + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(demo_node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + demo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(demo_node, servo_params); + Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + + // Wait for some time, so that the planning scene is loaded in rviz. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Set the command type for servo. + servo.setCommandType(CommandType::JOINT_JOG); + // JointJog command that moves only the 7th joint at +1.0 rad/s + JointJogCommand joint_jog{ { "panda_joint7" }, { 1.0 } }; + + // Frequency at which commands will be sent to the robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + std::chrono::seconds timeout_duration(3); + std::chrono::seconds time_elapsed(0); + auto start_time = std::chrono::steady_clock::now(); + + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + while (rclcpp::ok()) + { + const KinematicState joint_state = servo.getNextJointState(joint_jog); + const StatusCode status = servo.getStatus(); + + auto current_time = std::chrono::steady_clock::now(); + time_elapsed = std::chrono::duration_cast(current_time - start_time); + if (time_elapsed > timeout_duration) + { + RCLCPP_INFO_STREAM(LOGGER, "Timed out"); + break; + } + else if (status != StatusCode::INVALID) + { + trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state)); + } + rate.sleep(); + } + + RCLCPP_INFO(LOGGER, "Exiting demo."); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp new file mode 100644 index 0000000000..bfb5e92ad7 --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -0,0 +1,159 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2023, PickNik Inc. + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : demo_pose.cpp + * Project : moveit_servo + * Created : 06/07/2023 + * Author : V Mohammed Ibrahim + * Description : Example of controlling a robot through pose commands via the C++ API. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace moveit_servo; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_demo"); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + // The servo object expects to get a ROS node. + const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); + + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(demo_node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + demo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(demo_node, servo_params); + Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + + // Wait for some time, so that the planning scene is loaded in rviz. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // For syncing pose tracking thread and main thread. + std::mutex pose_guard; + std::atomic stop_tracking = false; + + // Set the command type for servo. + servo.setCommandType(CommandType::POSE); + + // The dynamically updated target pose. + PoseCommand target_pose; + target_pose.frame_id = servo_params.planning_frame; + // Initializing the target pose as end effector pose, this can be any pose. + target_pose.pose = servo.getEndEffectorPose(); + + // The pose tracking lambda that will be run in a separate thread. + auto pose_tracker = [&]() { + KinematicState joint_state; + rclcpp::WallRate tracking_rate(1 / servo_params.publish_period); + while (!stop_tracking && rclcpp::ok()) + { + { + std::lock_guard pguard(pose_guard); + joint_state = servo.getNextJointState(target_pose); + } + StatusCode status = servo.getStatus(); + if (status != StatusCode::INVALID) + trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state)); + + tracking_rate.sleep(); + } + }; + + // Pose tracking thread will exit upon reaching this pose. + Eigen::Isometry3d terminal_pose = target_pose.pose; + terminal_pose.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + terminal_pose.translate(Eigen::Vector3d(0.0, 0.0, -0.1)); + + std::thread tracker_thread(pose_tracker); + tracker_thread.detach(); + + // The target pose (frame being tracked) moves by this step size each iteration. + Eigen::Vector3d linear_step_size{ 0.0, 0.0, -0.002 }; + Eigen::AngleAxisd angular_step_size(0.01, Eigen::Vector3d::UnitZ()); + + // Frequency at which commands will be sent to the robot controller. + rclcpp::WallRate command_rate(50); + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + + while (!stop_tracking && rclcpp::ok()) + { + { + std::lock_guard pguard(pose_guard); + target_pose.pose = servo.getEndEffectorPose(); + const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox( + terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance); + const bool satisfies_angular_tolerance = + target_pose.pose.rotation().isApprox(terminal_pose.rotation(), servo_params.pose_tracking.angular_tolerance); + stop_tracking = satisfies_linear_tolerance && satisfies_angular_tolerance; + // Dynamically update the target pose. + if (!satisfies_linear_tolerance) + target_pose.pose.translate(linear_step_size); + if (!satisfies_angular_tolerance) + target_pose.pose.rotate(angular_step_size); + } + + command_rate.sleep(); + } + + RCLCPP_INFO_STREAM(LOGGER, "REACHED : " << stop_tracking); + stop_tracking = true; + + if (tracker_thread.joinable()) + tracker_thread.join(); + + RCLCPP_INFO(LOGGER, "Exiting demo."); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp new file mode 100644 index 0000000000..3bc53ca9fd --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -0,0 +1,119 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2023, PickNik Inc. + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : demo_twist.cpp + * Project : moveit_servo + * Created : 06/01/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of controlling a robot through twist commands via the C++ API. + */ + +#include +#include +#include +#include +#include +#include + +using namespace moveit_servo; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.twist_demo"); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + // The servo object expects to get a ROS node. + const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); + + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(demo_node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + demo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(demo_node, servo_params); + Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + + // Wait for some time, so that the planning scene is loaded in rviz. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Set the command type for servo. + servo.setCommandType(CommandType::TWIST); + + // Move end effector in the +z direction at 10 cm/s + // while turning around z axis in the +ve direction at 0.5 rad/s + TwistCommand target_twist{ servo_params.planning_frame, { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } }; + + // Frequency at which commands will be sent to the robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + std::chrono::seconds timeout_duration(5); + std::chrono::seconds time_elapsed(0); + auto start_time = std::chrono::steady_clock::now(); + + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + while (rclcpp::ok()) + { + const KinematicState joint_state = servo.getNextJointState(target_twist); + const StatusCode status = servo.getStatus(); + + auto current_time = std::chrono::steady_clock::now(); + time_elapsed = std::chrono::duration_cast(current_time - start_time); + if (time_elapsed > timeout_duration) + { + RCLCPP_INFO_STREAM(LOGGER, "Timed out"); + break; + } + else if (status != StatusCode::INVALID) + { + trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state)); + } + rate.sleep(); + } + + RCLCPP_INFO(LOGGER, "Exiting demo."); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp new file mode 100644 index 0000000000..8a6507c569 --- /dev/null +++ b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp @@ -0,0 +1,365 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Title : servo_keyboard_input.cpp + * Project : moveit_servo + * Created : 05/31/2021 + * Author : Adam Pettinger, V Mohammed Ibrahim + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Define used keys +namespace +{ +constexpr int8_t KEYCODE_RIGHT = 0x43; +constexpr int8_t KEYCODE_LEFT = 0x44; +constexpr int8_t KEYCODE_UP = 0x41; +constexpr int8_t KEYCODE_DOWN = 0x42; +constexpr int8_t KEYCODE_PERIOD = 0x2E; +constexpr int8_t KEYCODE_SEMICOLON = 0x3B; +constexpr int8_t KEYCODE_1 = 0x31; +constexpr int8_t KEYCODE_2 = 0x32; +constexpr int8_t KEYCODE_3 = 0x33; +constexpr int8_t KEYCODE_4 = 0x34; +constexpr int8_t KEYCODE_5 = 0x35; +constexpr int8_t KEYCODE_6 = 0x36; +constexpr int8_t KEYCODE_7 = 0x37; +constexpr int8_t KEYCODE_Q = 0x71; +constexpr int8_t KEYCODE_R = 0x72; +constexpr int8_t KEYCODE_J = 0x6A; +constexpr int8_t KEYCODE_T = 0x74; +constexpr int8_t KEYCODE_W = 0x77; +constexpr int8_t KEYCODE_E = 0x65; +} // namespace + +// Some constants used in the Servo Teleop demo +namespace +{ +const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; +const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; +const size_t ROS_QUEUE_SIZE = 10; +const std::string PLANNING_FRAME_ID = "panda_link0"; +const std::string EE_FRAME_ID = "panda_link8"; +} // namespace + +// A class for reading the key inputs from the terminal +class KeyboardReader +{ +public: + KeyboardReader() : file_descriptor_(0) + { + // get the console in raw mode + tcgetattr(file_descriptor_, &cooked_); + struct termios raw; + memcpy(&raw, &cooked_, sizeof(struct termios)); + raw.c_lflag &= ~(ICANON | ECHO); + // Setting a new line, then end of file + raw.c_cc[VEOL] = 1; + raw.c_cc[VEOF] = 2; + tcsetattr(file_descriptor_, TCSANOW, &raw); + } + void readOne(char* c) + { + int rc = read(file_descriptor_, c, 1); + if (rc < 0) + { + throw std::runtime_error("read failed"); + } + } + void shutdown() + { + tcsetattr(file_descriptor_, TCSANOW, &cooked_); + } + +private: + int file_descriptor_; + struct termios cooked_; +}; + +// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller +class KeyboardServo +{ +public: + KeyboardServo(); + int keyLoop(); + +private: + void spin(); + + rclcpp::Node::SharedPtr nh_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Client::SharedPtr switch_input_; + + std::shared_ptr request_; + double joint_vel_cmd_; + std::string command_frame_id_; +}; + +KeyboardServo::KeyboardServo() : joint_vel_cmd_(1.0), command_frame_id_{ "panda_link0" } +{ + nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); + + twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); + joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); + + // Client for switching input types + switch_input_ = nh_->create_client("servo_node/switch_command_type"); +} + +KeyboardReader input; + +void quit(int sig) +{ + (void)sig; + input.shutdown(); + rclcpp::shutdown(); + exit(0); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + KeyboardServo keyboard_servo; + + signal(SIGINT, quit); + + int rc = keyboard_servo.keyLoop(); + input.shutdown(); + rclcpp::shutdown(); + + return rc; +} + +void KeyboardServo::spin() +{ + while (rclcpp::ok()) + { + rclcpp::spin_some(nh_); + } +} + +int KeyboardServo::keyLoop() +{ + char c; + bool publish_twist = false; + bool publish_joint = false; + + std::thread{ [this]() { return spin(); } }.detach(); + + puts("Reading from keyboard"); + puts("---------------------------"); + puts("All commands are in the planning frame"); + puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); + puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging."); + puts("Use 'j' to select joint jog. "); + puts("Use 't' to select twist "); + puts("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame"); + puts("'Q' to quit."); + + for (;;) + { + // get the next event from the keyboard + try + { + input.readOne(&c); + } + catch (const std::runtime_error&) + { + perror("read():"); + return -1; + } + + RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); + + // // Create the messages we might publish + auto twist_msg = std::make_unique(); + auto joint_msg = std::make_unique(); + + joint_msg->joint_names.resize(7); + joint_msg->joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7" }; + + joint_msg->velocities.resize(7); + std::fill(joint_msg->velocities.begin(), joint_msg->velocities.end(), 0.0); + // Use read key-press + switch (c) + { + case KEYCODE_LEFT: + RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); + twist_msg->twist.linear.y = -0.5; + publish_twist = true; + break; + case KEYCODE_RIGHT: + RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); + twist_msg->twist.linear.y = 0.5; + publish_twist = true; + break; + case KEYCODE_UP: + RCLCPP_DEBUG(nh_->get_logger(), "UP"); + twist_msg->twist.linear.x = 0.5; + publish_twist = true; + break; + case KEYCODE_DOWN: + RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); + twist_msg->twist.linear.x = -0.5; + publish_twist = true; + break; + case KEYCODE_PERIOD: + RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); + twist_msg->twist.linear.z = -0.5; + publish_twist = true; + break; + case KEYCODE_SEMICOLON: + RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); + twist_msg->twist.linear.z = 0.5; + publish_twist = true; + break; + case KEYCODE_1: + RCLCPP_DEBUG(nh_->get_logger(), "1"); + joint_msg->velocities[0] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_2: + RCLCPP_DEBUG(nh_->get_logger(), "2"); + joint_msg->velocities[1] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_3: + RCLCPP_DEBUG(nh_->get_logger(), "3"); + joint_msg->velocities[2] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_4: + RCLCPP_DEBUG(nh_->get_logger(), "4"); + joint_msg->velocities[3] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_5: + RCLCPP_DEBUG(nh_->get_logger(), "5"); + joint_msg->velocities[4] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_6: + RCLCPP_DEBUG(nh_->get_logger(), "6"); + joint_msg->velocities[5] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_7: + RCLCPP_DEBUG(nh_->get_logger(), "7"); + joint_msg->velocities[6] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_R: + RCLCPP_DEBUG(nh_->get_logger(), "r"); + joint_vel_cmd_ *= -1; + break; + case KEYCODE_J: + RCLCPP_DEBUG(nh_->get_logger(), "j"); + request_ = std::make_shared(); + request_->command_type = moveit_msgs::srv::ServoCommandType::Request::JOINT_JOG; + if (switch_input_->wait_for_service(std::chrono::seconds(1))) + { + auto result = switch_input_->async_send_request(request_); + if (result.get()->success) + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: JointJog"); + } + else + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: JointJog"); + } + } + break; + case KEYCODE_T: + RCLCPP_DEBUG(nh_->get_logger(), "t"); + request_ = std::make_shared(); + request_->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST; + if (switch_input_->wait_for_service(std::chrono::seconds(1))) + { + auto result = switch_input_->async_send_request(request_); + if (result.get()->success) + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: Twist"); + } + else + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: Twist"); + } + } + break; + case KEYCODE_W: + RCLCPP_DEBUG(nh_->get_logger(), "w"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Command frame set to: " << PLANNING_FRAME_ID); + command_frame_id_ = PLANNING_FRAME_ID; + break; + case KEYCODE_E: + RCLCPP_DEBUG(nh_->get_logger(), "e"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Command frame set to: " << EE_FRAME_ID); + command_frame_id_ = EE_FRAME_ID; + break; + case KEYCODE_Q: + RCLCPP_DEBUG(nh_->get_logger(), "quit"); + return 0; + } + + // If a key requiring a publish was pressed, publish the message now + if (publish_twist) + { + twist_msg->header.stamp = nh_->now(); + twist_msg->header.frame_id = command_frame_id_; + twist_pub_->publish(std::move(twist_msg)); + publish_twist = false; + } + else if (publish_joint) + { + joint_msg->header.stamp = nh_->now(); + joint_msg->header.frame_id = PLANNING_FRAME_ID; + joint_pub_->publish(std::move(joint_msg)); + publish_joint = false; + } + } + + return 0; +} diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h deleted file mode 100644 index 22f3db24f7..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ /dev/null @@ -1,120 +0,0 @@ -/******************************************************************************* - * Title : collision_check.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -#include - -#include - -#include -#include -#include -#include -// Auto-generated -#include - -namespace moveit_servo -{ -class CollisionCheck -{ -public: - /** \brief Constructor - * \param parameters: common settings of moveit_servo - * \param planning_scene_monitor: PSM should have scene monitor and state monitor - * already started when passed into this class - */ - CollisionCheck(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); - - ~CollisionCheck() - { - stop(); - } - - /** \brief Start the Timer that regulates collision check rate */ - void start(); - - /** \brief Stop the Timer that regulates collision check rate */ - void stop(); - -private: - /** \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_; - - // Servo parameters - const std::shared_ptr servo_param_listener_; - servo::Params servo_params_; - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Robot state and collision matrix from planning scene - std::shared_ptr current_state_; - - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale_ = 1.0; - double self_collision_distance_ = 0.0; - double scene_collision_distance_ = 0.0; - bool collision_detected_ = false; - - const double self_velocity_scale_coefficient_; - const double scene_velocity_scale_coefficient_; - - // collision request - collision_detection::CollisionRequest collision_request_; - collision_detection::CollisionResult collision_result_; - - // ROS - rclcpp::TimerBase::SharedPtr timer_; - double period_; // The loop period, in seconds - rclcpp::Publisher::SharedPtr collision_velocity_scale_pub_; - - mutable std::mutex joint_state_mutex_; - sensor_msgs::msg::JointState latest_joint_state_; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp similarity index 51% rename from moveit_ros/moveit_servo/include/moveit_servo/servo.h rename to moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp index a18909dea2..34e020376c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : servo.h - * Project : moveit_servo - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -35,71 +30,60 @@ * 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. *******************************************************************************/ +/* + * Title : collision_monitor.hpp + * Project : moveit_servo + * Created : 06/08/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description: Monitors the planning scene for collision and publishes the velocity scaling. + */ #pragma once -// System -#include - -// Moveit2 -#include -#include #include +#include +#include namespace moveit_servo { -/** - * Class Servo - Jacobian based robot control with collision avoidance. - */ -class Servo + +class CollisionMonitor { public: - Servo(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); + CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const servo::Params& servo_params, std::atomic& collision_velocity_scale); - /** \brief Start servo node */ void start(); - /** \brief Stop servo node */ void stop(); +private: /** - * Get the MoveIt planning link transform. - * The transform from the MoveIt planning frame to robot_link_command_frame - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available + * \brief The collision checking function, this will run in a separate thread. */ - bool getCommandFrameTransform(Eigen::Isometry3d& transform); - bool getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform); + void checkCollisions(); - /** - * Get the End Effector link transform. - * The transform from the MoveIt planning frame to EE link - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getEEFrameTransform(Eigen::Isometry3d& transform); - bool getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform); + // Variables - // Give test access to private/protected methods - friend class ServoFixture; + const servo::Params& servo_params_; + moveit::core::RobotStatePtr robot_state_; + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; -private: - // Servo parameters - const std::shared_ptr servo_param_listener_; - servo::Params servo_params_; + // The collision monitor thread. + std::thread monitor_thread_; + // The flag used for stopping the collision monitor thread. + std::atomic stop_requested_; - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + // The scaling factor when approaching a collision. + std::atomic& collision_velocity_scale_; - ServoCalcs servo_calcs_; - CollisionCheck collision_checker_; + // The data structures used to get information about robot self collisions. + collision_detection::CollisionRequest self_collision_request_; + collision_detection::CollisionResult self_collision_result_; + // The data structures used to get information about robot collision with other objects in the collision scene. + collision_detection::CollisionRequest scene_collision_request_; + collision_detection::CollisionResult scene_collision_result_; }; -// ServoPtr using alias -using ServoPtr = std::shared_ptr; - } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h deleted file mode 100644 index fa0436a3a6..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************* - * Title : make_shared_from_pool.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Tyler Weaver - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -#include -#include - -namespace moveit -{ -namespace util -{ -// Useful template for creating messages from a message pool -template -std::shared_ptr make_shared_from_pool() -{ - using allocator_t = boost::fast_pool_allocator>; - return std::allocate_shared(allocator_t()); -} - -} // namespace util -} // namespace moveit diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h deleted file mode 100644 index c99d239a61..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ /dev/null @@ -1,195 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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: Andy Zelenak - Desc: Servoing. Track a pose setpoint in real time. -*/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Conventions: -// Calculations are done in the planning_frame_ unless otherwise noted. - -namespace moveit_servo -{ -struct PIDConfig -{ - // Default values - double dt = 0.001; - double k_p = 1; - double k_i = 0; - double k_d = 0; - double windup_limit = 0.1; -}; - -enum class PoseTrackingStatusCode : int8_t -{ - INVALID = -1, - SUCCESS = 0, - NO_RECENT_TARGET_POSE = 1, - NO_RECENT_END_EFFECTOR_POSE = 2, - STOP_REQUESTED = 3 -}; - -const std::unordered_map POSE_TRACKING_STATUS_CODE_MAP( - { { PoseTrackingStatusCode::INVALID, "Invalid" }, - { PoseTrackingStatusCode::SUCCESS, "Success" }, - { PoseTrackingStatusCode::NO_RECENT_TARGET_POSE, "No recent target pose" }, - { PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE, "No recent end effector pose" }, - { PoseTrackingStatusCode::STOP_REQUESTED, "Stop requested" } }); - -/** - * Class PoseTracking - subscribe to a target pose. - * Servo toward the target pose. - */ -class PoseTracking -{ -public: - /** \brief Constructor. Loads ROS parameters under the given namespace. */ - PoseTracking(const rclcpp::Node::SharedPtr& node, std::unique_ptr servo_param_listener, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - - PoseTrackingStatusCode moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance, - const double target_pose_timeout); - - /** \brief A method for a different thread to stop motion and return early from control loop */ - void stopMotion(); - - /** \brief Change PID parameters. Motion is stopped before the update */ - void updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, const double x_derivative_gain, - const double y_proportional_gain, const double y_integral_gain, const double y_derivative_gain, - const double z_proportional_gain, const double z_integral_gain, const double z_derivative_gain, - const double angular_proportional_gain, const double angular_integral_gain, - const double angular_derivative_gain); - - void getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error); - - /** - * Get the End Effector link transform. - * The transform from the MoveIt planning frame to EE link - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform); - - /** \brief Re-initialize the target pose to an empty message. Can be used to reset motion between waypoints. */ - void resetTargetPose(); - - // moveit_servo::Servo instance. Public so we can access member functions like setPaused() - std::unique_ptr servo_; - -private: - /** \brief Load ROS parameters for controller settings. */ - void readROSParams(); - - /** \brief Initialize a PID controller and add it to vector of controllers */ - void initializePID(const PIDConfig& pid_config, std::vector& pid_vector); - - /** \brief Return true if a target pose has been received within timeout [seconds] */ - bool haveRecentTargetPose(const double timeout); - - /** \brief Return true if an end effector pose has been received within timeout [seconds] */ - bool haveRecentEndEffectorPose(const double timeout); - - /** \brief Check if XYZ, roll/pitch/yaw tolerances are satisfied */ - bool satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance); - - /** \brief Subscribe to the target pose on this topic */ - void targetPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); - - /** \brief Update PID controller target positions & orientations */ - void updateControllerSetpoints(); - - /** \brief Update PID controller states (positions & orientations) */ - void updateControllerStateMeasurements(); - - /** \brief Use PID controllers to calculate a full spatial velocity toward a pose */ - geometry_msgs::msg::TwistStamped::ConstSharedPtr calculateTwistCommand(); - - /** \brief Reset flags and PID controllers after a motion completes */ - void doPostMotionReset(); - - rclcpp::Node::SharedPtr node_; - servo::Params servo_parameters_; - - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - moveit::core::RobotModelConstPtr robot_model_; - // Joint group used for controlling the motions - std::string move_group_name_; - - rclcpp::WallRate loop_rate_; - - // ROS interface to Servo - rclcpp::Publisher::SharedPtr twist_stamped_pub_; - - std::vector cartesian_position_pids_; - std::vector cartesian_orientation_pids_; - // Cartesian PID configs - PIDConfig x_pid_config_, y_pid_config_, z_pid_config_, angular_pid_config_; - - // Transforms w.r.t. planning_frame_ - Eigen::Isometry3d command_frame_transform_; - rclcpp::Time command_frame_transform_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - geometry_msgs::msg::PoseStamped target_pose_; - mutable std::mutex target_pose_mtx_; - - // Subscribe to target pose - rclcpp::Subscription::SharedPtr target_pose_sub_; - - tf2_ros::Buffer transform_buffer_; - tf2_ros::TransformListener transform_listener_; - - // Expected frame name, for error checking and transforms - std::string planning_frame_; - - // Flag that a different thread has requested a stop. - std::atomic stop_requested_; - - std::optional angular_error_; -}; - -// using alias -using PoseTrackingPtr = std::shared_ptr; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp new file mode 100644 index 0000000000..b193e439fa --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -0,0 +1,222 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : servo.hpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The core servoing logic. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +class Servo +{ +public: + Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + ~Servo(); + + // Disable copy construction. + Servo(const Servo&) = delete; + + // Disable copy assignment. + Servo& operator=(Servo&) = delete; + + /** + * \brief Computes the joint state required to follow the given command. + * @param command The command to follow, std::variant type, can handle JointJog, Twist and Pose. + * @return The required joint state. + */ + KinematicState getNextJointState(const ServoInput& command); + + /** + * \brief Set the type of incoming servo command. + * @param command_type The type of command servo should expect. + */ + void setCommandType(const CommandType& command_type); + + /** + * \brief Get the type of command that servo is currently expecting. + * @return The type of command. + */ + CommandType getCommandType() const; + + /** + * \brief Get the current status of servo. + * @return The current status. + */ + StatusCode getStatus() const; + + /** + * \brief Get the message associated with the current servo status. + * @return The status message. + */ + std::string getStatusMessage() const; + + /** + * \brief Returns the end effector pose in planning frame + */ + Eigen::Isometry3d getEndEffectorPose() const; + + /** + * \brief Start/Stop collision checking thread. + * @param check_collision Stops collision checking thread if false, starts it if true. + */ + void setCollisionChecking(const bool check_collision); + + /** + * \brief Returns the most recent servo parameters. + */ + servo::Params& getParams(); + + /** + * \brief Get the current state of the robot as given by planning scene monitor. + * @return The current state of the robot. + */ + KinematicState getCurrentRobotState() const; + + /** + * \brief Smoothly halt at a commanded state when command goes stale. + * @param The last commanded joint states. + * @return The next state stepping towards the required halting state. + */ + std::pair smoothHalt(const KinematicState& halt_state) const; + +private: + /** + * \brief Finds the transform from the planning frame to a specified command frame. + * If the command frame is part of the robot model, directly look up the transform using the robot model. + * Else, fall back to using TF to look up the transform. + * @param command_frame The command frame name. + * @return The transformation between planning frame and command frame. + */ + Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame) const; + + /** + * \brief Convert a given twist command to planning frame, + * The command frame specified by `command.frame_id` is expected to be a stationary frame or end-effector frame. + * References: + * https://core.ac.uk/download/pdf/154240607.pdf + * https://www.seas.upenn.edu/~meam520/notes02/Forces8.pdf + * @param command The twist command to be converted. + * @return The transformed twist command. + */ + TwistCommand toPlanningFrame(const TwistCommand& command) const; + + /** + * \brief Convert a given pose command to planning frame + * @param command The pose command to be converted + * @return The transformed pose command + */ + PoseCommand toPlanningFrame(const PoseCommand& command) const; + + /** + * \brief Compute the change in joint position required to follow the received command. + * @param command The incoming servo command. + * @return The joint position change required (delta). + */ + Eigen::VectorXd jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state); + + /** + * \brief Updates data depending on joint model group + */ + void updateJointModelGroup(); + + /** + * \brief Validate the servo parameters + * @param servo_params The servo parameters + * @return True if parameters are valid, else False + */ + bool validateParams(const servo::Params& servo_params) const; + + /** + * \brief Updates the servo parameters and performs validations. + */ + bool updateParams(); + + /** + * \brief Create and initialize the smoothing plugin to be used by servo. + */ + void setSmoothingPlugin(); + + /** + * \brief Apply halting logic to specified joints. + * @param joints_to_halt The indices of joints to be halted. + * @param current_state The current kinematic state. + * @param target_state The target kinematic state. + * @return The bounded kinematic state. + */ + KinematicState haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + const KinematicState& target_state) const; + + // Variables + + StatusCode servo_status_; + // This needs to be threadsafe so it can be updated in realtime. + std::atomic expected_command_type_; + + servo::Params servo_params_; + const rclcpp::Node::SharedPtr node_; + std::shared_ptr servo_param_listener_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // This value will be updated by CollisionMonitor in a separate thread. + std::atomic collision_velocity_scale_ = 1.0; + std::unique_ptr collision_monitor_; + + pluginlib::UniquePtr smoother_ = nullptr; + + // Map between joint subgroup names and corresponding joint name - move group indices map + std::unordered_map joint_name_to_index_maps_; +}; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h deleted file mode 100644 index 39f780e221..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ /dev/null @@ -1,271 +0,0 @@ -/******************************************************************************* - * Title : servo_calcs.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -// C++ -#include -#include -#include -#include - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// moveit_core -#include - -// moveit_servo -#include -#include -#include - -namespace moveit_servo -{ -enum class ServoType -{ - CARTESIAN_SPACE, - JOINT_SPACE -}; - -class ServoCalcs -{ -public: - ServoCalcs(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); - - ~ServoCalcs(); - - /** - * Start the timer where we do work and publish outputs - * - * @exception can throw a std::runtime_error if the setup was not completed - */ - void start(); - - /** \brief Stop the currently running thread */ - void stop(); - - /** - * Check for parameter update, and apply updates if any - * All dynamic parameters must be checked and updated within this method - */ - void updateParams(); - - /** - * Get the MoveIt planning link transform. - * The transform from the MoveIt planning frame to robot_link_command_frame - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getCommandFrameTransform(Eigen::Isometry3d& transform); - bool getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform); - - /** - * Get the End Effector link transform. - * The transform from the MoveIt planning frame to EE link - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getEEFrameTransform(Eigen::Isometry3d& transform); - bool getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform); - -protected: - /** \brief Run the main calculation loop */ - void mainCalcLoop(); - - /** \brief Do calculations for a single iteration. Publish one outgoing command */ - void calculateSingleIteration(); - - /** \brief Do servoing calculations for Cartesian twist commands. */ - bool cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd, - trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** \brief Do servoing calculations for direct commands to a joint. */ - bool jointServoCalcs(const control_msgs::msg::JointJog& cmd, trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** - * Checks a JointJog msg for valid (non-NaN) velocities - * @param cmd the desired joint servo command - * @return true if this represents a valid joint servo command, false otherwise - */ - bool checkValidCommand(const control_msgs::msg::JointJog& cmd); - - /** - * Checks a TwistStamped msg for valid (non-NaN) velocities - * @param cmd the desired twist servo command - * @return true if this represents a valid servo twist command, false otherwise - */ - bool checkValidCommand(const geometry_msgs::msg::TwistStamped& cmd); - - /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. - * Also, multiply by timestep to calculate a position change. - * @return a vector of position deltas - */ - Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::msg::TwistStamped& command); - - /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. - * Also, multiply by timestep to calculate a position change. - * @return a vector of position deltas - */ - Eigen::VectorXd scaleJointCommand(const control_msgs::msg::JointJog& command); - - /** \brief Come to a halt in a smooth way. Apply a smoothing plugin, if one is configured. - */ - void filteredHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** \brief Suddenly halt for a joint limit or other critical issue. - * Is handled differently for position vs. velocity control. - */ - void suddenHalt(sensor_msgs::msg::JointState& joint_state, - const std::vector& joints_to_halt) const; - - /** \brief Compose the outgoing JointTrajectory message */ - void composeJointTrajMessage(const sensor_msgs::msg::JointState& joint_state, - trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** \brief Set the filters to the specified values */ - void resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state); - - /** \brief Handles all aspects of the servoing after the desired joint commands are established - * Joint and Cartesian calcs feed into here - * Handles limit enforcement, internal state updated, collision scaling, and publishing the commands - * @param delta_theta Eigen vector of joint delta's, from joint or Cartesian servo calcs - * @param joint_trajectory Output trajectory message - * @param servo_type The type of servoing command being used - */ - bool internalServoUpdate(Eigen::ArrayXd& delta_theta, trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const ServoType servo_type); - - /* \brief Command callbacks */ - void twistStampedCB(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg); - void jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& msg); - void collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg); - - // Pointer to the ROS node - std::shared_ptr node_; - - // Servo parameters - const std::shared_ptr servo_param_listener_; - servo::Params servo_params_; - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Flag for staying inactive while there are no incoming commands - bool wait_for_servo_commands_ = true; - - // Flag saying if the filters were updated during the timer callback - bool updated_filters_ = false; - - // Incoming command messages - geometry_msgs::msg::TwistStamped twist_stamped_cmd_; - control_msgs::msg::JointJog joint_servo_cmd_; - - const moveit::core::JointModelGroup* joint_model_group_; - - moveit::core::RobotStatePtr current_state_; - - // These variables are mutex protected - // previous_joint_state_ holds the state q(t - dt) - // internal_joint_state_ holds the state q(t) as retrieved from the planning scene monitor. - sensor_msgs::msg::JointState previous_joint_state_, internal_joint_state_; - std::map joint_state_name_map_; - - // Smoothing algorithm (loads a plugin) - pluginlib::UniquePtr smoother_; - - trajectory_msgs::msg::JointTrajectory::SharedPtr last_sent_command_; - - // ROS - rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::Subscription::SharedPtr twist_stamped_sub_; - rclcpp::Subscription::SharedPtr joint_cmd_sub_; - rclcpp::Subscription::SharedPtr collision_velocity_scale_sub_; - rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub_; - rclcpp::Publisher::SharedPtr multiarray_outgoing_cmd_pub_; - - // Main tracking / result publisher loop - std::thread thread_; - bool stop_requested_; - - // Status - StatusCode status_ = StatusCode::NO_WARNING; - bool twist_command_is_stale_ = false; - bool joint_command_is_stale_ = false; - double collision_velocity_scale_ = 1.0; - - // Use ArrayXd type to enable more coefficient-wise operations - Eigen::ArrayXd delta_theta_; - - unsigned int num_joints_; - - // main_loop_mutex_ is used to protect the input state and dynamic parameters - mutable std::mutex main_loop_mutex_; - Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; - Eigen::Isometry3d tf_moveit_to_ee_frame_; - geometry_msgs::msg::TwistStamped::ConstSharedPtr latest_twist_stamped_; - control_msgs::msg::JointJog::ConstSharedPtr latest_joint_cmd_; - rclcpp::Time latest_twist_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME); - rclcpp::Time latest_joint_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME); - - // input condition variable used for low latency mode - std::condition_variable input_cv_; - bool new_input_cmd_ = false; - - // Load a smoothing plugin - pluginlib::ClassLoader smoothing_loader_; - - kinematics::KinematicsBaseConstPtr ik_solver_ = nullptr; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h deleted file mode 100644 index 762088b55e..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h +++ /dev/null @@ -1,83 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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. - *********************************************************************/ - -/* Title : servo_node.h - * Project : moveit_servo - * Created : 07/13/2020 - * Author : Adam Pettinger - */ - -#pragma once - -#include -#include -#include - -namespace moveit_servo -{ -class ServoNode -{ -public: - ServoNode(const rclcpp::NodeOptions& options); - - // NOLINTNEXTLINE(readability-identifier-naming) - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() - { - return node_->get_node_base_interface(); - } - -private: - std::shared_ptr node_; - std::unique_ptr servo_; - std::shared_ptr tf_buffer_; - std::shared_ptr planning_scene_monitor_; - - /*** - * \brief Most servo parameters are individually validated using the validation methods in - * `generate_parameter_library`, with limits specified in the parameters YAML file. This method performs additional - * validation for parameters whose values depend on each other. - */ - void validateParams(const servo::Params& servo_params); - - /** \brief Start the servo loop. Must be called once to begin Servoing. */ - void startCB(const std::shared_ptr& request, - const std::shared_ptr& response); - rclcpp::Service::SharedPtr start_servo_service_; - - /** \brief Stop the servo loop. */ - void stopCB(const std::shared_ptr& request, - const std::shared_ptr& response); - rclcpp::Service::SharedPtr stop_servo_service_; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp new file mode 100644 index 0000000000..88bce72f7c --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -0,0 +1,137 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : servo_node.hpp + * Project : moveit_servo + * Created : 01/07/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The ROS API for Servo + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +class ServoNode +{ +public: + explicit ServoNode(const rclcpp::NodeOptions& options); + + ~ServoNode(); + + // Disable copy construction. + ServoNode(const ServoNode&) = delete; + + // Disable copy assignment. + ServoNode& operator=(ServoNode&) = delete; + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html + // Skip linting due to unconventional function naming + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); // NOLINT + +private: + /** + * \brief Loop that handles different types of incoming commands. + */ + void servoLoop(); + + /** + * \brief The service to pause servoing, this does not exit the loop or stop the servo loop thread. + * The loop will be alive even after pausing, but no commands will be processed. + */ + void pauseServo(const std::shared_ptr& request, + const std::shared_ptr& response); + + /** + * \brief The service to set the command type for Servo. + * Supported command types can be found in utils/datatypes.hpp + * This service must be used to set the command type before sending any servoing commands. + */ + void switchCommandType(const std::shared_ptr& request, + const std::shared_ptr& response); + + void jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg); + void twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg); + void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); + + std::optional processJointJogCommand(); + std::optional processTwistCommand(); + std::optional processPoseCommand(); + + // Variables + + const rclcpp::Node::SharedPtr node_; + std::unique_ptr servo_; + servo::Params servo_params_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + KinematicState last_commanded_state_; // Used when commands go stale; + control_msgs::msg::JointJog latest_joint_jog_; + geometry_msgs::msg::TwistStamped latest_twist_; + geometry_msgs::msg::PoseStamped latest_pose_; + rclcpp::Subscription::SharedPtr joint_jog_subscriber_; + rclcpp::Subscription::SharedPtr twist_subscriber_; + rclcpp::Subscription::SharedPtr pose_subscriber_; + + rclcpp::Publisher::SharedPtr multi_array_publisher_; + rclcpp::Publisher::SharedPtr trajectory_publisher_; + rclcpp::Publisher::SharedPtr status_publisher_; + + rclcpp::Service::SharedPtr switch_command_type_; + rclcpp::Service::SharedPtr pause_servo_; + + // Used for communication with thread + std::atomic stop_servo_; + std::atomic servo_paused_; + std::atomic new_joint_jog_msg_, new_twist_msg_, new_pose_msg_; + + // Threads used by ServoNode + std::thread servo_loop_thread_; +}; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utilities.h b/moveit_ros/moveit_servo/include/moveit_servo/utilities.h deleted file mode 100644 index 30bedbd061..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/utilities.h +++ /dev/null @@ -1,136 +0,0 @@ -// Copyright 2022 PickNik Inc. -// -// 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 PickNik Inc. 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 HOLDER 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 : Andy Zelenak - Desc : Free functions. We keep them in a separate translation unit to reduce .o filesize - Title : utilities.h - Project : moveit_servo -*/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace moveit_servo -{ -// Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped -geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, - const std::string& parent_frame, - const std::string& child_frame); - -/** \brief Possibly calculate a velocity scaling factor, due to proximity of - * singularity and direction of motion - * @param[in] joint_model_group The MoveIt group - * @param[in] commanded_twist The commanded Cartesian twist - * @param[in] svd A singular value decomposition of the Jacobian - * @param[in] pseudo_inverse The pseudo-inverse of the Jacobian - * @param[in] hard_stop_singularity_threshold Halt if condition(Jacobian) > hard_stop_singularity_threshold - * @param[in] lower_singularity_threshold Decelerate if condition(Jacobian) > lower_singularity_threshold - * @param[in] leaving_singularity_threshold_multiplier Allow faster motion away from singularity - * @param[in, out] current_state The state of the robot. Used in internal calculations. - * @param[out] status Singularity status - */ -double velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, - const Eigen::VectorXd& commanded_twist, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse, - const double hard_stop_singularity_threshold, - const double lower_singularity_threshold, - const double leaving_singularity_threshold_multiplier, - const moveit::core::RobotStatePtr& current_state, StatusCode& status); - -/** \brief Joint-wise update of a sensor_msgs::msg::JointState with given delta values. - * Also filters and calculates the previous velocity - * @param publish_period The publishing rate for servo command, in seconds. - * @param delta_theta Eigen vector of joint delta values. - * @param previous_joint_state The previous joint state. - * @param current_joint_state The joint state object which will hold the current joint state to send as a command. - * @param smoother The trajectory smoother to be used. - * @return Returns true if successful and false otherwise. - */ -bool applyJointUpdate(const double publish_period, const Eigen::ArrayXd& delta_theta, - const sensor_msgs::msg::JointState& previous_joint_state, - sensor_msgs::msg::JointState& next_joint_state, - pluginlib::UniquePtr& smoother); - -/** \brief Converts a twist command from the command frame to the MoveIt planning frame of the robot - * @param cmd The twist command received from the user - * @param planning_frame Moveit planning frame of the robot - * @param current_state The state of the robot - */ -void transformTwistToPlanningFrame(geometry_msgs::msg::TwistStamped& cmd, const std::string& planning_frame, - const moveit::core::RobotStatePtr& current_state); - -/** \brief Converts the delta_x (change in cartesian position) to a pose to be used with IK solver. - * @param delta_x The change in cartesian position - * @param base_to_tip_frame_transform The transform from base of the robot to its end-effector - * @return Returns the resulting pose after applying delta_x - */ -geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, - const Eigen::Isometry3d& base_to_tip_frame_transform); - -/** - * @brief Computes the velocity scaling factor based on the joint velocity limits - * @param joint_model_group The MoveIt group - * @param velocity The vector containing the target velocities of the joints - * @return The velocity scaling factor - */ -double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model_group, const Eigen::VectorXd& velocity); - -/** - * @brief Decrease robot position change and velocity, if needed, to satisfy joint velocity limits - * @param joint_model_group Active joint group. Used to retrieve limits - * @param publish_period Period of the servo loop - * @param joint_state The command that will go to the robot - * @param override_velocity_scaling_factor Option if the user wants a constant override of the velocity scaling. - * A value greater than 0 will override the internal calculations done by getVelocityScalingFactor - */ -void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period, - sensor_msgs::msg::JointState& joint_state, - const double override_velocity_scaling_factor = 0.0); - -/** \brief Avoid overshooting joint limits - * @param joint_state The joint state to be checked - * @param joint_limit_margin The allowed margin for joint limit. This is a buffer, prior to the actual limit. - * @param joint_model_group The MoveIt group - * @return Vector of the joints that would move farther past position margin limits - */ -std::vector -enforcePositionLimits(sensor_msgs::msg::JointState& joint_state, const double joint_limit_margin, - const moveit::core::JointModelGroup* joint_model_group); - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp new file mode 100644 index 0000000000..2446670dbf --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -0,0 +1,101 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : command.hpp + * Project : moveit_servo + * Created : 06/04/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The methods that compute the required change in joint angles for various input types. + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +/** + * \brief Compute the change in joint position for the given joint jog command. + * @param command The joint jog command. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. + * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); + +/** + * \brief Compute the change in joint position for the given twist command. + * @param command The twist command. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. + * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); + +/** + * \brief Compute the change in joint position for the given pose command. + * @param command The pose command. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. + * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); + +/** + * \brief Computes the required change in joint angles for given Cartesian change, using the robot's IK solver. + * @param cartesian_position_delta The change in Cartesian position. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. + * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, + const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp new file mode 100644 index 0000000000..165d1bae9d --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -0,0 +1,169 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : utils.hpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + * + * Description : The utility functions used by MoveIt Servo. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +/** + * \brief Checks if a given VectorXd is a valid command. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const Eigen::VectorXd& command); + +/** + * \brief Checks if a given Isometry3d (pose) is a valid command. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const Eigen::Isometry3d& command); + +/** + * \brief Checks if a given Twist command is valid. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const TwistCommand& command); + +/** + * \brief Checks if a given Pose command is valid. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const PoseCommand& command); + +/** + * \brief Create a pose message for the provided change in Cartesian position. + * @param delta_x The change in Cartesian position. + * @param base_to_tip_frame_transform The transformation from robot base to ee frame. + * @return The pose message. + */ +geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, + const Eigen::Isometry3d& base_to_tip_frame_transform); + +/** + * \brief Create a trajectory message from given joint state. + * @param servo_params The configuration used by servo, required for setting some field of the trajectory message. + * @param joint_state The joint state to be added into the trajectory. + * @return The trajectory message. + */ +trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Params& servo_params, + const KinematicState& joint_state); + +/** + * \brief Create a Float64MultiArray message from given joint state + * @param servo_params The configuration used by servo, required for selecting position vs velocity. + * @param joint_state The joint state to be added into the Float64MultiArray. + * @return The Float64MultiArray message. + */ +std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& servo_params, + const KinematicState& joint_state); + +/** + * \brief Computes scaling factor for velocity when the robot is near a singularity. + * @param joint_model_group The joint model group of the robot, used for fetching the Jacobian. + * @param current_state The current state of the robot, used for singularity look ahead. + * @param target_delta_x The vector containing the required change in Cartesian position. + * @param servo_params The servo parameters, contains the singularity thresholds. + * @return The velocity scaling factor and the reason for scaling. + */ +std::pair velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr& robot_state, + const Eigen::VectorXd& target_delta_x, + const servo::Params& servo_params); + +/** + * \brief Apply velocity scaling based on joint limits. + * @param velocities The commanded velocities. + * @param joint_bounds The bounding information for the robot joints. + * @param scaling_override The user defined velocity scaling override. + * @return The velocity scaling factor. + */ +double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double scaling_override); + +/** + * \brief Finds the joints that are exceeding allowable position limits. + * @param positions The joint positions. + * @param velocities The current commanded velocities. + * @param joint_bounds The allowable limits for the robot joints. + * @param margin Additional buffer on the actual joint limits. + * @return The joints that are violating the specified position limits. + */ +std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double margin); + +/** + * \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped. + * @param eigen_tf The isometry to be converted to TransformStamped. + * @param parent_frame The target frame. + * @param child_frame The current frame. + * @return The isometry as a TransformStamped message. + */ +geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame); + +/** + * \brief Convert a PoseStamped message to a Servo Pose + * @param msg The PoseStamped message. + * @return The equivalent Servo Pose type. + */ +PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped& msg); + +/** + * \brief Creates the planning scene monitor used by servo + */ +planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, + const servo::Params& servo_params); +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp similarity index 56% rename from moveit_ros/moveit_servo/include/moveit_servo/status_codes.h rename to moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp index f5942fa4be..37d83f9a09 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : status_codes.h - * Project : moveit_servo - * Created : 2/25/2019 - * Author : Andy Zelenak - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,24 +31,33 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : datatypes.hpp + * Project : moveit_servo + * Created : 06/05/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + * + * Description : The custom datatypes used by Moveit Servo. + */ + #pragma once #include +#include #include namespace moveit_servo { + enum class StatusCode : int8_t { INVALID = -1, NO_WARNING = 0, DECELERATE_FOR_APPROACHING_SINGULARITY = 1, HALT_FOR_SINGULARITY = 2, - DECELERATE_FOR_COLLISION = 3, - HALT_FOR_COLLISION = 4, - JOINT_BOUND = 5, - DECELERATE_FOR_LEAVING_SINGULARITY = 6, - PAUSED = 7 + DECELERATE_FOR_LEAVING_SINGULARITY = 3, + DECELERATE_FOR_COLLISION = 4, + HALT_FOR_COLLISION = 5, + JOINT_BOUND = 6 }; const std::unordered_map SERVO_STATUS_CODE_MAP( @@ -61,9 +65,71 @@ const std::unordered_map SERVO_STATUS_CODE_MAP( { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, - { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" }, - { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, - { StatusCode::PAUSED, "Paused" } }); + { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); + +// The datatype that specifies the type of command that servo should expect. +enum class CommandType : int8_t +{ + JOINT_JOG = 0, + TWIST = 1, + POSE = 2, + + // Range of allowed values used for validation. + MIN = JOINT_JOG, + MAX = POSE +}; + +typedef std::pair JointDeltaResult; + +// The joint jog command, this will be vector of length equal to the number of joints of the robot. +struct JointJogCommand +{ + std::vector names; + std::vector velocities; +}; + +// The twist command, frame_id is the name of the frame in which the command is specified in. +// frame_id must always be specified. +struct TwistCommand +{ + std::string frame_id; + Eigen::Vector velocities; +}; + +// The Pose command, frame_id is the name of the frame in which the command is specified in. +// frame_id must always be specified. +struct PoseCommand +{ + std::string frame_id; + Eigen::Isometry3d pose; +}; + +// The generic input type for servo that can be JointJog, Twist or Pose. +typedef std::variant ServoInput; + +// The output datatype of servo, this structure contains the names of the joints along with their positions, velocities and accelerations. +struct KinematicState +{ + std::vector joint_names; + Eigen::VectorXd positions, velocities, accelerations; + + KinematicState(const int num_joints) + { + joint_names.resize(num_joints); + positions.resize(num_joints); + velocities.resize(num_joints); + accelerations.resize(num_joints); + } + + KinematicState() + { + } +}; + +// Mapping joint names and their position in the move group vector +typedef std::unordered_map JointNameToMoveGroupIndexMap; + } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py new file mode 100644 index 0000000000..6167c23083 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py @@ -0,0 +1,119 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # 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=[moveit_config.robot_description, ros2_controllers_path], + 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", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="demo_joint_jog", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/moveit_ros/moveit_servo/launch/demo_pose.launch.py b/moveit_ros/moveit_servo/launch/demo_pose.launch.py new file mode 100644 index 0000000000..674c40b9da --- /dev/null +++ b/moveit_ros/moveit_servo/launch/demo_pose.launch.py @@ -0,0 +1,119 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # 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=[moveit_config.robot_description, ros2_controllers_path], + 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", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="demo_pose", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py similarity index 89% rename from moveit_ros/moveit_servo/launch/servo_example.launch.py rename to moveit_ros/moveit_servo/launch/demo_ros_api.launch.py index 224cb6bf55..41ba40d3bc 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py @@ -9,17 +9,17 @@ def generate_launch_description(): - # Launch Servo as a standalone node or as a "node component" for better latency/efficiency - launch_as_standalone_node = LaunchConfiguration( - "launch_as_standalone_node", default="false" - ) - moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .to_moveit_configs() ) + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="false" + ) + # Get parameters for the Servo node servo_params = { "moveit_servo": ParameterBuilder("moveit_servo") @@ -32,7 +32,8 @@ def generate_launch_description(): # RViz rviz_config_file = ( - get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + get_package_share_directory("moveit_servo") + + "/config/demo_rviz_config_ros.rviz" ) rviz_node = launch_ros.actions.Node( package="rviz2", @@ -92,8 +93,10 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, + low_pass_filter_coeff, moveit_config.robot_description, moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, ], condition=UnlessCondition(launch_as_standalone_node), ), @@ -109,25 +112,15 @@ def generate_launch_description(): name="static_tf2_broadcaster", parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], ), - launch_ros.descriptions.ComposableNode( - package="moveit_servo", - plugin="moveit_servo::JoyToServoPub", - name="controller_to_servo_node", - ), - launch_ros.descriptions.ComposableNode( - package="joy", - plugin="joy::Joy", - name="joy_node", - ), ], output="screen", ) - - # Optionally launch a standalone Servo node. + # Launch a standalone Servo node. # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC servo_node = launch_ros.actions.Node( package="moveit_servo", - executable="servo_node_main", + executable="servo_node", + name="servo_node", parameters=[ servo_params, low_pass_filter_coeff, diff --git a/moveit_ros/moveit_servo/launch/demo_twist.launch.py b/moveit_ros/moveit_servo/launch/demo_twist.launch.py new file mode 100644 index 0000000000..63bd829c3b --- /dev/null +++ b/moveit_ros/moveit_servo/launch/demo_twist.launch.py @@ -0,0 +1,119 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # 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=[moveit_config.robot_description, ros2_controllers_path], + 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", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="demo_twist", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py deleted file mode 100644 index aac0882bab..0000000000 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ /dev/null @@ -1,110 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - - # Get parameters for the Pose Tracking node - pose_tracker_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/pose_tracking_settings.yaml") - .to_dict() - } - - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config_pose_tracking.yaml") - .to_dict() - } - - # RViz - rviz_config_file = ( - get_package_share_directory("moveit_servo") - + "/config/demo_rviz_pose_tracking.rviz" - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - # prefix=['xterm -e gdb -ex run --args'], - output="log", - arguments=["-d", rviz_config_file], - parameters=[moveit_config.to_dict()], - ) - - # Publishes tf's for the robot - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[moveit_config.robot_description], - ) - - # A node to publish world -> panda_link0 transform - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], - ) - - pose_tracking_node = Node( - package="moveit_servo", - executable="servo_pose_tracking_demo", - # prefix=['xterm -e gdb -ex run --args'], - output="screen", - parameters=[moveit_config.to_dict(), servo_params, pose_tracker_params], - ) - - # 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 = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[moveit_config.robot_description, ros2_controllers_path], - output="screen", - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - "--controller-manager-timeout", - "300", - "--controller-manager", - "/controller_manager", - ], - ) - - panda_arm_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["panda_arm_controller", "-c", "/controller_manager"], - ) - - return LaunchDescription( - [ - rviz_node, - static_tf, - pose_tracking_node, - ros2_control_node, - joint_state_broadcaster_spawner, - panda_arm_controller_spawner, - robot_state_publisher, - ] - ) diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index fa895b0117..2a96034fba 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -2,7 +2,7 @@ moveit_servo - 2.7.4 + 2.8.0 Provides real-time manipulator Cartesian and joint servoing. Blake Anderson Andy Zelenak @@ -24,11 +24,10 @@ moveit_common control_msgs - control_toolbox generate_parameter_library geometry_msgs - moveit_msgs moveit_core + moveit_msgs moveit_ros_planning_interface pluginlib realtime_tools @@ -43,10 +42,10 @@ joint_state_broadcaster joint_trajectory_controller joy + launch_param_builder + moveit_configs_utils robot_state_publisher tf2_ros - moveit_configs_utils - launch_param_builder ament_cmake_gtest ament_lint_auto diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp deleted file mode 100644 index 3a3c8ede94..0000000000 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 HOLDER 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. - *******************************************************************************/ - -/* Title : collision_check.cpp - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include - -#include -// #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.collision_check"); -static const double MIN_RECOMMENDED_COLLISION_RATE = 10; -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30 * 1000; // Milliseconds to throttle logs inside loops - -namespace moveit_servo -{ -// Constructor for the class that handles collision checking -CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener) - : node_(node) - , servo_param_listener_(servo_param_listener) - , servo_params_(servo_param_listener_->get_params()) - , planning_scene_monitor_(planning_scene_monitor) - , self_velocity_scale_coefficient_(-log(0.001) / servo_params_.self_collision_proximity_threshold) - , scene_velocity_scale_coefficient_(-log(0.001) / servo_params_.scene_collision_proximity_threshold) - , period_(1.0 / servo_params_.collision_check_rate) -{ - // Init collision request - collision_request_.group_name = servo_params_.move_group_name; - collision_request_.distance = true; // enable distance-based collision checking - collision_request_.contacts = true; // Record the names of collision pairs - - if (servo_params_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) - { - auto& clk = *node_->get_clock(); -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clk, ROS_LOG_THROTTLE_PERIOD, - "Collision check rate is low, increase it in yaml file if CPU allows"); -#pragma GCC diagnostic pop - } - - // ROS pubs/subs - collision_velocity_scale_pub_ = - node_->create_publisher("~/collision_velocity_scale", rclcpp::SystemDefaultsQoS()); - - 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(); }); -} - -void CollisionCheck::stop() -{ - if (timer_) - { - timer_->cancel(); - } -} - -void CollisionCheck::run() -{ - // Update the latest parameters - if (servo_params_.enable_parameter_update) - { - servo_params_ = servo_param_listener_->get_params(); - } - - // Only do collision checking if the check_collisions parameter is currently true. - velocity_scale_ = 1; - if (servo_params_.check_collisions) - { - // Update to the latest current state - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->updateCollisionBodyTransforms(); - collision_detected_ = false; - - // Do a timer-safe distance-based collision detection - collision_result_.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, - *current_state_); - 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()); - self_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); - - // If we're definitely in collision, stop immediately - if (collision_detected_) - { - velocity_scale_ = 0; - } - else - { - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When scene_collision_proximity_threshold is breached, start decelerating exponentially. - if (scene_collision_distance_ < servo_params_.scene_collision_proximity_threshold) - { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale_ = std::min( - velocity_scale_, exp(scene_velocity_scale_coefficient_ * - (scene_collision_distance_ - servo_params_.scene_collision_proximity_threshold))); - } - - if (self_collision_distance_ < servo_params_.self_collision_proximity_threshold) - { - velocity_scale_ = std::min(velocity_scale_, - exp(self_velocity_scale_coefficient_ * - (self_collision_distance_ - servo_params_.self_collision_proximity_threshold))); - } - } - } - - // Publish collision velocity scaling message. - { - auto msg = std::make_unique(); - msg->data = velocity_scale_; - collision_velocity_scale_pub_->publish(std::move(msg)); - } -} -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp new file mode 100644 index 0000000000..3e34311022 --- /dev/null +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -0,0 +1,159 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ +/* + * Title : collision_monitor.cpp + * Project : moveit_servo + * Created : 06/08/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + */ + +#include +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); +} + +namespace moveit_servo +{ + +CollisionMonitor::CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const servo::Params& servo_params, std::atomic& collision_velocity_scale) + : servo_params_(servo_params) + , planning_scene_monitor_(planning_scene_monitor) + , collision_velocity_scale_(collision_velocity_scale) +{ +} + +void CollisionMonitor::start() +{ + stop_requested_ = false; + if (!monitor_thread_.joinable()) + { + monitor_thread_ = std::thread(&CollisionMonitor::checkCollisions, this); + RCLCPP_INFO_STREAM(LOGGER, "Collision monitor started"); + } + else + { + RCLCPP_ERROR_STREAM(LOGGER, "Collision monitor could not be started"); + } +} + +void CollisionMonitor::stop() +{ + stop_requested_ = true; + if (monitor_thread_.joinable()) + { + monitor_thread_.join(); + } + RCLCPP_INFO_STREAM(LOGGER, "Collision monitor stopped"); +} + +void CollisionMonitor::checkCollisions() +{ + rclcpp::WallRate rate(servo_params_.collision_check_rate); + + bool approaching_self_collision, approaching_scene_collision; + double self_collision_threshold_delta, scene_collision_threshold_delta; + double self_collision_scale, scene_collision_scale; + const double log_val = -log(0.001); + + while (rclcpp::ok() && !stop_requested_) + { + const double self_velocity_scale_coefficient{ log_val / servo_params_.self_collision_proximity_threshold }; + const double scene_velocity_scale_coefficient{ log_val / servo_params_.scene_collision_proximity_threshold }; + + // Reset the scale on every iteration. + collision_velocity_scale_ = 1.0; + + if (servo_params_.check_collisions) + { + // Fetch latest robot state. + robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + // This must be called before doing collision checking. + robot_state_->updateCollisionBodyTransforms(); + + // Get a read-only copy of planning scene. + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + + // Check collision with environment. + scene_collision_result_.clear(); + locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_, + *robot_state_); + + // Check robot self collision. + self_collision_result_.clear(); + locked_scene->getCollisionEnvUnpadded()->checkSelfCollision( + self_collision_request_, self_collision_result_, *robot_state_, locked_scene->getAllowedCollisionMatrix()); + + // If collision detected scale velocity to 0, else start decelerating exponentially. + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + + if (self_collision_result_.collision || scene_collision_result_.collision) + { + collision_velocity_scale_ = 0.0; + } + else + { + self_collision_scale = scene_collision_scale = 1.0; + + approaching_scene_collision = + scene_collision_result_.distance < servo_params_.scene_collision_proximity_threshold; + approaching_self_collision = self_collision_result_.distance < servo_params_.self_collision_proximity_threshold; + + if (approaching_scene_collision) + { + scene_collision_threshold_delta = + scene_collision_result_.distance - servo_params_.scene_collision_proximity_threshold; + scene_collision_scale = std::exp(scene_velocity_scale_coefficient * scene_collision_threshold_delta); + } + + if (approaching_self_collision) + { + self_collision_threshold_delta = + self_collision_result_.distance - servo_params_.self_collision_proximity_threshold; + self_collision_scale = std::exp(self_velocity_scale_coefficient * self_collision_threshold_delta); + } + + // Use the scaling factor with lower value, i.e maximum scale down. + collision_velocity_scale_ = std::min(scene_collision_scale, self_collision_scale); + } + } + rate.sleep(); + } +} +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp b/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp deleted file mode 100644 index 2f52cf1657..0000000000 --- a/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/******************************************************************************* - * Title : pose_tracking_example.cpp - * Project : moveit_servo - * Created : 09/04/2020 - * Author : Adam Pettinger - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 HOLDER 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. - *******************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_tracking_demo"); - -// Class for monitoring status of moveit_servo -class StatusMonitor -{ -public: - StatusMonitor(const rclcpp::Node::SharedPtr& node, const std::string& topic) - { - sub_ = node->create_subscription(topic, rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Int8::ConstSharedPtr& msg) { - return statusCB(msg); - }); - } - -private: - void statusCB(const std_msgs::msg::Int8::ConstSharedPtr& msg) - { - moveit_servo::StatusCode latest_status = static_cast(msg->data); - if (latest_status != status_) - { - status_ = latest_status; - const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); - RCLCPP_INFO_STREAM(LOGGER, "Servo status: " << status_str); - } - } - - moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; - rclcpp::Subscription::SharedPtr sub_; -}; - -/** - * Instantiate the pose tracking interface. - * Send a pose slightly different from the starting pose - * Then keep updating the target pose a little bit - */ -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("pose_tracking_demo"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - std::thread executor_thread([&executor]() { executor.spin(); }); - - auto servo_param_listener = std::make_unique(node, "moveit_servo"); - auto servo_parameters = servo_param_listener->get_params(); - - // Load the planning scene monitor - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; - planning_scene_monitor = std::make_shared(node, "robot_description"); - if (!planning_scene_monitor->getPlanningScene()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - planning_scene_monitor->providePlanningSceneService(); - planning_scene_monitor->startSceneMonitor(); - planning_scene_monitor->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor->startStateMonitor(servo_parameters.joint_topic); - planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); - - // Wait for Planning Scene Monitor to setup - if (!planning_scene_monitor->waitForCurrentRobotState(node->now(), 5.0 /* seconds */)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error waiting for current robot state in PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - // Create the pose tracker - moveit_servo::PoseTracking tracker(node, std::move(servo_param_listener), planning_scene_monitor); - - // Make a publisher for sending pose commands - auto target_pose_pub = - node->create_publisher("target_pose", rclcpp::SystemDefaultsQoS()); - - // Subscribe to servo status (and log it when it changes) - StatusMonitor status_monitor(node, servo_parameters.status_topic); - - Eigen::Vector3d lin_tol{ 0.001, 0.001, 0.001 }; - double rot_tol = 0.01; - - // Get the current EE transform - geometry_msgs::msg::TransformStamped current_ee_tf; - tracker.getCommandFrameTransform(current_ee_tf); - - // Convert it to a Pose - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = current_ee_tf.header.frame_id; - target_pose.pose.position.x = current_ee_tf.transform.translation.x; - target_pose.pose.position.y = current_ee_tf.transform.translation.y; - target_pose.pose.position.z = current_ee_tf.transform.translation.z; - target_pose.pose.orientation = current_ee_tf.transform.rotation; - - // Modify it a little bit - target_pose.pose.position.x += 0.1; - - // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple - // waypoints - tracker.resetTargetPose(); - - // Publish target pose - target_pose.header.stamp = node->now(); - target_pose_pub->publish(target_pose); - - // Run the pose tracking in a new thread - std::thread move_to_pose_thread([&tracker, &lin_tol, &rot_tol] { - moveit_servo::PoseTrackingStatusCode tracking_status = - tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); - RCLCPP_INFO_STREAM(LOGGER, "Pose tracker exited with status: " - << moveit_servo::POSE_TRACKING_STATUS_CODE_MAP.at(tracking_status)); - }); - - rclcpp::WallRate loop_rate(50); - for (size_t i = 0; i < 500; ++i) - { - // Modify the pose target a little bit each cycle - // This is a dynamic pose target - target_pose.pose.position.z += 0.0004; - target_pose.header.stamp = node->now(); - target_pose_pub->publish(target_pose); - - loop_rate.sleep(); - } - - // Make sure the tracker is stopped and clean up - move_to_pose_thread.join(); - - // Kill executor thread before shutdown - executor.cancel(); - executor_thread.join(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; -} diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp deleted file mode 100644 index 359f202523..0000000000 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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. - *********************************************************************/ - -#include - -#include -using namespace std::literals; - -namespace -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_tracking"); -constexpr size_t LOG_THROTTLE_PERIOD = 10; // sec - -// Helper template for declaring and getting ros param -template -void declareOrGetParam(T& output_value, const std::string& param_name, const rclcpp::Node::SharedPtr& node, - const rclcpp::Logger& logger, const T default_value = T{}) -{ - try - { - if (node->has_parameter(param_name)) - { - node->get_parameter(param_name, output_value); - } - else - { - output_value = node->declare_parameter(param_name, default_value); - } - } - catch (const rclcpp::exceptions::InvalidParameterTypeException& e) - { - RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what()); - RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file"); - throw e; - } - - RCLCPP_INFO_STREAM(logger, "Found parameter - " << param_name << ": " << output_value); -} -} // namespace - -namespace moveit_servo -{ -PoseTracking::PoseTracking(const rclcpp::Node::SharedPtr& node, - std::unique_ptr servo_param_listener, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : node_(node) - , servo_parameters_(servo_param_listener->get_params()) - , planning_scene_monitor_(planning_scene_monitor) - , loop_rate_(1.0 / servo_parameters_.publish_period) - , transform_buffer_(node_->get_clock()) - , transform_listener_(transform_buffer_) - , stop_requested_(false) -{ - readROSParams(); - robot_model_ = planning_scene_monitor_->getRobotModel(); - - // Initialize PID controllers - initializePID(x_pid_config_, cartesian_position_pids_); - initializePID(y_pid_config_, cartesian_position_pids_); - initializePID(z_pid_config_, cartesian_position_pids_); - initializePID(angular_pid_config_, cartesian_orientation_pids_); - - // Connect to Servo ROS interfaces - target_pose_sub_ = node_->create_subscription( - "target_pose", rclcpp::SystemDefaultsQoS(), - [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { return targetPoseCallback(msg); }); - - // Publish outgoing twist commands to the Servo object - twist_stamped_pub_ = node_->create_publisher( - servo_parameters_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS()); - - // Use the C++ interface that Servo provides - servo_ = std::make_unique(node_, planning_scene_monitor_, std::move(servo_param_listener)); - servo_->start(); -} - -PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positional_tolerance, - const double angular_tolerance, const double target_pose_timeout) -{ - // Reset stop requested flag before starting motions - stop_requested_ = false; - // Wait a bit for a target pose message to arrive. - // The target pose may get updated by new messages as the robot moves (in a callback function). - const rclcpp::Time start_time = node_->now(); - - while ((!haveRecentTargetPose(target_pose_timeout) || !haveRecentEndEffectorPose(target_pose_timeout)) && - ((node_->now() - start_time).seconds() < target_pose_timeout)) - { - if (servo_->getCommandFrameTransform(command_frame_transform_)) - { - command_frame_transform_stamp_ = node_->now(); - } - std::this_thread::sleep_for(1ms); - } - - if (!haveRecentTargetPose(target_pose_timeout)) - { - RCLCPP_ERROR_STREAM(LOGGER, "The target pose was not updated recently. Aborting."); - return PoseTrackingStatusCode::NO_RECENT_TARGET_POSE; - } - - // Continue sending PID controller output to Servo until one of the following conditions is met: - // - Goal tolerance is satisfied - // - Target pose becomes outdated - // - Command frame transform becomes outdated - // - Another thread requested a stop - while (rclcpp::ok()) - { - if (satisfiesPoseTolerance(positional_tolerance, angular_tolerance)) - { - RCLCPP_INFO_STREAM(LOGGER, "The target pose is achieved!"); - break; - } - // Attempt to update robot pose - if (servo_->getCommandFrameTransform(command_frame_transform_)) - { - command_frame_transform_stamp_ = node_->now(); - } - - // Check that end-effector pose (command frame transform) is recent enough. - if (!haveRecentEndEffectorPose(target_pose_timeout)) - { - RCLCPP_ERROR_STREAM(LOGGER, "The end effector pose was not updated in time. Aborting."); - doPostMotionReset(); - return PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE; - } - - if (stop_requested_) - { - RCLCPP_INFO_STREAM(LOGGER, "Halting servo motion, a stop was requested."); - doPostMotionReset(); - return PoseTrackingStatusCode::STOP_REQUESTED; - } - - // Compute servo command from PID controller output and send it to the Servo object, for execution - twist_stamped_pub_->publish(*calculateTwistCommand()); - - if (!loop_rate_.sleep()) - { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), LOG_THROTTLE_PERIOD, "Target control rate was missed"); -#pragma GCC diagnostic pop - } - } - - doPostMotionReset(); - return PoseTrackingStatusCode::SUCCESS; -} - -void PoseTracking::readROSParams() -{ - planning_frame_ = servo_parameters_.planning_frame; - move_group_name_ = servo_parameters_.move_group_name; - - if (!planning_scene_monitor_->getRobotModel()->hasJointModelGroup(move_group_name_)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to find the specified joint model group: " << move_group_name_); - } - - x_pid_config_.dt = servo_parameters_.publish_period; - y_pid_config_.dt = servo_parameters_.publish_period; - z_pid_config_.dt = servo_parameters_.publish_period; - angular_pid_config_.dt = servo_parameters_.publish_period; - - x_pid_config_.windup_limit = servo_parameters_.windup_limit; - y_pid_config_.windup_limit = servo_parameters_.windup_limit; - z_pid_config_.windup_limit = servo_parameters_.windup_limit; - angular_pid_config_.windup_limit = servo_parameters_.windup_limit; - - x_pid_config_.k_p = servo_parameters_.x_proportional_gain; - x_pid_config_.k_i = servo_parameters_.x_integral_gain; - x_pid_config_.k_d = servo_parameters_.x_derivative_gain; - - y_pid_config_.k_p = servo_parameters_.y_proportional_gain; - y_pid_config_.k_i = servo_parameters_.y_integral_gain; - y_pid_config_.k_d = servo_parameters_.y_derivative_gain; - - z_pid_config_.k_p = servo_parameters_.z_proportional_gain; - z_pid_config_.k_i = servo_parameters_.z_integral_gain; - z_pid_config_.k_d = servo_parameters_.z_derivative_gain; - - angular_pid_config_.k_p = servo_parameters_.angular_proportional_gain; - angular_pid_config_.k_i = servo_parameters_.angular_integral_gain; - angular_pid_config_.k_d = servo_parameters_.angular_derivative_gain; -} - -void PoseTracking::initializePID(const PIDConfig& pid_config, std::vector& pid_vector) -{ - bool use_anti_windup = true; - pid_vector.push_back(control_toolbox::Pid(pid_config.k_p, pid_config.k_i, pid_config.k_d, pid_config.windup_limit, - -pid_config.windup_limit, use_anti_windup)); -} - -bool PoseTracking::haveRecentTargetPose(const double timespan) -{ - std::lock_guard lock(target_pose_mtx_); - return ((node_->now() - target_pose_.header.stamp).seconds() < timespan); -} - -bool PoseTracking::haveRecentEndEffectorPose(const double timespan) -{ - return ((node_->now() - command_frame_transform_stamp_).seconds() < timespan); -} - -bool PoseTracking::satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance) -{ - std::lock_guard lock(target_pose_mtx_); - double x_error = target_pose_.pose.position.x - command_frame_transform_.translation()(0); - double y_error = target_pose_.pose.position.y - command_frame_transform_.translation()(1); - double z_error = target_pose_.pose.position.z - command_frame_transform_.translation()(2); - - // If uninitialized, likely haven't received the target pose yet. - if (!angular_error_) - return false; - - return ((std::abs(x_error) < positional_tolerance(0)) && (std::abs(y_error) < positional_tolerance(1)) && - (std::abs(z_error) < positional_tolerance(2)) && (std::abs(*angular_error_) < angular_tolerance)); -} - -void PoseTracking::targetPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) -{ - std::lock_guard lock(target_pose_mtx_); - target_pose_ = *msg; - // If the target pose is not defined in planning frame, transform the target pose. - if (target_pose_.header.frame_id != planning_frame_) - { - try - { - geometry_msgs::msg::TransformStamped target_to_planning_frame = transform_buffer_.lookupTransform( - planning_frame_, target_pose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(100ms)); - tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); - - // Prevent doTransform from copying a stamp of 0, which will cause the haveRecentTargetPose check to fail servo motions - target_pose_.header.stamp = node_->now(); - } - catch (const tf2::TransformException& ex) - { - RCLCPP_WARN_STREAM(LOGGER, ex.what()); - return; - } - } -} - -geometry_msgs::msg::TwistStamped::ConstSharedPtr PoseTracking::calculateTwistCommand() -{ - // use the shared pool to create a message more efficiently - auto msg = moveit::util::make_shared_from_pool(); - - // Get twist components from PID controllers - geometry_msgs::msg::Twist& twist = msg->twist; - Eigen::Quaterniond q_desired; - - // Scope mutex locking only to operations which require access to target pose. - { - std::lock_guard lock(target_pose_mtx_); - msg->header.frame_id = target_pose_.header.frame_id; - - // Position - twist.linear.x = cartesian_position_pids_[0].computeCommand( - target_pose_.pose.position.x - command_frame_transform_.translation()(0), loop_rate_.period().count()); - twist.linear.y = cartesian_position_pids_[1].computeCommand( - target_pose_.pose.position.y - command_frame_transform_.translation()(1), loop_rate_.period().count()); - twist.linear.z = cartesian_position_pids_[2].computeCommand( - target_pose_.pose.position.z - command_frame_transform_.translation()(2), loop_rate_.period().count()); - - // Orientation algorithm: - // - Find the orientation error as a quaternion: q_error = q_desired * q_current ^ -1 - // - Use the angle-axis PID controller to calculate an angular rate - // - Convert to angular velocity for the TwistStamped message - q_desired = Eigen::Quaterniond(target_pose_.pose.orientation.w, target_pose_.pose.orientation.x, - target_pose_.pose.orientation.y, target_pose_.pose.orientation.z); - } - - Eigen::Quaterniond q_current(command_frame_transform_.rotation()); - Eigen::Quaterniond q_error = q_desired * q_current.inverse(); - - // Convert axis-angle to angular velocity - Eigen::AngleAxisd axis_angle(q_error); - // Cache the angular error, for rotation tolerance checking - angular_error_ = axis_angle.angle(); - - double ang_vel_magnitude = - cartesian_orientation_pids_[0].computeCommand(*angular_error_, loop_rate_.period().count()); - twist.angular.x = ang_vel_magnitude * axis_angle.axis()[0]; - twist.angular.y = ang_vel_magnitude * axis_angle.axis()[1]; - twist.angular.z = ang_vel_magnitude * axis_angle.axis()[2]; - - msg->header.stamp = node_->now(); - - return msg; -} - -void PoseTracking::stopMotion() -{ - stop_requested_ = true; - - // Send a 0 command to Servo to halt arm motion - auto msg = moveit::util::make_shared_from_pool(); - { - std::lock_guard lock(target_pose_mtx_); - msg->header.frame_id = target_pose_.header.frame_id; - } - msg->header.stamp = node_->now(); - twist_stamped_pub_->publish(*msg); -} - -void PoseTracking::doPostMotionReset() -{ - stopMotion(); - stop_requested_ = false; - angular_error_ = {}; - - // Reset error integrals and previous errors of PID controllers - cartesian_position_pids_[0].reset(); - cartesian_position_pids_[1].reset(); - cartesian_position_pids_[2].reset(); - cartesian_orientation_pids_[0].reset(); -} - -void PoseTracking::updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, - const double x_derivative_gain, const double y_proportional_gain, - const double y_integral_gain, const double y_derivative_gain, - const double z_proportional_gain, const double z_integral_gain, - const double z_derivative_gain, const double angular_proportional_gain, - const double angular_integral_gain, const double angular_derivative_gain) -{ - stopMotion(); - - x_pid_config_.k_p = x_proportional_gain; - x_pid_config_.k_i = x_integral_gain; - x_pid_config_.k_d = x_derivative_gain; - y_pid_config_.k_p = y_proportional_gain; - y_pid_config_.k_i = y_integral_gain; - y_pid_config_.k_d = y_derivative_gain; - z_pid_config_.k_p = z_proportional_gain; - z_pid_config_.k_i = z_integral_gain; - z_pid_config_.k_d = z_derivative_gain; - - angular_pid_config_.k_p = angular_proportional_gain; - angular_pid_config_.k_i = angular_integral_gain; - angular_pid_config_.k_d = angular_derivative_gain; - - cartesian_position_pids_.clear(); - cartesian_orientation_pids_.clear(); - initializePID(x_pid_config_, cartesian_position_pids_); - initializePID(y_pid_config_, cartesian_position_pids_); - initializePID(z_pid_config_, cartesian_position_pids_); - initializePID(angular_pid_config_, cartesian_orientation_pids_); - - doPostMotionReset(); -} - -void PoseTracking::getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error) -{ - double dummy1, dummy2; - cartesian_position_pids_.at(0).getCurrentPIDErrors(x_error, dummy1, dummy2); - cartesian_position_pids_.at(1).getCurrentPIDErrors(y_error, dummy1, dummy2); - cartesian_position_pids_.at(2).getCurrentPIDErrors(z_error, dummy1, dummy2); - cartesian_orientation_pids_.at(0).getCurrentPIDErrors(orientation_error, dummy1, dummy2); -} - -void PoseTracking::resetTargetPose() -{ - std::lock_guard lock(target_pose_mtx_); - target_pose_ = geometry_msgs::msg::PoseStamped(); - target_pose_.header.stamp = rclcpp::Time(RCL_ROS_TIME); -} - -bool PoseTracking::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) -{ - return servo_->getCommandFrameTransform(transform); -} -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 7bbe6c55c3..d32c3d427b 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -33,74 +33,584 @@ /* Title : servo.cpp * Project : moveit_servo - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * Created : 05/17/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim */ -#include -#include +#include +#include +#include +#include + +// Disable -Wold-style-cast because all _THROTTLE macros trigger this +#pragma GCC diagnostic ignored "-Wold-style-cast" -namespace moveit_servo -{ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); -constexpr double ROBOT_STATE_WAIT_TIME = 10.0; // seconds +constexpr double ROBOT_STATE_WAIT_TIME = 5.0; // seconds +constexpr double STOPPED_VELOCITY_EPS = 1e-4; } // namespace -Servo::Servo(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener) - : servo_param_listener_(servo_param_listener) - , servo_params_{ servo_param_listener_->get_params() } - , planning_scene_monitor_{ planning_scene_monitor } - , servo_calcs_{ node, planning_scene_monitor_, servo_param_listener_ } - , collision_checker_{ node, planning_scene_monitor_, servo_param_listener_ } +namespace moveit_servo { -} -void Servo::start() +Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : node_(node) + , servo_param_listener_{ std::move(servo_param_listener) } + , planning_scene_monitor_{ planning_scene_monitor } { + servo_params_ = servo_param_listener_->get_params(); + + if (!validateParams(servo_params_)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Got invalid parameters, exiting."); + std::exit(EXIT_FAILURE); + } + if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(servo_params_.move_group_name, ROBOT_STATE_WAIT_TIME)) { RCLCPP_ERROR(LOGGER, "Timeout waiting for current state"); - return; + std::exit(EXIT_FAILURE); } - servo_params_ = servo_param_listener_->get_params(); + // Planning scene monitor is passed in. + if (servo_params_.is_primary_planning_scene_monitor) + { + planning_scene_monitor_->providePlanningSceneService(); + } + else + { + planning_scene_monitor_->requestPlanningSceneState(); + } + + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + // Check if the transforms to planning frame and end effector frame exist. + if (!robot_state->knowsFrameTransform(servo_params_.planning_frame)) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for planning frame " << servo_params_.planning_frame); + } + else if (!robot_state->knowsFrameTransform(servo_params_.ee_frame)) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for end effector frame " << servo_params_.ee_frame); + } + else + { + // Load the smoothing plugin + if (servo_params_.use_smoothing) + { + setSmoothingPlugin(); + } + else + { + RCLCPP_WARN(LOGGER, "No smoothing plugin loaded"); + } + + // Create the collision checker and start collision checking. + collision_monitor_ = + std::make_unique(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_)); + collision_monitor_->start(); + + servo_status_ = StatusCode::NO_WARNING; + } + + const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel() + ->getJointModelGroup(servo_params_.move_group_name) + ->getActiveJointModelNames(); + // Create subgroup map + for (const auto& sub_group_name : planning_scene_monitor_->getRobotModel()->getJointModelGroupNames()) + { + // Skip move group + if (sub_group_name == servo_params_.move_group_name) + { + continue; + } + const auto& subgroup_joint_names = + planning_scene_monitor_->getRobotModel()->getJointModelGroup(sub_group_name)->getActiveJointModelNames(); + + JointNameToMoveGroupIndexMap new_map; + // For each joint name of the subgroup calculate the index in the move group joint vector + for (const auto& joint_name : subgroup_joint_names) + { + // Find subgroup joint name in move group joint names + const auto move_group_iterator = + std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name); + // Calculate position and add a new mapping of joint name to move group joint vector position + new_map.insert(std::make_pair( + std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator))); + } + // Add new joint name to index map to existing maps + joint_name_to_index_maps_.insert( + std::make_pair(std::string(sub_group_name), std::move(new_map))); + } + RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully"); +} + +Servo::~Servo() +{ + setCollisionChecking(false); +} + +void Servo::setSmoothingPlugin() +{ + // Load the smoothing plugin + try + { + pluginlib::ClassLoader smoothing_loader( + "moveit_core", "online_signal_smoothing::SmoothingBaseClass"); + smoother_ = smoothing_loader.createUniqueInstance(servo_params_.smoothing_filter_plugin_name); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", + servo_params_.smoothing_filter_plugin_name.c_str(), ex.what()); + std::exit(EXIT_FAILURE); + } + + // Initialize the smoothing plugin + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + const int num_joints = + robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size(); + if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints)) + { + RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); + std::exit(EXIT_FAILURE); + } + const KinematicState current_state = getCurrentRobotState(); + smoother_->reset(current_state.positions, current_state.velocities, current_state.accelerations); +} + +void Servo::setCollisionChecking(const bool check_collision) +{ + check_collision ? collision_monitor_->start() : collision_monitor_->stop(); +} + +bool Servo::validateParams(const servo::Params& servo_params) const +{ + bool params_valid = true; + auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); + if (joint_model_group == nullptr) + { + RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params.move_group_name << '`'); + params_valid = false; + } + + if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) + { + RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " + "should be greater than 'lower_singularity_threshold.' " + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && + !servo_params.publish_joint_accelerations) + { + RCLCPP_ERROR(LOGGER, "At least one of publish_joint_positions / " + "publish_joint_velocities / " + "publish_joint_accelerations must be true. " + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && + servo_params.publish_joint_velocities) + { + RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " + "you must select positions OR velocities." + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) + { + RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'." + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name && + !joint_model_group->isSubgroup(servo_params.active_subgroup)) + { + RCLCPP_ERROR(LOGGER, + "The value '%s' Parameter 'active_subgroup' does not name a valid subgroup of joint group '%s'.", + servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str()); + params_valid = false; + } + + return params_valid; +} + +bool Servo::updateParams() +{ + bool params_updated = false; + if (servo_param_listener_->is_old(servo_params_)) + { + const auto params = servo_param_listener_->get_params(); + + if (validateParams(params)) + { + if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor) + { + RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : " + << std::to_string(params.override_velocity_scaling_factor)); + } + + servo_params_ = params; + params_updated = true; + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Parameters will not be updated."); + } + } + return params_updated; +} + +servo::Params& Servo::getParams() +{ + return servo_params_; +} + +StatusCode Servo::getStatus() const +{ + return servo_status_; +} + +std::string Servo::getStatusMessage() const +{ + return SERVO_STATUS_CODE_MAP.at(servo_status_); +} - // Crunch the numbers in this timer - servo_calcs_.start(); +CommandType Servo::getCommandType() const +{ + return expected_command_type_; +} - // Check collisions in this timer - if (servo_params_.check_collisions) - collision_checker_.start(); +void Servo::setCommandType(const CommandType& command_type) +{ + expected_command_type_ = command_type; } -void Servo::stop() +Eigen::Isometry3d Servo::getEndEffectorPose() const { - servo_calcs_.stop(); - collision_checker_.stop(); + return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(servo_params_.ee_frame); } -bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) +KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + const KinematicState& target_state) const { - return servo_calcs_.getCommandFrameTransform(transform); + KinematicState bounded_state(target_state.joint_names.size()); + bounded_state.joint_names = target_state.joint_names; + + std::stringstream halting_joint_names; + for (const int idx : joints_to_halt) + { + halting_joint_names << bounded_state.joint_names[idx] + " "; + } + RCLCPP_WARN_STREAM(LOGGER, "Joint position limit reached on joints: " << halting_joint_names.str()); + + const bool all_joint_halt = + (getCommandType() == CommandType::JOINT_JOG && servo_params_.halt_all_joints_in_joint_mode) || + (getCommandType() == CommandType::TWIST && servo_params_.halt_all_joints_in_cartesian_mode); + + if (all_joint_halt) + { + // The velocities are initialized to zero by default, so we dont need to set it here. + bounded_state.positions = current_state.positions; + } + else + { + // Halt only the joints that are out of bounds + bounded_state.positions = target_state.positions; + bounded_state.velocities = target_state.velocities; + for (const int idx : joints_to_halt) + { + bounded_state.positions[idx] = current_state.positions[idx]; + bounded_state.velocities[idx] = 0.0; + } + } + + return bounded_state; } -bool Servo::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) +Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state) { - return servo_calcs_.getCommandFrameTransform(transform); + // Determine joint_name_group_index_map, if no subgroup is active, the map is empty + const auto& joint_name_group_index_map = + (!servo_params_.active_subgroup.empty() && servo_params_.active_subgroup != servo_params_.move_group_name) ? + joint_name_to_index_maps_.at(servo_params_.active_subgroup) : + JointNameToMoveGroupIndexMap(); + + const int num_joints = + robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size(); + Eigen::VectorXd joint_position_deltas(num_joints); + joint_position_deltas.setZero(); + + JointDeltaResult delta_result; + + const CommandType expected_type = getCommandType(); + if (command.index() == static_cast(expected_type)) + { + if (expected_type == CommandType::JOINT_JOG) + { + delta_result = jointDeltaFromJointJog(std::get(command), robot_state, servo_params_, + joint_name_group_index_map); + servo_status_ = delta_result.first; + } + else if (expected_type == CommandType::TWIST) + { + try + { + const TwistCommand command_in_planning_frame = toPlanningFrame(std::get(command)); + delta_result = + jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map); + servo_status_ = delta_result.first; + } + catch (tf2::TransformException& ex) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "Could not transform twist to planning frame."); + } + } + else if (expected_type == CommandType::POSE) + { + try + { + const PoseCommand command_in_planning_frame = toPlanningFrame(std::get(command)); + delta_result = + jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map); + servo_status_ = delta_result.first; + } + catch (tf2::TransformException& ex) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "Could not transform pose to planning frame."); + } + } + + if (servo_status_ != StatusCode::INVALID) + { + joint_position_deltas = delta_result.second; + } + } + else + { + servo_status_ = StatusCode::INVALID; + RCLCPP_WARN_STREAM(LOGGER, "Incoming servo command type does not match known command types."); + } + + return joint_position_deltas; } -bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform) +KinematicState Servo::getNextJointState(const ServoInput& command) { - return servo_calcs_.getEEFrameTransform(transform); + // Set status to clear + servo_status_ = StatusCode::NO_WARNING; + + // Update the parameters + updateParams(); + + // Get the robot state and joint model group info. + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params_.move_group_name); + + // Get necessary information about joints + const std::vector joint_names = joint_model_group->getActiveJointModelNames(); + const moveit::core::JointBoundsVector joint_bounds = joint_model_group->getActiveJointModelsBounds(); + const int num_joints = joint_names.size(); + + // State variables + KinematicState current_state(num_joints), target_state(num_joints); + target_state.joint_names = joint_names; + + // Copy current kinematic data from RobotState. + robot_state->copyJointGroupPositions(joint_model_group, current_state.positions); + robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities); + + // Compute the change in joint position due to the incoming command + Eigen::VectorXd joint_position_delta = jointDeltaFromCommand(command, robot_state); + + if (collision_velocity_scale_ > 0 && collision_velocity_scale_ < 1) + { + servo_status_ = StatusCode::DECELERATE_FOR_COLLISION; + } + else if (collision_velocity_scale_ == 0) + { + servo_status_ = StatusCode::HALT_FOR_COLLISION; + } + + // Continue rest of the computations only if the command is valid + // The computations can be skipped also in case we are halting. + if (servo_status_ != StatusCode::INVALID && servo_status_ != StatusCode::HALT_FOR_COLLISION) + { + // Apply collision scaling to the joint position delta + joint_position_delta *= collision_velocity_scale_; + + // Compute the next joint positions based on the joint position deltas + target_state.positions = current_state.positions + joint_position_delta; + + // TODO : apply filtering to the velocity instead of position + // Apply smoothing to the positions if a smoother was provided. + // Update filter state and apply filtering in position domain + if (smoother_) + { + smoother_->doSmoothing(target_state.positions, target_state.velocities, target_state.accelerations); + } + + // Compute velocities based on smoothed joint positions + target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; + + // Scale down the velocity based on joint velocity limit or user defined scaling if applicable. + const double joint_limit_scale = jointLimitVelocityScalingFactor(target_state.velocities, joint_bounds, + servo_params_.override_velocity_scaling_factor); + if (joint_limit_scale < 1.0) // 1.0 means no scaling. + { + RCLCPP_DEBUG_STREAM(LOGGER, "Joint velocity limit scaling applied by a factor of " << joint_limit_scale); + } + + target_state.velocities *= joint_limit_scale; + + // Adjust joint position based on scaled down velocity + target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period); + + // Check if any joints are going past joint position limits + const std::vector joints_to_halt = + jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margin); + + // Apply halting if any joints need to be halted. + if (!joints_to_halt.empty()) + { + servo_status_ = StatusCode::JOINT_BOUND; + target_state = haltJoints(joints_to_halt, current_state, target_state); + } + } + + return target_state; +} + +Eigen::Isometry3d Servo::getPlanningToCommandFrameTransform(const std::string& command_frame) const +{ + const moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + if (robot_state->knowsFrameTransform(command_frame)) + { + return robot_state->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * + robot_state->getGlobalLinkTransform(command_frame); + } + else + { + return tf2::transformToEigen(planning_scene_monitor_->getTFClient()->lookupTransform( + servo_params_.planning_frame, command_frame, rclcpp::Time(0))); + } } -bool Servo::getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform) +TwistCommand Servo::toPlanningFrame(const TwistCommand& command) const { - return servo_calcs_.getEEFrameTransform(transform); + Eigen::VectorXd transformed_twist = command.velocities; + + if (command.frame_id != servo_params_.planning_frame) + { + // Look up the transform between the planning and command frames. + const auto planning_to_command_tf = getPlanningToCommandFrameTransform(command.frame_id); + + if (servo_params_.apply_twist_commands_about_ee_frame) + { + // If the twist command is applied about the end effector frame, simply apply the rotation of the transform. + const auto planning_to_command_rotation = planning_to_command_tf.linear(); + const Eigen::Vector3d translation_vector = + planning_to_command_rotation * + Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]); + const Eigen::Vector3d angular_vector = + planning_to_command_rotation * + Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]); + + // Update the values of the original command message to reflect the change in frame + transformed_twist.head<3>() = translation_vector; + transformed_twist.tail<3>() = angular_vector; + } + else + { + // If the twist command is applied about the planning frame, the spatial twist is calculated + // as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf. + // The above equation defines twist as [angular; linear], but in our convention it is + // [linear; angular] so the adjoint matrix is also reordered accordingly. + Eigen::MatrixXd adjoint(6, 6); + + const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation(); + const Eigen::Vector3d& translation = planning_to_command_tf.translation(); + + Eigen::Matrix3d skew_translation; + skew_translation.row(0) << 0, -translation(2), translation(1); + skew_translation.row(1) << translation(2), 0, -translation(0); + skew_translation.row(2) << -translation(1), translation(0), 0; + + adjoint.topLeftCorner(3, 3) = skew_translation * rotation; + adjoint.topRightCorner(3, 3) = rotation; + adjoint.bottomLeftCorner(3, 3) = rotation; + adjoint.bottomRightCorner(3, 3).setZero(); + + transformed_twist = adjoint * transformed_twist; + } + } + + return TwistCommand{ servo_params_.planning_frame, transformed_twist }; } + +PoseCommand Servo::toPlanningFrame(const PoseCommand& command) const +{ + return PoseCommand{ servo_params_.planning_frame, + getPlanningToCommandFrameTransform(command.frame_id) * command.pose }; +} + +KinematicState Servo::getCurrentRobotState() const +{ + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params_.move_group_name); + const auto joint_names = joint_model_group->getActiveJointModelNames(); + + KinematicState current_state(joint_names.size()); + current_state.joint_names = joint_names; + robot_state->copyJointGroupPositions(joint_model_group, current_state.positions); + robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities); + robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations); + + return current_state; +} + +std::pair Servo::smoothHalt(const KinematicState& halt_state) const +{ + bool stopped = false; + auto target_state = halt_state; + const KinematicState current_state = getCurrentRobotState(); + + const size_t num_joints = current_state.joint_names.size(); + for (size_t i = 0; i < num_joints; i++) + { + const double vel = (target_state.positions[i] - current_state.positions[i]) / servo_params_.publish_period; + target_state.velocities[i] = (vel > STOPPED_VELOCITY_EPS) ? vel : 0.0; + target_state.accelerations[i] = + (target_state.velocities[i] - current_state.velocities[i]) / servo_params_.publish_period; + } + + // If all velocities are near zero, robot has decelerated to a stop. + stopped = + (std::accumulate(target_state.velocities.begin(), target_state.velocities.end(), 0.0) <= STOPPED_VELOCITY_EPS); + + if (smoother_) + { + smoother_->reset(current_state.positions, current_state.velocities, current_state.accelerations); + smoother_->doSmoothing(target_state.positions, target_state.velocities, target_state.accelerations); + } + + return std::make_pair(stopped, target_state); +} + } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp deleted file mode 100644 index ef185c25d1..0000000000 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ /dev/null @@ -1,966 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 HOLDER 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. - *******************************************************************************/ - -/* Title : servo_calcs.cpp - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -// Disable -Wold-style-cast because all _THROTTLE macros trigger this -// It would be too noisy to disable on a per-callsite basis -#pragma GCC diagnostic ignored "-Wold-style-cast" - -using namespace std::chrono_literals; // for s, ms, etc. - -namespace moveit_servo -{ -namespace -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_calcs"); -constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); -static constexpr double STOPPED_VELOCITY_EPS = 1e-4; // rad/s - -// This value is used when configuring the main loop to use SCHED_FIFO scheduling -// We use a slightly lower priority than the ros2_control default in order to reduce jitter -// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html -int const THREAD_PRIORITY = 40; -} // namespace - -// Constructor for the class that handles servoing calculations -ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener) - : node_(node) - , servo_param_listener_(servo_param_listener) - , servo_params_(servo_param_listener_->get_params()) - , planning_scene_monitor_(planning_scene_monitor) - , stop_requested_(true) - , smoothing_loader_("moveit_core", "online_signal_smoothing::SmoothingBaseClass") - -{ - // MoveIt Setup - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - joint_model_group_ = current_state_->getJointModelGroup(servo_params_.move_group_name); - if (joint_model_group_ == nullptr) - { - RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params_.move_group_name << '`'); - throw std::runtime_error("Invalid move group name"); - } - - // Subscribe to command topics - twist_stamped_sub_ = node_->create_subscription( - servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(), - [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { return twistStampedCB(msg); }); - - joint_cmd_sub_ = node_->create_subscription( - servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(), - [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointCmdCB(msg); }); - - // Subscribe to the collision_check topic - collision_velocity_scale_sub_ = node_->create_subscription( - "~/collision_velocity_scale", rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64::ConstSharedPtr& msg) { return collisionVelocityScaleCB(msg); }); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") - { - trajectory_outgoing_cmd_pub_ = node_->create_publisher( - servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); - } - else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") - { - multiarray_outgoing_cmd_pub_ = node_->create_publisher( - servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); - } - - // Publish status - status_pub_ = node_->create_publisher(servo_params_.status_topic, rclcpp::SystemDefaultsQoS()); - - internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); - num_joints_ = internal_joint_state_.name.size(); - internal_joint_state_.position.resize(num_joints_); - internal_joint_state_.velocity.resize(num_joints_); - delta_theta_.setZero(num_joints_); - - for (std::size_t i = 0; i < num_joints_; ++i) - { - // A map for the indices of incoming joint commands - joint_state_name_map_[internal_joint_state_.name[i]] = i; - } - - // Load the smoothing plugin - try - { - smoother_ = smoothing_loader_.createUniqueInstance(servo_params_.smoothing_filter_plugin_name); - } - catch (pluginlib::PluginlibException& ex) - { - RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", - servo_params_.smoothing_filter_plugin_name.c_str(), ex.what()); - std::exit(EXIT_FAILURE); - } - - // Initialize the smoothing plugin - if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints_)) - { - RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); - std::exit(EXIT_FAILURE); - } - - // A matrix of all zeros is used to check whether matrices have been initialized - Eigen::Matrix3d empty_matrix; - empty_matrix.setZero(); - tf_moveit_to_ee_frame_ = empty_matrix; - tf_moveit_to_robot_cmd_frame_ = empty_matrix; - - // Get the IK solver for the group - ik_solver_ = joint_model_group_->getSolverInstance(); - if (!ik_solver_) - { - RCLCPP_WARN( - LOGGER, - "No kinematics solver instantiated for group '%s'. Will use inverse Jacobian for servo calculations instead.", - joint_model_group_->getName().c_str()); - } - else if (!ik_solver_->supportsGroup(joint_model_group_)) - { - ik_solver_ = nullptr; - RCLCPP_WARN(LOGGER, - "The loaded kinematics plugin does not support group '%s'. Will use inverse Jacobian for servo " - "calculations instead.", - joint_model_group_->getName().c_str()); - } -} - -ServoCalcs::~ServoCalcs() -{ - stop(); -} - -void ServoCalcs::start() -{ - // Stop the thread if we are currently running - stop(); - - // Set up the "last" published message, in case we need to send it first - auto initial_joint_trajectory = std::make_unique(); - initial_joint_trajectory->header.stamp = node_->now(); - initial_joint_trajectory->header.frame_id = servo_params_.planning_frame; - initial_joint_trajectory->joint_names = internal_joint_state_.name; - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(servo_params_.publish_period); - if (servo_params_.publish_joint_positions) - { - planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(joint_model_group_, - point.positions); - } - if (servo_params_.publish_joint_velocities) - { - std::vector velocity(num_joints_); - point.velocities = velocity; - } - if (servo_params_.publish_joint_accelerations) - { - // I do not know of a robot that takes acceleration commands. - // However, some controllers check that this data is non-empty. - // Send all zeros, for now. - point.accelerations.resize(num_joints_); - } - initial_joint_trajectory->points.push_back(point); - last_sent_command_ = std::move(initial_joint_trajectory); - - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->copyJointGroupPositions(joint_model_group_, internal_joint_state_.position); - current_state_->copyJointGroupVelocities(joint_model_group_, internal_joint_state_.velocity); - // set previous state to same as internal state for t = 0 - previous_joint_state_ = internal_joint_state_; - - // Check that all links are known to the robot - auto check_link_is_known = [this](const std::string& frame_name) { - if (!current_state_->knowsFrameTransform(frame_name)) - { - throw std::runtime_error{ "Unknown frame: " + frame_name }; - } - }; - check_link_is_known(servo_params_.planning_frame); - check_link_is_known(servo_params_.ee_frame_name); - check_link_is_known(servo_params_.robot_link_command_frame); - - tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.ee_frame_name); - tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.robot_link_command_frame); - - // Always reset the low-pass filters when first starting servo - resetLowPassFilters(internal_joint_state_); - - stop_requested_ = false; - thread_ = std::thread([this] { - // Check if a realtime kernel is installed. Set a higher thread priority, if so - if (realtime_tools::has_realtime_kernel()) - { - if (!realtime_tools::configure_sched_fifo(THREAD_PRIORITY)) - { - RCLCPP_WARN(LOGGER, "Could not enable FIFO RT scheduling policy"); - } - } - else - { - RCLCPP_INFO(LOGGER, "RT kernel is recommended for better performance"); - } - mainCalcLoop(); - }); - new_input_cmd_ = false; -} - -void ServoCalcs::stop() -{ - // Request stop - stop_requested_ = true; - - // Notify condition variable in case the thread is blocked on it - { - // scope so the mutex is unlocked after so the thread can continue - // and therefore be joinable - const std::lock_guard lock(main_loop_mutex_); - new_input_cmd_ = false; - input_cv_.notify_all(); - } - - // Join the thread - if (thread_.joinable()) - { - thread_.join(); - } -} - -void ServoCalcs::updateParams() -{ - if (servo_param_listener_->is_old(servo_params_)) - { - auto params = servo_param_listener_->get_params(); - if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor) - { - RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : " - << std::to_string(params.override_velocity_scaling_factor)); - } - - if (params.robot_link_command_frame != servo_params_.robot_link_command_frame) - { - if (current_state_->knowsFrameTransform(params.robot_link_command_frame)) - { - RCLCPP_INFO_STREAM(LOGGER, "robot_link_command_frame changed to : " << params.robot_link_command_frame); - } - else - { - RCLCPP_ERROR_STREAM(LOGGER, "Failed to change robot_link_command_frame. Passed frame '" - << params.robot_link_command_frame - << "' is unknown, will keep using old command frame."); - // Replace frame in new param set with old frame value - // TODO : Is there a better behaviour here ? - params.robot_link_command_frame = servo_params_.robot_link_command_frame; - } - } - servo_params_ = params; - } -} - -void ServoCalcs::mainCalcLoop() -{ - rclcpp::WallRate rate(1.0 / servo_params_.publish_period); - - while (rclcpp::ok() && !stop_requested_) - { - // lock the input state mutex - std::unique_lock main_loop_lock(main_loop_mutex_); - - // Check if any parameters changed - if (servo_params_.enable_parameter_update) - { - updateParams(); - } - - // low latency mode -- begin calculations as soon as a new command is received. - if (servo_params_.low_latency_mode) - { - input_cv_.wait(main_loop_lock, [this] { return (new_input_cmd_ || stop_requested_); }); - } - - // reset new_input_cmd_ flag - new_input_cmd_ = false; - - // run servo calcs - const auto start_time = node_->now(); - calculateSingleIteration(); - const auto run_duration = node_->now() - start_time; - - // Log warning when the run duration was longer than the period - if (run_duration.seconds() > servo_params_.publish_period) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "run_duration: " << run_duration.seconds() << " (" << servo_params_.publish_period - << ')'); - } - - // normal mode, unlock input mutex and wait for the period of the loop - if (!servo_params_.low_latency_mode) - { - main_loop_lock.unlock(); - rate.sleep(); - } - } -} - -void ServoCalcs::calculateSingleIteration() -{ - // Publish status each loop iteration - auto status_msg = std::make_unique(); - status_msg->data = static_cast(status_); - status_pub_->publish(std::move(status_msg)); - - // After we publish, status, reset it back to no warnings - status_ = StatusCode::NO_WARNING; - - // Always update the joints and end-effector transform for 2 reasons: - // 1) in case the getCommandFrameTransform() method is being used - // 2) so the low-pass filters are up to date and don't cause a jump - // Get the latest joint group positions - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->copyJointGroupPositions(joint_model_group_, internal_joint_state_.position); - current_state_->copyJointGroupVelocities(joint_model_group_, internal_joint_state_.velocity); - - if (latest_twist_stamped_) - twist_stamped_cmd_ = *latest_twist_stamped_; - if (latest_joint_cmd_) - joint_servo_cmd_ = *latest_joint_cmd_; - - // Check for stale cmds - twist_command_is_stale_ = ((node_->now() - latest_twist_command_stamp_) >= - rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout)); - joint_command_is_stale_ = ((node_->now() - latest_joint_command_stamp_) >= - rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout)); - - // Get the transform from MoveIt planning frame to servoing command frame - // Calculate this transform to ensure it is available via C++ API - // We solve (planning_frame -> base -> robot_link_command_frame) - // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.robot_link_command_frame); - - // Calculate the transform from MoveIt planning frame to End Effector frame - // Calculate this transform to ensure it is available via C++ API - tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.ee_frame_name); - - // Don't end this function without updating the filters - updated_filters_ = false; - - // If waiting for initial servo commands, just keep the low-pass filters up to date with current - // joints so a jump doesn't occur when restarting - if (wait_for_servo_commands_) - { - resetLowPassFilters(internal_joint_state_); - - // Check if there are any new commands with valid timestamp - wait_for_servo_commands_ = - twist_stamped_cmd_.header.stamp == rclcpp::Time(0.) && joint_servo_cmd_.header.stamp == rclcpp::Time(0.); - - // Early exit - return; - } - - // If not waiting for initial command, - // Do servoing calculations only if the robot should move, for efficiency - // Create new outgoing joint trajectory command message - auto joint_trajectory = std::make_unique(); - - // Prioritize cartesian servoing above joint servoing - // Only run commands if not stale - if (!twist_command_is_stale_) - { - if (!cartesianServoCalcs(twist_stamped_cmd_, *joint_trajectory)) - { - resetLowPassFilters(internal_joint_state_); - return; - } - } - else if (!joint_command_is_stale_) - { - if (!jointServoCalcs(joint_servo_cmd_, *joint_trajectory)) - { - resetLowPassFilters(internal_joint_state_); - return; - } - } - - // Skip servoing publication if both types of commands are stale. - if (twist_command_is_stale_ && joint_command_is_stale_) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Skipping publishing because incoming commands are stale."); - filteredHalt(*joint_trajectory); - } - - // Clear out position commands if user did not request them (can cause interpolation issues) - if (!servo_params_.publish_joint_positions) - { - joint_trajectory->points[0].positions.clear(); - } - // Likewise for velocity and acceleration - if (!servo_params_.publish_joint_velocities) - { - joint_trajectory->points[0].velocities.clear(); - } - if (!servo_params_.publish_joint_accelerations) - { - joint_trajectory->points[0].accelerations.clear(); - } - - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") - { - // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" - // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement - joint_trajectory->header.stamp = rclcpp::Time(0); - *last_sent_command_ = *joint_trajectory; - trajectory_outgoing_cmd_pub_->publish(std::move(joint_trajectory)); - } - else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") - { - auto joints = std::make_unique(); - if (servo_params_.publish_joint_positions && !joint_trajectory->points.empty()) - { - joints->data = joint_trajectory->points[0].positions; - } - else if (servo_params_.publish_joint_velocities && !joint_trajectory->points.empty()) - { - joints->data = joint_trajectory->points[0].velocities; - } - *last_sent_command_ = *joint_trajectory; - multiarray_outgoing_cmd_pub_->publish(std::move(joints)); - } - - // Update the filters if we haven't yet - if (!updated_filters_) - resetLowPassFilters(internal_joint_state_); -} - -// Perform the servoing calculations -bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // Check for nan's in the incoming command - if (!checkValidCommand(cmd)) - return false; - - // Transform the command to the MoveGroup planning frame - if (cmd.header.frame_id.empty()) - { - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "No frame specified for command, will use planning_frame: " - << servo_params_.planning_frame); - cmd.header.frame_id = servo_params_.planning_frame; - } - if (cmd.header.frame_id != servo_params_.planning_frame) - { - transformTwistToPlanningFrame(cmd, servo_params_.planning_frame, current_state_); - } - - Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); - - Eigen::MatrixXd jacobian = current_state_->getJacobian(joint_model_group_); - - Eigen::JacobiSVD svd = - Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); - Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); - - // Convert from cartesian commands to joint commands - // Use an IK solver plugin if we have one, otherwise use inverse Jacobian. - if (ik_solver_) - { - const Eigen::Isometry3d base_to_tip_frame_transform = - current_state_->getGlobalLinkTransform(ik_solver_->getBaseFrame()).inverse() * - current_state_->getGlobalLinkTransform(ik_solver_->getTipFrame()); - - geometry_msgs::msg::Pose next_pose = poseFromCartesianDelta(delta_x, base_to_tip_frame_transform); - - // setup for IK call - std::vector solution(num_joints_); - moveit_msgs::msg::MoveItErrorCodes err; - kinematics::KinematicsQueryOptions opts; - opts.return_approximate_solution = true; - if (ik_solver_->searchPositionIK(next_pose, internal_joint_state_.position, servo_params_.publish_period / 2.0, - solution, err, opts)) - { - // find the difference in joint positions that will get us to the desired pose - for (size_t i = 0; i < num_joints_; ++i) - { - delta_theta_.coeffRef(i) = solution.at(i) - internal_joint_state_.position.at(i); - } - } - else - { - RCLCPP_WARN(LOGGER, "Could not find IK solution for requested motion, got error code %d", err.val); - return false; - } - } - else - { - // no supported IK plugin, use inverse Jacobian - delta_theta_ = pseudo_inverse * delta_x; - } - - const StatusCode last_status = status_; - delta_theta_ *= velocityScalingFactorForSingularity(joint_model_group_, delta_x, svd, pseudo_inverse, - servo_params_.hard_stop_singularity_threshold, - servo_params_.lower_singularity_threshold, - servo_params_.leaving_singularity_threshold_multiplier, - current_state_, status_); - // Status will have changed if approaching singularity - if (last_status != status_) - { - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status_)); - } - - return internalServoUpdate(delta_theta_, joint_trajectory, ServoType::CARTESIAN_SPACE); -} - -bool ServoCalcs::jointServoCalcs(const control_msgs::msg::JointJog& cmd, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // Check for nan's - if (!checkValidCommand(cmd)) - return false; - - // Apply user-defined scaling - delta_theta_ = scaleJointCommand(cmd); - - // Perform internal servo with the command - return internalServoUpdate(delta_theta_, joint_trajectory, ServoType::JOINT_SPACE); -} - -bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const ServoType servo_type) -{ - // The order of operations here is: - // 1. apply velocity scaling for collisions (in the position domain) - // 2. low-pass filter the position command in applyJointUpdate() - // 3. calculate velocities in applyJointUpdate() - // 4. apply velocity limits - // 5. apply position limits. This is a higher priority than velocity limits, so check it last. - - // Apply collision scaling - double collision_scale = collision_velocity_scale_; - if (collision_scale > 0 && collision_scale < 1) - { - status_ = StatusCode::DECELERATE_FOR_COLLISION; - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status_)); - } - else if (collision_scale == 0) - { - status_ = StatusCode::HALT_FOR_COLLISION; - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "Halting for collision!"); - } - delta_theta *= collision_scale; - - // Loop through joints and update them, calculate velocities, and filter - if (!applyJointUpdate(servo_params_.publish_period, delta_theta, previous_joint_state_, internal_joint_state_, - smoother_)) - { - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Lengths of output and increments do not match."); - return false; - } - - // Mark the lowpass filters as updated for this cycle - updated_filters_ = true; - - // Enforce SRDF velocity limits - enforceVelocityLimits(joint_model_group_, servo_params_.publish_period, internal_joint_state_, - servo_params_.override_velocity_scaling_factor); - - // Enforce SRDF position limits, might halt if needed, set prev_vel to 0 - const auto joints_to_halt = - enforcePositionLimits(internal_joint_state_, servo_params_.joint_limit_margin, joint_model_group_); - - if (!joints_to_halt.empty()) - { - std::ostringstream joint_names; - std::transform(joints_to_halt.cbegin(), joints_to_halt.cend(), std::ostream_iterator(joint_names, ""), - [](const auto& joint) { return " '" + joint->getName() + "'"; }); - - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joints" << joint_names.str() << " close to a position limit. Halting."); - - status_ = StatusCode::JOINT_BOUND; - if ((servo_type == ServoType::JOINT_SPACE && !servo_params_.halt_all_joints_in_joint_mode) || - (servo_type == ServoType::CARTESIAN_SPACE && !servo_params_.halt_all_joints_in_cartesian_mode)) - { - suddenHalt(internal_joint_state_, joints_to_halt); - } - else - { - suddenHalt(internal_joint_state_, joint_model_group_->getActiveJointModels()); - } - } - - // compose outgoing message - composeJointTrajMessage(internal_joint_state_, joint_trajectory); - - previous_joint_state_ = internal_joint_state_; - return true; -} - -void ServoCalcs::resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state) -{ - smoother_->reset(joint_state.position); - updated_filters_ = true; -} - -void ServoCalcs::composeJointTrajMessage(const sensor_msgs::msg::JointState& joint_state, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" - // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement - joint_trajectory.header.stamp = rclcpp::Time(0); - joint_trajectory.header.frame_id = servo_params_.planning_frame; - joint_trajectory.joint_names = joint_state.name; - - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(servo_params_.publish_period); - if (servo_params_.publish_joint_positions) - point.positions = joint_state.position; - if (servo_params_.publish_joint_velocities) - point.velocities = joint_state.velocity; - if (servo_params_.publish_joint_accelerations) - { - // I do not know of a robot that takes acceleration commands. - // However, some controllers check that this data is non-empty. - // Send all zeros, for now. - std::vector acceleration(num_joints_); - point.accelerations = acceleration; - } - joint_trajectory.points.push_back(point); -} - -void ServoCalcs::filteredHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // Prepare the joint trajectory message to stop the robot - joint_trajectory.points.clear(); - joint_trajectory.points.emplace_back(); - joint_trajectory.joint_names = joint_model_group_->getActiveJointModelNames(); - - // Deceleration algorithm: - // Set positions to internal_joint_state_ - // Filter - // Calculate velocities - // Check if velocities are close to zero. Round to zero, if so. - assert(internal_joint_state_.position.size() >= num_joints_); - joint_trajectory.points[0].positions = internal_joint_state_.position; - smoother_->doSmoothing(joint_trajectory.points[0].positions); - bool done_stopping = true; - if (servo_params_.publish_joint_velocities) - { - joint_trajectory.points[0].velocities = std::vector(num_joints_, 0); - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_trajectory.points[0].velocities.at(i) = - (joint_trajectory.points[0].positions.at(i) - internal_joint_state_.position.at(i)) / - servo_params_.publish_period; - // If velocity is very close to zero, round to zero - if (joint_trajectory.points[0].velocities.at(i) > STOPPED_VELOCITY_EPS) - { - done_stopping = false; - } - } - // If every joint is very close to stopped, round velocity to zero - if (done_stopping) - { - std::fill(joint_trajectory.points[0].velocities.begin(), joint_trajectory.points[0].velocities.end(), 0); - } - } - - if (servo_params_.publish_joint_accelerations) - { - joint_trajectory.points[0].accelerations = std::vector(num_joints_, 0); - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_trajectory.points[0].accelerations.at(i) = - (joint_trajectory.points[0].velocities.at(i) - internal_joint_state_.velocity.at(i)) / - servo_params_.publish_period; - } - } - - joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds(servo_params_.publish_period); -} - -void ServoCalcs::suddenHalt(sensor_msgs::msg::JointState& joint_state, - const std::vector& joints_to_halt) const -{ - // Set the position to the original position, and velocity to 0 for input joints - for (const auto& joint_to_halt : joints_to_halt) - { - const auto joint_it = std::find(joint_state.name.cbegin(), joint_state.name.cend(), joint_to_halt->getName()); - if (joint_it != joint_state.name.cend()) - { - const auto joint_index = std::distance(joint_state.name.cbegin(), joint_it); - joint_state.position.at(joint_index) = previous_joint_state_.position.at(joint_index); - joint_state.velocity.at(joint_index) = 0.0; - } - } -} - -bool ServoCalcs::checkValidCommand(const control_msgs::msg::JointJog& cmd) -{ - for (double velocity : cmd.velocities) - { - if (std::isnan(velocity)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "nan in incoming command. Skipping this datapoint."); - return false; - } - } - return true; -} - -bool ServoCalcs::checkValidCommand(const geometry_msgs::msg::TwistStamped& cmd) -{ - if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || - std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "nan in incoming command. Skipping this datapoint."); - return false; - } - - // If incoming commands should be in the range [-1:1], check for |delta|>1 - if (servo_params_.command_in_type == "unitless") - { - if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || - (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Component of incoming command is >1. Skipping this datapoint."); - return false; - } - } - - // Check that the command frame is known - if (!cmd.header.frame_id.empty() && !current_state_->knowsFrameTransform(cmd.header.frame_id)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Commanded frame '" << cmd.header.frame_id << "' is unknown, skipping this command"); - return false; - } - - return true; -} - -// Scale the incoming jog command. Returns a vector of position deltas -Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::msg::TwistStamped& command) -{ - Eigen::VectorXd result(6); - result.setZero(); // Or the else case below leads to misery - - // Apply user-defined scaling if inputs are unitless [-1:1] - if (servo_params_.command_in_type == "unitless") - { - result[0] = servo_params_.scale.linear * servo_params_.publish_period * command.twist.linear.x; - result[1] = servo_params_.scale.linear * servo_params_.publish_period * command.twist.linear.y; - result[2] = servo_params_.scale.linear * servo_params_.publish_period * command.twist.linear.z; - result[3] = servo_params_.scale.rotational * servo_params_.publish_period * command.twist.angular.x; - result[4] = servo_params_.scale.rotational * servo_params_.publish_period * command.twist.angular.y; - result[5] = servo_params_.scale.rotational * servo_params_.publish_period * command.twist.angular.z; - } - // Otherwise, commands are in m/s and rad/s - else if (servo_params_.command_in_type == "speed_units") - { - result[0] = command.twist.linear.x * servo_params_.publish_period; - result[1] = command.twist.linear.y * servo_params_.publish_period; - result[2] = command.twist.linear.z * servo_params_.publish_period; - result[3] = command.twist.angular.x * servo_params_.publish_period; - result[4] = command.twist.angular.y * servo_params_.publish_period; - result[5] = command.twist.angular.z * servo_params_.publish_period; - } - else - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "Unexpected command_in_type"); - } - - return result; -} - -Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::msg::JointJog& command) -{ - Eigen::VectorXd result(num_joints_); - result.setZero(); - - std::size_t c; - for (std::size_t m = 0; m < command.joint_names.size(); ++m) - { - try - { - c = joint_state_name_map_.at(command.joint_names[m]); - } - catch (const std::out_of_range& e) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "Ignoring joint " << command.joint_names[m]); - continue; - } - // Apply user-defined scaling if inputs are unitless [-1:1] - if (servo_params_.command_in_type == "unitless") - { - result[c] = command.velocities[m] * servo_params_.scale.joint * servo_params_.publish_period; - // Otherwise, commands are in m/s and rad/s - } - else if (servo_params_.command_in_type == "speed_units") - { - result[c] = command.velocities[m] * servo_params_.publish_period; - } - else - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Unexpected command_in_type, check yaml file."); - } - } - - return result; -} - -bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - transform = tf_moveit_to_robot_cmd_frame_; - - // All zeros means the transform wasn't initialized, so return false - return !transform.matrix().isZero(0); -} - -bool ServoCalcs::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - // All zeros means the transform wasn't initialized, so return false - if (tf_moveit_to_robot_cmd_frame_.matrix().isZero(0)) - { - return false; - } - - transform = convertIsometryToTransform(tf_moveit_to_robot_cmd_frame_, servo_params_.planning_frame, - servo_params_.robot_link_command_frame); - return true; -} - -bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - transform = tf_moveit_to_ee_frame_; - - // All zeros means the transform wasn't initialized, so return false - return !transform.matrix().isZero(0); -} - -bool ServoCalcs::getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - // All zeros means the transform wasn't initialized, so return false - if (tf_moveit_to_ee_frame_.matrix().isZero(0)) - { - return false; - } - - transform = - convertIsometryToTransform(tf_moveit_to_ee_frame_, servo_params_.planning_frame, servo_params_.ee_frame_name); - return true; -} - -void ServoCalcs::twistStampedCB(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) -{ - const std::lock_guard lock(main_loop_mutex_); - latest_twist_stamped_ = msg; - - if (msg->header.stamp != rclcpp::Time(0.)) - latest_twist_command_stamp_ = msg->header.stamp; - - // notify that we have a new input - new_input_cmd_ = true; - input_cv_.notify_all(); -} - -void ServoCalcs::jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& msg) -{ - const std::lock_guard lock(main_loop_mutex_); - latest_joint_cmd_ = msg; - - if (msg->header.stamp != rclcpp::Time(0.)) - latest_joint_command_stamp_ = msg->header.stamp; - - // notify that we have a new input - new_input_cmd_ = true; - input_cv_.notify_all(); -} - -void ServoCalcs::collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) -{ - collision_velocity_scale_ = msg->data; -} - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 17d7c11a7e..02dd61bce9 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -31,20 +31,43 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : servo_node.cpp - * Project : moveit_servo - * Created : 12/31/2018 - * Author : Andy Zelenak +/* Title : servo_node.cpp + * Project : moveit_servo + * Created : 12/31/2018 + * Author : Andy Zelenak, V Mohammed Ibrahim + * */ -#include +#include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); +} // namespace namespace moveit_servo { + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ServoNode::get_node_base_interface() +{ + return node_->get_node_base_interface(); +} + +ServoNode::~ServoNode() +{ + stop_servo_ = true; + if (servo_loop_thread_.joinable()) + servo_loop_thread_.join(); +} + ServoNode::ServoNode(const rclcpp::NodeOptions& options) : node_{ std::make_shared("servo_node", options) } + , stop_servo_{ false } + , servo_paused_{ false } + , new_joint_jog_msg_{ false } + , new_twist_msg_{ false } + , new_pose_msg_{ false } { if (!options.use_intra_process_comms()) { @@ -53,111 +76,274 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) "in the launch file"); } - // Set up services for interacting with Servo - start_servo_service_ = node_->create_service( - "~/start_servo", - [this](const std::shared_ptr& request, - const std::shared_ptr& response) { return startCB(request, response); }); - stop_servo_service_ = node_->create_service( - "~/stop_servo", - [this](const std::shared_ptr& request, - const std::shared_ptr& response) { return stopCB(request, response); }); - - // Can set robot_description name from parameters - std::string robot_description_name = "robot_description"; - node_->get_parameter_or("robot_description_name", robot_description_name, robot_description_name); - - // Get the servo parameters - auto param_listener = std::make_unique(node_, "moveit_servo"); - auto servo_parameters = param_listener->get_params(); - // Validate the parameters first. - validateParams(servo_parameters); - - // Set up planning_scene_monitor - planning_scene_monitor_ = std::make_shared( - 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_->setPlanningScenePublishingFrequency(25); - planning_scene_monitor_->getStateMonitor()->enableCopyDynamics(true); - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - std::string(node_->get_fully_qualified_name()) + - "/publish_planning_scene"); - // If the planning scene monitor in servo is the primary one we provide /get_planning_scene service so RViz displays - // or secondary planning scene monitors can fetch the scene, otherwise we request the planning scene from the - // primary planning scene monitor (e.g. move_group) - if (servo_parameters.is_primary_planning_scene_monitor) - { - planning_scene_monitor_->providePlanningSceneService(); + // Check if a realtime kernel is available + if (realtime_tools::has_realtime_kernel()) + { + if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) + { + RCLCPP_INFO_STREAM(LOGGER, "Realtime kernel available, higher thread priority has been set."); + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Could not enable FIFO RT scheduling policy."); + } } else { - planning_scene_monitor_->requestPlanningSceneState(); + RCLCPP_WARN_STREAM(LOGGER, "Realtime kernel is recommended for better performance."); + } + + std::shared_ptr servo_param_listener = + std::make_shared(node_, "moveit_servo"); + + // Create Servo instance + planning_scene_monitor_ = createPlanningSceneMonitor(node_, servo_param_listener->get_params()); + servo_ = std::make_unique(node_, servo_param_listener, planning_scene_monitor_); + + servo_params_ = servo_->getParams(); + + // Create subscriber for jointjog + joint_jog_subscriber_ = node_->create_subscription( + servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointJogCallback(msg); }); + + // Create subscriber for twist + twist_subscriber_ = node_->create_subscription( + servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { return twistCallback(msg); }); + + // Create subscriber for pose + pose_subscriber_ = node_->create_subscription( + servo_params_.pose_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { return poseCallback(msg); }); + + if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") + { + trajectory_publisher_ = node_->create_publisher( + servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); + } + else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") + { + multi_array_publisher_ = node_->create_publisher(servo_params_.command_out_topic, + rclcpp::SystemDefaultsQoS()); } + // Create status publisher + status_publisher_ = + node_->create_publisher(servo_params_.status_topic, rclcpp::SystemDefaultsQoS()); + + // Create service to enable switching command type + switch_command_type_ = node_->create_service( + "~/switch_command_type", [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return switchCommandType(request, response); + }); + + // Create service to pause/unpause servoing + pause_servo_ = node_->create_service( + "~/pause_servo", [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return pauseServo(request, response); + }); - // Create Servo - servo_ = std::make_unique(node_, planning_scene_monitor_, std::move(param_listener)); + // Start the servoing loop + servo_loop_thread_ = std::thread(&ServoNode::servoLoop, this); } -void ServoNode::validateParams(const servo::Params& servo_params) +void ServoNode::pauseServo(const std::shared_ptr& request, + const std::shared_ptr& response) { - bool has_error = false; - if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) + servo_paused_ = request->data; + response->success = (servo_paused_ == request->data); + if (servo_paused_) { - RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " - "should be greater than 'lower_singularity_threshold.' " - "Check the parameters YAML file used to launch this node."); - has_error = true; + servo_->setCollisionChecking(false); + response->message = "Servoing disabled"; } - - if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && - !servo_params.publish_joint_accelerations) + else { - RCLCPP_ERROR(LOGGER, "At least one of publish_joint_positions / " - "publish_joint_velocities / " - "publish_joint_accelerations must be true. " - "Check the parameters YAML file used to launch this node."); - has_error = true; + servo_->setCollisionChecking(true); + response->message = "Servoing enabled"; } +} - if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && - servo_params.publish_joint_velocities) +void ServoNode::switchCommandType(const std::shared_ptr& request, + const std::shared_ptr& response) +{ + const bool is_valid = (request->command_type >= static_cast(CommandType::MIN)) && + (request->command_type <= static_cast(CommandType::MAX)); + if (is_valid) { - RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " - "you must select positions OR velocities." - "Check the parameters YAML file used to launch this node."); - has_error = true; + servo_->setCommandType(static_cast(request->command_type)); } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Unknown command type " << request->command_type << " requested"); + } + response->success = (request->command_type == static_cast(servo_->getCommandType())); +} - if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) +void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg) +{ + latest_joint_jog_ = *msg; + new_joint_jog_msg_ = true; +} + +void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) +{ + latest_twist_ = *msg; + new_twist_msg_ = true; +} + +void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) +{ + latest_pose_ = *msg; + new_pose_msg_ = true; +} + +std::optional ServoNode::processJointJogCommand() +{ + std::optional next_joint_state = std::nullopt; + // Reject any other command types that had arrived simultaneously. + new_twist_msg_ = new_pose_msg_ = false; + + const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >= + rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); + if (!command_stale) + { + JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities }; + next_joint_state = servo_->getNextJointState(command); + } + else { - RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " - "than or equal to 'scene_collision_proximity_threshold'." - "Check the parameters YAML file used to launch this node."); - has_error = true; + auto result = servo_->smoothHalt(last_commanded_state_); + new_joint_jog_msg_ = result.first; + if (new_joint_jog_msg_) + { + next_joint_state = result.second; + RCLCPP_DEBUG_STREAM(LOGGER, "Joint jog command timed out. Halting to a stop."); + } } - if (has_error) + return next_joint_state; +} + +std::optional ServoNode::processTwistCommand() +{ + std::optional next_joint_state = std::nullopt; + + // Mark latest twist command as processed. + // Reject any other command types that had arrived simultaneously. + new_joint_jog_msg_ = new_pose_msg_ = false; + + const bool command_stale = (node_->now() - latest_twist_.header.stamp) >= + rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); + if (!command_stale) { - throw std::runtime_error("Servo failed to initialize : Invalid parameter values"); + const Eigen::Vector velocities{ latest_twist_.twist.linear.x, latest_twist_.twist.linear.y, + latest_twist_.twist.linear.z, latest_twist_.twist.angular.x, + latest_twist_.twist.angular.y, latest_twist_.twist.angular.z }; + const TwistCommand command{ latest_twist_.header.frame_id, velocities }; + next_joint_state = servo_->getNextJointState(command); } + else + { + auto result = servo_->smoothHalt(last_commanded_state_); + new_twist_msg_ = result.first; + if (new_twist_msg_) + { + next_joint_state = result.second; + RCLCPP_DEBUG_STREAM(LOGGER, "Twist command timed out. Halting to a stop."); + } + } + + return next_joint_state; } -void ServoNode::startCB(const std::shared_ptr& /* unused */, - const std::shared_ptr& response) +std::optional ServoNode::processPoseCommand() { - servo_->start(); - response->success = true; + std::optional next_joint_state = std::nullopt; + + // Mark latest pose command as processed. + // Reject any other command types that had arrived simultaneously. + new_joint_jog_msg_ = new_twist_msg_ = false; + + const bool command_stale = (node_->now() - latest_pose_.header.stamp) >= + rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); + if (!command_stale) + { + const PoseCommand command = poseFromPoseStamped(latest_pose_); + next_joint_state = servo_->getNextJointState(command); + } + else + { + auto result = servo_->smoothHalt(last_commanded_state_); + new_pose_msg_ = result.first; + if (new_pose_msg_) + { + next_joint_state = result.second; + RCLCPP_DEBUG_STREAM(LOGGER, "Pose command timed out. Halting to a stop."); + } + } + + return next_joint_state; } -void ServoNode::stopCB(const std::shared_ptr& /* unused */, - const std::shared_ptr& response) +void ServoNode::servoLoop() { - servo_->stop(); - response->success = true; + moveit_msgs::msg::ServoStatus status_msg; + std::optional next_joint_state = std::nullopt; + rclcpp::WallRate servo_frequency(1 / servo_params_.publish_period); + + while (rclcpp::ok() && !stop_servo_) + { + // Skip processing if servoing is disabled. + if (servo_paused_) + continue; + + next_joint_state = std::nullopt; + const CommandType expected_type = servo_->getCommandType(); + + if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_) + { + next_joint_state = processJointJogCommand(); + } + else if (expected_type == CommandType::TWIST && new_twist_msg_) + { + next_joint_state = processTwistCommand(); + } + else if (expected_type == CommandType::POSE && new_pose_msg_) + { + next_joint_state = processPoseCommand(); + } + else if (new_joint_jog_msg_ || new_twist_msg_ || new_pose_msg_) + { + new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false; + RCLCPP_WARN_STREAM(LOGGER, "Command type has not been set, cannot accept input"); + } + + if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID) && + (servo_->getStatus() != StatusCode::HALT_FOR_COLLISION)) + { + if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") + { + trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state.value())); + } + else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") + { + multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value())); + } + last_commanded_state_ = next_joint_state.value(); + } + + status_msg.code = static_cast(servo_->getStatus()); + status_msg.message = servo_->getStatusMessage(); + status_publisher_->publish(status_msg); + + servo_frequency.sleep(); + } } + } // namespace moveit_servo -// Register the component with class_loader -#include +#include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode) diff --git a/moveit_ros/moveit_servo/src/servo_node_main.cpp b/moveit_ros/moveit_servo/src/servo_node_main.cpp deleted file mode 100644 index 81fd43227e..0000000000 --- a/moveit_ros/moveit_servo/src/servo_node_main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, PickNik Inc. - * 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 HOLDER 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. - *******************************************************************************/ - -/* Title : servo_node_main.cpp - * Project : moveit_servo - * Created : 08/18/2021 - * Author : Joe Schornak - */ - -#include - -int main(int argc, char* argv[]) -{ - rclcpp::init(argc, argv); - - rclcpp::NodeOptions options; - - auto servo_node = std::make_shared(options); - - rclcpp::spin(servo_node->get_node_base_interface()); - - rclcpp::shutdown(); -} diff --git a/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp b/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp deleted file mode 100644 index b28fabb94a..0000000000 --- a/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp +++ /dev/null @@ -1,279 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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. - *********************************************************************/ - -/* Title : joystick_servo_example.cpp - * Project : moveit_servo - * Created : 08/07/2020 - * Author : Adam Pettinger - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if RCLCPP_VERSION_GTE(20, 0, 0) -#include -#else -#include -#endif -#include -#include -#include -#include - -// We'll just set up parameters here -const std::string JOY_TOPIC = "/joy"; -const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; -const std::string EEF_FRAME_ID = "panda_hand"; -const std::string BASE_FRAME_ID = "panda_link0"; - -// Enums for button names -> axis/button array index -// For XBOX 1 controller -enum Axis -{ - LEFT_STICK_X = 0, - LEFT_STICK_Y = 1, - LEFT_TRIGGER = 2, - RIGHT_STICK_X = 3, - RIGHT_STICK_Y = 4, - RIGHT_TRIGGER = 5, - D_PAD_X = 6, - D_PAD_Y = 7 -}; -enum Button -{ - A = 0, - B = 1, - X = 2, - Y = 3, - LEFT_BUMPER = 4, - RIGHT_BUMPER = 5, - CHANGE_VIEW = 6, - MENU = 7, - HOME = 8, - LEFT_STICK_CLICK = 9, - RIGHT_STICK_CLICK = 10 -}; - -// Some axes have offsets (e.g. the default trigger position is 1.0 not 0) -// This will map the default values for the axes -const std::map AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } }; - -// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2 -// functions -/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message - * @param axes The vector of continuous controller joystick axes - * @param buttons The vector of discrete controller button values - * @param twist A TwistStamped message to update in prep for publishing - * @param joint A JointJog message to update in prep for publishing - * @return return true if you want to publish a Twist, false if you want to publish a JointJog - */ -bool convertJoyToCmd(const std::vector& axes, const std::vector& buttons, - std::unique_ptr& twist, - std::unique_ptr& joint) -{ - // Give joint jogging priority because it is only buttons - // If any joint jog command is requested, we are only publishing joint commands - if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y]) - { - // Map the D_PAD to the proximal joints - joint->joint_names.push_back("panda_joint1"); - joint->velocities.push_back(axes[D_PAD_X]); - joint->joint_names.push_back("panda_joint2"); - joint->velocities.push_back(axes[D_PAD_Y]); - - // Map the diamond to the distal joints - joint->joint_names.push_back("panda_joint7"); - joint->velocities.push_back(buttons[B] - buttons[X]); - joint->joint_names.push_back("panda_joint6"); - joint->velocities.push_back(buttons[Y] - buttons[A]); - return false; - } - - // The bread and butter: map buttons to twist commands - twist->twist.linear.z = axes[RIGHT_STICK_Y]; - twist->twist.linear.y = axes[RIGHT_STICK_X]; - - double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER)); - double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER)); - twist->twist.linear.x = lin_x_right + lin_x_left; - - twist->twist.angular.y = axes[LEFT_STICK_Y]; - twist->twist.angular.x = axes[LEFT_STICK_X]; - - double roll_positive = buttons[RIGHT_BUMPER]; - double roll_negative = -1 * (buttons[LEFT_BUMPER]); - twist->twist.angular.z = roll_positive + roll_negative; - - return true; -} - -/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller - * @param frame_name Set the command frame to this - * @param buttons The vector of discrete controller button values - */ -void updateCmdFrame(std::string& frame_name, const std::vector& buttons) -{ - if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID) - { - frame_name = BASE_FRAME_ID; - } - else if (buttons[MENU] && frame_name == BASE_FRAME_ID) - { - frame_name = EEF_FRAME_ID; - } -} - -namespace moveit_servo -{ -class JoyToServoPub : public rclcpp::Node -{ -public: - JoyToServoPub(const rclcpp::NodeOptions& options) - : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID) - { - // Setup pub/sub - joy_sub_ = create_subscription(JOY_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { - return joyCB(msg); - }); - - twist_pub_ = create_publisher(TWIST_TOPIC, rclcpp::SystemDefaultsQoS()); - joint_pub_ = create_publisher(JOINT_TOPIC, rclcpp::SystemDefaultsQoS()); - collision_pub_ = create_publisher("/planning_scene", rclcpp::SystemDefaultsQoS()); - - // Create a service client to start the ServoNode - servo_start_client_ = create_client("/servo_node/start_servo"); - servo_start_client_->wait_for_service(std::chrono::seconds(1)); - servo_start_client_->async_send_request(std::make_shared()); - - // Load the collision scene asynchronously - collision_pub_thread_ = std::thread([this]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - // Create collision object, in the way of servoing - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "box"; - - shape_msgs::msg::SolidPrimitive table_1; - table_1.type = table_1.BOX; - table_1.dimensions = { 0.4, 0.6, 0.03 }; - - geometry_msgs::msg::Pose table_1_pose; - table_1_pose.position.x = 0.6; - table_1_pose.position.y = 0.0; - table_1_pose.position.z = 0.4; - - shape_msgs::msg::SolidPrimitive table_2; - table_2.type = table_2.BOX; - table_2.dimensions = { 0.6, 0.4, 0.03 }; - - geometry_msgs::msg::Pose table_2_pose; - table_2_pose.position.x = 0.0; - table_2_pose.position.y = 0.5; - table_2_pose.position.z = 0.25; - - collision_object.primitives.push_back(table_1); - collision_object.primitive_poses.push_back(table_1_pose); - collision_object.primitives.push_back(table_2); - collision_object.primitive_poses.push_back(table_2_pose); - collision_object.operation = collision_object.ADD; - - moveit_msgs::msg::PlanningSceneWorld psw; - psw.collision_objects.push_back(collision_object); - - auto ps = std::make_unique(); - ps->world = psw; - ps->is_diff = true; - collision_pub_->publish(std::move(ps)); - }); - } - - ~JoyToServoPub() override - { - if (collision_pub_thread_.joinable()) - collision_pub_thread_.join(); - } - - void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg) - { - // Create the messages we might publish - auto twist_msg = std::make_unique(); - auto joint_msg = std::make_unique(); - - // This call updates the frame for twist commands - updateCmdFrame(frame_to_publish_, msg->buttons); - - // Convert the joystick message to Twist or JointJog and publish - if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg)) - { - // publish the TwistStamped - twist_msg->header.frame_id = frame_to_publish_; - twist_msg->header.stamp = now(); - twist_pub_->publish(std::move(twist_msg)); - } - else - { - // publish the JointJog - joint_msg->header.stamp = now(); - joint_msg->header.frame_id = "panda_link3"; - joint_pub_->publish(std::move(joint_msg)); - } - } - -private: - rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr joint_pub_; - rclcpp::Publisher::SharedPtr collision_pub_; - rclcpp::Client::SharedPtr servo_start_client_; - - std::string frame_to_publish_; - - std::thread collision_pub_thread_; -}; // class JoyToServoPub - -} // namespace moveit_servo - -// Register the component with class_loader -#include -RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::JoyToServoPub) diff --git a/moveit_ros/moveit_servo/src/utilities.cpp b/moveit_ros/moveit_servo/src/utilities.cpp deleted file mode 100644 index 9c95a9801e..0000000000 --- a/moveit_ros/moveit_servo/src/utilities.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright 2022 PickNik Inc. -// -// 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 PickNik Inc. 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 HOLDER 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 : Andy Zelenak - Desc : Free functions. We keep them in a separate translation unit to reduce .o filesize - Title : utilities.cpp - Project : moveit_servo -*/ - -#include - -// Disable -Wold-style-cast because all _THROTTLE macros trigger this -// It would be too noisy to disable on a per-callsite basis -#pragma GCC diagnostic ignored "-Wold-style-cast" - -namespace moveit_servo -{ - -/** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/ -geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, - const std::string& parent_frame, - const std::string& child_frame) -{ - geometry_msgs::msg::TransformStamped output = tf2::eigenToTransform(eigen_tf); - output.header.frame_id = parent_frame; - output.child_frame_id = child_frame; - - return output; -} - -double velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, - const Eigen::VectorXd& commanded_twist, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse, - const double hard_stop_singularity_threshold, - const double lower_singularity_threshold, - const double leaving_singularity_threshold_multiplier, - const moveit::core::RobotStatePtr& current_state, StatusCode& status) -{ - double velocity_scale = 1; - std::size_t num_dimensions = commanded_twist.size(); - - // Find the direction away from nearest singularity. - // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. - // The sign can flip at any time, so we have to do some extra checking. - // Look ahead to see if the Jacobian's condition will decrease. - Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); - - double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); - - // This singular vector tends to flip direction unpredictably. See R. Bro, - // "Resolving the Sign Ambiguity in the Singular Value Decomposition". - // Look ahead to see if the Jacobian's condition will decrease in this - // direction. Start with a scaled version of the singular vector - Eigen::VectorXd delta_x(num_dimensions); - double scale = 100; - delta_x = vector_toward_singularity / scale; - - // Calculate a small change in joints - Eigen::VectorXd new_theta; - current_state->copyJointGroupPositions(joint_model_group, new_theta); - new_theta += pseudo_inverse * delta_x; - current_state->setJointGroupPositions(joint_model_group, new_theta); - Eigen::MatrixXd new_jacobian = current_state->getJacobian(joint_model_group); - - Eigen::JacobiSVD new_svd(new_jacobian); - double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); - // If new_condition < ini_condition, the singular vector does point towards a - // singularity. Otherwise, flip its direction. - if (ini_condition >= new_condition) - { - vector_toward_singularity *= -1; - } - - // If this dot product is positive, we're moving toward singularity - double dot = vector_toward_singularity.dot(commanded_twist); - // see https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation of algorithm - double upper_threshold = dot > 0 ? hard_stop_singularity_threshold : - (hard_stop_singularity_threshold - lower_singularity_threshold) * - leaving_singularity_threshold_multiplier + - lower_singularity_threshold; - if ((ini_condition > lower_singularity_threshold) && (ini_condition < hard_stop_singularity_threshold)) - { - velocity_scale = - 1. - (ini_condition - lower_singularity_threshold) / (upper_threshold - lower_singularity_threshold); - status = - dot > 0 ? StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY : StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY; - } - - // Very close to singularity, so halt. - else if (ini_condition >= upper_threshold) - { - velocity_scale = 0; - status = StatusCode::HALT_FOR_SINGULARITY; - } - - return velocity_scale; -} - -bool applyJointUpdate(const double publish_period, const Eigen::ArrayXd& delta_theta, - const sensor_msgs::msg::JointState& previous_joint_state, - sensor_msgs::msg::JointState& current_joint_state, - pluginlib::UniquePtr& smoother) -{ - // All the sizes must match - if (current_joint_state.position.size() != static_cast(delta_theta.size()) || - current_joint_state.velocity.size() != current_joint_state.position.size()) - { - return false; - } - - for (std::size_t i = 0; i < current_joint_state.position.size(); ++i) - { - // Increment joint - current_joint_state.position[i] += delta_theta[i]; - } - - smoother->doSmoothing(current_joint_state.position); - - // Calculate joint velocities - for (std::size_t i = 0; i < current_joint_state.position.size(); ++i) - { - current_joint_state.velocity[i] = - (current_joint_state.position.at(i) - previous_joint_state.position.at(i)) / publish_period; - } - - return true; -} - -void transformTwistToPlanningFrame(geometry_msgs::msg::TwistStamped& cmd, const std::string& planning_frame, - const moveit::core::RobotStatePtr& current_state) -{ - // We solve (planning_frame -> base -> cmd.header.frame_id) - // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) - const Eigen::Isometry3d tf_moveit_to_incoming_cmd_frame = - current_state->getGlobalLinkTransform(planning_frame).inverse() * - current_state->getGlobalLinkTransform(cmd.header.frame_id); - - // Apply the transform to linear and angular velocities - // v' = R * v and w' = R * w - Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); - Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; - angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; - - // Update the values of the original command message to reflect the change in frame - cmd.header.frame_id = planning_frame; - cmd.twist.linear.x = translation_vector(0); - cmd.twist.linear.y = translation_vector(1); - cmd.twist.linear.z = translation_vector(2); - cmd.twist.angular.x = angular_vector(0); - cmd.twist.angular.y = angular_vector(1); - cmd.twist.angular.z = angular_vector(2); -} - -geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, - const Eigen::Isometry3d& base_to_tip_frame_transform) -{ - // get a transformation matrix with the desired position change & - // get a transformation matrix with desired orientation change - Eigen::Isometry3d tf_pos_delta(Eigen::Isometry3d::Identity()); - tf_pos_delta.translate(Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2])); - - Eigen::Isometry3d tf_rot_delta(Eigen::Isometry3d::Identity()); - Eigen::Quaterniond q = Eigen::AngleAxisd(delta_x[3], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(delta_x[4], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(delta_x[5], Eigen::Vector3d::UnitZ()); - tf_rot_delta.rotate(q); - - // Find the new tip link position without newly applied rotation - const Eigen::Isometry3d tf_no_new_rot = tf_pos_delta * base_to_tip_frame_transform; - - // we want the rotation to be applied in the requested reference frame, - // but we want the rotation to be about the EE point in space, not the origin. - // So, we need to translate to origin, rotate, then translate back - // Given T = transformation matrix from origin -> EE point in space (translation component of tf_no_new_rot) - // and T' as the opposite transformation, EE point in space -> origin (translation only) - // apply final transformation as T * R * T' * tf_no_new_rot - const Eigen::Matrix tf_translation = tf_no_new_rot.translation(); - Eigen::Isometry3d tf_neg_translation = Eigen::Isometry3d::Identity(); // T' - tf_neg_translation(0, 3) = -tf_translation(0, 0); - tf_neg_translation(1, 3) = -tf_translation(1, 0); - tf_neg_translation(2, 3) = -tf_translation(2, 0); - Eigen::Isometry3d tf_pos_translation = Eigen::Isometry3d::Identity(); // T - tf_pos_translation(0, 3) = tf_translation(0, 0); - tf_pos_translation(1, 3) = tf_translation(1, 0); - tf_pos_translation(2, 3) = tf_translation(2, 0); - - // T * R * T' * tf_no_new_rot - return tf2::toMsg(tf_pos_translation * tf_rot_delta * tf_neg_translation * tf_no_new_rot); -} - -double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model_group, const Eigen::VectorXd& velocity) -{ - std::size_t joint_delta_index{ 0 }; - double velocity_scaling_factor{ 1.0 }; - for (const moveit::core::JointModel* joint : joint_model_group->getActiveJointModels()) - { - const auto& bounds = joint->getVariableBounds(joint->getName()); - if (bounds.velocity_bounded_ && velocity(joint_delta_index) != 0.0) - { - const double unbounded_velocity = velocity(joint_delta_index); - // Clamp each joint velocity to a joint specific [min_velocity, max_velocity] range. - const auto bounded_velocity = std::min(std::max(unbounded_velocity, bounds.min_velocity_), bounds.max_velocity_); - velocity_scaling_factor = std::min(velocity_scaling_factor, bounded_velocity / unbounded_velocity); - } - ++joint_delta_index; - } - - return velocity_scaling_factor; -} - -void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period, - sensor_msgs::msg::JointState& joint_state, const double override_velocity_scaling_factor) -{ - // Get the velocity scaling factor - Eigen::VectorXd velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - double velocity_scaling_factor = override_velocity_scaling_factor; - // if the override velocity scaling factor is approximately zero then the user is not overriding the value. - if (override_velocity_scaling_factor < 0.01) - velocity_scaling_factor = getVelocityScalingFactor(joint_model_group, velocity); - - // Take a smaller step if the velocity scaling factor is less than 1 - if (velocity_scaling_factor < 1) - { - Eigen::VectorXd velocity_residuals = (1 - velocity_scaling_factor) * velocity; - Eigen::VectorXd positions = - Eigen::Map(joint_state.position.data(), joint_state.position.size()); - positions -= velocity_residuals * publish_period; - - velocity *= velocity_scaling_factor; - // Back to sensor_msgs type - joint_state.velocity = std::vector(velocity.data(), velocity.data() + velocity.size()); - joint_state.position = std::vector(positions.data(), positions.data() + positions.size()); - } -} - -std::vector -enforcePositionLimits(sensor_msgs::msg::JointState& joint_state, const double joint_limit_margin, - const moveit::core::JointModelGroup* joint_model_group) -{ - // Halt if we're past a joint margin and joint velocity is moving even farther past - double joint_angle = 0; - std::vector joints_to_halt; - for (auto joint : joint_model_group->getActiveJointModels()) - { - for (std::size_t c = 0; c < joint_state.name.size(); ++c) - { - // Use the most recent robot joint state - if (joint_state.name[c] == joint->getName()) - { - joint_angle = joint_state.position.at(c); - break; - } - } - - if (!joint->satisfiesPositionBounds(&joint_angle, -joint_limit_margin)) - { - const std::vector& limits = joint->getVariableBoundsMsg(); - - // Joint limits are not defined for some joints. Skip them. - if (!limits.empty()) - { - // Check if pending velocity command is moving in the right direction - auto joint_itr = std::find(joint_state.name.begin(), joint_state.name.end(), joint->getName()); - auto joint_idx = std::distance(joint_state.name.begin(), joint_itr); - - if ((joint_state.velocity.at(joint_idx) < 0 && (joint_angle < (limits[0].min_position + joint_limit_margin))) || - (joint_state.velocity.at(joint_idx) > 0 && (joint_angle > (limits[0].max_position - joint_limit_margin)))) - { - joints_to_halt.push_back(joint); - } - } - } - } - return joints_to_halt; -} - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp new file mode 100644 index 0000000000..9ddfbda8b0 --- /dev/null +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -0,0 +1,334 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : command.cpp + * Project : moveit_servo + * Created : 06/04/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + */ + +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor"); + +/** + * @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the + * whole move group is created and all entries zeroed. The elements of the subgroup deltas vector are copied into the + * correct element of the bigger move group delta vector. + * @param sub_group_deltas Set of command deltas for a subgroup of the move group actuated by servo + * @param robot_state Current robot state + * @param servo_params Servo params + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. + * @return Delta vector for the whole move group. The elements that don't belong to the actuated subgroup are zero. + */ +const Eigen::VectorXd createMoveGroupDelta(const Eigen::VectorXd& sub_group_deltas, + const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const moveit_servo::JointNameToMoveGroupIndexMap& joint_name_group_index_map) +{ + const auto& subgroup_joint_names = + robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); + + // Create + Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero( + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size()); + for (size_t index = 0; index < subgroup_joint_names.size(); index++) + { + move_group_delta_theta[joint_name_group_index_map.at(subgroup_joint_names.at(index))] = sub_group_deltas[index]; + } + return move_group_delta_theta; +}; +} // namespace + +namespace moveit_servo +{ + +JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) +{ + // Find the target joint position based on the commanded joint velocity + StatusCode status = StatusCode::NO_WARNING; + const auto& group_name = + servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; + const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name); + const auto joint_names = joint_model_group->getActiveJointModelNames(); + Eigen::VectorXd joint_position_delta(joint_names.size()); + Eigen::VectorXd velocities(joint_names.size()); + + velocities.setZero(); + bool names_valid = true; + + for (size_t i = 0; i < command.names.size(); i++) + { + auto it = std::find(joint_names.begin(), joint_names.end(), command.names[i]); + if (it != std::end(joint_names)) + { + velocities[std::distance(joint_names.begin(), it)] = command.velocities[i]; + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint name: " << command.names[i]); + + names_valid = false; + break; + } + } + const bool velocity_valid = isValidCommand(velocities); + if (names_valid && velocity_valid) + { + joint_position_delta = velocities * servo_params.publish_period; + if (servo_params.command_in_type == "unitless") + { + joint_position_delta *= servo_params.scale.joint; + } + } + else + { + status = StatusCode::INVALID; + if (!names_valid) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command. Either you're sending commands for a joint " + "that is not part of the move group or certain joints cannot be moved because a " + "subgroup is active and they are not part of it."); + } + if (!velocity_valid) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid velocity values in joint jog command"); + } + } + + if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) + { + return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params, + joint_name_group_index_map)); + } + + return std::make_pair(status, joint_position_delta); +} + +JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) +{ + StatusCode status = StatusCode::NO_WARNING; + const int num_joints = + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size(); + Eigen::VectorXd joint_position_delta(num_joints); + Eigen::VectorXd cartesian_position_delta; + + const bool valid_command = isValidCommand(command); + const bool is_planning_frame = (command.frame_id == servo_params.planning_frame); + const bool is_zero = command.velocities.isZero(); + if (!is_zero && is_planning_frame && valid_command) + { + // Compute the Cartesian position delta based on incoming command, assumed to be in m/s + cartesian_position_delta = command.velocities * servo_params.publish_period; + // This scaling is supposed to be applied to the command. + // But since it is only used here, we avoid creating a copy of the command, + // by applying the scaling to the computed Cartesian delta instead. + if (servo_params.command_in_type == "unitless") + { + cartesian_position_delta.head<3>() *= servo_params.scale.linear; + cartesian_position_delta.tail<3>() *= servo_params.scale.rotational; + } + + // Compute the required change in joint angles. + const auto delta_result = + jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map); + status = delta_result.first; + if (status != StatusCode::INVALID) + { + joint_position_delta = delta_result.second; + // Get velocity scaling information for singularity. + const std::pair singularity_scaling_info = + velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params); + // Apply velocity scaling for singularity, if there was any scaling. + if (singularity_scaling_info.second != StatusCode::NO_WARNING) + { + status = singularity_scaling_info.second; + RCLCPP_WARN_STREAM(LOGGER, SERVO_STATUS_CODE_MAP.at(status)); + joint_position_delta *= singularity_scaling_info.first; + } + } + } + else if (is_zero) + { + joint_position_delta.setZero(); + } + else + { + status = StatusCode::INVALID; + if (!valid_command) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command."); + } + if (!is_planning_frame) + { + RCLCPP_WARN_STREAM(LOGGER, + "Command frame is: " << command.frame_id << ", expected: " << servo_params.planning_frame); + } + } + return std::make_pair(status, joint_position_delta); +} + +JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) +{ + StatusCode status = StatusCode::NO_WARNING; + const int num_joints = + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size(); + Eigen::VectorXd joint_position_delta(num_joints); + + const bool valid_command = isValidCommand(command); + const bool is_planning_frame = command.frame_id == servo_params.planning_frame; + + if (valid_command && is_planning_frame) + { + Eigen::Vector cartesian_position_delta; + + // Compute linear and angular change needed. + const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(servo_params.ee_frame) }; + const Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation()); + const Eigen::Quaterniond q_error = q_target * q_current.inverse(); + const Eigen::AngleAxisd angle_axis_error(q_error); + + cartesian_position_delta.head<3>() = command.pose.translation() - ee_pose.translation(); + cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle(); + + // Compute the required change in joint angles. + const auto delta_result = + jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map); + status = delta_result.first; + if (status != StatusCode::INVALID) + { + joint_position_delta = delta_result.second; + } + } + else + { + status = StatusCode::INVALID; + if (!valid_command) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command."); + } + if (!is_planning_frame) + { + RCLCPP_WARN_STREAM(LOGGER, + "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); + } + } + return std::make_pair(status, joint_position_delta); +} + +JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, + const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) +{ + const auto& group_name = + servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; + const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name); + + std::vector current_joint_positions; + robot_state->copyJointGroupPositions(joint_model_group, current_joint_positions); + + Eigen::VectorXd delta_theta(current_joint_positions.size()); + StatusCode status = StatusCode::NO_WARNING; + + const kinematics::KinematicsBaseConstPtr ik_solver = joint_model_group->getSolverInstance(); + bool ik_solver_supports_group = true; + if (ik_solver) + { + ik_solver_supports_group = ik_solver->supportsGroup(joint_model_group); + if (!ik_solver_supports_group) + { + status = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "Loaded IK plugin does not support group " << joint_model_group->getName()); + } + } + + if (ik_solver && ik_solver_supports_group) + { + const Eigen::Isometry3d base_to_tip_frame_transform = + robot_state->getGlobalLinkTransform(ik_solver->getBaseFrame()).inverse() * + robot_state->getGlobalLinkTransform(ik_solver->getTipFrame()); + + const geometry_msgs::msg::Pose next_pose = + poseFromCartesianDelta(cartesian_position_delta, base_to_tip_frame_transform); + + // setup for IK call + std::vector solution; + solution.reserve(current_joint_positions.size()); + moveit_msgs::msg::MoveItErrorCodes err; + kinematics::KinematicsQueryOptions opts; + opts.return_approximate_solution = true; + if (ik_solver->searchPositionIK(next_pose, current_joint_positions, servo_params.publish_period / 2.0, solution, + err, opts)) + { + // find the difference in joint positions that will get us to the desired pose + for (size_t i = 0; i < current_joint_positions.size(); ++i) + { + delta_theta[i] = solution.at(i) - current_joint_positions.at(i); + } + } + else + { + status = StatusCode::INVALID; + RCLCPP_WARN_STREAM(LOGGER, "Could not find IK solution for requested motion, got error code " << err.val); + } + } + else + { + // Robot does not have an IK solver, use inverse Jacobian to compute IK. + const Eigen::MatrixXd jacobian = robot_state->getJacobian(joint_model_group); + const Eigen::JacobiSVD svd = + Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); + const Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); + + delta_theta = pseudo_inverse * cartesian_position_delta; + } + + if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) + { + return std::make_pair(status, + createMoveGroupDelta(delta_theta, robot_state, servo_params, joint_name_group_index_map)); + } + + return std::make_pair(status, delta_theta); +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp new file mode 100644 index 0000000000..3936199653 --- /dev/null +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -0,0 +1,382 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : utils.cpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + */ + +#include + +namespace +{ +// The threshold above which `override_velocity_scaling_factor` will be used instead of computing the scaling from joint bounds. +const double SCALING_OVERRIDE_THRESHOLD = 0.01; +} // namespace + +namespace moveit_servo +{ + +bool isValidCommand(const Eigen::VectorXd& command) +{ + // returns true only if there are no nan values. + return command.allFinite(); +} + +bool isValidCommand(const Eigen::Isometry3d& command) +{ + Eigen::Matrix3d identity, rotation; + identity.setIdentity(); + rotation = command.linear(); + // checks rotation, will fail if there is nan + const bool is_valid_rotation = rotation.allFinite() && identity.isApprox(rotation.inverse() * rotation); + // Command is not vald if there is Nan + const bool is_valid_translation = isValidCommand(command.translation()); + return is_valid_rotation && is_valid_translation; +} + +bool isValidCommand(const TwistCommand& command) +{ + return !command.frame_id.empty() && isValidCommand(command.velocities); +} + +bool isValidCommand(const PoseCommand& command) +{ + return !command.frame_id.empty() && isValidCommand(command.pose); +} + +geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, + const Eigen::Isometry3d& base_to_tip_frame_transform) +{ + // Get a transformation matrix with the desired position change + Eigen::Isometry3d tf_pos_delta(Eigen::Isometry3d::Identity()); + tf_pos_delta.translate(Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2])); + + // Get a transformation matrix with desired orientation change + Eigen::Isometry3d tf_rot_delta(Eigen::Isometry3d::Identity()); + const Eigen::Quaterniond q = Eigen::AngleAxisd(delta_x[3], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(delta_x[4], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(delta_x[5], Eigen::Vector3d::UnitZ()); + tf_rot_delta.rotate(q); + + // Find the new tip link position without newly applied rotation + const Eigen::Isometry3d tf_no_new_rot = tf_pos_delta * base_to_tip_frame_transform; + + // we want the rotation to be applied in the requested reference frame, + // but we want the rotation to be about the EE point in space, not the origin. + // So, we need to translate to origin, rotate, then translate back + // Given T = transformation matrix from origin -> EE point in space (translation component of tf_no_new_rot) + // and T' as the opposite transformation, EE point in space -> origin (translation only) + // apply final transformation as T * R * T' * tf_no_new_rot + Eigen::Isometry3d tf_neg_translation = Eigen::Isometry3d::Identity(); // T' + tf_neg_translation.translation() = -1 * tf_no_new_rot.translation(); + Eigen::Isometry3d tf_pos_translation = Eigen::Isometry3d::Identity(); // T + tf_pos_translation.translation() = tf_no_new_rot.translation(); + + // T * R * T' * tf_no_new_rot + return tf2::toMsg(tf_pos_translation * tf_rot_delta * tf_neg_translation * tf_no_new_rot); +} + +trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Params& servo_params, + const KinematicState& joint_state) +{ + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + + trajectory_msgs::msg::JointTrajectory joint_trajectory; + joint_trajectory.header.stamp = rclcpp::Time(0); + joint_trajectory.header.frame_id = servo_params.planning_frame; + joint_trajectory.joint_names = joint_state.joint_names; + + static trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(servo_params.publish_period); + const size_t num_joints = joint_state.joint_names.size(); + if (point.positions.size() != num_joints) + { + point.positions.resize(num_joints); + point.velocities.resize(num_joints); + point.accelerations.resize(num_joints); + } + + // Set the fields of trajectory point based on which fields are requested. + // Some controllers check that acceleration data is non-empty, even if accelerations are not used + // Send all zeros (joint_state.accelerations is a vector of all zeros). + + if (servo_params.publish_joint_positions) + { + for (size_t i = 0; i < num_joints; ++i) + { + point.positions[i] = joint_state.positions[i]; + } + } + if (servo_params.publish_joint_velocities) + { + for (size_t i = 0; i < num_joints; ++i) + { + point.velocities[i] = joint_state.velocities[i]; + } + } + if (servo_params.publish_joint_accelerations) + { + for (size_t i = 0; i < num_joints; ++i) + { + point.accelerations[i] = joint_state.accelerations[i]; + } + } + + joint_trajectory.points.push_back(point); + return joint_trajectory; +} + +std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& servo_params, + const KinematicState& joint_state) +{ + std_msgs::msg::Float64MultiArray multi_array; + const size_t num_joints = joint_state.joint_names.size(); + multi_array.data.resize(num_joints); + if (servo_params.publish_joint_positions) + { + for (size_t i = 0; i < num_joints; ++i) + { + multi_array.data[i] = joint_state.positions[i]; + } + } + else if (servo_params.publish_joint_velocities) + { + for (size_t i = 0; i < num_joints; ++i) + { + multi_array.data[i] = joint_state.velocities[i]; + } + } + + return multi_array; +} + +std::pair velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr& robot_state, + const Eigen::VectorXd& target_delta_x, + const servo::Params& servo_params) +{ + // We need to send information back about if we are halting, moving away or towards the singularity. + StatusCode servo_status = StatusCode::NO_WARNING; + + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + // Get the thresholds. + const double lower_singularity_threshold = servo_params.lower_singularity_threshold; + const double hard_stop_singularity_threshold = servo_params.hard_stop_singularity_threshold; + const double leaving_singularity_threshold_multiplier = servo_params.leaving_singularity_threshold_multiplier; + + // Get size of total controllable dimensions. + const size_t dims = target_delta_x.size(); + + // Get the current Jacobian and compute SVD + const Eigen::JacobiSVD current_svd = Eigen::JacobiSVD( + robot_state->getJacobian(joint_model_group), Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::MatrixXd matrix_s = current_svd.singularValues().asDiagonal(); + + // Compute pseudo inverse + const Eigen::MatrixXd pseudo_inverse = current_svd.matrixV() * matrix_s.inverse() * current_svd.matrixU().transpose(); + + // Get the singular vector corresponding to least singular value. + // This vector represents the least responsive dimension. By convention this is the last column of the matrix U. + // The sign of the singular vector from result of SVD is not reliable, so we need to do extra checking to make sure of + // the sign. See R. Bro, "Resolving the Sign Ambiguity in the Singular Value Decomposition". + Eigen::VectorXd vector_towards_singularity = current_svd.matrixU().col(dims - 1); + + // Compute the current condition number. The ratio of max and min singular values. + // By convention these are the first and last element of the diagonal. + const double current_condition_number = current_svd.singularValues()(0) / current_svd.singularValues()(dims - 1); + + // Take a small step in the direction of vector_towards_singularity + const Eigen::VectorXd delta_x = vector_towards_singularity * servo_params.singularity_step_scale; + + // Compute the new joint angles if we take the small step delta_x + Eigen::VectorXd next_joint_angles; + robot_state->copyJointGroupPositions(joint_model_group, next_joint_angles); + next_joint_angles += pseudo_inverse * delta_x; + + // Compute the Jacobian SVD for the new robot state. + robot_state->setJointGroupPositions(joint_model_group, next_joint_angles); + const Eigen::JacobiSVD next_svd = Eigen::JacobiSVD( + robot_state->getJacobian(joint_model_group), Eigen::ComputeThinU | Eigen::ComputeThinV); + + // Compute condition number for the new Jacobian. + const double next_condition_number = next_svd.singularValues()(0) / next_svd.singularValues()(dims - 1); + + // If the condition number has increased, we are moving towards singularity and the direction of the + // vector_towards_singularity is correct. If the condition number has decreased, it means the sign of + // vector_towards_singularity needs to be flipped. + if (next_condition_number <= current_condition_number) + { + vector_towards_singularity *= -1; + } + + // Double check the direction using dot product. + const bool moving_towards_singularity = vector_towards_singularity.dot(target_delta_x) > 0; + + // Compute upper condition variable threshold based on if we are moving towards or away from singularity. + // See https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation. + double upper_threshold; + if (moving_towards_singularity) + { + upper_threshold = hard_stop_singularity_threshold; + } + else + { + const double threshold_size = (hard_stop_singularity_threshold - lower_singularity_threshold); + upper_threshold = lower_singularity_threshold + (threshold_size * leaving_singularity_threshold_multiplier); + } + + // Compute the scale based on the current condition number. + double velocity_scale = 1.0; + const bool is_above_lower_limit = current_condition_number > lower_singularity_threshold; + const bool is_below_hard_stop_limit = current_condition_number < hard_stop_singularity_threshold; + if (is_above_lower_limit && is_below_hard_stop_limit) + { + velocity_scale -= + (current_condition_number - lower_singularity_threshold) / (upper_threshold - lower_singularity_threshold); + + servo_status = moving_towards_singularity ? StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY : + StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY; + } + // If condition number has crossed hard stop limit, halt the robot. + else if (!is_below_hard_stop_limit) + { + servo_status = StatusCode::HALT_FOR_SINGULARITY; + velocity_scale = 0.0; + } + + return std::make_pair(velocity_scale, servo_status); +} + +double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double scaling_override) +{ + // If override value is close to zero, user is not overriding the scaling + if (scaling_override < SCALING_OVERRIDE_THRESHOLD) + { + scaling_override = 1.0; // Set to no scaling. + double bounded_vel; + std::vector velocity_scaling_factors; // The allowable fraction of computed veclocity + + for (size_t i = 0; i < joint_bounds.size(); i++) + { + const auto joint_bound = (joint_bounds[i])->front(); + if (joint_bound.velocity_bounded_ && velocities(i) != 0.0) + { + // Find the ratio of clamped velocity to original velocity + bounded_vel = std::clamp(velocities(i), joint_bound.min_velocity_, joint_bound.max_velocity_); + velocity_scaling_factors.push_back(bounded_vel / velocities(i)); + } + } + // Find the lowest scaling factor, this helps preserve Cartesian motion. + scaling_override = velocity_scaling_factors.empty() ? + scaling_override : + *std::min_element(velocity_scaling_factors.begin(), velocity_scaling_factors.end()); + } + + return scaling_override; +} + +std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double margin) +{ + std::vector joint_idxs_to_halt; + for (size_t i = 0; i < joint_bounds.size(); i++) + { + const auto joint_bound = (joint_bounds[i])->front(); + if (joint_bound.position_bounded_) + { + const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margin); + const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margin); + if (negative_bound || positive_bound) + { + joint_idxs_to_halt.push_back(i); + } + } + } + return joint_idxs_to_halt; +} + +/** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/ +geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame) +{ + geometry_msgs::msg::TransformStamped output = tf2::eigenToTransform(eigen_tf); + output.header.frame_id = parent_frame; + output.child_frame_id = child_frame; + return output; +} + +PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped& msg) +{ + PoseCommand command; + command.frame_id = msg.header.frame_id; + + const Eigen::Vector3d translation(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z); + const Eigen::Quaterniond rotation(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z); + + command.pose = Eigen::Isometry3d::Identity(); + command.pose.translate(translation); + command.pose.linear() = rotation.normalized().toRotationMatrix(); + + return command; +} + +planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, + const servo::Params& servo_params) +{ + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + // Can set robot_description name from parameters + std::string robot_description_name = "robot_description"; + node->get_parameter_or("robot_description_name", robot_description_name, robot_description_name); + // Set up planning_scene_monitor + planning_scene_monitor = std::make_shared(node, robot_description_name, + "planning_scene_monitor"); + + planning_scene_monitor->startStateMonitor(servo_params.joint_topic); + planning_scene_monitor->startSceneMonitor(servo_params.monitored_planning_scene_topic); + planning_scene_monitor->setPlanningScenePublishingFrequency(25); + planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); + planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + std::string(node->get_fully_qualified_name()) + + "/publish_planning_scene"); + + return planning_scene_monitor; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml b/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml deleted file mode 100644 index a0c8afdfe4..0000000000 --- a/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml +++ /dev/null @@ -1,16 +0,0 @@ -initial_positions: - panda_joint1: 0.0 - panda_joint2: -0.785 - panda_joint3: 0.0 - panda_joint4: -2.9 - panda_joint5: 0.0 - panda_joint6: 1.1 - panda_joint7: 0.785 -initial_velocities: - panda_joint1: 0.0 - panda_joint2: 0.0 - panda_joint3: 0.0 - panda_joint4: 0.0 - panda_joint5: 0.0 - panda_joint6: 0.0 - panda_joint7: 0.0 diff --git a/moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml b/moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml deleted file mode 100644 index 5c2a4889a2..0000000000 --- a/moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml +++ /dev/null @@ -1,22 +0,0 @@ -################################# -# PID parameters for pose seeking -################################# - - -# Maximum value of error integral for all PID controllers -windup_limit: 0.05 - -# PID gains -x_proportional_gain: 1.5 -y_proportional_gain: 1.5 -z_proportional_gain: 1.5 -x_integral_gain: 0.0 -y_integral_gain: 0.0 -z_integral_gain: 0.0 -x_derivative_gain: 0.0 -y_derivative_gain: 0.0 -z_derivative_gain: 0.0 - -angular_proportional_gain: 0.5 -angular_integral_gain: 0.0 -angular_derivative_gain: 0.0 diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml deleted file mode 100644 index 007aa8002f..0000000000 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ /dev/null @@ -1,58 +0,0 @@ -############################################### -# Modify all parameters related to servoing here -############################################### - -## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. - joint: 0.01 - -## Properties of outgoing commands -low_latency_mode: false # Set this to true to tie the output rate to the input rate -publish_period: 0.01 # 1/Nominal publish rate [seconds] - -# What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) -# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) -command_out_type: trajectory_msgs/JointTrajectory - -# What to publish? Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: false -publish_joint_accelerations: false - -## Plugins for smoothing outgoing commands -smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" - -## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' -planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' - -## Other frames -ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 1.0 # Stop servoing if X seconds elapse without a new command - -## Configure handling of singularities and joint limits -lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 90.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. - -## Topic names -cartesian_command_in_topic: servo_node/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: servo_node/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: joint_states -status_topic: servo_node/status # Publish status to this topic -command_out_topic: servo_node/command # Publish outgoing commands here - -## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. -self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] -scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py deleted file mode 100644 index 2a6d28a78e..0000000000 --- a/moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py +++ /dev/null @@ -1,27 +0,0 @@ -import launch_testing -import os -import sys -import unittest - -sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import generate_servo_test_description - - -def generate_test_description(): - return generate_servo_test_description( - gtest_name="test_servo_collision", - start_position_path="../config/collision_start_positions.yaml", - ) - - -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, servo_gtest): - self.proc_info.assertWaitForShutdown(servo_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, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py deleted file mode 100644 index 769c256f5b..0000000000 --- a/moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py +++ /dev/null @@ -1,24 +0,0 @@ -import launch_testing -import os -import sys -import unittest - -sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import generate_servo_test_description - - -def generate_test_description(): - return generate_servo_test_description(gtest_name="test_servo_integration") - - -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, servo_gtest): - self.proc_info.assertWaitForShutdown(servo_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, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp deleted file mode 100644 index 6651b5c93d..0000000000 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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: Andy Zelenak - Desc: Test of tracking toward a pose -*/ - -// C++ -#include -#include - -// ROS -#include - -// Testing -#include - -// Servo -#include -#include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_tracking_test"); - -static constexpr double TRANSLATION_TOLERANCE = 0.01; // meters -static constexpr double ROTATION_TOLERANCE = 0.1; // quaternion - -namespace moveit_servo -{ -class PoseTrackingFixture : public ::testing::Test -{ -public: - void SetUp() override - { - executor_->add_node(node_); - executor_thread_ = std::thread([this]() { this->executor_->spin(); }); - - auto servo_param_listener = std::make_unique(node_, "moveit_servo"); - servo_parameters_ = servo_param_listener->get_params(); - - // store test constants as shared pointer to constant struct - { - auto test_parameters = std::make_shared(); - test_parameters->publish_hz = 2.0 / servo_parameters_.incoming_command_timeout; - test_parameters->publish_period = 1.0 / test_parameters->publish_hz; - test_parameters->timeout_iterations = 10 * test_parameters->publish_hz; - test_parameters_ = test_parameters; - } - - // Load the planning scene monitor - if (!planning_scene_monitor_->getPlanningScene()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - planning_scene_monitor_->providePlanningSceneService(); - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor_->startStateMonitor(servo_parameters_.joint_topic); - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); - - // Wait for Planning Scene Monitor to setup - if (!planning_scene_monitor_->waitForCurrentRobotState(node_->now(), 5.0 /* seconds */)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error waiting for current robot state in PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - tracker_ = - std::make_shared(node_, std::move(servo_param_listener), planning_scene_monitor_); - - // Tolerance for pose seeking - translation_tolerance_ << TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE; - } - - PoseTrackingFixture() - : node_(std::make_shared("pose_tracking_testing")) - , executor_(std::make_shared()) - , planning_scene_monitor_( - std::make_shared(node_, "robot_description")) - { - // Init ROS interfaces - // Publishers - target_pose_pub_ = node_->create_publisher("target_pose", 1); - } - - void TearDown() override - { - executor_->cancel(); - if (executor_thread_.joinable()) - executor_thread_.join(); - } - -protected: - rclcpp::Node::SharedPtr node_; - rclcpp::Executor::SharedPtr executor_; - std::thread executor_thread_; - - servo::Params servo_parameters_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - struct TestParameters - { - double publish_hz; - double publish_period; - double timeout_iterations; - }; - std::shared_ptr test_parameters_; - - moveit_servo::PoseTrackingPtr tracker_; - - Eigen::Vector3d translation_tolerance_; - rclcpp::Publisher::SharedPtr target_pose_pub_; -}; // class PoseTrackingFixture - -// Check for commands going out to ros_control -TEST_F(PoseTrackingFixture, OutgoingMsgTest) -{ - // halt Servoing when first msg to ros_control is received - // and test some conditions - trajectory_msgs::msg::JointTrajectory last_received_msg; - std::function traj_callback = - [&/* this */](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { - EXPECT_EQ(msg->header.frame_id, "panda_link0"); - - // Exact joint positions may vary based on IK solver - // so check that the EE transform is as expected - moveit::core::RobotStatePtr temp_state(planning_scene_monitor_->getStateMonitor()->getCurrentState()); - const std::string group_name = "panda_arm"; - // copyJointGroupPositions can't take a const vector, make a copy - std::vector positions(msg->points[0].positions); - temp_state->copyJointGroupPositions(group_name, positions); - Eigen::Isometry3d hand_tf = temp_state->getFrameTransform("panda_hand"); - - moveit::core::RobotStatePtr test_state(planning_scene_monitor_->getStateMonitor()->getCurrentState()); - std::vector test_positions = { 0, -0.785, 0, -2.360, 0, 1.571, 0.758 }; - test_state->copyJointGroupPositions(group_name, test_positions); - Eigen::Isometry3d test_hand_tf = test_state->getFrameTransform("panda_hand"); - - double precision = 0.02; // Hilbert-Schmidt norm - EXPECT_TRUE(test_hand_tf.isApprox(hand_tf, precision)); - - this->tracker_->stopMotion(); - return; - }; - auto traj_sub = node_->create_subscription( - "/panda_arm_controller/joint_trajectory", 1, traj_callback); - - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = "panda_link4"; - target_pose.header.stamp = node_->now(); - target_pose.pose.position.x = 0.2; - target_pose.pose.position.y = 0.2; - target_pose.pose.position.z = 0.2; - target_pose.pose.orientation.w = 1; - - // Republish the target pose in a new thread, as if the target is moving - std::thread target_pub_thread([&] { - size_t msg_count = 0; - rclcpp::WallRate loop_rate(50); - while (++msg_count < 100) - { - target_pose_pub_->publish(target_pose); - loop_rate.sleep(); - } - }); - - // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple waypoints - tracker_->resetTargetPose(); - - tracker_->moveToPose(translation_tolerance_, ROTATION_TOLERANCE, 1 /* target pose timeout */); - - target_pub_thread.join(); -} - -} // namespace moveit_servo - -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/moveit_servo/test/publish_fake_jog_commands.cpp b/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp deleted file mode 100644 index b9a0c779c5..0000000000 --- a/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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 : Adam Pettinger - Desc : Simple node to publish commands to the Servo demo - Title : test_servo_parameters.cpp - Project : moveit_servo - Created : 07/16/2020 -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if RCLCPP_VERSION_GTE(20, 0, 0) -#include -#else -#include -#endif -#include -#include - -using namespace std::chrono_literals; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.publish_fake_jog_commands.cpp"); - - // ROS objects - rclcpp::NodeOptions node_options; - auto node = std::make_shared("publish_fake_jog_commands", node_options); - - // Call the start service to init and start the servo component - rclcpp::Client::SharedPtr servo_start_client = - node->create_client("/servo_node/start_servo"); - servo_start_client->wait_for_service(1s); - servo_start_client->async_send_request(std::make_shared()); - - // Create a publisher for publishing the jog commands - auto pub = node->create_publisher("/servo_node/delta_twist_cmds", 10); - std::weak_ptr::type> captured_pub = pub; - std::weak_ptr::type> captured_node = node; - auto callback = [captured_pub, captured_node]() -> void { - auto pub_ptr = captured_pub.lock(); - auto node_ptr = captured_node.lock(); - if (!pub_ptr || !node_ptr) - { - return; - } - auto msg = std::make_unique(); - msg->header.stamp = node_ptr->now(); - msg->header.frame_id = "panda_link0"; - msg->twist.linear.x = 0.3; - msg->twist.angular.z = 0.5; - pub_ptr->publish(std::move(msg)); - }; - auto timer = node->create_wall_timer(50ms, callback); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp b/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp deleted file mode 100644 index 5a8262ca35..0000000000 --- a/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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: Tyler Weaver, Andy Zelenak - Desc: Unit tests -*/ - -#include - -#include -#include -#include -#include - -namespace -{ -constexpr double PUBLISH_PERIOD = 0.01; - -void checkVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const Eigen::ArrayXd& velocity) -{ - std::size_t joint_index{ 0 }; - for (const moveit::core::JointModel* joint : joint_model_group->getActiveJointModels()) - { - const auto& bounds = joint->getVariableBounds(joint->getName()); - - if (bounds.velocity_bounded_) - { - EXPECT_GE(velocity(joint_index), bounds.min_velocity_) << "Joint " << joint_index << " violates velocity limit"; - EXPECT_LE(velocity(joint_index), bounds.max_velocity_) << "Joint " << joint_index << " violates velocity limit"; - } - ++joint_index; - } -} - -class ServoCalcsUnitTests : public testing::Test -{ -protected: - void SetUp() override - { - robot_model_ = moveit::core::loadTestingRobotModel("panda"); - joint_model_group_ = robot_model_->getJointModelGroup("panda_arm"); - } - - moveit::core::RobotModelPtr robot_model_; - const moveit::core::JointModelGroup* joint_model_group_; -}; - -} // namespace - -TEST_F(ServoCalcsUnitTests, VelocitiesTooFast) -{ - // Request velocities that are too fast - std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; - std::vector joint_velocity{ 0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; - sensor_msgs::msg::JointState joint_state; - joint_state.position = joint_position; - joint_state.velocity = joint_velocity; - - moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); - - Eigen::ArrayXd eigen_velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - checkVelocityLimits(joint_model_group_, eigen_velocity); -} - -TEST_F(ServoCalcsUnitTests, NegativeVelocitiesTooFast) -{ - // Negative velocities exceeding the limit - std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; - std::vector joint_velocity{ 0, -1.0, -2.0, -3.0, -4.0, -5.0, -6.0 }; - sensor_msgs::msg::JointState joint_state; - joint_state.position = joint_position; - joint_state.velocity = joint_velocity; - - moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); - - Eigen::ArrayXd eigen_velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - checkVelocityLimits(joint_model_group_, eigen_velocity); -} - -TEST_F(ServoCalcsUnitTests, AcceptableJointVelocities) -{ - // Final test with joint velocities that are acceptable - std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; - std::vector joint_velocity{ 0, 0.001, 0.001, -0.001, 0.001, 0.001, 0.001 }; - sensor_msgs::msg::JointState joint_state; - joint_state.position = joint_position; - joint_state.velocity = joint_velocity; - - moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); - - Eigen::ArrayXd eigen_velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - checkVelocityLimits(joint_model_group_, eigen_velocity); -} - -TEST_F(ServoCalcsUnitTests, SingularityScaling) -{ - // If we are at a singularity, we should halt - Eigen::VectorXd commanded_twist(6); - commanded_twist << 1, 0, 0, 0, 0, 0; - - // Start near a singularity - std::shared_ptr robot_state = std::make_shared(robot_model_); - robot_state->setToDefaultValues(); - robot_state->setVariablePosition("panda_joint1", 0.221); - robot_state->setVariablePosition("panda_joint2", 0.530); - robot_state->setVariablePosition("panda_joint3", -0.231); - robot_state->setVariablePosition("panda_joint4", -0.920); - robot_state->setVariablePosition("panda_joint5", 0.117); - robot_state->setVariablePosition("panda_joint6", 1.439); - robot_state->setVariablePosition("panda_joint7", -1.286); - - Eigen::MatrixXd jacobian = robot_state->getJacobian(joint_model_group_); - - Eigen::JacobiSVD svd = - Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); - Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); - - // Use very low thresholds to ensure they are triggered - double hard_stop_singularity_threshold = 2; - double lower_singularity_threshold = 1; - double leaving_singularity_threshold_multiplier = 2; - - rclcpp::Clock clock; - moveit_servo::StatusCode status; - - double scaling_factor = - moveit_servo::velocityScalingFactorForSingularity(joint_model_group_, commanded_twist, svd, pseudo_inverse, - hard_stop_singularity_threshold, lower_singularity_threshold, - leaving_singularity_threshold_multiplier, robot_state, status); - - EXPECT_EQ(scaling_factor, 0); -} - -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/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp deleted file mode 100644 index 86e260def9..0000000000 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ /dev/null @@ -1,426 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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. - *********************************************************************/ - -/* Title : servo_launch_test_common.hpp - * Project : moveit_servo - * Created : 08/03/2020 - * Author : Adam Pettinger - */ - -// C++ -#include - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Testing -#include - -// Servo -#include -// Auto-generated -#include - -#pragma once - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_launch_test_common.hpp"); - -namespace moveit_servo -{ -class ServoFixture : public ::testing::Test -{ -public: - void SetUp() override - { - executor_->add_node(node_); - executor_thread_ = std::thread([this]() { executor_->spin(); }); - } - - ServoFixture() - : node_(std::make_shared("servo_testing")) - , executor_(std::make_shared()) - { - auto servo_param_listener = std::make_unique(node_, "moveit_servo"); - servo_parameters_ = servo_param_listener->get_params(); - // store test constants as shared pointer to constant struct - { - auto test_parameters = std::make_shared(); - test_parameters->publish_hz = 2.0 / servo_parameters_.incoming_command_timeout; - test_parameters->publish_period = 1.0 / test_parameters->publish_hz; - test_parameters->timeout_iterations = 50 * test_parameters->publish_hz; - test_parameters->servo_node_name = "/servo_node"; - test_parameters_ = test_parameters; - } - - // Init ROS interfaces - // Publishers - pub_twist_cmd_ = node_->create_publisher( - resolveServoTopicName(servo_parameters_.cartesian_command_in_topic), rclcpp::SystemDefaultsQoS()); - pub_joint_cmd_ = node_->create_publisher( - resolveServoTopicName(servo_parameters_.joint_command_in_topic), rclcpp::SystemDefaultsQoS()); - } - - void TearDown() override - { - // If the stop client isn't null, we set it up and likely started the Servo. Stop it. - // Otherwise the Servo is still running when another test starts... - if (!client_servo_stop_) - { - stop(); - } - executor_->cancel(); - if (executor_thread_.joinable()) - executor_thread_.join(); - } - - std::string resolveServoTopicName(std::string topic_name) - { - if (topic_name.at(0) == '~') - { - return topic_name.replace(0, 1, test_parameters_->servo_node_name); - } - else - { - return topic_name; - } - } - - // Set up for callbacks (so they aren't run for EVERY test) - bool setupStartClient() - { - // Start client - client_servo_start_ = node_->create_client(resolveServoTopicName("~/start_servo")); - while (!client_servo_start_->service_is_ready()) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(LOGGER, "client_servo_start_ service not available, waiting again..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - } - - // If we setup the start client, also setup the stop client... - client_servo_stop_ = node_->create_client(resolveServoTopicName("~/stop_servo")); - while (!client_servo_stop_->service_is_ready()) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(LOGGER, "client_servo_stop_ service not available, waiting again..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - } - - // Status sub (we need this to check that we've started / stopped) - sub_servo_status_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.status_topic), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Int8::ConstSharedPtr& msg) { return statusCB(msg); }); - return true; - } - - bool setupCollisionScaleSub() - { - sub_collision_scale_ = node_->create_subscription( - resolveServoTopicName("~/collision_velocity_scale"), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64::ConstSharedPtr& msg) { return collisionScaleCB(msg); }); - return true; - } - - bool setupCommandSub(const std::string& command_type) - { - if (command_type == "trajectory_msgs/JointTrajectory") - { - sub_trajectory_cmd_output_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.command_out_topic), rclcpp::SystemDefaultsQoS(), - [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCommandCB(msg); }); - return true; - } - else if (command_type == "std_msgs/Float64MultiArray") - { - sub_array_cmd_output_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.command_out_topic), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64MultiArray::ConstSharedPtr& msg) { return arrayCommandCB(msg); }); - return true; - } - else - { - RCLCPP_ERROR(LOGGER, "Invalid command type for Servo output command"); - return false; - } - } - - bool setupJointStateSub() - { - sub_joint_state_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.joint_topic), rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { return jointStateCB(msg); }); - return true; - } - - void statusCB(const std_msgs::msg::Int8::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_status_; - latest_status_ = static_cast(msg->data); - if (latest_status_ == status_tracking_code_) - status_seen_ = true; - } - - void collisionScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_collision_scale_; - latest_collision_scale_ = msg->data; - } - - void jointStateCB(const sensor_msgs::msg::JointState::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_joint_state_; - latest_joint_state_ = msg; - } - - void trajectoryCommandCB(const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_commands_; - latest_traj_cmd_ = msg; - } - - void arrayCommandCB(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_commands_; - latest_array_cmd_ = msg; - } - - StatusCode getLatestStatus() - { - const std::lock_guard lock(latest_state_mutex_); - return latest_status_; - } - - size_t getNumStatus() - { - const std::lock_guard lock(latest_state_mutex_); - return num_status_; - } - - void resetNumStatus() - { - const std::lock_guard lock(latest_state_mutex_); - num_status_ = 0; - } - - double getLatestCollisionScale() - { - const std::lock_guard lock(latest_state_mutex_); - return latest_collision_scale_; - } - - size_t getNumCollisionScale() - { - const std::lock_guard lock(latest_state_mutex_); - return num_collision_scale_; - } - - void resetNumCollisionScale() - { - const std::lock_guard lock(latest_state_mutex_); - num_collision_scale_ = 0; - } - - sensor_msgs::msg::JointState getLatestJointState() - { - const std::lock_guard lock(latest_state_mutex_); - return *latest_joint_state_; - } - - size_t getNumJointState() - { - const std::lock_guard lock(latest_state_mutex_); - return num_joint_state_; - } - - void resetNumJointState() - { - const std::lock_guard lock(latest_state_mutex_); - num_joint_state_ = 0; - } - - trajectory_msgs::msg::JointTrajectory getLatestTrajCommand() - { - const std::lock_guard lock(latest_state_mutex_); - return *latest_traj_cmd_; - } - - std_msgs::msg::Float64MultiArray getLatestArrayCommand() - { - const std::lock_guard lock(latest_state_mutex_); - return *latest_array_cmd_; - } - - size_t getNumCommands() - { - const std::lock_guard lock(latest_state_mutex_); - return num_commands_; - } - - void resetNumCommands() - { - const std::lock_guard lock(latest_state_mutex_); - num_commands_ = 0; - } - - void watchForStatus(StatusCode code_to_watch_for) - { - const std::lock_guard lock(latest_state_mutex_); - status_seen_ = false; - status_tracking_code_ = code_to_watch_for; - } - - bool sawTrackedStatus() - { - const std::lock_guard lock(latest_state_mutex_); - return status_seen_; - } - - bool start() - { - auto time_start = node_->now(); - auto start_result = client_servo_start_->async_send_request(std::make_shared()); - if (!start_result.get()->success) - { - RCLCPP_ERROR(LOGGER, "Error returned form service call to start servo"); - return false; - } - RCLCPP_INFO_STREAM(LOGGER, "Wait for start servo: " << (node_->now() - time_start).seconds()); - - // Test that status messages start - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - time_start = node_->now(); - auto num_statuses_start = getNumStatus(); - size_t iterations = 0; - while (getNumStatus() == num_statuses_start && iterations++ < test_parameters_->timeout_iterations) - { - publish_loop_rate.sleep(); - } - - RCLCPP_INFO_STREAM(LOGGER, "Wait for status num increasing: " << (node_->now() - time_start).seconds()); - - if (iterations >= test_parameters_->timeout_iterations) - { - RCLCPP_ERROR(LOGGER, "Timeout waiting for status num increasing"); - return false; - } - - return getNumStatus() > num_statuses_start; - } - - bool stop() - { - auto time_start = node_->now(); - auto stop_result = client_servo_stop_->async_send_request(std::make_shared()); - if (!stop_result.get()->success) - { - RCLCPP_ERROR(LOGGER, "Error returned form service call to stop servo"); - return false; - } - return true; - } - -protected: - rclcpp::Node::SharedPtr node_; - rclcpp::Executor::SharedPtr executor_; - std::thread executor_thread_; - servo::Params servo_parameters_; - - struct TestParameters - { - double publish_hz; - double publish_period; - double timeout_iterations; - std::string servo_node_name; - }; - std::shared_ptr test_parameters_; - - // ROS interfaces - - // Service Clients - rclcpp::Client::SharedPtr client_servo_start_; - rclcpp::Client::SharedPtr client_servo_stop_; - - // Publishers - rclcpp::Publisher::SharedPtr pub_twist_cmd_; - rclcpp::Publisher::SharedPtr pub_joint_cmd_; - - // Subscribers - rclcpp::Subscription::SharedPtr sub_servo_status_; - rclcpp::Subscription::SharedPtr sub_collision_scale_; - rclcpp::Subscription::SharedPtr sub_joint_state_; - rclcpp::Subscription::SharedPtr sub_trajectory_cmd_output_; - rclcpp::Subscription::SharedPtr sub_array_cmd_output_; - - // Subscription counting and data - size_t num_status_; - StatusCode latest_status_ = StatusCode::NO_WARNING; - - size_t num_collision_scale_; - double latest_collision_scale_; - - size_t num_joint_state_; - sensor_msgs::msg::JointState::ConstSharedPtr latest_joint_state_; - - size_t num_commands_; - trajectory_msgs::msg::JointTrajectory::ConstSharedPtr latest_traj_cmd_; - std_msgs::msg::Float64MultiArray::ConstSharedPtr latest_array_cmd_; - - bool status_seen_; - StatusCode status_tracking_code_ = StatusCode::NO_WARNING; - - mutable std::mutex latest_state_mutex_; -}; // class ServoFixture - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/test_servo_collision.cpp b/moveit_ros/moveit_servo/test/test_servo_collision.cpp deleted file mode 100644 index 1bb13065c0..0000000000 --- a/moveit_ros/moveit_servo/test/test_servo_collision.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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. - *********************************************************************/ - -/* Title : test_servo_collision.cpp - * Project : moveit_servo - * Created : 08/03/2020 - * Author : Adam Pettinger - */ - -#include "servo_launch_test_common.hpp" -#include - -namespace moveit_servo -{ -TEST_F(ServoFixture, SelfCollision) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // Look for DECELERATE_FOR_COLLISION status - watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_COLLISION); - - // Publish some joint jog commands that will bring us to collision - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - log_time_start = node_->now(); - size_t iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->header.frame_id = "panda_link3"; - msg->joint_names.push_back("panda_joint4"); - msg->velocities.push_back(-1.0); - pub_joint_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for collision: " << (log_time_end - log_time_start).seconds()); -} - -TEST_F(ServoFixture, ExternalCollision) -{ - // NOTE: This test is meant to be run after the SelfCollision test - // It assumes the position is where the robot ends in SelfCollision test - - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - - // Create collision object, in the way of servoing - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "box"; - - shape_msgs::msg::SolidPrimitive box; - box.type = box.BOX; - box.dimensions = { 0.1, 0.4, 0.1 }; - - geometry_msgs::msg::Pose box_pose; - box_pose.position.x = 0.4; - box_pose.position.y = 0.0; - box_pose.position.z = 0.4; - - collision_object.primitives.push_back(box); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - - moveit_msgs::msg::PlanningSceneWorld psw; - psw.collision_objects.push_back(collision_object); - - moveit_msgs::msg::PlanningScene ps; - ps.is_diff = true; - ps.world = psw; - - // Publish the collision object to the planning scene - auto scene_pub = node_->create_publisher("/planning_scene", 10); - scene_pub->publish(ps); - - // Look for DECELERATE_FOR_COLLISION status - watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_COLLISION); - - // Now publish twist commands that collide with the box - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - log_time_start = node_->now(); - size_t iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = 0.2; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for collision: " << (log_time_end - log_time_start).seconds()); -} - -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - // It is important we init ros before google test because we are going to - // create a node during the google test init. - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/moveit_ros/moveit_servo/test/test_servo_interface.cpp b/moveit_ros/moveit_servo/test/test_servo_interface.cpp deleted file mode 100644 index f64451e6cd..0000000000 --- a/moveit_ros/moveit_servo/test/test_servo_interface.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * 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 PickNik Inc. 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: Dave Coleman and Tyler Weaver - Desc: Test for the C++ interface to moveit_servo -*/ - -#include "servo_launch_test_common.hpp" - -using namespace std::chrono_literals; - -namespace moveit_servo -{ -TEST_F(ServoFixture, SendTwistStampedTest) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - ASSERT_TRUE(setupCommandSub(servo_parameters_.command_out_type)); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // We want to count the number of commands Servo publishes, we need timing - auto time_start = node_->now(); - - // Publish N messages with some time between, ensure it's less than the timeout for Servo - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - size_t num_commands = 30; - resetNumCommands(); - for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = 0.1; - msg->twist.angular.z = 0.5; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - // Capture the time and number of received messages - auto time_end = node_->now(); - auto num_received = getNumCommands(); - - // Compare actual number received to expected number - auto num_expected = (time_end - time_start).seconds() / servo_parameters_.publish_period; - RCLCPP_INFO_STREAM(LOGGER, "Wait publish messages: " << (time_end - time_start).seconds()); - - EXPECT_GT(num_received, 0.5 * num_expected); - EXPECT_LT(num_received, 1.5 * num_expected); -} - -TEST_F(ServoFixture, SendJointServoTest) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - ASSERT_TRUE(setupCommandSub(servo_parameters_.command_out_type)); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // We want to count the number of commands Servo publishes, we need timing - auto time_start = node_->now(); - - // Publish N messages with some time between, ensure it's less than the timeout for Servo - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - size_t num_commands = 30; - resetNumCommands(); - for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->header.frame_id = "panda_link3"; - msg->joint_names.push_back("panda_joint1"); - msg->velocities.push_back(0.1); - pub_joint_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - // Capture the time and number of received messages - auto time_end = node_->now(); - auto num_received = getNumCommands(); - - // Compare actual number received to expected number - auto num_expected = (time_end - time_start).seconds() / servo_parameters_.publish_period; - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait publish messages: " << (time_end - time_start).seconds()); - - EXPECT_GT(num_received, 0.5 * num_expected); - EXPECT_LT(num_received, 1.5 * num_expected); - - // Now let's test the Servo input going stale - // We expect the command we were publishing above to continue for a while, then - // to continually receive Servo output, but with 0 velocity/delta_position - log_time_start = node_->now(); - - // Allow the last command to go stale and measure the output position - const int sleep_time = 1.5 * 1000 * servo_parameters_.incoming_command_timeout; - rclcpp::sleep_for(std::chrono::milliseconds(sleep_time)); - double joint_position_first = getLatestTrajCommand().points[0].positions[0]; - - // Now if we sleep a bit longer and check again, it should be the same - rclcpp::sleep_for(std::chrono::milliseconds(sleep_time)); - double joint_position_later = getLatestTrajCommand().points[0].positions[0]; - EXPECT_NEAR(joint_position_first, joint_position_later, 0.001); - - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait stale command: " << (log_time_end - log_time_start).seconds()); -} - -TEST_F(ServoFixture, DynamicParameterTest) -{ - ASSERT_TRUE(setupStartClient()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - auto set_parameters_client = - node_->create_client(test_parameters_->servo_node_name + "/set_parameters"); - - while (!set_parameters_client->service_is_ready()) - { - if (!rclcpp::ok()) - { - ASSERT_TRUE(false) << "Interrupted while waiting for the service. Exiting."; - } - RCLCPP_INFO(LOGGER, "set_parameters_client service not yet available, waiting again..."); - rclcpp::sleep_for(500ms); - } - - // Test changing dynamic parameter - auto request = std::make_shared(); - - // This is a dynamic parameter - request->parameters.push_back( - rclcpp::Parameter("moveit_servo.robot_link_command_frame", "panda_link4").to_parameter_msg()); - - // This is not even a parameter that is declared (it should fail) - request->parameters.push_back(rclcpp::Parameter("moveit_servo.not_set_parameter", 1.0).to_parameter_msg()); - - auto set_parameter_response = set_parameters_client->async_send_request(request).get(); - - // The first one should have succeeded - ASSERT_TRUE(set_parameter_response->results[0].successful) << set_parameter_response->results[0].reason; - - // This parameter should fail to set. - ASSERT_FALSE(set_parameter_response->results[1].successful) - << "`not_set_parameter` is not a parameter and should fail to be set"; -} -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py similarity index 60% rename from moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py rename to moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py index 002817cf51..ea6c730428 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py @@ -1,5 +1,6 @@ import os import launch +import unittest import launch_ros import launch_testing from ament_index_python.packages import get_package_share_directory @@ -7,35 +8,23 @@ from launch_param_builder import ParameterBuilder -def generate_servo_test_description( - *args, - gtest_name: launch.some_substitutions_type.SomeSubstitutionsType, - start_position_path: launch.some_substitutions_type.SomeSubstitutionsType = "" -): - # Get parameters using the demo config file - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config.yaml") - .to_dict() - } - - # Get URDF and SRDF - if start_position_path: - initial_positions_file = os.path.join( - os.path.dirname(__file__), start_position_path - ) - robot_description_mappings = {"initial_positions_file": initial_positions_file} - else: - robot_description_mappings = None - +def generate_test_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") - .robot_description( - file_path="config/panda.urdf.xacro", mappings=robot_description_mappings - ) + .robot_description(file_path="config/panda.urdf.xacro") .to_moveit_configs() ) + # Get parameters for the Servo node + servo_params = { + "moveit_servo_test": ParameterBuilder("moveit_servo") + .yaml("config/test_config_panda.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), @@ -69,7 +58,7 @@ def generate_servo_test_description( # Component nodes for tf and Servo test_container = launch_ros.actions.ComposableNodeContainer( - name="test_servo_integration_container", + name="servo_integration_tests_container", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -90,48 +79,20 @@ def generate_servo_test_description( output="screen", ) - servo_node = launch_ros.actions.Node( - package="moveit_servo", - executable="servo_node_main", - output="screen", + servo_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "moveit_servo_cpp_integration_test", + ] + ), parameters=[ servo_params, + low_pass_filter_coeff, moveit_config.robot_description, moveit_config.robot_description_semantic, - moveit_config.joint_limits, moveit_config.robot_description_kinematics, ], - ) - - # NOTE: The smoothing plugin doesn't seem to be accessible from containers - # servo_container = ComposableNodeContainer( - # name="servo_container", - # namespace="/", - # package="rclcpp_components", - # executable="component_container_mt", - # composable_node_descriptions=[ - # ComposableNode( - # package="moveit_servo", - # plugin="moveit_servo::ServoNode", - # name="servo_node", - # parameters=[ - # servo_params, - # robot_description, - # robot_description_semantic, - # joint_limits_yaml, - # ], - # ), - # ], - # output="screen", - # ) - - # Unknown how to set timeout - # https://github.com/ros2/launch/issues/466 - servo_gtest = launch_ros.actions.Node( - executable=launch.substitutions.PathJoinSubstitution( - [launch.substitutions.LaunchConfiguration("test_binary_dir"), gtest_name] - ), - parameters=[servo_params], output="screen", ) @@ -145,14 +106,23 @@ def generate_servo_test_description( ros2_control_node, joint_state_broadcaster_spawner, panda_arm_controller_spawner, - servo_node, test_container, launch.actions.TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] ), { - "servo_node": servo_node, - "test_container": test_container, "servo_gtest": servo_gtest, - "ros2_control_node": ros2_control_node, } + + +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, servo_gtest): + self.proc_info.assertWaitForShutdown(servo_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, servo_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py new file mode 100644 index 0000000000..94519096ca --- /dev/null +++ b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py @@ -0,0 +1,160 @@ +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 +from launch_param_builder import ParameterBuilder +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="true" + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/test_config_panda.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # 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=[moveit_config.robot_description, ros2_controllers_path], + 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"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + # Example of launching Servo as a node component + # Launching as a node component makes ROS 2 intraprocess communication more efficient. + launch_ros.descriptions.ComposableNode( + package="moveit_servo", + plugin="moveit_servo::ServoNode", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + condition=UnlessCondition(launch_as_standalone_node), + ), + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="servo_node", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + condition=IfCondition(launch_as_standalone_node), + ) + + servo_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "moveit_servo_ros_integration_test", + ] + ), + output="screen", + ) + + return launch.LaunchDescription( + [ + servo_node, + container, + launch.actions.TimerAction(period=3.0, actions=[ros2_control_node]), + launch.actions.TimerAction( + period=5.0, actions=[joint_state_broadcaster_spawner] + ), + launch.actions.TimerAction( + period=7.0, actions=[panda_arm_controller_spawner] + ), + launch.actions.TimerAction(period=9.0, actions=[servo_gtest]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "servo_gtest": servo_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, servo_gtest): + self.proc_info.assertWaitForShutdown(servo_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, servo_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py similarity index 74% rename from moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py rename to moveit_ros/moveit_servo/tests/launch/servo_utils.test.py index f96d7523a1..a656655334 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py @@ -1,31 +1,30 @@ import os -import pytest import launch +import unittest import launch_ros import launch_testing -import unittest from ament_index_python.packages import get_package_share_directory -from launch_param_builder import ParameterBuilder from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder -def generate_servo_test_description( - *args, gtest_name: launch.some_substitutions_type.SomeSubstitutionsType -): +def generate_test_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .to_moveit_configs() ) - # Get parameters for the Pose Tracking and Servo nodes + # Get parameters for the Servo node servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/pose_tracking_settings.yaml") - .yaml("config/panda_simulated_config_pose_tracking.yaml") + "moveit_servo_test": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") .to_dict() } + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), @@ -59,7 +58,7 @@ def generate_servo_test_description( # Component nodes for tf and Servo test_container = launch_ros.actions.ComposableNodeContainer( - name="test_pose_tracking_container", + name="servo_utils_tests_container", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -80,13 +79,19 @@ def generate_servo_test_description( output="screen", ) - pose_tracking_gtest = launch_ros.actions.Node( + servo_gtest = launch_ros.actions.Node( executable=launch.substitutions.PathJoinSubstitution( - [launch.substitutions.LaunchConfiguration("test_binary_dir"), gtest_name] + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "moveit_servo_utils_test", + ] ), parameters=[ - moveit_config.to_dict(), servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, ], output="screen", ) @@ -102,28 +107,22 @@ def generate_servo_test_description( joint_state_broadcaster_spawner, panda_arm_controller_spawner, test_container, - launch.actions.TimerAction(period=2.0, actions=[pose_tracking_gtest]), + launch.actions.TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] ), { - "test_container": test_container, - "servo_gtest": pose_tracking_gtest, - "ros2_control_node": ros2_control_node, + "servo_gtest": servo_gtest, } -def generate_test_description(): - return generate_servo_test_description(gtest_name="test_servo_pose_tracking") - - -class TestGTestProcessActive(unittest.TestCase): +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, servo_gtest): self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) @launch_testing.post_shutdown_test() class TestGTestProcessPostShutdown(unittest.TestCase): - def test_gtest_pass( - self, proc_info, test_container, servo_gtest, ros2_control_node - ): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, servo_gtest): launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp similarity index 53% rename from moveit_ros/moveit_servo/include/moveit_servo/servo_server.h rename to moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index f16e132039..55fff52865 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2020, PickNik Inc. + * Copyright (c) 2023, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,20 +32,43 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Title : servo_server.h - * Project : moveit_servo - * Created : 07/13/2020 - * Author : Adam Pettinger +/* Title : servo_cpp_fixture.hpp + * Project : moveit_servo + * Created : 07/13/2020 + * Author : Adam Pettinger, V Mohammed Ibrahim + * + * Description : Resources used by servo c++ integration tests */ -#pragma once +#include +#include +#include +#include +#include +#include + +class ServoCppFixture : public testing::Test +{ +protected: + ServoCppFixture() + { + // Create a node to be given to Servo. + servo_test_node_ = std::make_shared("moveit_servo_test"); -#pragma message("Header `servo_server.h` is deprecated, please use `servo_node.h`") + // Create a Servo object for testing. + const std::string servo_param_namespace = "moveit_servo_test"; + servo_param_listener_ = std::make_shared(servo_test_node_, servo_param_namespace); + servo_params_ = servo_param_listener_->get_params(); -#include + planning_scene_monitor_ = moveit_servo::createPlanningSceneMonitor(servo_test_node_, servo_params_); -namespace moveit_servo -{ -using ServoServer [[deprecated("use ServoNode from servo_node.h")]] = ServoNode; + servo_test_instance_ = + std::make_shared(servo_test_node_, servo_param_listener_, planning_scene_monitor_); + } -} // namespace moveit_servo + std::shared_ptr servo_test_node_; + std::shared_ptr servo_param_listener_; + servo::Params servo_params_; + std::shared_ptr servo_test_instance_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; +}; diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp new file mode 100644 index 0000000000..585c15eae4 --- /dev/null +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -0,0 +1,168 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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 : V Mohammed Ibrahim + Desc : Resources used by servo ROS integration tests + Title : servo_ros_fixture.hpp + Project : moveit_servo + Created : 07/23/2023 +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ServoRosFixture : public testing::Test +{ +protected: + void statusCallback(const moveit_msgs::msg::ServoStatus::ConstSharedPtr& msg) + { + status_ = static_cast(msg->code); + status_count_++; + } + + void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& msg) + { + joint_states_ = *msg; + state_count_++; + } + + void trajectoryCallback(const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) + { + published_trajectory_ = *msg; + traj_count_++; + } + + void SetUp() override + { + executor_->add_node(servo_test_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + + void waitForService() + { + auto logger = servo_test_node_->get_logger(); + while (!switch_input_client_->service_is_ready()) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(logger, "Interrupted while waiting for the service. Exiting."); + std::exit(EXIT_FAILURE); + } + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + } + RCLCPP_INFO(logger, "SERVICE READY"); + } + + void waitForJointStates() + { + auto logger = servo_test_node_->get_logger(); + auto wait_timeout = rclcpp::Duration::from_seconds(5); + auto start_time = servo_test_node_->now(); + + while (rclcpp::ok() && state_count_ == 0) + { + rclcpp::sleep_for(std::chrono::milliseconds(500)); + auto elapsed_time = servo_test_node_->now() - start_time; + if (elapsed_time >= wait_timeout) + { + RCLCPP_ERROR(logger, "Timed out waiting for joint states"); + FAIL(); + } + } + } + + ServoRosFixture() : state_count_{ 0 }, traj_count_{ 0 } + { + // Create a node to be given to Servo. + servo_test_node_ = std::make_shared("moveit_servo_test"); + executor_ = std::make_shared(); + + status_ = moveit_servo::StatusCode::INVALID; + + status_subscriber_ = servo_test_node_->create_subscription( + "/servo_node/status", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::ServoStatus::ConstSharedPtr& msg) { return statusCallback(msg); }); + + joint_state_subscriber_ = servo_test_node_->create_subscription( + "/joint_states", rclcpp::SystemDefaultsQoS(), + [this](const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { return jointStateCallback(msg); }); + + trajectory_subscriber_ = servo_test_node_->create_subscription( + "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), + [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCallback(msg); }); + + switch_input_client_ = + servo_test_node_->create_client("/servo_node/switch_command_type"); + + waitForService(); + } + + std::shared_ptr servo_test_node_; + + // Executor and a thread to run the executor. + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; + + rclcpp::Subscription::SharedPtr status_subscriber_; + rclcpp::Subscription::SharedPtr joint_state_subscriber_; + rclcpp::Subscription::SharedPtr trajectory_subscriber_; + rclcpp::Client::SharedPtr switch_input_client_; + + sensor_msgs::msg::JointState joint_states_; + trajectory_msgs::msg::JointTrajectory published_trajectory_; + + std::atomic status_count_; + std::atomic status_; + + std::atomic state_count_; + std::atomic traj_count_; +}; diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp new file mode 100644 index 0000000000..113031779d --- /dev/null +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -0,0 +1,158 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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 : V Mohammed Ibrahim + Desc : Integration tests for the servo c++ API + Title : test_integration.cpp + Project : moveit_servo + Created : 07/07/2023 +*/ + +#include "servo_cpp_fixture.hpp" + +namespace +{ + +TEST_F(ServoCppFixture, JointJogTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::JointJogCommand joint_jog_z{ { "panda_joint7" }, { 1.0 } }; + moveit_servo::JointJogCommand zero_joint_jog; + // Compute next state. + servo_test_instance_->setCommandType(moveit_servo::CommandType::JOINT_JOG); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(zero_joint_jog); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(joint_jog_z); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, 0.02, tol); +} + +TEST_F(ServoCppFixture, TwistTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::TwistCommand twist_non_zero{ servo_params_.planning_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; + moveit_servo::TwistCommand twist_zero{ servo_params_.planning_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + + servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(twist_zero); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(twist_non_zero); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + constexpr double expected_delta = -0.001693; + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, expected_delta, tol); +} + +TEST_F(ServoCppFixture, NonPlanningFrameTwistTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::TwistCommand twist_non_zero{ servo_params_.ee_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; + moveit_servo::TwistCommand twist_zero{ servo_params_.ee_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + + servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(twist_zero); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(twist_non_zero); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + constexpr double expected_delta = 0.001693; + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, expected_delta, tol); +} + +TEST_F(ServoCppFixture, PoseTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::PoseCommand zero_pose, non_zero_pose; + zero_pose.frame_id = servo_params_.planning_frame; + zero_pose.pose = servo_test_instance_->getEndEffectorPose(); + + non_zero_pose.frame_id = servo_params_.planning_frame; + non_zero_pose.pose = servo_test_instance_->getEndEffectorPose(); + non_zero_pose.pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + servo_test_instance_->setCommandType(moveit_servo::CommandType::POSE); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(zero_pose); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(non_zero_pose); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + constexpr double expected_delta = 0.057420; + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, expected_delta, tol); +} + +} // namespace + +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/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp new file mode 100644 index 0000000000..ed23f4d69e --- /dev/null +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -0,0 +1,200 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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 : V Mohammed Ibrahim + Desc : ROS API integration tests + Title : test_ros_integration.cpp + Project : moveit_servo + Created : 07/23/2023 +*/ + +#include "servo_ros_fixture.hpp" +#include +#include +#include + +/** + * Order of joints in joint state message + * + * - panda_finger_joint1 + * - panda_joint1 + * - panda_joint2 + * - panda_finger_joint2 + * - panda_joint3 + * - panda_joint4 + * - panda_joint5 + * - panda_joint6 + * - panda_joint7 --> index 8 + */ + +namespace +{ +const int NUM_COMMANDS = 10; +} + +namespace +{ +TEST_F(ServoRosFixture, testJointJog) +{ + waitForJointStates(); + + auto joint_jog_publisher = servo_test_node_->create_publisher( + "/servo_node/delta_joint_cmds", rclcpp::SystemDefaultsQoS()); + + // Call input type service + auto request = std::make_shared(); + request->command_type = 0; + auto response = switch_input_client_->async_send_request(request); + ASSERT_EQ(response.get()->success, true); + + ASSERT_NE(state_count_, 0); + + control_msgs::msg::JointJog jog_cmd; + + jog_cmd.header.frame_id = "panda_link0"; + jog_cmd.joint_names.resize(7); + jog_cmd.velocities.resize(7); + jog_cmd.joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7" }; + + std::fill(jog_cmd.velocities.begin(), jog_cmd.velocities.end(), 0.0); + + jog_cmd.velocities[6] = 1.0; + + size_t count = 0; + while (rclcpp::ok() && count < NUM_COMMANDS) + { + jog_cmd.header.stamp = servo_test_node_->now(); + joint_jog_publisher->publish(jog_cmd); + count++; + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } + + ASSERT_GE(traj_count_, NUM_COMMANDS); + + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); +} + +TEST_F(ServoRosFixture, testTwist) +{ + waitForJointStates(); + + auto twist_publisher = servo_test_node_->create_publisher( + "/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS()); + + auto request = std::make_shared(); + request->command_type = 1; + auto response = switch_input_client_->async_send_request(request); + ASSERT_EQ(response.get()->success, true); + + ASSERT_NE(state_count_, 0); + + geometry_msgs::msg::TwistStamped twist_cmd; + twist_cmd.header.frame_id = "panda_link0"; // Planning frame + twist_cmd.twist.linear.x = 0.0; + twist_cmd.twist.linear.y = 0.0; + twist_cmd.twist.linear.z = 0.0; + twist_cmd.twist.angular.x = 0.0; + twist_cmd.twist.angular.y = 0.0; + twist_cmd.twist.angular.z = 0.5; + + size_t count = 0; + while (rclcpp::ok() && count < NUM_COMMANDS) + { + twist_cmd.header.stamp = servo_test_node_->now(); + twist_publisher->publish(twist_cmd); + count++; + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } + + ASSERT_GE(traj_count_, NUM_COMMANDS); + + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after twist: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); +} + +TEST_F(ServoRosFixture, testPose) +{ + waitForJointStates(); + + auto pose_publisher = servo_test_node_->create_publisher( + "/servo_node/pose_target_cmds", rclcpp::SystemDefaultsQoS()); + + auto request = std::make_shared(); + request->command_type = 2; + auto response = switch_input_client_->async_send_request(request); + ASSERT_EQ(response.get()->success, true); + + geometry_msgs::msg::PoseStamped pose_cmd; + pose_cmd.header.frame_id = "panda_link0"; // Planning frame + + pose_cmd.pose.position.x = 0.3; + pose_cmd.pose.position.y = 0.0; + pose_cmd.pose.position.z = 0.6; + pose_cmd.pose.orientation.x = 0.7; + pose_cmd.pose.orientation.y = -0.7; + pose_cmd.pose.orientation.z = -0.000014; + pose_cmd.pose.orientation.w = -0.0000015; + + ASSERT_NE(state_count_, 0); + + size_t count = 0; + while (rclcpp::ok() && count < NUM_COMMANDS) + { + pose_cmd.header.stamp = servo_test_node_->now(); + pose_publisher->publish(pose_cmd); + count++; + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } + + ASSERT_GE(traj_count_, NUM_COMMANDS); + + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after pose: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); +} + +} // namespace + +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/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp new file mode 100644 index 0000000000..bd6eb9fcfc --- /dev/null +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -0,0 +1,234 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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 : V Mohammed Ibrahim + Desc : Tests for utilities that depend on the robot/ robot state. + Title : test_utils.cpp + Project : moveit_servo + Created : 06/20/2023 +*/ + +#include +#include +#include +#include +#include +#include + +namespace +{ + +TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::JointBoundsVector joint_bounds = robot_model->getActiveJointModelsBounds(); + + // Get the upper bound for the velocities of each joint. + Eigen::VectorXd incoming_velocities(joint_bounds.size()); + for (size_t i = 0; i < joint_bounds.size(); i++) + { + const auto joint_bound = (*joint_bounds[i])[0]; + if (joint_bound.velocity_bounded_) + { + incoming_velocities(i) = joint_bound.max_velocity_; + } + } + + // Create incoming velocities with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05 + // Scale down all other joint velocities by 0.3 to keep it within limits. + incoming_velocities(0) *= 1.1; + incoming_velocities(1) *= 1.05; + incoming_velocities.tail<5>() *= 0.7; + + // The resulting scaling factor selected should be approximately 0.95238 + double user_velocity_override = 0.0; + double scaling_factor = + moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); + constexpr double tol = 0.001; + ASSERT_NEAR(scaling_factor, 0.95238, tol); +} + +TEST(ServoUtilsUnitTests, validVector) +{ + Eigen::VectorXd valid_vector(7); + valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + EXPECT_TRUE(moveit_servo::isValidCommand(valid_vector)); +} + +TEST(ServoUtilsUnitTests, invalidVector) +{ + Eigen::VectorXd invalid_vector(6); + invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_vector)); +} + +TEST(ServoUtilsUnitTests, validTwist) +{ + Eigen::VectorXd valid_vector(6); + valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + moveit_servo::TwistCommand valid_twist{ "panda_link0", valid_vector }; + EXPECT_TRUE(moveit_servo::isValidCommand(valid_twist)); +} + +TEST(ServoUtilsUnitTests, emptyTwistFrame) +{ + Eigen::VectorXd invalid_vector(6); + invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; + moveit_servo::TwistCommand invalid_twist; + invalid_twist.velocities = invalid_vector; + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); +} + +TEST(ServoUtilsUnitTests, invalidTwistVelocities) +{ + Eigen::VectorXd invalid_vector(6); + invalid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, std::nan(""); + moveit_servo::TwistCommand invalid_twist{ "panda_link0", invalid_vector }; + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); +} + +TEST(ServoUtilsUnitTests, validIsometry) +{ + Eigen::Isometry3d valid_isometry; + valid_isometry.setIdentity(); + EXPECT_TRUE(moveit_servo::isValidCommand(valid_isometry)); +} + +TEST(ServoUtilsUnitTests, invalidIsometry) +{ + Eigen::Isometry3d invalid_isometry; + invalid_isometry.setIdentity(); + invalid_isometry.translation().z() = std::nan(""); + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_isometry)); +} + +TEST(ServoUtilsUnitTests, validPose) +{ + Eigen::Isometry3d valid_isometry; + valid_isometry.setIdentity(); + moveit_servo::PoseCommand valid_pose{ "panda_link0", valid_isometry }; + EXPECT_TRUE(moveit_servo::isValidCommand(valid_pose)); +} + +TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + robot_state->setToDefaultValues(); + + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Approach singularity + Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); +} + +TEST(ServoUtilsUnitTests, HaltForSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + robot_state->setToDefaultValues(); + + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Move to singular state. + Eigen::Vector singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 }; + robot_state->setJointGroupActivePositions(joint_model_group, singular_state); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::HALT_FOR_SINGULARITY); +} + +TEST(ServoUtilsUnitTests, LeavingSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + robot_state->setToDefaultValues(); + + Eigen::Vector cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Move away from singularity + Eigen::Vector state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); +} + +} // namespace + +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/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index 2183bc8247..6d581c4985 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 8c0aee6382..363011718c 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.7.4 + 2.8.0 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 178f4d7701..9229de43ba 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* Use `isnan` in `DepthImageOctomapUpdater` (`#2201 `_) +* Contributors: Ezra Brooks, Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index c3b1b1f818..ba32c93bf5 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -103,7 +103,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater std::unique_ptr > mesh_filter_; std::unique_ptr free_space_updater_; - std::vector x_cache_, y_cache_; + std::vector x_cache_, y_cache_; double inv_fx_, inv_fy_, K0_, K2_, K4_, K5_; std::vector filtered_labels_; rclcpp::Time last_depth_callback_start_; diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index dd297f82e6..958bf35a6f 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -445,7 +445,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; debug_msg.step = w * sizeof(float); debug_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); + mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); pub_model_depth_image_.publish(debug_msg, *info_msg); sensor_msgs::msg::Image filtered_depth_msg; @@ -456,7 +456,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; filtered_depth_msg.step = w * sizeof(float); filtered_depth_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); sensor_msgs::msg::Image label_msg; @@ -488,7 +488,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: if (filtered_data.size() < img_size) filtered_data.resize(img_size); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); unsigned short* msg_data = reinterpret_cast(&filtered_msg.data[0]); for (std::size_t i = 0; i < img_size; ++i) { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 92e3441c4c..cf33653249 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -70,7 +70,7 @@ class GLRenderer * \param[in] near distance of the near clipping plane in meters * \param[in] far distance of the far clipping plane in meters */ - GLRenderer(unsigned width, unsigned height, float near = 0.1, float far = 10.0); + GLRenderer(unsigned width, unsigned height, double near = 0.1, double far = 10.0); /** \brief destructor, destroys frame buffer objects and OpenGL context*/ ~GLRenderer(); @@ -106,7 +106,7 @@ class GLRenderer * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] buffer pointer to memory where the depth values need to be stored */ - void getDepthBuffer(float* buffer) const; + void getDepthBuffer(double* buffer) const; /** * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context. @@ -135,7 +135,7 @@ class GLRenderer * \param[in] cx x component of principal point * \param[in] cy y component of principal point */ - void setCameraParameters(float fx, float fy, float cx, float cy); + void setCameraParameters(double fx, double fy, double cx, double cy); /** * \brief sets the near and far clipping plane distances in meters @@ -143,21 +143,21 @@ class GLRenderer * \param[in] near distance of the near clipping plane in meters * \param[in] far distance of the far clipping plane in meters */ - void setClippingRange(float near, float far); + void setClippingRange(double near, double far); /** * \brief returns the distance of the near clipping plane in meters * \author Suat Gedikli (gedikli@willowgarage.com) * \return distance of near clipping plane in meters */ - const float& getNearClippingDistance() const; + const double& getNearClippingDistance() const; /** * \brief returns the distance of the far clipping plane in meters * \author Suat Gedikli (gedikli@willowgarage.com) * \return distance of the far clipping plane in meters */ - const float& getFarClippingDistance() const; + const double& getFarClippingDistance() const; /** * \brief returns the width of the frame buffer objectsin pixels @@ -174,7 +174,7 @@ class GLRenderer unsigned getHeight() const; /** - * \brief set the size of fram buffers + * \brief set the size of frame buffers * \author Suat Gedikli (gedikli@willowgarage.com) * \param[in] width width of frame buffer in pixels * \param[in] height height of frame buffer in pixels @@ -281,10 +281,10 @@ class GLRenderer GLuint program_; /** \brief distance of near clipping plane in meters*/ - float near_; + double near_; /** \brief distance of far clipping plane in meters*/ - float far_; + double far_; /** \brief focal length in x-direction of camera in pixels*/ float fx_; @@ -299,11 +299,11 @@ class GLRenderer float cy_; /** \brief map from thread id to OpenGL context */ - static std::map > context_; + static std::map > context; /* \brief lock for context map */ - static std::mutex context_lock_; + static std::mutex context_lock; - static bool glutInitialized_; + static bool glut_initialized; }; } // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index c319bdf35f..646c3ca569 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -131,7 +131,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] depth pointer to buffer to be filled with depth values. */ - void getFilteredDepth(float* depth) const; + void getFilteredDepth(double* depth) const; /** * \brief retrieves the labels of the rendered model @@ -149,7 +149,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] depth pointer to buffer to be filled with depth values. */ - void getModelDepth(float* depth) const; + void getModelDepth(double* depth) const; /** * \brief set the shadow threshold. points that are further away than the rendered model are filtered out. @@ -225,7 +225,7 @@ class MeshFilterBase void addJob(const JobPtr& job) const; /** - * \brief sets the size of the fram buffers + * \brief sets the size of the frame buffers * \author Suat Gedikli (gedikli@willowgarage.com) * \param[in] width width of frame buffers in pixels * \param[in] height height of frame buffers in pixels diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 607fb663c0..0ba4405281 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -103,13 +103,13 @@ class SensorModel * \brief transforms depth values from rendered model to metric depth values * \param[in,out] depth pointer to floating point depth buffer */ - virtual void transformModelDepthToMetricDepth(float* depth) const; + virtual void transformModelDepthToMetricDepth(double* depth) const; /** * \brief transforms depth values from filtered depth to metric depth values * \param[in,out] depth pointer to floating point depth buffer */ - virtual void transformFilteredDepthToMetricDepth(float* depth) const; + virtual void transformFilteredDepthToMetricDepth(double* depth) const; /** * \brief sets the image size diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index f77c692bcc..266d8948ab 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -113,7 +113,7 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d params.setCameraParameters(info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]); params.setImageSize(depth_msg->width, depth_msg->height); - const float* src = (const float*)&depth_msg->data[0]; + const double* src = (const double*)&depth_msg->data[0]; //* static unsigned data_size = 0; static unsigned short* data = nullptr; @@ -135,7 +135,7 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d if (static_cast(filtered_depth_ptr_->image.cols) != depth_msg->width || static_cast(filtered_depth_ptr_->image.rows) != depth_msg->height) filtered_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1); - mesh_filter_->getFilteredDepth((float*)filtered_depth_ptr_->image.data); + mesh_filter_->getFilteredDepth((double*)filtered_depth_ptr_->image.data); pub_filtered_depth_image_.publish(filtered_depth_ptr_->toImageMsg(), info_msg); } @@ -148,7 +148,7 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d if (static_cast(model_depth_ptr_->image.cols) != depth_msg->width || static_cast(model_depth_ptr_->image.rows) != depth_msg->height) model_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1); - mesh_filter_->getModelDepth((float*)model_depth_ptr_->image.data); + mesh_filter_->getModelDepth((double*)model_depth_ptr_->image.data); pub_model_depth_image_.publish(model_depth_ptr_->toImageMsg(), info_msg); } diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index a1e63ba9dd..2176b9db7d 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -55,7 +55,7 @@ using namespace std; static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.perception.gl_renderer"); -mesh_filter::GLRenderer::GLRenderer(unsigned width, unsigned height, float near, float far) +mesh_filter::GLRenderer::GLRenderer(unsigned width, unsigned height, double near, double far) : width_(width) , height_(height) , fbo_id_(0) @@ -92,7 +92,7 @@ void mesh_filter::GLRenderer::setBufferSize(unsigned width, unsigned height) } } -void mesh_filter::GLRenderer::setClippingRange(float near, float far) +void mesh_filter::GLRenderer::setClippingRange(double near, double far) { if (near_ <= 0) throw runtime_error("near clipping plane distance needs to be larger than 0"); @@ -102,7 +102,7 @@ void mesh_filter::GLRenderer::setClippingRange(float near, float far) far_ = far; } -void mesh_filter::GLRenderer::setCameraParameters(float fx, float fy, float cx, float cy) +void mesh_filter::GLRenderer::setCameraParameters(double fx, double fy, double cx, double cy) { fx_ = fx; fy_ = fy; @@ -112,10 +112,10 @@ void mesh_filter::GLRenderer::setCameraParameters(float fx, float fy, float cx, void mesh_filter::GLRenderer::setCameraParameters() const { - float left = near_ * -cx_ / fx_; - float right = near_ * (width_ - cx_) / fx_; - float top = near_ * cy_ / fy_; - float bottom = near_ * (cy_ - height_) / fy_; + double left = near_ * -cx_ / fx_; + double right = near_ * (width_ - cx_) / fx_; + double top = near_ * cy_ / fy_; + double bottom = near_ * (cy_ - height_) / fy_; glMatrixMode(GL_PROJECTION); glLoadIdentity(); @@ -214,7 +214,7 @@ void mesh_filter::GLRenderer::getColorBuffer(unsigned char* buffer) const glBindFramebuffer(GL_FRAMEBUFFER, 0); } -void mesh_filter::GLRenderer::getDepthBuffer(float* buffer) const +void mesh_filter::GLRenderer::getDepthBuffer(double* buffer) const { glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_); glBindTexture(GL_TEXTURE_2D, depth_id_); @@ -246,12 +246,12 @@ const GLuint& mesh_filter::GLRenderer::getProgramID() const return program_; } -const float& mesh_filter::GLRenderer::getNearClippingDistance() const +const double& mesh_filter::GLRenderer::getNearClippingDistance() const { return near_; } -const float& mesh_filter::GLRenderer::getFarClippingDistance() const +const double& mesh_filter::GLRenderer::getFarClippingDistance() const { return far_; } @@ -261,7 +261,7 @@ GLuint mesh_filter::GLRenderer::createShader(GLuint shaderType, const string& Sh GLuint shader_id = glCreateShader(shaderType); // Compile Shader - char const* source_pointer = ShaderCode.c_str(); + const char* source_pointer = ShaderCode.c_str(); glShaderSource(shader_id, 1, &source_pointer, nullptr); glCompileShader(shader_id); @@ -357,9 +357,9 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s return program_id; } -map > mesh_filter::GLRenderer::context_; -std::mutex mesh_filter::GLRenderer::context_lock_; -bool mesh_filter::GLRenderer::glutInitialized_ = false; +map > mesh_filter::GLRenderer::context; +std::mutex mesh_filter::GLRenderer::context_lock; +bool mesh_filter::GLRenderer::glut_initialized = false; namespace { @@ -370,8 +370,8 @@ void nullDisplayFunction() void mesh_filter::GLRenderer::createGLContext() { - std::unique_lock _(context_lock_); - if (!glutInitialized_) + std::unique_lock _(context_lock); + if (!glut_initialized) { char buffer[1]; char* args = buffer; @@ -379,16 +379,16 @@ void mesh_filter::GLRenderer::createGLContext() glutInit(&n, &args); glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH); - glutInitialized_ = true; + glut_initialized = true; } // check if our thread is initialized std::thread::id thread_id = std::this_thread::get_id(); - map >::iterator context_it = context_.find(thread_id); + map >::iterator context_it = context.find(thread_id); - if (context_it == context_.end()) + if (context_it == context.end()) { - context_[thread_id] = std::pair(1, 0); + context[thread_id] = std::pair(1, 0); glutInitWindowPosition(glutGet(GLUT_SCREEN_WIDTH) + 30000, 0); glutInitWindowSize(1, 1); @@ -409,7 +409,7 @@ void mesh_filter::GLRenderer::createGLContext() for (int i = 0; i < 10; ++i) glutMainLoopEvent(); - context_[thread_id] = std::pair(1, window_id); + context[thread_id] = std::pair(1, window_id); } else ++(context_it->second.first); @@ -417,10 +417,10 @@ void mesh_filter::GLRenderer::createGLContext() void mesh_filter::GLRenderer::deleteGLContext() { - std::unique_lock _(context_lock_); + std::unique_lock _(context_lock); std::thread::id thread_id = std::this_thread::get_id(); - map >::iterator context_it = context_.find(thread_id); - if (context_it == context_.end()) + map >::iterator context_it = context.find(thread_id); + if (context_it == context.end()) { stringstream error_msg; error_msg << "No OpenGL context exists for Thread " << thread_id; @@ -430,7 +430,7 @@ void mesh_filter::GLRenderer::deleteGLContext() if (--(context_it->second.first) == 0) { glutDestroyWindow(context_it->second.second); - context_.erase(context_it); + context.erase(context_it); } } diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 6b4b551b9f..793058297e 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -227,7 +227,7 @@ void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const job->wait(); } -void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const +void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const { JobPtr job1 = std::make_shared>([&renderer = *mesh_renderer_, depth] { renderer.getDepthBuffer(depth); }); @@ -243,7 +243,7 @@ void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const job2->wait(); } -void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const +void mesh_filter::MeshFilterBase::getFilteredDepth(double* depth) const { JobPtr job1 = std::make_shared>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); }); JobPtr job2 = std::make_shared>( diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 9ff5cdbf0c..562e7b4359 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -102,7 +102,7 @@ inline bool isAligned16(const void* pointer) #endif } // namespace -void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const +void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(double* depth) const { #if HAVE_SSE_EXTENSIONS const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_); @@ -161,7 +161,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(floa const float nf = near * far; const float f_n = far - near; - const float* depth_end = depth + width_ * height_; + const double* depth_end = depth + width_ * height_; while (depth < depth_end) { if (*depth != 0 && *depth != 1) @@ -178,7 +178,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(floa #endif } -void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(float* depth) const +void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(double* depth) const { #if HAVE_SSE_EXTENSIONS //* SSE version @@ -223,7 +223,7 @@ void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(f ++mmDepth; } #else - const float* depth_end = depth + width_ * height_; + const double* depth_end = depth + width_ * height_; const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_; const float offset = near_clipping_plane_distance_; while (depth < depth_end) diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 61e812cde7..1d3e8ab379 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -2,7 +2,7 @@ moveit_ros_perception - 2.7.4 + 2.8.0 Components of MoveIt connecting to perception Henning Kayser Tyler Weaver diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 36d0e778a9..e63ac1c5f2 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -71,7 +71,7 @@ SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node, { table_subscriber_ = node_handle_->create_subscription( - "table_array", 1, + "table_array", rclcpp::SystemDefaultsQoS(), [this](const object_recognition_msgs::msg::TableArray::ConstSharedPtr& msg) { return tableCallback(msg); }); visualization_publisher_ = node_handle_->create_publisher("visualize_place", 20); diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 4328552f00..343bf5aa4d 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,61 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Remove added path index from planner adapter function signature (`#2285 `_) +* Replaced boost::algorithm::join with fmt::join (`#2273 `_) + * Replaced boost::algorithm::join with fmt::join + * Made changes in CMakeLists.txt to accomodate fmt + * Updated package.xml files + * removed redundant boost dependencies + * Rename variables -> variable + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* Cleanup planning request adapter interface (`#2266 `_) + * Use default arguments instead of additional functions + * Use generate param lib for default plan request adapters + * Small cleanup of ResolveConstraintFrames + * Remove dublicate yaml file entry + * Move list_planning_adapter_plugins into own directory + * Apply suggestions from code review + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + * Fix copy& paste error + * Update parameter descriptions + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + * Apply suggestions from code review + Co-authored-by: Kyle Cesare + * EMPTY_PATH_INDEX_VECTOR -> empty_path_index_vector + * Update parameter yaml + * Make param listener unique + * Fix build error + * Use gt_eq instead of deprecated lower_bounds + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Kyle Cesare +* Prefer to use the active controller if multiple controllers apply (`#2251 `_) +* Don't default to random algorithm if no plugin is defined (`#2228 `_) + * Don't default to random algorithm if no plugin is defined + * Simplify selection logic & initialize default values in constructor + * Increase message severity +* Always set response planner id and warn if it is not set (`#2236 `_) +* Suppress redundant error message in CSM (`#2222 `_) + The CSM would spam the log if /joint_states messages + includes unkonwn joints. RobotModel::hasJointModel() + allows for verifying joint names in a safe way without + the error message. +* Minor cleanup to ros_control_interface and trajectory execution (`#2208 `_) +* Ensure that planning pipeline id is set (`#2202 `_) +* Add @brief descriptions for plan_request_adapters (`#2185 `_) +* Make loggers static or move into anonymous namespace (`#2184 `_) + * Make loggers static or move into anonymous namespace + * Update moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp + * Update moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp + * Move LOGGER out of class template +* Contributors: Henning Kayser, Sebastian Jahr, Shobuj Paul, Stephanie Eng + 2.7.4 (2023-05-18) ------------------ * Update default planning configs to use AddTimeOptimalParameterization (`#2167 `_) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 20c39ad0d8..e0bf25cdc6 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(fmt REQUIRED) find_package(generate_parameter_library REQUIRED) find_package(moveit_msgs REQUIRED) # find_package(moveit_ros_perception REQUIRED) @@ -46,12 +47,14 @@ set(THIS_PACKAGE_LIBRARIES moveit_kinematics_plugin_loader moveit_robot_model_loader moveit_constraint_sampler_manager_loader + planning_pipeline_parameters moveit_planning_pipeline moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager moveit_plan_execution moveit_planning_scene_monitor moveit_collision_plugin_loader + default_plan_request_adapter_parameters moveit_default_planning_request_adapter_plugins moveit_cpp ) @@ -79,19 +82,20 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) -add_subdirectory(rdf_loader) add_subdirectory(collision_plugin_loader) -add_subdirectory(kinematics_plugin_loader) -add_subdirectory(robot_model_loader) add_subdirectory(constraint_sampler_manager_loader) +add_subdirectory(introspection) +add_subdirectory(kinematics_plugin_loader) +add_subdirectory(moveit_cpp) +add_subdirectory(plan_execution) +add_subdirectory(planning_components_tools) add_subdirectory(planning_pipeline) add_subdirectory(planning_pipeline_interfaces) add_subdirectory(planning_request_adapter_plugins) add_subdirectory(planning_scene_monitor) -add_subdirectory(planning_components_tools) +add_subdirectory(rdf_loader) +add_subdirectory(robot_model_loader) add_subdirectory(trajectory_execution_manager) -add_subdirectory(plan_execution) -add_subdirectory(moveit_cpp) install( TARGETS ${THIS_PACKAGE_LIBRARIES} diff --git a/moveit_ros/planning/introspection/CMakeLists.txt b/moveit_ros/planning/introspection/CMakeLists.txt new file mode 100644 index 0000000000..23b5640b15 --- /dev/null +++ b/moveit_ros/planning/introspection/CMakeLists.txt @@ -0,0 +1,10 @@ +add_executable(moveit_list_planning_adapter_plugins src/list_planning_adapter_plugins.cpp) +ament_target_dependencies(moveit_list_planning_adapter_plugins + moveit_core + rclcpp + pluginlib +) + +install(TARGETS moveit_list_planning_adapter_plugins + DESTINATION lib/${PROJECT_NAME} +) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/list.cpp b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp similarity index 94% rename from moveit_ros/planning/planning_request_adapter_plugins/src/list.cpp rename to moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp index 9eb674ed78..30a1793966 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/list.cpp +++ b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp @@ -32,7 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan + * Desc: Simple executable to list the loadable PlanningRequestAdapter. To use it simply run: + * `ros2 run moveit_ros_planning moveit_list_planning_adapter_plugins` + */ #include #include diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index d7a498a280..0546581165 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -140,7 +140,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl // just to be sure, do not call the same pluginlib instance allocation function in parallel std::scoped_lock slock(lock_); - for (auto const& [group, solver] : possible_kinematics_solvers_) + for (const auto& [group, solver] : possible_kinematics_solvers_) { // Don't bother trying to load a solver for the wrong group if (group != jmg->getName()) @@ -206,7 +206,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl void status() const { - for (auto const& [group, solver] : possible_kinematics_solvers_) + for (const auto& [group, solver] : possible_kinematics_solvers_) { RCLCPP_INFO(LOGGER, "Solver for group '%s': '%s' (search resolution = %lf)", group.c_str(), solver.c_str(), search_res_.at(group)); diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 41da2710bd..a9d6fe3f9d 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -108,17 +108,7 @@ class MoveItCpp }; /** \brief Constructor */ - [[deprecated("Passing tf2_ros::Buffer to MoveItCpp's constructor is deprecated")]] MoveItCpp( - const rclcpp::Node::SharedPtr& node, const std::shared_ptr& /* unused */) - : MoveItCpp(node) - { - } MoveItCpp(const rclcpp::Node::SharedPtr& node); - [[deprecated("Passing tf2_ros::Buffer to MoveItCpp's constructor is deprecated")]] MoveItCpp( - const rclcpp::Node::SharedPtr& node, const Options& options, const std::shared_ptr& /* unused */) - : MoveItCpp(node, options) - { - } MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options); /** @@ -200,12 +190,3 @@ class MoveItCpp bool loadPlanningPipelines(const PlanningPipelineOptions& options); }; } // namespace moveit_cpp - -namespace moveit -{ -namespace planning_interface -{ -using MoveItCpp [[deprecated("use moveit_cpp")]] = moveit_cpp::MoveItCpp; -[[deprecated("use moveit_cpp")]] MOVEIT_DECLARE_PTR(MoveItCpp, moveit_cpp::MoveItCpp); -} // namespace planning_interface -} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 672e51b934..f8ef3b7fbd 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -114,12 +114,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/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index f7101404ba..d161be7f83 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -189,7 +189,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan( planning_scene->setCurrentState(request.start_state); } - auto const motion_plan_response_vector = moveit::planning_pipeline_interfaces::planWithParallelPipelines( + const auto motion_plan_response_vector = moveit::planning_pipeline_interfaces::planWithParallelPipelines( requests, planning_scene, moveit_cpp_->getPlanningPipelines(), stopping_criterion_callback, solution_selection_function); @@ -354,7 +354,7 @@ std::vector<::planning_interface::MotionPlanRequest> PlanningComponent::getMotio { std::vector<::planning_interface::MotionPlanRequest> motion_plan_requests; motion_plan_requests.reserve(multi_pipeline_plan_request_parameters.plan_request_parameter_vector.size()); - for (auto const& plan_request_parameters : multi_pipeline_plan_request_parameters.plan_request_parameter_vector) + for (const auto& plan_request_parameters : multi_pipeline_plan_request_parameters.plan_request_parameter_vector) { motion_plan_requests.push_back(getMotionPlanRequest(plan_request_parameters)); } diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index 37f8ea9b6a..0ae603e953 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning - 2.7.4 + 2.8.0 Planning components of MoveIt that use ROS Henning Kayser Tyler Weaver @@ -41,6 +41,7 @@ tf2_msgs tf2_ros eigen + fmt ament_lint_auto ament_lint_common diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 6be7790b25..76c43223e4 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -1,4 +1,7 @@ +generate_parameter_library(planning_pipeline_parameters res/planning_pipeline_parameters.yaml) + add_library(moveit_planning_pipeline SHARED src/planning_pipeline.cpp) +target_link_libraries(moveit_planning_pipeline planning_pipeline_parameters) include(GenerateExportHeader) generate_export_header(moveit_planning_pipeline) target_include_directories(moveit_planning_pipeline PUBLIC $) diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 89ee21acb4..96c0c0ab21 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -63,114 +63,93 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline public: /** \brief When motion plans are computed and they are supposed to be automatically displayed, they are sent to this * topic (moveit_msgs::msg::DisplauTrajectory) */ - static const std::string DISPLAY_PATH_TOPIC; - + static inline const std::string DISPLAY_PATH_TOPIC = std::string("display_planned_path"); /** \brief When motion planning requests are received and they are supposed to be automatically published, they are * sent to this topic (moveit_msgs::msg::MotionPlanRequest) */ - static const std::string MOTION_PLAN_REQUEST_TOPIC; - + static inline const std::string MOTION_PLAN_REQUEST_TOPIC = std::string("motion_plan_request"); /** \brief When contacts are found in the solution path reported by a planner, they can be published as markers on * this topic (visualization_msgs::MarkerArray) */ - static const std::string MOTION_CONTACTS_TOPIC; + static inline const std::string MOTION_CONTACTS_TOPIC = std::string("display_contacts"); /** \brief Given a robot model (\e model), a node handle (\e pipeline_nh), initialize the planning pipeline. \param model The robot model for which this pipeline is initialized. \param node The ROS node that should be used for reading parameters needed for configuration - \param the parameter namespace where the planner configurations are stored - \param planning_plugin_param_name The name of the ROS parameter under which the name of the planning plugin is - specified - \param adapter_plugins_param_name The name of the ROS parameter under which the names of the request adapter - plugins are specified (plugin names separated by space; order matters) + \param parameter_namespace parameter namespace where the planner configurations are stored */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, - const std::string& planning_plugin_param_name = "planning_plugin", - const std::string& adapter_plugins_param_name = "request_adapters"); + const std::string& parameter_namespace); /** \brief Given a robot model (\e model), a node handle (\e pipeline_nh), initialize the planning pipeline. \param model The robot model for which this pipeline is initialized. \param node The ROS node that should be used for reading parameters needed for configuration - \param the parameter namespace where the planner configurations are stored - \param planning_plugin_name The name of the planning plugin to load - \param adapter_plugins_names The names of the planning request adapter plugins to load + \param parameter_namespace parameter namespace where the planner configurations are stored */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace, const std::string& planning_plugin_name, const std::vector& adapter_plugin_names); - /** \brief Pass a flag telling the pipeline whether or not to publish the computed motion plans on DISPLAY_PATH_TOPIC. - * Default is true. */ - void displayComputedMotionPlans(bool flag); - - /** \brief Pass a flag telling the pipeline whether or not to publish the received motion planning requests on - * MOTION_PLAN_REQUEST_TOPIC. Default is false. */ - void publishReceivedRequests(bool flag); - - /** \brief Pass a flag telling the pipeline whether or not to re-check the solution paths reported by the planner. - * This is true by default. */ - void checkSolutionPaths(bool flag); - - /** \brief Get the flag set by displayComputedMotionPlans() */ - bool getDisplayComputedMotionPlans() const + /* + BEGIN BLOCK OF DEPRECATED FUNCTIONS: TODO(sjahr): Remove after 04/2024 (6 months from merge) + */ + [[deprecated("Use generatePlan or ROS parameter API instead.")]] void displayComputedMotionPlans(bool /*flag*/){}; + [[deprecated("Use generatePlan or ROS parameter API instead.")]] void publishReceivedRequests(bool /*flag*/){}; + [[deprecated("Use generatePlan or ROS parameter API instead.")]] void checkSolutionPaths(bool /*flag*/){}; + [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getDisplayComputedMotionPlans() const { - return display_computed_motion_plans_; + return false; } - - /** \brief Get the flag set by publishReceivedRequests() */ - bool getPublishReceivedRequests() const + [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getPublishReceivedRequests() const { - return publish_received_requests_; + return false; } - - /** \brief Get the flag set by checkSolutionPaths() */ - bool getCheckSolutionPaths() const + [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getCheckSolutionPaths() const { - return check_solution_paths_; + return false; } - - /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any). - \param planning_scene The planning scene where motion planning is to be done - \param req The request for motion planning - \param res The motion planning response */ - bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; + /* + END BLOCK OF DEPRECATED FUNCTIONS + */ /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any). \param planning_scene The planning scene where motion planning is to be done \param req The request for motion planning \param res The motion planning response - \param adapter_added_state_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be considered - invalid in all situations. */ - bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& adapter_added_state_index) const; + \param publish_received_requests Flag indicating whether received requests should be published just before + beginning processing (useful for debugging) + \param check_solution_paths Flag indicating whether the reported plans + should be checked once again, by the planning pipeline itself + \param display_computed_motion_plans Flag indicating + whether motion plans should be published as a moveit_msgs::msg::DisplayTrajectory + */ + [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + const bool publish_received_requests = false, const bool check_solution_paths = true, + const bool display_computed_motion_plans = true) const; /** \brief Request termination, if a generatePlan() function is currently computing plans */ void terminate() const; /** \brief Get the name of the planning plugin used */ - const std::string& getPlannerPluginName() const + [[nodiscard]] const std::string& getPlannerPluginName() const { return planner_plugin_name_; } /** \brief Get the names of the planning request adapter plugins used */ - const std::vector& getAdapterPluginNames() const + [[nodiscard]] const std::vector& getAdapterPluginNames() const { return adapter_plugin_names_; } /** \brief Get the planner manager for the loaded planning plugin */ - const planning_interface::PlannerManagerPtr& getPlannerManager() + [[nodiscard]] const planning_interface::PlannerManagerPtr& getPlannerManager() { return planner_instance_; } /** \brief Get the robot model that this pipeline is using */ - const moveit::core::RobotModelConstPtr& getRobotModel() const + [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -189,13 +168,10 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline std::shared_ptr node_; std::string parameter_namespace_; - /// Flag indicating whether motion plans should be published as a moveit_msgs::msg::DisplayTrajectory - bool display_computed_motion_plans_; + /// Optionally publish motion plans as a moveit_msgs::msg::DisplayTrajectory rclcpp::Publisher::SharedPtr display_path_publisher_; - /// Flag indicating whether received requests should be published just before beginning processing (useful for - /// debugging) - bool publish_received_requests_; + /// Optionally publish the request before beginning processing (useful for debugging) rclcpp::Publisher::SharedPtr received_request_publisher_; std::unique_ptr > planner_plugin_loader_; @@ -208,8 +184,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline moveit::core::RobotModelConstPtr robot_model_; - /// Flag indicating whether the reported plans should be checked once again, by the planning pipeline itself - bool check_solution_paths_; + /// Publish contacts if the generated plans are checked again by the planning pipeline rclcpp::Publisher::SharedPtr contacts_publisher_; }; diff --git a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml new file mode 100644 index 0000000000..f4b582e3ad --- /dev/null +++ b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml @@ -0,0 +1,11 @@ +planning_pipeline_parameters: + planning_plugin: { + type: string, + description: "Name of the planner plugin used by the pipeline", + default_value: "UNKNOWN", + } + request_adapters: { + type: string, + description: "Names of the planning request adapter plugins (plugin names separated by space).", + default_value: "", + } diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index bf4407ff25..24f31c7cb6 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -39,48 +39,32 @@ #include #include #include -#include +#include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); +#include -const std::string planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC = "display_planned_path"; -const std::string planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC = "motion_plan_request"; -const std::string planning_pipeline::PlanningPipeline::MOTION_CONTACTS_TOPIC = "display_contacts"; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); +} // namespace planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, - const std::string& planner_plugin_param_name, - const std::string& adapter_plugins_param_name) - : active_{ false } - , node_(node) - , parameter_namespace_(parameter_namespace) - , display_computed_motion_plans_{ false } - , publish_received_requests_{ false } - , robot_model_(model) - , check_solution_paths_{ false } + const std::string& parameter_namespace) + : active_{ false }, node_(node), parameter_namespace_(parameter_namespace), robot_model_(model) { - std::string planner_plugin_fullname = parameter_namespace_ + "." + planner_plugin_param_name; - if (parameter_namespace_.empty()) - planner_plugin_fullname = planner_plugin_param_name; - if (node_->has_parameter(planner_plugin_fullname)) - { - node_->get_parameter(planner_plugin_fullname, planner_plugin_name_); - } - - std::string adapter_plugins_fullname = parameter_namespace_ + "." + adapter_plugins_param_name; - if (parameter_namespace_.empty()) - adapter_plugins_fullname = adapter_plugins_param_name; - - std::string adapters; - if (node_->has_parameter(adapter_plugins_fullname)) + auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); + const auto params = param_listener.get_params(); + planner_plugin_name_ = params.planning_plugin; + if (!params.request_adapters.empty()) { - node_->get_parameter(adapter_plugins_fullname, adapters); boost::char_separator sep(" "); - boost::tokenizer> tok(adapters, sep); + boost::tokenizer> tok(params.request_adapters, sep); for (boost::tokenizer>::iterator beg = tok.begin(); beg != tok.end(); ++beg) + { adapter_plugin_names_.push_back(*beg); + } } configure(); @@ -94,18 +78,23 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM : active_{ false } , node_(node) , parameter_namespace_(parameter_namespace) - , display_computed_motion_plans_{ false } - , publish_received_requests_{ false } , planner_plugin_name_(planner_plugin_name) , adapter_plugin_names_(adapter_plugin_names) , robot_model_(model) - , check_solution_paths_{ false } { configure(); } void planning_pipeline::PlanningPipeline::configure() { + // Optional publishers for debugging + received_request_publisher_ = node_->create_publisher( + MOTION_PLAN_REQUEST_TOPIC, rclcpp::SystemDefaultsQoS()); + contacts_publisher_ = + node_->create_publisher(MOTION_CONTACTS_TOPIC, rclcpp::SystemDefaultsQoS()); + display_path_publisher_ = + node_->create_publisher(DISPLAY_PATH_TOPIC, rclcpp::SystemDefaultsQoS()); + // load the planning plugin try { @@ -118,15 +107,11 @@ void planning_pipeline::PlanningPipeline::configure() throw; } - std::vector classes; - if (planner_plugin_loader_) - classes = planner_plugin_loader_->getDeclaredClasses(); - - if (planner_plugin_name_.empty()) + if (planner_plugin_name_.empty() || planner_plugin_name_ == "UNKNOWN") { - std::string classes_str = boost::algorithm::join(classes, ", "); - throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " + - classes_str); + std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); + throw std::runtime_error("Planning plugin name is empty or not defined in namespace '" + parameter_namespace_ + + "'. Please choose one of the available plugins: " + classes_str); } try @@ -140,7 +125,7 @@ void planning_pipeline::PlanningPipeline::configure() } catch (pluginlib::PluginlibException& ex) { - std::string classes_str = boost::algorithm::join(classes, ", "); + std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); RCLCPP_FATAL(LOGGER, "Exception while loading planner '%s': %s" "Available plugins: %s", @@ -151,7 +136,7 @@ void planning_pipeline::PlanningPipeline::configure() // load the planner request adapters if (!adapter_plugin_names_.empty()) { - std::vector ads; + std::vector planning_request_adapter_vector; try { adapter_plugin_loader_ = @@ -168,10 +153,10 @@ void planning_pipeline::PlanningPipeline::configure() { for (const std::string& adapter_plugin_name : adapter_plugin_names_) { - planning_request_adapter::PlanningRequestAdapterPtr ad; + planning_request_adapter::PlanningRequestAdapterPtr planning_request_adapter; try { - ad = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name); + planning_request_adapter = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name); } catch (pluginlib::PluginlibException& ex) { @@ -179,20 +164,21 @@ void planning_pipeline::PlanningPipeline::configure() ex.what()); throw; } - if (ad) + if (planning_request_adapter) { - ad->initialize(node_, parameter_namespace_); - ads.push_back(std::move(ad)); + planning_request_adapter->initialize(node_, parameter_namespace_); + planning_request_adapter_vector.push_back(std::move(planning_request_adapter)); } } } - if (!ads.empty()) + if (!planning_request_adapter_vector.empty()) { adapter_chain_ = std::make_unique(); - for (planning_request_adapter::PlanningRequestAdapterConstPtr& ad : ads) + for (planning_request_adapter::PlanningRequestAdapterConstPtr& planning_request_adapter : + planning_request_adapter_vector) { - RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", ad->getDescription().c_str()); - adapter_chain_->addAdapter(ad); + RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", planning_request_adapter->getDescription().c_str()); + adapter_chain_->addAdapter(planning_request_adapter); } } } @@ -200,92 +186,39 @@ void planning_pipeline::PlanningPipeline::configure() { RCLCPP_WARN(LOGGER, "No planning request adapter names specified."); } - displayComputedMotionPlans(true); - checkSolutionPaths(true); -} - -void planning_pipeline::PlanningPipeline::displayComputedMotionPlans(bool flag) -{ - if (display_computed_motion_plans_ && !flag) - { - display_path_publisher_.reset(); - } - else if (!display_computed_motion_plans_ && flag) - { - display_path_publisher_ = node_->create_publisher(DISPLAY_PATH_TOPIC, 10); - } - display_computed_motion_plans_ = flag; -} - -void planning_pipeline::PlanningPipeline::publishReceivedRequests(bool flag) -{ - if (publish_received_requests_ && !flag) - { - received_request_publisher_.reset(); - } - else if (!publish_received_requests_ && flag) - { - received_request_publisher_ = - node_->create_publisher(MOTION_PLAN_REQUEST_TOPIC, 10); - } - publish_received_requests_ = flag; -} - -void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag) -{ - if (check_solution_paths_ && !flag) - { - contacts_publisher_.reset(); - } - else if (!check_solution_paths_ && flag) - { - contacts_publisher_ = node_->create_publisher(MOTION_CONTACTS_TOPIC, 10); - } - check_solution_paths_ = flag; -} - -bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - std::vector dummy; - return generatePlan(planning_scene, req, res, dummy); } bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& adapter_added_state_index) const + const bool publish_received_requests, + const bool check_solution_paths, + const bool display_computed_motion_plans) const { + assert(planner_instance_ != nullptr); + // Set planning pipeline active active_ = true; // broadcast the request we are about to work on, if needed - if (publish_received_requests_) + if (publish_received_requests) { received_request_publisher_->publish(req); } - adapter_added_state_index.clear(); - - if (!planner_instance_) - { - RCLCPP_ERROR(LOGGER, "No planning plugin loaded. Cannot plan."); - // Set planning pipeline to inactive - - active_ = false; - return false; - } + // --------------------------------- + // Solve the motion planning problem + // --------------------------------- bool solved = false; try { if (adapter_chain_) { - solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index); - if (!adapter_added_state_index.empty()) + solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res); + if (!res.added_path_index.empty()) { std::stringstream ss; - for (std::size_t added_index : adapter_added_state_index) + for (std::size_t added_index : res.added_path_index) ss << added_index << ' '; RCLCPP_INFO(LOGGER, "Planning adapters have added states at index positions: [ %s]", ss.str().c_str()); } @@ -301,58 +234,63 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla { RCLCPP_ERROR(LOGGER, "Exception caught: '%s'", ex.what()); // Set planning pipeline to inactive - active_ = false; return false; } - bool valid = true; + // ----------------- + // Validate solution + // ----------------- if (solved && res.trajectory) { std::size_t state_count = res.trajectory->getWayPointCount(); RCLCPP_DEBUG(LOGGER, "Motion planner reported a solution path with %ld states", state_count); - if (check_solution_paths_) + if (check_solution_paths) { visualization_msgs::msg::MarkerArray arr; visualization_msgs::msg::Marker m; m.action = visualization_msgs::msg::Marker::DELETEALL; arr.markers.push_back(m); - std::vector index; - if (!planning_scene->isPathValid(*res.trajectory, req.path_constraints, req.group_name, false, &index)) + std::vector indices; + if (!planning_scene->isPathValid(*res.trajectory, req.path_constraints, req.group_name, false, &indices)) { // check to see if there is any problem with the states that are found to be invalid // they are considered ok if they were added by a planning request adapter bool problem = false; - for (std::size_t i = 0; i < index.size() && !problem; ++i) + for (const auto& index : indices) { bool found = false; - for (std::size_t added_index : adapter_added_state_index) + for (const std::size_t& added_index : res.added_path_index) { - if (index[i] == added_index) + if (index == added_index) { found = true; break; } } if (!found) + { problem = true; + break; + } } if (problem) { - if (index.size() == 1 && index[0] == 0) + if (indices.size() == 1 && indices.at(0) == 0) { // ignore cases when the robot starts at invalid location RCLCPP_DEBUG(LOGGER, "It appears the robot is starting at an invalid state, but that is ok."); } else { - valid = false; res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; // display error messages std::stringstream ss; - for (std::size_t it : index) + for (std::size_t it : indices) + { ss << it << ' '; + } RCLCPP_ERROR_STREAM(LOGGER, "Computed path is not valid. Invalid states at index locations: [ " << ss.str() << "] out of " << state_count @@ -360,7 +298,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla << contacts_publisher_->get_topic_name()); // call validity checks in verbose mode for the problematic states - for (std::size_t it : index) + for (std::size_t it : indices) { // check validity with verbose on const moveit::core::RobotState& robot_state = res.trajectory->getWayPoint(it); @@ -396,30 +334,33 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla RCLCPP_DEBUG(LOGGER, "Planned path was found to be valid when rechecked"); contacts_publisher_->publish(arr); } - } - // display solution path if needed - if (display_computed_motion_plans_ && solved) - { - moveit_msgs::msg::DisplayTrajectory disp; - disp.model_id = robot_model_->getName(); - disp.trajectory.resize(1); - res.trajectory->getRobotTrajectoryMsg(disp.trajectory[0]); - moveit::core::robotStateToRobotStateMsg(res.trajectory->getFirstWayPoint(), disp.trajectory_start); - display_path_publisher_->publish(disp); + // Optionally publish DisplayTrajectory msg + if (display_computed_motion_plans) + { + moveit_msgs::msg::DisplayTrajectory disp; + disp.model_id = robot_model_->getName(); + disp.trajectory.resize(1); + res.trajectory->getRobotTrajectoryMsg(disp.trajectory.at(0)); + moveit::core::robotStateToRobotStateMsg(res.trajectory->getFirstWayPoint(), disp.trajectory_start); + display_path_publisher_->publish(disp); + } } - - if (!solved) + else // If no trajectory exists, let's see if it might be related to stacked constraints { // This should alert the user if planning failed because of contradicting constraints. // Could be checked more thoroughly, but it is probably not worth going to that length. bool stacked_constraints = false; if (req.path_constraints.position_constraints.size() > 1 || req.path_constraints.orientation_constraints.size() > 1) + { stacked_constraints = true; + } for (const auto& constraint : req.goal_constraints) { if (constraint.position_constraints.size() > 1 || constraint.orientation_constraints.size() > 1) + { stacked_constraints = true; + } } if (stacked_constraints) { @@ -430,7 +371,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } } - // Make sure that planner id is set + // Make sure that planner id is set in the response if (res.planner_id.empty()) { RCLCPP_WARN(LOGGER, "The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting " @@ -441,7 +382,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla // Set planning pipeline to inactive active_ = false; - return solved && valid; + return solved && bool(res); } void planning_pipeline::PlanningPipeline::terminate() const diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp index e4b0ec11e8..21e3763351 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -111,8 +111,6 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe std::unordered_map createPlanningPipelineMap(const std::vector& pipeline_names, const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace = std::string(), - const std::string& planning_plugin_param_name = "planning_plugin", - const std::string& adapter_plugins_param_name = "request_adapters"); + const std::string& parameter_namespace = std::string()); } // namespace planning_pipeline_interfaces } // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index d689397f44..af37378281 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -58,7 +58,10 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla return motion_plan_response; } const planning_pipeline::PlanningPipelinePtr pipeline = it->second; - pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response); + if (!pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response)) + { + motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; + } return motion_plan_response; } @@ -76,7 +79,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe // Print a warning if more parallel planning problems than available concurrent threads are defined. If // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work - auto const hardware_concurrency = std::thread::hardware_concurrency(); + const auto hardware_concurrency = std::thread::hardware_concurrency(); if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) { RCLCPP_WARN(LOGGER, @@ -157,8 +160,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe std::unordered_map createPlanningPipelineMap(const std::vector& pipeline_names, const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace, const std::string& planning_plugin_param_name, - const std::string& adapter_plugins_param_name) + const std::string& parameter_namespace) { std::unordered_map planning_pipelines; for (const auto& planning_pipeline_name : pipeline_names) @@ -171,10 +173,8 @@ createPlanningPipelineMap(const std::vector& pipeline_names, } // Create planning pipeline - planning_pipeline::PlanningPipelinePtr pipeline = - std::make_shared(robot_model, node, - parameter_namespace + planning_pipeline_name, - planning_plugin_param_name, adapter_plugins_param_name); + planning_pipeline::PlanningPipelinePtr pipeline = std::make_shared( + robot_model, node, parameter_namespace + planning_pipeline_name); if (!pipeline->getPlannerManager()) { diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp index f3fc96de98..121e2b4c52 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp @@ -44,7 +44,7 @@ ::planning_interface::MotionPlanResponse getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse>& solutions) { // Find trajectory with minimal path - auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), + const auto shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), [](const ::planning_interface::MotionPlanResponse& solution_a, const ::planning_interface::MotionPlanResponse& solution_b) { // If both solutions were successful, check which path is shorter diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp index f0bf424cee..8bfe9add7d 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp @@ -44,7 +44,7 @@ bool stopAtFirstSolution(const PlanResponsesContainer& plan_responses_container, const std::vector<::planning_interface::MotionPlanRequest>& /*plan_requests*/) { // Stop at the first successful plan - for (auto const& solution : plan_responses_container.getSolutions()) + for (const auto& solution : plan_responses_container.getSolutions()) { // bool(solution) is shorthand to evaluate the error code of the solution, checking for SUCCESS if (bool(solution)) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 277e653311..bb046af912 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -1,4 +1,6 @@ -set(SOURCE_FILES +generate_parameter_library(default_plan_request_adapter_parameters res/default_plan_request_adapter_params.yaml) + +add_library(moveit_default_planning_request_adapter_plugins SHARED src/empty.cpp src/fix_start_state_bounds.cpp src/fix_start_state_collision.cpp @@ -9,7 +11,7 @@ set(SOURCE_FILES src/resolve_constraint_frames.cpp ) -add_library(moveit_default_planning_request_adapter_plugins SHARED ${SOURCE_FILES}) +target_link_libraries(moveit_default_planning_request_adapter_plugins default_plan_request_adapter_parameters) set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_default_planning_request_adapter_plugins @@ -18,15 +20,3 @@ ament_target_dependencies(moveit_default_planning_request_adapter_plugins rclcpp pluginlib ) - -add_executable(moveit_list_request_adapter_plugins src/list.cpp) -ament_target_dependencies(moveit_list_request_adapter_plugins - Boost - moveit_core - rclcpp - pluginlib -) - -install(TARGETS moveit_list_request_adapter_plugins - DESTINATION lib/${PROJECT_NAME} -) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml new file mode 100644 index 0000000000..b3f4d79b1b --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml @@ -0,0 +1,45 @@ +# This files contains the parameters for all of MoveIt's default PlanRequestAdapters +default_plan_request_adapter_parameters: + path_tolerance: { + type: double, + description: "AddTimeOptimalParameterization: Tolerance per joint in which the time parameterization is allowed to deviate from the original path.", + default_value: 0.1, + } + resample_dt: { + type: double, + description: "AddTimeOptimalParameterization: Time steps between two adjacent waypoints of the parameterized trajectory. The trajectory is re-sampled from the original path.", + default_value: 0.1, + } + min_angle_change: { + type: double, + description: "AddTimeOptimalParameterization: Minimum joint value change to consider two waypoints unique.", + default_value: 0.001, + } + start_state_max_dt: { + type: double, + description: "FixStartStateCollision/FixStartStateBounds: Maximum temporal distance of the fixed start state from the original state.", + default_value: 0.5, + } + jiggle_fraction: { + type: double, + description: "FixStartStateCollision: Joint value perturbation as a percentage of the total range of motion for the joint.", + default_value: 0.02, + } + max_sampling_attempts: { + type: int, + description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.", + default_value: 100, + validation: { + gt_eq<>: [ 0 ], + } + } + start_state_max_bounds_error: { + type: double, + description: "FixStartStateBounds: Maximum allowable error outside joint bounds for the starting configuration.", + default_value: 0.05, + } + default_workspace_bounds: { + type: double, + description: "FixWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", + default_value: 10.0, + } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index b27a8203c4..2e3638397b 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -52,10 +52,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_traj_smo class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRequestAdapter { public: - AddRuckigTrajectorySmoothing() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& /* unused */, const std::string& /* unused */) override { } @@ -66,8 +62,8 @@ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRe } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 6ee7c21974..677494212c 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -38,6 +38,8 @@ #include #include +#include + namespace default_planner_request_adapters { using namespace trajectory_processing; @@ -48,15 +50,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_time_opt class AddTimeOptimalParameterization : public planning_request_adapter::PlanningRequestAdapter { public: - AddTimeOptimalParameterization() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - path_tolerance_ = getParam(node, LOGGER, parameter_namespace, "path_tolerance", 0.1); - resample_dt_ = getParam(node, LOGGER, parameter_namespace, "resample_dt", 0.1); - min_angle_change_ = getParam(node, LOGGER, parameter_namespace, "min_angle_change", 0.001); + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -65,14 +62,15 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory) { RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str()); - TimeOptimalTrajectoryGeneration totg(path_tolerance_, resample_dt_, min_angle_change_); + const auto params = param_listener_->get_params(); + TimeOptimalTrajectoryGeneration totg(params.path_tolerance, params.resample_dt, params.min_angle_change); if (!totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed."); @@ -84,9 +82,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning } protected: - double path_tolerance_; - double resample_dt_; - double min_angle_change_; + std::unique_ptr param_listener_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp index de08753980..577a995962 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp @@ -51,8 +51,8 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { return planner(planning_scene, req, res); } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index 9a8f572e83..ae755e8320 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -53,6 +53,8 @@ #include #include +#include + namespace default_planner_request_adapters { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_bounds"); @@ -61,14 +63,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdapter { public: - static const std::string BOUNDS_PARAM_NAME; - static const std::string DT_PARAM_NAME; - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - node_ = node; - bounds_dist_ = getParam(node_, LOGGER, parameter_namespace, BOUNDS_PARAM_NAME, 0.05); - max_dt_offset_ = getParam(node_, LOGGER, parameter_namespace, DT_PARAM_NAME, 0.5); + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -77,8 +75,8 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -137,13 +135,16 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } } + // Read parameters + const auto params = param_listener_->get_params(); + // pointer to a prefix state we could possibly add, if we detect we have to make changes moveit::core::RobotStatePtr prefix_state; for (const moveit::core::JointModel* jmodel : jmodels) { if (!start_state.satisfiesBounds(jmodel)) { - if (start_state.satisfiesBounds(jmodel, bounds_dist_)) + if (start_state.satisfiesBounds(jmodel, params.start_state_max_bounds_error)) { if (!prefix_state) prefix_state = std::make_shared(start_state); @@ -168,9 +169,10 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } RCLCPP_WARN(LOGGER, "Joint '%s' from the starting state is outside bounds by a significant margin: [%s] should be in " - "the range [%s], [%s] but the error above the ~%s parameter (currently set to %f)", + "the range [%s], [%s] but the error above the 'start_state_max_bounds_error' parameter " + "(currently set to %f)", jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(), - joint_bounds_hi.str().c_str(), BOUNDS_PARAM_NAME.c_str(), bounds_dist_); + joint_bounds_hi.str().c_str(), params.start_state_max_bounds_error); } } } @@ -191,26 +193,23 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to // the computed trajectory) - res.trajectory->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory->setWayPointDurationFromPrevious(0, std::min(params.start_state_max_dt, res.trajectory->getAverageSegmentDuration())); res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions - for (std::size_t& added_index : added_path_index) + for (std::size_t& added_index : res.added_path_index) + { added_index++; - added_path_index.push_back(0); + } + res.added_path_index.push_back(0); } return solved; } private: - rclcpp::Node::SharedPtr node_; - double bounds_dist_; - double max_dt_offset_; + std::unique_ptr param_listener_; }; - -const std::string FixStartStateBounds::BOUNDS_PARAM_NAME = "start_state_max_bounds_error"; -const std::string FixStartStateBounds::DT_PARAM_NAME = "start_state_max_dt"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateBounds, diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index d01b0c0587..b58196d059 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -49,6 +49,8 @@ #include #include +#include + namespace default_planner_request_adapters { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_collision"); @@ -58,21 +60,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStateCollision : public planning_request_adapter::PlanningRequestAdapter { public: - static const std::string DT_PARAM_NAME; - static const std::string JIGGLE_PARAM_NAME; - static const std::string ATTEMPTS_PARAM_NAME; - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - node_ = node; - max_dt_offset_ = getParam(node_, LOGGER, parameter_namespace, DT_PARAM_NAME, 0.5); - jiggle_fraction_ = getParam(node_, LOGGER, parameter_namespace, JIGGLE_PARAM_NAME, 0.02); - sampling_attempts_ = getParam(node_, LOGGER, parameter_namespace, ATTEMPTS_PARAM_NAME, 100); - if (sampling_attempts_ < 1) - { - sampling_attempts_ = 1; - RCLCPP_WARN(LOGGER, "Param '%s' needs to be at least 1.", ATTEMPTS_PARAM_NAME.c_str()); - } + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -81,8 +72,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -120,14 +111,16 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA planning_scene->getRobotModel()->getJointModels(); bool found = false; - for (int c = 0; !found && c < sampling_attempts_; ++c) + const auto params = param_listener_->get_params(); + + for (int c = 0; !found && c < params.max_sampling_attempts; ++c) { for (std::size_t i = 0; !found && i < jmodels.size(); ++i) { std::vector sampled_variable_values(jmodels[i]->getVariableCount()); const double* original_values = prefix_state->getJointPositions(jmodels[i]); jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, - jmodels[i]->getMaximumExtent() * jiggle_fraction_); + jmodels[i]->getMaximumExtent() * params.jiggle_fraction); start_state.setJointPositions(jmodels[i], sampled_variable_values); collision_detection::CollisionResult cres; planning_scene->checkCollision(creq, cres, start_state); @@ -149,22 +142,25 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a // prefix to the computed trajectory) - res.trajectory->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory->setWayPointDurationFromPrevious(0, std::min(params.start_state_max_dt, res.trajectory->getAverageSegmentDuration())); res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions - for (std::size_t& added_index : added_path_index) + for (std::size_t& added_index : res.added_path_index) + { added_index++; - added_path_index.push_back(0); + } + res.added_path_index.push_back(0); } return solved; } else { - RCLCPP_WARN(LOGGER, - "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling " - "attempts). Passing the original planning request to the planner.", - jiggle_fraction_, sampling_attempts_); + RCLCPP_WARN( + LOGGER, + "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %lu sampling " + "attempts). Passing the original planning request to the planner.", + params.jiggle_fraction, params.max_sampling_attempts); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; return false; // skip remaining adapters and/or planner } @@ -184,15 +180,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA } private: - rclcpp::Node::SharedPtr node_; - double max_dt_offset_; - double jiggle_fraction_; - int sampling_attempts_; + std::unique_ptr param_listener_; }; - -const std::string FixStartStateCollision::DT_PARAM_NAME = "start_state_max_dt"; -const std::string FixStartStateCollision::JIGGLE_PARAM_NAME = "jiggle_fraction"; -const std::string FixStartStateCollision::ATTEMPTS_PARAM_NAME = "max_sampling_attempts"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateCollision, diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index b94712847d..3453dd0ec1 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -59,10 +59,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter { public: - FixStartStatePathConstraints() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -73,8 +69,8 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -98,9 +94,9 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe // we call the planner for this additional request, but we do not want to include potential // index information from that call std::vector added_path_index_temp; - added_path_index_temp.swap(added_path_index); + added_path_index_temp.swap(res.added_path_index); bool solved1 = planner(planning_scene, req2, res2); - added_path_index_temp.swap(added_path_index); + added_path_index_temp.swap(res.added_path_index); if (solved1) { @@ -115,12 +111,16 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe if (solved2) { // since we add a prefix, we need to correct any existing index positions - for (std::size_t& added_index : added_path_index) + for (std::size_t& added_index : res.added_path_index) + { added_index += res2.trajectory->getWayPointCount(); + } // we mark the fact we insert a prefix path (we specify the index position we just added) for (std::size_t i = 0; i < res2.trajectory->getWayPointCount(); ++i) - added_path_index.push_back(i); + { + res.added_path_index.push_back(i); + } // we need to append the solution paths. res2.trajectory->append(*res.trajectory, 0.0); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index eab718442a..72c1bd924d 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -42,7 +42,8 @@ #include #include #include -#include + +#include namespace default_planner_request_adapters { @@ -53,13 +54,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_workspac class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapter { public: - static const std::string WBOUNDS_PARAM_NAME; - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - node_ = node; - workspace_extent_ = getParam(node_, LOGGER, parameter_namespace, WBOUNDS_PARAM_NAME, 10.0); - workspace_extent_ /= 2.0; + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -68,8 +66,8 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); const moveit_msgs::msg::WorkspaceParameters& wparams = req.workspace_parameters; @@ -80,8 +78,12 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt RCLCPP_DEBUG(LOGGER, "It looks like the planning volume was not specified. Using default values."); planning_interface::MotionPlanRequest req2 = req; moveit_msgs::msg::WorkspaceParameters& default_wp = req2.workspace_parameters; - default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = -workspace_extent_; - default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = workspace_extent_; + const auto params = param_listener_->get_params(); + + default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = + -params.default_workspace_bounds / 2.0; + default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = + params.default_workspace_bounds / 2.0; return planner(planning_scene, req2, res); } else @@ -91,11 +93,8 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt } private: - rclcpp::Node::SharedPtr node_; - double workspace_extent_; + std::unique_ptr param_listener_; }; - -const std::string FixWorkspaceBounds::WBOUNDS_PARAM_NAME = "default_workspace_bounds"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds, diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index b8c82c2b05..212c69db8a 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -45,10 +45,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.resolve_cons class ResolveConstraintFrames : public planning_request_adapter::PlanningRequestAdapter { public: - ResolveConstraintFrames() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -59,14 +55,16 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); planning_interface::MotionPlanRequest modified = req; kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints); for (moveit_msgs::msg::Constraints& constraint : modified.goal_constraints) + { kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); + } return planner(planning_scene, modified, res); } }; diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index b64f83b5e3..b25347f88e 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -15,7 +15,7 @@ ament_target_dependencies(moveit_planning_scene_monitor urdf pluginlib rclcpp - Boost + fmt moveit_msgs ) target_link_libraries(moveit_planning_scene_monitor @@ -27,7 +27,7 @@ add_executable(demo_scene demos/demo_scene.cpp) ament_target_dependencies(demo_scene PUBLIC rclcpp - Boost + fmt moveit_msgs urdf message_filters diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index b6f241640c..b9a7a04415 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -118,61 +118,31 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor /** @brief Constructor * @param robot_description The name of the ROS parameter that contains the URDF (in string format) - * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ - [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( - const rclcpp::Node::SharedPtr& node, const std::string& robot_description, - const std::shared_ptr& /* unused */, const std::string& name = "") - : PlanningSceneMonitor(node, robot_description, name) - { - } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, const std::string& name = ""); /** @brief Constructor * @param rml A pointer to a kinematic model loader - * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ - [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( - const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml, - const std::shared_ptr& /* unused */, const std::string& name = "") - : PlanningSceneMonitor(node, rml, name) - { - } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml, const std::string& name = ""); /** @brief Constructor * @param scene The scene instance to maintain up to date with monitored information * @param robot_description The name of the ROS parameter that contains the URDF (in string format) - * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ - [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( - const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const std::string& robot_description, const std::shared_ptr& /* unused */, - const std::string& name = "") - : PlanningSceneMonitor(node, scene, robot_description, name) - { - } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, const std::string& robot_description, const std::string& name = ""); /** @brief Constructor * @param scene The scene instance to maintain up to date with monitored information * @param rml A pointer to a kinematic model loader - * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ - [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( - const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const robot_model_loader::RobotModelLoaderPtr& rml, const std::shared_ptr& /* unused */, - const std::string& name = "") - : PlanningSceneMonitor(node, scene, rml, name) - { - } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rml, const std::string& name = ""); diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp index feeda9baca..33f18180de 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp @@ -56,10 +56,6 @@ namespace planning_scene_monitor { -namespace -{ -static const auto SUBSCRIPTION_QOS = rclcpp::QoS(25); -} CurrentStateMonitorMiddlewareHandle::CurrentStateMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node) : node_(node) @@ -75,7 +71,7 @@ void CurrentStateMonitorMiddlewareHandle::createJointStateSubscription(const std JointStateUpdateCallback callback) { joint_state_subscription_ = - node_->create_subscription(topic, SUBSCRIPTION_QOS, callback); + node_->create_subscription(topic, rclcpp::SystemDefaultsQoS(), callback); } void CurrentStateMonitorMiddlewareHandle::resetJointStateSubscription() 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 63c5c161fc..4e53f8756f 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 @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include @@ -233,17 +233,44 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc try { + bool publish_planning_scene = false; + bool publish_geometry_updates = false; + bool publish_state_updates = false; + bool publish_transform_updates = false; + double publish_planning_scene_hz = 4.0; + if (node_->has_parameter("publish_planning_scene")) + { + publish_planning_scene = node_->get_parameter("publish_planning_scene").as_bool(); + } + if (node_->has_parameter("publish_geometry_updates")) + { + publish_geometry_updates = node_->get_parameter("publish_geometry_updates").as_bool(); + } + if (node_->has_parameter("publish_state_updates")) + { + publish_state_updates = node_->get_parameter("publish_state_updates").as_bool(); + } + if (node_->has_parameter("publish_transforms_updates")) + { + publish_transform_updates = node_->get_parameter("publish_transforms_updates").as_bool(); + } + if (node_->has_parameter("publish_planning_scene_hz")) + { + publish_planning_scene_hz = node_->get_parameter("publish_planning_scene_hz").as_double(); + } + // Set up publishing parameters - bool publish_planning_scene = - declare_parameter("publish_planning_scene", false, "Set to True to publish Planning Scenes"); - bool publish_geometry_updates = declare_parameter("publish_geometry_updates", false, - "Set to True to publish geometry updates of the planning scene"); - bool publish_state_updates = - declare_parameter("publish_state_updates", false, "Set to True to publish state updates of the planning scene"); - bool publish_transform_updates = declare_parameter( - "publish_transforms_updates", false, "Set to True to publish transform updates of the planning scene"); - double publish_planning_scene_hz = declare_parameter( - "publish_planning_scene_hz", 4.0, "Set the maximum frequency at which planning scene updates are published"); + publish_planning_scene = + declare_parameter("publish_planning_scene", publish_planning_scene, "Set to True to publish Planning Scenes"); + publish_geometry_updates = declare_parameter("publish_geometry_updates", publish_geometry_updates, + "Set to True to publish geometry updates of the planning scene"); + publish_state_updates = declare_parameter("publish_state_updates", publish_state_updates, + "Set to True to publish state updates of the planning scene"); + publish_transform_updates = declare_parameter("publish_transforms_updates", publish_transform_updates, + "Set to True to publish transform updates of the planning scene"); + publish_planning_scene_hz = + declare_parameter("publish_planning_scene_hz", publish_planning_scene_hz, + "Set the maximum frequency at which planning scene updates are published"); updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates, publish_planning_scene, publish_planning_scene_hz); } @@ -1128,7 +1155,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic) if (!scene_topic.empty()) { planning_scene_subscriber_ = pnode_->create_subscription( - scene_topic, 100, [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { + scene_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { return newPlanningSceneCallback(scene); }); RCLCPP_INFO(LOGGER, "Listening to '%s'", planning_scene_subscriber_->get_topic_name()); @@ -1225,7 +1252,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio if (!collision_objects_topic.empty()) { collision_object_subscriber_ = pnode_->create_subscription( - collision_objects_topic, 1024, + collision_objects_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); }); RCLCPP_INFO(LOGGER, "Listening to '%s'", collision_objects_topic.c_str()); } @@ -1233,7 +1260,8 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio if (!planning_scene_world_topic.empty()) { planning_scene_world_subscriber_ = pnode_->create_subscription( - planning_scene_world_topic, 1, [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { + planning_scene_world_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { return newPlanningSceneWorldCallback(world); }); RCLCPP_INFO(LOGGER, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str()); @@ -1306,7 +1334,8 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top { // using regular message filter as there's no header attached_collision_object_subscriber_ = pnode_->create_subscription( - attached_objects_topic, 1024, [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { + attached_objects_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { return attachObjectCallback(obj); }); RCLCPP_INFO(LOGGER, "Listening to '%s' for attached collision objects", @@ -1452,7 +1481,7 @@ void PlanningSceneMonitor::updateSceneWithCurrentState() if (!current_state_monitor_->haveCompleteState(missing) && (time - current_state_monitor_->getMonitorStartTime()).seconds() > 1.0) { - std::string missing_str = boost::algorithm::join(missing, ", "); + std::string missing_str = fmt::format("{}", fmt::join(missing, ", ")); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 1000, "The complete state of the robot is not yet known. Missing %s", 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/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp index 623214aadf..fa36d0b5ab 100644 --- a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp +++ b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp @@ -120,10 +120,11 @@ bool SynchronizedStringParameter::shouldPublish() bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration& timeout) { - auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); - auto const temp_node = std::make_shared(nd_name); + const auto nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); + const auto temp_node = std::make_shared(nd_name); string_subscriber_ = temp_node->create_subscription( - name_, rclcpp::QoS(1).transient_local().reliable(), + name_, + rclcpp::QoS(1).transient_local().reliable(), // "transient_local()" is required for supporting late subscriptions [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return stringCallback(msg); }); rclcpp::WaitSet wait_set; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 03f13a3b46..b4b89168ff 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -186,7 +186,7 @@ void TrajectoryExecutionManager::initialize() auto options = rclcpp::SubscriptionOptions(); options.callback_group = callback_group; event_topic_subscriber_ = node_->create_subscription( - EXECUTION_EVENT_TOPIC, 100, + EXECUTION_EVENT_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::String::ConstSharedPtr& event) { return receiveEvent(event); }, options); controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring", @@ -539,9 +539,9 @@ struct OrderPotentialControllerCombination return false; // and then to active ones - if (nractive[a] < nractive[b]) - return true; if (nractive[a] > nractive[b]) + return true; + if (nractive[a] < nractive[b]) return false; return false; diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index bdc4068476..a8277957e2 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Specify controller name in MGI execution (`#2257 `_) + * Specify controller name in MGI execute + * Finish comment + --------- + Co-authored-by: Sebastian Jahr +* Remove deprecated move_group pick& place functionality + demo (`#2239 `_) + Co-authored-by: Jafar Uruç +* Contributors: Sebastian Jahr, Stephanie Eng + 2.7.4 (2023-05-18) ------------------ * Scale acceleration and velocity of cartesian interpolations (`#1968 `_) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index f4715d36b2..ec50305b17 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -64,8 +64,6 @@ namespace moveit /** \brief Simple interface to MoveIt components */ namespace planning_interface { -using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; - MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc /** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h @@ -502,9 +500,6 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Get the current joint state goal in a form compatible to setJointValueTarget() */ void getJointValueTarget(std::vector& group_variable_values) const; - /// Get the currently set joint state goal, replaced by private getTargetRobotState() - [[deprecated]] const moveit::core::RobotState& getJointValueTarget() const; - /**@}*/ /** @@ -715,17 +710,41 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface target. No execution is performed. The resulting plan is stored in \e plan*/ moveit::core::MoveItErrorCode plan(Plan& plan); - /** \brief Given a \e plan, execute it without waiting for completion. */ - moveit::core::MoveItErrorCode asyncExecute(const Plan& plan); - - /** \brief Given a \e robot trajectory, execute it without waiting for completion. */ - moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory); - - /** \brief Given a \e plan, execute it while waiting for completion. */ - moveit::core::MoveItErrorCode execute(const Plan& plan); - - /** \brief Given a \e robot trajectory, execute it while waiting for completion. */ - moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory); + /** \brief Given a \e plan, execute it without waiting for completion. + * \param [in] plan The motion plan for which to execute + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode asyncExecute(const Plan& plan, + const std::vector& controllers = std::vector()); + + /** \brief Given a \e robot trajectory, execute it without waiting for completion. + * \param [in] trajectory The trajectory to execute + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers = std::vector()); + + /** \brief Given a \e plan, execute it while waiting for completion. + * \param [in] plan Contains trajectory info as well as metadata such as a RobotModel. + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode execute(const Plan& plan, + const std::vector& controllers = std::vector()); + + /** \brief Given a \e robot trajectory, execute it while waiting for completion. + * \param [in] trajectory The trajectory to execute + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers = std::vector()); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the 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 70973a6af9..09e64e7db4 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 @@ -783,7 +783,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return res->error_code; } - moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait) + moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait, + const std::vector& controllers = std::vector()) { if (!execute_action_client_ || !execute_action_client_->action_server_is_ready()) { @@ -831,6 +832,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl moveit_msgs::action::ExecuteTrajectory::Goal goal; goal.trajectory = trajectory; + goal.controller_names = controllers; auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts); if (!wait) @@ -1455,24 +1457,27 @@ moveit::core::MoveItErrorCode MoveGroupInterface::move() return impl_->move(true); } -moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan, + const std::vector& controllers) { - return impl_->execute(plan.trajectory, false); + return impl_->execute(plan.trajectory, false, controllers); } -moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory) +moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers) { - return impl_->execute(trajectory, false); + return impl_->execute(trajectory, false, controllers); } -moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan, const std::vector& controllers) { - return impl_->execute(plan.trajectory, true); + return impl_->execute(plan.trajectory, true, controllers); } -moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory) +moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers) { - return impl_->execute(trajectory, true); + return impl_->execute(trajectory, true, controllers); } moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) @@ -1645,7 +1650,7 @@ void MoveGroupInterface::getJointValueTarget(std::vector& group_variable bool MoveGroupInterface::setJointValueTarget(const std::vector& joint_values) { - auto const n_group_joints = impl_->getJointModelGroup()->getVariableCount(); + const auto n_group_joints = impl_->getJointModelGroup()->getVariableCount(); if (joint_values.size() != n_group_joints) { RCLCPP_DEBUG_STREAM(LOGGER, "Provided joint value list has length " << joint_values.size() << " but group " @@ -1770,11 +1775,6 @@ bool MoveGroupInterface::setApproximateJointValueTarget(const Eigen::Isometry3d& return setApproximateJointValueTarget(msg, end_effector_link); } -const moveit::core::RobotState& MoveGroupInterface::getJointValueTarget() const -{ - return impl_->getTargetRobotState(); -} - const moveit::core::RobotState& MoveGroupInterface::getTargetRobotState() const { return impl_->getTargetRobotState(); diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index ce313494b3..872fb5710d 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning_interface - 2.7.4 + 2.8.0 Components of MoveIt that offer simpler remote (as from another ROS 2 node) 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 1e74618ce4..09e1c2116e 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* Contributors: Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index d5c802d5f6..85c347aeb7 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -2,7 +2,7 @@ moveit_ros_robot_interaction - 2.7.4 + 2.8.0 Components of MoveIt that offer interaction via interactive markers Henning Kayser Tyler Weaver diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 9bd80e3030..e85d54cbfd 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -56,8 +56,8 @@ namespace robot_interaction { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.robot_interaction"); -static const float END_EFFECTOR_UNREACHABLE_COLOR[4] = { 1.0, 0.3, 0.3, 1.0 }; -static const float END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 }; +static const double END_EFFECTOR_UNREACHABLE_COLOR[4] = { 1.0, 0.3, 0.3, 1.0 }; +static const double END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 }; const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interaction_interactive_marker_topic"; @@ -419,7 +419,7 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle } std_msgs::msg::ColorRGBA marker_color; - const float* color = handler->inError(eef) ? END_EFFECTOR_UNREACHABLE_COLOR : END_EFFECTOR_REACHABLE_COLOR; + const double* color = handler->inError(eef) ? END_EFFECTOR_UNREACHABLE_COLOR : END_EFFECTOR_REACHABLE_COLOR; marker_color.r = color[0]; marker_color.g = color[1]; marker_color.b = color[2]; @@ -602,8 +602,8 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) [this, marker_name](const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { moveInteractiveMarker(marker_name, *msg); }; - auto subscription = - node_->create_subscription(topic_name, 1, subscription_callback); + auto subscription = node_->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), subscription_callback); int_marker_move_subscribers_.push_back(subscription); } } diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 9400297d0e..3fc553ddff 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Remove boost from motion_planning_rviz_plugin (`#2308 `_) + Co-authored-by: Sebastian Jahr +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* fix for not having transparency in collision scenes on rviz. (`#2242 `_) + Co-authored-by: Alp Akgun +* Contributors: Sami Alperen Akgün, Shobuj Paul, Yang Lin + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 9594eac135..3e1dbca643 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -32,7 +32,6 @@ add_library(moveit_motion_planning_rviz_plugin_core SHARED ${SOURCE_FILES} ${HEA set_target_properties(moveit_motion_planning_rviz_plugin_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(moveit_motion_planning_rviz_plugin_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin) ament_target_dependencies(moveit_motion_planning_rviz_plugin_core - Boost moveit_ros_robot_interaction moveit_ros_planning_interface moveit_ros_warehouse @@ -40,7 +39,6 @@ ament_target_dependencies(moveit_motion_planning_rviz_plugin_core rviz_ogre_vendor Qt5 pluginlib - rviz_ogre_vendor ) target_include_directories(moveit_motion_planning_rviz_plugin_core PRIVATE "${OGRE_PREFIX_DIR}/include") @@ -48,7 +46,6 @@ add_library(moveit_motion_planning_rviz_plugin SHARED src/plugin_init.cpp) set_target_properties(moveit_motion_planning_rviz_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(moveit_motion_planning_rviz_plugin moveit_motion_planning_rviz_plugin_core) ament_target_dependencies(moveit_motion_planning_rviz_plugin - Boost moveit_ros_robot_interaction moveit_ros_warehouse pluginlib diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 17b02924e5..62cfcd0995 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -213,7 +213,7 @@ private Q_SLOTS: void onRobotModelLoaded() override; void onNewPlanningSceneState() override; void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; - void updateInternal(float wall_dt, float ros_dt) override; + void updateInternal(double wall_dt, double ros_dt) override; void renderWorkspaceBox(); void updateLinkColors(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 60ff93fbec..20190830d0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -121,7 +121,7 @@ class MotionPlanningFrame : public QWidget void initFromMoveGroupNS(); void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq); - void updateSceneMarkers(float wall_dt, float ros_dt); + void updateSceneMarkers(double wall_dt, double ros_dt); void updateExternalCommunication(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index ee15f6521c..76dd1cd4e2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -184,23 +184,23 @@ private Q_SLOTS: class ProgressBarEditor : public QWidget { Q_OBJECT - Q_PROPERTY(float value READ getValue WRITE setValue NOTIFY valueChanged USER true) + Q_PROPERTY(double value READ getValue WRITE setValue NOTIFY valueChanged USER true) public: /// Create a progressbar-like slider for editing values in range mix..max - ProgressBarEditor(QWidget* parent = nullptr, float min = -1.0, float max = 0.0, int digits = 0); + ProgressBarEditor(QWidget* parent = nullptr, double min = -1.0, double max = 0.0, int digits = 0); - void setValue(float value) + void setValue(double value) { value_ = value; } - float getValue() const + double getValue() const { return value_; } Q_SIGNALS: - void valueChanged(float value); + void valueChanged(double value); void editingFinished(); protected: @@ -210,9 +210,9 @@ class ProgressBarEditor : public QWidget void mouseReleaseEvent(QMouseEvent* event) override; private: - float value_; - float min_; - float max_; + double value_; + double min_; + double max_; int digits_; ///< number of decimal digits for formatting of the value }; 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 300f7c627a..7dd4ce0ee6 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 @@ -65,10 +65,6 @@ #include #include -#include -#include -#include - #include #include "ui_motion_planning_rviz_plugin_frame.h" @@ -284,7 +280,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable) if (enable) { planning_group_sub_ = node_->create_subscription( - "/rviz/moveit/select_planning_group", 1, + "/rviz/moveit/select_planning_group", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return selectPlanningGroupCallback(msg); }); } else @@ -432,17 +428,20 @@ void MotionPlanningDisplay::changedMetricsTextHeight() void MotionPlanningDisplay::displayTable(const std::map& values, const Ogre::ColourValue& color, const Ogre::Vector3& pos, const Ogre::Quaternion& orient) { - // the line we want to render - std::stringstream ss; - for (const std::pair& value : values) - ss << boost::format("%-10s %-4.2f") % value.first % value.second << '\n'; - - if (ss.str().empty()) + if (values.empty()) { text_to_display_->setVisible(false); return; } + // the line we want to render + std::stringstream ss; + ss.setf(std::ios_base::fixed); + ss.precision(2); + + for (const auto& [label, value] : values) + ss << label << ':' << value << '\n'; + text_to_display_->setCaption(ss.str()); text_to_display_->setColor(color); text_display_scene_node_->setPosition(pos); @@ -1343,7 +1342,7 @@ void MotionPlanningDisplay::update(float wall_dt, float ros_dt) PlanningSceneDisplay::update(wall_dt, ros_dt); } -void MotionPlanningDisplay::updateInternal(float wall_dt, float ros_dt) +void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt) { PlanningSceneDisplay::updateInternal(wall_dt, ros_dt); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 09ba2ad2e6..af37922f46 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -267,26 +267,28 @@ void MotionPlanningFrame::allowExternalProgramCommunication(bool enable) { using std::placeholders::_1; plan_subscriber_ = node_->create_subscription( - "/rviz/moveit/plan", 1, + "/rviz/moveit/plan", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remotePlanCallback(msg); }); execute_subscriber_ = node_->create_subscription( - "/rviz/moveit/execute", 1, + "/rviz/moveit/execute", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteExecuteCallback(msg); }); stop_subscriber_ = node_->create_subscription( - "/rviz/moveit/stop", 1, + "/rviz/moveit/stop", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteStopCallback(msg); }); update_start_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_start_state", 1, + "/rviz/moveit/update_start_state", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateStartStateCallback(msg); }); update_goal_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_goal_state", 1, + "/rviz/moveit/update_goal_state", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateGoalStateCallback(msg); }); update_custom_start_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_custom_start_state", 1, [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { + "/rviz/moveit/update_custom_start_state", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { return remoteUpdateCustomStartStateCallback(msg); }); update_custom_goal_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_custom_goal_state", 1, [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { + "/rviz/moveit/update_custom_goal_state", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { return remoteUpdateCustomGoalStateCallback(msg); }); } @@ -614,7 +616,7 @@ void MotionPlanningFrame::initFromMoveGroupNS() clear_octomap_service_client_ = node_->create_client(move_group::CLEAR_OCTOMAP_SERVICE_NAME); object_recognition_subscriber_ = node_->create_subscription( - "recognized_object_array", 1, + "recognized_object_array", rclcpp::SystemDefaultsQoS(), [this](const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& msg) { return listenDetectedObjects(msg); }); @@ -671,7 +673,7 @@ void MotionPlanningFrame::tabChanged(int index) } } -void MotionPlanningFrame::updateSceneMarkers(float /*wall_dt*/, float /*ros_dt*/) +void MotionPlanningFrame::updateSceneMarkers(double /*wall_dt*/, double /*ros_dt*/) { if (scene_marker_) scene_marker_->update(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index f9415cb478..3614f102cc 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -423,8 +423,8 @@ void ProgressBarDelegate::paint(QPainter* painter, const QStyleOptionViewItem& o if (vbounds.isValid()) { QPointF bounds = vbounds.toPointF(); - const float min = bounds.x(); - const float max = bounds.y(); + const double min = bounds.x(); + const double max = bounds.y(); QStyleOptionProgressBar opt; opt.rect = option.rect; @@ -452,8 +452,8 @@ QWidget* ProgressBarDelegate::createEditor(QWidget* parent, const QStyleOptionVi if (vbounds.isValid()) { QPointF bounds = vbounds.toPointF(); - float min = bounds.x(); - float max = bounds.y(); + double min = bounds.x(); + double max = bounds.y(); bool is_revolute = (index.data(JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE); if (is_revolute) { @@ -462,7 +462,7 @@ QWidget* ProgressBarDelegate::createEditor(QWidget* parent, const QStyleOptionVi } auto* editor = new ProgressBarEditor(parent, min, max, is_revolute ? 0 : 3); connect(editor, &ProgressBarEditor::editingFinished, this, &ProgressBarDelegate::commitAndCloseEditor); - connect(editor, &ProgressBarEditor::valueChanged, this, [=](float value) { + connect(editor, &ProgressBarEditor::valueChanged, this, [=](double value) { const_cast(index.model())->setData(index, value, Qt::EditRole); }); return editor; @@ -498,7 +498,7 @@ bool JointsWidgetEventFilter::eventFilter(QObject* /*target*/, QEvent* event) return false; } -ProgressBarEditor::ProgressBarEditor(QWidget* parent, float min, float max, int digits) +ProgressBarEditor::ProgressBarEditor(QWidget* parent, double min, double max, int digits) : QWidget(parent), min_(min), max_(max), digits_(digits) { // if left mouse button is pressed, grab all future mouse events until button(s) released @@ -530,7 +530,7 @@ void ProgressBarEditor::mousePressEvent(QMouseEvent* event) void ProgressBarEditor::mouseMoveEvent(QMouseEvent* event) { - float v = std::min(max_, std::max(min_, min_ + event->x() * (max_ - min_) / width())); + double v = std::min(max_, std::max(min_, min_ + event->x() * (max_ - min_) / width())); if (value_ != v) { value_ = v; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index 280e83c9c6..faf3a538c5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -53,8 +53,6 @@ #include "ui_motion_planning_rviz_plugin_frame.h" -#include - #include namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 9be4d288aa..e847986ce7 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -2,7 +2,7 @@ moveit_ros_visualization - 2.7.4 + 2.8.0 Components of MoveIt that offer visualization Henning Kayser Tyler Weaver diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 6207edeae7..3e37370448 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -178,7 +178,7 @@ protected Q_SLOTS: void fixedFrameChanged() override; // new virtual functions added by this plugin - virtual void updateInternal(float wall_dt, float ros_dt); + virtual void updateInternal(double wall_dt, double ros_dt); virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -199,7 +199,7 @@ protected Q_SLOTS: bool planning_scene_needs_render_; // or only the robot position (excluding attached object changes) bool robot_state_needs_render_; - float current_scene_time_; + double current_scene_time_; rviz_common::properties::Property* scene_category_; rviz_common::properties::Property* robot_category_; 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 0caafa535f..75de575ec9 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 @@ -670,7 +670,7 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) updateInternal(wall_dt, ros_dt); } -void PlanningSceneDisplay::updateInternal(float wall_dt, float /*ros_dt*/) +void PlanningSceneDisplay::updateInternal(double wall_dt, double /*ros_dt*/) { current_scene_time_ += wall_dt; if (planning_scene_render_ && diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 2acdc9dceb..c85f2ed3f1 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -304,7 +304,7 @@ void RobotStateDisplay::changedRobotStateTopic() setStatus(rviz_common::properties::StatusProperty::Warn, "RobotState", "No msg received"); robot_state_subscriber_ = node_->create_subscription( - robot_state_topic_property_->getStdString(), 10, + robot_state_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state) { return newRobotStateCallback(state); }); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index 0dc4a46ba5..06ab1d8fda 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -70,7 +70,7 @@ class PlanningSceneRender void renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene, const Ogre::ColourValue& default_scene_color, const Ogre::ColourValue& default_attached_color, OctreeVoxelRenderMode voxel_render_mode, - OctreeVoxelColorMode voxel_color_mode, float default_scene_alpha); + OctreeVoxelColorMode voxel_color_mode, double default_scene_alpha); void clear(); private: diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index d0838a097f..c7d7d3fcaf 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -59,8 +59,8 @@ class RenderShapes void renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, - const Ogre::ColourValue& color, float alpha); - void updateShapeColors(float r, float g, float b, float a); + const Ogre::ColourValue& color, double alpha); + void updateShapeColors(double r, double g, double b, double a); void clear(); private: diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index f04e32fd55..fae4c85c29 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -95,7 +95,7 @@ class RobotStateVisualization */ void setCollisionVisible(bool visible); - void setAlpha(float alpha); + void setAlpha(double alpha); private: void updateHelper(const moveit::core::RobotStateConstPtr& robot_state, diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 4fb7bd0190..48a16ad598 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -87,7 +87,7 @@ class TrajectoryVisualization : public QObject ~TrajectoryVisualization() override; - virtual void update(float wall_dt, float sim_dt); + virtual void update(double wall_dt, double sim_dt); virtual void reset(); void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node, @@ -132,7 +132,7 @@ private Q_SLOTS: * \return Positive values indicate a fixed time per state * Negative values indicate a realtime-factor */ - float getStateDisplayTime(); + double getStateDisplayTime(); void clearTrajectoryTrail(); // Handles actually drawing the robot along motion plans @@ -150,7 +150,7 @@ private Q_SLOTS: bool animating_path_; bool drop_displaying_trajectory_; int current_state_; - float current_state_time_; + double current_state_time_; std::mutex update_trajectory_message_; moveit::core::RobotModelConstPtr robot_model_; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index 8513ea8997..5a36c4d082 100755 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -224,7 +224,7 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& new_point.position.y = it.getY(); new_point.position.z = it.getZ(); - float cell_probability; + double cell_probability; switch (octree_color_mode) { 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..d5ebca65e2 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 @@ -75,7 +75,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen const Ogre::ColourValue& default_env_color, const Ogre::ColourValue& default_attached_color, OctreeVoxelRenderMode octree_voxel_rendering, - OctreeVoxelColorMode octree_color_mode, float default_scene_alpha) + OctreeVoxelColorMode octree_color_mode, double default_scene_alpha) { if (!scene) return; @@ -102,13 +102,14 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen { collision_detection::CollisionEnv::ObjectConstPtr object = scene->getWorld()->getObject(id); Ogre::ColourValue color = default_env_color; - float alpha = default_scene_alpha; + double alpha = default_scene_alpha; if (scene->hasObjectColor(id)) { const std_msgs::msg::ColorRGBA& c = scene->getObjectColor(id); 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/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 73c2b38654..dc340cd4ae 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -71,7 +71,7 @@ void RenderShapes::clear() void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, - const Ogre::ColourValue& color, float alpha) + const Ogre::ColourValue& color, double alpha) { rviz_rendering::Shape* ogre_shape = nullptr; Eigen::Vector3d translation = p.translation(); @@ -192,7 +192,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co } } -void RenderShapes::updateShapeColors(float r, float g, float b, float a) +void RenderShapes::updateShapeColors(double r, double g, double b, double a) { for (const std::unique_ptr& shape : scene_shapes_) shape->setColor(r, g, b, a); 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 b8e105ba53..2af820fa44 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 @@ -119,7 +119,7 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt for (const moveit::core::AttachedBody* attached_body : attached_bodies) { std_msgs::msg::ColorRGBA color = default_attached_object_color; - float alpha = robot_.getAlpha(); + double alpha = robot_.getAlpha(); if (color_map) { std::map::const_iterator it = color_map->find(attached_body->getName()); @@ -176,7 +176,7 @@ void RobotStateVisualization::setCollisionVisible(bool visible) robot_.setCollisionVisible(visible); } -void RobotStateVisualization::setAlpha(float alpha) +void RobotStateVisualization::setAlpha(double alpha) { robot_.setAlpha(alpha); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index c0e42a62cd..1b82869435 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -184,7 +184,7 @@ void TrajectoryVisualization::onInitialize(const rclcpp::Node::SharedPtr& node, } trajectory_topic_sub_ = node_->create_subscription( - trajectory_topic_property_->getStdString(), 2, + trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); }); } @@ -294,7 +294,7 @@ void TrajectoryVisualization::changedTrajectoryTopic() if (!trajectory_topic_property_->getStdString().empty() && robot_state_) { trajectory_topic_sub_ = node_->create_subscription( - trajectory_topic_property_->getStdString(), 2, + trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); }); @@ -364,15 +364,15 @@ void TrajectoryVisualization::interruptCurrentDisplay() animating_path_ = false; } -float TrajectoryVisualization::getStateDisplayTime() +double TrajectoryVisualization::getStateDisplayTime() { constexpr char default_time_string[] = "3x"; - constexpr float default_time_value = -3.0f; + constexpr double default_time_value = -3.0; std::string tm = state_display_time_property_->getStdString(); boost::trim(tm); - float type; + double type; if (tm.back() == 'x') { @@ -391,7 +391,7 @@ float TrajectoryVisualization::getStateDisplayTime() tm.resize(tm.size() - 1); boost::trim_right(tm); - float value; + double value; try { value = std::stof(tm); @@ -416,7 +416,7 @@ void TrajectoryVisualization::dropTrajectory() drop_displaying_trajectory_ = true; } -void TrajectoryVisualization::update(float wall_dt, float sim_dt) +void TrajectoryVisualization::update(double wall_dt, double sim_dt) { if (drop_displaying_trajectory_) { @@ -478,7 +478,7 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) { current_state_time_ += wall_dt; } - float tm = getStateDisplayTime(); + double tm = getStateDisplayTime(); if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused()) { @@ -493,7 +493,7 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) else if (tm < 0.0) { // using realtime factors: skip to next waypoint based on elapsed display time - const float rt_factor = -tm; // negative tm is the intended realtime factor + const double rt_factor = -tm; // negative tm is the intended realtime factor while (current_state_ < waypoint_count && (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1) / rt_factor) < current_state_time_) diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 2de123865e..177dae4ab8 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced boost::algorithm::join with fmt::join (`#2273 `_) + * Replaced boost::algorithm::join with fmt::join + * Made changes in CMakeLists.txt to accomodate fmt + * Updated package.xml files + * removed redundant boost dependencies + * Rename variables -> variable + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Replaced numbers with SystemDefaultsQos() (`#2271 `_) +* Contributors: Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index 5b3e38525d..b15a4aa2b1 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(fmt REQUIRED) find_package(rclcpp REQUIRED) find_package(moveit_core REQUIRED) find_package(warehouse_ros REQUIRED) @@ -19,6 +20,7 @@ include(ConfigExtras.cmake) include_directories(include) set(THIS_PACKAGE_INCLUDE_DEPENDS + fmt Boost moveit_core rclcpp @@ -52,7 +54,7 @@ target_link_libraries(moveit_warehouse_broadcast moveit_warehouse) add_executable(moveit_save_to_warehouse src/save_to_warehouse.cpp) ament_target_dependencies(moveit_save_to_warehouse ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(moveit_save_to_warehouse moveit_warehouse) +target_link_libraries(moveit_save_to_warehouse moveit_warehouse fmt) add_executable(moveit_warehouse_import_from_text src/import_from_text.cpp) ament_target_dependencies(moveit_warehouse_import_from_text ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index de61235e88..b878976a7a 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -2,7 +2,7 @@ moveit_ros_warehouse - 2.7.4 + 2.8.0 Components of MoveIt connecting to MongoDB Henning Kayser Tyler Weaver @@ -25,6 +25,7 @@ moveit_ros_planning tf2_eigen tf2_ros + fmt ament_lint_auto ament_lint_common diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index 24f7490be6..208713d589 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -64,8 +64,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) @@ -75,6 +73,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; @@ -141,7 +140,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); @@ -154,14 +153,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); } @@ -195,7 +194,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); @@ -217,7 +216,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); @@ -227,7 +226,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_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 1f0a479bfe..894212587d 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -38,11 +38,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -210,16 +210,18 @@ int main(int argc, char** argv) psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); }); auto mplan_req_sub = node->create_subscription( - "motion_plan_request", 100, + "motion_plan_request", rclcpp::SystemDefaultsQoS(), [&](const moveit_msgs::msg::MotionPlanRequest& msg) { onMotionPlanRequest(msg, psm, pss); }); auto constr_sub = node->create_subscription( - "constraints", 100, [&](const moveit_msgs::msg::Constraints& msg) { onConstraints(msg, cs); }); + "constraints", rclcpp::SystemDefaultsQoS(), + [&](const moveit_msgs::msg::Constraints& msg) { onConstraints(msg, cs); }); auto state_sub = node->create_subscription( - "robot_state", 100, [&](const moveit_msgs::msg::RobotState& msg) { onRobotState(msg, rs); }); + "robot_state", rclcpp::SystemDefaultsQoS(), + [&](const moveit_msgs::msg::RobotState& msg) { onRobotState(msg, rs); }); std::vector topics; psm.getMonitoredTopics(topics); - RCLCPP_INFO_STREAM(LOGGER, "Listening for scene updates on topics " << boost::algorithm::join(topics, ", ")); + RCLCPP_INFO_STREAM(LOGGER, "Listening for scene updates on topics " << fmt::format("{}", fmt::join(topics, ", "))); RCLCPP_INFO_STREAM(LOGGER, "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); RCLCPP_INFO_STREAM(LOGGER, "Listening for named constraints on topic " << constr_sub->get_topic_name()); RCLCPP_INFO_STREAM(LOGGER, "Listening for states on topic " << state_sub->get_topic_name()); diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index 4833751092..eaed554c45 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 0ccf50405f..a80bb76c5c 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -2,7 +2,7 @@ moveit_runtime - 2.7.4 + 2.8.0 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 4085c7eab6..ff3ab44878 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_setup_app_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced boost::algorithm::join with fmt::join (`#2273 `_) + * Replaced boost::algorithm::join with fmt::join + * Made changes in CMakeLists.txt to accomodate fmt + * Updated package.xml files + * removed redundant boost dependencies + * Rename variables -> variable + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Contributors: Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp index 604bfa6518..bca8be5bd1 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp @@ -139,9 +139,9 @@ class LaunchBundle bool write() override { // Add function name as a TemplateVariable, then remove it - variables_.push_back(TemplateVariable("FUNCTION_NAME", function_name_)); + variables.push_back(TemplateVariable("FUNCTION_NAME", function_name_)); bool ret = TemplatedGeneratedFile::write(); - variables_.pop_back(); + variables.pop_back(); return ret; } diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp index 18b60c13bc..1c13c20757 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp @@ -86,7 +86,7 @@ class PerceptionWidget : public SetupStepWidget QGroupBox* point_cloud_group_; QGroupBox* depth_map_group_; - // Point Cloud plugin feilds + // Point Cloud plugin fields QLineEdit* point_cloud_topic_field_; QLineEdit* max_range_field_; QLineEdit* point_subsample_field_; @@ -95,7 +95,7 @@ class PerceptionWidget : public SetupStepWidget QLineEdit* max_update_rate_field_; QLineEdit* filtered_cloud_topic_field_; - // Depth Map plugin feilds + // Depth Map plugin fields QLineEdit* image_topic_field_; QLineEdit* queue_size_field_; QLineEdit* near_clipping_field_; diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml index 2384cebfea..c65f86f60a 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.7.4 + 2.8.0 Various specialty plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause diff --git a/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst index a4cfd5a0d8..79c92f6f8d 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp index 7ff6162f40..d6195a6e4b 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp @@ -58,10 +58,10 @@ class NavigationWidget : public QListView explicit NavigationWidget(QWidget* parent = nullptr); void setNavs(const QList& navs); - void setEnabled(const int& index, bool enabled); - void setSelected(const int& index); + void setEnabled(int index, bool enabled); + void setSelected(int index); - bool isEnabled(const int& index) const; + bool isEnabled(int index) const; private: QStandardItemModel* model_; diff --git a/moveit_setup_assistant/moveit_setup_assistant/package.xml b/moveit_setup_assistant/moveit_setup_assistant/package.xml index fd45ad739e..c55e82c0d7 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.7.4 + 2.8.0 Generates a configuration package that makes it easy to use MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp index aaed6a997a..4c2bbe5e93 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp @@ -90,7 +90,7 @@ void NavigationWidget::setNavs(const QList& navs) setModel(model_); } -void NavigationWidget::setEnabled(const int& index, bool enabled) +void NavigationWidget::setEnabled(int index, bool enabled) { if (enabled) { @@ -103,7 +103,7 @@ void NavigationWidget::setEnabled(const int& index, bool enabled) } } -void NavigationWidget::setSelected(const int& index) +void NavigationWidget::setSelected(int index) { // First make sure item is enabled setEnabled(index, true); @@ -117,7 +117,7 @@ void NavigationWidget::setSelected(const int& index) selectionModel()->select(selection, QItemSelectionModel::Select); } -bool NavigationWidget::isEnabled(const int& index) const +bool NavigationWidget::isEnabled(int index) const { return model_->item(index)->flags() > Qt::NoItemFlags; } diff --git a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst index 7afc6842ee..4bf5540d7e 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_setup_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_setup_assistant/moveit_setup_controllers/package.xml b/moveit_setup_assistant/moveit_setup_controllers/package.xml index 13c5a29430..50c0379985 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.7.4 + 2.8.0 MoveIt Setup Steps for ROS 2 Control David V. Lu!! BSD-3-Clause diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst index 2fd869bc77..01c0a35727 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.8.0 (2023-09-10) +------------------ +* Replaced boost::algorithm::join with fmt::join (`#2273 `_) + * Replaced boost::algorithm::join with fmt::join + * Made changes in CMakeLists.txt to accomodate fmt + * Updated package.xml files + * removed redundant boost dependencies + * Rename variables -> variable + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Contributors: Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml index 5d684adf84..324ead3999 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.7.4 + 2.8.0 Core (meta) plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp index 1388809114..74ba926163 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp @@ -47,7 +47,7 @@ void ConfigurationFiles::onInit() void ConfigurationFiles::loadTemplateVariables() { - auto& variables = TemplatedGeneratedFile::variables_; + auto& variables = TemplatedGeneratedFile::variables; variables.clear(); for (const auto& config : config_data_->getConfigured()) { diff --git a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst index 2b1652f243..a115abf105 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_setup_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ +* Replaced boost::algorithm::join with fmt::join (`#2273 `_) + * Replaced boost::algorithm::join with fmt::join + * Made changes in CMakeLists.txt to accomodate fmt + * Updated package.xml files + * removed redundant boost dependencies + * Rename variables -> variable + --------- + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Sebastian Jahr +* Contributors: Shobuj Paul + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt index f48d789b53..466994c486 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt @@ -8,6 +8,7 @@ moveit_package() find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(fmt REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_visualization REQUIRED) @@ -83,6 +84,7 @@ install(TARGETS moveit_setup_framework INCLUDES DESTINATION include/moveit_setup_framework ) +ament_export_dependencies(fmt) ament_export_dependencies(rclcpp) ament_export_dependencies(Qt5Core) ament_export_dependencies(Qt5Widgets) diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp index 2188b3d6a3..c3b444e205 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp @@ -132,7 +132,7 @@ class SetupConfig * * @param[in] package_path the path to the root of the config package * @param[in] last_gen_time The time (if any) when the config package was last generated - * @parma[out] files Where to put the new generated files + * @param[out] files Where to put the new generated files */ virtual void collectFiles(const std::filesystem::path& /*package_path*/, const GeneratedTime& /*last_gen_time*/, std::vector& /*files*/) diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp index fb90a75703..1555eee16b 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp @@ -69,7 +69,7 @@ class TemplatedGeneratedFile : public GeneratedFile bool write() override; - static std::vector variables_; + static std::vector variables; }; } // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/package.xml b/moveit_setup_assistant/moveit_setup_framework/package.xml index 0ba58ffec5..fc8f3cb310 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.7.4 + 2.8.0 C++ Interface for defining setup steps for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause @@ -20,6 +20,7 @@ rviz_rendering srdfdom urdf + fmt ament_lint_auto ament_clang_format 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/templates.cpp b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp index 6fc39d0671..37ff291e99 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp @@ -40,7 +40,7 @@ namespace moveit_setup { -std::vector TemplatedGeneratedFile::variables_; +std::vector TemplatedGeneratedFile::variables; static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_setup.templates"); bool TemplatedGeneratedFile::write() @@ -71,7 +71,7 @@ bool TemplatedGeneratedFile::write() template_stream.close(); // Replace keywords in string ------------------------------------------------------------ - for (const auto& variable : variables_) + for (const auto& variable : variables) { std::string key_with_brackets = "[" + variable.key + "]"; boost::replace_all(template_string, key_with_brackets, variable.value); diff --git a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp index 164c7817c9..2480848908 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include namespace moveit_setup { @@ -85,39 +85,52 @@ void URDFConfig::loadFromPath(const std::filesystem::path& urdf_file_path, const { urdf_path_ = urdf_file_path; xacro_args_vec_ = xacro_args; - xacro_args_ = boost::algorithm::join(xacro_args_vec_, " "); + xacro_args_ = fmt::format("{}", fmt::join(xacro_args_vec_, " ")); setPackageName(); load(); } void URDFConfig::setPackageName() { - bool package_found = extractPackageNameFromPath(urdf_path_, urdf_pkg_name_, urdf_pkg_relative_path_); - if (!package_found) - { - urdf_pkg_name_ = ""; - urdf_pkg_relative_path_ = urdf_path_; // just the absolute path - } - // Check that ROS can find the package - const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_); + // Reset to defaults: no package name, relative path is set to absolute path + urdf_pkg_name_ = ""; + urdf_pkg_relative_path_ = urdf_path_; - if (robot_desc_pkg_path.empty()) + std::string pkg_name; + std::filesystem::path relative_path; + if (extractPackageNameFromPath(urdf_path_, pkg_name, relative_path)) { - RCLCPP_WARN(*logger_, - "Package Not Found In ROS Workspace. ROS was unable to find the package name '%s'" - " within the ROS workspace. This may cause issues later.", - urdf_pkg_name_.c_str()); + // Check that ROS can find the package, update members accordingly + const std::filesystem::path robot_desc_pkg_path = getSharePath(pkg_name); + if (!robot_desc_pkg_path.empty()) + { + urdf_pkg_name_ = pkg_name; + urdf_pkg_relative_path_ = relative_path; + } + else + { + RCLCPP_WARN(*logger_, + "Found package name '%s' but failed to resolve ROS package path." + "Attempting to load the URDF from absolute path, instead.", + pkg_name.c_str()); + } } } void URDFConfig::loadFromPackage(const std::filesystem::path& package_name, const std::filesystem::path& relative_path, const std::string& xacro_args) { + const std::filesystem::path package_path = getSharePath(package_name); + if (package_path.empty()) + { + throw std::runtime_error("URDF/COLLADA package not found: ''" + package_name.string()); + } + urdf_pkg_name_ = package_name; urdf_pkg_relative_path_ = relative_path; xacro_args_ = xacro_args; - urdf_path_ = getSharePath(urdf_pkg_name_) / relative_path; + urdf_path_ = package_path / relative_path; load(); } @@ -131,11 +144,6 @@ void URDFConfig::load() throw std::runtime_error("URDF/COLLADA file not found: " + urdf_path_.string()); } - if (urdf_pkg_name_.empty()) - { - throw std::runtime_error("URDF/COLLADA package not found for file path: " + urdf_path_.string()); - } - if (urdf_string_.empty() && rdf_loader::RDFLoader::isXacroFile(urdf_path_)) { throw std::runtime_error("Running xacro failed.\nPlease check console for errors."); diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst index 6a915228a7..29220cf6d0 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_setup_srdf_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-10) +------------------ + 2.7.4 (2023-05-18) ------------------ diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml index bad98555e2..b6d4ce00cd 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.7.4 + 2.8.0 SRDF-based plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index 9ac7c7b43c..7fc7a7da62 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -107,20 +107,20 @@ static bool setLinkPair(const std::string& linkA, const std::string& linkB, cons /** * \brief Build the robot links connection graph and then check for links with no geomotry * \param link The root link to begin a breadth first search on - * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description + * \param link_graph A representation of all bi-directional joint connections between links in robot_description */ static void computeConnectionGraph(const moveit::core::LinkModel* link, LinkGraph& link_graph); /** * \brief Recursively build the adj list of link connections * \param link The root link to begin a breadth first search on - * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description + * \param link_graph A representation of all bi-directional joint connections between links in robot_description */ static void computeConnectionGraphRec(const moveit::core::LinkModel* link, LinkGraph& link_graph); /** * \brief Disable collision checking for adjacent links, or adjacent with no geometry links between them - * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description + * \param link_graph A representation of all bi-directional joint connections between links in robot_description * \param scene A reference to the robot in the planning scene * \param link_pairs List of all unique link pairs and each pair's properties * \return number of adjacent links found and disabled @@ -349,7 +349,7 @@ void computeConnectionGraph(const moveit::core::LinkModel* start_link, LinkGraph // Recursively build adj list of link connections computeConnectionGraphRec(start_link, link_graph); - // Repeatidly check for links with no geometry and remove them, then re-check until no more removals are detected + // Repeatedly check for links with no geometry and remove them, then re-check until no more removals are detected bool update = true; // track if a no geometry link was found while (update) {