From afc17917db96806881c5472f46b9fb32357e440c Mon Sep 17 00:00:00 2001 From: Abhijeet Dasgupta Date: Sun, 9 Apr 2023 19:16:57 +0530 Subject: [PATCH] loading one yaml param MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Increase priority for constrained planning state space (#1300) * Change priority for the constrained planning state space * Fix constrained planning tests * Use PRM instead of RRTConnect --------- Co-authored-by: Sebastian Jahr Remove "new" from smart pointer instantiation (#2019) Temporarily disable TestPathConstraints with the Panda robot (#2016) This test has become flaky since it was modified to use the OMPL constrained state space (https://github.com/ros-planning/moveit2/issues/2015). Fix mimic joints with TOTG (#1989) Fix include install destination (#2008) Co-authored-by: Henning Kayser Co-authored-by: Tyler Weaver Ruckig-smoothing : reduce number of duration extensions (#1990) * extend duration only for failed segment * update comment * Remove trajectory reset before extension * readability improvement * Remove call to RobotState update --------- Co-authored-by: ibrahiminfinite Co-authored-by: AndyZe Add stale GHA (#2022) * Issues and PRs are labeled as stale after 45 days. * Stale issues are closed after another 45 days. Enable workflow_dispatch for stale GHA Remove invalid description field in GHA Add callback for velocity scaling override + fix params namespace not being set (#2021) Fix python tests (#1979) * ensure joint models in robot_model submodule * add build tests Upgrade apt dependencies --with-new-pkgs (#2039) 2.7.1 🛠️ Bump actions/stale from 7 to 8 (#2037) Allow ci-testing Dockerfile to update the ROS_DISTRO (#2035) Move displaced launch file into planning_component_tools (#2044) Delete the Ruckig "batches" option, deprecated by #1990 (#2028) Added set_robot_trajectory_msg to python bindings (#2050) Use $DISPLAY rather than assuming :0 (#2049) * Use $DISPLAY rather than assuming : * Double quotes --------- Co-authored-by: Sebastian Jahr Optionally mitigate Ruckig overshoot (#2051) * Optionally mitigate Ruckig overshoot * Cleanup Update description of moveit_ros_planning_interface (#2045) * Update description of moveit_ros_planning_interface * Update moveit_ros/planning_interface/package.xml Co-authored-by: Henning Kayser --------- Co-authored-by: Henning Kayser Add URDF Loader Exceptions and Fix Infinite While-Loop when URDF file isn't in a ROS Package (#2032) * Fixed infinite while loop in utilities.cpp and added some exception handling to start screen widget * Fix trailing whitespace, fix getSharePath exception catch on empty request * Fix clang tidy suggestion and error message updates based on pr comments [hybrid planning] improve planning scene monitoring (#1090) * Create new PSM in local constraint solver. Different type of collision checking. * Small boolean logic fixup * Don't configure planning scene monitor twice and pass scene as const * Remove not required call of updateSceneWithCurrentState() * Revert PlanningSceneMonitorConstPtr to PlanningSceneMonitorPtr * Set planning_scene_monitor update rate * Decrease planning scene update rate * Use `updateSceneWithCurrentState()` in psm * Revert the manner of collision checking --------- Co-authored-by: Sebastian Jahr Document how collision checking includes descendent links (#2058) Move stateless PlanningScene helper functions out of the class (#2025) Readability: kinematic_state -> robot_state (#2078) moveit_py citation (#2029) Extract parallel planning from moveit cpp (#2043) * Add parallel_planning_interface * Add parallel planning interface * Rename package to pipeline_planning_interface * Move plan_responses_container into own header + source file * Add plan_responses_contrainer source file * Add solution selection and stopping criterion function files * Remove parallel planning from moveit_cpp * Move parallel planning into planning package * Update moveit_cpp * Drop planning_interface changes * Add documentation * Update other moveit packages * Remove removed header * Address CI complains * Address clang-tidy complains * Address clang-tidy complains 2 * Address clang-tidy complains 3 * Extract planning pipeline map creation function from moveit_cpp * Cleanup comment * Use const moveit::core::RobotModelConstPtr& * Formatting * Add header descriptions * Remove superfluous TODOs * Cleanup PILZ: Throw if IK solver doesn't exist (#2082) * Throw if IK solver doesn't exist * Format enabled -wformat (#2065) Co-authored-by: Sebastian Jahr Add test and debug issue where TOTG returns accels > limit (#2084) lint fix lint fix 1 lint fix 2 lint fix 3 --- .docker/ci-testing/Dockerfile | 12 +- .docker/ci/Dockerfile | 2 +- .github/workflows/docker.yaml | 2 +- .github/workflows/stale.yaml | 26 ++ moveit/CHANGELOG.rst | 3 + moveit/package.xml | 2 +- moveit_common/CHANGELOG.rst | 3 + moveit_common/cmake/moveit_package.cmake | 2 +- moveit_common/package.xml | 2 +- moveit_configs_utils/CHANGELOG.rst | 3 + .../moveit_configs_utils/launches.py | 4 +- moveit_configs_utils/package.xml | 2 +- moveit_configs_utils/setup.py | 2 +- moveit_core/CHANGELOG.rst | 30 +++ .../collision_detection/collision_common.h | 6 +- moveit_core/package.xml | 2 +- moveit_core/planning_scene/CMakeLists.txt | 4 +- .../moveit/planning_scene/planning_scene.h | 72 +++--- .../planning_scene/src/planning_scene.cpp | 126 +++++---- .../moveit/robot_model/joint_model_group.h | 6 +- .../robot_model/src/joint_model_group.cpp | 26 +- moveit_core/robot_model/src/robot_model.cpp | 2 +- .../ruckig_traj_smoothing.h | 41 ++- .../time_optimal_trajectory_generation.h | 8 + .../src/ruckig_traj_smoothing.cpp | 190 ++++++-------- .../time_optimal_trajectory_generation.cpp | 70 ++--- ...est_time_optimal_trajectory_generation.cpp | 62 +++++ moveit_kinematics/CHANGELOG.rst | 13 + moveit_kinematics/package.xml | 2 +- moveit_kinematics/test/benchmark_ik.cpp | 20 +- .../chomp/chomp_interface/CHANGELOG.rst | 7 + .../chomp/chomp_interface/package.xml | 2 +- .../chomp/chomp_motion_planner/CHANGELOG.rst | 7 + .../chomp/chomp_motion_planner/package.xml | 2 +- .../chomp_optimizer_adapter/CHANGELOG.rst | 7 + .../chomp/chomp_optimizer_adapter/package.xml | 2 +- moveit_planners/moveit_planners/CHANGELOG.rst | 3 + moveit_planners/moveit_planners/package.xml | 2 +- moveit_planners/ompl/CHANGELOG.rst | 18 ++ moveit_planners/ompl/CMakeLists.txt | 2 +- .../ompl/ompl_interface/CMakeLists.txt | 2 +- ...constrained_planning_state_space_factory.h | 8 +- ...nstrained_planning_state_space_factory.cpp | 12 +- .../test/test_planning_context_manager.cpp | 24 +- moveit_planners/ompl/package.xml | 2 +- .../CHANGELOG.rst | 11 + .../package.xml | 2 +- .../src/trajectory_generator_lin.cpp | 4 +- .../integrationtest_command_list_manager.cpp | 10 +- ...ntegrationtest_plan_components_builder.cpp | 8 +- .../src/unittest_get_solver_tip_frame.cpp | 4 +- ..._pilz_industrial_motion_planner_direct.cpp | 4 +- .../src/unittest_trajectory_functions.cpp | 4 + .../src/unittest_trajectory_generator.cpp | 38 ++- .../unittest_trajectory_generator_circ.cpp | 22 +- .../src/unittest_trajectory_generator_lin.cpp | 8 +- .../src/unittest_trajectory_generator_ptp.cpp | 12 +- .../CHANGELOG.rst | 3 + .../package.xml | 2 +- .../CHANGELOG.rst | 7 + .../CMakeLists.txt | 2 +- .../package.xml | 2 +- .../prbt_moveit_config/CHANGELOG.rst | 3 + .../prbt_moveit_config/package.xml | 2 +- .../prbt_pg70_support/CHANGELOG.rst | 3 + .../prbt_pg70_support/package.xml | 2 +- .../test_configs/prbt_support/CHANGELOG.rst | 3 + .../test_configs/prbt_support/package.xml | 2 +- moveit_plugins/moveit_plugins/CHANGELOG.rst | 3 + moveit_plugins/moveit_plugins/package.xml | 2 +- .../CHANGELOG.rst | 9 + .../moveit_ros_control_interface/package.xml | 2 +- .../CHANGELOG.rst | 7 + .../package.xml | 2 +- moveit_py/CITATION.cff | 8 + moveit_py/CMakeLists.txt | 28 +- moveit_py/README.md | 11 + moveit_py/package.xml | 6 +- .../moveit_core/robot_model/joint_model.cpp | 4 +- .../robot_model/joint_model_group.cpp | 4 +- .../robot_trajectory/robot_trajectory.cpp | 20 ++ .../robot_trajectory/robot_trajectory.h | 3 + .../src/moveit/moveit_py_utils/CMakeLists.txt | 2 +- .../moveit_cpp/planning_component.cpp | 19 +- .../moveit_cpp/planning_component.h | 5 +- moveit_py/test/unit/test_robot_model.py | 48 ++-- moveit_py/test/unit/test_robot_state.py | 115 ++++----- moveit_ros/benchmarks/CHANGELOG.rst | 12 + moveit_ros/benchmarks/package.xml | 2 +- .../benchmarks/src/BenchmarkExecutor.cpp | 5 +- moveit_ros/hybrid_planning/CHANGELOG.rst | 7 + .../src/forward_trajectory.cpp | 5 +- .../src/local_planner_component.cpp | 6 +- moveit_ros/hybrid_planning/package.xml | 2 +- moveit_ros/move_group/CHANGELOG.rst | 10 + moveit_ros/move_group/package.xml | 2 +- moveit_ros/moveit_ros/CHANGELOG.rst | 3 + moveit_ros/moveit_ros/package.xml | 2 +- moveit_ros/moveit_servo/CHANGELOG.rst | 5 + .../include/moveit_servo/servo_calcs.h | 2 + .../include/moveit_servo/servo_parameters.h | 2 +- .../launch/servo_example.launch.py | 17 ++ moveit_ros/moveit_servo/package.xml | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 26 +- .../moveit_servo/src/servo_parameters.cpp | 2 + .../occupancy_map_monitor/CHANGELOG.rst | 3 + moveit_ros/occupancy_map_monitor/package.xml | 2 +- moveit_ros/perception/CHANGELOG.rst | 7 + moveit_ros/perception/package.xml | 2 +- moveit_ros/planning/CHANGELOG.rst | 24 ++ moveit_ros/planning/CMakeLists.txt | 3 + moveit_ros/planning/moveit_cpp/CMakeLists.txt | 2 +- .../include/moveit/moveit_cpp/moveit_cpp.h | 6 +- .../moveit/moveit_cpp/planning_component.h | 38 +-- .../planning/moveit_cpp/src/moveit_cpp.cpp | 29 +-- .../moveit_cpp/src/planning_component.cpp | 241 ++++++++---------- moveit_ros/planning/package.xml | 2 +- .../launch/collision_checker_compare.launch | 0 .../CMakeLists.txt | 21 ++ .../plan_responses_container.hpp | 74 ++++++ .../planning_pipeline_interfaces.hpp | 118 +++++++++ .../solution_selection_functions.hpp | 54 ++++ .../stopping_criterion_functions.hpp} | 17 +- .../src/plan_responses_container.cpp | 60 +++++ .../src/planning_pipeline_interfaces.cpp | 192 ++++++++++++++ .../src/solution_selection_functions.cpp} | 62 +---- .../src/stopping_criterion_function.cpp} | 17 +- moveit_ros/planning_interface/CHANGELOG.rst | 10 + moveit_ros/planning_interface/package.xml | 4 +- moveit_ros/robot_interaction/CHANGELOG.rst | 7 + moveit_ros/robot_interaction/package.xml | 2 +- moveit_ros/visualization/CHANGELOG.rst | 11 + moveit_ros/visualization/package.xml | 2 +- .../planning_link_updater.h | 4 +- .../robot_state_visualization.h | 10 +- .../src/planning_link_updater.cpp | 6 +- .../src/robot_state_visualization.cpp | 22 +- moveit_ros/warehouse/CHANGELOG.rst | 7 + moveit_ros/warehouse/package.xml | 2 +- moveit_runtime/CHANGELOG.rst | 3 + moveit_runtime/package.xml | 2 +- .../moveit_setup_app_plugins/CHANGELOG.rst | 7 + .../moveit_setup_app_plugins/package.xml | 2 +- .../moveit_setup_assistant/CHANGELOG.rst | 3 + .../moveit_setup_assistant/package.xml | 2 +- .../moveit_setup_controllers/CHANGELOG.rst | 7 + .../moveit_setup_controllers/package.xml | 2 +- .../moveit_setup_core_plugins/CHANGELOG.rst | 3 + .../moveit_setup_core_plugins/package.xml | 2 +- .../src/start_screen_widget.cpp | 14 +- .../moveit_setup_framework/CHANGELOG.rst | 7 + .../moveit_setup_framework/utilities.hpp | 10 +- .../moveit_setup_framework/package.xml | 2 +- .../src/urdf_config.cpp | 24 +- .../moveit_setup_framework/src/utilities.cpp | 3 +- .../moveit_setup_srdf_plugins/CHANGELOG.rst | 7 + .../moveit_setup_srdf_plugins/package.xml | 2 +- 157 files changed, 1771 insertions(+), 805 deletions(-) create mode 100644 .github/workflows/stale.yaml create mode 100644 moveit_py/CITATION.cff rename moveit_ros/planning/{ => planning_components_tools}/launch/collision_checker_compare.launch (100%) create mode 100644 moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt create mode 100644 moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp create mode 100644 moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp create mode 100644 moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp rename moveit_ros/planning/{moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp => planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp} (80%) create mode 100644 moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp create mode 100644 moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp rename moveit_ros/planning/{moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp => planning_pipeline_interfaces/src/solution_selection_functions.cpp} (59%) rename moveit_ros/planning/{moveit_cpp/src/parallel_planning_callbacks.cpp => planning_pipeline_interfaces/src/stopping_criterion_function.cpp} (80%) diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index 88ee8eb1ee7..45c18e07d63 100644 --- a/.docker/ci-testing/Dockerfile +++ b/.docker/ci-testing/Dockerfile @@ -1,12 +1,20 @@ -# ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing +# ghcr.io/ros-planning/moveit2:${OUR_ROS_DISTRO}-ci-testing # CI image using the ROS testing repository -ARG ROS_DISTRO=rolling FROM osrf/ros2:testing LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de ENV TERM xterm +# Overwrite the ROS_DISTRO set in osrf/ros2:testing to the distro tied to this Dockerfile (OUR_ROS_DISTRO). +# In case ROS_DISTRO is now different from what was set in osrf/ros2:testing, run `rosdep update` again +# to get any missing dependencies. +# https://docs.docker.com/engine/reference/builder/#using-arg-variables explains why ARG and ENV can't have +# the same name (ROS_DISTRO is an ENV in the osrf/ros2:testing image). +ARG OUR_ROS_DISTRO=rolling +ENV ROS_DISTRO=${OUR_ROS_DISTRO} +RUN rosdep update --rosdistro $ROS_DISTRO + # Install ROS 2 base packages and build tools # We are installing ros--ros-base here to mimic the behavior of the ros:-ros-base images. # This step is split into a separate layer so that we can rely on cached dependencies instead of having diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index 931f3d6c1e5..c6aa5ee42ce 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -18,7 +18,7 @@ COPY . src/moveit2 RUN \ # Update apt package list as previous containers clear the cache apt-get -q update && \ - apt-get -q -y upgrade && \ + apt-get -q -y upgrade --with-new-pkgs && \ # # Install some base dependencies apt-get -q install --no-install-recommends -y \ diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 35e75e1db4c..68ac0ba0e09 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -131,7 +131,7 @@ jobs: uses: docker/build-push-action@v4 with: file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} + build-args: OUR_ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} no-cache: true tags: | diff --git a/.github/workflows/stale.yaml b/.github/workflows/stale.yaml new file mode 100644 index 00000000000..9fde349d0e6 --- /dev/null +++ b/.github/workflows/stale.yaml @@ -0,0 +1,26 @@ +name: 'Stale issues and PRs' +on: + workflow_dispatch: + schedule: + # UTC noon every workday + - cron: '0 12 * * MON-FRI' + +jobs: + stale: + runs-on: ubuntu-latest + permissions: + issues: write + pull-requests: write + steps: + - uses: actions/stale@v8 + with: + stale-issue-label: 'stale' + stale-pr-label: 'stale' + stale-issue-message: 'This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.' + close-issue-message: 'This issue was closed because it has been stalled for 45 days with no activity.' + stale-pr-message: 'This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.' + days-before-stale: 45 + days-before-close: 45 + days-before-pr-close: -1 + exempt-all-milestones: true + exempt-issue-labels: good first issue,persistent,release,roadmap,Epic diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 0289c9eb777..8afbdf35375 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit/package.xml b/moveit/package.xml index 77dd45b22cb..37e78dd8b5b 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -2,7 +2,7 @@ moveit - 2.7.0 + 2.7.1 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 85ee0f045cd..f58673d8732 100644 --- a/moveit_common/CHANGELOG.rst +++ b/moveit_common/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_common/cmake/moveit_package.cmake b/moveit_common/cmake/moveit_package.cmake index 22b47e4cbcd..332334d5f10 100644 --- a/moveit_common/cmake/moveit_package.cmake +++ b/moveit_common/cmake/moveit_package.cmake @@ -44,7 +44,7 @@ macro(moveit_package) if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") # Enable warnings add_compile_options(-Wall -Wextra - -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual -Wold-style-cast) + -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual -Wold-style-cast -Wformat=2) else() # Defaults for Microsoft C++ compiler add_compile_options(/W3 /wd4251 /wd4068 /wd4275) diff --git a/moveit_common/package.xml b/moveit_common/package.xml index abe95d1c212..3e7e34d8b1c 100644 --- a/moveit_common/package.xml +++ b/moveit_common/package.xml @@ -2,7 +2,7 @@ moveit_common - 2.7.0 + 2.7.1 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 783e7c74fdf..b389659c45c 100644 --- a/moveit_configs_utils/CHANGELOG.rst +++ b/moveit_configs_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_configs_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * feat: adds compatibility to robot_description from topic instead of parameter (`#1806 `_) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index 40b08d8f19f..206ae94115f 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -1,3 +1,5 @@ +import os + from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -237,7 +239,7 @@ def generate_move_group_launch(moveit_config): parameters=move_group_params, extra_debug_args=["--debug"], # Set the display variable, in case OpenGL code is used internally - additional_env={"DISPLAY": ":0"}, + additional_env={"DISPLAY": os.environ["DISPLAY"]}, ) return ld diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index af5b01f798e..265e1153cbe 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -2,7 +2,7 @@ moveit_configs_utils - 2.7.0 + 2.7.1 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 60188f14ccf..bcd2920de19 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version="2.5.3", + version="2.7.1", 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 deb5a4460cc..0aea4d74a05 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Ruckig-smoothing : reduce number of duration extensions (`#1990 `_) + * extend duration only for failed segment + * update comment + * Remove trajectory reset before extension + * readability improvement + * Remove call to RobotState update + --------- + Co-authored-by: ibrahiminfinite + Co-authored-by: AndyZe +* Fix mimic joints with TOTG (`#1989 `_) +* changed C style cast to C++ style cast for void type (`#2010 `_) + (void) -> static_cast +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Fix Ruckig termination condition (`#1963 `_) +* Fix ci-testing build issues (`#1998 `_) +* Fix invalid case style for private member in RobotTrajectory +* Fix unreachable child logger instance +* Document the Butterworth filter better (`#1961 `_) +* Merge pull request `#1546 `_ from peterdavidfagan/moveit_py + Python Bindings - moveit_py +* remove old python bindings +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, AndyZe, Henning Kayser, Jafar, Robert Haschke, Sebastian Castro, Shobuj Paul, V Mohammed Ibrahim, peterdavidfagan + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index c553f2c1369..a5098ac99d5 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -197,7 +197,7 @@ struct CollisionRequest { } - /** \brief The group name to check collisions for (optional; if empty, assume the complete robot) */ + /** \brief The group name to check collisions for (optional; if empty, assume the complete robot). Descendent links are included. */ std::string group_name; /** \brief If true, compute proximity distance */ @@ -254,7 +254,9 @@ struct DistanceRequest { } - /// Compute \e active_components_only_ based on \e req_ + /*** \brief Compute \e active_components_only_ based on \e req_. This + includes links that are in the kinematic model but outside this group, if those links are descendants of + joints in this group that have their values updated. */ void enableGroup(const moveit::core::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(group_name)) diff --git a/moveit_core/package.xml b/moveit_core/package.xml index afb62513733..4afb88d2976 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -2,7 +2,7 @@ moveit_core - 2.7.0 + 2.7.1 Core libraries used by MoveIt Henning Kayser diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 37ab0f515bd..9eb48ce44ef 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -1,4 +1,6 @@ -add_library(moveit_planning_scene SHARED src/planning_scene.cpp) +add_library(moveit_planning_scene SHARED + src/planning_scene.cpp +) target_include_directories(moveit_planning_scene PUBLIC $ $ 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 02818427a08..b7f81e375d7 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 @@ -302,13 +302,14 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /**@{*/ /** \brief Check if the current state is in collision (with the environment or self collision). - If a group name is specified, collision checking is done for that group only. + If a group name is specified, collision checking is done for that group only (plus descendent links). Since the function is non-const, the current state transforms are updated before the collision check. */ bool isStateColliding(const std::string& group = "", bool verbose = false); /** \brief Check if the current state is in collision (with the environment or self collision). If a group name is specified, - collision checking is done for that group only. It is expected the current state transforms are up to date. */ + collision checking is done for that group only (plus descendent links). It is expected the current state + transforms are up to date. */ bool isStateColliding(const std::string& group = "", bool verbose = false) const { return isStateColliding(getCurrentState(), group, verbose); @@ -316,8 +317,8 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is specified, - collision checking is done for that group only. The link transforms for \e state are updated before the collision - check. */ + collision checking is done for that group only (plus descendent links). The link transforms for \e state are + updated before the collision check. */ bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const { state.updateCollisionBodyTransforms(); @@ -325,13 +326,13 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } /** \brief Check if a given state is in collision (with the environment or self collision) - If a group name is specified, collision checking is done for that group only. It is expected that the link - transforms of \e state are up to date. */ + If a group name is specified, collision checking is done for that group only (plus descendent links). It is + expected that the link transforms of \e state are up to date. */ bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is in collision (with the environment or self collision) - If a group name is specified, collision checking is done for that group only. */ + If a group name is specified, collision checking is done for that group only (plus descendent links). */ bool isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group = "", bool verbose = false) const; @@ -522,7 +523,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - * Can be restricted to links part of or updated by \e group_name */ + * Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, const std::string& group_name = "") const { @@ -531,7 +532,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. Update the link transforms for \e robot_state if needed. - Can be restricted to links part of or updated by \e group_name */ + Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, moveit::core::RobotState& robot_state, const std::string& group_name = "") const { @@ -542,7 +543,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. - Can be restricted to links part of or updated by \e group_name*/ + Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, const std::string& group_name = "") const @@ -552,7 +553,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */ + allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, @@ -821,43 +822,46 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro bool isStateConstrained(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; - /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ + /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent + * links of \e group. */ bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", bool verbose = false) const; - /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ + /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent + * links of \e group. */ bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well */ + * specified validity conditions hold as well. Includes descendent links of \e group. */ bool isStateValid(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well */ + * specified validity conditions hold as well. Includes descendent links of \e group. */ bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well */ + * specified validity conditions hold as well. Includes descendent links of \e group. */ bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group = "", bool verbose = false) const; - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). + * Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", @@ -865,7 +869,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", @@ -873,7 +877,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", @@ -881,19 +885,20 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). */ + * constraint satisfaction). Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). + * Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; @@ -902,8 +907,8 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, std::set& costs, double overlap_fraction = 0.9) const; - /** \brief Get the top \e max_costs cost sources for a specified trajectory, but only for group \e group_name. The - * resulting costs are stored in \e costs */ + /** \brief Get the top \e max_costs cost sources for a specified trajectory, but only for group \e group_name (plus + * descendent links). The resulting costs are stored in \e costs */ void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, const std::string& group_name, std::set& costs, double overlap_fraction = 0.9) const; @@ -912,8 +917,8 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, std::set& costs) const; - /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name. The - * resulting costs are stored in \e costs */ + /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name (plus + * descendent links). The resulting costs are stored in \e costs */ void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, std::set& costs) const; @@ -963,22 +968,11 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro * Requires a valid robot_model_ */ void initialize(); - /* helper function to create a RobotModel from a urdf/srdf. */ - static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model); - /* Helper functions for processing collision objects */ bool processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object); bool processCollisionObjectRemove(const moveit_msgs::msg::CollisionObject& object); bool processCollisionObjectMove(const moveit_msgs::msg::CollisionObject& object); - /* For exporting and importing the planning scene */ - bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) const; - void writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) const; - - /** convert Pose msg to Eigen::Isometry, normalizing the quaternion part if necessary. */ - static void poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out); - MOVEIT_STRUCT_FORWARD(CollisionDetector); /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */ diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 2722ee6e619..ab92e0bf4a8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -59,6 +59,48 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_scene.p const std::string PlanningScene::OCTOMAP_NS = ""; const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; +namespace utilities +{ +/** + * convert Pose msg to Eigen::Isometry, normalizing the quaternion part if necessary. + * @param msg Input message + * @param out Output Eigen transform + */ +void poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out) +{ + Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); + Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); + quaternion.normalize(); + out = translation * quaternion; +} + +/** \brief Read a pose from text */ +bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) +{ + double x, y, z, rx, ry, rz, rw; + if (!(in >> x >> y >> z)) + { + RCLCPP_ERROR(LOGGER, "Improperly formatted translation in scene geometry file"); + return false; + } + if (!(in >> rx >> ry >> rz >> rw)) + { + RCLCPP_ERROR(LOGGER, "Improperly formatted rotation in scene geometry file"); + return false; + } + pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); + return true; +} + +/** \brief Write a pose to text */ +void writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) +{ + out << pose.translation().x() << ' ' << pose.translation().y() << ' ' << pose.translation().z() << '\n'; + Eigen::Quaterniond r(pose.linear()); + out << r.x() << ' ' << r.y() << ' ' << r.z() << ' ' << r.w() << '\n'; +} +} // namespace utilities + class SceneTransforms : public moveit::core::Transforms { public: @@ -135,9 +177,12 @@ PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, if (!srdf_model) throw moveit::ConstructException("The SRDF model cannot be nullptr"); - robot_model_ = createRobotModel(urdf_model, srdf_model); - if (!robot_model_) + robot_model_ = std::make_shared(urdf_model, srdf_model); + if (!robot_model_ || !robot_model_->getRootJoint()) + { + robot_model_ = nullptr; throw moveit::ConstructException("Could not create RobotModel"); + } initialize(); } @@ -163,17 +208,6 @@ void PlanningScene::initialize() allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } -// return nullptr on failure -moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model) -{ - auto robot_model = std::make_shared(urdf_model, srdf_model); - if (!robot_model || !robot_model->getRootJoint()) - return moveit::core::RobotModelPtr(); - - return robot_model; -} - PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(parent) { if (!parent_) @@ -917,7 +951,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const { out << "* " << id << '\n'; // New object start // Write object pose - writePoseToText(out, obj->pose_); + utilities::writePoseToText(out, obj->pose_); // Write shapes and shape poses out << obj->shapes_.size() << '\n'; // Number of shapes @@ -925,7 +959,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const { shapes::saveAsText(obj->shapes_[j].get(), out); // shape_poses_ is valid isometry by contract - writePoseToText(out, obj->shape_poses_[j]); + utilities::writePoseToText(out, obj->shape_poses_[j]); if (hasObjectColor(id)) { const std_msgs::msg::ColorRGBA& c = getObjectColor(id); @@ -939,8 +973,8 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const out << obj->subframe_poses_.size() << '\n'; // Number of subframes for (auto& pose_pair : obj->subframe_poses_) { - out << pose_pair.first << '\n'; // Subframe name - writePoseToText(out, pose_pair.second); // Subframe pose + out << pose_pair.first << '\n'; // Subframe name + utilities::writePoseToText(out, pose_pair.second); // Subframe pose } } } @@ -1000,7 +1034,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet // Read in object pose (added in the new scene format) pose.setIdentity(); - if (uses_new_scene_format && !readPoseFromText(in, pose)) + if (uses_new_scene_format && !utilities::readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read object pose from scene file"); return false; @@ -1019,7 +1053,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet RCLCPP_ERROR(LOGGER, "Failed to load shape from scene file"); return false; } - if (!readPoseFromText(in, pose)) + if (!utilities::readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read pose from scene file"); return false; @@ -1055,7 +1089,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet { std::string subframe_name; in >> subframe_name; - if (!readPoseFromText(in, pose)) + if (!utilities::readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read subframe pose from scene file"); return false; @@ -1078,30 +1112,6 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } while (true); } -bool PlanningScene::readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) const -{ - double x, y, z, rx, ry, rz, rw; - if (!(in >> x >> y >> z)) - { - RCLCPP_ERROR(LOGGER, "Improperly formatted translation in scene geometry file"); - return false; - } - if (!(in >> rx >> ry >> rz >> rw)) - { - RCLCPP_ERROR(LOGGER, "Improperly formatted rotation in scene geometry file"); - return false; - } - pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); - return true; -} - -void PlanningScene::writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) const -{ - out << pose.translation().x() << ' ' << pose.translation().y() << ' ' << pose.translation().z() << '\n'; - Eigen::Quaterniond r(pose.linear()); - out << r.x() << ' ' << r.y() << ' ' << r.z() << ' ' << r.w() << '\n'; -} - void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state) { // The attached bodies will be processed separately by processAttachedCollisionObjectMsgs @@ -1379,7 +1389,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); Eigen::Isometry3d p; - PlanningScene::poseMsgToEigen(map.origin, p); + utilities::poseMsgToEigen(map.origin, p); p = t * p; world_->addToObject(OCTOMAP_NS, std::make_shared(om), p); } @@ -1497,7 +1507,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At Eigen::Isometry3d subframe_pose; for (std::size_t i = 0; i < object.object.subframe_poses.size(); ++i) { - PlanningScene::poseMsgToEigen(object.object.subframe_poses[i], subframe_pose); + utilities::poseMsgToEigen(object.object.subframe_poses[i], subframe_pose); std::string name = object.object.subframe_names[i]; subframe_poses[name] = subframe_pose; } @@ -1673,22 +1683,6 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::msg::CollisionO return false; } -void PlanningScene::poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out) -{ - Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); - Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); - if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0)) - { - RCLCPP_WARN(LOGGER, "Empty quaternion found in pose message. Setting to neutral orientation."); - quaternion.setIdentity(); - } - else - { - quaternion.normalize(); - } - out = translation * quaternion; -} - bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::msg::CollisionObject& object, Eigen::Isometry3d& object_pose, std::vector& shapes, @@ -1714,7 +1708,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - PlanningScene::poseMsgToEigen(object.pose, object_pose); + utilities::poseMsgToEigen(object.pose, object_pose); bool switch_object_pose_and_shape_pose = false; if (num_shapes == 1) @@ -1731,7 +1725,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: if (!s) return; Eigen::Isometry3d pose; - PlanningScene::poseMsgToEigen(pose_msg, pose); + utilities::poseMsgToEigen(pose_msg, pose); if (!switch_object_pose_and_shape_pose) { shape_poses.emplace_back(std::move(pose)); @@ -1812,7 +1806,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::msg::CollisionO Eigen::Isometry3d subframe_pose; for (std::size_t i = 0; i < object.subframe_poses.size(); ++i) { - PlanningScene::poseMsgToEigen(object.subframe_poses[i], subframe_pose); + utilities::poseMsgToEigen(object.subframe_poses[i], subframe_pose); std::string name = object.subframe_names[i]; subframes[name] = subframe_pose; } @@ -1854,7 +1848,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision const Eigen::Isometry3d& world_to_object_header_transform = getFrameTransform(object.header.frame_id); Eigen::Isometry3d header_to_pose_transform; - PlanningScene::poseMsgToEigen(object.pose, header_to_pose_transform); + utilities::poseMsgToEigen(object.pose, header_to_pose_transform); const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform; world_->setObjectPose(object.id, object_frame_transform); diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 1b9fe18b77b..65112176a4c 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -582,9 +582,11 @@ class JointModelGroup bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size, double dt) const; -protected: - bool computeIKIndexBijection(const std::vector& ik_jnames, std::vector& joint_bijection) const; + /** \brief Computes the indices of joint variables given a vector of joint names to look up */ + bool computeJointVariableIndices(const std::vector& joint_names, + std::vector& joint_bijection) const; +protected: /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, there are no values to be read (\e values is only the group state) */ diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index f4e32c9312e..a9da7f9aa6b 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -138,6 +138,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode if (vc > 1) is_single_dof_ = false; const std::vector& name_order = joint_model->getVariableNames(); + if (joint_model->getMimic() == nullptr) { active_joint_model_vector_.push_back(joint_model); @@ -605,25 +606,25 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout) it.second.default_ik_timeout_ = ik_timeout; } -bool JointModelGroup::computeIKIndexBijection(const std::vector& ik_jnames, - std::vector& joint_bijection) const +bool JointModelGroup::computeJointVariableIndices(const std::vector& joint_names, + std::vector& joint_bijection) const { joint_bijection.clear(); - for (const std::string& ik_jname : ik_jnames) + for (const std::string& joint_name : joint_names) { - VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jname); + VariableIndexMap::const_iterator it = joint_variables_index_map_.find(joint_name); if (it == joint_variables_index_map_.end()) { // skip reported fixed joints - if (hasJointModel(ik_jname) && getJointModel(ik_jname)->getType() == JointModel::FIXED) + if (hasJointModel(joint_name) && getJointModel(joint_name)->getType() == JointModel::FIXED) continue; RCLCPP_ERROR(LOGGER, - "IK solver computes joint values for joint '%s' " + "Looking for variables for joint '%s', " "but group '%s' does not contain such a joint.", - ik_jname.c_str(), getName().c_str()); + joint_name.c_str(), getName().c_str()); return false; } - const JointModel* jm = getJointModel(ik_jname); + const JointModel* jm = getJointModel(joint_name); for (size_t k = 0; k < jm->getVariableCount(); ++k) joint_bijection.push_back(it->second + k); } @@ -639,8 +640,8 @@ void JointModelGroup::setSolverAllocators(const std::pairsetDefaultTimeout(group_kinematics_.first.default_ik_timeout_); - if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(), - group_kinematics_.first.bijection_)) + if (!computeJointVariableIndices(group_kinematics_.first.solver_instance_->getJointNames(), + group_kinematics_.first.bijection_)) group_kinematics_.first.reset(); } } @@ -655,7 +656,7 @@ void JointModelGroup::setSolverAllocators(const std::pair(it.first)->getSolverInstance(); ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_; - if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_)) + if (!computeJointVariableIndices(ks.solver_instance_->getJointNames(), ks.bijection_)) { group_kinematics_.second.clear(); break; @@ -730,8 +731,7 @@ void JointModelGroup::printGroupInfo(std::ostream& out) const out << '\n'; out << " " << parent_model_->getVariableBounds(variable_name) << '\n'; } - out << " * Variables Index List:\n"; - out << " "; + out << " * Variables Index List:\n "; for (int variable_index : variable_index_list_) out << variable_index << ' '; if (is_contiguous_index_list_) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 30ef3db65bb..609224965dd 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1550,7 +1550,7 @@ void RobotModel::setKinematicsAllocators(const std::map& velocity_limits, const std::unordered_map& acceleration_limits, const std::unordered_map& jerk_limits, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0); + const double max_acceleration_scaling_factor = 1.0, const bool mitigate_overshoot = false, + const double overshoot_threshold = 0.01); /** * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. @@ -120,35 +128,21 @@ class RuckigSmoothing * \param first_waypoint The Ruckig input/output parameters are initialized to the values at this waypoint * \param joint_group The MoveIt JointModelGroup of interest * \param[out] rucking_input Input parameters to Ruckig. Initialized here. - * \param[out] ruckig_output Output from the Ruckig algorithm. Initialized here. */ static void initializeRuckigState(const moveit::core::RobotState& first_waypoint, const moveit::core::JointModelGroup* joint_group, - ruckig::InputParameter& ruckig_input, - ruckig::OutputParameter& ruckig_output); - - /** - * \brief Break the `trajectory` parameter into batches of reasonable size (~100), run Ruckig on each of them, then - * re-combine into a single trajectory again. - * runRuckig() stretches all input waypoints in time until all kinematic limits are obeyed. This works but it can - * slow the trajectory more than necessary. It's better to feed in just a few waypoints at once, so that only the - * waypoints needing it get stretched. - * Here, break the trajectory waypoints into batches so the output is closer to time-optimal. - * There is a trade-off between time-optimality of the output trajectory and runtime of the smoothing algorithm. - * \param[in, out] trajectory Trajectory to smooth. - * \param[in, out] ruckig_input Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) - */ - static std::optional - runRuckigInBatches(const size_t num_waypoints, const robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input, size_t batch_size = 100); + ruckig::InputParameter& ruckig_input); /** * \brief A utility function to instantiate and run Ruckig for a series of waypoints. * \param[in, out] trajectory Trajectory to smooth. * \param[in, out] ruckig_input Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) + * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. + * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) */ [[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input); + ruckig::InputParameter& ruckig_input, + const bool mitigate_overshoot = false, const double overshoot_threshold = 0.01); /** * \brief Extend the duration of every trajectory segment @@ -163,5 +157,10 @@ class RuckigSmoothing const size_t num_dof, const std::vector& move_group_idx, const robot_trajectory::RobotTrajectory& original_trajectory, robot_trajectory::RobotTrajectory& trajectory); + + /** \brief Check if a trajectory out of Ruckig overshoots the target state */ + static bool checkOvershoot(ruckig::Trajectory& ruckig_trajectory, + const size_t num_dof, ruckig::InputParameter& ruckig_input, + const double overshoot_threshold); }; } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 527b0c6c8d9..0a80bff316f 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -87,7 +87,15 @@ class Path Eigen::VectorXd getConfig(double s) const; Eigen::VectorXd getTangent(double s) const; Eigen::VectorXd getCurvature(double s) const; + + /** @brief Get the next switching point. + * @param[in] s Arc length traveled so far + * @param[out] discontinuity True if this switching point is a discontinuity + * @return arc length to the switching point + **/ double getNextSwitchingPoint(double s, bool& discontinuity) const; + + /// @brief Return a list of all switching points as a pair (arc length to switching point, discontinuity) std::list> getSwitchingPoints() const; private: diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index d72ae5b53b1..be966d26770 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -52,11 +52,14 @@ constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 constexpr double DEFAULT_MAX_JERK = 1000; // rad/s^3 constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0; constexpr double DURATION_EXTENSION_FRACTION = 1.1; +// If "mitigate_overshoot" is enabled, overshoot is checked with this timestep +constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec } // namespace bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor) + const double max_acceleration_scaling_factor, const bool mitigate_overshoot, + const double overshoot_threshold) { if (!validateGroup(trajectory)) { @@ -81,7 +84,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto return false; } - return runRuckig(trajectory, ruckig_input); + return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold); } bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, @@ -89,7 +92,8 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto const std::unordered_map& acceleration_limits, const std::unordered_map& jerk_limits, const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor) + const double max_acceleration_scaling_factor, const bool mitigate_overshoot, + const double overshoot_threshold) { if (!validateGroup(trajectory)) { @@ -139,68 +143,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } } - return runRuckig(trajectory, ruckig_input); -} - -std::optional -RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, const robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input, size_t batch_size) -{ - // We take the batch size as the lesser of 0.1*num_waypoints or batch_size, to keep a balance between run time and - // time-optimality. - batch_size = [num_waypoints, batch_size]() { - const size_t temp_batch_size = std::min(size_t(0.1 * num_waypoints), size_t(batch_size)); - // We need at least 2 waypoints - return std::max(size_t(2), temp_batch_size); - }(); - size_t batch_start_idx = 0; - size_t batch_end_idx = batch_size - 1; - const size_t full_traj_final_idx = num_waypoints - 1; - // A deep copy is not needed since the waypoints are cleared immediately - robot_trajectory::RobotTrajectory sub_trajectory = - robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */); - robot_trajectory::RobotTrajectory output_trajectory = - robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */); - output_trajectory.clear(); - - while (batch_end_idx <= full_traj_final_idx) - { - sub_trajectory.clear(); - for (size_t waypoint_idx = batch_start_idx; waypoint_idx <= batch_end_idx; ++waypoint_idx) - { - sub_trajectory.addSuffixWayPoint(trajectory.getWayPoint(waypoint_idx), - trajectory.getWayPointDurationFromPrevious(waypoint_idx)); - } - - // When starting a new batch, set the last Ruckig output equal to the new, starting robot state - bool first_point_previously_smoothed = false; - if (output_trajectory.getWayPointCount() > 0) - { - sub_trajectory.addPrefixWayPoint(output_trajectory.getLastWayPoint(), 0); - first_point_previously_smoothed = true; - } - - if (!runRuckig(sub_trajectory, ruckig_input)) - { - return std::nullopt; - } - - // Skip appending the first waypoint in sub_trajectory if it was smoothed in - // the previous iteration - size_t first_new_waypoint = first_point_previously_smoothed ? 1 : 0; - - // Add smoothed waypoints to the output - for (size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.getWayPointCount(); ++waypoint_idx) - { - output_trajectory.addSuffixWayPoint(sub_trajectory.getWayPoint(waypoint_idx), - sub_trajectory.getWayPointDurationFromPrevious(waypoint_idx)); - } - - batch_start_idx += batch_size; - batch_end_idx += batch_size; - } - - return std::make_optional(output_trajectory, true /* deep copy */); + return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold); } bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, @@ -297,7 +240,8 @@ bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_fact } bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input) + ruckig::InputParameter& ruckig_input, + const bool mitigate_overshoot, const double overshoot_threshold) { const size_t num_waypoints = trajectory.getWayPointCount(); moveit::core::JointModelGroup const* const group = trajectory.getGroup(); @@ -309,7 +253,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, // Initialize the smoother ruckig::Ruckig ruckig(num_dof, trajectory.getAverageSegmentDuration()); - initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output); + initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input); // Cache the trajectory in case we need to reset it robot_trajectory::RobotTrajectory original_trajectory = @@ -318,53 +262,55 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, ruckig::Result ruckig_result; double duration_extension_factor = 1; bool smoothing_complete = false; + size_t waypoint_idx = 0; while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete) { - for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx) + while (waypoint_idx < num_waypoints - 1) { + moveit::core::RobotStatePtr curr_waypoint = trajectory.getWayPointPtr(waypoint_idx); moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1); - getNextRuckigInput(trajectory.getWayPointPtr(waypoint_idx), next_waypoint, group, ruckig_input); + getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input); // Run Ruckig - ruckig_result = ruckig.update(ruckig_input, ruckig_output); + ruckig::Trajectory ruckig_trajectory(num_dof); + ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory); + + // Step through the trajectory at the given OVERSHOOT_CHECK_PERIOD and check for overshoot. + // We will extend the duration to mitigate it. + bool overshoots = false; + if (mitigate_overshoot) + { + overshoots = checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold); + } // The difference between Result::Working and Result::Finished is that Finished can be reached in one // Ruckig timestep (constructor parameter). Both are acceptable for trajectories. // (The difference is only relevant for streaming mode.) // If successful and at the last trajectory segment - if ((waypoint_idx == num_waypoints - 2) && + if (!overshoots && (waypoint_idx == num_waypoints - 2) && (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished)) { - trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration()); + trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_trajectory.get_duration()); smoothing_complete = true; break; } - // TODO(andyz): why doesn't this work? - // // If successful, on to the next waypoint - // if (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished) - // { - // trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration()); - // continue; - // } - // Extend the trajectory duration if Ruckig could not reach the waypoint successfully - if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished) + if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)) { duration_extension_factor *= DURATION_EXTENSION_FRACTION; - // Reset the trajectory - trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */); const std::vector& move_group_idx = group->getVariableIndexList(); - extendTrajectoryDuration(duration_extension_factor, num_waypoints, num_dof, move_group_idx, trajectory, - original_trajectory); + extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, original_trajectory, + trajectory); - initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output); - // Begin the for() loop again + initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input); + // Continue the loop from failed segment, but with increased duration extension factor break; } + ++waypoint_idx; } } @@ -377,38 +323,35 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, return true; } -void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints, +void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_factor, size_t waypoint_idx, const size_t num_dof, const std::vector& move_group_idx, const robot_trajectory::RobotTrajectory& original_trajectory, robot_trajectory::RobotTrajectory& trajectory) { - for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx) + trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, + duration_extension_factor * + original_trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1)); + // re-calculate waypoint velocity and acceleration + auto target_state = trajectory.getWayPointPtr(waypoint_idx + 1); + const auto prev_state = trajectory.getWayPointPtr(waypoint_idx); + + double timestep = trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1); + + for (size_t joint = 0; joint < num_dof; ++joint) { - trajectory.setWayPointDurationFromPrevious( - time_stretch_idx, - duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx)); - // re-calculate waypoint velocity and acceleration - auto target_state = trajectory.getWayPointPtr(time_stretch_idx); - const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1); - double timestep = trajectory.getAverageSegmentDuration(); - for (size_t joint = 0; joint < num_dof; ++joint) - { - target_state->setVariableVelocity(move_group_idx.at(joint), - (1 / duration_extension_factor) * - target_state->getVariableVelocity(move_group_idx.at(joint))); + target_state->setVariableVelocity(move_group_idx.at(joint), + (1 / duration_extension_factor) * + target_state->getVariableVelocity(move_group_idx.at(joint))); - double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint)); - double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint)); - target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep); - } - target_state->update(); + double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint)); + double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint)); + target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep); } } void RuckigSmoothing::initializeRuckigState(const moveit::core::RobotState& first_waypoint, const moveit::core::JointModelGroup* joint_group, - ruckig::InputParameter& ruckig_input, - ruckig::OutputParameter& ruckig_output) + ruckig::InputParameter& ruckig_input) { const size_t num_dof = joint_group->getVariableCount(); const std::vector& idx = joint_group->getVariableIndexList(); @@ -431,10 +374,6 @@ void RuckigSmoothing::initializeRuckigState(const moveit::core::RobotState& firs std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin()); std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin()); std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin()); - // Initialize output data struct - ruckig_output.new_position = ruckig_input.current_position; - ruckig_output.new_velocity = ruckig_input.current_velocity; - ruckig_output.new_acceleration = ruckig_input.current_acceleration; } void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint, @@ -471,4 +410,31 @@ void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStateConstPtr& ruckig_input.max_acceleration.at(joint)); } } + +bool RuckigSmoothing::checkOvershoot(ruckig::Trajectory& ruckig_trajectory, + const size_t num_dof, ruckig::InputParameter& ruckig_input, + const double overshoot_threshold) +{ + // For every timestep + for (double time_from_start = OVERSHOOT_CHECK_PERIOD; time_from_start < ruckig_trajectory.get_duration(); + time_from_start += OVERSHOOT_CHECK_PERIOD) + { + std::vector new_position(num_dof); + std::vector new_velocity(num_dof); + std::vector new_acceleration(num_dof); + ruckig_trajectory.at_time(time_from_start, new_position, new_velocity, new_acceleration); + // For every joint + for (size_t joint = 0; joint < num_dof; ++joint) + { + // If the sign of the error changed and the threshold difference was exceeded + double error = new_position[joint] - ruckig_input.target_position.at(joint); + if (((error / (ruckig_input.current_position.at(joint) - ruckig_input.target_position.at(joint))) < 0.0) && + std::fabs(error) > overshoot_threshold) + { + return true; + } + } + } + return false; +} } // namespace trajectory_processing 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 3a117190503..6e455662e53 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -831,7 +831,6 @@ Eigen::VectorXd Trajectory::getVelocity(double time) const const double acceleration = 2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step); - time_step = time - previous->time_; const double path_pos = previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration; const double path_vel = previous->path_vel_ + time_step * acceleration; @@ -849,7 +848,6 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const const double acceleration = 2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step); - time_step = time - previous->time_; const double path_pos = previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration; const double path_vel = previous->path_vel_ + time_step * acceleration; @@ -884,16 +882,21 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY); double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION); - const std::vector& vars = group->getVariableNames(); + // Get the velocity and acceleration limits for all active joints const moveit::core::RobotModel& rmodel = group->getParentModel(); - const unsigned num_joints = group->getVariableCount(); + const std::vector& vars = group->getVariableNames(); + std::vector indices; + if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), indices)) + { + RCLCPP_ERROR(LOGGER, "Failed to get active variable indices."); + } - // Get the vel/accel limits + const size_t num_joints = indices.size(); Eigen::VectorXd max_velocity(num_joints); Eigen::VectorXd max_acceleration(num_joints); - for (size_t j = 0; j < num_joints; ++j) + for (const auto idx : indices) { - const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[idx]); // Limits need to be non-zero, otherwise we never exit if (bounds.velocity_bounded_) @@ -901,16 +904,16 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT if (bounds.max_velocity_ <= 0.0) { RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", - bounds.max_velocity_, vars[j].c_str()); + bounds.max_velocity_, vars[idx].c_str()); return false; } - max_velocity[j] = + max_velocity[idx] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor; } else { RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint " - << vars[j].c_str() + << vars[idx].c_str() << "! You have to define velocity limits in the URDF or joint_limits.yaml"); return false; } @@ -920,16 +923,16 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT if (bounds.max_acceleration_ < 0.0) { RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", - bounds.max_acceleration_, vars[j].c_str()); + bounds.max_acceleration_, vars[idx].c_str()); return false; } - max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * - acceleration_scaling_factor; + max_acceleration[idx] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * + acceleration_scaling_factor; } else { RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint " - << vars[j].c_str() + << vars[idx].c_str() << "! You have to define acceleration limits in the URDF or " "joint_limits.yaml"); return false; @@ -982,23 +985,30 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY); double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION); - const unsigned num_joints = group->getVariableCount(); - const std::vector& vars = group->getVariableNames(); + // Get the velocity and acceleration limits for specified joints const moveit::core::RobotModel& rmodel = group->getParentModel(); + const std::vector& vars = group->getVariableNames(); + std::vector indices; + if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), indices)) + { + RCLCPP_ERROR(LOGGER, "Failed to get active variable indices."); + } + + const size_t num_joints = indices.size(); Eigen::VectorXd max_velocity(num_joints); Eigen::VectorXd max_acceleration(num_joints); - for (size_t j = 0; j < num_joints; ++j) + for (const auto idx : indices) { - const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[idx]); // VELOCITY LIMIT // Check if a custom limit was specified as an argument bool set_velocity_limit = false; - auto it = velocity_limits.find(vars[j]); + auto it = velocity_limits.find(vars[idx]); if (it != velocity_limits.end()) { - max_velocity[j] = it->second * velocity_scaling_factor; + max_velocity[idx] = it->second * velocity_scaling_factor; set_velocity_limit = true; } @@ -1008,10 +1018,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( if (bounds.max_velocity_ < 0.0) { RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", - bounds.max_velocity_, vars[j].c_str()); + bounds.max_velocity_, vars[idx].c_str()); return false; } - max_velocity[j] = + max_velocity[idx] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor; set_velocity_limit = true; } @@ -1019,7 +1029,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( if (!set_velocity_limit) { RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint " - << vars[j].c_str() + << vars[idx].c_str() << "! You have to define velocity limits in the URDF or " "joint_limits.yaml"); return false; @@ -1028,10 +1038,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( // ACCELERATION LIMIT // Check if a custom limit was specified as an argument bool set_acceleration_limit = false; - it = acceleration_limits.find(vars[j]); + it = acceleration_limits.find(vars[idx]); if (it != acceleration_limits.end()) { - max_acceleration[j] = it->second * acceleration_scaling_factor; + max_acceleration[idx] = it->second * acceleration_scaling_factor; set_acceleration_limit = true; } @@ -1041,17 +1051,17 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( if (bounds.max_acceleration_ < 0.0) { RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", - bounds.max_acceleration_, vars[j].c_str()); + bounds.max_acceleration_, vars[idx].c_str()); return false; } - max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * - acceleration_scaling_factor; + max_acceleration[idx] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * + acceleration_scaling_factor; set_acceleration_limit = true; } if (!set_acceleration_limit) { RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint " - << vars[j].c_str() + << vars[idx].c_str() << "! You have to define acceleration limits in the URDF or " "joint_limits.yaml"); return false; @@ -1192,7 +1202,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::JointModelGroup* group) const { - const std::vector& joint_models = group->getJointModels(); + const std::vector& joint_models = group->getActiveJointModels(); bool have_prismatic = std::any_of(joint_models.cbegin(), joint_models.cend(), [](const moveit::core::JointModel* joint_model) { 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 36df09cd2e8..5d067c7c82f 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 @@ -97,6 +97,11 @@ TEST(time_optimal_trajectory_generation, test1) EXPECT_DOUBLE_EQ(985.000244140625, trajectory.getPosition(trajectory.getDuration())[1]); EXPECT_DOUBLE_EQ(2126.0, trajectory.getPosition(trajectory.getDuration())[2]); EXPECT_DOUBLE_EQ(0.0, trajectory.getPosition(trajectory.getDuration())[3]); + + // Start at rest and end at rest + const double traj_duration = trajectory.getDuration(); + EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1); + EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1); } TEST(time_optimal_trajectory_generation, test2) @@ -135,6 +140,11 @@ TEST(time_optimal_trajectory_generation, test2) EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]); EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]); EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]); + + // Start at rest and end at rest + const double traj_duration = trajectory.getDuration(); + EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1); + EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1); } TEST(time_optimal_trajectory_generation, test3) @@ -173,6 +183,11 @@ TEST(time_optimal_trajectory_generation, test3) EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]); EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]); EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]); + + // Start at rest and end at rest + const double traj_duration = trajectory.getDuration(); + EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1); + EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1); } // Test the version of computeTimeStamps that takes custom velocity/acceleration limits @@ -476,6 +491,53 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints) EXPECT_NEAR(trajectory.getWayPointCount(), desired_num_waypoints, 1); } +TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) +{ + // Test a (prior) specific failure case + Eigen::VectorXd waypoint(1); + std::list waypoints; + + const double start_position = 1.881943; + waypoint << start_position; + waypoints.push_back(waypoint); + waypoint << 2.600542; + waypoints.push_back(waypoint); + + Eigen::VectorXd max_velocities(1); + max_velocities << 4.54; + Eigen::VectorXd max_accelerations(1); + max_accelerations << 28.0; + + Trajectory trajectory(Path(waypoints, 0.1 /* path tolerance */), max_velocities, max_accelerations, + 0.001 /* timestep */); + EXPECT_TRUE(trajectory.isValid()); + + EXPECT_GT(trajectory.getDuration(), 0.0); + const double traj_duration = trajectory.getDuration(); + EXPECT_NEAR(0.320681, traj_duration, 1e-3); + + // Start matches + EXPECT_DOUBLE_EQ(start_position, trajectory.getPosition(0.0)[0]); + // Start at rest and end at rest + EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1); + EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1); + + // Check vels and accels at all points + for (double time = 0; time < traj_duration; time += 0.01) + { + // This trajectory has a single switching point + double t_switch = 0.1603407; + if (time < t_switch) + { + EXPECT_NEAR(trajectory.getAcceleration(time)[0], max_accelerations[0], 1e-3) << "Time: " << time; + } + else if (time > t_switch) + { + EXPECT_NEAR(trajectory.getAcceleration(time)[0], -max_accelerations[0], 1e-3) << "Time: " << time; + } + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 669d76e4ded..f88f3271e3d 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Add ament export (`#1887 `_) + * Add ament export + Also sort `find_package` entries alphabetically. + * Minor cleanup + --------- + Co-authored-by: AndyZe +* Contributors: Gaël Écorchard, Robert Haschke + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index a50abe78c18..a64af240922 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -2,7 +2,7 @@ moveit_kinematics - 2.7.0 + 2.7.1 Package for all inverse kinematics solvers in MoveIt Henning Kayser diff --git a/moveit_kinematics/test/benchmark_ik.cpp b/moveit_kinematics/test/benchmark_ik.cpp index 42150bd6026..324d89aebf5 100644 --- a/moveit_kinematics/test/benchmark_ik.cpp +++ b/moveit_kinematics/test/benchmark_ik.cpp @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) // const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); const moveit::core::RobotModelPtr& kinematic_model = moveit::core::loadTestingRobotModel("panda"); planning_scene::PlanningScene planning_scene(kinematic_model); - moveit::core::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst(); + moveit::core::RobotState& robot_state = planning_scene.getCurrentStateNonConst(); collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; std::chrono::duration ik_time(0); @@ -121,17 +121,17 @@ int main(int argc, char* argv[]) // perform first IK call to load the cache, so that the time for loading is not included in // average IK call time - kinematic_state.setToDefaultValues(); + robot_state.setToDefaultValues(); EigenSTL::vector_Isometry3d default_eef_states; for (const auto& end_effector : end_effectors) - default_eef_states.push_back(kinematic_state.getGlobalLinkTransform(end_effector)); + default_eef_states.push_back(robot_state.getGlobalLinkTransform(end_effector)); if (end_effectors.size() == 1) { - kinematic_state.setFromIK(group, default_eef_states[0], end_effectors[0], 0.1); + robot_state.setFromIK(group, default_eef_states[0], end_effectors[0], 0.1); } else { - kinematic_state.setFromIK(group, default_eef_states, end_effectors, 0.1); + robot_state.setFromIK(group, default_eef_states, end_effectors, 0.1); } bool found_ik; @@ -140,7 +140,7 @@ int main(int argc, char* argv[]) unsigned int i = 0; while (i < num) { - kinematic_state.setToRandomPositions(group); + robot_state.setToRandomPositions(group); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); if (collision_result.collision) @@ -149,17 +149,17 @@ int main(int argc, char* argv[]) continue; } for (unsigned j = 0; j < end_effectors.size(); ++j) - end_effector_states[j] = kinematic_state.getGlobalLinkTransform(end_effectors[j]); + end_effector_states[j] = robot_state.getGlobalLinkTransform(end_effectors[j]); if (reset_to_default) - kinematic_state.setToDefaultValues(); + robot_state.setToDefaultValues(); start = std::chrono::system_clock::now(); if (end_effectors.size() == 1) { - found_ik = kinematic_state.setFromIK(group, end_effector_states[0], end_effectors[0], 0.1); + found_ik = robot_state.setFromIK(group, end_effector_states[0], end_effectors[0], 0.1); } else { - found_ik = kinematic_state.setFromIK(group, end_effector_states, end_effectors, 0.1); + found_ik = robot_state.setFromIK(group, end_effector_states, end_effectors, 0.1); } ik_time += std::chrono::system_clock::now() - start; if (!found_ik) diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 0367ff8d332..58f2503f8f7 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index 11fb9c8971a..966413046e4 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp - 2.7.0 + 2.7.1 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 c3bd77e0acd..4c7685b45e5 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb + 2.7.0 (2023-01-29) ------------------ * Merge https://github.com/ros-planning/moveit/commit/9225971216885490e933ece25390c63ca14f8a58 diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index dda5057fe20..98c96c56e65 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.0 + 2.7.1 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 27a750eb9cb..7b08135d713 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index ca77aa06df9..ab65b98909e 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.0 + 2.7.1 Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 9b91813d170..c91746167ae 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index f37ba78ec15..a87d791f8d5 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -2,7 +2,7 @@ moveit_planners - 2.7.0 + 2.7.1 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 b9bb572e736..b7a62359e50 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix include install destination (`#2008 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Tyler Weaver +* Temporarily disable TestPathConstraints with the Panda robot (`#2016 `_) + This test has become flaky since it was modified to use the OMPL constrained state space (https://github.com/ros-planning/moveit2/issues/2015). +* Increase priority for constrained planning state space (`#1300 `_) + * Change priority for the constrained planning state space + * Fix constrained planning tests + * Use PRM instead of RRTConnect + --------- + Co-authored-by: Sebastian Jahr +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: Abhijeet Dasgupta, AlexWebb, Stephanie Eng + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index 60d33fca2d7..87dbf5aec52 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -31,7 +31,7 @@ install(TARGETS moveit_ompl_interface moveit_ompl_planner_plugin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include + INCLUDES DESTINATION include/moveit_planners_ompl ) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(moveit_core ompl) diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 39aef34c7ae..933e19b6e1b 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -63,7 +63,7 @@ target_link_libraries(moveit_ompl_planner_plugin moveit_ompl_interface) install(TARGETS moveit_generate_state_database RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY include/ DESTINATION include/moveit_planners) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h index e83c7cfb119..e7a80672665 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h @@ -48,9 +48,11 @@ class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory /** \brief Return a priority that this planner should be used for this specific planning problem. * - * This state space factory is currently only used if `use_ompl_constrained_state_space` was set to `true` in - * ompl_planning.yaml. In that case it is the only factory to choose from, so the priority does not matter. - * It returns a low priority so it will never be chosen when others are available. + * This state space factory is currently only used if there is exactly one position or orientation constraint, + * or if `enforce_constrained_state_space` was set to `true` in ompl_planning.yaml. + * In the first case, we prefer planning in the constrained state space and return a priority of 200. + * In the second case, it is the only factory to choose from, so the priority does not matter, + * and it returns a low priority so it will never be chosen when others are available. * (The second lowest priority is -1 in the PoseModelStateSpaceFactory.) * * For more details on this state space selection process, see: diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp index c69e6adc782..7830f81c372 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp @@ -46,10 +46,18 @@ ConstrainedPlanningStateSpaceFactory::ConstrainedPlanningStateSpaceFactory() : M } int ConstrainedPlanningStateSpaceFactory::canRepresentProblem( - const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& /*req*/, + const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& /*robot_model*/) const { - // Return the lowest priority currently existing in the ompl interface. + // If we have exactly one position or orientation constraint, prefer the constrained planning state space + auto num_constraints = + req.path_constraints.position_constraints.size() + req.path_constraints.orientation_constraints.size(); + if (num_constraints == 1 && req.path_constraints.joint_constraints.empty() && + req.path_constraints.visibility_constraints.empty()) + { + return 200; + } + // Otherwise, return the lowest priority currently existing in the ompl interface. // This state space will only be selected if it is the only option to choose from. // See header file for more info. return -2; diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index 55dcbdf0a31..9bae150b007 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -63,6 +63,7 @@ #include #include #include +#include // static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_planning_context_manager"); @@ -115,10 +116,14 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public SCOPED_TRACE("testPathConstraints"); // create all the test specific input necessary to make the getPlanningContext call possible + const auto& joint_names = joint_model_group_->getJointModelNames(); + planning_interface::PlannerConfigurationSettings pconfig_settings; pconfig_settings.group = group_name_; pconfig_settings.name = group_name_; - pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; + pconfig_settings.config = { { "enforce_joint_model_state_space", "0" }, + { "projection_evaluator", "joints(" + joint_names[0] + "," + joint_names[1] + ")" }, + { "type", "geometric::PRM" } }; planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; moveit_msgs::msg::MoveItErrorCodes error_code; @@ -145,8 +150,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); - // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space - EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); planning_interface::MotionPlanDetailedResponse response; ASSERT_TRUE(pc->solve(response)); @@ -181,8 +186,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); - // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space - EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); // Create a new response, because the solve method does not clear the given respone planning_interface::MotionPlanDetailedResponse response2; @@ -316,10 +321,11 @@ TEST_F(PandaTestPlanningContext, testSimpleRequest) testSimpleRequest({ 0., -0.785, 0., -2.356, 0, 1.571, 0.785 }, { 0., -0.785, 0., -2.356, 0, 1.571, 0.685 }); } -TEST_F(PandaTestPlanningContext, testPathConstraints) -{ - testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }, { .0, -0.785, 0., -2.356, 0., 1.571, 0.685 }); -} +// TODO(seng): This test is temporarily disabled as it is flaky since #1300. Re-enable when #2015 is resolved. +// TEST_F(PandaTestPlanningContext, testPathConstraints) +// { +// testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }, { .0, -0.785, 0., -2.356, 0., 1.571, 0.685 }); +// } /*************************************************************************** * Run all tests on the Fanuc robot diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 7c7f24af55f..39609451693 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -2,7 +2,7 @@ moveit_planners_ompl - 2.7.0 + 2.7.1 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 4c9698e60c7..068860166fe 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Remove "new" from smart pointer instantiation (`#2019 `_) +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, Robert Haschke, Shobuj Paul + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index fc3f219934d..e37edddaa94 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.0 + 2.7.1 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Christian Henkel 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 a236b715e45..aec23ccaff2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -34,6 +34,8 @@ #include +#include + #include #include #include @@ -71,7 +73,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) { - info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name)); if (req.goal_constraints.front().joint_constraints.size() != robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp index 628882a460c..46dfa8300ed 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp @@ -130,19 +130,19 @@ void IntegrationTestCommandListManager::SetUp() */ TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping) { - std::shared_ptr nbr_ex{ new NegativeBlendRadiusException("") }; + auto nbr_ex = std::make_shared(""); EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); - std::shared_ptr lbrnz_ex{ new LastBlendRadiusNotZeroException("") }; + auto lbrnz_ex = std::make_shared(""); EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); - std::shared_ptr sss_ex{ new StartStateSetException("") }; + auto sss_ex = std::make_shared(""); EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - std::shared_ptr obr_ex{ new OverlappingBlendRadiiException("") }; + auto obr_ex = std::make_shared(""); EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); - std::shared_ptr pp_ex{ new PlanningPipelineException("") }; + auto pp_ex = std::make_shared(""); EXPECT_EQ(pp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp index 22a1cc193bd..498e743e6b9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp @@ -82,16 +82,16 @@ void IntegrationTestPlanComponentBuilder::SetUp() */ TEST_F(IntegrationTestPlanComponentBuilder, TestExceptionErrorCodeMapping) { - std::shared_ptr nbs_ex{ new NoBlenderSetException("") }; + auto nbs_ex = std::make_shared(""); EXPECT_EQ(nbs_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); - std::shared_ptr ntffse_ex{ new NoTipFrameFunctionSetException("") }; + auto ntffse_ex = std::make_shared(""); EXPECT_EQ(ntffse_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); - std::shared_ptr nrms_ex{ new NoRobotModelSetException("") }; + auto nrms_ex = std::make_shared(""); EXPECT_EQ(nrms_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); - std::shared_ptr bf_ex{ new BlendingFailedException("") }; + auto bf_ex = std::make_shared(""); EXPECT_EQ(bf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } 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 c547a6657f1..cad029206ab 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 @@ -89,12 +89,12 @@ void GetSolverTipFrameTest::SetUp() TEST_F(GetSolverTipFrameTest, TestExceptionErrorCodeMapping) { { - std::shared_ptr nse_ex{ new NoSolverException("") }; + auto nse_ex = std::make_shared(""); EXPECT_EQ(nse_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { - std::shared_ptr ex{ new MoreThanOneTipFrameException("") }; + auto ex = std::make_shared(""); EXPECT_EQ(ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp index 8b8f3ac77f1..16d8d3c6d78 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp @@ -44,8 +44,8 @@ using namespace pilz_industrial_motion_planner; TEST(CommandPlannerTestDirect, ExceptionCoverage) { - std::shared_ptr p_ex{ new PlanningException("") }; - std::shared_ptr clr_ex{ new ContextLoaderRegistrationException("") }; + auto p_ex = std::make_shared(""); + auto clr_ex = std::make_shared(""); } /** 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 af3e4827efa..c942fe22107 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 @@ -216,6 +216,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); + if (!solver) + { + throw("No IK solver configured for group '" + planning_group_ + "'"); + } // robot state moveit::core::RobotState rstate(robot_model_); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp index e04522d4b18..dafd6fa5352 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp @@ -47,94 +47,92 @@ using namespace pilz_industrial_motion_planner; TEST(TrajectoryGeneratorTest, TestExceptionErrorCodeMapping) { { - std::shared_ptr tgil_ex{ new TrajectoryGeneratorInvalidLimitsException( - "") }; + auto tgil_ex = std::make_shared(""); EXPECT_EQ(tgil_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { - std::shared_ptr vsi_ex{ new VelocityScalingIncorrect("") }; + auto vsi_ex = std::make_shared(""); EXPECT_EQ(vsi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr asi_ex{ new AccelerationScalingIncorrect("") }; + auto asi_ex = std::make_shared(""); EXPECT_EQ(asi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr upg_ex{ new UnknownPlanningGroup("") }; + auto upg_ex = std::make_shared(""); EXPECT_EQ(upg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); } { - std::shared_ptr njniss_ex{ new NoJointNamesInStartState("") }; + auto njniss_ex = std::make_shared(""); EXPECT_EQ(njniss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { - std::shared_ptr smiss_ex{ new SizeMismatchInStartState("") }; + auto smiss_ex = std::make_shared(""); EXPECT_EQ(smiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { - std::shared_ptr jofssoor_ex{ new JointsOfStartStateOutOfRange("") }; + auto jofssoor_ex = std::make_shared(""); EXPECT_EQ(jofssoor_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { - std::shared_ptr nzviss_ex{ new NonZeroVelocityInStartState("") }; + auto nzviss_ex = std::make_shared(""); EXPECT_EQ(nzviss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { - std::shared_ptr neogcg_ex{ new NotExactlyOneGoalConstraintGiven("") }; + auto neogcg_ex = std::make_shared(""); EXPECT_EQ(neogcg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr oogta_ex{ new OnlyOneGoalTypeAllowed("") }; + auto oogta_ex = std::make_shared(""); EXPECT_EQ(oogta_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr ssgsm_ex{ new StartStateGoalStateMismatch("") }; + auto ssgsm_ex = std::make_shared(""); EXPECT_EQ(ssgsm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr jcdnbtg_ex{ new JointConstraintDoesNotBelongToGroup("") }; + auto jcdnbtg_ex = std::make_shared(""); EXPECT_EQ(jcdnbtg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr jogoor_ex{ new JointsOfGoalOutOfRange("") }; + auto jogoor_ex = std::make_shared(""); EXPECT_EQ(jogoor_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr pcnm_ex{ new PositionConstraintNameMissing("") }; + auto pcnm_ex = std::make_shared(""); EXPECT_EQ(pcnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr ocnm_ex{ new OrientationConstraintNameMissing("") }; + auto ocnm_ex = std::make_shared(""); EXPECT_EQ(ocnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr pocnm_ex{ new PositionOrientationConstraintNameMismatch( - "") }; + auto pocnm_ex = std::make_shared(""); EXPECT_EQ(pocnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr nisa_ex{ new NoIKSolverAvailable("") }; + auto nisa_ex = std::make_shared(""); EXPECT_EQ(nisa_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } { - std::shared_ptr nppg_ex{ new NoPrimitivePoseGiven("") }; + auto nppg_ex = std::make_shared(""); EXPECT_EQ(nppg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } } 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 896c36ce2ab..5e1228e3d80 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 @@ -214,57 +214,57 @@ class TrajectoryGeneratorCIRCTest : public testing::Test TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) { { - std::shared_ptr cnp_ex{ new CircleNoPlane("") }; + auto cnp_ex = std::make_shared(""); EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr cts_ex{ new CircleToSmall("") }; + auto cts_ex = std::make_shared(""); EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr cpdr_ex{ new CenterPointDifferentRadius("") }; + auto cpdr_ex = std::make_shared(""); EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr ctcf_ex{ new CircTrajectoryConversionFailure("") }; + auto ctcf_ex = std::make_shared(""); EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr upcn_ex{ new UnknownPathConstraintName("") }; + auto upcn_ex = std::make_shared(""); EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr npc_ex{ new NoPositionConstraints("") }; + auto npc_ex = std::make_shared(""); EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr npp_ex{ new NoPrimitivePose("") }; + auto npp_ex = std::make_shared(""); EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { - std::shared_ptr ulnoap_ex{ new UnknownLinkNameOfAuxiliaryPoint("") }; + auto ulnoap_ex = std::make_shared(""); EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); } { - std::shared_ptr nocm_ex{ new NumberOfConstraintsMismatch("") }; + auto nocm_ex = std::make_shared(""); EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr cjmiss_ex{ new CircJointMissingInStartState("") }; + auto cjmiss_ex = std::make_shared(""); EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { - std::shared_ptr cifgi_ex{ new CircInverseForGoalIncalculable("") }; + auto cifgi_ex = std::make_shared(""); EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } 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 c4e3044b70c..98b8b02b780 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 @@ -183,22 +183,22 @@ class TrajectoryGeneratorLINTest : public testing::Test TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) { { - std::shared_ptr ltcf_ex{ new LinTrajectoryConversionFailure("") }; + auto ltcf_ex = std::make_shared(""); EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { - std::shared_ptr jnm_ex{ new JointNumberMismatch("") }; + auto jnm_ex = std::make_shared(""); EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { - std::shared_ptr ljmiss_ex{ new LinJointMissingInStartState("") }; + auto ljmiss_ex = std::make_shared(""); EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { - std::shared_ptr lifgi_ex{ new LinInverseForGoalIncalculable("") }; + auto lifgi_ex = std::make_shared(""); EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } 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 df24d36a44c..aca597f3dc5 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 @@ -158,12 +158,12 @@ class TrajectoryGeneratorPTPTest : public testing::Test TEST_F(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) { { - std::shared_ptr pvpsf_ex{ new PtpVelocityProfileSyncFailed("") }; + auto pvpsf_ex = std::make_shared(""); EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { - std::shared_ptr pnisfgp_ex{ new PtpNoIkSolutionForGoalPose("") }; + auto pnisfgp_ex = std::make_shared(""); EXPECT_EQ(pnisfgp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } @@ -294,8 +294,8 @@ TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) EXPECT_THROW( { - std::unique_ptr ptp_error( - new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits, planning_group_)); + auto ptp_error = + std::make_unique(robot_model_, insufficient_planner_limits, planning_group_); }, TrajectoryGeneratorInvalidLimitsException); @@ -333,8 +333,8 @@ TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) sufficient_planner_limits.setJointLimits(sufficient_joint_limits); EXPECT_NO_THROW({ - std::unique_ptr ptp_no_error( - new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits, planning_group_)); + auto ptp_no_error = + std::make_unique(robot_model_, sufficient_planner_limits, planning_group_); }); } diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index 9b364b3c257..843a14b2bba 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index 6e0d067b119..384758f9ea1 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.0 + 2.7.1 Helper scripts and functionality to test industrial motion generation Christian Henkel diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst index 37c5761a271..69b02d2c55f 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,13 @@ Changelog for package moveit_resources_prbt_ikfast_manipulator_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix include install destination (`#2008 `_) + Co-authored-by: Henning Kayser + Co-authored-by: Tyler Weaver +* Contributors: Abhijeet Dasgupta + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt index 1ad5b4a3d5c..526e47a4364 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt @@ -61,7 +61,7 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include + INCLUDES DESTINATION include/moveit_resources_prbt_ikfast_manipulator_plugin ) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 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 560504dccbb..284f92f23f5 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.0 + 2.7.1 The prbt_ikfast_manipulator_plugin package Alexander Gutenkunst Christian Henkel diff --git a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst index 5a37f7148dc..db55348c1be 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_planners/test_configs/prbt_moveit_config/package.xml b/moveit_planners/test_configs/prbt_moveit_config/package.xml index c5567b8d4a5..07ad309b464 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.0 + 2.7.1

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 ea6054105f0..23bc8ede178 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ diff --git a/moveit_planners/test_configs/prbt_pg70_support/package.xml b/moveit_planners/test_configs/prbt_pg70_support/package.xml index 27f5321bd56..fc3ee01910f 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.0 + 2.7.1 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 a3f9fe18578..2916129881e 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ diff --git a/moveit_planners/test_configs/prbt_support/package.xml b/moveit_planners/test_configs/prbt_support/package.xml index 126b06f3122..42dc7b64668 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.0 + 2.7.1 Mechanical, kinematic and visual description of the Pilz light weight arm PRBT. diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index 32a410a3bb6..a721be8a1a4 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index f8ba29d5a2b..ce8980ae374 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -2,7 +2,7 @@ moveit_plugins - 2.7.0 + 2.7.1 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 34cc2b901b5..f8f00561471 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Add Warning Message for Out of Date Controller Information (`#1983 `_) + Co-authored-by: Joseph Schornak + Co-authored-by: Joseph Schornak +* Update SwitchController API usage (`#1996 `_) + Fixes deprecated and now removed message fields https://github.com/ros-controls/ros2_control/pull/948 +* Contributors: Erik Holum, Henning Kayser + 2.7.0 (2023-01-29) ------------------ * Fix parameters for ros2_control namespaces (`#1833 `_) diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index b55c8f4cfa3..e997f76d6da 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.0 + 2.7.1 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 c9d245cf3b5..3f52cf551ea 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Contributors: Robert Haschke + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index 8b305adc292..a0e4c6a11f0 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.0 + 2.7.1 A generic, simple controller manager plugin for MoveIt. Michael Görner Henning Kayser diff --git a/moveit_py/CITATION.cff b/moveit_py/CITATION.cff new file mode 100644 index 00000000000..ab0a59f44b5 --- /dev/null +++ b/moveit_py/CITATION.cff @@ -0,0 +1,8 @@ +cff-version: 1.2.0 +authors: + - family-names: Fagan + given-names: Peter David +date-released: 2023-03-20 +message: "If you use this software, please cite it as below." +title: "MoveIt 2 Python Library: A Software Library for Robotics Education and Research" +url: "https://github.com/ros-planning/moveit2/tree/main/moveit_py" diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 1ad76f62825..5a881b23477 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -12,13 +12,22 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) +# enables using the Python extensions from the build space for testing +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_moveit/__init__.py" "") + add_subdirectory(src/moveit/moveit_py_utils) ament_python_install_package(moveit) # Set the build location and install location for a CPython extension function(configure_build_install_location _library_name) - install(TARGETS ${_library_name} + # Install into test_moveit folder in build space for unit tests to import + set_target_properties(${_library_name} PROPERTIES + # Use generator expression to avoid prepending a build type specific directory on Windows + LIBRARY_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit> + RUNTIME_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>) + + install(TARGETS ${_library_name} DESTINATION "${PYTHON_INSTALL_DIR}/moveit" ) endfunction() @@ -64,4 +73,21 @@ target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp moveit_py_utils) configure_build_install_location(planning) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/unit/test_robot_model.py + test/unit/test_robot_state.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV AMENT_PREFIX_INDEX=${ament_index_build_path} PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" + ) + endforeach() +endif() + ament_package() diff --git a/moveit_py/README.md b/moveit_py/README.md index ecc5018d397..cf987806c2f 100644 --- a/moveit_py/README.md +++ b/moveit_py/README.md @@ -20,5 +20,16 @@ Community contributions are welcome. For detailed contribution guidelines please consult the official [MoveIt contribution guidelines](https://moveit.ros.org/documentation/contributing/). +## Citing the Library +If you use this library in your work please use the following citation: +```bibtex +@software{fagan2023moveitpy, + author = {Fagan, Peter David}, + title = {{MoveIt 2 Python Library: A Software Library for Robotics Education and Research}}, + url = {https://github.com/ros-planning/moveit2/tree/main/moveit_py}, + year = {2023} +} +``` + ## Acknowledgements Thank you to the [Google Summer of Code program](https://summerofcode.withgoogle.com/) for sponsoring the development of this Python library. Thank you to the MoveIt maintainers Henning Kayser (@henningkayser) and Michael Gorner (@v4hn) for their guidance as supervisors of my GSoC project. Finally thank you to the [ML Collective](https://mlcollective.org/) for providing compute support for this project. diff --git a/moveit_py/package.xml b/moveit_py/package.xml index 77d66108455..070ee896bbb 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -2,7 +2,7 @@ moveit_py - 0.0.0 + 2.7.1 Python binding for MoveIt 2 Peter David Fagan @@ -24,6 +24,10 @@ moveit_ros_planning_interface moveit_ros_planning moveit_core + + ament_cmake_pytest + python3-pytest + ament_cmake diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp index 49d85bd79b5..8132615eaa7 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp @@ -44,7 +44,9 @@ namespace bind_robot_model void init_joint_model(py::module& m) { - py::class_>(m, "VariableBounds") + py::module robot_model = m.def_submodule("robot_model"); + + py::class_>(robot_model, "VariableBounds") .def_readonly("min_position", &moveit::core::VariableBounds::min_position_) .def_readonly("max_position", &moveit::core::VariableBounds::max_position_) .def_readonly("position_bounded", &moveit::core::VariableBounds::position_bounded_) diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp index 45c3c0608f2..03f84b32986 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp @@ -49,7 +49,9 @@ bool satisfies_position_bounds(const moveit::core::JointModelGroup* jmg, const E void init_joint_model_group(py::module& m) { - py::class_(m, "JointModelGroup", + py::module robot_model = m.def_submodule("robot_model"); + + py::class_(robot_model, "JointModelGroup", R"( Representation of a group of joints that are part of a robot model. )") 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 57d00fa68b7..98797692167 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 @@ -50,6 +50,13 @@ get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_ return msg; } +robot_trajectory::RobotTrajectory +set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg) +{ + return robot_trajectory->setRobotTrajectoryMsg(robot_state, msg); +} + void init_robot_trajectory(py::module& m) { py::module robot_trajectory = m.def_submodule("robot_trajectory"); @@ -60,6 +67,14 @@ void init_robot_trajectory(py::module& m) Maintains a sequence of waypoints and the durations between these waypoints. )") + .def(py::init&>(), + R"( + Initializes an empty robot trajectory from a robot model. + + Args: + :py:class:`moveit_py.core.RobotModel`: The robot model associated to the instantiated robot state. + + )") .def("__getitem__", &robot_trajectory::RobotTrajectory::getWayPoint, py::arg("idx"), R"( Get the waypoint at the specified index in the trajectory. @@ -124,6 +139,11 @@ void init_robot_trajectory(py::module& m) Get the trajectory as a `moveit_msgs.msg.RobotTrajectory` message. Returns: moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. + )") + .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg, + py::arg("robot_state"), py::arg("msg"), + R"( + Set the trajectory from a `moveit_msgs.msg.RobotTrajectory` message. )"); // TODO (peterdavidfagan): support other methods such as appending trajectories } 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 a8bdb91eb08..6b8f3b7e8b8 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 @@ -51,6 +51,9 @@ namespace bind_robot_trajectory moveit_msgs::msg::RobotTrajectory get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, const std::vector& joint_filter); +robot_trajectory::RobotTrajectory +set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg); void init_robot_trajectory(py::module& m); } // namespace bind_robot_trajectory diff --git a/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt index daa7d74f2e9..a76b69e94c2 100644 --- a/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt +++ b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt @@ -13,4 +13,4 @@ install( ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) -install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY include/ DESTINATION include/moveit_py) diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp index de43e609f2b..6bb6907e8bc 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -45,8 +45,8 @@ planning_interface::MotionPlanResponse plan(std::shared_ptr& planning_component, std::shared_ptr& single_plan_parameters, std::shared_ptr& multi_plan_parameters, - std::optional solution_selection_callback, - std::optional stopping_criterion_callback) + std::optional solution_selection_function, + std::optional stopping_criterion_callback) { // parameter argument checking if (single_plan_parameters && multi_plan_parameters) @@ -70,18 +70,19 @@ plan(std::shared_ptr& planning_component, std::const_pointer_cast( multi_plan_parameters); - if (solution_selection_callback && stopping_criterion_callback) + if (solution_selection_function && stopping_criterion_callback) { - return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_callback), + return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function), *stopping_criterion_callback); } - else if (solution_selection_callback) + else if (solution_selection_function) { - return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_callback)); + return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function)); } else if (stopping_criterion_callback) { - return planning_component->plan(*const_multi_plan_parameters, moveit_cpp::getShortestSolution, + return planning_component->plan(*const_multi_plan_parameters, + moveit::planning_pipeline_interfaces::getShortestSolution, *stopping_criterion_callback); } else @@ -241,7 +242,7 @@ void init_multi_plan_request_parameters(py::module& m) return params; })) .def_readonly("multi_plan_request_parameters", - &moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters::multi_plan_request_parameters); + &moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters::plan_request_parameter_vector); } void init_planning_component(py::module& m) { @@ -320,7 +321,7 @@ void init_planning_component(py::module& m) // TODO (peterdavidfagan): improve the plan API .def("plan", &moveit_py::bind_planning_component::plan, py::arg("single_plan_parameters") = nullptr, - py::arg("multi_plan_parameters") = nullptr, py::arg("solution_selection_callback") = nullptr, + py::arg("multi_plan_parameters") = nullptr, py::arg("solution_selection_function") = nullptr, py::arg("stopping_criterion_callback") = nullptr, py::return_value_policy::move, R"( Plan a motion plan using the current start and goal states. diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h index 44b23a73dc8..615a3facc5f 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h @@ -45,7 +45,6 @@ #include #include #include -#include #include #include @@ -63,8 +62,8 @@ planning_interface::MotionPlanResponse plan(std::shared_ptr& planning_component, std::shared_ptr& single_plan_parameters, std::shared_ptr& multi_plan_parameters, - std::optional solution_selection_callback, - std::optional stopping_criterion_callback); + std::optional solution_selection_function, + std::optional stopping_criterion_callback); bool set_goal(std::shared_ptr& planning_component, std::optional configuration_name, std::optional robot_state, diff --git a/moveit_py/test/unit/test_robot_model.py b/moveit_py/test/unit/test_robot_model.py index 9cad84dc94f..a616ba7a514 100644 --- a/moveit_py/test/unit/test_robot_model.py +++ b/moveit_py/test/unit/test_robot_model.py @@ -1,53 +1,55 @@ import unittest - -from moveit_py.core import JointModelGroup, RobotModel +from test_moveit.core.robot_model import JointModelGroup, RobotModel # TODO (peterdavidfagan): depend on moveit_resources package directly. # (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp) + +import os + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) + + class TestRobotModel(unittest.TestCase): def test_initialization(self): """ Test that the RobotModel can be initialized with xml filepaths. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertIsInstance(robot, RobotModel) def test_name_property(self): """ Test that the RobotModel name property returns the correct name. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertEqual(robot.name, "panda") def test_model_frame_property(self): """ Test that the RobotModel model_frame property returns the correct name. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertEqual(robot.model_frame, "world") def test_root_joint_name_property(self): """ Test that the RobotModel root_link property returns the correct name. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertEqual(robot.root_joint_name, "virtual_joint") def test_joint_model_group_names_property(self): """ Test that the RobotModel joint_model_group_names property returns the correct names. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertCountEqual( robot.joint_model_group_names, ["panda_arm", "hand", "panda_arm_hand"] ) @@ -56,18 +58,14 @@ def test_joint_model_groups_property(self): """ Test that the RobotModel joint_model_groups returns a list of JointModelGroups. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertIsInstance(robot.joint_model_groups[0], JointModelGroup) def test_has_joint_model_group(self): """ Test that the RobotModel has_joint_model_group returns True for existing groups. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertTrue(robot.has_joint_model_group("panda_arm")) self.assertFalse(robot.has_joint_model_group("The answer is 42.")) @@ -75,9 +73,7 @@ def test_get_joint_model_group(self): """ Test that the RobotModel get_joint_model_group returns the correct group. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() jmg = robot.get_joint_model_group("panda_arm") self.assertIsInstance(jmg, JointModelGroup) self.assertEqual(jmg.name, "panda_arm") diff --git a/moveit_py/test/unit/test_robot_state.py b/moveit_py/test/unit/test_robot_state.py index d4ccd2fe02e..f5f6d8ed4a4 100644 --- a/moveit_py/test/unit/test_robot_state.py +++ b/moveit_py/test/unit/test_robot_state.py @@ -3,7 +3,23 @@ from geometry_msgs.msg import Pose -from moveit_py.core import RobotState, RobotModel +from test_moveit.core.robot_state import RobotState +from test_moveit.core.robot_model import RobotModel + + +# TODO (peterdavidfagan): depend on moveit_resources package directly. +# (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp) + +import os + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) class TestRobotState(unittest.TestCase): @@ -11,9 +27,7 @@ def test_initialization(self): """ Test that RobotState can be initialized with a RobotModel """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) self.assertIsInstance(robot_state, RobotState) @@ -22,9 +36,7 @@ def test_robot_model_property(self): """ Test that the robot_model property returns the correct RobotModel """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) self.assertEqual(robot_state.robot_model, robot_model) @@ -33,11 +45,9 @@ def test_get_frame_transform(self): """ Test that the frame transform is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() frame_transform = robot_state.get_frame_transform("panda_link0") self.assertIsInstance(frame_transform, np.ndarray) @@ -47,10 +57,9 @@ def test_get_pose(self): """ Test that the pose is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) + robot_state.update() pose = robot_state.get_pose(link_name="panda_link8") self.assertIsInstance(pose, Pose) @@ -60,10 +69,9 @@ def test_get_jacobian_1(self): """ Test that the jacobian is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) + robot_state.update() jacobian = robot_state.get_jacobian( joint_model_group_name="panda_arm", reference_point_position=np.array([0.0, 0.0, 0.0]), @@ -76,10 +84,9 @@ def test_get_jacobian_2(self): """ Test that the jacobian is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) + robot_state.update() jacobian = robot_state.get_jacobian( joint_model_group_name="panda_arm", link_name="panda_link6", @@ -93,11 +100,9 @@ def test_set_joint_group_positions(self): """ Test that the joint group positions can be set """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() joint_group_positions = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) robot_state.set_joint_group_positions( joint_model_group_name="panda_arm", position_values=joint_group_positions @@ -105,18 +110,16 @@ def test_set_joint_group_positions(self): self.assertEqual( joint_group_positions.tolist(), - robot_state.copy_joint_group_positions("panda_arm").tolist(), + robot_state.get_joint_group_positions("panda_arm").tolist(), ) def test_set_joint_group_velocities(self): """ Test that the joint group velocities can be set """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() joint_group_velocities = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) robot_state.set_joint_group_velocities( joint_model_group_name="panda_arm", velocity_values=joint_group_velocities @@ -124,18 +127,16 @@ def test_set_joint_group_velocities(self): self.assertEqual( joint_group_velocities.tolist(), - robot_state.copy_joint_group_velocities("panda_arm").tolist(), + robot_state.get_joint_group_velocities("panda_arm").tolist(), ) def test_set_joint_group_accelerations(self): """ Test that the joint group accelerations can be set """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() joint_group_accelerations = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) robot_state.set_joint_group_accelerations( joint_model_group_name="panda_arm", @@ -144,32 +145,32 @@ def test_set_joint_group_accelerations(self): self.assertEqual( joint_group_accelerations.tolist(), - robot_state.copy_joint_group_accelerations("panda_arm").tolist(), + robot_state.get_joint_group_accelerations("panda_arm").tolist(), ) # TODO (peterdavidfagan): requires kinematics solver to be loaded - def test_set_from_ik(self): - """ - Test that the robot state can be set from an IK solution - """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) - robot_state = RobotState(robot_model) - - pose = Pose() - pose.position.x = 0.5 - pose.position.y = 0.5 - pose.position.z = 0.5 - pose.orientation.w = 1.0 - - robot_state.set_from_ik( - joint_model_group_name="panda_arm", - geometry_pose=pose, - tip_name="panda_link8", - ) - - self.assertEqual(robot_state.get_pose("panda_link8"), pose) + # def test_set_from_ik(self): + # """ + # Test that the robot state can be set from an IK solution + # """ + # robot_model = RobotModel( + # urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + # ) + # robot_state = RobotState(robot_model) + # robot_state.update() + # pose = Pose() + # pose.position.x = 0.5 + # pose.position.y = 0.5 + # pose.position.z = 0.5 + # pose.orientation.w = 1.0 + + # robot_state.set_from_ik( + # joint_model_group_name="panda_arm", + # geometry_pose=pose, + # tip_name="panda_link8", + # ) + + # self.assertEqual(robot_state.get_pose("panda_link8"), pose) if __name__ == "__main__": diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index 56f544b813e..bf705bba862 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Benchmark parallel planning pipelines (`#1539 `_) + * Remove launch and config files (moved to moveit_resources) +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, Robert Haschke, Sebastian Jahr + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index edf615c071b..88c3acad5c0 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -2,7 +2,7 @@ moveit_ros_benchmarks - 2.7.0 + 2.7.1 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 66de2f0dec5..7308854c6c1 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -934,7 +934,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request .max_velocity_scaling_factor = request.max_velocity_scaling_factor, .max_acceleration_scaling_factor = request.max_acceleration_scaling_factor }; - multi_pipeline_plan_request.multi_plan_request_parameters.push_back(plan_req_params); + multi_pipeline_plan_request.plan_request_parameter_vector.push_back(plan_req_params); } // Iterate runs @@ -960,7 +960,8 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request 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, &moveit_cpp::getShortestSolution, + auto const response = planning_component->plan(multi_pipeline_plan_request, + &moveit::planning_pipeline_interfaces::getShortestSolution, nullptr, planning_scene_); auto const t2 = std::chrono::system_clock::now(); diff --git a/moveit_ros/hybrid_planning/CHANGELOG.rst b/moveit_ros/hybrid_planning/CHANGELOG.rst index 876c83a9ada..17d9ea2a397 100644 --- a/moveit_ros/hybrid_planning/CHANGELOG.rst +++ b/moveit_ros/hybrid_planning/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_hybrid_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index e74015c0bca..fe774dc01fa 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -60,10 +60,12 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node, { stop_before_collision_ = node->declare_parameter("stop_before_collision", false); } - planning_scene_monitor_ = planning_scene_monitor; node_ = node; path_invalidation_event_send_ = false; num_iterations_stuck_ = 0; + + planning_scene_monitor_ = planning_scene_monitor; + return true; } @@ -106,6 +108,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto bool is_path_valid = false; // Lock the planning scene as briefly as possible { + planning_scene_monitor_->updateSceneWithCurrentState(); planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(planning_scene_monitor_); current_state = std::make_shared(locked_planning_scene->getCurrentState()); is_path_valid = locked_planning_scene->isPathValid(local_trajectory, local_trajectory.getGroupName(), false); 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 2c34cec5c41..0edc36e3410 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 @@ -95,7 +95,9 @@ bool LocalPlannerComponent::initialize() // Start state and scene monitors planning_scene_monitor_->startSceneMonitor(config_.monitored_planning_scene_topic); planning_scene_monitor_->startWorldGeometryMonitor(config_.collision_object_topic); - planning_scene_monitor_->startStateMonitor(config_.joint_states_topic); + planning_scene_monitor_->startStateMonitor(config_.joint_states_topic, "/attached_collision_object"); + planning_scene_monitor_->monitorDiffs(true); + planning_scene_monitor_->stopPublishingPlanningScene(); // Load trajectory operator plugin try @@ -264,8 +266,6 @@ void LocalPlannerComponent::executeIteration() // If the planner received an action request and a global solution it starts to plan locally case LocalPlannerState::LOCAL_PLANNING_ACTIVE: { - planning_scene_monitor_->updateSceneWithCurrentState(); - // Read current robot state const moveit::core::RobotState current_robot_state = [this] { planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor_); diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml index 64a0d4be474..a7d2b39d0ad 100644 --- a/moveit_ros/hybrid_planning/package.xml +++ b/moveit_ros/hybrid_planning/package.xml @@ -1,6 +1,6 @@ moveit_hybrid_planning - 2.7.0 + 2.7.1 Hybrid planning components of MoveIt 2 Sebastian Jahr diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 1eacd940368..0077f7fb946 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, Robert Haschke + 2.7.0 (2023-01-29) ------------------ * move_group: Delete unused execute_trajectory_service_capability (`#1836 `_) diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index 81ec98ee346..5b4f4e0933c 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.0 + 2.7.1 The move_group node for MoveIt Michael Görner Henning Kayser diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index a0af10d1cea..d9edcc9dcf2 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 2927887b725..ebd7f77fb03 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -2,7 +2,7 @@ moveit_ros - 2.7.0 + 2.7.1 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 42402f09dc4..97e87285b8a 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Add callback for velocity scaling override + fix params namespace not being set (`#2021 `_) +* Contributors: Sebastian Castro + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 5420f54584c..b417ef6a34e 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -337,6 +337,8 @@ class ServoCalcs // dynamic parameters std::string robot_link_command_frame_; rcl_interfaces::msg::SetParametersResult robotLinkCommandFrameCallback(const rclcpp::Parameter& parameter); + double override_velocity_scaling_factor_; + rcl_interfaces::msg::SetParametersResult overrideVelocityScalingFactorCallback(const rclcpp::Parameter& parameter); // Load a smoothing plugin pluginlib::ClassLoader smoothing_loader_; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h index efd580543ae..81772b85496 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -54,7 +54,7 @@ struct ServoParameters using SharedConstPtr = std::shared_ptr; // Parameter namespace - const std::string ns; + std::string ns; // ROS Parameters // Note that all of these are effectively const because the only way to create one of these diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py index 070ce335d22..a7a3a1dab81 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py @@ -39,6 +39,17 @@ def generate_launch_description(): servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml") servo_params = {"moveit_servo": servo_yaml} + # Get the ee_frame_name parameter + ee_frame_name_servo_param = servo_params["moveit_servo"]["ee_frame_name"] + + # Declare ee_frame_name launch parameter + ee_frame_name_launch_arg = launch.actions.DeclareLaunchArgument( + "ee_frame_name", + default_value=launch.substitutions.TextSubstitution( + text=ee_frame_name_servo_param + ), + ) + # RViz rviz_config_file = ( get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" @@ -137,6 +148,11 @@ def generate_launch_description(): executable="servo_node_main", parameters=[ servo_params, + { + "ee_frame_name": launch.substitutions.LaunchConfiguration( + "ee_frame_name" + ) + }, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, @@ -146,6 +162,7 @@ def generate_launch_description(): return launch.LaunchDescription( [ + ee_frame_name_launch_arg, rviz_node, ros2_control_node, joint_state_broadcaster_spawner, diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index bd26833bade..177597ea334 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -2,7 +2,7 @@ moveit_servo - 2.7.0 + 2.7.1 Provides real-time manipulator Cartesian and joint servoing. Blake Anderson Andy Zelenak diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 6e071b1d951..8201bf32697 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -81,6 +81,7 @@ ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node, , stop_requested_(true) , paused_(false) , robot_link_command_frame_(parameters->robot_link_command_frame) + , override_velocity_scaling_factor_(parameters->override_velocity_scaling_factor) , smoothing_loader_("moveit_core", "online_signal_smoothing::SmoothingBaseClass") { // Register callback for changes in robot_link_command_frame @@ -90,7 +91,16 @@ ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node, }); if (!callback_success) { - throw std::runtime_error("Failed to register setParameterCallback"); + throw std::runtime_error("Failed to register setParameterCallback for robot_link_command_frame"); + } + + // Register callback for changes in override_velocity_scaling_factor + callback_success = parameters_->registerSetParameterCallback( + parameters->ns + ".override_velocity_scaling_factor", + [this](const rclcpp::Parameter& parameter) { return overrideVelocityScalingFactorCallback(parameter); }); + if (!callback_success) + { + throw std::runtime_error("Failed to register setParameterCallback for override_velocity_scaling_factor"); } // MoveIt Setup @@ -517,6 +527,18 @@ rcl_interfaces::msg::SetParametersResult ServoCalcs::robotLinkCommandFrameCallba return result; }; +rcl_interfaces::msg::SetParametersResult +ServoCalcs::overrideVelocityScalingFactorCallback(const rclcpp::Parameter& parameter) +{ + const std::lock_guard lock(main_loop_mutex_); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + override_velocity_scaling_factor_ = parameter.as_double(); + RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to: " + << std::to_string(override_velocity_scaling_factor_)); + return result; +}; + // Perform the servoing calculations bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd, trajectory_msgs::msg::JointTrajectory& joint_trajectory) @@ -705,7 +727,7 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, // Enforce SRDF velocity limits enforceVelocityLimits(joint_model_group_, parameters_->publish_period, internal_joint_state_, - parameters_->override_velocity_scaling_factor); + 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_); diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index a6f566a5093..702717b808f 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -95,6 +95,7 @@ void ServoParameters::declare(const std::string& ns, using rclcpp::ParameterType::PARAMETER_INTEGER; using rclcpp::ParameterType::PARAMETER_STRING; auto parameters = ServoParameters{}; + parameters.ns = ns; // ROS Parameters node_parameters->declare_parameter( @@ -265,6 +266,7 @@ ServoParameters ServoParameters::get(const std::string& ns, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& node_parameters) { auto parameters = ServoParameters{}; + parameters.ns = ns; // ROS Parameters parameters.use_gazebo = node_parameters->get_parameter(ns + ".use_gazebo").as_bool(); diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index 86f83a6dd12..4e59254f663 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 60ba008dcac..0824a9f231a 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.0 + 2.7.1 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 2721c43960b..1b42a3926a3 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Contributors: Robert Haschke + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 05cdf8685e5..3003f9d3151 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -2,7 +2,7 @@ moveit_ros_perception - 2.7.0 + 2.7.1 Components of MoveIt connecting to perception Henning Kayser Tyler Weaver diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index a4674363f20..48eb0825e45 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Benchmark parallel planning pipelines (`#1539 `_) + * Remove launch and config files (moved to moveit_resources) +* Merge pull request `#1546 `_ from peterdavidfagan/moveit_py + Python Bindings - moveit_py +* add new python bindings + Co-authored-by: Henning Kayser + Co-authored-by: Michael Gorner + Co-authored-by: Robert Haschke + Co-authored-by: AndyZe + Co-authored-by: Peter Mitrano + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + Co-authored-by: Jafar + Co-authored-by: Shahwas Khan +* moveit_cpp: handle the case where blocking==false (`#1834 `_) +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, AndyZe, Jafar, Robert Haschke, Sebastian Jahr, peterdavidfagan + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 9dcf18a6f0f..65856956585 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -33,6 +33,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS robot_model_loader/include constraint_sampler_manager_loader/include planning_pipeline/include + planning_pipeline_interfaces/include planning_scene_monitor/include trajectory_execution_manager/include plan_execution/include @@ -46,6 +47,7 @@ set(THIS_PACKAGE_LIBRARIES moveit_robot_model_loader moveit_constraint_sampler_manager_loader moveit_planning_pipeline + moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager moveit_plan_execution moveit_planning_scene_monitor @@ -83,6 +85,7 @@ add_subdirectory(kinematics_plugin_loader) add_subdirectory(robot_model_loader) add_subdirectory(constraint_sampler_manager_loader) 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) diff --git a/moveit_ros/planning/moveit_cpp/CMakeLists.txt b/moveit_ros/planning/moveit_cpp/CMakeLists.txt index 3e920a5e89d..facf2e13101 100644 --- a/moveit_ros/planning/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning/moveit_cpp/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(moveit_cpp SHARED src/moveit_cpp.cpp - src/parallel_planning_callbacks.cpp src/planning_component.cpp ) set_target_properties(moveit_cpp PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -11,6 +10,7 @@ ament_target_dependencies(moveit_cpp target_link_libraries(moveit_cpp moveit_planning_scene_monitor moveit_planning_pipeline + moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) 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 644bdb3faa8..32d8a54714a 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 @@ -150,7 +150,7 @@ class MoveItCpp moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0); /** \brief Get all loaded planning pipeline instances mapped to their reference names */ - const std::map& getPlanningPipelines() const; + const std::unordered_map& getPlanningPipelines() const; /** \brief Get the stored instance of the planning scene monitor */ const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; @@ -189,8 +189,8 @@ class MoveItCpp planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Planning - std::map planning_pipelines_; - std::map> groups_algorithms_map_; + std::unordered_map planning_pipelines_; + std::unordered_map> groups_algorithms_map_; // Execution trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 46a1c8e86c2..4d134810c02 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -39,7 +39,9 @@ #include #include -#include +#include +#include +#include #include #include #include @@ -106,13 +108,13 @@ class PlanningComponent MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node, const std::vector& planning_pipeline_names) { - multi_plan_request_parameters.reserve(planning_pipeline_names.size()); + plan_request_parameter_vector.reserve(planning_pipeline_names.size()); for (const auto& planning_pipeline_name : planning_pipeline_names) { PlanRequestParameters parameters; parameters.load(node, planning_pipeline_name); - multi_plan_request_parameters.push_back(parameters); + plan_request_parameter_vector.push_back(parameters); } } @@ -122,18 +124,9 @@ class PlanningComponent } // Plan request parameters for the individual planning pipelines which run concurrently - std::vector multi_plan_request_parameters; + std::vector plan_request_parameter_vector; }; - /** \brief A solution callback function type for the parallel planning API of planning component */ - typedef std::function& solutions)> - SolutionCallbackFunction; - /** \brief A stopping criterion callback function for the parallel planning API of planning component */ - typedef std::function - StoppingCriterionFunction; - /** \brief Constructor */ PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node); PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp); @@ -202,16 +195,17 @@ class PlanningComponent /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. */ planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, - planning_scene::PlanningScenePtr const_planning_scene = nullptr); + planning_scene::PlanningScenePtr planning_scene = nullptr); /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. This defaults to taking the full planning time (null stopping_criterion_callback) * and finding the shortest solution in joint space. */ planning_interface::MotionPlanResponse plan(const MultiPipelinePlanRequestParameters& parameters, - const SolutionCallbackFunction& solution_selection_callback = &getShortestSolution, - StoppingCriterionFunction stopping_criterion_callback = nullptr, - const planning_scene::PlanningScenePtr planning_scene = nullptr); + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = + &moveit::planning_pipeline_interfaces::getShortestSolution, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr, + planning_scene::PlanningScenePtr planning_scene = nullptr); /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates * after the execution is complete. The execution can be run in background by setting blocking to false. */ @@ -220,6 +214,15 @@ class PlanningComponent return false; }; + /** \brief Utility function to get a MotionPlanRequest from PlanRequestParameters and the internal state of the + * PlanningComponent instance */ + ::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters); + + /** \brief Utility function to get a Vector of MotionPlanRequest from a vector of PlanRequestParameters and the + * internal state of the PlanningComponent instance */ + std::vector<::planning_interface::MotionPlanRequest> + getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters); + private: // Core properties and instances rclcpp::Node::SharedPtr node_; @@ -234,7 +237,6 @@ class PlanningComponent std::vector current_goal_constraints_; moveit_msgs::msg::Constraints current_path_constraints_; moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_; - PlanRequestParameters plan_request_parameters_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; bool workspace_parameters_set_ = false; diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index aa8436be5dd..bb64bcb7a5a 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -44,7 +45,6 @@ namespace moveit_cpp { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.moveit_cpp"); -constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Options(node)) { @@ -129,34 +129,13 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) { // TODO(henningkayser): Use parent namespace for planning pipeline config lookup - // ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace); - for (const auto& planning_pipeline_name : options.pipeline_names) - { - if (planning_pipelines_.count(planning_pipeline_name) > 0) - { - RCLCPP_WARN(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); - continue; - } - RCLCPP_INFO(LOGGER, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); - planning_pipeline::PlanningPipelinePtr pipeline; - pipeline = std::make_shared(robot_model_, node_, planning_pipeline_name, - PLANNING_PLUGIN_PARAM); - - if (!pipeline->getPlannerManager()) - { - RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); - continue; - } - - planning_pipelines_[planning_pipeline_name] = pipeline; - } - + planning_pipelines_ = + moveit::planning_pipeline_interfaces::createPlanningPipelineMap(options.pipeline_names, robot_model_, node_); if (planning_pipelines_.empty()) { RCLCPP_ERROR(LOGGER, "Failed to load any planning pipelines."); return false; } - return true; } @@ -191,7 +170,7 @@ moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait) return current_state; } -const std::map& MoveItCpp::getPlanningPipelines() const +const std::unordered_map& MoveItCpp::getPlanningPipelines() const { return planning_pipelines_; } diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 72a1edc553d..0f10c6554a2 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -57,15 +57,6 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } - plan_request_parameters_.load(node_); - RCLCPP_DEBUG_STREAM( - LOGGER, "Default plan request parameters loaded with --" - << " planning_pipeline: " << plan_request_parameters_.planning_pipeline << ',' - << " planner_id: " << plan_request_parameters_.planner_id << ',' - << " planning_time: " << plan_request_parameters_.planning_time << ',' - << " planning_attempts: " << plan_request_parameters_.planning_attempts << ',' - << " max_velocity_scaling_factor: " << plan_request_parameters_.max_velocity_scaling_factor << ',' - << " max_acceleration_scaling_factor: " << plan_request_parameters_.max_acceleration_scaling_factor); } PlanningComponent::PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node) @@ -125,6 +116,14 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest return plan_solution; } + // Check if goal constraints exist + if (current_goal_constraints_.empty()) + { + RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; + return plan_solution; + } + if (!planning_scene) { // Clone current planning scene planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor(); @@ -136,155 +135,91 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest planning_scene_monitor.reset(); // release this pointer} } // Init MotionPlanRequest - ::planning_interface::MotionPlanRequest req; - req.group_name = group_name_; - req.planner_id = parameters.planner_id; - req.num_planning_attempts = std::max(1, parameters.planning_attempts); - req.allowed_planning_time = parameters.planning_time; - req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; - req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; - - if (workspace_parameters_set_) - { - req.workspace_parameters = workspace_parameters_; - } + ::planning_interface::MotionPlanRequest request = getMotionPlanRequest(parameters); // Set start state - moveit::core::RobotStatePtr start_state = considered_start_state_; - if (!start_state) + planning_scene->setCurrentState(request.start_state); + + // Run planning attempt + return moveit::planning_pipeline_interfaces::planWithSinglePipeline(request, planning_scene, + moveit_cpp_->getPlanningPipelines()); +} + +planning_interface::MotionPlanResponse PlanningComponent::plan( + const MultiPipelinePlanRequestParameters& parameters, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, + planning_scene::PlanningScenePtr planning_scene) +{ + auto plan_solution = planning_interface::MotionPlanResponse(); + + // check if joint_model_group exists + if (!joint_model_group_) { - start_state = moveit_cpp_->getCurrentState(); + RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; + return plan_solution; } - start_state->update(); - moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); - planning_scene->setCurrentState(*start_state); - // Set goal constraints + // Check if goal constraints exist if (current_goal_constraints_.empty()) { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return plan_solution; } - req.goal_constraints = current_goal_constraints_; - // Set path constraints - req.path_constraints = current_path_constraints_; - // Set trajectory constraints - req.trajectory_constraints = current_trajectory_constraints_; - // Run planning attempt - const auto& pipelines = moveit_cpp_->getPlanningPipelines(); - auto it = pipelines.find(parameters.planning_pipeline); - if (it == pipelines.end()) - { - RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; - return plan_solution; + if (!planning_scene) + { // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor(); + planning_scene_monitor->updateFrameTransforms(); + planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + planning_scene_monitor.reset(); // release this pointer} } - const planning_pipeline::PlanningPipelinePtr pipeline = it->second; - - ::planning_interface::MotionPlanResponse res; - pipeline->generatePlan(planning_scene, req, res); + // Init MotionPlanRequest + std::vector<::planning_interface::MotionPlanRequest> requests = getMotionPlanRequestVector(parameters); - plan_solution.error_code = res.error_code; - if (res.error_code.val != res.error_code.SUCCESS) + // Set start state + for (const auto& request : requests) { - RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - return plan_solution; + planning_scene->setCurrentState(request.start_state); } - plan_solution.trajectory = res.trajectory; - plan_solution.planning_time = res.planning_time; - plan_solution.start_state = req.start_state; - plan_solution.error_code = res.error_code.val; - - // TODO(henningkayser): Visualize trajectory - // std::vector eef_links; - // if (joint_model_group->getEndEffectorTips(eef_links)) - //{ - // for (const auto& eef_link : eef_links) - // { - // RCLCPP_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName()); - // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link); - // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false); - // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); - // } - //} - return plan_solution; -} - -planning_interface::MotionPlanResponse PlanningComponent::plan( - const MultiPipelinePlanRequestParameters& parameters, const SolutionCallbackFunction& solution_selection_callback, - StoppingCriterionFunction stopping_criterion_callback, const planning_scene::PlanningScenePtr planning_scene) -{ - // Create solutions container - PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() }; - std::vector planning_threads; - planning_threads.reserve(parameters.multi_plan_request_parameters.size()); - - // 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(); - if (parameters.multi_plan_request_parameters.size() > hardware_concurrency && hardware_concurrency != 0) - { - RCLCPP_WARN(LOGGER, - "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " - "hardware ('%d')", - parameters.multi_plan_request_parameters.size(), hardware_concurrency); - } + auto const motion_plan_response_vector = moveit::planning_pipeline_interfaces::planWithParallelPipelines( + requests, planning_scene, moveit_cpp_->getPlanningPipelines(), stopping_criterion_callback, + solution_selection_function); - // Launch planning threads - for (const auto& plan_request_parameter : parameters.multi_plan_request_parameters) + try { - auto planning_thread = std::thread([&]() { - auto plan_solution = planning_interface::MotionPlanResponse(); - try - { - // Use planning scene if provided, otherwise the planning scene from planning scene monitor is used - plan_solution = plan(plan_request_parameter, planning_scene); - } - catch (const std::exception& e) - { - RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str() - << "' threw exception '" << e.what() << '\''); - plan_solution = planning_interface::MotionPlanResponse(); - plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; - } - plan_solution.planner_id = plan_request_parameter.planner_id; - planning_solutions.pushBack(plan_solution); - - if (stopping_criterion_callback != nullptr) - { - if (stopping_criterion_callback(planning_solutions, parameters)) - { - // Terminate planning pipelines - RCLCPP_ERROR_STREAM(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); - for (const auto& plan_request_parameter : parameters.multi_plan_request_parameters) - { - moveit_cpp_->terminatePlanningPipeline(plan_request_parameter.planning_pipeline); - } - } - } - }); - planning_threads.push_back(std::move(planning_thread)); + // If a solution_selection function is passed to the parallel pipeline interface, the returned vector contains only + // the selected solution + plan_solution = motion_plan_response_vector.at(0); } - - // Wait for threads to finish - for (auto& planning_thread : planning_threads) + catch (std::out_of_range&) { - if (planning_thread.joinable()) - { - planning_thread.join(); - } + RCLCPP_ERROR(LOGGER, "MotionPlanResponse vector was empty after parallel planning"); + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; } - - // Return best solution determined by user defined callback (Default: Shortest path) - return solution_selection_callback(planning_solutions.getSolutions()); + // Run planning attempt + return plan_solution; } planning_interface::MotionPlanResponse PlanningComponent::plan() { - return plan(plan_request_parameters_); + PlanRequestParameters plan_request_parameters; + plan_request_parameters.load(node_); + RCLCPP_DEBUG_STREAM( + LOGGER, "Default plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' + << " planner_id: " << plan_request_parameters.planner_id << ',' + << " planning_time: " << plan_request_parameters.planning_time << ',' + << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' + << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' + << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); + return plan(plan_request_parameters); } bool PlanningComponent::setStartState(const moveit::core::RobotState& start_state) @@ -382,4 +317,46 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name) goal_state.setToDefaultValues(joint_model_group_, goal_state_name); return setGoal(goal_state); } + +::planning_interface::MotionPlanRequest +PlanningComponent::getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters) +{ + ::planning_interface::MotionPlanRequest request; + request.group_name = group_name_; + request.pipeline_id = plan_request_parameters.planning_pipeline; + request.planner_id = plan_request_parameters.planner_id; + request.num_planning_attempts = std::max(1, plan_request_parameters.planning_attempts); + request.allowed_planning_time = plan_request_parameters.planning_time; + request.max_velocity_scaling_factor = plan_request_parameters.max_velocity_scaling_factor; + request.max_acceleration_scaling_factor = plan_request_parameters.max_acceleration_scaling_factor; + if (workspace_parameters_set_) + { + request.workspace_parameters = workspace_parameters_; + } + request.goal_constraints = current_goal_constraints_; + request.path_constraints = current_path_constraints_; + request.trajectory_constraints = current_trajectory_constraints_; + + // Set start state + moveit::core::RobotStatePtr start_state = considered_start_state_; + if (!start_state) + { + start_state = moveit_cpp_->getCurrentState(); + } + start_state->update(); + moveit::core::robotStateToRobotStateMsg(*start_state, request.start_state); + return request; +} + +std::vector<::planning_interface::MotionPlanRequest> PlanningComponent::getMotionPlanRequestVector( + const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters) +{ + 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) + { + motion_plan_requests.push_back(getMotionPlanRequest(plan_request_parameters)); + } + return motion_plan_requests; +} } // namespace moveit_cpp diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index 057885b07bb..c1d56f50b8c 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning - 2.7.0 + 2.7.1 Planning components of MoveIt that use ROS Henning Kayser Tyler Weaver diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/planning_components_tools/launch/collision_checker_compare.launch similarity index 100% rename from moveit_ros/planning/launch/collision_checker_compare.launch rename to moveit_ros/planning/planning_components_tools/launch/collision_checker_compare.launch diff --git a/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt b/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt new file mode 100644 index 00000000000..21025589abe --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt @@ -0,0 +1,21 @@ +add_library(moveit_planning_pipeline_interfaces SHARED + src/planning_pipeline_interfaces.cpp + src/plan_responses_container.cpp + src/solution_selection_functions.cpp + src/stopping_criterion_function.cpp +) + +include(GenerateExportHeader) +generate_export_header(moveit_planning_pipeline_interfaces) +target_include_directories(moveit_planning_pipeline_interfaces PUBLIC $) +set_target_properties(moveit_planning_pipeline_interfaces PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_planning_pipeline_interfaces moveit_planning_pipeline) + +ament_target_dependencies(moveit_planning_pipeline_interfaces + moveit_core + moveit_msgs + rclcpp +) + +install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_interfaces_export.h DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp new file mode 100644 index 00000000000..779a7dd9d4d --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp @@ -0,0 +1,74 @@ +/********************************************************************* + * 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: Sebastian Jahr + Desc: A thread safe container to store motion plan responses */ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +MOVEIT_CLASS_FORWARD(PlanResponsesContainer); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc +/** \brief A container to thread-safely store multiple MotionPlanResponses */ +class PlanResponsesContainer +{ +public: + /** \brief Constructor + * \param [in] expected_size Number of expected solutions + */ + PlanResponsesContainer(const size_t expected_size = 0); + + /** \brief Thread safe method to add PlanResponsesContainer to this data structure TODO(sjahr): Refactor this method to an + * insert method similar to https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161. + * This way, it is possible to create a sorted container e.g. according to a user specified criteria + * \param [in] plan_solution MotionPlanResponse to push back into the vector + */ + void pushBack(const ::planning_interface::MotionPlanResponse& plan_solution); + + /** \brief Get solutions + * \return Read-only access to the responses vector + */ + const std::vector<::planning_interface::MotionPlanResponse>& getSolutions() const; + +private: + std::vector<::planning_interface::MotionPlanResponse> solutions_; + std::mutex solutions_mutex_; +}; +} // namespace planning_pipeline_interfaces +} // namespace moveit 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 new file mode 100644 index 00000000000..e4b0ec11e84 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -0,0 +1,118 @@ +/********************************************************************* + * 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: Sebastian Jahr + Desc: Functions to create and use a map of PlanningPipelines to solve MotionPlanRequests */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +/** \brief A stopping criterion callback function for the parallel planning API of planning component + * \param [in] plan_responses_container Container with responses to be taken into account for the stopping decision + * \param [in] plan_requests Motion plan requests for the parallel planner + * \return True to stop parallel planning and abort planning pipelines that haven't terminated by now + */ +typedef std::function& plan_requests)> + StoppingCriterionFunction; + +/** \brief A solution callback function type for the parallel planning API of planning component + * \param [in] solutions Motion plan responses to choose from + * \return Selected motion plan response + */ +typedef std::function<::planning_interface::MotionPlanResponse( + const std::vector<::planning_interface::MotionPlanResponse>& solutions)> + SolutionSelectionFunction; + +/** \brief Function to calculate the MotionPlanResponse for a given MotionPlanRequest and a PlanningScene + * \param [in] motion_plan_request Motion planning problem to be solved + * \param [in] planning_scene Planning scene for which the given planning problem needs to be solved + * \param [in] planning_pipelines Pipelines available to solve the problem, if the requested pipeline is not provided + * the MotionPlanResponse will be FAILURE \return MotionPlanResponse for the given planning problem + */ +::planning_interface::MotionPlanResponse planWithSinglePipeline( + const ::planning_interface::MotionPlanRequest& motion_plan_request, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines); + +/** \brief Function to solve multiple planning problems in parallel threads with multiple planning pipelines at the same + time + * \param [in] motion_plan_request Motion planning problems to be solved + * \param [in] planning_scene Planning scene for which the given planning problem needs to be solved + * \param [in] planning_pipelines Pipelines available to solve the problems, if a requested pipeline is not provided the + MotionPlanResponse will be FAILURE + * \param [in] stopping_criterion_callback If this function returns true, the planning pipelines that are still running + will be terminated and the existing solutions will be evaluated. If no callback is provided, all planning pipelines + terminate after the max. planning time defined in the MotionPlanningRequest is reached. + * \param [in] solution_selection_function Function to select a specific solution out of all available solution. If no + function is provided, all solutions are returned. + + \return If a solution_selection_function is provided a vector containing the selected response is returned, otherwise + the vector contains all solutions produced. +*/ +const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipelines( + const std::vector<::planning_interface::MotionPlanRequest>& motion_plan_requests, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines, + const StoppingCriterionFunction& stopping_criterion_callback = nullptr, + const SolutionSelectionFunction& solution_selection_function = nullptr); + +/** \brief Utility function to create a map of named planning pipelines + * \param [in] pipeline_names Vector of planning pipeline names to be used. Each name is also the namespace from which + * the pipeline parameters are loaded + * \param [in] robot_model Robot model used to initialize the pipelines + * \param [in] node Node used to load parameters + * \param [in] parameter_namespace Optional prefix for the pipeline parameter + * namespace. Empty by default, so only the pipeline name is used as namespace + * \param [in] planning_plugin_param_name + * Optional name of the planning plugin namespace + * \param [in] adapter_plugins_param_name Optional name of the adapter plugin namespace + * \return Map of PlanningPipelinePtr's associated with a name for faster look-up + */ +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"); +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp new file mode 100644 index 00000000000..535a9aecc21 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp @@ -0,0 +1,54 @@ +/********************************************************************* + * 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: Sebastian Jahr + Desc: Solution selection function implementations */ + +#pragma once + +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...) + * \param [in] solutions Vector of solutions to chose the shortest one from + * \return Shortest solution, trajectory of the returned MotionPlanResponse is a nullptr if no solution is found! + */ +::planning_interface::MotionPlanResponse +getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse>& solutions); + +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp similarity index 80% rename from moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp rename to moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp index e70742de557..5549090bcb2 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp @@ -32,17 +32,20 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sebastian Jahr - Desc: Common callback functions for parallel planning */ +/* Author: AndyZe, Sebastian Jahr + Desc: Stopping criterion function implementations */ #pragma once -#include +#include -namespace moveit_cpp +namespace moveit +{ +namespace planning_pipeline_interfaces { /** \brief A callback function that can be used as a parallel planning stop criterion. * It stops parallel planning as soon as any planner finds a solution. */ -bool stopAtFirstSolution(PlanSolutions const& plan_solutions, - PlanningComponent::MultiPipelinePlanRequestParameters const& /*plan_request_parameters*/); -} // namespace moveit_cpp +bool stopAtFirstSolution(const PlanResponsesContainer& plan_responses_container, + const std::vector<::planning_interface::MotionPlanRequest>& plan_requests); +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp new file mode 100644 index 00000000000..88967e08943 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp @@ -0,0 +1,60 @@ + +/********************************************************************* + * 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: Sebastian Jahr */ + +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +PlanResponsesContainer::PlanResponsesContainer(const size_t expected_size) +{ + solutions_.reserve(expected_size); +} + +void PlanResponsesContainer::pushBack(const ::planning_interface::MotionPlanResponse& plan_solution) +{ + std::lock_guard lock_guard(solutions_mutex_); + solutions_.push_back(plan_solution); +} + +const std::vector<::planning_interface::MotionPlanResponse>& PlanResponsesContainer::getSolutions() const +{ + return solutions_; +} +} // 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 new file mode 100644 index 00000000000..d689397f441 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -0,0 +1,192 @@ +/********************************************************************* + * 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: Sebastian Jahr */ + +#include + +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.::planning_interface.planning_pipeline_interfaces"); + +::planning_interface::MotionPlanResponse +planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_plan_request, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines) +{ + ::planning_interface::MotionPlanResponse motion_plan_response; + auto it = planning_pipelines.find(motion_plan_request.pipeline_id); + if (it == planning_pipelines.end()) + { + RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); + motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; + return motion_plan_response; + } + const planning_pipeline::PlanningPipelinePtr pipeline = it->second; + pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response); + return motion_plan_response; +} + +const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipelines( + const std::vector<::planning_interface::MotionPlanRequest>& motion_plan_requests, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines, + const StoppingCriterionFunction& stopping_criterion_callback, + const SolutionSelectionFunction& solution_selection_function) +{ + // Create solutions container + PlanResponsesContainer plan_responses_container{ motion_plan_requests.size() }; + std::vector planning_threads; + planning_threads.reserve(motion_plan_requests.size()); + + // 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(); + if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) + { + RCLCPP_WARN(LOGGER, + "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " + "hardware ('%d')", + motion_plan_requests.size(), hardware_concurrency); + } + + // Launch planning threads + for (const auto& request : motion_plan_requests) + { + auto planning_thread = std::thread([&]() { + auto plan_solution = ::planning_interface::MotionPlanResponse(); + try + { + // Use planning scene if provided, otherwise the planning scene from planning scene monitor is used + plan_solution = planWithSinglePipeline(request, planning_scene, planning_pipelines); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(LOGGER, "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); + plan_solution = ::planning_interface::MotionPlanResponse(); + plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; + } + plan_solution.planner_id = request.planner_id; + plan_responses_container.pushBack(plan_solution); + + if (stopping_criterion_callback != nullptr) + { + if (stopping_criterion_callback(plan_responses_container, motion_plan_requests)) + { + // Terminate planning pipelines + RCLCPP_INFO(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); + for (const auto& request : motion_plan_requests) + { + try + { + const auto& planning_pipeline = planning_pipelines.at(request.pipeline_id); + if (planning_pipeline->isActive()) + { + planning_pipeline->terminate(); + } + } + catch (const std::out_of_range&) + { + RCLCPP_WARN(LOGGER, "Cannot terminate pipeline '%s' because no pipeline with that name exists", + request.pipeline_id.c_str()); + } + } + } + } + }); + planning_threads.push_back(std::move(planning_thread)); + } + + // Wait for threads to finish + for (auto& planning_thread : planning_threads) + { + if (planning_thread.joinable()) + { + planning_thread.join(); + } + } + + // If a solution selection function is provided, it is used to compute the return value + if (solution_selection_function) + { + std::vector<::planning_interface::MotionPlanResponse> solutions; + solutions.reserve(1); + solutions.push_back(solution_selection_function(plan_responses_container.getSolutions())); + return solutions; + } + + // Otherwise, just return the unordered list of solutions + return plan_responses_container.getSolutions(); +} + +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) +{ + std::unordered_map planning_pipelines; + for (const auto& planning_pipeline_name : pipeline_names) + { + // Check if pipeline already exists + if (planning_pipelines.count(planning_pipeline_name) > 0) + { + RCLCPP_WARN(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + + // 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); + + if (!pipeline->getPlannerManager()) + { + RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + + planning_pipelines[planning_pipeline_name] = pipeline; + } + + return planning_pipelines; +} + +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp similarity index 59% rename from moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp rename to moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp index a15f1f4661d..f3fc96de98f 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2019, PickNik Inc. + * Copyright (c) 2023, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,61 +32,21 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sebastian Jahr - Desc: A safe data structure for MotionPlanResponses and free functions to analyze them */ +/* Author: Sebastian Jahr */ -#pragma once +#include -#include -#include -#include -#include - -namespace moveit_cpp +namespace moveit { -MOVEIT_CLASS_FORWARD(PlanSolutions); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc - -/** \brief A container to thread-safely store multiple MotionPlanResponses for later usage */ -class PlanSolutions +namespace planning_pipeline_interfaces { -public: - PlanSolutions(const size_t expected_size = 0) - { - solutions_.reserve(expected_size); - } - - /** \brief Thread safe method to add PlanSolutions to this data structure TODO(sjahr): Refactor this method to an - * insert method similar to https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161. - * This way, it is possible to create a sorted container e.g. according to a user specified criteria - */ - void pushBack(const planning_interface::MotionPlanResponse& plan_solution) - { - std::lock_guard lock_guard(solutions_mutex_); - solutions_.push_back(plan_solution); - } - - /** \brief Get solutions */ - const std::vector& getSolutions() const - { - return solutions_; - } - -private: - std::vector solutions_; - std::mutex solutions_mutex_; -}; - -/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...) - * \param [in] solutions Vector of solutions to chose the shortest one from - * \return Shortest solution, trajectory of the returned MotionPlanResponse is a nullptr if no solution is found! - */ -static inline planning_interface::MotionPlanResponse -getShortestSolution(const std::vector& solutions) +::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 planning_interface::MotionPlanResponse& solution_a, - const planning_interface::MotionPlanResponse& solution_b) { + [](const ::planning_interface::MotionPlanResponse& solution_a, + const ::planning_interface::MotionPlanResponse& solution_b) { // If both solutions were successful, check which path is shorter if (solution_a && solution_b) { @@ -103,4 +63,6 @@ getShortestSolution(const std::vector& s }); return *shortest_trajectory; } -} // namespace moveit_cpp + +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/src/parallel_planning_callbacks.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp similarity index 80% rename from moveit_ros/planning/moveit_cpp/src/parallel_planning_callbacks.cpp rename to moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp index 9db08dd0f1c..f0bf424ceee 100644 --- a/moveit_ros/planning/moveit_cpp/src/parallel_planning_callbacks.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp @@ -34,17 +34,17 @@ /* Author: Sebastian Jahr */ -#include +#include -namespace moveit_cpp +namespace moveit { -/** \brief A callback function that can be used as a parallel planning stop criterion. - * It stops parallel planning as soon as any planner finds a solution. */ -bool stopAtFirstSolution(PlanSolutions const& plan_solutions, - PlanningComponent::MultiPipelinePlanRequestParameters const& /*plan_request_parameters*/) +namespace planning_pipeline_interfaces +{ +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_solutions.getSolutions()) + for (auto const& solution : plan_responses_container.getSolutions()) { // bool(solution) is shorthand to evaluate the error code of the solution, checking for SUCCESS if (bool(solution)) @@ -56,4 +56,5 @@ bool stopAtFirstSolution(PlanSolutions const& plan_solutions, // Return false when parallel planning should continue because it hasn't found a successful solution yet return false; } -} // namespace moveit_cpp +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 54a6a016bb8..bfbbdd60eb1 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, Robert Haschke + 2.7.0 (2023-01-29) ------------------ * Merge https://github.com/ros-planning/moveit/commit/9225971216885490e933ece25390c63ca14f8a58 diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index b1120c852ff..5518fe82eba 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -2,8 +2,8 @@ moveit_ros_planning_interface - 2.7.0 - Components of MoveIt that offer simpler interfaces to planning and execution + 2.7.1 + Components of MoveIt that offer simpler remote (as from another ROS 2 node) interfaces to planning and execution Henning Kayser Tyler Weaver Michael Görner diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 3f45fa7ed6f..55dccb94665 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Contributors: Robert Haschke + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index 383a7c320ab..76804aecd50 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.0 + 2.7.1 Components of MoveIt that offer interaction via interactive markers Henning Kayser Tyler Weaver diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 9bd64108016..a3cfc4f2cf8 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Doxygen tag (`#1955 `_) + * Generate Doxygen Tag + * Install tagfile in output directory + * Fix problematic override for Doxygen linking +* remove underscore from public member in MotionPlanResponse (`#1939 `_) + * remove underscore from private members + * fix more uses of the suffix notation +* Contributors: AlexWebb, Henning Kayser + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 68b1f2ae001..4f4eb3a641b 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -2,7 +2,7 @@ moveit_ros_visualization - 2.7.0 + 2.7.1 Components of MoveIt that offer visualization Henning Kayser Tyler Weaver diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index 7bcab41fbe2..ada06febcb1 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -45,7 +45,7 @@ namespace moveit_rviz_plugin class PlanningLinkUpdater : public rviz_default_plugins::robot::LinkUpdater { public: - PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : kinematic_state_(state) + PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : robot_state_(state) { } @@ -54,6 +54,6 @@ class PlanningLinkUpdater : public rviz_default_plugins::robot::LinkUpdater Ogre::Quaternion& collision_orientation) const override; private: - moveit::core::RobotStateConstPtr kinematic_state_; + moveit::core::RobotStateConstPtr robot_state_; }; } // namespace moveit_rviz_plugin 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 26bd1049ae7..f04e32fd55b 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 @@ -61,13 +61,13 @@ class RobotStateVisualization void load(const urdf::ModelInterface& descr, bool visual = true, bool collision = true); void clear(); - void update(const moveit::core::RobotStateConstPtr& kinematic_state); - void update(const moveit::core::RobotStateConstPtr& kinematic_state, + void update(const moveit::core::RobotStateConstPtr& robot_state); + void update(const moveit::core::RobotStateConstPtr& robot_state, const std_msgs::msg::ColorRGBA& default_attached_object_color); - void update(const moveit::core::RobotStateConstPtr& kinematic_state, + void update(const moveit::core::RobotStateConstPtr& robot_state, const std_msgs::msg::ColorRGBA& default_attached_object_color, const std::map& color_map); - void updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state); + void updateKinematicState(const moveit::core::RobotStateConstPtr& robot_state); void setDefaultAttachedObjectColor(const std_msgs::msg::ColorRGBA& default_attached_object_color); /// update color of all attached object shapes void updateAttachedObjectColors(const std_msgs::msg::ColorRGBA& attached_object_color); @@ -98,7 +98,7 @@ class RobotStateVisualization void setAlpha(float alpha); private: - void updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, + void updateHelper(const moveit::core::RobotStateConstPtr& robot_state, const std_msgs::msg::ColorRGBA& default_attached_object_color, const std::map* color_map); rviz_default_plugins::robot::Robot robot_; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 4c104ed29b4..b01dd7978af 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -43,7 +43,7 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const { - const moveit::core::LinkModel* link_model = kinematic_state_->getLinkModel(link_name); + const moveit::core::LinkModel* link_model = robot_state_->getLinkModel(link_name); if (!link_model) { @@ -51,8 +51,8 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin } // getGlobalLinkTransform() returns a valid isometry by contract - const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation(); - Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear()); + const Eigen::Vector3d& robot_visual_position = robot_state_->getGlobalLinkTransform(link_model).translation(); + Eigen::Quaterniond robot_visual_orientation(robot_state_->getGlobalLinkTransform(link_model).linear()); visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z()); visual_orientation = Ogre::Quaternion(robot_visual_orientation.w(), robot_visual_orientation.x(), robot_visual_orientation.y(), robot_visual_orientation.z()); 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 95cc5ca8d77..b8e105ba538 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 @@ -89,33 +89,33 @@ void RobotStateVisualization::updateAttachedObjectColors(const std_msgs::msg::Co robot_.getAlpha()); } -void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state) +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& robot_state) { - updateHelper(kinematic_state, default_attached_object_color_, nullptr); + updateHelper(robot_state, default_attached_object_color_, nullptr); } -void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& robot_state, const std_msgs::msg::ColorRGBA& default_attached_object_color) { - updateHelper(kinematic_state, default_attached_object_color, nullptr); + updateHelper(robot_state, default_attached_object_color, nullptr); } -void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& robot_state, const std_msgs::msg::ColorRGBA& default_attached_object_color, const std::map& color_map) { - updateHelper(kinematic_state, default_attached_object_color, &color_map); + updateHelper(robot_state, default_attached_object_color, &color_map); } -void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& robot_state, const std_msgs::msg::ColorRGBA& default_attached_object_color, const std::map* color_map) { - robot_.update(PlanningLinkUpdater(kinematic_state)); + robot_.update(PlanningLinkUpdater(robot_state)); render_shapes_->clear(); std::vector attached_bodies; - kinematic_state->getAttachedBodies(attached_bodies); + robot_state->getAttachedBodies(attached_bodies); for (const moveit::core::AttachedBody* attached_body : attached_bodies) { std_msgs::msg::ColorRGBA color = default_attached_object_color; @@ -153,9 +153,9 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt robot_.setVisible(visible_); } -void RobotStateVisualization::updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state) +void RobotStateVisualization::updateKinematicState(const moveit::core::RobotStateConstPtr& robot_state) { - robot_.update(PlanningLinkUpdater(kinematic_state)); + robot_.update(PlanningLinkUpdater(robot_state)); } void RobotStateVisualization::setVisible(bool visible) diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index a3549cded0b..39c27e73ed7 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Contributors: Robert Haschke + 2.7.0 (2023-01-29) ------------------ * converted characters from string format to character format (`#1881 `_) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 6211b38059a..a852a2aa828 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -2,7 +2,7 @@ moveit_ros_warehouse - 2.7.0 + 2.7.1 Components of MoveIt connecting to MongoDB Henning Kayser Tyler Weaver diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index f758c1f4d39..2b523c0dba5 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index f82b2825b7b..1c9eee7358d 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -2,7 +2,7 @@ moveit_runtime - 2.7.0 + 2.7.1 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 34184c497fc..0526b5a0ee7 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_setup_app_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* add missing dependencies on config utils (`#1962 `_) + when installing ros-humble-moveit-setup-assistant from debs, + the package cannot currently run due to this missing depend +* Contributors: Michael Ferguson + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml index 954df73ad63..db968d16a81 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.0 + 2.7.1 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 0811e895018..5214301c92c 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.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_setup_assistant/moveit_setup_assistant/package.xml b/moveit_setup_assistant/moveit_setup_assistant/package.xml index 8d79fbac0e3..1046b0b9ee0 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.0 + 2.7.1 Generates a configuration package that makes it easy to use MoveIt Henning Kayser Tyler Weaver diff --git a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst index 970f32c1ee7..33a16c28687 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_setup_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* add missing dependencies on config utils (`#1962 `_) + when installing ros-humble-moveit-setup-assistant from debs, + the package cannot currently run due to this missing depend +* Contributors: Michael Ferguson + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_setup_assistant/moveit_setup_controllers/package.xml b/moveit_setup_assistant/moveit_setup_controllers/package.xml index 98bba53fd24..13ad9f12170 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.0 + 2.7.1 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 001624599a7..02e9a7df13b 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_setup_core_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml index ea540f770c0..59421fd2e6a 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.0 + 2.7.1 Core (meta) plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp index 0fa446ab57e..74fd7ee166f 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp @@ -305,8 +305,16 @@ void StartScreenWidget::onPackagePathChanged(const QString& /*path*/) void StartScreenWidget::onUrdfPathChanged(const QString& path) { - setup_step_.loadURDFFile(path.toStdString(), urdf_file_->getArgs().toStdString()); - urdf_file_->setArgsEnabled(setup_step_.isXacroFile()); + try + { + setup_step_.loadURDFFile(path.toStdString(), urdf_file_->getArgs().toStdString()); + urdf_file_->setArgsEnabled(setup_step_.isXacroFile()); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR(setup_step_.getLogger(), "Error Loading URDF Path. %s", e.what()); + QMessageBox::warning(this, "Error Loading URDF Path", QString(e.what())); + } } bool StartScreenWidget::loadPackageSettings(bool show_warnings) @@ -343,8 +351,8 @@ bool StartScreenWidget::loadExistingFiles() } catch (const std::runtime_error& e) { + RCLCPP_ERROR(setup_step_.getLogger(), "Error Loading SRDF. %s", e.what()); QMessageBox::warning(this, "Error Loading SRDF", QString(e.what())); - RCLCPP_ERROR(setup_step_.getLogger(), "%s", e.what()); return false; } diff --git a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst index 50ceed2cb46..b75528c0773 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_setup_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Contributors: Robert Haschke + 2.7.0 (2023-01-29) ------------------ * Merge PR `#1712 `_: fix clang compiler warnings + stricter CI diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp index 3fc15b301ff..ace3237c6b7 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include @@ -49,7 +50,14 @@ namespace moveit_setup */ inline std::filesystem::path getSharePath(const std::string& package_name) { - return std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)); + try + { + return std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)); + } + catch (const std::runtime_error& e) + { + return std::filesystem::path(); + } } /** diff --git a/moveit_setup_assistant/moveit_setup_framework/package.xml b/moveit_setup_assistant/moveit_setup_framework/package.xml index f629dce618a..6f2a08b18a8 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.0 + 2.7.1 C++ Interface for defining setup steps for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause 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 290c9e8a2aa..164c7817c96 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp @@ -98,18 +98,15 @@ void URDFConfig::setPackageName() urdf_pkg_name_ = ""; urdf_pkg_relative_path_ = urdf_path_; // just the absolute path } - else + // Check that ROS can find the package + const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_); + + if (robot_desc_pkg_path.empty()) { - // Check that ROS can find the package - const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_); - - if (robot_desc_pkg_path.empty()) - { - 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()); - } + 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()); } } @@ -134,6 +131,11 @@ 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_framework/src/utilities.cpp b/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp index de0c0c38573..3625d945943 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp @@ -53,7 +53,8 @@ bool extractPackageNameFromPath(const std::filesystem::path& path, std::string& } // truncate path step by step and check if it contains a package.xml - while (!sub_path.empty()) + // This runs until the path is either empty "" or at the root "/" or "C:\\" + while (!sub_path.empty() && sub_path != sub_path.root_path()) { if (std::filesystem::is_regular_file(sub_path / "package.xml")) { diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst index 45a5a869476..e68a0f53dd6 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_setup_srdf_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2023-03-23) +------------------ +* Fix member naming (`#1949 `_) + * Update clang-tidy rules for readability-identifier-naming + Co-authored-by: Sebastian Jahr +* Contributors: Robert Haschke + 2.7.0 (2023-01-29) ------------------ * Fix BSD license in package.xml (`#1796 `_) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml index 7c7e50bd7ea..9fad2fc5a42 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.0 + 2.7.1 SRDF-based plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause