diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index 11fc0910d9..4275e71d20 100644 --- a/.docker/ci-testing/Dockerfile +++ b/.docker/ci-testing/Dockerfile @@ -7,7 +7,7 @@ LABEL maintainer="Robert Haschke rhaschke@techfak.uni-bielefeld.de" # Switch to ros-testing RUN apt-get update && \ - apt-get install -y ros2-testing-apt-source && \ + apt-get install -y ros2-testing-apt-source --no-install-recommends && \ # Upgrade packages to ros-testing and clean apt-cache within one RUN command apt-get update && \ apt-get -qq -y dist-upgrade && \ diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index 2b47a28d84..050c480410 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -24,6 +24,9 @@ RUN \ apt-get -q install --no-install-recommends -y \ # Some basic requirements wget git sudo curl \ + # Use default gfortran for libopenmpi-dev + # https://bugs.launchpad.net/ubuntu/+source/openmpi/+bug/2166011 + gfortran \ # Preferred build tools clang clang-format clang-tidy clang-tools \ ccache && \ @@ -34,12 +37,12 @@ RUN \ # # Fetch all dependencies from moveit2.repos vcs import src < src/moveit2/moveit2.repos && \ - if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \ + if [ -r "src/moveit2/moveit2_${ROS_DISTRO}.repos" ] ; then vcs import src < "src/moveit2/moveit2_${ROS_DISTRO}.repos" ; fi && \ # # Download all dependencies of MoveIt rosdep update && \ DEBIAN_FRONTEND=noninteractive \ - rosdep install -y -r --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false || true && \ + rosdep install -y -r --from-paths src --ignore-src --rosdistro "${ROS_DISTRO}" --as-root=apt:false || true && \ # Remove the source code from this container rm -rf src && \ # diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 0d2767ad00..1bccc90321 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -10,7 +10,8 @@ ENV RTI_NC_LICENSE_ACCEPTED=yes # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size RUN apt-get update -q && \ - apt-get install -y ros2-testing-apt-source && apt-get update -q && \ + apt-get install -y ros2-testing-apt-source --no-install-recommends && \ + apt-get update -q && \ apt-get upgrade -q -y && \ - apt-get install -y "ros-${ROS_DISTRO}-moveit-*" --no-install-recommends && \ + apt-get install -y gfortran "ros-${ROS_DISTRO}-moveit-*" --no-install-recommends && \ rm -rf /var/lib/apt/lists/* diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 4d8e7fd083..42cd2da234 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -16,7 +16,7 @@ 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/ \ +RUN --mount=type=cache,target=/root/.ccache \ # Enable ccache PATH=/usr/lib/ccache:$PATH && \ # Fetch required upstream sources for building @@ -28,7 +28,7 @@ RUN --mount=type=cache,target=/root/.ccache/ \ apt-get -q update && \ rosdep update && \ DEBIAN_FRONTEND=noninteractive \ - rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ + 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 \ diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile index f2f2bd47f0..5e716d695d 100644 --- a/.docker/tutorial-source/Dockerfile +++ b/.docker/tutorial-source/Dockerfile @@ -4,36 +4,32 @@ # Source build of the repos file from the tutorial site ARG ROS_DISTRO=rolling -ARG GZ_VERSION=ionic FROM moveit/moveit2:${ROS_DISTRO}-source -LABEL maintainer="Tyler Weaver tyler@picknik.ai" +LABEL maintainer="Robert Haschke rhaschke@techfak.uni-bielefeld.de" # 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 \ +RUN --mount=type=cache,target=/root/.ccache \ # Install Gazebo, which is needed by some dependencies. - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - && \ - sudo apt update && \ - sudo apt-get install -y "gz-${GZ_VERSION}" && \ + apt-get -q update && \ + apt-get install -y "ros-${ROS_DISTRO}-ros-gz" --no-install-recommends && \ # Enable ccache PATH=/usr/lib/ccache:$PATH && \ # Checkout the tutorial repo git clone https://github.com/moveit/moveit2_tutorials src/moveit2_tutorials && \ # Fetch required upstream sources for building - vcs import --skip-existing src < src/moveit2_tutorials/moveit2_tutorials.repos && \ + vcs import --skip-existing src < src/moveit2_tutorials/.github/upstream.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 && \ + 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 && \ + ccache -s diff --git a/.dockerignore b/.dockerignore index 250f1b74de..66a7807eb1 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,36 +1,6 @@ # ignore everything -* +** # but include these: !moveit2*.repos !**/package.xml !**/COLCON_IGNORE - -# https://github.com/moby/moby/issues/42788 -!moveit_plugins/moveit_plugins/package.xml -!moveit_plugins/moveit_ros_control_interface/package.xml -!moveit_plugins/moveit_simple_controller_manager/package.xml -!moveit_kinematics/package.xml -!moveit_common/package.xml -!moveit_setup_assistant/package.xml -!moveit_core/package.xml -!moveit_commander/package.xml -!moveit_planners/ompl/package.xml -!moveit_planners/chomp/chomp_motion_planner/package.xml -!moveit_planners/chomp/chomp_interface/package.xml -!moveit_planners/pilz_industrial_motion_planner_testutils/package.xml -!moveit_planners/pilz_industrial_motion_planner/package.xml -!moveit_planners/moveit_planners/package.xml -!moveit_runtime/package.xml -!moveit/package.xml -!moveit_ros/warehouse/package.xml -!moveit_ros/moveit_servo/package.xml -!moveit_ros/occupancy_map_monitor/package.xml -!moveit_ros/perception/package.xml -!moveit_ros/move_group/package.xml -!moveit_ros/robot_interaction/package.xml -!moveit_ros/visualization/package.xml -!moveit_ros/planning/package.xml -!moveit_ros/planning_interface/package.xml -!moveit_ros/benchmarks/package.xml -!moveit_ros/moveit_ros/package.xml -!moveit_ros/hybrid_planning/package.xml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 513fb73826..26ddf9afd7 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -24,13 +24,12 @@ jobs: - IMAGE: lyrical-ci-testing IKFAST_TEST: true EXTRA_CXX_FLAGS: -Wno-error=deprecated-declarations + CLANG_TIDY: pedantic - IMAGE: rolling-ci - IMAGE: lyrical-ci-testing EXTRA_CXX_FLAGS: -Wno-error=deprecated-declarations - IMAGE: jazzy-ci IKFAST_TEST: true - # Moved here because Ubuntu 26.04 has issues compiling with clang++ - CLANG_TIDY: pedantic - IMAGE: humble-ci env: # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option @@ -176,6 +175,7 @@ jobs: - name: Generate codecov report uses: rhaschke/lcov-action@main if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + continue-on-error: true # ignore non-suppressable errors of lcov with: docker: ${{ env.DOCKER_IMAGE }} workdir: ${{ env.BASEDIR }}/target_ws diff --git a/.github/workflows/docker.build.yaml b/.github/workflows/docker.build.yaml new file mode 100644 index 0000000000..c1e486b02e --- /dev/null +++ b/.github/workflows/docker.build.yaml @@ -0,0 +1,93 @@ +name: Build Docker image + +on: + workflow_call: + inputs: + image: + required: true + type: string + dockerfile: + required: true + type: string + ros_distro: + required: true + type: string + check_apt_updates: + default: false + type: boolean + full_source_context: + default: false + type: boolean + +jobs: + build: + runs-on: ubuntu-latest + permissions: + packages: write + contents: read + env: + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} + + steps: + - uses: actions/checkout@v7 + - uses: rhaschke/docker-run-action@main + name: Check for apt updates + continue-on-error: true + id: apt + if: inputs.check_apt_updates + with: + image: ${{ inputs.image }} + run: | + apt-get update + have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) + echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v4 + id: setup-buildx + - name: Login to Github Container Registry + if: env.PUSH == 'true' + uses: docker/login-action@v4 + with: + registry: ghcr.io + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Login to DockerHub + if: env.PUSH == 'true' + uses: docker/login-action@v4 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Cache + uses: actions/cache@v5 + id: cache + with: + path: caches + key: docker-tutorial-cache-${{ inputs.ros_distro }}-${{ hashFiles(inputs.dockerfile) }} + - name: Inject caches into docker + uses: reproducible-containers/buildkit-cache-dance@v3 + with: + builder: ${{ steps.setup-buildx.outputs.name }} + cache-dir: caches + dockerfile: ${{ inputs.dockerfile }} + skip-extraction: ${{ steps.cache.outputs.cache-hit }} + + - name: Remove .dockerignore + if: inputs.full_source_context + run: rm .dockerignore # enforce full source context + - name: Build and Push + uses: docker/build-push-action@v7 + with: + context: . + file: ${{ inputs.dockerfile }} + build-args: ROS_DISTRO=${{ inputs.ros_distro }} + push: ${{ env.PUSH }} + no-cache: ${{ (inputs.check_apt_updates && steps.apt.outputs.no_cache) || github.event_name == 'workflow_dispatch' }} + cache-to: type=gha,mode=max + cache-from: | + type=gha + type=registry,ref=ghcr.io/${{ inputs.image }} + tags: | + ${{ inputs.image }} + ghcr.io/${{ inputs.image }} diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index ff170a1163..630c194a9a 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -11,8 +11,9 @@ on: pull_request: paths: - .docker/** + - .github/workflows/docker.build.yaml - .github/workflows/docker.yaml - - moveit2.repos + - "*.repos" jobs: release: @@ -20,104 +21,32 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [lyrical, rolling] - runs-on: ubuntu-latest permissions: packages: write contents: read - env: - IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} - - steps: - - uses: rhaschke/docker-run-action@main - name: Check for apt updates - continue-on-error: true - id: apt - with: - image: ${{ env.IMAGE }} - run: | - apt-get update - have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) - echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v4 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build and Push - uses: docker/build-push-action@v7 - with: - file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - push: ${{ env.PUSH }} - no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} - cache-from: type=registry,ref=ghcr.io/${{ env.IMAGE }} - cache-to: type=inline - tags: | - ${{ env.IMAGE }} - ghcr.io/${{ env.IMAGE }} + uses: ./.github/workflows/docker.build.yaml + with: + image: moveit/moveit2:${{ matrix.ROS_DISTRO }}-release + dockerfile: .docker/release/Dockerfile + ros_distro: ${{ matrix.ROS_DISTRO }} + check_apt_updates: true + secrets: inherit ci: strategy: fail-fast: false matrix: ROS_DISTRO: [lyrical, rolling] - runs-on: ubuntu-latest permissions: packages: write contents: read - env: - IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} - - steps: - - uses: rhaschke/docker-run-action@main - name: Check for apt updates - continue-on-error: true - id: apt - with: - image: ${{ env.IMAGE }} - run: | - apt-get update - have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) - echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v4 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build and Push - uses: docker/build-push-action@v7 - with: - file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - push: ${{ env.PUSH }} - no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} - cache-from: type=registry,ref=ghcr.io/${{ env.IMAGE }} - cache-to: type=inline - tags: | - ${{ env.IMAGE }} - ghcr.io/${{ env.IMAGE }} + uses: ./.github/workflows/docker.build.yaml + with: + image: moveit/moveit2:${{ matrix.ROS_DISTRO }}-ci + dockerfile: .docker/ci/Dockerfile + ros_distro: ${{ matrix.ROS_DISTRO }} + check_apt_updates: true + secrets: inherit ci-testing: needs: ci @@ -125,52 +54,16 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [lyrical, rolling] - runs-on: ubuntu-latest permissions: packages: write contents: read - env: - IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} - - steps: - - uses: rhaschke/docker-run-action@main - name: Check for apt updates - continue-on-error: true - id: apt - with: - image: ${{ env.IMAGE }} - run: | - apt-get update - have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) - echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v4 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build and Push - uses: docker/build-push-action@v7 - with: - file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - push: ${{ env.PUSH }} - no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} - cache-from: type=registry,ref=ghcr.io/${{ env.IMAGE }} - cache-to: type=inline - tags: | - ${{ env.IMAGE }} - ghcr.io/${{ env.IMAGE }} + uses: ./.github/workflows/docker.build.yaml + with: + image: moveit/moveit2:${{ matrix.ROS_DISTRO }}-ci-testing + dockerfile: .docker/ci-testing/Dockerfile + ros_distro: ${{ matrix.ROS_DISTRO }} + check_apt_updates: true + secrets: inherit source: needs: ci-testing @@ -178,45 +71,16 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [lyrical, rolling] - runs-on: ubuntu-latest permissions: packages: write contents: read - env: - IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} - - steps: - - uses: actions/checkout@v7 - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v4 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v4 - 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@v7 - with: - context: . - file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - push: ${{ env.PUSH }} - cache-from: type=registry,ref=ghcr.io/${{ env.IMAGE }} - cache-to: type=inline - tags: | - ${{ env.IMAGE }} - ghcr.io/${{ env.IMAGE }} + uses: ./.github/workflows/docker.build.yaml + with: + image: moveit/moveit2:${{ matrix.ROS_DISTRO }}-source + dockerfile: .docker/source/Dockerfile + ros_distro: ${{ matrix.ROS_DISTRO }} + full_source_context: true + secrets: inherit tutorial-source: needs: source @@ -224,57 +88,15 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [lyrical, rolling] - runs-on: ubuntu-latest permissions: packages: write contents: read - env: - IMAGE: moveit/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} - - steps: - - uses: actions/checkout@v7 - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v4 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v4 - with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v4 - 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@v5 - with: - path: .ccache - key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }} - - name: inject ccache into docker - uses: reproducible-containers/buildkit-cache-dance@v3.3.0 - with: - cache-map: | - { - ".ccache": "/root/.ccache/" - } - - name: Build and Push - uses: docker/build-push-action@v7 - 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.IMAGE }} - ghcr.io/${{ env.IMAGE }} + uses: ./.github/workflows/docker.build.yaml + with: + image: moveit/moveit2:main-${{ matrix.ROS_DISTRO }}-tutorial-source + dockerfile: .docker/tutorial-source/Dockerfile + ros_distro: ${{ matrix.ROS_DISTRO }} + secrets: inherit delete_untagged: runs-on: ubuntu-latest diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index 8fd25b1637..d4f12ac205 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -98,10 +98,14 @@ void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& p obj->pose_ = pose; } else + { ensureUnique(obj); + } for (std::size_t i = 0; i < shapes.size(); ++i) + { addToObjectInternal(obj, shapes[i], shape_poses[i]); + } notify(obj, Action(action)); } diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index b57e0767ea..674faa829a 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -108,7 +108,9 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, std::make_shared(link_geometry->collision_geometry_)); } else + { RCLCPP_ERROR(getLogger(), "Unable to construct collision geometry for link '%s'", link->getName().c_str()); + } } } @@ -146,7 +148,9 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, robot_fcl_objs_[index] = std::make_shared(g->collision_geometry_); } else + { RCLCPP_ERROR(getLogger(), "Unable to construct collision geometry for link '%s'", link->getName().c_str()); + } } } @@ -511,7 +515,9 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& li } } else + { RCLCPP_ERROR(getLogger(), "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } } } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index f521391b21..c878f13396 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -184,7 +184,9 @@ void PR2ArmIK::addJointToChainInfo(const urdf::JointConstSharedPtr& joint, movei limit.has_position_limits = true; } else + { limit.has_position_limits = false; + } } else { @@ -198,7 +200,9 @@ void PR2ArmIK::addJointToChainInfo(const urdf::JointConstSharedPtr& joint, movei limit.has_velocity_limits = 1; } else + { limit.has_velocity_limits = 0; + } info.limits.push_back(limit); } 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 0bf50e5559..e1c10bd2da 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -89,7 +89,9 @@ bool PR2ArmIKSolver::getCount(int& count, int max_count, int min_count) return true; } else + { return false; + } } } @@ -160,7 +162,9 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i return 1; } else + { return -1; + } } int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 022d7a7196..ecb322e34d 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -163,10 +163,14 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_model_ = robot_model_->getJointModel(jc.joint_name.substr(0, pos)); if (pos + 1 < jc.joint_name.length()) + { local_variable_name_ = jc.joint_name.substr(pos + 1); + } } else + { joint_model_ = robot_model_->getJointModel(jc.joint_name); + } } if (joint_model_) @@ -267,7 +271,9 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) constraint_weight_ = 1.0; } else + { constraint_weight_ = jc.weight; + } } return joint_model_ != nullptr; } @@ -309,7 +315,9 @@ ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotStat } } else + { dif = current_joint_position - joint_position_; + } // check bounds bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits::epsilon()) && @@ -354,7 +362,9 @@ void JointConstraint::print(std::ostream& out) const out << '\n'; } else + { out << "No constraint" << '\n'; + } } bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& pc, const moveit::core::Transforms& tf) @@ -415,7 +425,9 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p constraint_region_.push_back(body); } else + { RCLCPP_WARN(getLogger(), "Could not construct primitive shape %zu", i); + } } // load meshes @@ -454,7 +466,9 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p constraint_weight_ = 1.0; } else + { constraint_weight_ = pc.weight; + } return !constraint_region_.empty(); } @@ -788,7 +802,9 @@ void OrientationConstraint::print(std::ostream& out) const out << "Desired orientation:" << q_des.x() << ',' << q_des.y() << ',' << q_des.z() << ',' << q_des.w() << '\n'; } else + { out << "No constraint" << '\n'; + } } VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model) @@ -829,7 +845,9 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain cone_sides_ = 3; } else + { cone_sides_ = vc.cone_sides; + } // compute the points on the base circle of the cone that make up the cone sides points_.clear(); @@ -874,7 +892,9 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain constraint_weight_ = 1.0; } else + { constraint_weight_ = vc.weight; + } max_view_angle_ = vc.max_view_angle; max_range_angle_ = vc.max_range_angle; @@ -1215,7 +1235,9 @@ void VisibilityConstraint::print(std::ostream& out) const out << "Target radius: " << target_radius_ << ", using " << cone_sides_ << " sides." << '\n'; } else + { out << "No constraint" << '\n'; + } } void KinematicConstraintSet::clear() diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 0c44ef718b..4f720184b2 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -28,7 +28,15 @@ angles assimp - boost + libboost-dev + libboost-chrono-dev + libboost-date-time-dev + libboost-filesystem-dev + libboost-iostreams-dev + libboost-program-options-dev + libboost-regex-dev + libboost-serialization-dev + libboost-thread-dev bullet common_interfaces eigen diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index cccc55d7a8..71d4eac188 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1068,7 +1068,9 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const out << c.r << ' ' << c.g << ' ' << c.b << ' ' << c.a << '\n'; } else + { out << "0 0 0 0" << '\n'; + } } // Write subframes @@ -1231,7 +1233,9 @@ void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state) moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, robot_state_.value()); } else + { moveit::core::robotStateMsgToRobotState(*scene_transforms_.value(), state_no_attached, robot_state_.value()); + } for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i) { @@ -1868,7 +1872,9 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: geometry_msgs::msg::Pose()); // Empty shape pose => Identity } else + { append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]); + } } } else diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index e3b6e3255b..e5b9066626 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -201,7 +201,9 @@ bool FloatingJointModel::normalizeRotation(double* values) const return true; } else + { return false; + } } unsigned int FloatingJointModel::getStateSpaceDimension() const diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index bed2f9dd0c..ffb61550c4 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -152,7 +152,9 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode active_variable_count_ += vc; } else + { mimic_joints_.push_back(joint_model); + } for (const std::string& name : name_order) { variable_names_.push_back(name); @@ -174,7 +176,9 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode variable_count_ += vc; } else + { fixed_joints_.push_back(joint_model); + } } // now we need to find all the set of joints within this group @@ -718,7 +722,9 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const } } else + { return true; + } } // Did not find any valid tip frame links to use diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 515cc3ea3a..ebf02047f9 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -91,7 +91,9 @@ void RevoluteJointModel::setContinuous(bool flag) variable_bounds_[0].max_position_ = M_PI; } else + { variable_bounds_[0].position_bounded_ = true; + } computeVariableBoundsMsg(); } @@ -167,7 +169,9 @@ void RevoluteJointModel::interpolate(const double* from, const double* to, const } } else + { state[0] = from[0] + (to[0] - from[0]) * t; + } } double RevoluteJointModel::distance(const double* values1, const double* values2) const @@ -178,7 +182,9 @@ double RevoluteJointModel::distance(const double* values1, const double* values2 return (d > M_PI) ? 2.0 * M_PI - d : d; } else + { return fabs(values1[0] - values2[0]); + } } bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 67082811aa..e1e89f85fd 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -155,7 +155,9 @@ void robotStateToMultiDofJointState(const RobotState& state, sensor_msgs::msg::M p = tf2::eigenToTransform(t); } else + { p = tf2::eigenToTransform(state.getJointTransform(joint_model)); + } mjs.joint_names.push_back(joint_model->getName()); mjs.transforms.push_back(p.transform); } @@ -354,7 +356,9 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol } } else + { RCLCPP_ERROR(getLogger(), "The attached body for link '%s' has no geometry", aco.link_name.c_str()); + } } else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { @@ -365,7 +369,9 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol } } else + { RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", aco.object.operation); + } } bool robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index ddb3fa830c..7bc86416ea 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1188,7 +1188,9 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const return nullptr; } else + { return it->second.get(); + } } void RobotState::attachBody(std::unique_ptr attached_body) @@ -1308,7 +1310,9 @@ bool RobotState::clearAttachedBody(const std::string& id) return true; } else + { return false; + } } const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_id, bool* frame_found) @@ -1689,7 +1693,9 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig return constraint(this, jmg, &values[0]); } else + { return true; + } } bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, double timeout, @@ -1876,7 +1882,9 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is return false; } else if (consistency_limit_sets.size() == 1) + { consistency_limits = consistency_limit_sets[0]; + } const std::vector& solver_tip_frames = solver->getTipFrames(); @@ -2334,10 +2342,14 @@ void RobotState::printStatePositionsWithJointLimits(const JointModelGroup* jmg, marker_shown = true; } else + { out << '-'; + } } if (!marker_shown) + { out << '|'; + } // show max position out << " \t" << std::fixed << std::setprecision(5) << bound.max_position_ << " \t" << joint->getName() @@ -2375,7 +2387,9 @@ void RobotState::printStateInfo(std::ostream& out) const out << '\n'; } else + { out << " * Position: NULL\n"; + } if (!velocity_.empty()) { @@ -2385,7 +2399,9 @@ void RobotState::printStateInfo(std::ostream& out) const out << '\n'; } else + { out << " * Velocity: NULL\n"; + } if (has_acceleration_) { @@ -2395,7 +2411,9 @@ void RobotState::printStateInfo(std::ostream& out) const out << '\n'; } else + { out << " * Acceleration: NULL\n"; + } out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL\n"); out << " * Dirty Collision Body Transforms: " diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index d87de7773d..72e57d630b 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -115,10 +115,14 @@ double RobotTrajectory::getAverageSegmentDuration() const return 0.0; } else + { return getDuration() / static_cast(duration_from_previous_.size() - 1); + } } else + { return getDuration() / static_cast(duration_from_previous_.size()); + } } void RobotTrajectory::swap(RobotTrajectory& other) noexcept diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 71edb5473e..7c2f1fe3f8 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -753,7 +753,7 @@ TEST_F(OneRobot, MultiDofTrajectoryToJointStates) // WHEN the optional trajectory result is valid (always assumed) ASSERT_TRUE(maybe_trajectory_msg.has_value()); - const auto traj = maybe_trajectory_msg.value(); + const auto& traj = maybe_trajectory_msg.value(); const auto& joint_names = traj.joint_names; size_t joint_variable_count = 0u; diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 808f18e5d6..c8d394b77b 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -145,7 +145,9 @@ void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& fro RCLCPP_ERROR(getLogger(), "Cannot record transform with empty name"); } else + { transforms_map_[from_frame] = t; + } } void Transforms::setTransform(const geometry_msgs::msg::TransformStamped& transform) diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp index 3c3a1d234a..2836712006 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp @@ -96,7 +96,9 @@ int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, jacToJacReduced(jac_, jac_reduced_); } else + { jnt2jac_.JntToJac(q_in, jac_reduced_); + } // weight Jacobian auto& jac = jac_reduced_.data; diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index fd39bab192..738f8d3393 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -64,7 +64,6 @@ rclcpp::Logger getLogger() const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; const double DEFAULT_SEARCH_DISCRETIZATION = 0.01f; const double EXPECTED_SUCCESS_RATE = 0.8; -static const std::string UNDEFINED = ""; // As loading of parameters is quite slow, we share them across all tests class SharedData diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 41186e9db4..094b68e6e3 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -239,7 +239,9 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s } } else + { break; + } } // end of while loop // resetting the CHOMP Parameters to the original values after a successful plan diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp index cd06ec5c95..9a832a90ed 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp @@ -99,7 +99,9 @@ void ompl_interface::ConstrainedSampler::sampleUniformNear(ob::State* state, con } } else + { default_->sampleUniformNear(state, near, distance); + } } void ompl_interface::ConstrainedSampler::sampleGaussian(ob::State* state, const ob::State* mean, const double stdDev) @@ -115,5 +117,7 @@ void ompl_interface::ConstrainedSampler::sampleGaussian(ob::State* state, const } } else + { default_->sampleGaussian(state, mean, stdDev); + } } diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index d066d9f9b4..e64f1e2d40 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -158,7 +158,9 @@ class ConstraintApproximationStateSampler : public ob::StateSampler space_->interpolate(near, state_storage_->getState(index), d / dist, state); } else + { space_->copyState(state, state_storage_->getState(index)); + } } void sampleGaussian(ob::State* state, const ob::State* mean, const double stdDev) override @@ -455,7 +457,9 @@ ConstraintApproximationConstructionResults ConstraintsLibrary::addConstraintAppr res.approx = constraint_approx; } else + { RCLCPP_ERROR(getLogger(), "Unable to construct constraint approximation for group '%s'", group.c_str()); + } return res; } diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 4b86b48c96..974fb479cf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -47,8 +47,6 @@ moveit_configs_utils launch_param_builder - boost - ament_cmake_gmock ament_cmake_gtest ros_testing 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 03083f3ae5..61a1a07593 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 @@ -165,13 +165,12 @@ void CommandListManager::checkForOverlappingRadii(const MotionResponseCont& resp CommandListManager::RobotState_OptRef CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_responses, const std::string& group_name) { - for (MotionResponseCont::const_reverse_iterator it = motion_plan_responses.crbegin(); - it != motion_plan_responses.crend(); ++it) + const auto it = + std::find_if(motion_plan_responses.crbegin(), motion_plan_responses.crend(), + [&group_name](const auto& response) { return response.trajectory->getGroupName() == group_name; }); + if (it != motion_plan_responses.crend()) { - if (it->trajectory->getGroupName() == group_name) - { - return std::reference_wrapper(it->trajectory->getLastWayPoint()); - } + return std::reference_wrapper(it->trajectory->getLastWayPoint()); } return {}; } 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 24acb2b0a2..0274d7158b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -1029,7 +1029,9 @@ bool testutils::getBlendTestData(const rclcpp::Node::SharedPtr& node, const size return true; } else + { return false; + } }; for (size_t i = 1; i < dataset_num + 1; ++i) 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 4609170b03..4608b72bf9 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 @@ -70,10 +70,6 @@ static constexpr double L2{ 0.3070 }; // Height of second connector static constexpr double L3{ 0.0840 }; // Distance last joint to flange // parameters from parameter server -const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); -const std::string GROUP_TIP_LINK_NAME("group_tip_link"); -const std::string ROBOT_TCP_LINK_NAME("tcp_link"); -const std::string IK_FAST_LINK_NAME("ik_fast_link"); const std::string RANDOM_TEST_NUMBER("random_test_number"); /** 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 3fcb254aac..b1ee2d893e 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 @@ -48,8 +48,6 @@ #include // parameters from parameter server -const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); -const std::string PARAM_TARGET_LINK_NAME("target_link"); const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance"); diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp index 7240a0da7a..505b792a8a 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -120,7 +120,7 @@ class CmdGetterAdapter : public XmlTestdataLoader::AbstractCmdGetterAdapter public: using FuncType = std::function; - CmdGetterAdapter(FuncType func) : AbstractCmdGetterAdapter(), func_(func) + CmdGetterAdapter(FuncType func) : AbstractCmdGetterAdapter(), func_(std::move(func)) { } diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index b38c84a596..166759420a 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -59,7 +59,7 @@ namespace * @return Concatenated result string. */ template -std::string concatenateWithSeparator(char separator, T... content) +std::string concatenateWithSeparator(char separator, const T&... content) { std::string result; (result.append(content).append({ separator }), ...); @@ -75,7 +75,7 @@ std::string concatenateWithSeparator(char separator, T... content) * base_namespace.controller_name.param_name */ template -std::string makeParameterName(T... strings) +std::string makeParameterName(const T&... strings) { return concatenateWithSeparator('.', strings...); } diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp index 625ec2eeae..f21a6d9593 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -67,13 +67,13 @@ void update(moveit::core::RobotState* self, bool force, std::string& category) Eigen::MatrixXd getFrameTransform(const moveit::core::RobotState* self, std::string& frame_id) { bool frame_found; - auto transformation = self->getFrameTransform(frame_id, &frame_found); + const auto& transformation = self->getFrameTransform(frame_id, &frame_found); return transformation.matrix(); } Eigen::MatrixXd getGlobalLinkTransform(const moveit::core::RobotState* self, std::string& link_name) { - auto transformation = self->getGlobalLinkTransform(link_name); + const auto& transformation = self->getGlobalLinkTransform(link_name); return transformation.matrix(); } diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 3a788d8919..01505aee87 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -256,6 +256,7 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) } std::vector> pipeline_planner_id_pairs; + pipeline_planner_id_pairs.reserve(pipelines.size()); for (size_t i = 0; i < pipelines.size(); ++i) { pipeline_planner_id_pairs.push_back(std::pair(pipelines.at(i), planner_ids.at(i))); diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 0bf79bb987..3e260a4298 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -213,10 +213,14 @@ bool MoveGroupCartesianPathService::computeService( } } else + { res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE; + } } else + { res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; + } return true; } diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 2120ee1d20..b6e2d37edf 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -119,10 +119,14 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::msg::PositionIKRequest& error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } else + { error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + } } else + { error_code.val = moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE; + } } else { @@ -156,13 +160,17 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::msg::PositionIKRequest& error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } else + { error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + } } } } } else + { error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; + } } bool MoveGroupKinematicsService::computeIKService(const std::shared_ptr& /* unused */, 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 0198a71b5b..6e1174f6e8 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 @@ -104,7 +104,9 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptrplanned_trajectory); // @todo: Response messages diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index e071d7648c..98449912da 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -130,7 +130,9 @@ class MoveGroupExe } } else + { RCLCPP_ERROR(getLogger(), "No MoveGroup context created. Nothing will work."); + } } MoveGroupContextPtr getContext() @@ -341,7 +343,9 @@ int main(int argc, char** argv) rclcpp::shutdown(); } else + { RCLCPP_ERROR(nh->get_logger(), "Planning scene not configured"); + } return 0; } diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index e8943aa02b..7b8bb7a42e 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -174,10 +174,14 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) } } else + { updater->setTransformCacheCallback(transform_cache_callback_); + } } else + { RCLCPP_ERROR(logger_, "nullptr updater was specified"); + } } void OccupancyMapMonitor::publishDebugInformation(bool flag) @@ -265,15 +269,21 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s return false; } else + { cache[jt->second] = it.second; + } } return true; } else + { return false; + } } else + { return false; + } } bool OccupancyMapMonitor::saveMapCallback(const std::shared_ptr& /* unused */, 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 9703000230..3dbb114ab6 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 @@ -216,7 +216,9 @@ mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::Sha } } else + { RCLCPP_ERROR(logger_, "Mesh filter not yet initialized!"); + } return h; } @@ -370,7 +372,9 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: } } else + { return; + } } if (!updateTransformCache(depth_msg->header.frame_id, depth_msg->header.stamp)) diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index e528cb5b55..f74bb8267e 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -411,7 +411,9 @@ void mesh_filter::GLRenderer::createGLContext() s_context.at(thread_id) = std::pair(1, window_id); } else + { ++(context_it->second.first); + } } void mesh_filter::GLRenderer::deleteGLContext() diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index 73a532fbe4..7686f0e6b0 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -94,7 +94,9 @@ point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addSh used_handles_[next_handle_] = insert_op.first; } else + { return 0; + } ShapeHandle ret = next_handle_; const std::size_t sz = min_handle_ + bodies_.size() + 1; @@ -123,7 +125,9 @@ void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle) min_handle_ = handle; } else + { RCLCPP_ERROR(getLogger(), "Unable to remove shape handle %u", handle); + } } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::msg::PointCloud2& data_in, diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 076ae0d0ea..e11a9f41d6 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -270,7 +270,9 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::msg::PointClo } } else + { return; + } } /* compute sensor origin in map frame */ diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index 9f8993c613..b64ebca3e8 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -138,7 +138,9 @@ int main(int argc, char** argv) } } else + { psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions(); + } moveit_msgs::msg::PlanningScene psmsg; psm.getPlanningScene()->getPlanningSceneMsg(psmsg); diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index 7c9d15eb55..acef597808 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -110,7 +110,9 @@ int main(int argc, char** argv) std::cin.get(); } else + { rclcpp::sleep_for(500ms); + } std::vector states; RCLCPP_INFO(node->get_logger(), "Sampling %u valid states...", nthreads); @@ -132,6 +134,7 @@ int main(int argc, char** argv) } std::vector threads; + threads.reserve(states.size()); runCollisionDetection(10, trials, *psm.getPlanningScene(), *states[0]); for (unsigned int i = 0; i < states.size(); ++i) { @@ -147,7 +150,9 @@ int main(int argc, char** argv) } } else + { RCLCPP_ERROR(node->get_logger(), "Planning scene not configured"); + } return 0; } diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index eca1c474b9..3241f0a1ab 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -215,7 +215,9 @@ bool CurrentStateMonitor::haveCompleteStateHelper(const rclcpp::Time& oldest_all joint->getName().c_str(), (oldest_allowed_update_time - it->second).seconds()); } else + { continue; + } if (missing_joints) { @@ -314,7 +316,9 @@ bool CurrentStateMonitor::waitForCompleteState(const std::string& group, double } } else + { ok = false; + } } return ok; } 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 e7912941be..83f67e15cb 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 @@ -705,7 +705,9 @@ void PlanningSceneMonitor::updatePublishSettings(bool publish_geom_updates, bool startPublishingPlanningScene(event); } else + { stopPublishingPlanningScene(); + } } void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) @@ -1391,7 +1393,9 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top } } else + { RCLCPP_ERROR(logger_, "Cannot monitor robot state because planning scene is not configured"); + } } void PlanningSceneMonitor::stopStateMonitor() 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 126469542a..3b45debfc3 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 @@ -545,7 +545,9 @@ void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci } } else if (verbose_) + { RCLCPP_INFO(logger_, "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str()); + } } void TrajectoryExecutionManager::updateControllersState(const rclcpp::Duration& age) @@ -1234,10 +1236,14 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) } if (auto_clear) + { clear(); + } } else + { execution_state_mutex_.unlock(); + } } else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we // join it now @@ -1307,7 +1313,9 @@ void TrajectoryExecutionManager::clear() trajectories_.clear(); } else + { RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!"); + } } void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback, @@ -1557,7 +1565,9 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } } else + { handle->waitForExecution(); + } // if something made the trajectory stop, we stop this thread too if (execution_complete_) @@ -1769,7 +1779,9 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector diff; std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(), @@ -1823,10 +1835,14 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vectorswitchControllers(controllers_to_activate, controllers_to_deactivate); } else + { return false; + } } else + { return true; + } } else { 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 e83c649668..6bb460d2fa 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 @@ -88,8 +88,6 @@ namespace planning_interface const std::string MoveGroupInterface::ROBOT_DESCRIPTION = "robot_description"; // name of the robot description (a param name, so it can be changed externally) -const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps - namespace { enum ActiveTargetType @@ -462,10 +460,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl c->enforceBounds(); getTargetRobotState() = *c; if (!getTargetRobotState().satisfiesBounds(getGoalJointTolerance())) + { return false; + } } else + { return false; + } // we may need to do approximate IK kinematics::KinematicsQueryOptions o; @@ -498,7 +500,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } else + { return false; + } } void setEndEffectorLink(const std::string& end_effector) @@ -685,7 +689,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl RCLCPP_INFO(logger_, "Planning request rejected"); } else + { RCLCPP_INFO(logger_, "Planning request accepted"); + } }; send_goal_opts.result_callback = [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -762,7 +768,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl RCLCPP_INFO(logger_, "Plan and Execute request rejected"); } else + { RCLCPP_INFO(logger_, "Plan and Execute request accepted"); + } }; send_goal_opts.result_callback = [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -825,7 +833,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl RCLCPP_INFO(logger_, "Execute request rejected"); } else + { RCLCPP_INFO(logger_, "Execute request accepted"); + } }; send_goal_opts.result_callback = [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -902,7 +912,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return response->fraction; } else + { return -1.0; + } } else { @@ -1073,10 +1085,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } else + { RCLCPP_ERROR(logger_, "Unable to construct MotionPlanRequest representation"); + } if (path_constraints_) + { request.path_constraints = *path_constraints_; + } if (trajectory_constraints_) request.trajectory_constraints = *trajectory_constraints_; } @@ -1103,10 +1119,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return true; } else + { return false; + } } else + { return false; + } } void clearPathConstraints() diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index 3426dee360..6bd3c138f7 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -242,7 +242,9 @@ void InteractionHandler::handleEndEffector( pose_map_lock_.unlock(); } else + { return; + } StateChangeCallbackFn callback; @@ -274,7 +276,9 @@ void InteractionHandler::handleJoint(const JointInteraction& vj, pose_map_lock_.unlock(); } else + { return; + } StateChangeCallbackFn callback; diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 6f9c2a8fc0..0831464779 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -81,8 +81,6 @@ namespace { const std::string EXECUTION_TIME = "execution_time_s"; -const std::string FRACTION = "fraction"; -const std::string PLANNING_TIME = "planning_time_s"; } // namespace 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 dfad31931d..2faf9b1c9c 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 @@ -344,10 +344,14 @@ void MotionPlanningDisplay::updateBackgroundJobProgressBar() { p->setMaximum(n); if (n > 1) // only show bar if there will be a progress to show + { p->show(); + } } else // progress + { p->setValue(p->maximum() - n); + } p->update(); } } @@ -677,7 +681,9 @@ void MotionPlanningDisplay::drawQueryStartState() } } else + { query_robot_start_->setVisible(false); + } context_->queueRender(); } @@ -801,7 +807,9 @@ void MotionPlanningDisplay::drawQueryGoalState() } } else + { query_robot_goal_->setVisible(false); + } context_->queueRender(); } @@ -986,7 +994,9 @@ bool MotionPlanningDisplay::isIKSolutionCollisionFree(moveit::core::RobotState* return res; } else + { return true; + } } void MotionPlanningDisplay::updateLinkColors() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index f3faa13b3b..60d22f4084 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -183,10 +183,14 @@ void MotionPlanningFrame::sceneScaleChanged(int value) planning_display_->queueRenderSceneGeometry(); } else + { scaled_object_.reset(); + } } else + { scaled_object_.reset(); + } } } @@ -362,7 +366,9 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() } } else + { ui_->object_status->setText("ERROR: '" + sel[0]->text() + "' should be a collision object but it is not"); + } } if (update_scene_marker && ui_->tabWidget->tabText(ui_->tabWidget->currentIndex()).toStdString() == TAB_OBJECTS) { @@ -716,10 +722,14 @@ void MotionPlanningFrame::computeLoadSceneButtonClicked() planning_scene_publisher_->publish(diff); } else + { planning_scene_publisher_->publish(static_cast(*scene_m)); + } } else + { planning_scene_publisher_->publish(static_cast(*scene_m)); + } } else { @@ -1052,7 +1062,9 @@ void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path) RCLCPP_INFO(logger_, "Saved current scene geometry to '%s'", p.c_str()); } else + { RCLCPP_WARN(logger_, "Unable to save current scene geometry to '%s'", p.c_str()); + } } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 115f32bc6a..bf9338a207 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -99,10 +99,14 @@ void MotionPlanningFrame::pathConstraintsIndexChanged(int index) { std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString(); if (!move_group_->setPathConstraints(c)) + { RCLCPP_WARN_STREAM(logger_, "Unable to set the path constraints: " << c); + } } else + { move_group_->clearPathConstraints(); + } } } 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 bfb391b823..d7989d2756 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 @@ -184,7 +184,9 @@ void MotionPlanningFrame::saveQueryButtonClicked() } } else + { return; + } } } planning_display_->addBackgroundJob( diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 1280f2e38a..e40add3a42 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -167,7 +167,9 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const moveit::core::RobotS } } else + { QMessageBox::warning(this, "Start state not saved", "Cannot use an empty name for a new start state."); + } } populateRobotStatesList(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp index 58ce158323..eaa8ebc9e9 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp @@ -109,7 +109,9 @@ rviz_common::properties::Property* MotionPlanningParamWidget::createPropertyTree new rviz_common::properties::FloatProperty(key, value_double, QString(), root, SLOT(changedValue()), this); } else + { new rviz_common::properties::StringProperty(key, value, QString(), root, SLOT(changedValue()), this); + } } return root; } 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 483b10d9ed..e8c9134bf8 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 @@ -426,7 +426,9 @@ void RobotStateDisplay::loadRobotModel() } } else + { setStatus(rviz_common::properties::StatusProperty::Error, "RobotModel", "Loading failed"); + } highlights_.clear(); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp index 3afee04963..f8e6844b77 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp @@ -130,10 +130,14 @@ void MeshShape::endTriangles() offset_node_->attachObject(entity_); } else + { RVIZ_COMMON_LOG_ERROR("Unable to construct triangle mesh"); + } } else + { RVIZ_COMMON_LOG_ERROR("No triangles added"); + } } void MeshShape::clear() diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index a33d2b328a..3e95687782 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -139,7 +139,9 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())); } else + { RCLCPP_ERROR(getLogger(), "Unknown link constraint element: '%s'", type.c_str()); + } in >> type; } diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index af622f8c9d..f049b8cef7 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -219,7 +219,9 @@ bool moveit_warehouse::PlanningSceneStorage::getPlanningSceneWorld(moveit_msgs:: return true; } else + { return false; + } } bool moveit_warehouse::PlanningSceneStorage::getPlanningScene(PlanningSceneWithMetadata& scene_m, diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index c54f86e385..9c4312d470 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -93,7 +93,9 @@ void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor& psm, moveit_war } } else + { RCLCPP_INFO(getLogger(), "Scene name is empty. Not saving."); + } } void onMotionPlanRequest(const moveit_msgs::msg::MotionPlanRequest& req, diff --git a/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp b/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp index e3eb184b59..687a9e6447 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp @@ -58,7 +58,9 @@ void XmlSyntaxHighlighter::addTag(const QString& tag, const QTextCharFormat& for }); } else + { rule.parent = rules_.end(); + } rules_.insert(std::make_pair(rules_.size(), rule)); } diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp index bca5543eae..e33a54e1d1 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp @@ -619,12 +619,18 @@ void PlanningGroupsWidget::deleteGroup() // Get the user custom properties of the currently selected row PlanGroupType plan_group = item->data(0, Qt::UserRole).value(); if (plan_group.group_) + { group_to_delete = plan_group.group_->name_; + } } else + { current_edit_group_.clear(); + } if (group_to_delete.empty()) + { return; + } // Confirm user wants to delete group if (QMessageBox::question(this, "Confirm Group Deletion", diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp index 9568f5cfa2..9808bf1471 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp @@ -564,7 +564,9 @@ void RobotPosesWidget::doneEditing() } } else + { searched_data = current_edit_pose_; // overwrite edited pose + } // Save the new pose name or create the new pose ---------------------------- bool is_new = false;