From 1aa4e17d2be38f24fabfced085170aa342812502 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 26 Jan 2023 16:03:37 +0100 Subject: [PATCH 01/22] Fix parameters for ros2_control namespaces (#1833) (#1897) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: AndyZe Co-authored-by: Henning Kayser (cherry picked from commit 5838ce890975e3a058cdc9ab699b27941374c3a2) Co-authored-by: Pablo Iñigo Blasco --- .../src/controller_manager_plugin.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index 0cc3b04dcf..f57cae4d1c 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -256,8 +256,20 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan if (!ns_.empty()) { if (!node_->has_parameter("ros_control_namespace")) + { ns_ = node_->declare_parameter("ros_control_namespace", "/"); + } + else + { + node_->get_parameter("ros_control_namespace", ns_); + } } + else if (node->has_parameter("ros_control_namespace")) + { + node_->get_parameter("ros_control_namespace", ns_); + RCLCPP_INFO_STREAM(LOGGER, "Namespace for controller manager was specified, namespace: " << ns_); + } + list_controllers_service_ = node_->create_client( getAbsName("controller_manager/list_controllers")); switch_controller_service_ = node_->create_client( From 2525d4f2d0d52dbe781937488f826d32c678a9b9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 7 Feb 2023 15:52:09 -0700 Subject: [PATCH 02/22] Add xacro subsititution class and use it for loading urdf & srdf (backport #1805) (#1937) * Add xacro subsititution class and use it for loading urdf & srdf (#1805) * Add Xacro substitution type * Use Xacro substitution for robot description and robot description semantic * Install subsititution folder * Default to load_xacro if there's no launch substitution specified in the mappings (cherry picked from commit 4bc83c3c9e6bfa9efea8c431794a630fbf27dddc) # Conflicts: # moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py * Fix merge conflicts --------- Co-authored-by: Jafar Co-authored-by: Tyler Weaver --- .../moveit_configs_builder.py | 73 ++++++++++++++---- .../substitutions/__init__.py | 1 + .../substitutions/xacro.py | 74 +++++++++++++++++++ moveit_configs_utils/setup.py | 4 +- moveit_configs_utils/test/robot.urdf.xacro | 19 +++++ moveit_configs_utils/test/test_xacro.py | 55 ++++++++++++++ 6 files changed, 209 insertions(+), 17 deletions(-) create mode 100644 moveit_configs_utils/moveit_configs_utils/substitutions/__init__.py create mode 100644 moveit_configs_utils/moveit_configs_utils/substitutions/xacro.py create mode 100644 moveit_configs_utils/test/robot.urdf.xacro create mode 100644 moveit_configs_utils/test/test_xacro.py diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 693c58d061..9190adc369 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -58,7 +58,10 @@ from ament_index_python.packages import get_package_share_directory from launch_param_builder import ParameterBuilder, load_yaml, load_xacro - +from launch_param_builder.utils import ParameterBuilderFileNotFoundError +from moveit_configs_utils.substitutions import Xacro +from launch.some_substitutions_type import SomeSubstitutionsType +from launch_ros.parameter_descriptions import ParameterValue moveit_configs_utils_path = Path(get_package_share_directory("moveit_configs_utils")) @@ -198,7 +201,11 @@ def __init__( self.__robot_description = robot_description - def robot_description(self, file_path: Optional[str] = None, mappings: dict = None): + def robot_description( + self, + file_path: Optional[str] = None, + mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] = None, + ): """Load robot description. :param file_path: Absolute or relative path to the URDF file (w.r.t. robot_name_moveit_config). @@ -209,15 +216,35 @@ def robot_description(self, file_path: Optional[str] = None, mappings: dict = No robot_description_file_path = self.__urdf_package / self.__urdf_file_path else: robot_description_file_path = self._package_path / file_path - self.__moveit_configs.robot_description = { - self.__robot_description: load_xacro( - robot_description_file_path, mappings=mappings - ) - } + if (mappings is None) or all( + (isinstance(key, str) and isinstance(value, str)) + for key, value in mappings.items() + ): + try: + self.__moveit_configs.robot_description = { + self.__robot_description: load_xacro( + robot_description_file_path, mappings=mappings + ) + } + except ParameterBuilderFileNotFoundError as e: + logging.warning(f"\x1b[33;21m{e}\x1b[0m") + logging.warning( + f"\x1b[33;21mThe robot description will be loaded from /robot_description topic \x1b[0m" + ) + + else: + self.__moveit_configs.robot_description = { + self.__robot_description: ParameterValue( + Xacro(str(robot_description_file_path), mappings=mappings), + value_type=str, + ) + } return self def robot_description_semantic( - self, file_path: Optional[str] = None, mappings: dict = None + self, + file_path: Optional[str] = None, + mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] = None, ): """Load semantic robot description. @@ -225,13 +252,29 @@ def robot_description_semantic( :param mappings: mappings to be passed when loading the xacro file. :return: Instance of MoveItConfigsBuilder with robot_description_semantic loaded. """ - self.__moveit_configs.robot_description_semantic = { - self.__robot_description - + "_semantic": load_xacro( - self._package_path / (file_path or self.__srdf_file_path), - mappings=mappings, - ) - } + + if (mappings is None) or all( + (isinstance(key, str) and isinstance(value, str)) + for key, value in mappings.items() + ): + self.__moveit_configs.robot_description_semantic = { + self.__robot_description + + "_semantic": load_xacro( + self._package_path / (file_path or self.__srdf_file_path), + mappings=mappings, + ) + } + else: + self.__moveit_configs.robot_description_semantic = { + self.__robot_description + + "_semantic": ParameterValue( + Xacro( + str(self._package_path / (file_path or self.__srdf_file_path)), + mappings=mappings, + ), + value_type=str, + ) + } return self def robot_description_kinematics(self, file_path: Optional[str] = None): diff --git a/moveit_configs_utils/moveit_configs_utils/substitutions/__init__.py b/moveit_configs_utils/moveit_configs_utils/substitutions/__init__.py new file mode 100644 index 0000000000..7137bb84fb --- /dev/null +++ b/moveit_configs_utils/moveit_configs_utils/substitutions/__init__.py @@ -0,0 +1 @@ +from .xacro import Xacro diff --git a/moveit_configs_utils/moveit_configs_utils/substitutions/xacro.py b/moveit_configs_utils/moveit_configs_utils/substitutions/xacro.py new file mode 100644 index 0000000000..f1ef3a34fd --- /dev/null +++ b/moveit_configs_utils/moveit_configs_utils/substitutions/xacro.py @@ -0,0 +1,74 @@ +from pathlib import Path +from typing import Iterable, Text, Optional + +from launch.frontend import expose_substitution +from launch.launch_context import LaunchContext +from launch.some_substitutions_type import SomeSubstitutionsType +from launch.substitution import Substitution +from launch.utilities import normalize_to_list_of_substitutions +from launch_param_builder import load_xacro + + +@expose_substitution("xacro") +class Xacro(Substitution): + """Substitution that can access load xacro file with mappings involving any subsititutable.""" + + def __init__( + self, + file_path: SomeSubstitutionsType, + *, + mappings: Optional[dict[SomeSubstitutionsType, SomeSubstitutionsType]] = None, + ) -> None: + """Create a Xacro substitution.""" + super().__init__() + + self.__file_path = normalize_to_list_of_substitutions(file_path) + if mappings is None: + self.__mappings = {} + else: + self.__mappings = mappings + + @classmethod + def parse(cls, data: Iterable[SomeSubstitutionsType]): + """Parse `XacroSubstitution` substitution.""" + if len(data) != 1: + raise TypeError( + "xacro substitution expects only support one argument use 'command' subsititutoin for parsing args" + ) + kwargs = {} + kwargs["file_path"] = data[0] + return cls, kwargs + + @property + def file_path(self) -> list[Substitution]: + """Getter for file_path.""" + return self.__file_path + + @property + def mappings(self) -> dict[SomeSubstitutionsType, SomeSubstitutionsType]: + """Getter for mappings.""" + return self.__mappings + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + mappings_formatted = ", ".join( + [f"{k.describe()}:={v.describe()}" for k, v in self.mappings.items()] + ) + return f"Xacro(file_path = {self.file_path}, mappings = {mappings_formatted})" + + def perform(self, context: LaunchContext) -> Text: + """ + Perform the substitution by retrieving the mappings and context. + """ + from launch.utilities import perform_substitutions + + expanded_file_path = perform_substitutions(context, self.__file_path) + expanded_mappings = {} + for (key, value) in self.__mappings.items(): + normalized_key = normalize_to_list_of_substitutions(key) + normalized_value = normalize_to_list_of_substitutions(value) + expanded_mappings[ + perform_substitutions(context, normalized_key) + ] = perform_substitutions(context, normalized_value) + + return load_xacro(Path(expanded_file_path), mappings=expanded_mappings) diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index cd5878f22d..60188f14cc 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -1,4 +1,4 @@ -from setuptools import setup +from setuptools import setup, find_packages from glob import glob package_name = "moveit_configs_utils" @@ -6,7 +6,7 @@ setup( name=package_name, version="2.5.3", - packages=[package_name], + packages=find_packages(), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), diff --git a/moveit_configs_utils/test/robot.urdf.xacro b/moveit_configs_utils/test/robot.urdf.xacro new file mode 100644 index 0000000000..ae244cabcb --- /dev/null +++ b/moveit_configs_utils/test/robot.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs_utils/test/test_xacro.py b/moveit_configs_utils/test/test_xacro.py new file mode 100644 index 0000000000..68709416dd --- /dev/null +++ b/moveit_configs_utils/test/test_xacro.py @@ -0,0 +1,55 @@ +from pathlib import Path + +from launch import LaunchContext +from launch.actions import SetLaunchConfiguration +from launch.frontend.parse_substitution import parse_substitution +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + TextSubstitution, + ThisLaunchFileDir, +) + +from moveit_configs_utils.substitutions import Xacro + + +def test_xacro_subst(): + subst = parse_substitution(r"$(xacro '$(dirname)/robot.urdf.xacro')") + context = LaunchContext() + context.extend_locals({"current_launch_file_directory": str(Path(__file__).parent)}) + assert len(subst) == 1 + assert isinstance(subst[0].file_path[0], ThisLaunchFileDir) + assert isinstance(subst[0].file_path[1], TextSubstitution) + assert isinstance(subst[0], Xacro) + subst[0].perform(context) + + +def test_xacro_no_mappings(): + xacro = Xacro([ThisLaunchFileDir(), "robot.urdf.xacro"]) + context = LaunchContext() + context.extend_locals( + {"current_launch_file_directory": str(Path(__file__).parent) + "/"} + ) + assert xacro.perform(context) + + +def test_xacro_with_mappings(): + xacro = Xacro( + PathJoinSubstitution([ThisLaunchFileDir(), "robot.urdf.xacro"]), + mappings={ + ("test_", LaunchConfiguration("myrobot")): [ + LaunchConfiguration("myrobot"), + "sadf", + ], + "number": LaunchConfiguration("number"), + "postfix": LaunchConfiguration("postfix"), + }, + ) + context = LaunchContext() + context.extend_locals( + {"current_launch_file_directory": str(Path(__file__).parent) + "/"} + ) + SetLaunchConfiguration("myrobot", "robot").visit(context) + SetLaunchConfiguration("number", "2000").visit(context) + SetLaunchConfiguration("postfix", "mypostfix").visit(context) + assert xacro.perform(context) From b585c7c24f550e3ae356f6c0f722380fe4756641 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 12 Feb 2023 14:49:10 +0100 Subject: [PATCH 03/22] Fix clang compiler warnings (backport of #1712) (#1896) - Fix warning: definition of implicit copy assignment operator is deprecated - Fix warning: expression with side effects will be evaluated - Fix warning: passing by value - Enable -Werror - Fix -Wdelete-non-abstract-non-virtual-dtor - Fix more clang warnings - Modernize gtest: TYPED_TEST_CASE -> TYPED_TEST_SUITE - Fix GoogleTestVerification.UninstantiatedTypeParameterizedTestSuite - Add default copy/move constructors/assignment operators As a user-declared destructor deletes any implicitly-defined move constructor/assignment operator, we need to declared them manually. This in turn requires to declare the copy constructor/assignment as well. - Explicitly declare overrides - Add default constructors as they are not implicitly declared anymore - Declare selected classes as final - Add noexcept specifier to constructors - Fixup gmock/gtest warnings --- .github/workflows/ci.yaml | 8 +++++-- .../collision_detection/collision_matrix.h | 3 +++ .../test_collision_common_panda.h | 8 +++---- .../test_collision_common_pr2.h | 6 ++--- .../test_bullet_collision_detection_panda.cpp | 10 +++++++-- .../test_bullet_collision_detection_pr2.cpp | 4 ++-- .../test_fcl_collision_detection_panda.cpp | 18 ++++++++++----- .../test/test_fcl_collision_detection_pr2.cpp | 4 ++-- moveit_core/robot_state/src/robot_state.cpp | 5 +++-- .../time_parameterization.h | 5 +++++ .../detail/GreedyKCenters.h | 5 ++++- .../detail/NearestNeighbors.h | 5 ++++- .../basecmd.h | 9 ++++---- .../goalconstraintsmsgconvertible.h | 6 +++++ .../robotstatemsgconvertible.h | 6 +++++ .../testdata_loader.h | 4 ++++ .../xml_testdata_loader.h | 8 ++++++- .../trajopt_interface/problem_description.h | 5 +++++ .../global_planner/global_planner_interface.h | 6 +++++ .../global_planner/moveit_planning_pipeline.h | 2 +- .../planner_logic_interface.h | 7 ++++++ .../replan_invalidated_trajectory.h | 2 +- .../single_plan_execution.h | 2 +- .../forward_trajectory.h | 2 +- .../local_constraint_solver_interface.h | 6 +++++ .../trajectory_operator_interface.h | 6 +++++ .../simple_sampler.h | 2 +- .../test/hybrid_planning_demo_node.cpp | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 14 ++++++------ .../test/servo_launch_test_common.hpp | 22 +++++++++---------- .../perception_config.hpp | 2 +- .../moveit_setup_controllers/controllers.hpp | 7 ++++++ .../include/moveit_setup_framework/config.hpp | 7 ++++++ .../moveit_setup_framework/generated_file.hpp | 4 ++++ .../moveit_setup_framework/setup_step.hpp | 7 ++++++ 35 files changed, 164 insertions(+), 55 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 470c517158..d6ef85a0d6 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -23,6 +23,10 @@ jobs: ROS_DISTRO: humble IKFAST_TEST: true CLANG_TIDY: pedantic + # Silent gmock/gtest warnings: https://github.com/ament/googletest/pull/21 + AFTER_BUILD_UPSTREAM_WORKSPACE: | + git clone --quiet --branch clalancette/update-to-gtest-1.11 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest" + builder_run_build "/opt/ros/${ROS_DISTRO}" "${BASEDIR}/upstream_ws" --packages-select gtest_vendor gmock_vendor env: CXXFLAGS: >- -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls @@ -40,13 +44,13 @@ jobs: BEFORE_BUILD_UPSTREAM_WORKSPACE: ccache -z AFTER_BUILD_TARGET_WORKSPACE: ccache -s # Changing linker to lld as ld has a behavior where it takes a long time to finish - # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings) + # Compile CCOV with Debug. Enable -Werror. TARGET_CMAKE_ARGS: > -DCMAKE_EXE_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_SHARED_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_MODULE_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} - -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" + -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''" DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 18ea04470c..1192ff87f6 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -96,6 +96,9 @@ class AllowedCollisionMatrix /** @brief Copy constructor */ AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; + /** @brief Copy assignment */ + AllowedCollisionMatrix& operator=(const AllowedCollisionMatrix&) = default; + /** @brief Get the type of the allowed collision between two elements. * Return true if the entry is included in the collision matrix. Return false if the entry is not found. * @param name1 name of first element diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 571ba98081..36567bf365 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -354,9 +354,9 @@ TYPED_TEST_P(DistanceFullPandaTest, DistancePoints) } } -REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, - RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); +REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, + RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); -REGISTER_TYPED_TEST_CASE_P(DistanceCheckPandaTest, DistanceSingle); +REGISTER_TYPED_TEST_SUITE_P(DistanceCheckPandaTest, DistanceSingle); -REGISTER_TYPED_TEST_CASE_P(DistanceFullPandaTest, DistancePoints); +REGISTER_TYPED_TEST_SUITE_P(DistanceFullPandaTest, DistancePoints); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index b3c3f00374..db9ba733d9 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -564,6 +564,6 @@ TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) } } -REGISTER_TYPED_TEST_CASE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, - ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, - TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize); +REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, + ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, + TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize); diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp index 339cd15c3b..1e164b33f7 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp @@ -37,8 +37,14 @@ #include #include -INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheckPanda, CollisionDetectorPandaTest, - collision_detection::CollisionDetectorAllocatorBullet); +INSTANTIATE_TYPED_TEST_SUITE_P(BulletCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorBullet); + +// These are not instantiated, because be don't yet have distance checking for Bullet +#ifdef GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST +GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST(DistanceCheckPandaTest); +GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST(DistanceFullPandaTest); +#endif int main(int argc, char* argv[]) { diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp index e522c6d67e..997d73b689 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp @@ -37,8 +37,8 @@ #include #include -INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheck, CollisionDetectorTest, - collision_detection::CollisionDetectorAllocatorBullet); +INSTANTIATE_TYPED_TEST_SUITE_P(BulletCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorBullet); int main(int argc, char* argv[]) { diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp index d6c810ab88..78aa8732b8 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp @@ -37,19 +37,25 @@ #include #include -INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, - collision_detection::CollisionDetectorAllocatorFCL); +INSTANTIATE_TYPED_TEST_SUITE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); // FCL < 0.6 incorrectly reports distance results in the object coordinate frame. // See: https://github.com/flexible-collision-library/fcl/issues/171 // and https://github.com/flexible-collision-library/fcl/pull/288. // So only execute the full distance test suite on FCL >= 0.6. #if MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0) -INSTANTIATE_TYPED_TEST_CASE_P(FCLDistanceCheckPanda, DistanceFullPandaTest, - collision_detection::CollisionDetectorAllocatorFCL); +INSTANTIATE_TYPED_TEST_SUITE_P(FCLDistanceCheckPanda, DistanceFullPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); +#ifdef GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST +GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST(DistanceCheckPandaTest); +#endif #else -INSTANTIATE_TYPED_TEST_CASE_P(FCLDistanceCheckPanda, DistanceCheckPandaTest, - collision_detection::CollisionDetectorAllocatorFCL); +INSTANTIATE_TYPED_TEST_SUITE_P(FCLDistanceCheckPanda, DistanceCheckPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); +#ifdef GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST +GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST(DistanceFullPandaTest); +#endif #endif int main(int argc, char* argv[]) diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp index efe312cf3b..6818fb13a9 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp @@ -37,8 +37,8 @@ #include #include -INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheck, CollisionDetectorTest, - collision_detection::CollisionDetectorAllocatorFCL); +INSTANTIATE_TYPED_TEST_SUITE_P(FCLCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorFCL); int main(int argc, char* argv[]) { diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 7cd29e3f15..91810f6f0e 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1605,8 +1605,9 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::string error_msg; if (!solver->supportsGroup(jmg, &error_msg)) { - RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(), - jmg->getName().c_str(), error_msg.c_str()); + const kinematics::KinematicsBase& solver_ref = *solver; + RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", + typeid(solver_ref).name(), jmg->getName().c_str(), error_msg.c_str()); valid_solver = false; } } diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h index 4746e11967..98d362dd25 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -11,6 +11,11 @@ MOVEIT_CLASS_FORWARD(TimeParameterization); class TimeParameterization { public: + TimeParameterization() = default; + TimeParameterization(const TimeParameterization&) = default; + TimeParameterization(TimeParameterization&&) = default; + TimeParameterization& operator=(const TimeParameterization&) = default; + TimeParameterization& operator=(TimeParameterization&&) = default; virtual ~TimeParameterization() = default; virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h index b1120c3864..3746696e9a 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h @@ -57,7 +57,10 @@ class GreedyKCenters using Matrix = Eigen::MatrixXd; GreedyKCenters() = default; - + GreedyKCenters(const GreedyKCenters&) = default; + GreedyKCenters(GreedyKCenters&&) noexcept = default; + GreedyKCenters& operator=(const GreedyKCenters&) = default; + GreedyKCenters& operator=(GreedyKCenters&&) noexcept = default; virtual ~GreedyKCenters() = default; /** \brief Set the distance function to use */ diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h index 0cdeb250b5..359dfe5157 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h @@ -52,7 +52,10 @@ class NearestNeighbors typedef std::function DistanceFunction; NearestNeighbors() = default; - + NearestNeighbors(const NearestNeighbors&) = default; + NearestNeighbors(NearestNeighbors&&) noexcept = default; + NearestNeighbors& operator=(const NearestNeighbors&) = default; + NearestNeighbors& operator=(NearestNeighbors&&) noexcept = default; virtual ~NearestNeighbors() = default; /** \brief Set the distance function to use */ diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h index bd8f7cdb23..42166ffb35 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h @@ -44,10 +44,11 @@ template class BaseCmd : public MotionCmd { public: - BaseCmd() : MotionCmd() - { - } - + BaseCmd() = default; + BaseCmd(const BaseCmd&) = default; + BaseCmd(BaseCmd&&) noexcept = default; + BaseCmd& operator=(const BaseCmd&) = default; + BaseCmd& operator=(BaseCmd&&) noexcept = default; virtual ~BaseCmd() = default; public: diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h index 620d2fe468..dd4a03d07d 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h @@ -48,6 +48,12 @@ namespace pilz_industrial_motion_planner_testutils class GoalConstraintMsgConvertible { public: + GoalConstraintMsgConvertible() = default; + GoalConstraintMsgConvertible(const GoalConstraintMsgConvertible&) = default; + GoalConstraintMsgConvertible(GoalConstraintMsgConvertible&&) = default; + GoalConstraintMsgConvertible& operator=(const GoalConstraintMsgConvertible&) = default; + GoalConstraintMsgConvertible& operator=(GoalConstraintMsgConvertible&&) = default; + virtual ~GoalConstraintMsgConvertible() = default; virtual moveit_msgs::msg::Constraints toGoalConstraints() const = 0; }; } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h index 7fb3fe7e87..7e9da57924 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h @@ -46,6 +46,12 @@ namespace pilz_industrial_motion_planner_testutils class RobotStateMsgConvertible { public: + RobotStateMsgConvertible() = default; + RobotStateMsgConvertible(const RobotStateMsgConvertible&) = default; + RobotStateMsgConvertible(RobotStateMsgConvertible&&) = default; + RobotStateMsgConvertible& operator=(const RobotStateMsgConvertible&) = default; + RobotStateMsgConvertible& operator=(RobotStateMsgConvertible&&) = default; + virtual ~RobotStateMsgConvertible() = default; virtual moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const = 0; }; } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h index 19099ba0e7..10ea232571 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h @@ -60,6 +60,10 @@ class TestdataLoader { } + TestdataLoader(const TestdataLoader&) = default; + TestdataLoader(TestdataLoader&&) = default; + TestdataLoader& operator=(const TestdataLoader&) = default; + TestdataLoader& operator=(TestdataLoader&&) = default; virtual ~TestdataLoader() = default; public: diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h index 810f3c468c..c523df53d1 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h @@ -184,8 +184,14 @@ class XmlTestdataLoader : public TestdataLoader class AbstractCmdGetterAdapter { public: - virtual CmdVariant getCmd(const std::string& /*cmd_name*/) const = 0; + AbstractCmdGetterAdapter() = default; + AbstractCmdGetterAdapter(const AbstractCmdGetterAdapter&) = default; + AbstractCmdGetterAdapter(AbstractCmdGetterAdapter&&) = default; + AbstractCmdGetterAdapter& operator=(const AbstractCmdGetterAdapter&) = default; + AbstractCmdGetterAdapter& operator=(AbstractCmdGetterAdapter&&) = default; virtual ~AbstractCmdGetterAdapter() = default; + + virtual CmdVariant getCmd(const std::string& /*cmd_name*/) const = 0; }; private: diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index 3a84346eee..c8a8598856 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -144,6 +144,11 @@ struct TermInfo using MakerFunc = TermInfoPtr (*)(void); static void RegisterMaker(const std::string& type, MakerFunc); + TermInfo() = default; + TermInfo(const TermInfo&) = default; + TermInfo(TermInfo&&) = default; + TermInfo& operator=(const TermInfo&) = default; + TermInfo& operator=(TermInfo&&) = default; virtual ~TermInfo() = default; protected: diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h index 8cef1260c1..7cfcafa96a 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h @@ -55,6 +55,12 @@ namespace moveit::hybrid_planning class GlobalPlannerInterface { public: + GlobalPlannerInterface() = default; + GlobalPlannerInterface(const GlobalPlannerInterface&) = default; + GlobalPlannerInterface(GlobalPlannerInterface&&) = default; + GlobalPlannerInterface& operator=(const GlobalPlannerInterface&) = default; + GlobalPlannerInterface& operator=(GlobalPlannerInterface&&) = default; + virtual ~GlobalPlannerInterface() = default; /** * Initialize global planner * @return True if initialization was successful diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h index 0db65f68b2..a50afbef64 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h @@ -50,7 +50,7 @@ class MoveItPlanningPipeline : public GlobalPlannerInterface { public: MoveItPlanningPipeline() = default; - ~MoveItPlanningPipeline() = default; + ~MoveItPlanningPipeline() override = default; bool initialize(const rclcpp::Node::SharedPtr& node) override; bool reset() noexcept override; moveit_msgs::msg::MotionPlanResponse diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h index d42a3eeae8..971b4858eb 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h @@ -104,6 +104,13 @@ class HybridPlanningManager; // Forward declaration class PlannerLogicInterface { public: + PlannerLogicInterface() = default; + PlannerLogicInterface(const PlannerLogicInterface&) = default; + PlannerLogicInterface(PlannerLogicInterface&&) = default; + PlannerLogicInterface& operator=(const PlannerLogicInterface&) = default; + PlannerLogicInterface& operator=(PlannerLogicInterface&&) = default; + virtual ~PlannerLogicInterface() = default; + /** * Initialize the planner logic * @param hybrid_planning_manager The hybrid planning manager instance to initialize this logic with. diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h index 6ce0f65dc3..7fd25903a8 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h @@ -47,7 +47,7 @@ class ReplanInvalidatedTrajectory : public SinglePlanExecution // Inherit from { public: ReplanInvalidatedTrajectory() = default; - ~ReplanInvalidatedTrajectory() = default; + ~ReplanInvalidatedTrajectory() override = default; ReactionResult react(const std::string& event) override; }; } // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h index 339631ef9d..62b6e713a6 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h @@ -46,7 +46,7 @@ class SinglePlanExecution : public PlannerLogicInterface { public: SinglePlanExecution() = default; - ~SinglePlanExecution() = default; + ~SinglePlanExecution() override = default; bool initialize(const std::shared_ptr& hybrid_planning_manager) override; ReactionResult react(const HybridPlanningEvent& event) override; ReactionResult react(const std::string& event) override; diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h index 4685bb3f08..d63ddda28a 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h @@ -49,7 +49,7 @@ class ForwardTrajectory : public LocalConstraintSolverInterface { public: ForwardTrajectory() = default; - ~ForwardTrajectory() = default; + ~ForwardTrajectory() override = default; bool initialize(const rclcpp::Node::SharedPtr& node, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::string& /* unused */) override; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h index 25ba1fda8d..c3390be622 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h @@ -59,6 +59,12 @@ namespace moveit::hybrid_planning class LocalConstraintSolverInterface { public: + LocalConstraintSolverInterface() = default; + LocalConstraintSolverInterface(const LocalConstraintSolverInterface&) = default; + LocalConstraintSolverInterface(LocalConstraintSolverInterface&&) = default; + LocalConstraintSolverInterface& operator=(const LocalConstraintSolverInterface&) = default; + LocalConstraintSolverInterface& operator=(LocalConstraintSolverInterface&&) = default; + virtual ~LocalConstraintSolverInterface() = default; /** * Initialize local constraint solver * @return True if initialization was successful diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h index 35ca5940c5..f27c1c4650 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h @@ -60,6 +60,12 @@ namespace moveit::hybrid_planning class TrajectoryOperatorInterface { public: + TrajectoryOperatorInterface() = default; + TrajectoryOperatorInterface(const TrajectoryOperatorInterface&) = default; + TrajectoryOperatorInterface(TrajectoryOperatorInterface&&) = default; + TrajectoryOperatorInterface& operator=(const TrajectoryOperatorInterface&) = default; + TrajectoryOperatorInterface& operator=(TrajectoryOperatorInterface&&) = default; + virtual ~TrajectoryOperatorInterface() = default; /** * Initialize trajectory operator * @param node Node handle to access parameters diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h index e462ed3b41..ed73e0221d 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h @@ -47,7 +47,7 @@ class SimpleSampler : public TrajectoryOperatorInterface { public: SimpleSampler() = default; - ~SimpleSampler() = default; + ~SimpleSampler() override = default; bool initialize([[maybe_unused]] const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name) override; diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp index 5829956f75..92edd22f66 100644 --- a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp +++ b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp @@ -265,7 +265,7 @@ class HybridPlanningDemo send_goal_options.feedback_callback = [](const rclcpp_action::ClientGoalHandle::SharedPtr& /*unused*/, const std::shared_ptr& feedback) { - RCLCPP_INFO(LOGGER, feedback->feedback.c_str()); + RCLCPP_INFO_STREAM(LOGGER, feedback->feedback); }; RCLCPP_INFO(LOGGER, "Sending hybrid planning goal"); diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index cf21da4fa8..5e87081646 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -1258,10 +1258,10 @@ void ServoCalcs::twistStampedCB(const geometry_msgs::msg::TwistStamped::ConstSha { const std::lock_guard lock(main_loop_mutex_); latest_twist_stamped_ = msg; - latest_twist_cmd_is_nonzero_ = isNonZero(*latest_twist_stamped_.get()); + latest_twist_cmd_is_nonzero_ = isNonZero(*latest_twist_stamped_); - if (msg.get()->header.stamp != rclcpp::Time(0.)) - latest_twist_command_stamp_ = msg.get()->header.stamp; + if (msg->header.stamp != rclcpp::Time(0.)) + latest_twist_command_stamp_ = msg->header.stamp; // notify that we have a new input new_input_cmd_ = true; @@ -1272,10 +1272,10 @@ void ServoCalcs::jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& m { const std::lock_guard lock(main_loop_mutex_); latest_joint_cmd_ = msg; - latest_joint_cmd_is_nonzero_ = isNonZero(*latest_joint_cmd_.get()); + latest_joint_cmd_is_nonzero_ = isNonZero(*latest_joint_cmd_); - if (msg.get()->header.stamp != rclcpp::Time(0.)) - latest_joint_command_stamp_ = msg.get()->header.stamp; + if (msg->header.stamp != rclcpp::Time(0.)) + latest_joint_command_stamp_ = msg->header.stamp; // notify that we have a new input new_input_cmd_ = true; @@ -1284,7 +1284,7 @@ void ServoCalcs::jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& m void ServoCalcs::collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) { - collision_velocity_scale_ = msg.get()->data; + collision_velocity_scale_ = msg->data; } void ServoCalcs::changeDriftDimensions(const std::shared_ptr& req, diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index 0b62b650be..60dc70863b 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -160,7 +160,7 @@ class ServoFixture : public ::testing::Test // Status sub (we need this to check that we've started / stopped) sub_servo_status_ = node_->create_subscription( resolveServoTopicName(servo_parameters_->status_topic), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Int8::SharedPtr msg) { return statusCB(msg); }); + [this](const std_msgs::msg::Int8::ConstSharedPtr& msg) { return statusCB(msg); }); return true; } @@ -234,7 +234,7 @@ class ServoFixture : public ::testing::Test { sub_collision_scale_ = node_->create_subscription( resolveServoTopicName("~/collision_velocity_scale"), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64::SharedPtr msg) { return collisionScaleCB(msg); }); + [this](const std_msgs::msg::Float64::ConstSharedPtr& msg) { return collisionScaleCB(msg); }); return true; } @@ -244,14 +244,14 @@ class ServoFixture : public ::testing::Test { sub_trajectory_cmd_output_ = node_->create_subscription( resolveServoTopicName(servo_parameters_->command_out_topic), rclcpp::SystemDefaultsQoS(), - [this](const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) { return trajectoryCommandCB(msg); }); + [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCommandCB(msg); }); return true; } else if (command_type == "std_msgs/Float64MultiArray") { sub_array_cmd_output_ = node_->create_subscription( resolveServoTopicName(servo_parameters_->command_out_topic), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) { return arrayCommandCB(msg); }); + [this](const std_msgs::msg::Float64MultiArray::ConstSharedPtr& msg) { return arrayCommandCB(msg); }); return true; } else @@ -269,20 +269,20 @@ class ServoFixture : public ::testing::Test return true; } - void statusCB(const std_msgs::msg::Int8::SharedPtr& msg) + void statusCB(const std_msgs::msg::Int8::ConstSharedPtr& msg) { const std::lock_guard lock(latest_state_mutex_); ++num_status_; - latest_status_ = static_cast(msg.get()->data); + latest_status_ = static_cast(msg->data); if (latest_status_ == status_tracking_code_) status_seen_ = true; } - void collisionScaleCB(const std_msgs::msg::Float64::SharedPtr& msg) + void collisionScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) { const std::lock_guard lock(latest_state_mutex_); ++num_collision_scale_; - latest_collision_scale_ = msg.get()->data; + latest_collision_scale_ = msg->data; } void jointStateCB(const sensor_msgs::msg::JointState::ConstSharedPtr& msg) @@ -292,14 +292,14 @@ class ServoFixture : public ::testing::Test latest_joint_state_ = msg; } - void trajectoryCommandCB(const trajectory_msgs::msg::JointTrajectory::SharedPtr& msg) + void trajectoryCommandCB(const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { const std::lock_guard lock(latest_state_mutex_); ++num_commands_; latest_traj_cmd_ = msg; } - void arrayCommandCB(const std_msgs::msg::Float64MultiArray::SharedPtr& msg) + void arrayCommandCB(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& msg) { const std::lock_guard lock(latest_state_mutex_); ++num_commands_; @@ -488,7 +488,7 @@ class ServoFixture : public ::testing::Test sensor_msgs::msg::JointState::ConstSharedPtr latest_joint_state_; size_t num_commands_; - trajectory_msgs::msg::JointTrajectory::SharedPtr latest_traj_cmd_; + trajectory_msgs::msg::JointTrajectory::ConstSharedPtr latest_traj_cmd_; std_msgs::msg::Float64MultiArray::ConstSharedPtr latest_array_cmd_; bool status_seen_; diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp index e3f64b6a94..947add0c93 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp @@ -72,7 +72,7 @@ class PerceptionConfig : public SetupConfig void setConfig(const SensorParameters& parameters); - class GeneratedSensorConfig : public YamlGeneratedFile + class GeneratedSensorConfig final : public YamlGeneratedFile { public: GeneratedSensorConfig(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp index 7fb608f23e..662dbea5d1 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp @@ -54,6 +54,13 @@ class AdditionalControllerField { } + AdditionalControllerField() = default; + AdditionalControllerField(const AdditionalControllerField&) = default; + AdditionalControllerField(AdditionalControllerField&&) = default; + AdditionalControllerField& operator=(const AdditionalControllerField&) = default; + AdditionalControllerField& operator=(AdditionalControllerField&&) = default; + virtual ~AdditionalControllerField() = default; + /** * @brief Overridable method for changing the default value based on the controller_type */ diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp index aef41b207e..2188b3d6a3 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp @@ -57,6 +57,13 @@ MOVEIT_CLASS_FORWARD(SetupConfig); // Defines SetupConfigPtr, ConstPtr, WeakPtr class SetupConfig { public: + SetupConfig() = default; + SetupConfig(const SetupConfig&) = default; + SetupConfig(SetupConfig&&) = default; + SetupConfig& operator=(const SetupConfig&) = default; + SetupConfig& operator=(SetupConfig&&) = default; + virtual ~SetupConfig() = default; + /** * @brief Called after construction to initialize the step * @param config_data Pointer to all the other configs diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp index 0798d21297..4f3288ead4 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp @@ -66,6 +66,10 @@ MOVEIT_CLASS_FORWARD(GeneratedFile); // Defines GeneratedFilePtr, ConstPtr, Wea class GeneratedFile : public std::enable_shared_from_this { public: + GeneratedFile(const GeneratedFile&) = default; + GeneratedFile(GeneratedFile&&) = default; + virtual ~GeneratedFile() = default; + GeneratedFile(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time) : package_path_(package_path), last_gen_time_(last_gen_time) { diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp index 79e05ca37f..55594359bc 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp @@ -46,6 +46,13 @@ namespace moveit_setup class SetupStep { public: + SetupStep() = default; + SetupStep(const SetupStep&) = default; + SetupStep(SetupStep&&) = default; + SetupStep& operator=(const SetupStep&) = default; + SetupStep& operator=(SetupStep&&) = default; + virtual ~SetupStep() = default; + /** * @brief Called after construction to initialize the step * @param parent_node Shared pointer to the parent node From adab7e6300911922c1af7cb869303c30d7df91a9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 17 Feb 2023 20:41:20 +0100 Subject: [PATCH 04/22] Doxygen tag (backport #1955) (#1958) * Generate Doxygen Tag * Install tagfile in output directory * Fix problematic override for Doxygen linking (cherry picked from commit 752571e9ff027b3137b9720227681ed6b57e42d6) Co-authored-by: Henning Kayser --- Doxyfile | 2 +- .../motion_planning_frame_joints_widget.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Doxyfile b/Doxyfile index 26ee59811d..956785f4de 100644 --- a/Doxyfile +++ b/Doxyfile @@ -2081,7 +2081,7 @@ TAGFILES = # tag file that is based on the input files it reads. See section "Linking to # external documentation" for more information about the usage of tag files. -GENERATE_TAGFILE = +GENERATE_TAGFILE = $(DOXYGEN_OUTPUT_DIRECTORY)/MoveIt.tag # If the ALLEXTERNALS tag is set to YES, all external class will be listed in # the class index. If set to NO, only the inherited external classes will be diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index 679cf7d1a8..ee15f6521c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -184,7 +184,7 @@ private Q_SLOTS: class ProgressBarEditor : public QWidget { Q_OBJECT - Q_PROPERTY(float value READ value WRITE setValue NOTIFY valueChanged USER true) + Q_PROPERTY(float value READ getValue WRITE setValue NOTIFY valueChanged USER true) public: /// Create a progressbar-like slider for editing values in range mix..max @@ -194,7 +194,7 @@ class ProgressBarEditor : public QWidget { value_ = value; } - float value() const + float getValue() const { return value_; } From f467d56a137812cbc6949da5fcac9f1fd30a00e6 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 9 Mar 2023 21:16:06 +0100 Subject: [PATCH 05/22] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20docker/bui?= =?UTF-8?q?ld-push-action=20from=203=20to=204=20(#1913)=20(#1999)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit (cherry picked from commit 0c1cf381fe8a2a6aa492b4708305f33eded343f3) Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/docker.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 5517f2257f..6ab8408a11 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -46,7 +46,7 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v4 with: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} @@ -87,7 +87,7 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v4 with: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} @@ -129,7 +129,7 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v4 with: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} @@ -174,7 +174,7 @@ jobs: - name: "Remove .dockerignore" run: rm .dockerignore # enforce full source context - name: Build and Push - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v4 with: context: . file: .docker/${{ github.job }}/Dockerfile From c29de410aa7ca702881e4c2aa120850cf45310b0 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 9 Mar 2023 21:16:55 +0100 Subject: [PATCH 06/22] Fix ci testing (backport #1994) (#1997) (cherry picked from commit caac9da542857d71d45d8b6d632cab8469847b46) Co-authored-by: Henning Kayser --- .docker/ci-testing/Dockerfile | 56 +++++++++++++++++++++++++++++++---- .github/workflows/docker.yaml | 1 - 2 files changed, 50 insertions(+), 7 deletions(-) diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index c304416645..88ee8eb1ee 100644 --- a/.docker/ci-testing/Dockerfile +++ b/.docker/ci-testing/Dockerfile @@ -2,15 +2,59 @@ # CI image using the ROS testing repository ARG ROS_DISTRO=rolling -FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci +FROM osrf/ros2:testing LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de -# Switch to ros-testing -RUN echo "deb http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2-latest.list +ENV TERM xterm -# Upgrade packages to ros-shadow-fixed and clean apt-cache within one RUN command -RUN apt-get -qq update && \ - apt-get -qq upgrade && \ +# Install ROS 2 base packages and build tools +# We are installing ros--ros-base here to mimic the behavior of the ros:-ros-base images. +# This step is split into a separate layer so that we can rely on cached dependencies instead of having +# to install them with every new build. The testing image and packages will only update every couple weeks. +RUN \ + # Update apt package list as previous containers clear the cache + apt-get -q update && \ + apt-get -q -y upgrade && \ + # + # Install base dependencies + apt-get -q install --no-install-recommends -y \ + # Some basic requirements + wget git sudo curl \ + # Preferred build tools + clang clang-format-14 clang-tidy clang-tools \ + ccache \ + ros-"$ROS_DISTRO"-ros-base && \ + # + # Clear apt-cache to reduce image size + rm -rf /var/lib/apt/lists/* + +# Setup (temporary) ROS workspace +WORKDIR /root/ws_moveit + +# Copy MoveIt sources from docker context +COPY . src/moveit2 + +# Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size +# https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers +RUN \ + # Update apt package list as previous containers clear the cache + apt-get -q update && \ + apt-get -q -y upgrade && \ + # + # Globally disable git security + # https://github.blog/2022-04-12-git-security-vulnerability-announced + git config --global --add safe.directory "*" && \ + # + # Fetch all dependencies from moveit2.repos + vcs import src < src/moveit2/moveit2.repos && \ + if [ -r src/moveit2/moveit2_"$ROS_DISTRO".repos ] ; then vcs import src < src/moveit2/moveit2_"$ROS_DISTRO".repos ; fi && \ + # + # Download all dependencies of MoveIt + rosdep update && \ + DEBIAN_FRONTEND=noninteractive \ + rosdep install -y --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" --as-root=apt:false && \ + # Remove the source code from this container + rm -rf src && \ # # Clear apt-cache to reduce image size rm -rf /var/lib/apt/lists/* diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 6ab8408a11..ff37875017 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -98,7 +98,6 @@ jobs: ${{ env.DH_IMAGE }} ci-testing: - needs: ci strategy: fail-fast: false matrix: From cdb4b9faf492ff436540cc2c25883e9f79fd4733 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 9 Mar 2023 19:30:46 +0000 Subject: [PATCH 07/22] Fix unreachable child logger instance (cherry picked from commit 1323d05c89a8815450f8f4edf7a1d7b520871d18) --- .../planning_request_adapter/src/planning_request_adapter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index 8a456d3d0d..92d904d05b 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -41,7 +41,7 @@ namespace planning_request_adapter { -rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("planning_request_adapter"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.planning_request_adapter"); namespace { From 05f3c6d69ec94feb48ad6ba002c766d329fb2658 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 9 Mar 2023 20:04:22 +0000 Subject: [PATCH 08/22] Fix invalid case style for private member in RobotTrajectory (cherry picked from commit 31e07d3d6a6c1d59bca5876cc0acc51abb960997) --- .../moveit/robot_trajectory/robot_trajectory.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index c46fdaa124..3d3442e496 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -310,19 +310,19 @@ class RobotTrajectory class Iterator { - std::deque::iterator waypoint_iterator; - std::deque::iterator duration_iterator; + std::deque::iterator waypoint_iterator_; + std::deque::iterator duration_iterator_; public: - explicit Iterator(const std::deque::iterator& _waypoint_iterator, - const std::deque::iterator& _duration_iterator) - : waypoint_iterator(_waypoint_iterator), duration_iterator(_duration_iterator) + explicit Iterator(const std::deque::iterator& waypoint_iterator, + const std::deque::iterator& duration_iterator) + : waypoint_iterator_(waypoint_iterator), duration_iterator_(duration_iterator) { } Iterator& operator++() { - waypoint_iterator++; - duration_iterator++; + waypoint_iterator_++; + duration_iterator_++; return *this; } Iterator operator++(int) @@ -333,7 +333,7 @@ class RobotTrajectory } bool operator==(const Iterator& other) const { - return ((waypoint_iterator == other.waypoint_iterator) && (duration_iterator == other.duration_iterator)); + return ((waypoint_iterator_ == other.waypoint_iterator_) && (duration_iterator_ == other.duration_iterator_)); } bool operator!=(const Iterator& other) const { @@ -341,7 +341,7 @@ class RobotTrajectory } std::pair operator*() const { - return std::pair{ *waypoint_iterator, *duration_iterator }; + return std::pair{ *waypoint_iterator_, *duration_iterator_ }; } // iterator traits From 870599a0936fe7a56a15ff0e5965cb82362005b4 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 4 Apr 2023 08:02:52 -0500 Subject: [PATCH 09/22] [Servo] CI simplification (backport #1556) (#1980) --- moveit_ros/moveit_servo/CMakeLists.txt | 37 +- .../include/moveit_servo/pose_tracking.h | 2 +- .../include/moveit_servo/servo_calcs.h | 9 - .../include/moveit_servo/utilities.h | 81 +++ .../cpp_interface_demo/pose_tracking_demo.cpp | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 123 +--- moveit_ros/moveit_servo/src/utilities.cpp | 159 +++++ .../moveit_servo/test/basic_servo_tests.cpp | 165 ------ .../config/servo_settings_low_latency.yaml | 62 -- .../config/singularity_start_positions.yaml | 16 - .../test/launch/test_basic_servo.test.py | 26 - .../launch/test_servo_pose_tracking.test.py | 2 +- .../launch/test_servo_singularity.test.py | 27 - .../test/launch/unit_test_servo_calcs.test.py | 56 -- .../moveit_servo/test/pose_tracking_test.cpp | 2 +- ...s_tests.cpp => servo_calcs_unit_tests.cpp} | 57 +- .../test/servo_launch_test_common.hpp | 4 +- .../test/test_servo_collision.cpp | 4 +- .../test/test_servo_interface.cpp | 4 +- .../test/test_servo_singularity.cpp | 160 ----- .../test/unit_test_servo_calcs.cpp | 552 ------------------ .../test/unit_test_servo_calcs.hpp | 91 --- 22 files changed, 316 insertions(+), 1325 deletions(-) create mode 100644 moveit_ros/moveit_servo/include/moveit_servo/utilities.h create mode 100644 moveit_ros/moveit_servo/src/utilities.cpp delete mode 100644 moveit_ros/moveit_servo/test/basic_servo_tests.cpp delete mode 100644 moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml delete mode 100644 moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml delete mode 100644 moveit_ros/moveit_servo/test/launch/test_basic_servo.test.py delete mode 100644 moveit_ros/moveit_servo/test/launch/test_servo_singularity.test.py delete mode 100644 moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py rename moveit_ros/moveit_servo/test/{enforce_limits_tests.cpp => servo_calcs_unit_tests.cpp} (70%) delete mode 100644 moveit_ros/moveit_servo/test/test_servo_singularity.cpp delete mode 100644 moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp delete mode 100644 moveit_ros/moveit_servo/test/unit_test_servo_calcs.hpp diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 2a907623f5..48621bc8d7 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -77,6 +77,7 @@ add_library(${SERVO_LIB_NAME} SHARED src/enforce_limits.cpp src/servo.cpp src/servo_calcs.cpp + src/utilities.cpp ) set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${SERVO_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -181,16 +182,6 @@ if(BUILD_TESTING) # Run all lint tests in package.xml except those listed above ament_lint_auto_find_test_dependencies() - # Unit test for ServoCalcs - # TODO (vatanaksoytezer): Rewrite this similar to enforce limits test - # ament_add_gtest_executable( - # unit_test_servo_calcs - # test/unit_test_servo_calcs.cpp - # ) - # target_link_libraries(unit_test_servo_calcs ${SERVO_LIB_NAME}) - # ament_target_dependencies(unit_test_servo_calcs ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) - # add_ros_test(test/launch/unit_test_servo_calcs.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # Servo integration launch test ament_add_gtest_executable(test_servo_integration test/test_servo_interface.cpp @@ -209,16 +200,6 @@ if(BUILD_TESTING) ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # TODO(sjahr): Debug and re-enable this test - # Servo singularity checking integration test - #ament_add_gtest_executable(test_servo_singularity - # test/test_servo_singularity.cpp - # test/servo_launch_test_common.hpp - #) - #target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME}) - #ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS}) - #add_ros_test(test/launch/test_servo_singularity.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # pose_tracking ament_add_gtest_executable(test_servo_pose_tracking test/pose_tracking_test.cpp @@ -227,19 +208,11 @@ if(BUILD_TESTING) target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING}) add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # basic_servo_tests - ament_add_gtest_executable(basic_servo_tests - test/basic_servo_tests.cpp - ) - ament_target_dependencies(basic_servo_tests ${THIS_PACKAGE_INCLUDE_DEPENDS}) - target_link_libraries(basic_servo_tests ${POSE_TRACKING}) - add_ros_test(test/launch/test_basic_servo.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # Enforce limits unit tests - ament_add_gtest(enforce_limits_tests - test/enforce_limits_tests.cpp + # Unit tests + ament_add_gtest(servo_calcs_unit_tests + test/servo_calcs_unit_tests.cpp ) - target_link_libraries(enforce_limits_tests ${SERVO_LIB_NAME}) + target_link_libraries(servo_calcs_unit_tests ${SERVO_LIB_NAME}) endif() diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h index 10a4f60451..51c7c19eb6 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -159,7 +159,7 @@ class PoseTracking // Joint group used for controlling the motions std::string move_group_name_; - rclcpp::Rate loop_rate_; + rclcpp::WallRate loop_rate_; // ROS interface to Servo rclcpp::Publisher::SharedPtr twist_stamped_pub_; 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 632fb17ed6..15d4a3eb32 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -58,7 +58,6 @@ #include #include #include -#include #include // moveit_core @@ -173,13 +172,6 @@ class ServoCalcs */ std::vector enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) const; - /** \brief Possibly calculate a velocity scaling factor, due to proximity of - * singularity and direction of motion - */ - double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse); - /** \brief Compose the outgoing JointTrajectory message */ void composeJointTrajMessage(const sensor_msgs::msg::JointState& joint_state, trajectory_msgs::msg::JointTrajectory& joint_trajectory); @@ -314,7 +306,6 @@ class ServoCalcs rclcpp::Publisher::SharedPtr status_pub_; rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub_; rclcpp::Publisher::SharedPtr multiarray_outgoing_cmd_pub_; - rclcpp::Publisher::SharedPtr condition_pub_; rclcpp::Service::SharedPtr control_dimensions_server_; rclcpp::Service::SharedPtr drift_dimensions_server_; rclcpp::Service::SharedPtr reset_servo_status_; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utilities.h b/moveit_ros/moveit_servo/include/moveit_servo/utilities.h new file mode 100644 index 0000000000..02aadea30f --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utilities.h @@ -0,0 +1,81 @@ +// Copyright 2022 PickNik Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the PickNik Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* Author : Andy Zelenak + Desc : Free functions. We keep them in a separate translation unit to reduce .o filesize + Title : utilities.h + Project : moveit_servo +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace moveit_servo +{ +// Helper function for detecting zeroed message +bool isNonZero(const geometry_msgs::msg::TwistStamped& msg); + +// Helper function for detecting zeroed message +bool isNonZero(const control_msgs::msg::JointJog& msg); + +// Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped +geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame); + +/** \brief Possibly calculate a velocity scaling factor, due to proximity of + * singularity and direction of motion + * @param[in] joint_model_group The MoveIt group + * @param[in] commanded_twist The commanded Cartesian twist + * @param[in] svd A singular value decomposition of the Jacobian + * @param[in] pseudo_inverse The pseudo-inverse of the Jacobian + * @param[in] hard_stop_singularity_threshold Halt if condition(Jacobian) > hard_stop_singularity_threshold + * @param[in] lower_singularity_threshold Decelerate if condition(Jacobian) > lower_singularity_threshold + * @param[in] leaving_singularity_threshold_multiplier Allow faster motion away from singularity + * @param[in, out] clock A ROS clock, for logging + * @param[in, out] current_state The state of the robot. Used in internal calculations. + * @param[out] status Singularity status + */ +double velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, + const Eigen::VectorXd& commanded_twist, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse, + const double hard_stop_singularity_threshold, + const double lower_singularity_threshold, + const double leaving_singularity_threshold_multiplier, rclcpp::Clock& clock, + moveit::core::RobotStatePtr& current_state, StatusCode& status); + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp b/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp index 2e8e628c1f..02b2d47b26 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp @@ -168,7 +168,7 @@ int main(int argc, char** argv) << moveit_servo::POSE_TRACKING_STATUS_CODE_MAP.at(tracking_status)); }); - rclcpp::Rate loop_rate(50); + rclcpp::WallRate loop_rate(50); for (size_t i = 0; i < 500; ++i) { // Modify the pose target a little bit each cycle diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 5e87081646..036c37b8f8 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -46,50 +46,19 @@ #include // #include // TODO(adamp): create an issue about this -#include #include +#include +#include using namespace std::chrono_literals; // for s, ms, etc. -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_calcs"); -constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); -static constexpr double STOPPED_VELOCITY_EPS = 1e-4; // rad/s - namespace moveit_servo { namespace { -constexpr char CONDITION_TOPIC[] = "~/condition"; - -// Helper function for detecting zeroed message -bool isNonZero(const geometry_msgs::msg::TwistStamped& msg) -{ - return msg.twist.linear.x != 0.0 || msg.twist.linear.y != 0.0 || msg.twist.linear.z != 0.0 || - msg.twist.angular.x != 0.0 || msg.twist.angular.y != 0.0 || msg.twist.angular.z != 0.0; -} - -// Helper function for detecting zeroed message -bool isNonZero(const control_msgs::msg::JointJog& msg) -{ - bool all_zeros = true; - for (double delta : msg.velocities) - { - all_zeros &= (delta == 0.0); - }; - return !all_zeros; -} - -// Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped -geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, - const std::string& parent_frame, - const std::string& child_frame) -{ - geometry_msgs::msg::TransformStamped output = tf2::eigenToTransform(eigen_tf); - output.header.frame_id = parent_frame; - output.child_frame_id = child_frame; - - return output; -} +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_calcs"); +constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); +static constexpr double STOPPED_VELOCITY_EPS = 1e-4; // rad/s } // namespace // Constructor for the class that handles servoing calculations @@ -175,7 +144,6 @@ ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node, // Publish status status_pub_ = node_->create_publisher(parameters_->status_topic, rclcpp::SystemDefaultsQoS()); - condition_pub_ = node_->create_publisher(CONDITION_TOPIC, rclcpp::SystemDefaultsQoS()); internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); num_joints_ = internal_joint_state_.name.size(); @@ -674,7 +642,11 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd, delta_theta_ = pseudo_inverse * delta_x; } - delta_theta_ *= velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse); + delta_theta_ *= velocityScalingFactorForSingularity(joint_model_group_, delta_x, svd, pseudo_inverse, + parameters_->hard_stop_singularity_threshold, + parameters_->lower_singularity_threshold, + parameters_->leaving_singularity_threshold_multiplier, + *node_->get_clock(), current_state_, status_); return internalServoUpdate(delta_theta_, joint_trajectory, ServoType::CARTESIAN_SPACE); } @@ -842,81 +814,6 @@ void ServoCalcs::composeJointTrajMessage(const sensor_msgs::msg::JointState& joi joint_trajectory.points.push_back(point); } -// Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion -double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse) -{ - double velocity_scale = 1; - std::size_t num_dimensions = commanded_velocity.size(); - - // Find the direction away from nearest singularity. - // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. - // The sign can flip at any time, so we have to do some extra checking. - // Look ahead to see if the Jacobian's condition will decrease. - Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); - - double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); - - auto condition_msg = std::make_unique(); - condition_msg->data = ini_condition; - condition_pub_->publish(std::move(condition_msg)); - - // This singular vector tends to flip direction unpredictably. See R. Bro, - // "Resolving the Sign Ambiguity in the Singular Value Decomposition". - // Look ahead to see if the Jacobian's condition will decrease in this - // direction. Start with a scaled version of the singular vector - Eigen::VectorXd delta_x(num_dimensions); - double scale = 100; - delta_x = vector_toward_singularity / scale; - - // Calculate a small change in joints - Eigen::VectorXd new_theta; - current_state_->copyJointGroupPositions(joint_model_group_, new_theta); - new_theta += pseudo_inverse * delta_x; - current_state_->setJointGroupPositions(joint_model_group_, new_theta); - Eigen::MatrixXd new_jacobian = current_state_->getJacobian(joint_model_group_); - - Eigen::JacobiSVD new_svd(new_jacobian); - double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); - // If new_condition < ini_condition, the singular vector does point towards a - // singularity. Otherwise, flip its direction. - if (ini_condition >= new_condition) - { - vector_toward_singularity *= -1; - } - - // If this dot product is positive, we're moving toward singularity - double dot = vector_toward_singularity.dot(commanded_velocity); - // see https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation of algorithm - double upper_threshold = - dot > 0 ? parameters_->hard_stop_singularity_threshold : - (parameters_->hard_stop_singularity_threshold - parameters_->lower_singularity_threshold) * - parameters_->leaving_singularity_threshold_multiplier + - parameters_->lower_singularity_threshold; - if ((ini_condition > parameters_->lower_singularity_threshold) && - (ini_condition < parameters_->hard_stop_singularity_threshold)) - { - velocity_scale = 1. - (ini_condition - parameters_->lower_singularity_threshold) / - (upper_threshold - parameters_->lower_singularity_threshold); - status_ = - dot > 0 ? StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY : StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY; - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status_)); - } - - // Very close to singularity, so halt. - else if (ini_condition >= upper_threshold) - { - velocity_scale = 0; - status_ = StatusCode::HALT_FOR_SINGULARITY; - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status_)); - } - - return velocity_scale; -} - std::vector ServoCalcs::enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) const { diff --git a/moveit_ros/moveit_servo/src/utilities.cpp b/moveit_ros/moveit_servo/src/utilities.cpp new file mode 100644 index 0000000000..e92f5933f9 --- /dev/null +++ b/moveit_ros/moveit_servo/src/utilities.cpp @@ -0,0 +1,159 @@ +// Copyright 2022 PickNik Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the PickNik Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* Author : Andy Zelenak + Desc : Free functions. We keep them in a separate translation unit to reduce .o filesize + Title : utilities.cpp + Project : moveit_servo +*/ + +#include + +namespace moveit_servo +{ +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.utilities"); +constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); +} // namespace + +/** \brief Helper function for detecting zeroed message **/ +bool isNonZero(const geometry_msgs::msg::TwistStamped& msg) +{ + return msg.twist.linear.x != 0.0 || msg.twist.linear.y != 0.0 || msg.twist.linear.z != 0.0 || + msg.twist.angular.x != 0.0 || msg.twist.angular.y != 0.0 || msg.twist.angular.z != 0.0; +} + +/** \brief Helper function for detecting zeroed message **/ +bool isNonZero(const control_msgs::msg::JointJog& msg) +{ + bool all_zeros = true; + for (double delta : msg.velocities) + { + all_zeros &= (delta == 0.0); + } + return !all_zeros; +} + +/** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/ +geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame) +{ + geometry_msgs::msg::TransformStamped output = tf2::eigenToTransform(eigen_tf); + output.header.frame_id = parent_frame; + output.child_frame_id = child_frame; + + return output; +} + +/** \brief Possibly calculate a velocity scaling factor, due to proximity of + * singularity and direction of motion + * @param[in] joint_model_group The MoveIt group + * @param[in] commanded_twist The commanded Cartesian twist + * @param[in] svd A singular value decomposition of the Jacobian + * @param[in] pseudo_inverse The pseudo-inverse of the Jacobian + * @param[in] hard_stop_singularity_threshold Halt if condition(Jacobian) > hard_stop_singularity_threshold + * @param[in] lower_singularity_threshold Decelerate if condition(Jacobian) > lower_singularity_threshold + * @param[in] leaving_singularity_threshold_multiplier Allow faster motion away from singularity + * @param[in, out] clock A ROS clock, for logging + * @param[in, out] current_state The state of the robot. Used in internal calculations. + * @param[out] status Singularity status + */ +double velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, + const Eigen::VectorXd& commanded_twist, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse, + const double hard_stop_singularity_threshold, + const double lower_singularity_threshold, + const double leaving_singularity_threshold_multiplier, rclcpp::Clock& clock, + moveit::core::RobotStatePtr& current_state, StatusCode& status) +{ + double velocity_scale = 1; + std::size_t num_dimensions = commanded_twist.size(); + + // Find the direction away from nearest singularity. + // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. + // The sign can flip at any time, so we have to do some extra checking. + // Look ahead to see if the Jacobian's condition will decrease. + Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); + + double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + + // This singular vector tends to flip direction unpredictably. See R. Bro, + // "Resolving the Sign Ambiguity in the Singular Value Decomposition". + // Look ahead to see if the Jacobian's condition will decrease in this + // direction. Start with a scaled version of the singular vector + Eigen::VectorXd delta_x(num_dimensions); + double scale = 100; + delta_x = vector_toward_singularity / scale; + + // Calculate a small change in joints + Eigen::VectorXd new_theta; + current_state->copyJointGroupPositions(joint_model_group, new_theta); + new_theta += pseudo_inverse * delta_x; + current_state->setJointGroupPositions(joint_model_group, new_theta); + Eigen::MatrixXd new_jacobian = current_state->getJacobian(joint_model_group); + + Eigen::JacobiSVD new_svd(new_jacobian); + double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); + // If new_condition < ini_condition, the singular vector does point towards a + // singularity. Otherwise, flip its direction. + if (ini_condition >= new_condition) + { + vector_toward_singularity *= -1; + } + + // If this dot product is positive, we're moving toward singularity + double dot = vector_toward_singularity.dot(commanded_twist); + // see https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation of algorithm + double upper_threshold = dot > 0 ? hard_stop_singularity_threshold : + (hard_stop_singularity_threshold - lower_singularity_threshold) * + leaving_singularity_threshold_multiplier + + lower_singularity_threshold; + if ((ini_condition > lower_singularity_threshold) && (ini_condition < hard_stop_singularity_threshold)) + { + velocity_scale = + 1. - (ini_condition - lower_singularity_threshold) / (upper_threshold - lower_singularity_threshold); + status = + dot > 0 ? StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY : StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY; + RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status)); + } + + // Very close to singularity, so halt. + else if (ini_condition >= upper_threshold) + { + velocity_scale = 0; + status = StatusCode::HALT_FOR_SINGULARITY; + RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status)); + } + + return velocity_scale; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/basic_servo_tests.cpp b/moveit_ros/moveit_servo/test/basic_servo_tests.cpp deleted file mode 100644 index db823c7ef2..0000000000 --- a/moveit_ros/moveit_servo/test/basic_servo_tests.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Tyler Weaver, Andy Zelenak - Desc: Basic Functionality tests -*/ - -// System -#include - -// ROS -#include - -// Testing -#include - -// Servo -#include -#include -#include "servo_launch_test_common.hpp" - -namespace moveit_servo -{ -TEST_F(ServoFixture, SendTwistStampedTest) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - auto parameters = servo_parameters_; - - // count trajectory messages sent by servo - size_t received_count = 0; - std::function traj_callback = - [&received_count](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr /* unused */) { ++received_count; }; - auto traj_sub = node_->create_subscription( - resolveServoTopicName(parameters->command_out_topic), 1, traj_callback); - - // Create publisher to send servo commands - auto twist_stamped_pub = node_->create_publisher( - resolveServoTopicName(parameters->cartesian_command_in_topic), 1); - - constexpr double test_duration = 1.0; - const double publish_period = parameters->publish_period; - const size_t num_commands = static_cast(test_duration / publish_period); - - // Set the rate differently from the publish period from the parameters to show that - // the number of outputs is set by the number of commands sent and not the rate they are sent. - rclcpp::Rate publish_rate(2. / publish_period); - - // Send a few Cartesian velocity commands - for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) - { - geometry_msgs::msg::TwistStamped msg; - msg.header.stamp = node_->now(); - msg.header.frame_id = "panda_link0"; - msg.twist.angular.y = 1.0; - - // Send the message - twist_stamped_pub->publish(msg); - publish_rate.sleep(); - } - - EXPECT_GT(received_count, num_commands - 20); - EXPECT_GT(received_count, (unsigned)0); - EXPECT_LT(received_count, num_commands + 20); -} - -TEST_F(ServoFixture, SendJointServoTest) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - auto parameters = servo_parameters_; - auto cmd_out_topic = resolveServoTopicName(parameters->command_out_topic); - auto cmd_in_topic = resolveServoTopicName(parameters->joint_command_in_topic); - RCLCPP_INFO_STREAM(LOGGER, "\nOut: " << cmd_out_topic << " In: " << cmd_in_topic << "\n"); - - // count trajectory messages sent by servo - size_t received_count = 0; - std::function traj_callback = - [&received_count](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr /* unused */) { ++received_count; }; - auto traj_sub = node_->create_subscription(cmd_out_topic, 1, traj_callback); - - // Create publisher to send servo commands - auto joint_servo_pub = node_->create_publisher(cmd_in_topic, 1); - - constexpr double test_duration = 1.0; - const double publish_period = parameters->publish_period; - const size_t num_commands = static_cast(test_duration / publish_period); - - // Set the rate differently from the publish period from the parameters to show that - // the number of outputs is set by the number of commands sent and not the rate they are sent. - rclcpp::Rate publish_rate(2. / publish_period); - - // Send a few joint velocity commands - for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) - { - control_msgs::msg::JointJog msg; - msg.header.stamp = node_->now(); - msg.header.frame_id = "panda_link3"; - msg.velocities.push_back(0.1); - - // Send the message - joint_servo_pub->publish(msg); - publish_rate.sleep(); - } - - EXPECT_GT(received_count, num_commands - 20); - EXPECT_GT(received_count, (unsigned)0); - EXPECT_LT(received_count, num_commands + 20); -} -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml deleted file mode 100644 index 7a6722da59..0000000000 --- a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml +++ /dev/null @@ -1,62 +0,0 @@ -############################################### -# Modify all parameters related to servoing here -############################################### -use_gazebo: false # Whether the robot is started in a Gazebo simulation environment - -## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. - joint: 0.01 - -## Properties of outgoing commands -low_latency_mode: true # Set this to true to tie the output rate to the input rate -publish_period: 0.01 # 1/Nominal publish rate [seconds] - -# What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) -# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) -command_out_type: trajectory_msgs/JointTrajectory - -# What to publish? Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: false -publish_joint_accelerations: false - -## Plugins for smoothing outgoing commands -smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" - -## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' -planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' - -## Other frames -ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 1.0 # Stop servoing if X seconds elapse without a new command -# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. -# Important because ROS may drop some messages and we need the robot to halt reliably. -num_outgoing_halt_msgs_to_publish: 1 - -## Configure handling of singularities and joint limits -lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 90.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. - -## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: /joint_states -status_topic: ~/status # Publish status to this topic -command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here - -## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. -self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] -scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] diff --git a/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml b/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml deleted file mode 100644 index 4199bc3dee..0000000000 --- a/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml +++ /dev/null @@ -1,16 +0,0 @@ -initial_positions: - panda_joint1: 0.221 - panda_joint2: 0.530 - panda_joint3: -0.231 - panda_joint4: -0.920 - panda_joint5: 0.117 - panda_joint6: 1.439 - panda_joint7: -1.286 -initial_velocities: - panda_joint1: 0.0 - panda_joint2: 0.0 - panda_joint3: 0.0 - panda_joint4: 0.0 - panda_joint5: 0.0 - panda_joint6: 0.0 - panda_joint7: 0.0 diff --git a/moveit_ros/moveit_servo/test/launch/test_basic_servo.test.py b/moveit_ros/moveit_servo/test/launch/test_basic_servo.test.py deleted file mode 100644 index d0a3a0a52b..0000000000 --- a/moveit_ros/moveit_servo/test/launch/test_basic_servo.test.py +++ /dev/null @@ -1,26 +0,0 @@ -import launch_testing -import os -import sys -import unittest - -sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import generate_servo_test_description - - -def generate_test_description(): - return generate_servo_test_description( - gtest_name="basic_servo_tests", - ) - - -class TestGTestWaitForCompletion(unittest.TestCase): - # Waits for test to complete, then waits a bit to make sure result files are generated - def test_gtest_run_complete(self, servo_gtest): - self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestGTestProcessPostShutdown(unittest.TestCase): - # Checks if the test has been completed with acceptable exit codes (successful codes) - def test_gtest_pass(self, proc_info, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index 0a25d8cc69..0adb0da899 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -11,8 +11,8 @@ from launch.actions import ExecuteProcess, TimerAction from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from moveit_configs_utils import MoveItConfigsBuilder from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_singularity.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_singularity.test.py deleted file mode 100644 index 9db1daa5c7..0000000000 --- a/moveit_ros/moveit_servo/test/launch/test_servo_singularity.test.py +++ /dev/null @@ -1,27 +0,0 @@ -import launch_testing -import os -import sys -import unittest - -sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import generate_servo_test_description - - -def generate_test_description(): - return generate_servo_test_description( - gtest_name="test_servo_singularity", - start_position_path="../config/singularity_start_positions.yaml", - ) - - -class TestGTestWaitForCompletion(unittest.TestCase): - # Waits for test to complete, then waits a bit to make sure result files are generated - def test_gtest_run_complete(self, servo_gtest): - self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestGTestProcessPostShutdown(unittest.TestCase): - # Checks if the test has been completed with acceptable exit codes (successful codes) - def test_gtest_pass(self, proc_info, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py b/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py deleted file mode 100644 index f79bbb061d..0000000000 --- a/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py +++ /dev/null @@ -1,56 +0,0 @@ -import launch -import launch_ros -import launch_testing -import os -import sys -import unittest -from launch_param_builder import ParameterBuilder - -sys.path.append(os.path.dirname(__file__)) - - -def generate_test_description(): - # Get parameters using the demo config file - servo_params = ( - ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config.yaml", parameter_namespace="moveit_servo") - .to_dict() - ) - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - - test_binary_dir_arg = launch.actions.DeclareLaunchArgument( - name="test_binary_dir", - description="Binary directory of package " "containing test executables", - ) - - servo_gtest = launch_ros.actions.Node( - executable=launch.substitutions.PathJoinSubstitution( - [ - launch.substitutions.LaunchConfiguration("test_binary_dir"), - "unit_test_servo_calcs", - ] - ), - parameters=[ - servo_params, - moveit_config.robot_description_kinematics, - ], - output="screen", - # prefix="kitty gdb -e run --args" - ) - - return ( - launch.LaunchDescription( - [test_binary_dir_arg, servo_gtest, launch_testing.actions.ReadyToTest()] - ), - {"servo_gtest": servo_gtest}, - ) - - -@launch_testing.post_shutdown_test() -class TestGTestProcessPostShutdown(unittest.TestCase): - def test_gtest_pass(self, proc_info, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp index 5a456394ac..aad2dc0761 100644 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -196,7 +196,7 @@ TEST_F(PoseTrackingFixture, OutgoingMsgTest) // Republish the target pose in a new thread, as if the target is moving std::thread target_pub_thread([&] { size_t msg_count = 0; - rclcpp::Rate loop_rate(50); + rclcpp::WallRate loop_rate(50); while (++msg_count < 100) { target_pose_pub_->publish(target_pose); diff --git a/moveit_ros/moveit_servo/test/enforce_limits_tests.cpp b/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp similarity index 70% rename from moveit_ros/moveit_servo/test/enforce_limits_tests.cpp rename to moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp index 12f79421a0..d88c926adb 100644 --- a/moveit_ros/moveit_servo/test/enforce_limits_tests.cpp +++ b/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp @@ -33,12 +33,18 @@ *********************************************************************/ /* Author: Tyler Weaver, Andy Zelenak - Desc: Enforce limits unit tests + Desc: Unit tests */ -#include #include + +#include + #include +#include +#include +#include +#include namespace { @@ -60,7 +66,7 @@ void checkVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, } } -class EnforceLimitsTests : public testing::Test +class ServoCalcsUnitTests : public testing::Test { protected: void SetUp() override @@ -75,7 +81,7 @@ class EnforceLimitsTests : public testing::Test } // namespace -TEST_F(EnforceLimitsTests, VelocityScalingTest) +TEST_F(ServoCalcsUnitTests, VelocitiesTooFast) { // Request velocities that are too fast std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; @@ -91,7 +97,7 @@ TEST_F(EnforceLimitsTests, VelocityScalingTest) checkVelocityLimits(joint_model_group_, eigen_velocity); } -TEST_F(EnforceLimitsTests, NegativeJointAngleDeltasTest) +TEST_F(ServoCalcsUnitTests, NegativeVelocitiesTooFast) { // Negative velocities exceeding the limit std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; @@ -107,7 +113,7 @@ TEST_F(EnforceLimitsTests, NegativeJointAngleDeltasTest) checkVelocityLimits(joint_model_group_, eigen_velocity); } -TEST_F(EnforceLimitsTests, LowJointVelocityDeltaTest) +TEST_F(ServoCalcsUnitTests, AcceptableJointVelocities) { // Final test with joint velocities that are acceptable std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; @@ -123,6 +129,45 @@ TEST_F(EnforceLimitsTests, LowJointVelocityDeltaTest) checkVelocityLimits(joint_model_group_, eigen_velocity); } +TEST_F(ServoCalcsUnitTests, SingularityScaling) +{ + // If we are at a singularity, we should halt + Eigen::VectorXd commanded_twist(6); + commanded_twist << 1, 0, 0, 0, 0, 0; + + // Start near a singularity + std::shared_ptr robot_state = std::make_shared(robot_model_); + robot_state->setToDefaultValues(); + robot_state->setVariablePosition("panda_joint1", 0.221); + robot_state->setVariablePosition("panda_joint2", 0.530); + robot_state->setVariablePosition("panda_joint3", -0.231); + robot_state->setVariablePosition("panda_joint4", -0.920); + robot_state->setVariablePosition("panda_joint5", 0.117); + robot_state->setVariablePosition("panda_joint6", 1.439); + robot_state->setVariablePosition("panda_joint7", -1.286); + + Eigen::MatrixXd jacobian = robot_state->getJacobian(joint_model_group_); + + Eigen::JacobiSVD svd = + Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); + Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); + + // Use very low thresholds to ensure they are triggered + double hard_stop_singularity_threshold = 2; + double lower_singularity_threshold = 1; + double leaving_singularity_threshold_multiplier = 2; + + rclcpp::Clock clock; + moveit_servo::StatusCode status; + + double scaling_factor = moveit_servo::velocityScalingFactorForSingularity( + joint_model_group_, commanded_twist, svd, pseudo_inverse, hard_stop_singularity_threshold, + lower_singularity_threshold, leaving_singularity_threshold_multiplier, clock, robot_state, status); + + EXPECT_EQ(scaling_factor, 0); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index 60dc70863b..8214bf5372 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -94,7 +94,7 @@ class ServoFixture : public ::testing::Test auto test_parameters = std::make_shared(); test_parameters->publish_hz = 2.0 / servo_parameters_->incoming_command_timeout; test_parameters->publish_period = 1.0 / test_parameters->publish_hz; - test_parameters->timeout_iterations = 10 * test_parameters->publish_hz; + test_parameters->timeout_iterations = 50 * test_parameters->publish_hz; test_parameters->servo_node_name = "/servo_node"; test_parameters_ = test_parameters; } @@ -409,7 +409,7 @@ class ServoFixture : public ::testing::Test RCLCPP_INFO_STREAM(LOGGER, "Wait for start servo: " << (node_->now() - time_start).seconds()); // Test that status messages start - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); + rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); time_start = node_->now(); auto num_statuses_start = getNumStatus(); size_t iterations = 0; diff --git a/moveit_ros/moveit_servo/test/test_servo_collision.cpp b/moveit_ros/moveit_servo/test/test_servo_collision.cpp index 7cb4eeff4d..1bb13065c0 100644 --- a/moveit_ros/moveit_servo/test/test_servo_collision.cpp +++ b/moveit_ros/moveit_servo/test/test_servo_collision.cpp @@ -58,7 +58,7 @@ TEST_F(ServoFixture, SelfCollision) watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_COLLISION); // Publish some joint jog commands that will bring us to collision - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); + rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); log_time_start = node_->now(); size_t iterations = 0; while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) @@ -123,7 +123,7 @@ TEST_F(ServoFixture, ExternalCollision) watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_COLLISION); // Now publish twist commands that collide with the box - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); + rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); log_time_start = node_->now(); size_t iterations = 0; while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) diff --git a/moveit_ros/moveit_servo/test/test_servo_interface.cpp b/moveit_ros/moveit_servo/test/test_servo_interface.cpp index a1e2e49940..3304a7b5fb 100644 --- a/moveit_ros/moveit_servo/test/test_servo_interface.cpp +++ b/moveit_ros/moveit_servo/test/test_servo_interface.cpp @@ -58,7 +58,7 @@ TEST_F(ServoFixture, SendTwistStampedTest) auto time_start = node_->now(); // Publish N messages with some time between, ensure it's less than the timeout for Servo - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); + rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); size_t num_commands = 30; resetNumCommands(); for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) @@ -99,7 +99,7 @@ TEST_F(ServoFixture, SendJointServoTest) auto time_start = node_->now(); // Publish N messages with some time between, ensure it's less than the timeout for Servo - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); + rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); size_t num_commands = 30; resetNumCommands(); for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) diff --git a/moveit_ros/moveit_servo/test/test_servo_singularity.cpp b/moveit_ros/moveit_servo/test/test_servo_singularity.cpp deleted file mode 100644 index 3332f4248d..0000000000 --- a/moveit_ros/moveit_servo/test/test_servo_singularity.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : test_servo_singularity.cpp - * Project : moveit_servo - * Created : 08/05/2020 - * Author : Adam Pettinger - */ - -#include "servo_launch_test_common.hpp" -#include - -namespace moveit_servo -{ -TEST_F(ServoFixture, ReachSingular) -{ - ASSERT_TRUE(setupStartClient()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // Publish some twist commands that will move us away from singularity - // Look for DECELERATE_FOR_LEAVING_SINGULARITY status - watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); - - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); - auto log_time_start = node_->now(); - size_t iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = -0.1; - msg->twist.angular.z = -0.5; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - // Test that we didn't timeout - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, - "Wait for DECELERATE_FOR_LEAVING_SINGULARITY: " << (log_time_end - log_time_start).seconds()); - - // Look for NO_WARNING status - watchForStatus(moveit_servo::StatusCode::NO_WARNING); - resetNumStatus(); - - // If we continue moving this direction, we should leave the singularity warning region - log_time_start = node_->now(); - iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = -0.1; - msg->twist.angular.z = -0.5; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - // Test that we didn't timeout - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for NO_WARNING: " << (log_time_end - log_time_start).seconds()); - - // Move away a couple more times - iterations = 0; - while (iterations++ < 2) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - // Publish some twist commands that will bring us to singularity - // Look for DECELERATE_FOR_APPROACHING_SINGULARITY status - watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); - resetNumStatus(); - - log_time_start = node_->now(); - iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = 0.8; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - // Test that we didn't timeout - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, - "Wait for DECELERATE_FOR_APPROACHING_SINGULARITY: " << (log_time_end - log_time_start).seconds()); - - // Continue moving towards singularity until we are halted - // Look for HALT_FOR_SINGULARITY status - watchForStatus(moveit_servo::StatusCode::HALT_FOR_SINGULARITY); - resetNumStatus(); - - log_time_start = node_->now(); - iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = 0.8; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - // Test that we didn't timeout - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for HALT_FOR_SINGULARITY: " << (log_time_end - log_time_start).seconds()); -} - -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp b/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp deleted file mode 100644 index 06a55673c4..0000000000 --- a/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp +++ /dev/null @@ -1,552 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : unit_test_servo_calcs.cpp - * Project : moveit_servo - * Created : 07/24/2020 - * Author : Adam Pettinger - * Desc : Loads a Servo instance without launching or parameters - */ - -#include -#include - -#include -#include -#include -#include -#include -#include "unit_test_servo_calcs.hpp" - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.unit_test_servo_calcs.cpp"); - -void loadModelFile(std::string package_name, std::string filename, std::string& file_content) -{ - std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name)); - std::string xml_string; - std::fstream xml_file((res_path / filename).string().c_str(), std::fstream::in); - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - file_content = xml_string; -} - -FriendServoCalcs::FriendServoCalcs(const rclcpp::Node::SharedPtr& node, - const std::shared_ptr& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : ServoCalcs(node, parameters, planning_scene_monitor) -{ -} - -ServoCalcsTestFixture::ServoCalcsTestFixture() : node_(TEST_NODE) -{ - servo_calcs_ = std::make_unique(node_, TEST_PARAMS, TEST_PSM); - servo_calcs_->start(); -} - -sensor_msgs::msg::JointState ServoCalcsTestFixture::getJointState(std::vector pos, std::vector vel) -{ - sensor_msgs::msg::JointState msg; - std::vector effort; - effort.assign(9, 0); - - msg.name = PANDA_JOINT_NAMES; - msg.position = pos; - msg.velocity = vel; - msg.effort = effort; - msg.header.stamp = node_->now(); - - return msg; -} - -TEST_F(ServoCalcsTestFixture, TestRemoveSingleDimension) -{ - int rows = 6; - int cols = 7; - - // Create a matrix and vector, set one value so we can track the removal was correct - Eigen::MatrixXd matrix(rows, cols); - matrix.setZero(); - matrix(2, 0) = 4.0; - Eigen::VectorXd vector(rows); - vector.setZero(); - vector(2) = 4.0; - - // Remove the second row - servo_calcs_->removeDimension(matrix, vector, 1); - - // Size should be reduced, and the set values should have moved up - EXPECT_EQ(matrix.rows(), rows - 1); - EXPECT_EQ(vector.size(), rows - 1); - EXPECT_EQ(matrix(1, 0), 4.0); - EXPECT_EQ(vector(1), 4.0); - - // Remove the last row - servo_calcs_->removeDimension(matrix, vector, 4); - - // Size should be reduced, but the set values should not move - EXPECT_EQ(matrix.rows(), rows - 2); - EXPECT_EQ(vector.size(), rows - 2); - EXPECT_EQ(matrix(1, 0), 4.0); - EXPECT_EQ(vector(1), 4.0); - - // Sanity check that the columns stayed the same - EXPECT_EQ(matrix.cols(), cols); -} - -TEST_F(ServoCalcsTestFixture, TestRemoveDriftDimensions) -{ - int rows = 6; - int cols = 7; - - // Create a matrix and vector, set one value so we can track the removal was correct - Eigen::MatrixXd matrix(rows, cols); - matrix.setZero(); - matrix(2, 0) = 4.0; - Eigen::VectorXd vector(rows); - vector.setZero(); - vector(2) = 4.0; - - // Remove with default drift_dimensions_ values should not change - servo_calcs_->removeDriftDimensions(matrix, vector); - EXPECT_EQ(matrix.rows(), rows); - EXPECT_EQ(matrix.cols(), cols); - EXPECT_EQ(vector.size(), rows); - EXPECT_EQ(matrix(2, 0), 4.0); - EXPECT_EQ(vector(2), 4.0); - - // Set drift_dimensions_ to be something with True's in it - servo_calcs_->drift_dimensions_[0] = true; - servo_calcs_->drift_dimensions_[1] = true; - servo_calcs_->drift_dimensions_[5] = true; - - // Now a remove should have changes - servo_calcs_->removeDriftDimensions(matrix, vector); - EXPECT_EQ(matrix.rows(), rows - 3); - EXPECT_EQ(matrix.cols(), cols); - EXPECT_EQ(vector.size(), rows - 3); - EXPECT_EQ(matrix(0, 0), 4.0); - EXPECT_EQ(vector(0), 4.0); -} - -TEST_F(ServoCalcsTestFixture, TestEnforceControlDimensions) -{ - // Create a TwistStamped message to test with - geometry_msgs::msg::TwistStamped msg; - msg.twist.linear.x = 1.0; - msg.twist.linear.y = 2.0; - msg.twist.linear.z = 3.0; - msg.twist.angular.x = 4.0; - msg.twist.angular.y = 5.0; - msg.twist.angular.z = 6.0; - geometry_msgs::msg::TwistStamped init_copy = msg; - - // Enforcing with the default values should not change anything - servo_calcs_->enforceControlDimensions(msg); - EXPECT_EQ(msg, init_copy); - - // Lets set the whole array to false and make sure each value is changed to 0 - for (size_t i = 0; i < 6; ++i) - { - servo_calcs_->control_dimensions_[i] = false; - } - geometry_msgs::msg::TwistStamped empty_msg; - servo_calcs_->enforceControlDimensions(msg); - EXPECT_EQ(msg, empty_msg); -} - -TEST_F(ServoCalcsTestFixture, TestCheckValidCommand) -{ - // Create a valid JointJog message and test it - control_msgs::msg::JointJog joint_msg; - joint_msg.velocities.push_back(1.0); - EXPECT_TRUE(servo_calcs_->checkValidCommand(joint_msg)); - - // Now give it a NaN and check again - joint_msg.velocities.push_back(NAN); - EXPECT_FALSE(servo_calcs_->checkValidCommand(joint_msg)); - - // Create a valid TwistStamped message and test it - geometry_msgs::msg::TwistStamped twist_msg; - twist_msg.twist.linear.x = 1.0; - EXPECT_TRUE(servo_calcs_->checkValidCommand(twist_msg)); - - // Now give it a NaN and check again - twist_msg.twist.linear.y = NAN; - EXPECT_FALSE(servo_calcs_->checkValidCommand(twist_msg)); - - // Now set the scaling to unitless and give it a number with abs() > 1, expecting a fail - const_cast(servo_calcs_->parameters_.get())->command_in_type = "speed_units"; - twist_msg.twist.linear.y = -10.0; - EXPECT_TRUE(servo_calcs_->checkValidCommand(twist_msg)); - const_cast(servo_calcs_->parameters_.get())->command_in_type = "unitless"; - EXPECT_FALSE(servo_calcs_->checkValidCommand(twist_msg)); -} - -TEST_F(ServoCalcsTestFixture, TestApplyJointUpdate) -{ - // Create Eigen arrays for testing - Eigen::ArrayXd deltas(3); - deltas[0] = 1.0; - deltas[1] = 2.0; - deltas[2] = 3.0; - Eigen::ArrayXd prev_vel(3); - prev_vel.setZero(); - - // Test catching size mismatches - sensor_msgs::msg::JointState joint_state; - EXPECT_FALSE(servo_calcs_->applyJointUpdate(deltas, joint_state, prev_vel)); - - // Now test it with properly sized arrays - joint_state.position.assign(3, 10.0); - joint_state.velocity.assign(3, 0); - EXPECT_TRUE(servo_calcs_->applyJointUpdate(deltas, joint_state, prev_vel)); - - // Can't test equality on the position because of filtering - EXPECT_NE(joint_state.position[0], 10); - EXPECT_NE(joint_state.position[2], 10); - - // Velocities should match though - EXPECT_EQ(joint_state.velocity[0], deltas[0] / servo_calcs_->parameters_->publish_period); - EXPECT_EQ(joint_state.velocity[2], deltas[2] / servo_calcs_->parameters_->publish_period); - EXPECT_EQ(joint_state.velocity[0], prev_vel[0]); -} - -TEST_F(ServoCalcsTestFixture, TestInsertRedundantPoints) -{ - // Create a joint trajectory message for testing - trajectory_msgs::msg::JointTrajectory joint_traj; - trajectory_msgs::msg::JointTrajectoryPoint point; - point.positions.push_back(1.0); - point.positions.push_back(2.0); - point.velocities.push_back(3.0); - point.velocities.push_back(4.0); - joint_traj.points.push_back(point); - - auto init_copy = joint_traj; - - // Test for count 0 and 1, this should leave the message unchanged - servo_calcs_->insertRedundantPointsIntoTrajectory(joint_traj, 0); - EXPECT_EQ(joint_traj, init_copy); - servo_calcs_->insertRedundantPointsIntoTrajectory(joint_traj, 1); - EXPECT_EQ(joint_traj, init_copy); - - // Test for a more reasonable count - size_t count = 10; - servo_calcs_->insertRedundantPointsIntoTrajectory(joint_traj, count); - EXPECT_NE(joint_traj, init_copy); - EXPECT_EQ(joint_traj.points.size(), count); - EXPECT_EQ(joint_traj.points[0].positions, joint_traj.points.back().positions); -} - -TEST_F(ServoCalcsTestFixture, TestSuddenHalt) -{ - // We need to set up some ServoCalcs members - servo_calcs_->num_joints_ = 3; - servo_calcs_->original_joint_state_.position.push_back(1.0); - servo_calcs_->original_joint_state_.position.push_back(2.0); - servo_calcs_->original_joint_state_.position.push_back(3.0); - - // Let's make sure to test publishing position and velocity - const_cast(servo_calcs_->parameters_.get())->publish_joint_positions = true; - const_cast(servo_calcs_->parameters_.get())->publish_joint_velocities = true; - - // Let's start with an empty trajectory - trajectory_msgs::msg::JointTrajectory msg; - servo_calcs_->suddenHalt(msg); - EXPECT_EQ(msg.points.size(), 1UL); - EXPECT_EQ(msg.points[0].positions.size(), 3UL); - EXPECT_EQ(msg.points[0].velocities.size(), 3UL); - EXPECT_EQ(msg.points[0].positions[2], 3.0); - EXPECT_EQ(msg.points[0].velocities[2], 0.0); - - // Now if we change those values and call it again, they should go back - msg.points[0].positions[2] = 100.0; - msg.points[0].velocities[2] = 100.0; - servo_calcs_->suddenHalt(msg); - EXPECT_EQ(msg.points[0].positions[2], 3.0); - EXPECT_EQ(msg.points[0].velocities[2], 0.0); -} - -TEST_F(ServoCalcsTestFixture, TestEnforcePosLimits) -{ - // Set the position to the upper limits - std::vector position{ 0, 0, 2.8973, 1.9628, 2.8973, 0.0175, 2.8973, 3.7525, 2.8973 }; - std::vector velocity; - velocity.assign(9, 1.0); - - // Set the position in the ServoCalcs object - sensor_msgs::msg::JointState joint_state = getJointState(position, velocity); - servo_calcs_->original_joint_state_ = joint_state; - servo_calcs_->current_state_->setVariableValues(joint_state); - - // Test here, expecting to be violating joint position limits - EXPECT_FALSE(servo_calcs_->enforcePositionLimits()); - - // At the upper limits with negative velocity, we should not be 'violating' - velocity.assign(9, -1.0); - joint_state = getJointState(position, velocity); - servo_calcs_->original_joint_state_ = joint_state; - servo_calcs_->current_state_->setVariableValues(joint_state); - EXPECT_TRUE(servo_calcs_->enforcePositionLimits()); - - // However, if we change 1 of the joints to the bottom limit and stay negative velocity - // We expect to violate joint position limits again - position[2] = -2.8973; - joint_state = getJointState(position, velocity); - servo_calcs_->original_joint_state_ = joint_state; - servo_calcs_->current_state_->setVariableValues(joint_state); - EXPECT_FALSE(servo_calcs_->enforcePositionLimits()); - - // For completeness, we'll set the position to lower limits with positive vel and expect a pass - std::vector lower_position{ 0, 0, -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973 }; - velocity.assign(9, 1.0); - joint_state = getJointState(lower_position, velocity); - servo_calcs_->original_joint_state_ = joint_state; - servo_calcs_->current_state_->setVariableValues(joint_state); - EXPECT_TRUE(servo_calcs_->enforcePositionLimits()); -} - -TEST_F(ServoCalcsTestFixture, TestEnforceVelLimits) -{ - // First, define the velocity limits (from panda URDF) - std::vector vel_limits{ 2.1750, 2.1750, 2.1750, 2.1750, 2.610, 2.610, 2.610 }; - - // Let's test the Velocity limits first - // Set prev_joint_velocity_ == desired_velocity, both above the limits - // to avoid acceleration limits (accel is 0) - Eigen::ArrayXd desired_velocity(7); - desired_velocity << 3, 3, 3, 3, 3, 3, 3; // rad/s - desired_velocity *= servo_calcs_->parameters_->publish_period; // rad/loop - servo_calcs_->prev_joint_velocity_ = desired_velocity; - - Eigen::ArrayXd delta_theta = servo_calcs_->parameters_->publish_period * desired_velocity; - - // Do the enforcing and check it - servo_calcs_->enforceVelLimits(delta_theta); - for (size_t i = 0; i < 7; ++i) - { - // We need to check vs radians-per-loop allowable rate (not rad/s) - EXPECT_LE(delta_theta[i], vel_limits[i] * servo_calcs_->parameters_->publish_period); - } - - // Let's check negative velocity limits too - delta_theta *= -1; - servo_calcs_->prev_joint_velocity_ = -1 * desired_velocity; - servo_calcs_->enforceVelLimits(delta_theta); - for (size_t i = 0; i < 7; ++i) - { - // We need to check vs radians-per-loop allowable rate (not rad/s) - EXPECT_GE(delta_theta[i], -1 * vel_limits[i] * servo_calcs_->parameters_->publish_period); - } -} - -// TODO(henningkayser): re-enable acceleration limit enforcement -// The accel/vel limit enforcement has been refactored, enforceSingleVelAccelLimit() was removed -// since all values need to be scaled by the lowest bound -// TEST_F(ServoCalcsTestFixture, TestEnforceAccelLimits) -// { -// // The panda URDF defines no accel limits -// // So we get the bound from joint_model_group_ and modify it -// auto joint_model = servo_calcs_->joint_model_group_->getActiveJointModels()[3]; -// auto bounds = joint_model->getVariableBounds(joint_model->getName()); -// bounds.acceleration_bounded_ = true; -// bounds.min_acceleration_ = -3; -// bounds.max_acceleration_ = 3; -// -// // Pick previous_velocity and desired_velocity to violate limits -// double previous_velocity = -2; // rad/s & within velocity limits -// double desired_velocity = 2; // rad/s & within velocity limits -// -// // From those, calculate desired delta_theta and current acceleration -// double delta_theta = desired_velocity * servo_calcs_->parameters_->publish_period; // rad -// double acceleration = (desired_velocity - previous_velocity) / servo_calcs_->parameters_->publish_period; // rad/s^2 -// -// // Enforce the bounds -// double init_delta_theta = delta_theta; -// servo_calcs_->enforceSingleVelAccelLimit(bounds, desired_velocity, previous_velocity, acceleration, delta_theta); -// -// // The delta_theta should have dropped to be within the limit -// EXPECT_LT(delta_theta, init_delta_theta); -// -// // In fact we can calculate the maximum delta_theta at the limit as: -// // delta_limit = delta_t * (accel_lim * delta_t _ + vel_prev) -// double delta_at_limit = servo_calcs_->parameters_->publish_period * -// (previous_velocity + bounds.max_acceleration_ * servo_calcs_->parameters_->publish_period); -// EXPECT_EQ(delta_theta, delta_at_limit); -// -// // Let's test again, but with only a small change in velocity -// desired_velocity = -1.9; -// delta_theta = desired_velocity * servo_calcs_->parameters_->publish_period; // rad -// acceleration = (desired_velocity - previous_velocity) / servo_calcs_->parameters_->publish_period; // rad/s^2 -// init_delta_theta = delta_theta; -// servo_calcs_->enforceSingleVelAccelLimit(bounds, desired_velocity, previous_velocity, acceleration, delta_theta); -// -// // Now, the delta_theta should not have changed -// EXPECT_EQ(delta_theta, init_delta_theta); -// } - -TEST_F(ServoCalcsTestFixture, TestScaleCartesianCommand) -{ - // Create a twist msg to test - geometry_msgs::msg::TwistStamped msg; - msg.twist.linear.x = 2.0; - msg.twist.angular.z = 6.0; - - // Lets test an invalid scaling type first - const_cast(servo_calcs_->parameters_.get())->command_in_type = "invalid_string"; - Eigen::VectorXd result = servo_calcs_->scaleCartesianCommand(msg); - EXPECT_TRUE(result.isZero()); - - // Now let's try with unitless - const_cast(servo_calcs_->parameters_.get())->command_in_type = "unitless"; - result = servo_calcs_->scaleCartesianCommand(msg); - EXPECT_NEAR(result[0], - msg.twist.linear.x * servo_calcs_->parameters_->linear_scale * servo_calcs_->parameters_->publish_period, - 0.001); - EXPECT_NEAR(result[5], - msg.twist.angular.z * servo_calcs_->parameters_->rotational_scale * - servo_calcs_->parameters_->publish_period, - 0.001); - - // And finally with speed_units - const_cast(servo_calcs_->parameters_.get())->command_in_type = "speed_units"; - result = servo_calcs_->scaleCartesianCommand(msg); - EXPECT_NEAR(result[0], msg.twist.linear.x * servo_calcs_->parameters_->publish_period, 0.001); - EXPECT_NEAR(result[5], msg.twist.angular.z * servo_calcs_->parameters_->publish_period, 0.001); -} - -TEST_F(ServoCalcsTestFixture, TestScaleJointCommand) -{ - // Get a JointJog msg to test - control_msgs::msg::JointJog msg; - msg.joint_names = PANDA_JOINT_NAMES; - std::vector vel{ 0, 0, 1, 1, 1, 1, 1, 1, 1 }; - msg.velocities = vel; - - // Test with unitless - const_cast(servo_calcs_->parameters_.get())->command_in_type = "unitless"; - Eigen::VectorXd result = servo_calcs_->scaleJointCommand(msg); - EXPECT_EQ(result[0], servo_calcs_->parameters_->joint_scale * servo_calcs_->parameters_->publish_period); - - // And with speed_units - const_cast(servo_calcs_->parameters_.get())->command_in_type = "speed_units"; - result = servo_calcs_->scaleJointCommand(msg); - EXPECT_EQ(result[0], servo_calcs_->parameters_->publish_period); - - // And for completeness, with invalid scaling type - const_cast(servo_calcs_->parameters_.get())->command_in_type = "invalid_string"; - result = servo_calcs_->scaleJointCommand(msg); - EXPECT_TRUE(result.isZero()); -} - -TEST_F(ServoCalcsTestFixture, TestComposeOutputMsg) -{ - // Create the input and output message - trajectory_msgs::msg::JointTrajectory traj; - sensor_msgs::msg::JointState joint_state; - joint_state.name.push_back("some_joint"); - joint_state.position.push_back(1.0); - joint_state.velocity.push_back(2.0); - - // Perform the compisition with all 3 modes published - const_cast(servo_calcs_->parameters_.get())->publish_joint_positions = true; - const_cast(servo_calcs_->parameters_.get())->publish_joint_velocities = true; - const_cast(servo_calcs_->parameters_.get())->publish_joint_accelerations = true; - servo_calcs_->composeJointTrajMessage(joint_state, traj); - - // Check the header info - EXPECT_TRUE(traj.header.stamp == rclcpp::Time(0)); // Time should be set - EXPECT_EQ(traj.header.frame_id, servo_calcs_->parameters_->planning_frame); - EXPECT_EQ(traj.joint_names[0], "some_joint"); - - // Check the trajectory info - EXPECT_EQ(traj.points.size(), 1UL); - EXPECT_EQ(traj.points[0].positions.size(), 1UL); // Set to input length - EXPECT_EQ(traj.points[0].velocities.size(), 1UL); - EXPECT_EQ(traj.points[0].accelerations.size(), 7UL); // Set to num joints - EXPECT_EQ(traj.points[0].positions[0], 1.0); - EXPECT_EQ(traj.points[0].velocities[0], 2.0); - EXPECT_EQ(traj.points[0].accelerations[0], 0.0); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - // Set up the shared node - TEST_NODE = std::make_shared("servo_calcs_test"); - - // "Load" parameters from yaml as node parameters - std::string robot_description_string, srdf_string; - loadModelFile("moveit_resources_panda_description", "urdf/panda.urdf", robot_description_string); - loadModelFile("moveit_resources_panda_moveit_config", "config/panda.srdf", srdf_string); - TEST_NODE->declare_parameter("robot_description", robot_description_string); - TEST_NODE->declare_parameter("robot_description_semantic", srdf_string); - - // Startup planning_scene_monitor - TEST_TF_BUFFER = std::make_shared(TEST_NODE->get_clock()); - TEST_PSM = std::make_shared(TEST_NODE, "robot_description", - TEST_TF_BUFFER, "planning_scene_monitor"); - // Initialize CurrentStateMonitor - TEST_PSM->startStateMonitor(); - - // read parameters and store them in shared pointer to constant - TEST_PARAMS = moveit_servo::ServoParameters::makeServoParameters(TEST_NODE); - if (TEST_PARAMS == nullptr) - { - RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters"); - rclcpp::shutdown(); - return 1; - } - - RCLCPP_INFO(LOGGER, "Init finished, beginning test"); - - // Actually run the tests - int ret = RUN_ALL_TESTS(); - - // Shut down the shared stuff - TEST_PARAMS.reset(); - TEST_PSM.reset(); - TEST_TF_BUFFER.reset(); - TEST_NODE.reset(); - - rclcpp::shutdown(); - return ret; -} diff --git a/moveit_ros/moveit_servo/test/unit_test_servo_calcs.hpp b/moveit_ros/moveit_servo/test/unit_test_servo_calcs.hpp deleted file mode 100644 index 99a621903c..0000000000 --- a/moveit_ros/moveit_servo/test/unit_test_servo_calcs.hpp +++ /dev/null @@ -1,91 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : unit_test_servo_calcs.hpp - * Project : moveit_servo - * Created : 08/04/2020 - * Author : Adam Pettinger - * Desc : Sets up test fixtures for unit testing ServoCalcs - */ - -#pragma once - -#include -#include - -const std::vector PANDA_JOINT_NAMES{ "panda_finger_joint1", "panda_finger_joint2", "panda_joint1", - "panda_joint2", "panda_joint3", "panda_joint4", - "panda_joint5", "panda_joint6", "panda_joint7" }; - -// subclassing and friending so we can access member variables -class FriendServoCalcs : public moveit_servo::ServoCalcs -{ - FRIEND_TEST(ServoCalcsTestFixture, TestRemoveSingleDimension); - FRIEND_TEST(ServoCalcsTestFixture, TestRemoveDriftDimensions); - FRIEND_TEST(ServoCalcsTestFixture, TestEnforceControlDimensions); - FRIEND_TEST(ServoCalcsTestFixture, TestCheckValidCommand); - FRIEND_TEST(ServoCalcsTestFixture, TestApplyJointUpdate); - FRIEND_TEST(ServoCalcsTestFixture, TestInsertRedundantPoints); - FRIEND_TEST(ServoCalcsTestFixture, TestSuddenHalt); - FRIEND_TEST(ServoCalcsTestFixture, TestEnforcePosLimits); - FRIEND_TEST(ServoCalcsTestFixture, TestEnforceVelLimits); - FRIEND_TEST(ServoCalcsTestFixture, TestEnforceAccelLimits); - FRIEND_TEST(ServoCalcsTestFixture, TestScaleCartesianCommand); - FRIEND_TEST(ServoCalcsTestFixture, TestScaleJointCommand); - FRIEND_TEST(ServoCalcsTestFixture, TestComposeOutputMsg); - -public: - FriendServoCalcs(const rclcpp::Node::SharedPtr& node, - const std::shared_ptr& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); -}; - -class ServoCalcsTestFixture : public ::testing::Test -{ -public: - ServoCalcsTestFixture(); - ~ServoCalcsTestFixture() override = default; - -protected: - rclcpp::Node::SharedPtr node_; - std::unique_ptr servo_calcs_; - - sensor_msgs::msg::JointState getJointState(std::vector pos, std::vector vel); -}; - -// These are shared between each individual unit test -std::shared_ptr TEST_NODE; -std::shared_ptr TEST_TF_BUFFER; -std::shared_ptr TEST_PSM; -std::shared_ptr TEST_PARAMS; From 66b144a6ad67957168bca6fcde5961296c16c5ba Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 5 Apr 2023 01:22:01 -0600 Subject: [PATCH 10/22] Backport #1908 and #2035 (#2081) --- .docker/ci-testing/Dockerfile | 12 ++++++++++-- .docker/source/Dockerfile | 2 +- .github/workflows/ci.yaml | 6 ++++-- .github/workflows/docker.yaml | 2 +- 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index 88ee8eb1ee..45c18e07d6 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/source/Dockerfile b/.docker/source/Dockerfile index f6864cb642..2b9082885e 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -4,7 +4,7 @@ # Downloads the moveit source code and install remaining debian dependencies ARG ROS_DISTRO=rolling -FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing +FROM moveit/moveit2:${ROS_DISTRO}-ci-testing LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de # Export ROS_UNDERLAY for downstream docker containers diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index d6ef85a0d6..8e2959657a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -31,8 +31,10 @@ jobs: CXXFLAGS: >- -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file - DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }} - UPSTREAM_WORKSPACE: moveit2.repos $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f) + DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} + UPSTREAM_WORKSPACE: > + moveit2.repos + $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f) # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src # Uninstall binaries that are duplicated in the .repos file diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index ff37875017..55e382cac3 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: | From f8b764b09d453f4b212b7fd373009f951aac6646 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 6 Apr 2023 13:16:47 +0900 Subject: [PATCH 11/22] Upgrade apt dependencies --with-new-pkgs (#2039) (#2040) --- .docker/ci/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index 931f3d6c1e..c6aa5ee42c 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 \ From a98adf7ae7d217ded54597e127f51878efa7fe4f Mon Sep 17 00:00:00 2001 From: andrey-pr Date: Mon, 8 May 2023 16:17:24 +0200 Subject: [PATCH 12/22] Added butterworth_filter_coeff parameter (#2129) * Added butterworth_filter_coeff parameter * Added formating like in original PR #2091 * Update moveit_core/online_signal_smoothing/src/butterworth_filter.cpp Co-authored-by: AndyZe * Update moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h Co-authored-by: AndyZe * Alphabetized dependencies * Update moveit_core/package.xml Co-authored-by: AndyZe --------- Co-authored-by: andrey <> Co-authored-by: AndyZe --- moveit_core/CMakeLists.txt | 3 +++ .../online_signal_smoothing/CMakeLists.txt | 5 ++++ .../butterworth_filter.h | 2 ++ .../src/butterworth_filter.cpp | 9 ++++--- .../src/butterworth_parameters.yaml | 9 +++++++ moveit_core/package.xml | 24 +++++++++---------- 6 files changed, 37 insertions(+), 15 deletions(-) create mode 100644 moveit_core/online_signal_smoothing/src/butterworth_parameters.yaml diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index d08a116642..93a479fd7d 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_kdl REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(eigen_stl_containers REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(geometric_shapes REQUIRED) find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) @@ -86,6 +87,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS set(THIS_PACKAGE_LIBRARIES moveit_butterworth_filter + moveit_butterworth_parameters moveit_collision_distance_field moveit_collision_detection moveit_collision_detection_fcl @@ -115,6 +117,7 @@ set(THIS_PACKAGE_LIBRARIES set(THIS_PACKAGE_INCLUDE_DEPENDS angles eigen_stl_containers + generate_parameter_library geometric_shapes geometry_msgs kdl_parser diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index 851bde9b1a..0982a5d4d1 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -27,9 +27,14 @@ target_include_directories(${BUTTERWORTH_FILTER_LIB} PUBLIC set_target_properties(${BUTTERWORTH_FILTER_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}" ) + +generate_parameter_library(moveit_butterworth_parameters src/butterworth_parameters.yaml) + target_link_libraries(${BUTTERWORTH_FILTER_LIB} ${SMOOTHING_BASE_LIB} + moveit_butterworth_parameters moveit_robot_model + moveit_smoothing_base ) ament_target_dependencies(${BUTTERWORTH_FILTER_LIB} srdfdom # include dependency from moveit_robot_model diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index 9563224504..6e9f25bb3d 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -40,6 +40,8 @@ #include +// Auto-generated +#include #include #include diff --git a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp index f7313c2ad4..7e8c6dfcc4 100644 --- a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp @@ -96,12 +96,15 @@ bool ButterworthFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::c { node_ = node; num_joints_ = num_joints; + double filter_coeff = 1.5; + { + online_signal_smoothing::ParamListener param_listener(node_); + filter_coeff = param_listener.get_params().butterworth_filter_coeff; + } for (std::size_t i = 0; i < num_joints_; ++i) { - // Low-pass filters for the joint positions - // TODO(andyz): read a parameter - position_filters_.emplace_back(1.5 /* filter coefficient, should be >1 */); + position_filters_.emplace_back(filter_coeff); } return true; }; diff --git a/moveit_core/online_signal_smoothing/src/butterworth_parameters.yaml b/moveit_core/online_signal_smoothing/src/butterworth_parameters.yaml new file mode 100644 index 0000000000..9bff1bc11f --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/butterworth_parameters.yaml @@ -0,0 +1,9 @@ +online_signal_smoothing: + butterworth_filter_coeff: { + type: double, + default_value: 1.5, + description: "Filter coefficient for the Butterworth filter", + validation: { + gt<>: 1.0 + } + } diff --git a/moveit_core/package.xml b/moveit_core/package.xml index b5d0a47fdd..0d3cb224da 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -25,40 +25,40 @@ pkg-config eigen3_cmake_module eigen3_cmake_module - moveit_common - angles - rclcpp - common_interfaces assimp boost - eigen bullet + common_interfaces eigen_stl_containers - libfcl-dev + eigen + generate_parameter_library geometric_shapes geometry_msgs kdl_parser - urdf - urdfdom - urdfdom_headers + libfcl-dev + moveit_common moveit_msgs - octomap octomap_msgs + octomap pluginlib pybind11_vendor random_numbers + rclcpp + ruckig sensor_msgs shape_msgs srdfdom std_msgs - tf2 tf2_eigen tf2_geometry_msgs tf2_kdl + tf2 trajectory_msgs + urdf + urdfdom_headers + urdfdom visualization_msgs - ruckig python3-sphinx-rtd-theme From c06ba6cef28e082db539b93e17ac1caf42c68e9d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 8 May 2023 15:42:10 +0000 Subject: [PATCH 13/22] Fix timeout in waitForCurrentState (backport #1899) (#1938) (cherry picked from commit 2c48478ac9b4ccfa6784f0e3a9cbf92ef1ecd363) Co-authored-by: Carlo Rizzardo Co-authored-by: Henning Kayser --- .../current_state_monitor.h | 7 ++++++ ...urrent_state_monitor_middleware_handle.hpp | 7 ++++++ .../src/current_state_monitor.cpp | 23 +++++++++++-------- ...urrent_state_monitor_middleware_handle.cpp | 5 ++++ .../test/current_state_monitor_tests.cpp | 1 + .../test/trajectory_monitor_tests.cpp | 1 + 6 files changed, 34 insertions(+), 10 deletions(-) diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 526a75ffe1..1b4c9b4995 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -124,6 +124,13 @@ class CurrentStateMonitor */ virtual bool sleepFor(const std::chrono::nanoseconds& nanoseconds) const = 0; + /** + * @brief Uses rclcpp::ok to check the context status + * + * @return Return of rclcpp::ok + */ + virtual bool ok() const = 0; + /** * @brief Get the static transform topic name * diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp index 9316d4a59a..c5f33cfc96 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp @@ -110,6 +110,13 @@ class CurrentStateMonitorMiddlewareHandle : public CurrentStateMonitor::Middlewa */ bool sleepFor(const std::chrono::nanoseconds& nanoseconds) const override; + /** + * @brief Uses rclcpp::ok to check the context status + * + * @return Return of rclcpp::ok + */ + bool ok() const override; + /** * @brief Get the static transform topic name * diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 10626e9a96..836f017660 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -234,6 +234,7 @@ bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait rclcpp::Duration elapsed(0, 0); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(wait_time_s); + rclcpp::Clock steady_clock(RCL_STEADY_TIME); std::unique_lock lock(state_update_lock_); while (current_state_time_ < t) { @@ -243,27 +244,29 @@ bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait { /* We cannot know if the reason of timeout is slow time or absence of * state messages, warn the user. */ - rclcpp::Clock steady_clock(RCL_STEADY_TIME); RCLCPP_WARN_SKIPFIRST_THROTTLE(LOGGER, steady_clock, 1000, - "No state update received within 100ms of system clock"); - } - else - { - elapsed = middleware_handle_->now() - start; + "No state update received within 100ms of system clock. " + "Have been waiting for %fs, timeout is %fs", + elapsed.seconds(), wait_time_s); } } else { state_update_condition_.wait_for(lock, (timeout - elapsed).to_chrono>()); - elapsed = middleware_handle_->now() - start; } + elapsed = middleware_handle_->now() - start; if (elapsed > timeout) { RCLCPP_INFO(LOGGER, - "Didn't received robot state (joint angles) with recent timestamp within " - "%f seconds.\n" + "Didn't receive robot state (joint angles) with recent timestamp within " + "%f seconds. Requested time %f, but latest received state has time %f.\n" "Check clock synchronization if your are running ROS across multiple machines!", - wait_time_s); + wait_time_s, t.seconds(), current_state_time_.seconds()); + return false; + } + if (!middleware_handle_->ok()) + { + RCLCPP_DEBUG(LOGGER, "ROS context shut down while waiting for current robot state."); return false; } } diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp index 986a58c7af..40f29bd075 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp @@ -95,6 +95,11 @@ bool CurrentStateMonitorMiddlewareHandle::sleepFor(const std::chrono::nanosecond return rclcpp::sleep_for(nanoseconds); } +bool CurrentStateMonitorMiddlewareHandle::ok() const +{ + return rclcpp::ok(); +} + void CurrentStateMonitorMiddlewareHandle::createDynamicTfSubscription(TfCallback callback) { transform_subscriber_ = diff --git a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp index 2ba6f1893f..6e46e6538b 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp @@ -53,6 +53,7 @@ struct MockMiddlewareHandle : public planning_scene_monitor::CurrentStateMonitor MOCK_METHOD(void, resetJointStateSubscription, (), (override)); MOCK_METHOD(std::string, getJointStateTopicName, (), (const, override)); MOCK_METHOD(bool, sleepFor, (const std::chrono::nanoseconds& nanoseconds), (const, override)); + MOCK_METHOD(bool, ok, (), (const, override)); MOCK_METHOD(void, createStaticTfSubscription, (TfCallback callback), (override)); MOCK_METHOD(void, createDynamicTfSubscription, (TfCallback callback), (override)); MOCK_METHOD(std::string, getStaticTfTopicName, (), (const, override)); diff --git a/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp b/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp index f30267199f..002f2b395a 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp @@ -62,6 +62,7 @@ struct MockCurrentStateMonitorMiddlewareHandle : public planning_scene_monitor:: MOCK_METHOD(void, resetJointStateSubscription, (), (override)); MOCK_METHOD(std::string, getJointStateTopicName, (), (const, override)); MOCK_METHOD(bool, sleepFor, (const std::chrono::nanoseconds& nanoseconds), (const, override)); + MOCK_METHOD(bool, ok, (), (const, override)); MOCK_METHOD(void, createStaticTfSubscription, (TfCallback callback), (override)); MOCK_METHOD(void, createDynamicTfSubscription, (TfCallback callback), (override)); MOCK_METHOD(std::string, getStaticTfTopicName, (), (const, override)); From 8f420506ff2e3c324b2628eb462d4b64f2791def Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 10 May 2023 23:06:20 +0000 Subject: [PATCH 14/22] Update default planning configs to use AddTimeOptimalParameterization (#2167) (#2170) (cherry picked from commit 895e9268bd5d9337bebdede07a7f68a99055a1df) Co-authored-by: Anthony Baker --- .../default_configs/chomp_planning.yaml | 2 +- .../default_configs/ompl_planning.yaml | 2 +- .../planning_request_adapters_plugin_description.xml | 11 ----------- .../ompl-chomp_planning_pipeline.launch.xml | 2 +- 4 files changed, 3 insertions(+), 14 deletions(-) diff --git a/moveit_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index 47002cc036..fa0d77c5ae 100644 --- a/moveit_configs_utils/default_configs/chomp_planning.yaml +++ b/moveit_configs_utils/default_configs/chomp_planning.yaml @@ -2,7 +2,7 @@ planning_plugin: chomp_interface/CHOMPPlanner enable_failure_recovery: true jiggle_fraction: 0.05 request_adapters: >- - default_planner_request_adapters/AddTimeParameterization + default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index a5d0ea8522..36c2c762c8 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -2,7 +2,7 @@ planning_plugin: ompl_interface/OMPLPlanner start_state_max_bounds_error: 0.1 jiggle_fraction: 0.05 request_adapters: >- - default_planner_request_adapters/AddTimeParameterization + default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds diff --git a/moveit_ros/planning/planning_request_adapters_plugin_description.xml b/moveit_ros/planning/planning_request_adapters_plugin_description.xml index 19dc92da8f..e7f7ffa980 100644 --- a/moveit_ros/planning/planning_request_adapters_plugin_description.xml +++ b/moveit_ros/planning/planning_request_adapters_plugin_description.xml @@ -31,17 +31,6 @@ - - - - - - - - A deprecated time parameterization algorithm. - - - This is an improved time parameterization using Time-Optimal Trajectory Generation for Path Following With Bounded Acceleration and Velocity (Kunz and Stilman, RSS 2008). Assumes starting and ending at rest. Not jerk limited. diff --git a/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml index b078992417..07e984b6f1 100644 --- a/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml @@ -2,7 +2,7 @@ Date: Fri, 12 May 2023 14:15:01 +0000 Subject: [PATCH 15/22] Fix Constraint Planning Segfault (#2130) (#2173) * Fix Constraint Planning Segfault * Reuse planner data * apply clang formatting * apply clang formatting round 2 * add FIXME note and verbose output of planning graph size --------- Co-authored-by: Sebastian Jahr (cherry picked from commit 92a7951f74baaf26d07356612a2f5dca0bac5065) Co-authored-by: Marq Rasmussen --- .../src/planning_context_manager.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 458266aaf1..539de26cec 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -94,9 +94,10 @@ MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() // Store all planner data for (const auto& entry : planner_data_storage_paths_) { - RCLCPP_INFO(LOGGER, "Storing planner data"); ob::PlannerData data(planners_[entry.first]->getSpaceInformation()); planners_[entry.first]->getPlannerData(data); + RCLCPP_INFO_STREAM(LOGGER, "Storing planner data. NumEdges: " << data.numEdges() + << ", NumVertices: " << data.numVertices()); storage_.store(data, entry.second.c_str()); } } @@ -117,11 +118,17 @@ ompl::base::PlannerPtr MultiQueryPlannerAllocator::allocatePlanner(const ob::Spa } if (multi_query_planning_enabled) { - // If we already have an instance, use that one + // If we already have an instance, reuse it's planning data + // FIXME: make reusing PlannerPtr not crash, so that we don't have to reconstruct a PlannerPtr instance auto planner_map_it = planners_.find(new_name); if (planner_map_it != planners_.end()) { - return planner_map_it->second; + ob::PlannerData data(si); + planner_map_it->second->getPlannerData(data); + RCLCPP_INFO_STREAM(LOGGER, "Reusing planner data. NumEdges: " << data.numEdges() + << ", NumVertices: " << data.numVertices()); + planners_[planner_map_it->first] = std::shared_ptr{ allocatePersistentPlanner(data) }; + return planners_[planner_map_it->first]; } // Certain multi-query planners allow loading and storing the generated planner data. This feature can be @@ -173,9 +180,10 @@ MultiQueryPlannerAllocator::allocatePlannerImpl(const ob::SpaceInformationPtr& s // Try to initialize planner with loaded planner data if (load_planner_data) { - RCLCPP_INFO(LOGGER, "Loading planner data"); ob::PlannerData data(si); storage_.load(file_path.c_str(), data); + RCLCPP_INFO_STREAM(LOGGER, "Loading planner data. NumEdges: " << data.numEdges() + << ", NumVertices: " << data.numVertices()); planner = std::shared_ptr{ allocatePersistentPlanner(data) }; if (!planner) { From ae51f50d7d722729b058f3d591ef51480b3ca4d2 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 12 May 2023 14:48:42 +0000 Subject: [PATCH 16/22] Do not add Pilz parameters to MoveIt Configs Utils if Pilz is not used (#1583) (#2174) (cherry picked from commit 1c7fa52edeef08bf8eb1e9cc73c1b0835aaf17e7) Co-authored-by: Stephanie Eng --- .../moveit_configs_utils/moveit_configs_builder.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 9190adc369..9c3636fb34 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -128,9 +128,10 @@ def to_dict(self): parameters.update(self.joint_limits) parameters.update(self.moveit_cpp) # Update robot_description_planning with pilz cartesian limits - parameters["robot_description_planning"].update( - self.pilz_cartesian_limits["robot_description_planning"] - ) + if self.pilz_cartesian_limits: + parameters["robot_description_planning"].update( + self.pilz_cartesian_limits["robot_description_planning"] + ) return parameters From 5c9c05273c865b41553a698505298af30cc848c3 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 23 May 2023 16:14:36 +0000 Subject: [PATCH 17/22] Fix ikfast package template (#2195) (#2199) (cherry picked from commit 21036b58e99876928b46e3cc4603a9eb9b85e11d) Co-authored-by: Jafar --- .../scripts/create_ikfast_moveit_plugin.py | 3 +++ .../ikfast_kinematics_plugin/templates/CMakeLists.txt | 1 + 2 files changed, 4 insertions(+) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index e7aae42a5d..49805eb02f 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -219,6 +219,9 @@ def create_ikfast_package(args): xmlElement("maintainer", email="%s@todo.todo" % user_name, text=user_name) ) root.append(xmlElement("buildtool_depend", text="ament_cmake")) + export = xmlElement("export") + export.append(xmlElement("build_type", text="ament_cmake")) + root.append(export) etree.ElementTree(root).write( pkg_xml_path, xml_declaration=True, pretty_print=True, encoding="UTF-8" ) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index edd5b5bb81..e7df5cc67d 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -26,6 +26,7 @@ ament_target_dependencies(${IKFAST_LIBRARY_NAME} tf2_kdl orocos_kdl tf2_eigen + LAPACK ) # suppress warnings about unused variables in OpenRave's solver code target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable -Wno-unused-parameter) From 8d7a2140eab5d7ab344ab82f232e1e842fe21432 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 25 May 2023 20:19:47 +0000 Subject: [PATCH 18/22] add missing dependencies on config utils (backport #1962) (#2206) when installing ros-humble-moveit-setup-assistant from debs, the package cannot currently run due to this missing depend (cherry picked from commit cc635471aadfb9446398ece319ae31c6b72bec86) Co-authored-by: Michael Ferguson --- moveit_setup_assistant/moveit_setup_app_plugins/package.xml | 1 + moveit_setup_assistant/moveit_setup_controllers/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml index 75f3c22adc..111550cbd3 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml @@ -11,6 +11,7 @@ ament_cmake ament_index_cpp + moveit_configs_utils moveit_ros_visualization moveit_setup_framework pluginlib diff --git a/moveit_setup_assistant/moveit_setup_controllers/package.xml b/moveit_setup_assistant/moveit_setup_controllers/package.xml index f5b00fab2e..f84640d2f9 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/package.xml +++ b/moveit_setup_assistant/moveit_setup_controllers/package.xml @@ -20,6 +20,7 @@ ament_cmake_lint_cmake ament_cmake_xmllint ament_lint_auto + moveit_configs_utils moveit_resources_fanuc_moveit_config moveit_resources_panda_moveit_config From 360fce81dfe4bcf8a7b715ecede39a565fbfb9b5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 1 Aug 2023 16:39:11 +0200 Subject: [PATCH 19/22] fix for kinematic constraints parsing (#2267) (#2268) (cherry picked from commit b0f0f680c3f86b8074d208a1e78c92cfa75cf5ca) Co-authored-by: Jorge Nicho --- moveit_core/kinematic_constraints/src/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 320c086ae9..736747b786 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -479,7 +479,7 @@ static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::v const auto constraint_param = "constraints." + constraint_id; if (!node->has_parameter(constraint_param + ".type")) { - RCLCPP_ERROR(LOGGER, "constraint parameter does not specify its type"); + RCLCPP_ERROR(LOGGER, "constraint parameter \"%s\" does not specify its type", constraint_param.c_str()); return false; } std::string constraint_type; @@ -529,7 +529,7 @@ bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string return false; for (auto& constraint_id : constraint_ids) - constraint_id.insert(0, constraints_param); + constraint_id.insert(0, constraints_param + std::string(".")); return collectConstraints(node, constraint_ids, constraints); } From 019d0ee5ced132eb686590fceb37092c2da01fbc Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 2 Aug 2023 07:46:57 +0200 Subject: [PATCH 20/22] Fix comment formatting (#2276) (#2278) (cherry picked from commit 83892d6a7cb2f84485ebd96d41adb3acd8c44bee) Co-authored-by: Stephanie Eng --- .../include/moveit/planning_scene/planning_scene.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 02818427a0..de3981af86 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -255,7 +255,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro return world_const_; } - // brief Get the representation of the world + /** \brief Get the representation of the world */ const collision_detection::WorldPtr& getWorldNonConst() { // we always have a world representation From c1c63b2dba58be5c59fa7a14d1220cbd10394ae5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 2 Aug 2023 07:47:29 +0200 Subject: [PATCH 21/22] Replaced numbers with SystemDefaultsQos() (#2271) (#2277) (cherry picked from commit 5506dd516a91bc145e462b493668ef8623d43521) Co-authored-by: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> --- .../src/local_planner_component.cpp | 3 ++- .../moveit_servo/test/pose_tracking_test.cpp | 2 +- .../semantic_world/src/semantic_world.cpp | 2 +- ...current_state_monitor_middleware_handle.cpp | 6 +----- .../src/planning_scene_monitor.cpp | 10 ++++++---- .../src/synchronized_string_parameter.cpp | 3 ++- .../src/trajectory_execution_manager.cpp | 2 +- .../src/robot_interaction.cpp | 4 ++-- .../src/motion_planning_display.cpp | 2 +- .../src/motion_planning_frame.cpp | 18 ++++++++++-------- .../src/robot_state_display.cpp | 2 +- .../src/trajectory_visualization.cpp | 4 ++-- moveit_ros/warehouse/src/save_to_warehouse.cpp | 8 +++++--- 13 files changed, 35 insertions(+), 31 deletions(-) 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 282062b5b7..3d17ab17ad 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 @@ -201,7 +201,8 @@ bool LocalPlannerComponent::initialize() // Initialize global trajectory listener global_solution_subscriber_ = node_->create_subscription( - config_.global_solution_topic, 1, [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { + config_.global_solution_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { // Add received trajectory to internal reference trajectory robot_trajectory::RobotTrajectory new_trajectory(planning_scene_monitor_->getRobotModel(), msg->group_name); moveit::core::RobotState start_state(planning_scene_monitor_->getRobotModel()); diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp index aad2dc0761..0761e45c75 100644 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -183,7 +183,7 @@ TEST_F(PoseTrackingFixture, OutgoingMsgTest) return; }; auto traj_sub = node_->create_subscription( - "/panda_arm_controller/joint_trajectory", 1, traj_callback); + "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), traj_callback); geometry_msgs::msg::PoseStamped target_pose; target_pose.header.frame_id = "panda_link4"; diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index bf50620fa0..30fd2af823 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -66,7 +66,7 @@ SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node, { table_subscriber_ = node_handle_->create_subscription( - "table_array", 1, + "table_array", rclcpp::SystemDefaultsQoS(), [this](const object_recognition_msgs::msg::TableArray::ConstSharedPtr& msg) { return tableCallback(msg); }); visualization_publisher_ = node_handle_->create_publisher("visualize_place", 20); diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp index 40f29bd075..bb2997e2e0 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp @@ -51,10 +51,6 @@ namespace planning_scene_monitor { -namespace -{ -static const auto SUBSCRIPTION_QOS = rclcpp::QoS(25); -} CurrentStateMonitorMiddlewareHandle::CurrentStateMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node) : node_(node) @@ -70,7 +66,7 @@ void CurrentStateMonitorMiddlewareHandle::createJointStateSubscription(const std JointStateUpdateCallback callback) { joint_state_subscription_ = - node_->create_subscription(topic, SUBSCRIPTION_QOS, callback); + node_->create_subscription(topic, rclcpp::SystemDefaultsQoS(), callback); } void CurrentStateMonitorMiddlewareHandle::resetJointStateSubscription() diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index d800a14dd1..27e6d796a1 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -1090,7 +1090,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic) if (!scene_topic.empty()) { planning_scene_subscriber_ = pnode_->create_subscription( - scene_topic, 100, [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { + scene_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) { return newPlanningSceneCallback(scene); }); RCLCPP_INFO(LOGGER, "Listening to '%s'", planning_scene_subscriber_->get_topic_name()); @@ -1178,7 +1178,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio if (!collision_objects_topic.empty()) { collision_object_subscriber_ = pnode_->create_subscription( - collision_objects_topic, 1024, + collision_objects_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); }); RCLCPP_INFO(LOGGER, "Listening to '%s'", collision_objects_topic.c_str()); } @@ -1186,7 +1186,8 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio if (!planning_scene_world_topic.empty()) { planning_scene_world_subscriber_ = pnode_->create_subscription( - planning_scene_world_topic, 1, [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { + planning_scene_world_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { return newPlanningSceneWorldCallback(world); }); RCLCPP_INFO(LOGGER, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str()); @@ -1257,7 +1258,8 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top { // using regular message filter as there's no header attached_collision_object_subscriber_ = pnode_->create_subscription( - attached_objects_topic, 1024, [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { + attached_objects_topic, rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { return attachObjectCallback(obj); }); RCLCPP_INFO(LOGGER, "Listening to '%s' for attached collision objects", diff --git a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp index 623214aadf..189e3b6c4e 100644 --- a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp +++ b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp @@ -123,7 +123,8 @@ bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration& timeout auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); auto const temp_node = std::make_shared(nd_name); string_subscriber_ = temp_node->create_subscription( - name_, rclcpp::QoS(1).transient_local().reliable(), + name_, + rclcpp::QoS(1).transient_local().reliable(), // "transient_local()" is required for supporting late subscriptions [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return stringCallback(msg); }); rclcpp::WaitSet wait_set; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index e0a6033d9f..32e4e27b4d 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -186,7 +186,7 @@ void TrajectoryExecutionManager::initialize() auto options = rclcpp::SubscriptionOptions(); options.callback_group = callback_group; event_topic_subscriber_ = node_->create_subscription( - EXECUTION_EVENT_TOPIC, 100, + EXECUTION_EVENT_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::String::ConstSharedPtr& event) { return receiveEvent(event); }, options); controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring", diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index c4047d5982..09453cfccc 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -574,8 +574,8 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) [this, marker_name](const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { moveInteractiveMarker(marker_name, *msg); }; - auto subscription = - node_->create_subscription(topic_name, 1, subscription_callback); + auto subscription = node_->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), subscription_callback); int_marker_move_subscribers_.push_back(subscription); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 68b0b1d650..e3b915fd1e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -284,7 +284,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable) if (enable) { planning_group_sub_ = node_->create_subscription( - "/rviz/moveit/select_planning_group", 1, + "/rviz/moveit/select_planning_group", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return selectPlanningGroupCallback(msg); }); } else diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index b0857cc7e9..74703516e7 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -267,26 +267,28 @@ void MotionPlanningFrame::allowExternalProgramCommunication(bool enable) { using std::placeholders::_1; plan_subscriber_ = node_->create_subscription( - "/rviz/moveit/plan", 1, + "/rviz/moveit/plan", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remotePlanCallback(msg); }); execute_subscriber_ = node_->create_subscription( - "/rviz/moveit/execute", 1, + "/rviz/moveit/execute", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteExecuteCallback(msg); }); stop_subscriber_ = node_->create_subscription( - "/rviz/moveit/stop", 1, + "/rviz/moveit/stop", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteStopCallback(msg); }); update_start_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_start_state", 1, + "/rviz/moveit/update_start_state", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateStartStateCallback(msg); }); update_goal_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_goal_state", 1, + "/rviz/moveit/update_goal_state", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateGoalStateCallback(msg); }); update_custom_start_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_custom_start_state", 1, [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { + "/rviz/moveit/update_custom_start_state", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { return remoteUpdateCustomStartStateCallback(msg); }); update_custom_goal_state_subscriber_ = node_->create_subscription( - "/rviz/moveit/update_custom_goal_state", 1, [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { + "/rviz/moveit/update_custom_goal_state", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) { return remoteUpdateCustomGoalStateCallback(msg); }); } @@ -610,7 +612,7 @@ void MotionPlanningFrame::initFromMoveGroupNS() clear_octomap_service_client_ = node_->create_client(move_group::CLEAR_OCTOMAP_SERVICE_NAME); object_recognition_subscriber_ = node_->create_subscription( - "recognized_object_array", 1, + "recognized_object_array", rclcpp::SystemDefaultsQoS(), [this](const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& msg) { return listenDetectedObjects(msg); }); diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 7a5200e5fd..3d56533e25 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -304,7 +304,7 @@ void RobotStateDisplay::changedRobotStateTopic() setStatus(rviz_common::properties::StatusProperty::Warn, "RobotState", "No msg received"); robot_state_subscriber_ = node_->create_subscription( - robot_state_topic_property_->getStdString(), 10, + robot_state_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state) { return newRobotStateCallback(state); }); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 36cede757c..a717256d68 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -184,7 +184,7 @@ void TrajectoryVisualization::onInitialize(const rclcpp::Node::SharedPtr& node, } trajectory_topic_sub_ = node_->create_subscription( - trajectory_topic_property_->getStdString(), 2, + trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); }); } @@ -293,7 +293,7 @@ void TrajectoryVisualization::changedTrajectoryTopic() if (!trajectory_topic_property_->getStdString().empty() && robot_state_) { trajectory_topic_sub_ = node_->create_subscription( - trajectory_topic_property_->getStdString(), 2, + trajectory_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); }); diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index ecfba513bd..d1be5eabf8 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -197,12 +197,14 @@ int main(int argc, char** argv) psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); }); auto mplan_req_sub = node->create_subscription( - "motion_plan_request", 100, + "motion_plan_request", rclcpp::SystemDefaultsQoS(), [&](const moveit_msgs::msg::MotionPlanRequest& msg) { onMotionPlanRequest(msg, psm, pss); }); auto constr_sub = node->create_subscription( - "constraints", 100, [&](const moveit_msgs::msg::Constraints& msg) { onConstraints(msg, cs); }); + "constraints", rclcpp::SystemDefaultsQoS(), + [&](const moveit_msgs::msg::Constraints& msg) { onConstraints(msg, cs); }); auto state_sub = node->create_subscription( - "robot_state", 100, [&](const moveit_msgs::msg::RobotState& msg) { onRobotState(msg, rs); }); + "robot_state", rclcpp::SystemDefaultsQoS(), + [&](const moveit_msgs::msg::RobotState& msg) { onRobotState(msg, rs); }); std::vector topics; psm.getMonitoredTopics(topics); From 9d1a8d5a5b9c3855a5668552b544c4f67b41cab0 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 2 Aug 2023 17:25:36 +0200 Subject: [PATCH 22/22] Use googletest 1.11.9000 in CI (#2283) The humble branch of https://github.com/ament/googletest is stuck on 1.10, producing compiler warnings. We are fixing the version to a more recent one by installing a later release of the vendor package. --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8e2959657a..8eb1919ebe 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -23,9 +23,9 @@ jobs: ROS_DISTRO: humble IKFAST_TEST: true CLANG_TIDY: pedantic - # Silent gmock/gtest warnings: https://github.com/ament/googletest/pull/21 + # Silent gmock/gtest warnings by picking more recent googletest version AFTER_BUILD_UPSTREAM_WORKSPACE: | - git clone --quiet --branch clalancette/update-to-gtest-1.11 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest" + git clone --depth 1 --quiet --branch 1.11.9000 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest" builder_run_build "/opt/ros/${ROS_DISTRO}" "${BASEDIR}/upstream_ws" --packages-select gtest_vendor gmock_vendor env: CXXFLAGS: >-