moveit
- 2.9.0
+ 2.10.0
Meta package that contains all essential packages of MoveIt 2
Henning Kayser
Tyler Weaver
@@ -12,8 +12,8 @@
BSD-3-Clause
http://moveit.ros.org
- https://github.com/ros-planning/moveit2
- https://github.com/ros-planning/moveit2/issues
+ https://github.com/moveit/moveit2
+ https://github.com/moveit/moveit2/issues
Ioan Sucan
Sachin Chitta
@@ -30,10 +30,6 @@
moveit_ros
moveit_setup_assistant
-
- ament_lint_auto
- ament_lint_common
-
ament_cmake
diff --git a/moveit/scripts/create_maintainer_table.py b/moveit/scripts/create_maintainer_table.py
index c1d1ca0a44..2cd7999fd0 100644
--- a/moveit/scripts/create_maintainer_table.py
+++ b/moveit/scripts/create_maintainer_table.py
@@ -161,7 +161,7 @@ def get_first_folder(path):
def populate_package_data(path, package):
output = (
- "| "
+ package.name
diff --git a/moveit2.repos b/moveit2.repos
index 866f7ccdc7..aec6f2851e 100644
--- a/moveit2.repos
+++ b/moveit2.repos
@@ -1,10 +1,14 @@
repositories:
# Keep moveit_* repos here because they are released with moveit
+ geometric_shapes:
+ type: git
+ url: https://github.com/moveit/geometric_shapes.git
+ version: ros2
moveit_msgs:
type: git
- url: https://github.com/ros-planning/moveit_msgs.git
+ url: https://github.com/moveit/moveit_msgs.git
version: ros2
moveit_resources:
type: git
- url: https://github.com/ros-planning/moveit_resources.git
+ url: https://github.com/moveit/moveit_resources.git
version: ros2
diff --git a/moveit2_humble.repos b/moveit2_humble.repos
new file mode 100644
index 0000000000..71a9316f22
--- /dev/null
+++ b/moveit2_humble.repos
@@ -0,0 +1,5 @@
+repositories:
+ generate_parameter_library:
+ type: git
+ url: https://github.com/PickNikRobotics/generate_parameter_library.git
+ version: 0.3.7
diff --git a/moveit2_iron.repos b/moveit2_iron.repos
new file mode 100644
index 0000000000..71a9316f22
--- /dev/null
+++ b/moveit2_iron.repos
@@ -0,0 +1,5 @@
+repositories:
+ generate_parameter_library:
+ type: git
+ url: https://github.com/PickNikRobotics/generate_parameter_library.git
+ version: 0.3.7
diff --git a/moveit_common/CHANGELOG.rst b/moveit_common/CHANGELOG.rst
index 4c9f32767a..3817d0958d 100644
--- a/moveit_common/CHANGELOG.rst
+++ b/moveit_common/CHANGELOG.rst
@@ -2,6 +2,17 @@
Changelog for package moveit_common
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2.10.0 (2024-06-13)
+-------------------
+* Migrate ros-planning org to moveit (`#2847 `_)
+ * Rename github.com/ros-planning -> github.com/moveit
+ * Rename ros-planning.github.io -> moveit.github.io
+ * Rename ros-planning organization in docker and CI workflow files
+ - ghcr.io/ros-planning -> ghcr.io/moveit
+ - github.repository == 'moveit/*''
+* CMake format and lint in pre-commit (`#2683 `_)
+* Contributors: Robert Haschke, Tyler Weaver
+
2.9.0 (2024-01-09)
------------------
* Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2
diff --git a/moveit_common/CMakeLists.txt b/moveit_common/CMakeLists.txt
index 825539f025..c263c6d363 100644
--- a/moveit_common/CMakeLists.txt
+++ b/moveit_common/CMakeLists.txt
@@ -3,16 +3,6 @@ project(moveit_common NONE)
find_package(ament_cmake REQUIRED)
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- ament_lint_auto_find_test_dependencies()
-endif()
+ament_package(CONFIG_EXTRAS "moveit_common-extras.cmake")
-ament_package(
- CONFIG_EXTRAS "moveit_common-extras.cmake"
-)
-
-install(
- DIRECTORY cmake
- DESTINATION share/moveit_common
-)
+install(DIRECTORY cmake DESTINATION share/moveit_common)
diff --git a/moveit_common/cmake/moveit_package.cmake b/moveit_common/cmake/moveit_package.cmake
index 332334d5f1..0906b4726c 100644
--- a/moveit_common/cmake/moveit_package.cmake
+++ b/moveit_common/cmake/moveit_package.cmake
@@ -3,31 +3,30 @@
# 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 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.
+# * 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.
+# * 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.
+# 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.
-
-macro(moveit_package)
+# Module for common settings in MoveIt packages.
+macro(MOVEIT_PACKAGE)
# Set ${PROJECT_NAME}_VERSION
find_package(ament_cmake REQUIRED)
ament_package_xml()
@@ -43,8 +42,16 @@ macro(moveit_package)
if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC")
# Enable warnings
- add_compile_options(-Wall -Wextra
- -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual -Wold-style-cast -Wformat=2)
+ add_compile_options(
+ -Wall
+ -Wextra
+ -Wwrite-strings
+ -Wunreachable-code
+ -Wpointer-arith
+ -Wredundant-decls
+ -Wcast-qual
+ -Wold-style-cast
+ -Wformat=2)
else()
# Defaults for Microsoft C++ compiler
add_compile_options(/W3 /wd4251 /wd4068 /wd4275)
@@ -54,27 +61,41 @@ macro(moveit_package)
# Enable Math Constants
# https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019
- add_compile_definitions(
- _USE_MATH_DEFINES
- )
+ add_compile_definitions(_USE_MATH_DEFINES)
endif()
option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" ON)
if(MOVEIT_CI_WARNINGS)
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
- add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual)
+ add_compile_options(
+ -Wall
+ -Wextra
+ -Wwrite-strings
+ -Wunreachable-code
+ -Wpointer-arith
+ -Wredundant-decls
+ -Wcast-qual)
# This too often has false-positives
add_compile_options(-Wno-maybe-uninitialized)
endif()
if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual)
+ add_compile_options(
+ -Wall
+ -Wextra
+ -Wwrite-strings
+ -Wunreachable-code
+ -Wpointer-arith
+ -Wredundant-decls
+ -Wcast-qual)
endif()
endif()
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
- message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance")
+ message(
+ "${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance"
+ )
set(CMAKE_BUILD_TYPE Release)
endif()
endmacro()
diff --git a/moveit_common/moveit_common-extras.cmake b/moveit_common/moveit_common-extras.cmake
index b2fc2d01a0..08e01eb31b 100644
--- a/moveit_common/moveit_common-extras.cmake
+++ b/moveit_common/moveit_common-extras.cmake
@@ -3,28 +3,26 @@
# 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 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.
+# * 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.
+# * 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.
-
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
include("${moveit_common_DIR}/moveit_package.cmake")
diff --git a/moveit_common/package.xml b/moveit_common/package.xml
index 2ef95923a0..497a394c9c 100644
--- a/moveit_common/package.xml
+++ b/moveit_common/package.xml
@@ -2,7 +2,7 @@
moveit_common
- 2.9.0
+ 2.10.0
Common support functionality used throughout MoveIt
Henning Kayser
Tyler Weaver
@@ -11,8 +11,8 @@
BSD-3-Clause
http://moveit.ros.org
- https://github.com/ros-planning/moveit2/issues
- https://github.com/ros-planning/moveit2
+ https://github.com/moveit/moveit2/issues
+ https://github.com/moveit/moveit2
Lior Lustgarten
@@ -20,9 +20,6 @@
backward_ros
- ament_lint_auto
- ament_lint_common
-
ament_cmake
diff --git a/moveit_configs_utils/CHANGELOG.rst b/moveit_configs_utils/CHANGELOG.rst
index 076e9b1415..ef4153feb6 100644
--- a/moveit_configs_utils/CHANGELOG.rst
+++ b/moveit_configs_utils/CHANGELOG.rst
@@ -2,6 +2,25 @@
Changelog for package moveit_configs_utils
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2.10.0 (2024-06-13)
+-------------------
+* Migrate ros-planning org to moveit (`#2847 `_)
+ * Rename github.com/ros-planning -> github.com/moveit
+ * Rename ros-planning.github.io -> moveit.github.io
+ * Rename ros-planning organization in docker and CI workflow files
+ - ghcr.io/ros-planning -> ghcr.io/moveit
+ - github.repository == 'moveit/*''
+* Fix xacro args loading issue (`#2684 `_)
+ * Fixed xacro args loading issue
+ * Formatting fixes with pre-commit action
+* Pass along move_group_capabilities parameters (`#2587 `_)
+ * Pass along move_group_capabilities parameters
+ * fix lint check
+ * Use move_group_capabilities as default launch argument
+* CMake format and lint in pre-commit (`#2683 `_)
+* Use different packages for launch and config packages in generate_demo_launch (`#2647 `_)
+* Contributors: Alex Navarro, Forrest Rogers-Marcovitz, Robert Haschke, Tyler Weaver
+
2.9.0 (2024-01-09)
------------------
* Update ros2_control usage (`#2620 `_)
diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py
index 808d520d86..c8e3bb533c 100644
--- a/moveit_configs_utils/moveit_configs_utils/launches.py
+++ b/moveit_configs_utils/moveit_configs_utils/launches.py
@@ -197,7 +197,12 @@ def generate_move_group_launch(moveit_config):
DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
)
# load non-default MoveGroup capabilities (space separated)
- ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
+ ld.add_action(
+ DeclareLaunchArgument(
+ "capabilities",
+ default_value=moveit_config.move_group_capabilities["capabilities"],
+ )
+ )
# inhibit these default MoveGroup capabilities (space separated)
ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
@@ -244,10 +249,12 @@ def generate_move_group_launch(moveit_config):
return ld
-def generate_demo_launch(moveit_config):
+def generate_demo_launch(moveit_config, launch_package_path=None):
"""
Launches a self contained demo
+ launch_package_path is optional to use different launch and config packages
+
Includes
* static_virtual_joint_tfs
* robot_state_publisher
@@ -256,6 +263,9 @@ def generate_demo_launch(moveit_config):
* warehouse_db (optional)
* ros2_control_node + controller spawners
"""
+ if launch_package_path == None:
+ launch_package_path = moveit_config.package_path
+
ld = LaunchDescription()
ld.add_action(
DeclareBooleanLaunchArg(
@@ -272,11 +282,11 @@ def generate_demo_launch(moveit_config):
)
)
ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True))
-
# If there are virtual joints, broadcast static tf by including virtual_joints launch
virtual_joints_launch = (
- moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py"
+ launch_package_path / "launch/static_virtual_joint_tfs.launch.py"
)
+
if virtual_joints_launch.exists():
ld.add_action(
IncludeLaunchDescription(
@@ -288,7 +298,7 @@ def generate_demo_launch(moveit_config):
ld.add_action(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- str(moveit_config.package_path / "launch/rsp.launch.py")
+ str(launch_package_path / "launch/rsp.launch.py")
),
)
)
@@ -296,7 +306,7 @@ def generate_demo_launch(moveit_config):
ld.add_action(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- str(moveit_config.package_path / "launch/move_group.launch.py")
+ str(launch_package_path / "launch/move_group.launch.py")
),
)
)
@@ -305,7 +315,7 @@ def generate_demo_launch(moveit_config):
ld.add_action(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- str(moveit_config.package_path / "launch/moveit_rviz.launch.py")
+ str(launch_package_path / "launch/moveit_rviz.launch.py")
),
condition=IfCondition(LaunchConfiguration("use_rviz")),
)
@@ -315,7 +325,7 @@ def generate_demo_launch(moveit_config):
ld.add_action(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- str(moveit_config.package_path / "launch/warehouse_db.launch.py")
+ str(launch_package_path / "launch/warehouse_db.launch.py")
),
condition=IfCondition(LaunchConfiguration("db")),
)
@@ -338,7 +348,7 @@ def generate_demo_launch(moveit_config):
ld.add_action(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- str(moveit_config.package_path / "launch/spawn_controllers.launch.py")
+ str(launch_package_path / "launch/spawn_controllers.launch.py")
),
)
)
diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
index 41e154c25a..04cb077514 100644
--- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
+++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
@@ -173,20 +173,20 @@ def __init__(
if setup_assistant_file.exists():
setup_assistant_yaml = load_yaml(setup_assistant_file)
config = setup_assistant_yaml.get("moveit_setup_assistant_config", {})
- urdf_config = config.get("urdf", config.get("URDF"))
- if urdf_config and self.__urdf_package is None:
- self.__urdf_package = Path(
- get_package_share_directory(urdf_config["package"])
- )
- self.__urdf_file_path = Path(urdf_config["relative_path"])
- if (xacro_args := urdf_config.get("xacro_args")) is not None:
+ if urdf_config := config.get("urdf", config.get("URDF")):
+ if self.__urdf_package is None:
+ self.__urdf_package = Path(
+ get_package_share_directory(urdf_config["package"])
+ )
+ self.__urdf_file_path = Path(urdf_config["relative_path"])
+
+ if xacro_args := urdf_config.get("xacro_args"):
self.__urdf_xacro_args = dict(
arg.split(":=") for arg in xacro_args.split(" ") if arg
)
- srdf_config = config.get("srdf", config.get("SRDF"))
- if srdf_config:
+ if srdf_config := config.get("srdf", config.get("SRDF")):
self.__srdf_file_path = Path(srdf_config["relative_path"])
if not self.__urdf_package or not self.__urdf_file_path:
@@ -414,7 +414,7 @@ def sensors_3d(self, file_path: Optional[str] = None):
if sensors_path.exists():
sensors_data = load_yaml(sensors_path)
# TODO(mikeferguson): remove the second part of this check once
- # https://github.com/ros-planning/moveit_resources/pull/141 has made through buildfarm
+ # https://github.com/moveit/moveit_resources/pull/141 has made through buildfarm
if len(sensors_data["sensors"]) > 0 and sensors_data["sensors"][0]:
self.__moveit_configs.sensors_3d = sensors_data
return self
diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml
index 05bf3e5619..f72fd7510a 100644
--- a/moveit_configs_utils/package.xml
+++ b/moveit_configs_utils/package.xml
@@ -2,7 +2,7 @@
moveit_configs_utils
- 2.9.0
+ 2.10.0
Python library for loading moveit config parameters in launch files
MoveIt Release Team
BSD-3-Clause
@@ -15,9 +15,6 @@
launch_ros
srdfdom
- ament_lint_auto
- ament_lint_common
-
ament_python
diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py
index 8007a515af..8f6c23d322 100644
--- a/moveit_configs_utils/setup.py
+++ b/moveit_configs_utils/setup.py
@@ -5,7 +5,7 @@
setup(
name=package_name,
- version="2.9.0",
+ version="2.10.0",
packages=find_packages(),
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst
index d8708fde06..2b4633600b 100644
--- a/moveit_core/CHANGELOG.rst
+++ b/moveit_core/CHANGELOG.rst
@@ -2,6 +2,49 @@
Changelog for package moveit_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2.10.0 (2024-06-13)
+-------------------
+* Enforce liboctomap-dev by using a cmake version range
+* Add utility functions to get limits and trajectory message (`#2861 `_)
+* Migrate ros-planning org to moveit (`#2847 `_)
+ * Rename github.com/ros-planning -> github.com/moveit
+ * Rename ros-planning.github.io -> moveit.github.io
+ * Rename ros-planning organization in docker and CI workflow files
+ - ghcr.io/ros-planning -> ghcr.io/moveit
+ - github.repository == 'moveit/*''
+* Use std::optional instead of nullptr checking (`#2454 `_)
+* Enable mdof trajectory execution (`#2740 `_)
+ * Add RobotTrajectory conversion from MDOF to joints
+ * Convert MDOF trajectories to joint trajectories in planning interfaces
+ * Treat mdof joint variables as common joints in
+ TrajectoryExecutionManager
+ * Convert multi-DOF trajectories to joints in TEM
+ * Revert "Convert MDOF trajectories to joint trajectories in planning interfaces"
+ This reverts commit 885ee2718594859555b73dc341311a859d31216e.
+ * Handle multi-DOF variables in TEM's bound checking
+ * Add parameter to optionally enable multi-dof conversion
+ * Improve error message about unknown controllers
+ * Fix name ordering in JointTrajectory conversion
+ * Improve DEBUG output in TEM
+ * Comment RobotTrajectory test
+ * add acceleration to avoid out of bounds read
+* Fix doc reference to non-existent function (`#2765 `_)
+* (core) Remove unused python docs folder (`#2746 `_)
+* Unify log names (`#2720 `_)
+* (core) Install collision_detector_fcl_plugin (`#2699 `_)
+ FCL version of acda563
+* Simplify Isometry multiplication benchmarks (`#2628 `_)
+ With the benchmark library, there is no need to specify an iteration count.
+ Interestingly, 4x4 matrix multiplication is faster than affine*matrix
+* CMake format and lint in pre-commit (`#2683 `_)
+* Merge pull request `#2660 `_ from MarqRazz/pr-fix_model_with_collision
+ Fix getLinkModelNamesWithCollisionGeometry to include the base link
+* validate link has parent
+* pre-commit
+* Fix getLinkModelNamesWithCollisionGeometry to include the base link of the planning group
+* Acceleration Limited Smoothing Plugin for Servo (`#2651 `_)
+* Contributors: Henning Kayser, Marq Rasmussen, Matthijs van der Burgh, Paul Gesel, Robert Haschke, Sebastian Jahr, Shobuj Paul, Tyler Weaver, marqrazz
+
2.9.0 (2024-01-09)
------------------
* (core) Remove all references to python wrapper from the core pkg (`#2623 `_)
diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
index 107ce7fa6f..90b2851ded 100644
--- a/moveit_core/CMakeLists.txt
+++ b/moveit_core/CMakeLists.txt
@@ -18,8 +18,11 @@ find_package(geometric_shapes REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(kdl_parser REQUIRED)
find_package(moveit_msgs REQUIRED)
-find_package(OCTOMAP REQUIRED)
+# Enforce system version liboctomap-dev
+# https://github.com/moveit/moveit2/issues/2862
+find_package(octomap 1.9.7...<1.10.0 REQUIRED)
find_package(octomap_msgs REQUIRED)
+find_package(osqp REQUIRED)
find_package(pluginlib REQUIRED)
find_package(random_numbers REQUIRED)
find_package(rclcpp REQUIRED)
@@ -65,39 +68,39 @@ add_subdirectory(transforms)
add_subdirectory(utils)
add_subdirectory(version)
-
install(
- TARGETS
- collision_detector_bullet_plugin
- moveit_butterworth_filter
- moveit_butterworth_parameters
- moveit_collision_detection
- moveit_collision_detection_bullet
- moveit_collision_detection_fcl
- moveit_collision_distance_field
- moveit_constraint_samplers
- moveit_distance_field
- moveit_dynamics_solver
- moveit_exceptions
- moveit_kinematic_constraints
- moveit_kinematics_base
- moveit_kinematics_metrics
- moveit_macros
- moveit_planning_interface
- moveit_planning_scene
- moveit_robot_model
- moveit_robot_state
- moveit_robot_trajectory
- moveit_smoothing_base
- moveit_test_utils
- moveit_trajectory_processing
- moveit_transforms
- moveit_utils
+ TARGETS collision_detector_bullet_plugin
+ collision_detector_fcl_plugin
+ moveit_acceleration_filter
+ moveit_acceleration_filter_parameters
+ moveit_butterworth_filter
+ moveit_butterworth_filter_parameters
+ moveit_collision_detection
+ moveit_collision_detection_bullet
+ moveit_collision_detection_fcl
+ moveit_collision_distance_field
+ moveit_constraint_samplers
+ moveit_distance_field
+ moveit_dynamics_solver
+ moveit_exceptions
+ moveit_kinematic_constraints
+ moveit_kinematics_base
+ moveit_kinematics_metrics
+ moveit_macros
+ moveit_planning_interface
+ moveit_planning_scene
+ moveit_robot_model
+ moveit_robot_state
+ moveit_robot_trajectory
+ moveit_smoothing_base
+ moveit_test_utils
+ moveit_trajectory_processing
+ moveit_transforms
+ moveit_utils
EXPORT moveit_coreTargets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
- RUNTIME DESTINATION bin
-)
+ RUNTIME DESTINATION bin)
ament_export_targets(moveit_coreTargets HAS_LIBRARY_TARGET)
ament_export_dependencies(
@@ -113,8 +116,9 @@ ament_export_dependencies(
geometry_msgs
kdl_parser
moveit_msgs
- OCTOMAP
+ octomap
octomap_msgs
+ osqp
pluginlib
random_numbers
rclcpp
@@ -131,26 +135,16 @@ ament_export_dependencies(
urdf
urdfdom
urdfdom_headers
- visualization_msgs
-)
+ visualization_msgs)
# Plugin exports
-pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml)
-pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml)
-pluginlib_export_plugin_description_file(moveit_core filter_plugin_butterworth.xml)
-
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
-
- # These don't pass yet, disable them for now
- set(ament_cmake_copyright_FOUND TRUE)
- set(ament_cmake_cpplint_FOUND TRUE)
- set(ament_cmake_flake8_FOUND TRUE)
- set(ament_cmake_uncrustify_FOUND TRUE)
- set(ament_cmake_lint_cmake_FOUND TRUE)
-
- # Run all lint tests in package.xml except those listed above
- ament_lint_auto_find_test_dependencies()
-endif()
+pluginlib_export_plugin_description_file(moveit_core
+ collision_detector_fcl_description.xml)
+pluginlib_export_plugin_description_file(
+ moveit_core collision_detector_bullet_description.xml)
+pluginlib_export_plugin_description_file(moveit_core
+ filter_plugin_butterworth.xml)
+pluginlib_export_plugin_description_file(moveit_core
+ filter_plugin_acceleration.xml)
ament_package(CONFIG_EXTRAS ConfigExtras.cmake)
diff --git a/moveit_core/ConfigExtras.cmake b/moveit_core/ConfigExtras.cmake
index 66d6e50b17..8475da4b4a 100644
--- a/moveit_core/ConfigExtras.cmake
+++ b/moveit_core/ConfigExtras.cmake
@@ -1,6 +1,8 @@
# Extras module needed for dependencies to find boost components
-find_package(Boost REQUIRED
+find_package(
+ Boost
+ REQUIRED
chrono
date_time
filesystem
@@ -9,5 +11,4 @@ find_package(Boost REQUIRED
regex
serialization
system
- thread
-)
+ thread)
diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt
index 2bf8b6bfa2..cd90e4d5c5 100644
--- a/moveit_core/collision_detection/CMakeLists.txt
+++ b/moveit_core/collision_detection/CMakeLists.txt
@@ -1,4 +1,5 @@
-add_library(moveit_collision_detection SHARED
+add_library(
+ moveit_collision_detection SHARED
src/allvalid/collision_env_allvalid.cpp
src/collision_common.cpp
src/collision_matrix.cpp
@@ -7,16 +8,16 @@ add_library(moveit_collision_detection SHARED
src/world.cpp
src/world_diff.cpp
src/collision_env.cpp
- src/collision_plugin_cache.cpp
-)
-target_include_directories(moveit_collision_detection PUBLIC
- $
- $
-)
+ src/collision_plugin_cache.cpp)
+target_include_directories(
+ moveit_collision_detection
+ PUBLIC $
+ $)
include(GenerateExportHeader)
generate_export_header(moveit_collision_detection)
-ament_target_dependencies(moveit_collision_detection
+ament_target_dependencies(
+ moveit_collision_detection
pluginlib
rclcpp
rmw_implementation
@@ -27,38 +28,44 @@ ament_target_dependencies(moveit_collision_detection
visualization_msgs
tf2_eigen
geometric_shapes
- OCTOMAP
-)
-target_include_directories(moveit_collision_detection BEFORE PUBLIC $)
-target_include_directories(moveit_collision_detection PUBLIC $)
+ octomap)
+target_include_directories(
+ moveit_collision_detection BEFORE
+ PUBLIC $)
+target_include_directories(
+ moveit_collision_detection
+ PUBLIC $)
-set_target_properties(moveit_collision_detection PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
+set_target_properties(moveit_collision_detection
+ PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
-target_link_libraries(moveit_collision_detection
- moveit_robot_state
- moveit_utils
-)
+target_link_libraries(moveit_collision_detection moveit_robot_state
+ moveit_utils)
# unit tests
if(BUILD_TESTING)
if(WIN32)
# TODO add window paths
else()
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils"
+ )
endif()
- ament_add_gtest(test_world test/test_world.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ ament_add_gtest(test_world test/test_world.cpp APPEND_LIBRARY_DIRS
+ "${APPEND_LIBRARY_DIRS}")
target_link_libraries(test_world moveit_collision_detection)
- ament_add_gtest(test_world_diff test/test_world_diff.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ ament_add_gtest(test_world_diff test/test_world_diff.cpp APPEND_LIBRARY_DIRS
+ "${APPEND_LIBRARY_DIRS}")
target_link_libraries(test_world_diff moveit_collision_detection)
- ament_add_gtest(test_all_valid test/test_all_valid.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
- target_link_libraries(test_all_valid moveit_collision_detection moveit_robot_model)
+ ament_add_gtest(test_all_valid test/test_all_valid.cpp APPEND_LIBRARY_DIRS
+ "${APPEND_LIBRARY_DIRS}")
+ target_link_libraries(test_all_valid moveit_collision_detection
+ moveit_robot_model)
endif()
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_export.h DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_export.h
+ DESTINATION include/moveit_core)
diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
index 8c140ee3c9..2605289cab 100644
--- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
+++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
@@ -46,7 +46,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection.world_allvalid");
+ return moveit::getLogger("moveit.core.collision_detection.env_allvalid");
}
} // namespace
diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp
index d869e61f31..8ee2090959 100644
--- a/moveit_core/collision_detection/src/collision_common.cpp
+++ b/moveit_core/collision_detection/src/collision_common.cpp
@@ -46,7 +46,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_common");
+ return moveit::getLogger("moveit.core.collision_detection.collision_common");
}
} // namespace
diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp
index 338567527d..a353ec54cc 100644
--- a/moveit_core/collision_detection/src/collision_env.cpp
+++ b/moveit_core/collision_detection/src/collision_env.cpp
@@ -44,7 +44,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_env");
+ return moveit::getLogger("moveit.core.collision_detection_env");
}
} // namespace
diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 1d727f0bb5..eb2ef1fa1a 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -47,7 +47,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_matrix");
+ return moveit::getLogger("moveit.core.collision_detection_matrix");
}
} // namespace
diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
index 327a9789a0..aca52fa2c8 100644
--- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp
+++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
@@ -49,7 +49,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_octomap_filter");
+ return moveit::getLogger("moveit.core.collision_detection_octomap_filter");
}
} // namespace
@@ -126,15 +126,15 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
octree->begin_leafs_bbx(bbx_min, bbx_max);
octomap::OcTreeBaseImpl::leaf_bbx_iterator leafs_end =
octree->end_leafs_bbx();
- int count = 0;
+ // int count = 0;
for (; it != leafs_end; ++it)
{
const octomap::point3d pt = it.getCoordinate();
- // double prob = it->getOccupancy();
if (octree->isNodeOccupied(*it)) // magic number!
{
- count++;
node_centers.push_back(pt);
+ // count++;
+ // double prob = it->getOccupancy();
// RCLCPP_INFO(getLogger(), "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
// count, prob, pt.x(), pt.y(), pt.z());
}
diff --git a/moveit_core/collision_detection/src/collision_plugin_cache.cpp b/moveit_core/collision_detection/src/collision_plugin_cache.cpp
index cad03b98cf..619bece8f4 100644
--- a/moveit_core/collision_detection/src/collision_plugin_cache.cpp
+++ b/moveit_core/collision_detection/src/collision_plugin_cache.cpp
@@ -45,7 +45,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_plugin_cache");
+ return moveit::getLogger("moveit.core.collision_detection_plugin_cache");
}
} // namespace
diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp
index ddd154aefa..eae07cf01c 100644
--- a/moveit_core/collision_detection/src/world.cpp
+++ b/moveit_core/collision_detection/src/world.cpp
@@ -46,7 +46,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_world");
+ return moveit::getLogger("moveit.core.collision_detection_world");
}
} // namespace
@@ -109,6 +109,7 @@ void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& p
std::vector World::getObjectIds() const
{
std::vector ids;
+ ids.reserve(objects_.size());
for (const auto& object : objects_)
ids.push_back(object.first);
return ids;
diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt
index 0572a9183a..feadeb6533 100644
--- a/moveit_core/collision_detection_bullet/CMakeLists.txt
+++ b/moveit_core/collision_detection_bullet/CMakeLists.txt
@@ -1,66 +1,68 @@
-add_library(moveit_collision_detection_bullet SHARED
+add_library(
+ moveit_collision_detection_bullet SHARED
src/bullet_integration/bullet_utils.cpp
src/bullet_integration/bullet_discrete_bvh_manager.cpp
src/bullet_integration/bullet_cast_bvh_manager.cpp
src/collision_env_bullet.cpp
src/bullet_integration/bullet_bvh_manager.cpp
src/bullet_integration/contact_checker_common.cpp
- src/bullet_integration/ros_bullet_utils.cpp
-)
-target_include_directories(moveit_collision_detection_bullet PUBLIC
- $
- $
-)
+ src/bullet_integration/ros_bullet_utils.cpp)
+target_include_directories(
+ moveit_collision_detection_bullet
+ PUBLIC $
+ $)
include(GenerateExportHeader)
generate_export_header(moveit_collision_detection_bullet)
-target_include_directories(moveit_collision_detection_bullet PUBLIC $)
-set_target_properties(moveit_collision_detection_bullet PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_collision_detection_bullet SYSTEM
- BULLET
-)
-ament_target_dependencies(moveit_collision_detection_bullet
+target_include_directories(
+ moveit_collision_detection_bullet
+ PUBLIC $)
+set_target_properties(moveit_collision_detection_bullet
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_collision_detection_bullet SYSTEM BULLET)
+ament_target_dependencies(
+ moveit_collision_detection_bullet
rclcpp
rmw_implementation
urdf
urdfdom
urdfdom_headers
visualization_msgs
- octomap_msgs
-)
+ octomap_msgs)
target_link_libraries(moveit_collision_detection_bullet
- moveit_collision_detection
- moveit_utils
-)
+ moveit_collision_detection moveit_utils)
-add_library(collision_detector_bullet_plugin SHARED src/collision_detector_bullet_plugin_loader.cpp)
-set_target_properties(collision_detector_bullet_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(collision_detector_bullet_plugin SYSTEM
- BULLET
-)
-ament_target_dependencies(collision_detector_bullet_plugin
+add_library(collision_detector_bullet_plugin SHARED
+ src/collision_detector_bullet_plugin_loader.cpp)
+set_target_properties(collision_detector_bullet_plugin
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(collision_detector_bullet_plugin SYSTEM BULLET)
+ament_target_dependencies(
+ collision_detector_bullet_plugin
rclcpp
urdf
visualization_msgs
pluginlib
rmw_implementation
- octomap_msgs
-)
-target_link_libraries(collision_detector_bullet_plugin
- moveit_collision_detection_bullet
- moveit_planning_scene
- moveit_utils
-)
+ octomap_msgs)
+target_link_libraries(
+ collision_detector_bullet_plugin moveit_collision_detection_bullet
+ moveit_planning_scene moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_bullet_export.h DESTINATION include/moveit_core)
+install(
+ FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_bullet_export.h
+ DESTINATION include/moveit_core)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
if(WIN32)
- # set(append_library_dirs "$;$")
+ # set(APPEND_LIBRARY_DIRS
+ # "$;$")
else()
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils"
+ )
endif()
# TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished
@@ -68,12 +70,18 @@ if(BUILD_TESTING)
add_compile_options(-Wno-deprecated-declarations)
endif()
- ament_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp)
- target_link_libraries(test_bullet_collision_detection moveit_test_utils moveit_collision_detection_bullet)
+ ament_add_gtest(test_bullet_collision_detection
+ test/test_bullet_collision_detection_pr2.cpp)
+ target_link_libraries(test_bullet_collision_detection moveit_test_utils
+ moveit_collision_detection_bullet)
- ament_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp)
- target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils moveit_collision_detection_bullet)
+ ament_add_gtest(test_bullet_collision_detection_panda
+ test/test_bullet_collision_detection_panda.cpp)
+ target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils
+ moveit_collision_detection_bullet)
- ament_add_gtest(test_bullet_continuous_collision_checking test/test_bullet_continuous_collision_checking.cpp)
- target_link_libraries(test_bullet_continuous_collision_checking moveit_test_utils moveit_collision_detection_bullet)
+ ament_add_gtest(test_bullet_continuous_collision_checking
+ test/test_bullet_continuous_collision_checking.cpp)
+ target_link_libraries(test_bullet_continuous_collision_checking
+ moveit_test_utils moveit_collision_detection_bullet)
endif()
diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp
index 9f9e713430..b326780991 100644
--- a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp
+++ b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp
@@ -115,6 +115,6 @@ Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose)
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection_bullet");
+ return moveit::getLogger("moveit.core.collision_detection_bullet");
}
} // namespace collision_detection_bullet
diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
index 94853d4d57..16c62799ff 100644
--- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
+++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
@@ -56,7 +56,7 @@ namespace cb = collision_detection_bullet;
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_detection.bullet_test");
+ return moveit::getLogger("moveit.core.collision_detection.bullet_test");
}
/** \brief Brings the panda robot in user defined home position */
diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt
index e449297e0f..ece406480c 100644
--- a/moveit_core/collision_detection_fcl/CMakeLists.txt
+++ b/moveit_core/collision_detection_fcl/CMakeLists.txt
@@ -1,52 +1,50 @@
-add_library(moveit_collision_detection_fcl SHARED
- src/collision_common.cpp
- src/collision_env_fcl.cpp
-)
-target_include_directories(moveit_collision_detection_fcl PUBLIC
- $
- $
-)
+add_library(moveit_collision_detection_fcl SHARED src/collision_common.cpp
+ src/collision_env_fcl.cpp)
+target_include_directories(
+ moveit_collision_detection_fcl
+ PUBLIC $
+ $)
include(GenerateExportHeader)
generate_export_header(moveit_collision_detection_fcl)
-target_include_directories(moveit_collision_detection_fcl PUBLIC $)
-set_target_properties(moveit_collision_detection_fcl PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_collision_detection_fcl
+target_include_directories(
+ moveit_collision_detection_fcl
+ PUBLIC $)
+set_target_properties(moveit_collision_detection_fcl
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(
+ moveit_collision_detection_fcl
rclcpp
rmw_implementation
urdf
urdfdom
urdfdom_headers
- visualization_msgs
-)
-target_link_libraries(moveit_collision_detection_fcl
- moveit_collision_detection
- moveit_utils
- fcl
-)
+ visualization_msgs)
+target_link_libraries(moveit_collision_detection_fcl moveit_collision_detection
+ moveit_utils fcl)
-add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp)
-set_target_properties(collision_detector_fcl_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(collision_detector_fcl_plugin
- rclcpp
- urdf
- visualization_msgs
- pluginlib
- rmw_implementation
-)
-target_link_libraries(collision_detector_fcl_plugin
- moveit_collision_detection_fcl
- moveit_planning_scene
- moveit_utils
-)
+add_library(collision_detector_fcl_plugin SHARED
+ src/collision_detector_fcl_plugin_loader.cpp)
+set_target_properties(collision_detector_fcl_plugin
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(collision_detector_fcl_plugin rclcpp urdf
+ visualization_msgs pluginlib rmw_implementation)
+target_link_libraries(
+ collision_detector_fcl_plugin moveit_collision_detection_fcl
+ moveit_planning_scene moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_fcl_export.h DESTINATION include/moveit_core)
+install(
+ FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_fcl_export.h
+ DESTINATION include/moveit_core)
if(BUILD_TESTING)
if(WIN32)
- # set(append_library_dirs "$;$")
+ # set(APPEND_LIBRARY_DIRS
+ # "$;$")
else()
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils"
+ )
endif()
# TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished
@@ -55,11 +53,16 @@ if(BUILD_TESTING)
endif()
ament_add_gtest(test_fcl_collision_env test/test_fcl_env.cpp)
- target_link_libraries(test_fcl_collision_env moveit_test_utils moveit_collision_detection_fcl)
+ target_link_libraries(test_fcl_collision_env moveit_test_utils
+ moveit_collision_detection_fcl)
- ament_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp)
- target_link_libraries(test_fcl_collision_detection moveit_test_utils moveit_collision_detection_fcl)
+ ament_add_gtest(test_fcl_collision_detection
+ test/test_fcl_collision_detection_pr2.cpp)
+ target_link_libraries(test_fcl_collision_detection moveit_test_utils
+ moveit_collision_detection_fcl)
- ament_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp)
- target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils moveit_collision_detection_fcl)
+ ament_add_gtest(test_fcl_collision_detection_panda
+ test/test_fcl_collision_detection_panda.cpp)
+ target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils
+ moveit_collision_detection_fcl)
endif()
diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp
index de0be9ce66..faf5e8eab4 100644
--- a/moveit_core/collision_detection_fcl/src/collision_common.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp
@@ -60,7 +60,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("moveit_collision_detection_fcl");
+ return moveit::getLogger("moveit.core.moveit_collision_detection_fcl");
}
} // namespace
diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
index e1406dc499..54a007d325 100644
--- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
@@ -55,7 +55,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("moveit_collision_detection_fcl");
+ return moveit::getLogger("moveit.core.collision_detection_fcl");
}
// Check whether this FCL version supports the requested computations
diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp
index ff6458b044..e1a59bbddb 100644
--- a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp
+++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp
@@ -66,7 +66,7 @@ inline void setToHome(moveit::core::RobotState& panda_state)
rclcpp::Logger getLogger()
{
- return moveit::getLogger("moveit_collision_detection_fcl.test.test_fcl_env");
+ return moveit::getLogger("moveit.core.collision_detection_fcl.test_fcl_env");
}
class CollisionDetectionEnvTest : public testing::Test
diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt
index 5c116aa689..63b63a013d 100644
--- a/moveit_core/collision_distance_field/CMakeLists.txt
+++ b/moveit_core/collision_distance_field/CMakeLists.txt
@@ -1,48 +1,42 @@
-add_library(moveit_collision_distance_field SHARED
+add_library(
+ moveit_collision_distance_field SHARED
src/collision_distance_field_types.cpp
- src/collision_common_distance_field.cpp
- src/collision_env_distance_field.cpp
- src/collision_env_hybrid.cpp
-)
-target_include_directories(moveit_collision_distance_field PUBLIC
- $
- $
-)
+ src/collision_common_distance_field.cpp src/collision_env_distance_field.cpp
+ src/collision_env_hybrid.cpp)
+target_include_directories(
+ moveit_collision_distance_field
+ PUBLIC $
+ $)
include(GenerateExportHeader)
generate_export_header(moveit_collision_distance_field)
-target_include_directories(moveit_collision_distance_field PUBLIC $)
-set_target_properties(moveit_collision_distance_field PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_include_directories(
+ moveit_collision_distance_field
+ PUBLIC $)
+set_target_properties(moveit_collision_distance_field
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_collision_distance_field
- urdf
- visualization_msgs
- tf2_eigen
- geometric_shapes
- OCTOMAP
-)
+ament_target_dependencies(moveit_collision_distance_field urdf
+ visualization_msgs tf2_eigen geometric_shapes octomap)
-target_link_libraries(moveit_collision_distance_field
- moveit_planning_scene
- moveit_distance_field
- moveit_collision_detection
- moveit_robot_state
-)
+target_link_libraries(
+ moveit_collision_distance_field moveit_planning_scene moveit_distance_field
+ moveit_collision_detection moveit_robot_state)
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_distance_field_export.h DESTINATION include/moveit_core)
+install(
+ FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_distance_field_export.h
+ DESTINATION include/moveit_core)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(resource_retriever REQUIRED)
- ament_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp)
- ament_target_dependencies(test_collision_distance_field
- geometric_shapes
- OCTOMAP
- srdfdom
- resource_retriever
- )
- target_link_libraries(test_collision_distance_field
+ ament_add_gtest(test_collision_distance_field
+ test/test_collision_distance_field.cpp)
+ ament_target_dependencies(test_collision_distance_field geometric_shapes
+ octomap srdfdom resource_retriever)
+ target_link_libraries(
+ test_collision_distance_field
moveit_collision_distance_field
moveit_collision_detection
moveit_distance_field
@@ -50,6 +44,5 @@ if(BUILD_TESTING)
moveit_robot_state
moveit_test_utils
moveit_transforms
- moveit_planning_scene
- )
+ moveit_planning_scene)
endif()
diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h
index b0f4a2ccd7..5887a9cdab 100644
--- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h
+++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h
@@ -373,7 +373,8 @@ class PosedBodySphereDecompositionVector
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- PosedBodySphereDecompositionVector() : logger_(moveit::getLogger("posed_body_sphere_decomposition_vector"))
+ PosedBodySphereDecompositionVector()
+ : logger_(moveit::getLogger("moveit.core.posed_body_sphere_decomposition_vector"))
{
}
@@ -447,7 +448,7 @@ class PosedBodyPointDecompositionVector
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("posed_body_point_decomposition_vector"))
+ PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("moveit.core.posed_body_point_decomposition_vector"))
{
}
diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
index 3cba64aadb..4402a4e44c 100644
--- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
+++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
@@ -50,7 +50,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_common_distance_field");
+ return moveit::getLogger("moveit.core.collision_common_distance_field");
}
} // namespace
diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp
index 40bde7f13c..1754a74b4d 100644
--- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp
+++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp
@@ -52,7 +52,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("collision_distance_field_types");
+ return moveit::getLogger("moveit.core.collision_distance_field_types");
}
} // namespace
diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
index c9bb341352..947ce31263 100644
--- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
+++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
@@ -61,7 +61,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(
const std::map>& link_body_decompositions, double size_x, double size_y,
double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
double collision_tolerance, double max_propogation_distance, double /*padding*/, double /*scale*/)
- : CollisionEnv(robot_model), logger_(moveit::getLogger("collision_robot_distance_field"))
+ : CollisionEnv(robot_model), logger_(moveit::getLogger("moveit.core.collision_robot_distance_field"))
{
initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field,
resolution, collision_tolerance, max_propogation_distance);
@@ -80,7 +80,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(
const std::map>& link_body_decompositions, double size_x, double size_y,
double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
double collision_tolerance, double max_propogation_distance, double padding, double scale)
- : CollisionEnv(robot_model, world, padding, scale), logger_(moveit::getLogger("collision_robot_distance_field"))
+ : CollisionEnv(robot_model, world, padding, scale)
+ , logger_(moveit::getLogger("moveit.core.collision_robot_distance_field"))
{
initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field,
resolution, collision_tolerance, max_propogation_distance);
diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt
index caedd76c9a..25cda7dc66 100644
--- a/moveit_core/constraint_samplers/CMakeLists.txt
+++ b/moveit_core/constraint_samplers/CMakeLists.txt
@@ -1,29 +1,24 @@
-add_library(moveit_constraint_samplers SHARED
- src/constraint_sampler.cpp
- src/constraint_sampler_manager.cpp
- src/constraint_sampler_tools.cpp
- src/default_constraint_samplers.cpp
- src/union_constraint_sampler.cpp
-)
-target_include_directories(moveit_constraint_samplers PUBLIC
- $
- $
-)
-set_target_properties(moveit_constraint_samplers PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_constraint_samplers
- urdf
- urdfdom
- urdfdom_headers
- visualization_msgs
-)
-target_link_libraries(moveit_constraint_samplers
+add_library(
+ moveit_constraint_samplers SHARED
+ src/constraint_sampler.cpp src/constraint_sampler_manager.cpp
+ src/constraint_sampler_tools.cpp src/default_constraint_samplers.cpp
+ src/union_constraint_sampler.cpp)
+target_include_directories(
+ moveit_constraint_samplers
+ PUBLIC $
+ $)
+set_target_properties(moveit_constraint_samplers
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_constraint_samplers urdf urdfdom
+ urdfdom_headers visualization_msgs)
+target_link_libraries(
+ moveit_constraint_samplers
moveit_robot_trajectory
moveit_robot_state
moveit_kinematic_constraints
moveit_kinematics_base
moveit_planning_scene
- moveit_utils
-)
+ moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -32,26 +27,23 @@ if(BUILD_TESTING)
find_package(orocos_kdl REQUIRED)
find_package(angles REQUIRED)
find_package(tf2_kdl REQUIRED)
- include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS})
+ include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS}
+ ${tf2_kdl_INCLUDE_DIRS})
if(WIN32)
- # set(append_library_dirs "$;$")
+ # set(APPEND_LIBRARY_DIRS
+ # "$;$")
else()
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint"
+ )
endif()
- ament_add_gmock(test_constraint_samplers
- test/test_constraint_samplers.cpp
- test/pr2_arm_kinematics_plugin.cpp
- test/pr2_arm_ik.cpp
- )
- target_include_directories(test_constraint_samplers PUBLIC
- ${geometry_msgs_INCLUDE_DIRS}
- )
+ ament_add_gmock(test_constraint_samplers test/test_constraint_samplers.cpp
+ test/pr2_arm_kinematics_plugin.cpp test/pr2_arm_ik.cpp)
+ target_include_directories(test_constraint_samplers
+ PUBLIC ${geometry_msgs_INCLUDE_DIRS})
ament_target_dependencies(test_constraint_samplers kdl_parser)
- target_link_libraries(test_constraint_samplers
- moveit_test_utils
- moveit_utils
- moveit_constraint_samplers
- )
+ target_link_libraries(test_constraint_samplers moveit_test_utils moveit_utils
+ moveit_constraint_samplers)
endif()
diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp
index 654e47bd7f..0f855b14d4 100644
--- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp
+++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp
@@ -45,7 +45,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("constraint_sampler");
+ return moveit::getLogger("moveit.core.constraint_sampler");
}
} // namespace
diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp
index 16d401b16c..a3fea06024 100644
--- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp
+++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp
@@ -48,7 +48,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("constraint_sampler_manager");
+ return moveit::getLogger("moveit.core.constraint_sampler_manager");
}
} // namespace
diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp
index 5cdba1adb4..2aaec85145 100644
--- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp
+++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp
@@ -49,7 +49,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("constraint_sampler_tools");
+ return moveit::getLogger("moveit.core.constraint_sampler_tools");
}
} // namespace
diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
index 0935213ac9..f5db2a6968 100644
--- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
@@ -47,7 +47,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("default_constraint_samplers");
+ return moveit::getLogger("moveit.core.default_constraint_samplers");
}
} // namespace
diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp
index 47db822504..13983d2ff5 100644
--- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp
+++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp
@@ -47,7 +47,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("union_constraint_sampler");
+ return moveit::getLogger("moveit.core.union_constraint_sampler");
}
} // namespace
diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp
index 88a387ab70..b35496a1a4 100644
--- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp
+++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp
@@ -54,7 +54,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("moveit_constaint_samplers.test.pr2_arm_ik");
+ return moveit::getLogger("moveit.core.moveit_constaint_samplers.test.pr2_arm_ik");
}
} // namespace
@@ -804,6 +804,6 @@ bool PR2ArmIK::checkJointLimits(const double joint_value, const int joint_num) c
jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
}
- return !(jv < min_angles_[joint_num] || jv > max_angles_[joint_num]);
+ return jv >= min_angles_[joint_num] && jv <= max_angles_[joint_num];
}
} // namespace pr2_arm_kinematics
diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp
index a1e1978a35..e32ee40eda 100644
--- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp
+++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp
@@ -53,7 +53,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin");
+ return moveit::getLogger("moveit.core.moveit_constraint_samplers.test.pr2_arm_kinematics_plugin");
}
} // namespace
diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt
index 27a12fe87a..c70886f12f 100644
--- a/moveit_core/distance_field/CMakeLists.txt
+++ b/moveit_core/distance_field/CMakeLists.txt
@@ -1,23 +1,23 @@
-add_library(moveit_distance_field SHARED
- src/distance_field.cpp
- src/find_internal_points.cpp
- src/propagation_distance_field.cpp
-)
-target_include_directories(moveit_distance_field PUBLIC
- $
- $
-)
+add_library(
+ moveit_distance_field SHARED
+ src/distance_field.cpp src/find_internal_points.cpp
+ src/propagation_distance_field.cpp)
+target_include_directories(
+ moveit_distance_field
+ PUBLIC $
+ $)
target_link_libraries(moveit_distance_field moveit_macros moveit_utils)
-set_target_properties(moveit_distance_field PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_distance_field
+set_target_properties(moveit_distance_field
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(
+ moveit_distance_field
Boost
urdfdom
urdfdom_headers
visualization_msgs
geometric_shapes
tf2_eigen
- OCTOMAP
-)
+ octomap)
install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -28,5 +28,5 @@ if(BUILD_TESTING)
target_link_libraries(test_voxel_grid moveit_distance_field)
ament_add_gtest(test_distance_field test/test_distance_field.cpp)
- target_link_libraries(test_distance_field moveit_distance_field)
+ target_link_libraries(test_distance_field moveit_distance_field octomap)
endif()
diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp
index c4a94708f7..c619f95d51 100644
--- a/moveit_core/distance_field/src/distance_field.cpp
+++ b/moveit_core/distance_field/src/distance_field.cpp
@@ -51,7 +51,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("distance_field");
+ return moveit::getLogger("moveit.core.distance_field");
}
} // namespace
diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp
index fcaa85b3b1..6b70134120 100644
--- a/moveit_core/distance_field/src/propagation_distance_field.cpp
+++ b/moveit_core/distance_field/src/propagation_distance_field.cpp
@@ -49,7 +49,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("propagation_distance_field");
+ return moveit::getLogger("moveit.core.propagation_distance_field");
}
} // namespace
diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp
index 5792eaa336..62cbfe6d63 100644
--- a/moveit_core/distance_field/test/test_distance_field.cpp
+++ b/moveit_core/distance_field/test/test_distance_field.cpp
@@ -807,20 +807,17 @@ TEST(TestSignedPropagationDistanceField, TestPerformance)
PERF_ORIGIN_Z, PERF_MAX_DIST, true);
EigenSTL::vector_Vector3d bad_vec;
- unsigned int count = 0;
for (unsigned int z = UNIFORM_DISTANCE; z < worstdfu.getZNumCells() - UNIFORM_DISTANCE; z += UNIFORM_DISTANCE)
{
for (unsigned int x = UNIFORM_DISTANCE; x < worstdfu.getXNumCells() - UNIFORM_DISTANCE; x += UNIFORM_DISTANCE)
{
for (unsigned int y = UNIFORM_DISTANCE; y < worstdfu.getYNumCells() - UNIFORM_DISTANCE; y += UNIFORM_DISTANCE)
{
- count++;
Eigen::Vector3d loc;
bool valid = worstdfu.gridToWorld(x, y, z, loc.x(), loc.y(), loc.z());
if (!valid)
{
- // RCLCPP_WARN("distance_field", "Something wrong");
continue;
}
bad_vec.push_back(loc);
diff --git a/moveit_core/doc/_templates/custom-class-template.rst b/moveit_core/doc/_templates/custom-class-template.rst
deleted file mode 100644
index f73eda50ec..0000000000
--- a/moveit_core/doc/_templates/custom-class-template.rst
+++ /dev/null
@@ -1,34 +0,0 @@
-{{ fullname | escape | underline}}
-
-.. currentmodule:: {{ module }}
-
-.. autoclass:: {{ objname }}
- :members:
- :show-inheritance:
- :inherited-members:
- :special-members: __call__, __add__, __mul__
-
- {% block methods %}
- {% if methods %}
- .. rubric:: {{ _('Methods') }}
-
- .. autosummary::
- :nosignatures:
- {% for item in methods %}
- {%- if not item.startswith('_') %}
- ~{{ name }}.{{ item }}
- {%- endif -%}
- {%- endfor %}
- {% endif %}
- {% endblock %}
-
- {% block attributes %}
- {% if attributes %}
- .. rubric:: {{ _('Attributes') }}
-
- .. autosummary::
- {% for item in attributes %}
- ~{{ name }}.{{ item }}
- {%- endfor %}
- {% endif %}
- {% endblock %}
diff --git a/moveit_core/doc/_templates/custom-module-template.rst b/moveit_core/doc/_templates/custom-module-template.rst
deleted file mode 100644
index d066d0e4dc..0000000000
--- a/moveit_core/doc/_templates/custom-module-template.rst
+++ /dev/null
@@ -1,66 +0,0 @@
-{{ fullname | escape | underline}}
-
-.. automodule:: {{ fullname }}
-
- {% block attributes %}
- {% if attributes %}
- .. rubric:: Module attributes
-
- .. autosummary::
- :toctree:
- {% for item in attributes %}
- {{ item }}
- {%- endfor %}
- {% endif %}
- {% endblock %}
-
- {% block functions %}
- {% if functions %}
- .. rubric:: {{ _('Functions') }}
-
- .. autosummary::
- :toctree:
- :nosignatures:
- {% for item in functions %}
- {{ item }}
- {%- endfor %}
- {% endif %}
- {% endblock %}
-
- {% block classes %}
- {% if classes %}
- .. rubric:: {{ _('Classes') }}
-
- .. autosummary::
- :toctree:
- :template: custom-class-template.rst
- :nosignatures:
- {% for item in classes %}
- {{ item }}
- {%- endfor %}
- {% endif %}
- {% endblock %}
-
- {% block exceptions %}
- {% if exceptions %}
- .. rubric:: {{ _('Exceptions') }}
-
- .. autosummary::
- :toctree:
- {% for item in exceptions %}
- {{ item }}
- {%- endfor %}
- {% endif %}
- {% endblock %}
-
-{% block modules %}
-{% if modules %}
-.. autosummary::
- :toctree:
- :template: custom-module-template.rst
- :recursive:
-{% for item in modules %}
- {{ item }}
-{%- endfor %}
-{% endif %}
-{% endblock %}
diff --git a/moveit_core/doc/api.rst b/moveit_core/doc/api.rst
deleted file mode 100644
index 58c6bd1760..0000000000
--- a/moveit_core/doc/api.rst
+++ /dev/null
@@ -1,6 +0,0 @@
-.. autosummary::
- :toctree: _autosummary
- :template: custom-module-template.rst
- :recursive:
-
- moveit.core
diff --git a/moveit_core/doc/conf.py b/moveit_core/doc/conf.py
deleted file mode 100644
index 0ae380d607..0000000000
--- a/moveit_core/doc/conf.py
+++ /dev/null
@@ -1,200 +0,0 @@
-# -*- coding: utf-8 -*-
-#
-# Configuration file for the Sphinx documentation builder.
-#
-# This file does only contain a selection of the most common options. For a
-# full list see the documentation:
-# http://www.sphinx-doc.org/en/master/config
-
-# -- Path setup --------------------------------------------------------------
-
-# If extensions (or modules to document with autodoc) are in another directory,
-# add these directories to sys.path here. If the directory is relative to the
-# documentation root, use os.path.abspath to make it absolute, like shown here.
-#
-import os
-import sys
-import sphinx_rtd_theme
-
-
-# -- Project information -----------------------------------------------------
-
-project = "moveit.core"
-copyright = "2021, MoveIt maintainer team"
-author = "MoveIt maintainer team"
-
-# The short X.Y version
-version = ""
-# The full version, including alpha/beta/rc tags
-release = ""
-
-
-# -- General configuration ---------------------------------------------------
-
-# If your documentation needs a minimal Sphinx version, state it here.
-#
-# needs_sphinx = '1.0'
-
-# Add any Sphinx extension module names here, as strings. They can be
-# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
-# ones.
-extensions = [
- "sphinx.ext.autodoc",
- "sphinx.ext.autosummary",
- "sphinx.ext.intersphinx",
- "sphinx_rtd_theme",
-]
-
-# NOTE: Important variables that make auto-generation work,
-# see https://www.sphinx-doc.org/en/master/usage/extensions/autodoc.html
-autosummary_generate = True
-autosummary_imported_members = True
-autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries
-
-# Customization, not as important
-html_show_sourcelink = (
- True # Remove 'view source code' from top of page (for html, not python)
-)
-autodoc_inherit_docstrings = True # If no docstring, inherit from base class
-set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints
-add_module_names = False
-
-
-# Add any paths that contain templates here, relative to this directory.
-templates_path = ["_templates"]
-
-# The suffix(es) of source filenames.
-# You can specify multiple suffix as a list of string:
-#
-# source_suffix = ['.rst', '.md']
-source_suffix = ".rst"
-
-# The master toctree document.
-master_doc = "index"
-
-# The language for content autogenerated by Sphinx. Refer to documentation
-# for a list of supported languages.
-#
-# This is also used if you do content translation via gettext catalogs.
-# Usually you set "language" from the command line for these cases.
-language = None
-
-# List of patterns, relative to source directory, that match files and
-# directories to ignore when looking for source files.
-# This pattern also affects html_static_path and html_extra_path.
-exclude_patterns = ["_build", "Thumbs.db", ".DS_Store", "_templates"]
-
-# The name of the Pygments (syntax highlighting) style to use.
-pygments_style = None
-
-
-# -- Options for HTML output -------------------------------------------------
-
-# The theme to use for HTML and HTML Help pages. See the documentation for
-# a list of builtin themes.
-#
-html_theme = "sphinx_rtd_theme"
-
-
-# Theme options are theme-specific and customize the look and feel of a theme
-# further. For a list of options available for each theme, see the
-# documentation.
-#
-# html_theme_options = {}
-
-# Add any paths that contain custom static files (such as style sheets) here,
-# relative to this directory. They are copied after the builtin static files,
-# so a file named "default.css" will overwrite the builtin "default.css".
-html_static_path = ["_static"]
-
-# Custom sidebar templates, must be a dictionary that maps document names
-# to template names.
-#
-# The default sidebars (for documents that don't match any pattern) are
-# defined by theme itself. Builtin themes are using these templates by
-# default: ``['localtoc.html', 'relations.html', 'sourcelink.html',
-# 'searchbox.html']``.
-#
-# html_sidebars = {}
-
-
-# -- Options for HTMLHelp output ---------------------------------------------
-
-# Output file base name for HTML help builder.
-htmlhelp_basename = "moveit_coredoc"
-
-
-# -- Options for LaTeX output ------------------------------------------------
-
-latex_elements = {
- # The paper size ('letterpaper' or 'a4paper').
- #
- # 'papersize': 'letterpaper',
- # The font size ('10pt', '11pt' or '12pt').
- #
- # 'pointsize': '10pt',
- # Additional stuff for the LaTeX preamble.
- #
- # 'preamble': '',
- # Latex figure (float) alignment
- #
- # 'figure_align': 'htbp',
-}
-
-# Grouping the document tree into LaTeX files. List of tuples
-# (source start file, target name, title,
-# author, documentclass [howto, manual, or own class]).
-latex_documents = [
- (master_doc, "moveit_core.tex", "moveit\\_core Documentation", "peter", "manual"),
-]
-
-
-# -- Options for manual page output ------------------------------------------
-
-# One entry per manual page. List of tuples
-# (source start file, name, description, authors, manual section).
-man_pages = [(master_doc, "moveit_core", "moveit_core Documentation", [author], 1)]
-
-
-# -- Options for Texinfo output ----------------------------------------------
-
-# Grouping the document tree into Texinfo files. List of tuples
-# (source start file, target name, title, author,
-# dir menu entry, description, category)
-texinfo_documents = [
- (
- master_doc,
- "moveit_core",
- "moveit_core Documentation",
- author,
- "moveit_core",
- "One line description of project.",
- "Miscellaneous",
- ),
-]
-
-
-# -- Options for Epub output -------------------------------------------------
-
-# Bibliographic Dublin Core info.
-epub_title = project
-
-# The unique identifier of the text. This can be a ISBN number
-# or the project homepage.
-#
-# epub_identifier = ''
-
-# A unique identification for the text.
-#
-# epub_uid = ''
-
-# A list of files that should not be packed into the epub file.
-epub_exclude_files = ["search.html"]
-
-
-# -- Extension configuration -------------------------------------------------
-
-# -- Options for intersphinx extension ---------------------------------------
-
-# Example configuration for intersphinx: refer to the Python standard library.
-intersphinx_mapping = {"https://docs.python.org/3": None}
diff --git a/moveit_core/doc/index.rst b/moveit_core/doc/index.rst
deleted file mode 100644
index 9c719062c5..0000000000
--- a/moveit_core/doc/index.rst
+++ /dev/null
@@ -1,13 +0,0 @@
-Python API Reference: moveit_core
-=======================================
-
-.. note::
- These docs are autogenerated. You may see reference to module names like pymoveit_core,
- but you should not use these in your code. Instead, use the modules under the `moveit` namespace.
- For example, you should write ``import moveit.core`` instead of ``import pymoveit_core``.
- The latter will still work, but will not contain the full API that ``import moveit.core`` does.
-
-.. toctree::
-
- Home page
- API reference
diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt
index 7e83f476a5..04afe66ef0 100644
--- a/moveit_core/dynamics_solver/CMakeLists.txt
+++ b/moveit_core/dynamics_solver/CMakeLists.txt
@@ -1,20 +1,13 @@
add_library(moveit_dynamics_solver SHARED src/dynamics_solver.cpp)
-target_include_directories(moveit_dynamics_solver PUBLIC
- $
- $
-)
-set_target_properties(moveit_dynamics_solver PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_include_directories(
+ moveit_dynamics_solver
+ PUBLIC $
+ $)
+set_target_properties(moveit_dynamics_solver
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_dynamics_solver
- urdf
- urdfdom_headers
- orocos_kdl
- visualization_msgs
- kdl_parser
-)
-target_link_libraries(moveit_dynamics_solver
- moveit_robot_state
- moveit_utils
-)
+ament_target_dependencies(moveit_dynamics_solver urdf urdfdom_headers
+ orocos_kdl visualization_msgs kdl_parser)
+target_link_libraries(moveit_dynamics_solver moveit_robot_state moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp
index 8806f82345..9c39863869 100644
--- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp
+++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp
@@ -49,7 +49,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("dynamics_solver");
+ return moveit::getLogger("moveit.core.dynamics_solver");
}
} // namespace
diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt
index aca217c569..26591d955a 100644
--- a/moveit_core/exceptions/CMakeLists.txt
+++ b/moveit_core/exceptions/CMakeLists.txt
@@ -1,15 +1,12 @@
add_library(moveit_exceptions SHARED src/exceptions.cpp)
-target_include_directories(moveit_exceptions PUBLIC
- $
- $
-)
-set_target_properties(moveit_exceptions PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_exceptions
- Boost
- rclcpp
- urdfdom
- urdfdom_headers
-)
+target_include_directories(
+ moveit_exceptions
+ PUBLIC $
+ $)
+set_target_properties(moveit_exceptions PROPERTIES VERSION
+ "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_exceptions Boost rclcpp urdfdom
+ urdfdom_headers)
target_link_libraries(moveit_exceptions moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
diff --git a/moveit_core/filter_plugin_acceleration.xml b/moveit_core/filter_plugin_acceleration.xml
new file mode 100644
index 0000000000..943ecf3f55
--- /dev/null
+++ b/moveit_core/filter_plugin_acceleration.xml
@@ -0,0 +1,8 @@
+
+
+
+ Limits acceleration of commands to generate smooth motion.
+
+
+
diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt
index 5157908c68..51f33e4950 100644
--- a/moveit_core/kinematic_constraints/CMakeLists.txt
+++ b/moveit_core/kinematic_constraints/CMakeLists.txt
@@ -1,36 +1,34 @@
if(WIN32)
- # set(append_library_dirs "$;$")
+ # set(APPEND_LIBRARY_DIRS
+ # "$;$")
else()
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl"
+ )
endif()
-add_library(moveit_kinematic_constraints SHARED
- src/kinematic_constraint.cpp
- src/utils.cpp
-)
-target_include_directories(moveit_kinematic_constraints PUBLIC
- $
- $
-)
-set_target_properties(moveit_kinematic_constraints PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+add_library(moveit_kinematic_constraints SHARED src/kinematic_constraint.cpp
+ src/utils.cpp)
+target_include_directories(
+ moveit_kinematic_constraints
+ PUBLIC $
+ $)
+set_target_properties(moveit_kinematic_constraints
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_kinematic_constraints
+ament_target_dependencies(
+ moveit_kinematic_constraints
urdf
urdfdom
urdfdom_headers
tf2_geometry_msgs
geometry_msgs
visualization_msgs
- tf2_eigen
-)
+ tf2_eigen)
-target_link_libraries(moveit_kinematic_constraints
- moveit_collision_detection_fcl
- moveit_kinematics_base
- moveit_robot_state
- moveit_robot_model
- moveit_utils
-)
+target_link_libraries(
+ moveit_kinematic_constraints moveit_collision_detection_fcl
+ moveit_kinematics_base moveit_robot_state moveit_robot_model moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -38,10 +36,12 @@ if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_constraints test/test_constraints.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}"
- )
- target_link_libraries(test_constraints moveit_test_utils moveit_kinematic_constraints)
+ APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}")
+ target_link_libraries(test_constraints moveit_test_utils
+ moveit_kinematic_constraints)
- ament_add_gtest(test_orientation_constraints test/test_orientation_constraints.cpp)
- target_link_libraries(test_orientation_constraints moveit_test_utils moveit_kinematic_constraints)
+ ament_add_gtest(test_orientation_constraints
+ test/test_orientation_constraints.cpp)
+ target_link_libraries(test_orientation_constraints moveit_test_utils
+ moveit_kinematic_constraints)
endif()
diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
index 13a4cc5865..33e4efbbde 100644
--- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
+++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -60,7 +60,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("kinematic_constraints");
+ return moveit::getLogger("moveit.core.kinematic_constraints");
}
} // namespace
diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp
index a776d08df6..6a66f6826f 100644
--- a/moveit_core/kinematic_constraints/src/utils.cpp
+++ b/moveit_core/kinematic_constraints/src/utils.cpp
@@ -57,7 +57,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("moveit_kinematic_constraints");
+ return moveit::getLogger("moveit.core.kinematic_constraints");
}
} // namespace
diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt
index 13dc3c9c9c..1802539000 100644
--- a/moveit_core/kinematics_base/CMakeLists.txt
+++ b/moveit_core/kinematics_base/CMakeLists.txt
@@ -1,12 +1,15 @@
add_library(moveit_kinematics_base SHARED src/kinematics_base.cpp)
-target_include_directories(moveit_kinematics_base PUBLIC
- $
- $
-)
-target_link_libraries(moveit_kinematics_base moveit_macros moveit_robot_model moveit_utils)
+target_include_directories(
+ moveit_kinematics_base
+ PUBLIC $
+ $)
+target_link_libraries(moveit_kinematics_base moveit_macros moveit_robot_model
+ moveit_utils)
include(GenerateExportHeader)
generate_export_header(moveit_kinematics_base)
-target_include_directories(moveit_kinematics_base PUBLIC $) # for this library
+target_include_directories(
+ moveit_kinematics_base PUBLIC $
+)# for this library
# This line ensures that messages are built before the library is built
ament_target_dependencies(
@@ -14,10 +17,11 @@ ament_target_dependencies(
rclcpp
urdf
urdfdom_headers
- srdfdom moveit_msgs
+ srdfdom
+ moveit_msgs
geometric_shapes
- geometry_msgs
-)
+ geometry_msgs)
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_kinematics_base_export.h DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_kinematics_base_export.h
+ DESTINATION include/moveit_core)
diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
index c51cd4572e..1963d7c389 100644
--- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
+++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
@@ -322,7 +322,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
}
// Otherwise throw error because this function should have been implemented
- RCLCPP_ERROR(moveit::getLogger("kinematics_base"),
+ RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"),
"This kinematic solver does not support searchPositionIK with multiple poses");
return false;
}
@@ -363,7 +363,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback,
error_code, options, context_state);
}
- RCLCPP_ERROR(moveit::getLogger("kinematics_base"),
+ RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"),
"This kinematic solver does not support IK solution cost functions");
return false;
}
@@ -437,8 +437,9 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
{
if (tip_frames_.size() > 1)
{
- RCLCPP_ERROR(moveit::getLogger("kinematics_base"), "This kinematic solver has more than one tip frame, "
- "do not call getTipFrame()");
+ RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"),
+ "This kinematic solver has more than one tip frame, "
+ "do not call getTipFrame()");
}
return tip_frames_[0];
diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp
index 7b333109b3..3b8d61854d 100644
--- a/moveit_core/kinematics_base/src/kinematics_base.cpp
+++ b/moveit_core/kinematics_base/src/kinematics_base.cpp
@@ -45,7 +45,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("kinematics_base");
+ return moveit::getLogger("moveit.core.kinematics_base");
}
} // namespace
diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt
index 52fbbd8e29..e760b834ea 100644
--- a/moveit_core/kinematics_metrics/CMakeLists.txt
+++ b/moveit_core/kinematics_metrics/CMakeLists.txt
@@ -1,19 +1,15 @@
add_library(moveit_kinematics_metrics SHARED src/kinematics_metrics.cpp)
-target_include_directories(moveit_kinematics_metrics PUBLIC
- $
- $
-)
-set_target_properties(moveit_kinematics_metrics PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_include_directories(
+ moveit_kinematics_metrics
+ PUBLIC $
+ $)
+set_target_properties(moveit_kinematics_metrics
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_kinematics_metrics
- urdf
- urdfdom_headers
- visualization_msgs)
+ament_target_dependencies(moveit_kinematics_metrics urdf urdfdom_headers
+ visualization_msgs)
-target_link_libraries(moveit_kinematics_metrics
- moveit_robot_model
- moveit_robot_state
- moveit_utils
-)
+target_link_libraries(moveit_kinematics_metrics moveit_robot_model
+ moveit_robot_state moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp
index d0c4625918..c6ab4eaf44 100644
--- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp
+++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp
@@ -49,7 +49,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("kinematics_metrics");
+ return moveit::getLogger("moveit.core.kinematics_metrics");
}
} // namespace
diff --git a/moveit_core/macros/CMakeLists.txt b/moveit_core/macros/CMakeLists.txt
index 12eb608040..c5cd95113d 100644
--- a/moveit_core/macros/CMakeLists.txt
+++ b/moveit_core/macros/CMakeLists.txt
@@ -1,7 +1,6 @@
add_library(moveit_macros INTERFACE)
-target_include_directories(moveit_macros INTERFACE
- $
- $
-)
+target_include_directories(
+ moveit_macros INTERFACE $
+ $)
install(DIRECTORY include/ DESTINATION include/moveit_core)
diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt
index 6fb01e2aad..bcfca1c75e 100644
--- a/moveit_core/online_signal_smoothing/CMakeLists.txt
+++ b/moveit_core/online_signal_smoothing/CMakeLists.txt
@@ -1,62 +1,76 @@
# Base class
-add_library(moveit_smoothing_base SHARED
- src/smoothing_base_class.cpp
-)
-target_include_directories(moveit_smoothing_base PUBLIC
- $
- $
- $
-)
-target_link_libraries(moveit_smoothing_base
- ${Eigen_LIBRARIES}
- moveit_macros
-)
+add_library(moveit_smoothing_base SHARED src/smoothing_base_class.cpp)
+target_include_directories(
+ moveit_smoothing_base
+ PUBLIC $
+ $
+ $)
+target_link_libraries(moveit_smoothing_base ${Eigen_LIBRARIES} moveit_macros)
include(GenerateExportHeader)
generate_export_header(moveit_smoothing_base)
-set_target_properties(moveit_smoothing_base PROPERTIES VERSION
- "${${PROJECT_NAME}_VERSION}"
-)
-ament_target_dependencies(moveit_smoothing_base
- rclcpp
- tf2_eigen
-)
+set_target_properties(moveit_smoothing_base
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_smoothing_base rclcpp tf2_eigen)
# Plugin implementations
-add_library(moveit_butterworth_filter SHARED
- src/butterworth_filter.cpp
-)
+add_library(moveit_butterworth_filter SHARED src/butterworth_filter.cpp)
generate_export_header(moveit_butterworth_filter)
-target_include_directories(moveit_butterworth_filter PUBLIC
- $
-)
-set_target_properties(moveit_butterworth_filter PROPERTIES VERSION
- "${${PROJECT_NAME}_VERSION}"
-)
-
-generate_parameter_library(moveit_butterworth_parameters src/butterworth_parameters.yaml)
-
-target_link_libraries(moveit_butterworth_filter
- moveit_butterworth_parameters
- moveit_robot_model
- moveit_smoothing_base
-)
-ament_target_dependencies(moveit_butterworth_filter
- srdfdom # include dependency from moveit_robot_model
- pluginlib
-)
+target_include_directories(
+ moveit_butterworth_filter
+ PUBLIC $)
+set_target_properties(moveit_butterworth_filter
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+
+generate_parameter_library(moveit_butterworth_filter_parameters
+ src/butterworth_parameters.yaml)
+
+target_link_libraries(
+ moveit_butterworth_filter moveit_butterworth_filter_parameters
+ moveit_robot_model moveit_smoothing_base)
+ament_target_dependencies(
+ moveit_butterworth_filter
+ srdfdom # include dependency from moveit_robot_model
+ pluginlib)
+
+add_library(moveit_acceleration_filter SHARED src/acceleration_filter.cpp)
+generate_export_header(moveit_acceleration_filter)
+target_include_directories(
+ moveit_acceleration_filter
+ PUBLIC $)
+set_target_properties(moveit_acceleration_filter
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+
+generate_parameter_library(moveit_acceleration_filter_parameters
+ src/acceleration_filter_parameters.yaml)
+
+target_link_libraries(
+ moveit_acceleration_filter moveit_acceleration_filter_parameters
+ moveit_robot_state moveit_smoothing_base osqp::osqp)
+ament_target_dependencies(
+ moveit_acceleration_filter srdfdom # include dependency from
+ # moveit_robot_model
+ pluginlib)
# Installation
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_smoothing_base_export.h DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_smoothing_base_export.h
+ DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h
+ DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h
+ DESTINATION include/moveit_core)
# Testing
if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
# Lowpass filter unit test
ament_add_gtest(test_butterworth_filter test/test_butterworth_filter.cpp)
target_link_libraries(test_butterworth_filter moveit_butterworth_filter)
+
+ # Acceleration filter unit test
+ ament_add_gtest(test_acceleration_filter test/test_acceleration_filter.cpp)
+ target_link_libraries(test_acceleration_filter moveit_acceleration_filter
+ moveit_test_utils)
endif()
diff --git a/moveit_core/online_signal_smoothing/README.md b/moveit_core/online_signal_smoothing/README.md
new file mode 100644
index 0000000000..f6197856c1
--- /dev/null
+++ b/moveit_core/online_signal_smoothing/README.md
@@ -0,0 +1,12 @@
+### AccelerationLimitedPlugin
+Applies smoothing by limiting the acceleration between consecutive commands.
+The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes
+to the servo command topics.
+
+There are three cases considered for acceleration-limiting illustrated in the following figures:
+
+In the figures, the red arrows show the displacement that will occur given the current velocity. The blue line shows the displacement between the current position and the desired position. The black dashed line shows the maximum possible displacements that are within the acceleration limits. The green line shows the acceleration commands that will be used by this acceleration-limiting plugin.
+
+- Figure A: The desired position is within the acceleration limits. The next commanded point will be exactly the desired point.
+- Figure B: The line between the current position and the desired position intersects the acceleration limits, but the reference position is not within the bounds. The next commanded point will be the point on the displacement line that is closest to the reference.
+- Figure C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while maintaining its direction.
diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h
new file mode 100644
index 0000000000..361b14852e
--- /dev/null
+++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h
@@ -0,0 +1,162 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, 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: Paul Gesel
+Description: applies smoothing by limiting the acceleration between consecutive commands.
+The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes
+to the servo command topics.
+
+ In the diagrams below, the c-v lines show the displacement that will occur given the current velocity. The t-c lines
+ shows the displacement between the current position and the desired position. The dashed lines shows the maximum
+ possible displacements that are within the acceleration limits. The v-t lines shows the acceleration commands that
+ will be used by this acceleration-limiting plugin. The x point shows the position that will be used for each scenario.
+
+Scenario A: The desired position is within the acceleration limits. The next commanded point will be exactly the
+desired point.
+ ________
+ | |
+c --|-----xt |
+ \__|__ v |
+ |________|
+
+Scenario B: The line between the current position and the desired position intersects the acceleration limits, but the
+reference position is not within the bounds. The next commanded point will be the point on the displacement line that
+is closest to the reference.
+ ________
+ | |
+c --|--------x------t
+ \__|__ v |
+ |________|
+
+Scenario C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within
+the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while
+maintaining its direction.
+ ________
+ | |
+c --------x--- v |
+ \ | |
+ \ |________|
+ t
+ */
+
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+namespace online_signal_smoothing
+{
+MOVEIT_STRUCT_FORWARD(OSQPDataWrapper);
+
+// Plugin
+class AccelerationLimitedPlugin : public SmoothingBaseClass
+{
+public:
+ /**
+ * Initialize the acceleration based smoothing plugin
+ * @param node ROS node, used for parameter retrieval
+ * @param robot_model typically used to retrieve vel/accel/jerk limits
+ * @param num_joints number of actuated joints in the JointGroup Servo controls
+ * @return True if initialization was successful
+ */
+ bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model,
+ size_t num_joints) override;
+
+ /**
+ * Smooth the command signals for all DOF. This function limits the change in velocity using the acceleration
+ * specified in the robot model.
+ * @param positions array of joint position commands
+ * @param velocities array of joint velocity commands
+ * @param accelerations (unused)
+ * @return True if smoothing was successful
+ */
+ bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override;
+
+ /**
+ * Reset to a given joint state. This method must be called before doSmoothing.
+ * @param positions reset the filters to the joint positions
+ * @param velocities reset the filters to the joint velocities
+ * @param accelerations (unused)
+ * @return True if reset was successful
+ */
+ bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+ const Eigen::VectorXd& accelerations) override;
+
+ /**
+ * memory allocated by osqp is freed in destructor
+ */
+ ~AccelerationLimitedPlugin() override
+ {
+ if (osqp_workspace_ != nullptr)
+ {
+ osqp_cleanup(osqp_workspace_);
+ }
+ }
+
+private:
+ /** \brief Pointer to rclcpp node handle. */
+ rclcpp::Node::SharedPtr node_;
+ /** \brief Parameters for plugin. */
+ online_signal_smoothing::Params params_;
+ /** \brief The number of joints in the robot's planning group. */
+ size_t num_joints_;
+ /** \brief Last velocities and positions received */
+ Eigen::VectorXd last_velocities_;
+ Eigen::VectorXd last_positions_;
+ /** \brief Intermediate variables used in calculations */
+ Eigen::VectorXd cur_acceleration_;
+ Eigen::VectorXd positions_offset_;
+ Eigen::VectorXd velocities_offset_;
+ /** \brief Extracted joint limits from robot model */
+ Eigen::VectorXd max_acceleration_limits_;
+ Eigen::VectorXd min_acceleration_limits_;
+ /** \brief Pointer to robot model */
+ moveit::core::RobotModelConstPtr robot_model_;
+ /** \brief Constraint matrix for optimization problem */
+ Eigen::SparseMatrix constraints_sparse_;
+ /** \brief osqp types used for optimization problem */
+ OSQPDataWrapperPtr osqp_data_;
+ OSQPWorkspace* osqp_workspace_ = nullptr;
+ OSQPSettings osqp_settings_;
+};
+} // namespace online_signal_smoothing
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 bdcf6a7e5b..80212d2fd2 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
@@ -41,8 +41,7 @@
#include
-// Auto-generated
-#include
+#include
#include
#include
diff --git a/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png b/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png
new file mode 100644
index 0000000000..fe32aa78d9
Binary files /dev/null and b/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png differ
diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp
new file mode 100644
index 0000000000..4bfc69ed14
--- /dev/null
+++ b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp
@@ -0,0 +1,349 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of PickNik Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include
+#include
+#include
+
+// Disable -Wold-style-cast because all _THROTTLE macros trigger this
+#pragma GCC diagnostic ignored "-Wold-style-cast"
+
+namespace online_signal_smoothing
+{
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("moveit.core.acceleration_limited_plugin");
+}
+
+// The threshold below which any velocity or position difference is considered zero (rad and rad/s).
+constexpr double COMMAND_DIFFERENCE_THRESHOLD = 1E-4;
+// The scaling parameter alpha between the current point and commanded point must be less than 1.0
+constexpr double ALPHA_UPPER_BOUND = 1.0;
+// The scaling parameter alpha must also be greater than 0.0
+constexpr double ALPHA_LOWER_BOUND = 0.0;
+
+/** \brief Wrapper struct to make memory management easier for using osqp's C sparse_matrix types */
+struct CSCWrapper
+{
+ /// row indices, size nzmax starting from 0
+ std::vector row_indices;
+ /// column pointers (size n+1); col indices (size nzmax)
+ std::vector column_pointers;
+ /// holds the non-zero values in Compressed Sparse Column (CSC) form
+ std::vector elements;
+ /// osqp C sparse_matrix type
+ csc csc_sparse_matrix;
+
+ CSCWrapper(Eigen::SparseMatrix& M)
+ {
+ M.makeCompressed();
+
+ csc_sparse_matrix.n = M.cols();
+ csc_sparse_matrix.m = M.rows();
+ row_indices.assign(M.innerSize(), 0);
+ csc_sparse_matrix.i = row_indices.data();
+ column_pointers.assign(M.outerSize() + 1, 0);
+ csc_sparse_matrix.p = column_pointers.data();
+ csc_sparse_matrix.nzmax = M.nonZeros();
+ csc_sparse_matrix.nz = -1;
+ elements.assign(M.nonZeros(), 0.0);
+ csc_sparse_matrix.x = elements.data();
+
+ update(M);
+ }
+
+ /// Update the the data point to by sparse_matrix without reallocating memory
+ void update(Eigen::SparseMatrix& M)
+ {
+ for (size_t ind = 0; ind < row_indices.size(); ++ind)
+ {
+ row_indices[ind] = M.innerIndexPtr()[ind];
+ }
+
+ for (size_t ind = 0; ind < column_pointers.size(); ++ind)
+ {
+ column_pointers[ind] = M.outerIndexPtr()[ind];
+ }
+ for (size_t ind = 0; ind < elements.size(); ++ind)
+ {
+ elements[ind] = M.data().at(ind);
+ }
+ }
+};
+
+MOVEIT_STRUCT_FORWARD(OSQPDataWrapper);
+
+/** \brief Wrapper struct to make memory management easier for using osqp's C API */
+struct OSQPDataWrapper
+{
+ OSQPDataWrapper(Eigen::SparseMatrix& objective_sparse, Eigen::SparseMatrix& constraints_sparse)
+ : P{ objective_sparse }, A{ constraints_sparse }
+ {
+ data.n = objective_sparse.rows();
+ data.m = constraints_sparse.rows();
+ data.P = &P.csc_sparse_matrix;
+ q = Eigen::VectorXd::Zero(objective_sparse.rows());
+ data.q = q.data();
+ data.A = &A.csc_sparse_matrix;
+ l = Eigen::VectorXd::Zero(constraints_sparse.rows());
+ data.l = l.data();
+ u = Eigen::VectorXd::Zero(constraints_sparse.rows());
+ data.u = u.data();
+ }
+
+ /// Update the constraint matrix A without reallocating memory
+ void updateA(OSQPWorkspace* work, Eigen::SparseMatrix& constraints_sparse)
+ {
+ constraints_sparse.makeCompressed();
+ A.update(constraints_sparse);
+ osqp_update_A(work, A.elements.data(), OSQP_NULL, A.elements.size());
+ }
+
+ CSCWrapper P;
+ CSCWrapper A;
+ Eigen::VectorXd q;
+ Eigen::VectorXd l;
+ Eigen::VectorXd u;
+ OSQPData data{};
+};
+
+bool AccelerationLimitedPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model,
+ size_t num_joints)
+{
+ // copy inputs into member variables
+ node_ = node;
+ num_joints_ = num_joints;
+ robot_model_ = robot_model;
+ cur_acceleration_ = Eigen::VectorXd::Zero(num_joints);
+
+ // get node parameters and store in member variables
+ auto param_listener = online_signal_smoothing::ParamListener(node_);
+ params_ = param_listener.get_params();
+
+ // get robot acceleration limits and store in member variables
+ auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name);
+ auto joint_bounds = joint_model_group->getActiveJointModelsBounds();
+ min_acceleration_limits_ = Eigen::VectorXd::Zero(num_joints);
+ max_acceleration_limits_ = Eigen::VectorXd::Zero(num_joints);
+ size_t ind = 0;
+ for (const auto& joint_bound : joint_bounds)
+ {
+ for (const auto& variable_bound : *joint_bound)
+ {
+ if (variable_bound.acceleration_bounded_)
+ {
+ min_acceleration_limits_[ind] = variable_bound.min_acceleration_;
+ max_acceleration_limits_[ind] = variable_bound.max_acceleration_;
+ }
+ else
+ {
+ RCLCPP_ERROR(getLogger(), "The robot must have acceleration joint limits specified for all joints to "
+ "use AccelerationLimitedPlugin.");
+ return false;
+ }
+ }
+ ind++;
+ }
+
+ // setup osqp optimization problem
+ Eigen::SparseMatrix objective_sparse(1, 1);
+ objective_sparse.insert(0, 0) = 1.0;
+ size_t num_constraints = num_joints + 1;
+ constraints_sparse_ = Eigen::SparseMatrix(num_constraints, 1);
+ for (size_t i = 0; i < num_constraints - 1; ++i)
+ {
+ constraints_sparse_.insert(i, 0) = 0;
+ }
+ constraints_sparse_.insert(num_constraints - 1, 0) = 0;
+ osqp_set_default_settings(&osqp_settings_);
+ osqp_settings_.warm_start = 0;
+ osqp_settings_.verbose = 0;
+ osqp_data_ = std::make_shared(objective_sparse, constraints_sparse_);
+ osqp_data_->q[0] = 0;
+
+ if (osqp_setup(&osqp_workspace_, &osqp_data_->data, &osqp_settings_) != 0)
+ {
+ osqp_settings_.verbose = 1;
+ // call setup again with verbose enables to trigger error message printing
+ osqp_setup(&osqp_workspace_, &osqp_data_->data, &osqp_settings_);
+ RCLCPP_ERROR(getLogger(), "Failed to initialize osqp problem.");
+ return false;
+ }
+
+ return true;
+}
+
+double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations,
+ const moveit::core::JointBoundsVector& joint_bounds)
+{
+ double min_scaling_factor = 1.0;
+
+ // Now get the scaling factor from joint limits.
+ size_t idx = 0;
+ for (const auto& joint_bound : joint_bounds)
+ {
+ for (const auto& variable_bound : *joint_bound)
+ {
+ const auto& target_accel = accelerations(idx);
+ if (variable_bound.acceleration_bounded_ && target_accel != 0.0)
+ {
+ // Find the ratio of clamped acceleration to original acceleration
+ const auto bounded_vel =
+ std::clamp(target_accel, variable_bound.min_acceleration_, variable_bound.max_acceleration_);
+ double joint_scaling_factor = bounded_vel / target_accel;
+ min_scaling_factor = std::min(min_scaling_factor, joint_scaling_factor);
+ }
+ ++idx;
+ }
+ }
+
+ return min_scaling_factor;
+}
+
+inline bool updateData(const OSQPDataWrapperPtr& data, OSQPWorkspace* work,
+ Eigen::SparseMatrix& constraints_sparse, const Eigen::VectorXd& lower_bound,
+ const Eigen::VectorXd& upper_bound)
+{
+ data->updateA(work, constraints_sparse);
+ size_t num_constraints = constraints_sparse.rows();
+ data->u.block(0, 0, num_constraints - 1, 1) = upper_bound;
+ data->l.block(0, 0, num_constraints - 1, 1) = lower_bound;
+ data->u[num_constraints - 1] = ALPHA_UPPER_BOUND;
+ data->l[num_constraints - 1] = ALPHA_LOWER_BOUND;
+ return 0 == osqp_update_bounds(work, data->l.data(), data->u.data());
+}
+
+bool AccelerationLimitedPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities,
+ Eigen::VectorXd& /* unused */)
+{
+ const size_t num_positions = velocities.size();
+ if (num_positions != num_joints_)
+ {
+ RCLCPP_ERROR_THROTTLE(
+ getLogger(), *node_->get_clock(), 1000,
+ "The length of the joint positions parameter is not equal to the number of joints, expected %zu got %zu.",
+ num_joints_, num_positions);
+ return false;
+ }
+ else if (last_positions_.size() != positions.size())
+ {
+ RCLCPP_ERROR_THROTTLE(getLogger(), *node_->get_clock(), 1000,
+ "The length of the last joint positions not equal to the current, expected %zu got %zu. Make "
+ "sure the reset was called.",
+ last_positions_.size(), positions.size());
+ return false;
+ }
+
+ // formulate a quadratic program to find the best new reference point subject to the robot's acceleration limits
+ // p_c: robot's current position
+ // v_c: robot's current velocity
+ // p_t: robot's target position
+ // acc: acceleration to be applied
+ // p_n: next position
+ // dt: time step
+ // p_n_hat: parameterize solution to be along the line from p_c to p_t
+ // p_n_hat = p_t*alpha + p_c*(1-alpha)
+ // define constraints
+ // p_c + v_c*dt + acc_min*dt^2 < p_n_hat < p_c + v_c*dt + acc_max*dt^2
+ // p_c + v_c*dt -p_t + acc_min*dt^2 < (p_c-p_t)alpha < p_c + v_c*dt -p_t + acc_max*dt^2
+ // 0 < alpha < 1
+ // define optimization
+ // opt ||alpha||
+ // s.t. constraints
+ // p_n = p_t*alpha + p_c*(1-alpha)
+
+ double& update_period = params_.update_period;
+ size_t num_constraints = num_joints_ + 1;
+ positions_offset_ = last_positions_ - positions;
+ velocities_offset_ = last_velocities_ - velocities;
+ for (size_t i = 0; i < num_constraints - 1; ++i)
+ {
+ constraints_sparse_.coeffRef(i, 0) = positions_offset_[i];
+ }
+ constraints_sparse_.coeffRef(num_constraints - 1, 0) = 1;
+ Eigen::VectorXd vel_point = last_positions_ + last_velocities_ * update_period;
+ Eigen::VectorXd upper_bound = vel_point - positions + max_acceleration_limits_ * (update_period * update_period);
+ Eigen::VectorXd lower_bound = vel_point - positions + min_acceleration_limits_ * (update_period * update_period);
+ if (!updateData(osqp_data_, osqp_workspace_, constraints_sparse_, lower_bound, upper_bound))
+ {
+ RCLCPP_ERROR_THROTTLE(getLogger(), *node_->get_clock(), 1000,
+ "failed to set osqp_update_bounds. Make sure the robot's acceleration limits are valid");
+ return false;
+ }
+
+ if (positions_offset_.norm() < COMMAND_DIFFERENCE_THRESHOLD &&
+ velocities_offset_.norm() < COMMAND_DIFFERENCE_THRESHOLD)
+ {
+ positions = last_positions_;
+ velocities = last_velocities_;
+ }
+ else if (osqp_solve(osqp_workspace_) == 0 &&
+ osqp_workspace_->solution->x[0] >= ALPHA_LOWER_BOUND - osqp_settings_.eps_abs &&
+ osqp_workspace_->solution->x[0] <= ALPHA_UPPER_BOUND + osqp_settings_.eps_abs)
+ {
+ double alpha = osqp_workspace_->solution->x[0];
+ positions = alpha * last_positions_ + (1.0 - alpha) * positions.eval();
+ velocities = (positions - last_positions_) / update_period;
+ }
+ else
+ {
+ auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name);
+ auto joint_bounds = joint_model_group->getActiveJointModelsBounds();
+ cur_acceleration_ = -(last_velocities_) / update_period;
+ cur_acceleration_ *= jointLimitAccelerationScalingFactor(cur_acceleration_, joint_bounds);
+ velocities = last_velocities_ + cur_acceleration_ * update_period;
+ positions = last_positions_ + velocities * update_period;
+ }
+
+ last_velocities_ = velocities;
+ last_positions_ = positions;
+
+ return true;
+}
+
+bool AccelerationLimitedPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+ const Eigen::VectorXd& /* unused */)
+{
+ last_velocities_ = velocities;
+ last_positions_ = positions;
+ cur_acceleration_ = Eigen::VectorXd::Zero(num_joints_);
+
+ return true;
+}
+
+} // namespace online_signal_smoothing
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::AccelerationLimitedPlugin, online_signal_smoothing::SmoothingBaseClass)
diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml b/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml
new file mode 100644
index 0000000000..92575e5059
--- /dev/null
+++ b/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml
@@ -0,0 +1,16 @@
+online_signal_smoothing:
+ update_period: {
+ type: double,
+ description: "The expected time in seconds between calls to `doSmoothing` method.",
+ read_only: true,
+ validation: {
+ gt<>: 0.0
+ }
+ }
+ planning_group_name: {
+ type: string,
+ read_only: true,
+ description: "The name of the MoveIt planning group of the robot \
+ This parameter does not have a default value and \
+ must be passed to the node during launch time."
+ }
diff --git a/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp
new file mode 100644
index 0000000000..aeff731f2a
--- /dev/null
+++ b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp
@@ -0,0 +1,207 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of PickNik Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include
+#include
+#include
+
+constexpr std::string_view PLANNING_GROUP_NAME = "panda_arm";
+constexpr size_t PANDA_NUM_JOINTS = 7u;
+constexpr std::string_view ROBOT_MODEL = "panda";
+
+class AccelerationFilterTest : public testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ robot_model_ = moveit::core::loadTestingRobotModel(ROBOT_MODEL.data());
+ };
+
+ void setLimits(std::optional acceleration_limits)
+ {
+ const std::vector joint_models = robot_model_->getJointModels();
+ auto joint_model_group = robot_model_->getJointModelGroup(PLANNING_GROUP_NAME.data());
+ size_t ind = 0;
+ for (auto& joint_model : joint_models)
+ {
+ if (!joint_model_group->hasJointModel(joint_model->getName()))
+ {
+ continue;
+ }
+ std::vector joint_bounds_msg(joint_model->getVariableBoundsMsg());
+ for (auto& joint_bound : joint_bounds_msg)
+ {
+ joint_bound.has_acceleration_limits = acceleration_limits.has_value();
+ if (joint_bound.has_acceleration_limits)
+ {
+ joint_bound.max_acceleration = acceleration_limits.value()[ind];
+ }
+ }
+ joint_model->setVariableBounds(joint_bounds_msg);
+ ind++;
+ }
+ }
+
+protected:
+ moveit::core::RobotModelPtr robot_model_;
+};
+
+TEST_F(AccelerationFilterTest, FilterInitialize)
+{
+ online_signal_smoothing::AccelerationLimitedPlugin filter;
+ rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest");
+
+ // fail because the update_period parameter is not set
+ EXPECT_THROW(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS),
+ rclcpp::exceptions::ParameterUninitializedException);
+
+ node = std::make_shared("AccelerationFilterTest");
+ node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data());
+ node->declare_parameter("update_period", 0.01);
+
+ // fail because the number of joints is wrong
+ EXPECT_FALSE(filter.initialize(node, robot_model_, 3u));
+
+ // fail because the robot does not have acceleration limits
+ setLimits({});
+ EXPECT_FALSE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS));
+
+ // succeed with acceleration limits
+ Eigen::VectorXd acceleration_limits = 1.2 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS);
+ setLimits(acceleration_limits);
+ EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS));
+}
+
+TEST_F(AccelerationFilterTest, FilterDoSmooth)
+{
+ online_signal_smoothing::AccelerationLimitedPlugin filter;
+
+ rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest");
+ node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data());
+ const double update_period = 1.0;
+ node->declare_parameter("update_period", update_period);
+ Eigen::VectorXd acceleration_limits = 1.2 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS);
+ setLimits(acceleration_limits);
+
+ EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS));
+
+ // fail when called with the wrong number of joints
+ Eigen::VectorXd position = Eigen::VectorXd::Zero(5);
+ Eigen::VectorXd velocity = Eigen::VectorXd::Zero(5);
+ Eigen::VectorXd acceleration = Eigen::VectorXd::Zero(5);
+ EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration));
+
+ // fail because reset was not called
+ position = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS);
+ velocity = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS);
+ acceleration = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS);
+ EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration));
+
+ // succeed
+ filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS),
+ Eigen::VectorXd::Zero(PANDA_NUM_JOINTS));
+ EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration));
+
+ // succeed: apply acceleration limits
+ filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS),
+ Eigen::VectorXd::Zero(PANDA_NUM_JOINTS));
+ position.array() = 3.0;
+ EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration));
+ EXPECT_TRUE((position.array() - update_period * update_period * acceleration_limits.array()).matrix().norm() < 1E-3);
+
+ // succeed: apply acceleration limits
+ position.array() = 0.9;
+ filter.reset(position * 0, velocity * 0, acceleration * 0);
+ EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration));
+ EXPECT_TRUE((position.array() - 0.9).matrix().norm() < 1E-3);
+
+ // succeed: slow down
+ velocity = 10 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS);
+ filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), velocity, Eigen::VectorXd::Zero(PANDA_NUM_JOINTS));
+ position.array() = 0.01;
+ Eigen::VectorXd expected_offset =
+ velocity.array() * update_period - update_period * update_period * acceleration_limits.array();
+ EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration));
+ EXPECT_TRUE((velocity * update_period - expected_offset).norm() < 1E-3);
+}
+
+TEST_F(AccelerationFilterTest, FilterBadAccelerationConfig)
+{
+ online_signal_smoothing::AccelerationLimitedPlugin filter;
+
+ rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest");
+ node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data());
+ const double update_period = 0.1;
+ node->declare_parameter("update_period", update_period);
+ Eigen::VectorXd acceleration_limits = -1.0 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS);
+ setLimits(acceleration_limits);
+ EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS));
+ Eigen::VectorXd position = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS);
+ Eigen::VectorXd velocity = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS);
+ Eigen::VectorXd acceleration = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS);
+ EXPECT_TRUE(filter.reset(position, velocity, acceleration));
+ EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration));
+}
+
+TEST_F(AccelerationFilterTest, FilterDoSmoothRandomized)
+{
+ online_signal_smoothing::AccelerationLimitedPlugin filter;
+ rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest");
+ const double update_period = 0.1;
+ node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data());
+ node->declare_parameter("update_period", update_period);
+ Eigen::VectorXd acceleration_limits = 1.2 * (1.0 + Eigen::VectorXd::Random(PANDA_NUM_JOINTS).array());
+ setLimits(acceleration_limits);
+ EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS));
+
+ for (size_t iter = 0; iter < 64; ++iter)
+ {
+ acceleration_limits = 1.2 * (1.0 + Eigen::VectorXd::Random(PANDA_NUM_JOINTS).array());
+ setLimits(acceleration_limits);
+ EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS));
+ filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS),
+ Eigen::VectorXd::Zero(PANDA_NUM_JOINTS));
+ Eigen::VectorXd position = Eigen::VectorXd::Random(PANDA_NUM_JOINTS);
+ Eigen::VectorXd velocity = Eigen::VectorXd::Random(PANDA_NUM_JOINTS);
+ Eigen::VectorXd acceleration = Eigen::VectorXd::Random(PANDA_NUM_JOINTS);
+ EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration));
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/moveit_core/package.xml b/moveit_core/package.xml
index 2b22cc7cab..76dd4b5596 100644
--- a/moveit_core/package.xml
+++ b/moveit_core/package.xml
@@ -2,7 +2,7 @@
moveit_core
- 2.9.0
+ 2.10.0
Core libraries used by MoveIt
Henning Kayser
@@ -13,8 +13,8 @@
BSD-3-Clause
http://moveit.ros.org
- https://github.com/ros-planning/moveit2/issues
- https://github.com/ros-planning/moveit2
+ https://github.com/moveit/moveit2/issues
+ https://github.com/moveit/moveit2
Ioan Sucan
Sachin Chitta
@@ -31,8 +31,8 @@
boost
bullet
common_interfaces
- eigen_stl_containers
eigen
+ eigen_stl_containers
generate_parameter_library
geometric_shapes
geometry_msgs
@@ -41,8 +41,15 @@
libfcl-dev
moveit_common
moveit_msgs
- octomap_msgs
+
+ octomap_msgs
+ osqp_vendor
pluginlib
random_numbers
rclcpp
@@ -52,14 +59,14 @@
shape_msgs
srdfdom
std_msgs
+ tf2
tf2_eigen
tf2_geometry_msgs
tf2_kdl
- tf2
trajectory_msgs
urdf
- urdfdom_headers
urdfdom
+ urdfdom_headers
visualization_msgs
python3-sphinx-rtd-theme
@@ -76,9 +83,6 @@
rclpy
rcl_interfaces
- ament_lint_auto
- ament_lint_common
-
ament_cmake
diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt
index 46e7ff7789..338765e2f9 100644
--- a/moveit_core/planning_interface/CMakeLists.txt
+++ b/moveit_core/planning_interface/CMakeLists.txt
@@ -1,23 +1,14 @@
-add_library(moveit_planning_interface SHARED
- src/planning_interface.cpp
- src/planning_response.cpp
-)
-target_include_directories(moveit_planning_interface PUBLIC
- $
- $
-)
-set_target_properties(moveit_planning_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_planning_interface
- moveit_msgs
- urdf
- urdfdom
- urdfdom_headers
-)
-target_link_libraries(moveit_planning_interface
- moveit_robot_trajectory
- moveit_robot_state
- moveit_planning_scene
- moveit_utils
-)
+add_library(moveit_planning_interface SHARED src/planning_interface.cpp
+ src/planning_response.cpp)
+target_include_directories(
+ moveit_planning_interface
+ PUBLIC $
+ $)
+set_target_properties(moveit_planning_interface
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(moveit_planning_interface moveit_msgs urdf urdfdom
+ urdfdom_headers)
+target_link_libraries(moveit_planning_interface moveit_robot_trajectory
+ moveit_robot_state moveit_planning_scene moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp
index 7e37b942b8..67ec7fe7d5 100644
--- a/moveit_core/planning_interface/src/planning_interface.cpp
+++ b/moveit_core/planning_interface/src/planning_interface.cpp
@@ -47,7 +47,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("planning_interface");
+ return moveit::getLogger("moveit.core.planning_interface");
}
} // namespace
diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt
index 9eb48ce44e..debb3e5b65 100644
--- a/moveit_core/planning_scene/CMakeLists.txt
+++ b/moveit_core/planning_scene/CMakeLists.txt
@@ -1,25 +1,26 @@
-add_library(moveit_planning_scene SHARED
- src/planning_scene.cpp
-)
-target_include_directories(moveit_planning_scene PUBLIC
- $
- $
-)
+add_library(moveit_planning_scene SHARED src/planning_scene.cpp)
+target_include_directories(
+ moveit_planning_scene
+ PUBLIC $
+ $)
include(GenerateExportHeader)
generate_export_header(moveit_planning_scene)
-target_include_directories(moveit_planning_scene PUBLIC $)
-#TODO: Fix the versioning
-set_target_properties(moveit_planning_scene PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_planning_scene
+target_include_directories(
+ moveit_planning_scene PUBLIC $)
+# TODO: Fix the versioning
+set_target_properties(moveit_planning_scene
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(
+ moveit_planning_scene
Boost
rclcpp
urdfdom
urdfdom_headers
octomap_msgs
- OCTOMAP
-)
+ octomap)
-target_link_libraries(moveit_planning_scene
+target_link_libraries(
+ moveit_planning_scene
moveit_robot_model
moveit_robot_state
moveit_exceptions
@@ -29,32 +30,34 @@ target_link_libraries(moveit_planning_scene
moveit_kinematic_constraints
moveit_robot_trajectory
moveit_trajectory_processing
- moveit_utils
-)
+ moveit_utils)
install(DIRECTORY include/ DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_export.h DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_export.h
+ DESTINATION include/moveit_core)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
if(UNIX OR APPLE)
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_BINARY_DIR}/../utils:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_BINARY_DIR}/../utils:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection"
+ )
endif()
ament_add_gtest(test_planning_scene test/test_planning_scene.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
- ament_target_dependencies(test_planning_scene
- geometric_shapes
- srdfdom
- )
- target_link_libraries(test_planning_scene moveit_test_utils moveit_planning_scene)
+ APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}")
+ ament_target_dependencies(test_planning_scene geometric_shapes srdfdom)
+ target_link_libraries(test_planning_scene moveit_test_utils
+ moveit_planning_scene)
ament_add_gtest(test_collision_objects test/test_collision_objects.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
- target_link_libraries(test_collision_objects moveit_test_utils moveit_planning_scene)
+ APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}")
+ target_link_libraries(test_collision_objects moveit_test_utils
+ moveit_planning_scene)
ament_add_gtest(test_multi_threaded test/test_multi_threaded.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
- target_link_libraries(test_multi_threaded moveit_test_utils moveit_planning_scene)
+ APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}")
+ target_link_libraries(test_multi_threaded moveit_test_utils
+ moveit_planning_scene)
endif()
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 155b703420..3d71a117e7 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
@@ -56,6 +56,7 @@
#include
#include
#include
+#include
#include
#include
@@ -159,7 +160,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
const moveit::core::RobotState& getCurrentState() const
{
// if we have an updated state, return it; otherwise, return the parent one
- return robot_state_ ? *robot_state_ : parent_->getCurrentState();
+ return robot_state_.has_value() ? robot_state_.value() : parent_->getCurrentState();
}
/** \brief Get the state at which the robot is assumed to be. */
moveit::core::RobotState& getCurrentStateNonConst();
@@ -176,15 +177,15 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
const std::string& getPlanningFrame() const
{
// if we have an updated set of transforms, return it; otherwise, return the parent one
- return scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame();
+ return scene_transforms_.has_value() ? scene_transforms_.value()->getTargetFrame() : parent_->getPlanningFrame();
}
/** \brief Get the set of fixed transforms from known frames to the planning frame */
const moveit::core::Transforms& getTransforms() const
{
- if (scene_transforms_ || !parent_)
+ if (scene_transforms_.has_value() || !parent_)
{
- return *scene_transforms_;
+ return *scene_transforms_.value();
}
// if this planning scene is a child of another, and doesn't have its own custom transforms
@@ -290,7 +291,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
/** \brief Get the allowed collision matrix */
const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const
{
- return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
+ return acm_.has_value() ? acm_.value() : parent_->getAllowedCollisionMatrix();
}
/** \brief Get the allowed collision matrix */
@@ -1011,14 +1012,14 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
- moveit::core::RobotStatePtr robot_state_; // if nullptr use parent's
+ std::optional robot_state_; // if there is no value use parent's
// Called when changes are made to attached bodies
moveit::core::AttachedBodyCallback current_state_attached_body_callback_;
// This variable is not necessarily used by child planning scenes
// This Transforms class is actually a SceneTransforms class
- moveit::core::TransformsPtr scene_transforms_; // if nullptr use parent's
+ std::optional scene_transforms_; // if there is no value use parent's
collision_detection::WorldPtr world_; // never nullptr, never shared with parent/child
collision_detection::WorldConstPtr world_const_; // copy of world_
@@ -1028,7 +1029,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
CollisionDetectorPtr collision_detector_; // Never nullptr.
- collision_detection::AllowedCollisionMatrixPtr acm_; // if nullptr use parent's
+ std::optional acm_; // if there is no value use parent's
StateFeasibilityFn state_feasibility_;
MotionFeasibilityFn motion_feasibility_;
@@ -1038,6 +1039,6 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
std::unique_ptr original_object_colors_;
// a map of object types
- std::unique_ptr object_types_;
+ std::optional object_types_;
};
} // namespace planning_scene
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index d75e0707f2..908a053df8 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -59,7 +59,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("planning_scene");
+ return moveit::getLogger("moveit.core.planning_scene");
}
} // namespace
@@ -189,13 +189,13 @@ void PlanningScene::initialize()
{
name_ = DEFAULT_SCENE_NAME;
- scene_transforms_ = std::make_shared(this);
+ scene_transforms_.emplace(std::make_shared(this));
- robot_state_ = std::make_shared(robot_model_);
- robot_state_->setToDefaultValues();
- robot_state_->update();
+ robot_state_.emplace(moveit::core::RobotState(robot_model_));
+ robot_state_.value().setToDefaultValues();
+ robot_state_.value().update();
- acm_ = std::make_shared(*getRobotModel()->getSRDF());
+ acm_.emplace(collision_detection::AllowedCollisionMatrix(*getRobotModel()->getSRDF()));
allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
}
@@ -336,15 +336,15 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
if (!parent_)
return;
- if (scene_transforms_)
- scene->getTransformsNonConst().setAllTransforms(scene_transforms_->getAllTransforms());
+ if (scene_transforms_.has_value())
+ scene->getTransformsNonConst().setAllTransforms(scene_transforms_.value()->getAllTransforms());
- if (robot_state_)
+ if (robot_state_.has_value())
{
- scene->getCurrentStateNonConst() = *robot_state_;
+ scene->getCurrentStateNonConst() = robot_state_.value();
// push colors and types for attached objects
std::vector attached_objs;
- robot_state_->getAttachedBodies(attached_objs);
+ robot_state_.value().getAttachedBodies(attached_objs);
for (const moveit::core::AttachedBody* attached_obj : attached_objs)
{
if (hasObjectType(attached_obj->getName()))
@@ -354,8 +354,8 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
}
}
- if (acm_)
- scene->getAllowedCollisionMatrixNonConst() = *acm_;
+ if (acm_.has_value())
+ scene->getAllowedCollisionMatrixNonConst() = acm_.value();
collision_detection::CollisionEnvPtr active_cenv = scene->getCollisionEnvNonConst();
active_cenv->setLinkPadding(collision_detector_->cenv_->getLinkPadding());
@@ -526,13 +526,13 @@ const collision_detection::CollisionEnvPtr& PlanningScene::getCollisionEnvNonCon
moveit::core::RobotState& PlanningScene::getCurrentStateNonConst()
{
- if (!robot_state_)
+ if (!robot_state_.has_value())
{
- robot_state_ = std::make_shared(parent_->getCurrentState());
- robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
+ robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState()));
+ robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
}
- robot_state_->update();
- return *robot_state_;
+ robot_state_.value().update();
+ return robot_state_.value();
}
moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const
@@ -545,8 +545,8 @@ moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_m
void PlanningScene::setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback)
{
current_state_attached_body_callback_ = callback;
- if (robot_state_)
- robot_state_->setAttachedBodyUpdateCallback(callback);
+ if (robot_state_.has_value())
+ robot_state_.value().setAttachedBodyUpdateCallback(callback);
}
void PlanningScene::setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback)
@@ -560,9 +560,9 @@ void PlanningScene::setCollisionObjectUpdateCallback(const collision_detection::
collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionMatrixNonConst()
{
- if (!acm_)
- acm_ = std::make_shared(parent_->getAllowedCollisionMatrix());
- return *acm_;
+ if (!acm_.has_value())
+ acm_.emplace(collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
+ return acm_.value();
}
void PlanningScene::setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm)
@@ -581,14 +581,14 @@ moveit::core::Transforms& PlanningScene::getTransformsNonConst()
{
// Trigger an update of the robot transforms
getCurrentStateNonConst().update();
- if (!scene_transforms_)
+ if (!scene_transforms_.has_value())
{
// The only case when there are no transforms is if this planning scene has a parent. When a non-const version of
// the planning scene is requested, a copy of the parent's transforms is forced
- scene_transforms_ = std::make_shared(this);
- scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
+ scene_transforms_.emplace(std::make_shared(this));
+ scene_transforms_.value()->setAllTransforms(parent_->getTransforms().getAllTransforms());
}
- return *scene_transforms_;
+ return *scene_transforms_.value();
}
void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& scene_msg) const
@@ -597,18 +597,18 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce
scene_msg.robot_model_name = getRobotModel()->getName();
scene_msg.is_diff = true;
- if (scene_transforms_)
+ if (scene_transforms_.has_value())
{
- scene_transforms_->copyTransforms(scene_msg.fixed_frame_transforms);
+ scene_transforms_.value()->copyTransforms(scene_msg.fixed_frame_transforms);
}
else
{
scene_msg.fixed_frame_transforms.clear();
}
- if (robot_state_)
+ if (robot_state_.has_value())
{
- moveit::core::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state);
+ moveit::core::robotStateToRobotStateMsg(robot_state_.value(), scene_msg.robot_state);
}
else
{
@@ -616,9 +616,9 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce
}
scene_msg.robot_state.is_diff = true;
- if (acm_)
+ if (acm_.has_value())
{
- acm_->getMessage(scene_msg.allowed_collision_matrix);
+ acm_.value().getMessage(scene_msg.allowed_collision_matrix);
}
else
{
@@ -1118,15 +1118,15 @@ void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state)
if (parent_)
{
- if (!robot_state_)
+ if (!robot_state_.has_value())
{
- robot_state_ = std::make_shared(parent_->getCurrentState());
- robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
+ robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState()));
+ robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
}
- moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, *robot_state_);
+ moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, robot_state_.value());
}
else
- moveit::core::robotStateMsgToRobotState(*scene_transforms_, state_no_attached, *robot_state_);
+ moveit::core::robotStateMsgToRobotState(*scene_transforms_.value(), state_no_attached, robot_state_.value());
for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
{
@@ -1153,20 +1153,20 @@ void PlanningScene::decoupleParent()
return;
// This child planning scene did not have its own copy of frame transforms
- if (!scene_transforms_)
+ if (!scene_transforms_.has_value())
{
- scene_transforms_ = std::make_shared(this);
- scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
+ scene_transforms_.emplace(std::make_shared(this));
+ scene_transforms_.value()->setAllTransforms(parent_->getTransforms().getAllTransforms());
}
- if (!robot_state_)
+ if (!robot_state_.has_value())
{
- robot_state_ = std::make_shared(parent_->getCurrentState());
- robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
+ robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState()));
+ robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
}
- if (!acm_)
- acm_ = std::make_shared(parent_->getAllowedCollisionMatrix());
+ if (!acm_.has_value())
+ acm_.emplace(collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
world_diff_.reset();
@@ -1187,11 +1187,11 @@ void PlanningScene::decoupleParent()
}
}
- if (!object_types_)
+ if (!object_types_.has_value())
{
ObjectTypeMap kc;
parent_->getKnownObjectTypes(kc);
- object_types_ = std::make_unique(kc);
+ object_types_.emplace(ObjectTypeMap(kc));
}
else
{
@@ -1199,8 +1199,8 @@ void PlanningScene::decoupleParent()
parent_->getKnownObjectTypes(kc);
for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
{
- if (object_types_->find(it->first) == object_types_->end())
- (*object_types_)[it->first] = it->second;
+ if (object_types_.value().find(it->first) == object_types_.value().end())
+ (object_types_.value())[it->first] = it->second;
}
}
@@ -1225,9 +1225,9 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen
// if the list is empty, then nothing has been set
if (!scene_msg.fixed_frame_transforms.empty())
{
- if (!scene_transforms_)
- scene_transforms_ = std::make_shared(this);
- scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
+ if (!scene_transforms_.has_value())
+ scene_transforms_.emplace(std::make_shared(this));
+ scene_transforms_.value()->setTransforms(scene_msg.fixed_frame_transforms);
}
// if at least some joints have been specified, we set them
@@ -1237,7 +1237,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen
// if at least some links are mentioned in the allowed collision matrix, then we have an update
if (!scene_msg.allowed_collision_matrix.entry_names.empty())
- acm_ = std::make_shared(scene_msg.allowed_collision_matrix);
+ acm_.emplace(collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
{
@@ -1275,9 +1275,9 @@ bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& s
decoupleParent();
object_types_.reset();
- scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
+ scene_transforms_.value()->setTransforms(scene_msg.fixed_frame_transforms);
setCurrentState(scene_msg.robot_state);
- acm_ = std::make_shared(scene_msg.allowed_collision_matrix);
+ acm_.emplace(collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
collision_detector_->cenv_->setPadding(scene_msg.link_padding);
collision_detector_->cenv_->setScale(scene_msg.link_scale);
object_colors_ = std::make_unique();
@@ -1442,12 +1442,12 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
return false;
}
- if (!robot_state_) // there must be a parent in this case
+ if (!robot_state_.has_value()) // there must be a parent in this case
{
- robot_state_ = std::make_shared(parent_->getCurrentState());
- robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
+ robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState()));
+ robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
}
- robot_state_->update();
+ robot_state_.value().update();
// The ADD/REMOVE operations follow this order:
// STEP 1: Get info about the object from either the message or the world/RobotState
@@ -1478,7 +1478,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
RCLCPP_DEBUG(getLogger(), "Attaching world object '%s' to link '%s'", object.object.id.c_str(),
object.link_name.c_str());
- object_pose_in_link = robot_state_->getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_;
+ object_pose_in_link = robot_state_.value().getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_;
shapes = obj_in_world->shapes_;
shape_poses = obj_in_world->shape_poses_;
subframe_poses = obj_in_world->subframe_poses_;
@@ -1499,7 +1499,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
return false;
const Eigen::Isometry3d world_to_header_frame = getFrameTransform(object.object.header.frame_id);
const Eigen::Isometry3d link_to_header_frame =
- robot_state_->getGlobalLinkTransform(link_model).inverse() * world_to_header_frame;
+ robot_state_.value().getGlobalLinkTransform(link_model).inverse() * world_to_header_frame;
object_pose_in_link = link_to_header_frame * header_frame_to_object_pose;
Eigen::Isometry3d subframe_pose;
@@ -1540,23 +1540,23 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
// STEP 3: Attach the object to the robot
if (object.object.operation == moveit_msgs::msg::CollisionObject::ADD ||
- !robot_state_->hasAttachedBody(object.object.id))
+ !robot_state_.value().hasAttachedBody(object.object.id))
{
- if (robot_state_->clearAttachedBody(object.object.id))
+ if (robot_state_.value().clearAttachedBody(object.object.id))
{
RCLCPP_DEBUG(getLogger(),
"The robot state already had an object named '%s' attached to link '%s'. "
"The object was replaced.",
object.object.id.c_str(), object.link_name.c_str());
}
- robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links,
- object.link_name, object.detach_posture, subframe_poses);
+ robot_state_.value().attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links,
+ object.link_name, object.detach_posture, subframe_poses);
RCLCPP_DEBUG(getLogger(), "Attached object '%s' to link '%s'", object.object.id.c_str(),
object.link_name.c_str());
}
else // APPEND: augment to existing attached object
{
- const moveit::core::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id);
+ const moveit::core::AttachedBody* ab = robot_state_.value().getAttachedBody(object.object.id);
// Allow overriding the body's pose if provided, otherwise keep the old one
if (moveit::core::isEmpty(object.object.pose))
@@ -1572,9 +1572,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
touch_links.insert(std::make_move_iterator(object.touch_links.begin()),
std::make_move_iterator(object.touch_links.end()));
- robot_state_->clearAttachedBody(object.object.id);
- robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links,
- object.link_name, detach_posture, subframe_poses);
+ robot_state_.value().clearAttachedBody(object.object.id);
+ robot_state_.value().attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links,
+ object.link_name, detach_posture, subframe_poses);
RCLCPP_DEBUG(getLogger(), "Appended things to object '%s' attached to link '%s'", object.object.id.c_str(),
object.link_name.c_str());
}
@@ -1595,16 +1595,16 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
object.link_name.empty() ? nullptr : getRobotModel()->getLinkModel(object.link_name);
if (link_model)
{ // if we have a link model specified, only fetch bodies attached to this link
- robot_state_->getAttachedBodies(attached_bodies, link_model);
+ robot_state_.value().getAttachedBodies(attached_bodies, link_model);
}
else
{
- robot_state_->getAttachedBodies(attached_bodies);
+ robot_state_.value().getAttachedBodies(attached_bodies);
}
}
else // A specific object id will be removed.
{
- const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id);
+ const moveit::core::AttachedBody* body = robot_state_.value().getAttachedBody(object.object.id);
if (body)
{
if (!object.link_name.empty() && (body->getAttachedLinkName() != object.link_name))
@@ -1648,7 +1648,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At
name.c_str(), object.link_name.c_str());
}
- robot_state_->clearAttachedBody(name);
+ robot_state_.value().clearAttachedBody(name);
}
if (!attached_bodies.empty() || object.object.id.empty())
return true;
@@ -1923,9 +1923,9 @@ bool PlanningScene::knowsFrameTransform(const moveit::core::RobotState& state, c
bool PlanningScene::hasObjectType(const std::string& object_id) const
{
- if (object_types_)
+ if (object_types_.has_value())
{
- if (object_types_->find(object_id) != object_types_->end())
+ if (object_types_.value().find(object_id) != object_types_.value().end())
return true;
}
if (parent_)
@@ -1935,10 +1935,10 @@ bool PlanningScene::hasObjectType(const std::string& object_id) const
const object_recognition_msgs::msg::ObjectType& PlanningScene::getObjectType(const std::string& object_id) const
{
- if (object_types_)
+ if (object_types_.has_value())
{
- ObjectTypeMap::const_iterator it = object_types_->find(object_id);
- if (it != object_types_->end())
+ ObjectTypeMap::const_iterator it = object_types_.value().find(object_id);
+ if (it != object_types_.value().end())
return it->second;
}
if (parent_)
@@ -1949,15 +1949,15 @@ const object_recognition_msgs::msg::ObjectType& PlanningScene::getObjectType(con
void PlanningScene::setObjectType(const std::string& object_id, const object_recognition_msgs::msg::ObjectType& type)
{
- if (!object_types_)
- object_types_ = std::make_unique();
- (*object_types_)[object_id] = type;
+ if (!object_types_.has_value())
+ object_types_.emplace(ObjectTypeMap());
+ (object_types_.value())[object_id] = type;
}
void PlanningScene::removeObjectType(const std::string& object_id)
{
- if (object_types_)
- object_types_->erase(object_id);
+ if (object_types_.has_value())
+ object_types_.value().erase(object_id);
}
void PlanningScene::getKnownObjectTypes(ObjectTypeMap& kc) const
@@ -1965,10 +1965,10 @@ void PlanningScene::getKnownObjectTypes(ObjectTypeMap& kc) const
kc.clear();
if (parent_)
parent_->getKnownObjectTypes(kc);
- if (object_types_)
+ if (object_types_.has_value())
{
- for (ObjectTypeMap::const_iterator it = object_types_->begin(); it != object_types_->end(); ++it)
- kc[it->first] = it->second;
+ for (const auto& it : object_types_.value())
+ kc[it.first] = it.second;
}
}
diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt
index 0d11decc6f..1a1c59fb49 100644
--- a/moveit_core/robot_model/CMakeLists.txt
+++ b/moveit_core/robot_model/CMakeLists.txt
@@ -1,4 +1,5 @@
-add_library(moveit_robot_model SHARED
+add_library(
+ moveit_robot_model SHARED
src/aabb.cpp
src/fixed_joint_model.cpp
src/floating_joint_model.cpp
@@ -8,16 +9,27 @@ add_library(moveit_robot_model SHARED
src/planar_joint_model.cpp
src/prismatic_joint_model.cpp
src/revolute_joint_model.cpp
- src/robot_model.cpp
-)
-target_include_directories(moveit_robot_model PUBLIC
- $
- $ # Work around cyclic dependency between this and kinematics base
- $ # Ditto but for finding the export header
- $
-)
-set_target_properties(moveit_robot_model PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(moveit_robot_model
+ src/robot_model.cpp)
+target_include_directories(
+ moveit_robot_model
+ PUBLIC
+ $
+ $ # Work
+ # around
+ # cyclic
+ # dependency
+ # between
+ # this and
+ # kinematics
+ # base
+ $ # Ditto but for
+ # finding the
+ # export header
+ $)
+set_target_properties(moveit_robot_model
+ PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(
+ moveit_robot_model
angles
moveit_msgs
Eigen3
@@ -25,20 +37,14 @@ ament_target_dependencies(moveit_robot_model
urdf
urdfdom_headers
srdfdom
- visualization_msgs
-)
-target_link_libraries(moveit_robot_model
- moveit_exceptions
- moveit_macros
- moveit_utils
-)
+ visualization_msgs)
+target_link_libraries(moveit_robot_model moveit_exceptions moveit_macros
+ moveit_utils)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_robot_model test/test.cpp)
- ament_target_dependencies(test_robot_model
- rclcpp
- )
+ ament_target_dependencies(test_robot_model rclcpp)
target_link_libraries(test_robot_model moveit_test_utils moveit_robot_model)
endif()
diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
index 65112176a4..c63112fde4 100644
--- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
+++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
@@ -406,7 +406,7 @@ class JointModelGroup
void interpolate(const double* from, const double* to, double t, double* state) const;
/** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic
- joints, so will always be >= the number of items returned by getActiveVariableNames() */
+ joints, so will always be >= getActiveVariableCount() */
unsigned int getVariableCount() const
{
return variable_count_;
@@ -586,6 +586,21 @@ class JointModelGroup
bool computeJointVariableIndices(const std::vector& joint_names,
std::vector& joint_bijection) const;
+ /**
+ * @brief Get the lower and upper position limits of all active variables in the group.
+ *
+ * @return std::pair Containing the lower and upper joint limits for all active variables.
+ */
+ [[nodiscard]] std::pair getLowerAndUpperLimits() const;
+
+ /**
+ * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains
+ * only single-variable joints,
+ * @details In case of asymmetric velocity or acceleration limits, this function will return the most limiting component.
+ * @return std::pair Containing the velocity and acceleration limits
+ */
+ [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds() const;
+
protected:
/** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates
mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group,
diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp
index 94fb8d2f6e..776ed5efd9 100644
--- a/moveit_core/robot_model/src/floating_joint_model.cpp
+++ b/moveit_core/robot_model/src/floating_joint_model.cpp
@@ -54,7 +54,7 @@ constexpr size_t STATE_SPACE_DIMENSION = 7;
rclcpp::Logger getLogger()
{
- return moveit::getLogger("floating_joint_model");
+ return moveit::getLogger("moveit.core.floating_joint_model");
}
} // namespace
diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp
index dc9bfb39a5..2776bb0c9d 100644
--- a/moveit_core/robot_model/src/joint_model_group.cpp
+++ b/moveit_core/robot_model/src/joint_model_group.cpp
@@ -54,7 +54,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("joint_model_group");
+ return moveit::getLogger("moveit.core.joint_model_group");
}
// check if a parent or ancestor of joint is included in this group
@@ -229,6 +229,14 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode
{
link_model_map_[link_model->getName()] = link_model;
link_model_name_vector_.push_back(link_model->getName());
+ // if this is the first link of the group with a valid parent and includes geometry (for example `base_link`) it should included
+ if (link_model_with_geometry_vector_.empty() && link_model->getParentLinkModel() &&
+ !link_model->getParentLinkModel()->getShapes().empty())
+ {
+ link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel());
+ link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName());
+ }
+ // all child links with collision geometry should also be included
if (!link_model->getShapes().empty())
{
link_model_with_geometry_vector_.push_back(link_model);
@@ -823,5 +831,56 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d
return true;
}
+
+std::pair JointModelGroup::getLowerAndUpperLimits() const
+{
+ // Get the group joints lower/upper position limits.
+ Eigen::VectorXd lower_limits(active_variable_count_);
+ Eigen::VectorXd upper_limits(active_variable_count_);
+ int variable_index = 0;
+ for (const moveit::core::JointModel::Bounds* joint_bounds : active_joint_models_bounds_)
+ {
+ for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds)
+ {
+ lower_limits[variable_index] = variable_bounds.min_position_;
+ upper_limits[variable_index] = variable_bounds.max_position_;
+ variable_index++;
+ }
+ }
+ return { lower_limits, upper_limits };
+}
+
+std::pair JointModelGroup::getMaxVelocitiesAndAccelerationBounds() const
+{
+ Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(active_variable_count_, 0.0);
+ Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(active_variable_count_, 0.0);
+ // Check if variable count matches number of joint model bounds
+ if (active_joint_models_bounds_.size() != active_variable_count_)
+ {
+ // TODO(sjahr) Support multiple variables
+ RCLCPP_ERROR(getLogger(), "Number of active joint models does not match number of active joint model bounds. "
+ "Returning bound vectors with zeros");
+ return { max_joint_velocities, max_joint_accelerations };
+ }
+ // Check if the joint group contains multi-dof joints
+ for (const auto& bound : active_joint_models_bounds_)
+ {
+ if (bound->size() != 1)
+ {
+ RCLCPP_ERROR(getLogger(), "Multi-dof joints are currently not supported by "
+ "getMaxVelocitiesAndAccelerationBounds(). Returning bound vectors with zeros.");
+ return { max_joint_velocities, max_joint_accelerations };
+ }
+ }
+ // Populate max_joint_velocity and acceleration vectors
+ for (std::size_t i = 0; i < active_joint_models_bounds_.size(); ++i)
+ {
+ max_joint_velocities[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_velocity_,
+ active_joint_models_bounds_[i]->at(0).max_velocity_);
+ max_joint_accelerations[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_acceleration_,
+ active_joint_models_bounds_[i]->at(0).max_acceleration_);
+ }
+ return { max_joint_velocities, max_joint_accelerations };
+}
} // end of namespace core
} // end of namespace moveit
diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp
index e229910d2f..81e9128053 100644
--- a/moveit_core/robot_model/src/prismatic_joint_model.cpp
+++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp
@@ -79,7 +79,7 @@ void PrismaticJointModel::getVariableDefaultPositions(double* values, const Boun
bool PrismaticJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
{
- return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
+ return values[0] >= bounds[0].min_position_ - margin && values[0] <= bounds[0].max_position_ + margin;
}
void PrismaticJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp
index 1b80b4e2e7..524632159c 100644
--- a/moveit_core/robot_model/src/revolute_joint_model.cpp
+++ b/moveit_core/robot_model/src/revolute_joint_model.cpp
@@ -189,7 +189,7 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou
}
else
{
- return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
+ return values[0] >= bounds[0].min_position_ - margin && values[0] <= bounds[0].max_position_ + margin;
}
}
diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp
index 7c5536c117..f65ff86a11 100644
--- a/moveit_core/robot_model/src/robot_model.cpp
+++ b/moveit_core/robot_model/src/robot_model.cpp
@@ -54,7 +54,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("robot_model");
+ return moveit::getLogger("moveit.core.robot_model");
}
} // namespace
diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt
index 6448b21296..fad8b36830 100644
--- a/moveit_core/robot_state/CMakeLists.txt
+++ b/moveit_core/robot_state/CMakeLists.txt
@@ -1,26 +1,16 @@
-add_library(moveit_robot_state SHARED
- src/attached_body.cpp
- src/conversions.cpp
- src/robot_state.cpp
- src/cartesian_interpolator.cpp
-)
-target_include_directories(moveit_robot_state PUBLIC
- $
- $
-)
-set_target_properties(moveit_robot_state PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
-ament_target_dependencies(moveit_robot_state
- urdf
- tf2_geometry_msgs
- geometric_shapes
- urdfdom_headers
- Boost
-)
-target_link_libraries(moveit_robot_state
- moveit_robot_model
- moveit_kinematics_base
- moveit_transforms
-)
+add_library(
+ moveit_robot_state SHARED src/attached_body.cpp src/conversions.cpp
+ src/robot_state.cpp src/cartesian_interpolator.cpp)
+target_include_directories(
+ moveit_robot_state
+ PUBLIC $
+ $)
+set_target_properties(moveit_robot_state PROPERTIES VERSION
+ ${${PROJECT_NAME}_VERSION})
+ament_target_dependencies(moveit_robot_state urdf tf2_geometry_msgs
+ geometric_shapes urdfdom_headers Boost)
+target_link_libraries(moveit_robot_state moveit_robot_model
+ moveit_kinematics_base moveit_transforms)
install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -37,51 +27,32 @@ if(BUILD_TESTING)
if(WIN32)
# TODO add windows paths
else()
- set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils")
+ set(APPEND_LIBRARY_DIRS
+ "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils")
endif()
ament_add_gtest(test_robot_state test/robot_state_test.cpp
- APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}")
- target_link_libraries(test_robot_state
- moveit_test_utils
- moveit_utils
- moveit_exceptions
- moveit_robot_state
- )
+ target_link_libraries(test_robot_state moveit_test_utils moveit_utils
+ moveit_exceptions moveit_robot_state)
ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp)
- target_link_libraries(test_robot_state_complex
- moveit_test_utils
- moveit_utils
- moveit_exceptions
- moveit_robot_state
- )
+ target_link_libraries(test_robot_state_complex moveit_test_utils moveit_utils
+ moveit_exceptions moveit_robot_state)
ament_add_gtest(test_aabb test/test_aabb.cpp)
- target_link_libraries(test_aabb
- moveit_test_utils
- moveit_utils
- moveit_exceptions
- moveit_robot_state
- )
-
- ament_add_gtest(test_cartesian_interpolator test/test_cartesian_interpolator.cpp)
- target_link_libraries(test_cartesian_interpolator
- moveit_test_utils
- moveit_robot_state
- moveit_kinematics_base
- )
-
- ament_add_google_benchmark(
- robot_state_benchmark
- test/robot_state_benchmark.cpp)
- ament_target_dependencies(robot_state_benchmark
- kdl_parser
- )
- target_link_libraries(robot_state_benchmark
- moveit_robot_model
- moveit_test_utils
- moveit_robot_state
- )
+ target_link_libraries(test_aabb moveit_test_utils moveit_utils
+ moveit_exceptions moveit_robot_state)
+
+ ament_add_gtest(test_cartesian_interpolator
+ test/test_cartesian_interpolator.cpp)
+ target_link_libraries(test_cartesian_interpolator moveit_test_utils
+ moveit_robot_state moveit_kinematics_base)
+
+ ament_add_google_benchmark(robot_state_benchmark
+ test/robot_state_benchmark.cpp)
+ ament_target_dependencies(robot_state_benchmark kdl_parser)
+ target_link_libraries(robot_state_benchmark moveit_robot_model
+ moveit_test_utils moveit_robot_state)
endif()
diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp
index 5a9f7fbe31..02707c3481 100644
--- a/moveit_core/robot_state/src/cartesian_interpolator.cpp
+++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp
@@ -57,7 +57,7 @@ namespace
rclcpp::Logger getLogger()
{
- return moveit::getLogger("cartesian_interpolator");
+ return moveit::getLogger("moveit.core.cartesian_interpolator");
}
std::optional hasRelativeJointSpaceJump(const std::vector& waypoints,
diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp
index 50b588e16b..89538525b5 100644
--- a/moveit_core/robot_state/src/conversions.cpp
+++ b/moveit_core/robot_state/src/conversions.cpp
@@ -56,7 +56,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("conversions");
+ return moveit::getLogger("moveit.core.conversions");
}
bool jointStateToRobotStateImpl(const sensor_msgs::msg::JointState& joint_state, RobotState& state)
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index 5f7bf96010..b16373da04 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -59,7 +59,7 @@ namespace
{
rclcpp::Logger getLogger()
{
- return moveit::getLogger("robot_state");
+ return moveit::getLogger("moveit.core.robot_state");
}
} // namespace
diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp
index 8a5e8e46a3..06c40df745 100644
--- a/moveit_core/robot_state/test/robot_state_benchmark.cpp
+++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp
@@ -38,21 +38,16 @@
// To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary.
#include
+#include
#include
-#include
+#include
#include
#include
#include
-#include
// Robot and planning group for benchmarks.
constexpr char PANDA_TEST_ROBOT[] = "panda";
constexpr char PANDA_TEST_GROUP[] = "panda_arm";
-constexpr char PR2_TEST_ROBOT[] = "pr2";
-constexpr char PR2_TIP_LINK[] = "r_wrist_roll_link";
-
-// Number of iterations to use in matrix multiplication / inversion benchmarks.
-constexpr int MATRIX_OPS_N_ITERATIONS = 1e7;
namespace
{
@@ -66,267 +61,228 @@ Eigen::Isometry3d createTestIsometry()
} // namespace
// Benchmark time to multiply an Eigen::Affine3d with an Eigen::Matrix4d.
-static void multiplyAffineTimesMatrix(benchmark::State& st)
+// The NoAlias versions just use Eigen's .noalias() modifier, allowing to write the result of matrix multiplication
+// directly into the result matrix instead of using an intermediate temporary (which is the default).
+static void multiplyAffineTimesMatrixNoAlias(benchmark::State& st)
{
- int n_iters = st.range(0);
Eigen::Isometry3d isometry = createTestIsometry();
+ Eigen::Affine3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Affine3d result;
- benchmark::DoNotOptimize(result = isometry.affine() * isometry.matrix());
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result.affine().noalias() = isometry.affine() * isometry.matrix());
+ benchmark::ClobberMemory();
}
}
// Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d.
-static void multiplyMatrixTimesMatrix(benchmark::State& st)
+static void multiplyMatrixTimesMatrixNoAlias(benchmark::State& st)
{
- int n_iters = st.range(0);
Eigen::Isometry3d isometry = createTestIsometry();
+ Eigen::Isometry3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Matrix4d result;
- benchmark::DoNotOptimize(result = isometry.matrix() * isometry.matrix());
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result.matrix().noalias() = isometry.matrix() * isometry.matrix());
+ benchmark::ClobberMemory();
}
}
+// Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d.
+static void multiplyIsometryTimesIsometryNoAlias(benchmark::State& st)
+{
+ Eigen::Isometry3d isometry = createTestIsometry();
+ Eigen::Isometry3d result;
+ for (auto _ : st)
+ {
+ benchmark::DoNotOptimize(result.affine().noalias() = isometry.affine() * isometry.matrix());
+ benchmark::ClobberMemory();
+ }
+}
+
+// Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d.
+static void multiplyMatrixTimesMatrix(benchmark::State& st)
+{
+ Eigen::Isometry3d isometry = createTestIsometry();
+ Eigen::Isometry3d result;
+ for (auto _ : st)
+ {
+ benchmark::DoNotOptimize(result.matrix() = isometry.matrix() * isometry.matrix());
+ benchmark::ClobberMemory();
+ }
+}
// Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d.
static void multiplyIsometryTimesIsometry(benchmark::State& st)
{
- int n_iters = st.range(0);
Eigen::Isometry3d isometry = createTestIsometry();
+ Eigen::Isometry3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Isometry3d result;
- benchmark::DoNotOptimize(result = isometry * isometry);
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result = isometry * isometry);
+ benchmark::ClobberMemory();
}
}
// Benchmark time to invert an Eigen::Isometry3d.
static void inverseIsometry3d(benchmark::State& st)
{
- int n_iters = st.range(0);
Eigen::Isometry3d isometry = createTestIsometry();
+ Eigen::Isometry3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Isometry3d result;
- benchmark::DoNotOptimize(result = isometry.inverse());
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result = isometry.inverse());
+ benchmark::ClobberMemory();
}
}
// Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry).
static void inverseAffineIsometry(benchmark::State& st)
{
- int n_iters = st.range(0);
- Eigen::Isometry3d isometry = createTestIsometry();
- Eigen::Affine3d affine;
- affine.matrix() = isometry.matrix();
-
+ Eigen::Affine3d affine = createTestIsometry();
+ Eigen::Affine3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Affine3d result;
- benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry).affine());
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry));
+ benchmark::ClobberMemory();
}
}
// Benchmark time to invert an Eigen::Affine3d.
static void inverseAffine(benchmark::State& st)
{
- int n_iters = st.range(0);
- Eigen::Isometry3d isometry = createTestIsometry();
- Eigen::Affine3d affine;
- affine.matrix() = isometry.matrix();
-
+ Eigen::Affine3d affine = createTestIsometry();
+ Eigen::Affine3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Affine3d result;
- benchmark::DoNotOptimize(result = affine.inverse().affine());
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result = affine.inverse());
+ benchmark::ClobberMemory();
}
}
// Benchmark time to invert an Eigen::Matrix4d.
static void inverseMatrix4d(benchmark::State& st)
{
- int n_iters = st.range(0);
- Eigen::Isometry3d isometry = createTestIsometry();
- Eigen::Affine3d affine;
- affine.matrix() = isometry.matrix();
-
+ Eigen::Affine3d affine = createTestIsometry();
+ Eigen::Affine3d result;
for (auto _ : st)
{
- for (int i = 0; i < n_iters; ++i)
- {
- Eigen::Affine3d result;
- benchmark::DoNotOptimize(result = affine.matrix().inverse());
- benchmark::ClobberMemory();
- }
+ benchmark::DoNotOptimize(result = affine.matrix().inverse());
+ benchmark::ClobberMemory();
}
}
-// Benchmark time to construct a RobotState given a RobotModel.
-static void robotStateConstruct(benchmark::State& st)
+struct RobotStateBenchmark : ::benchmark::Fixture
{
- int n_states = st.range(0);
- const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
-
- // Make sure the group exists, otherwise exit early with an error.
- if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP))
+ void SetUp(const ::benchmark::State& /*state*/) override
{
- st.SkipWithError("The planning group doesn't exist.");
- return;
+ std::ignore = rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_WARN);
+ robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
}
- for (auto _ : st)
+ std::vector constructStates(size_t num)
{
- for (int i = 0; i < n_states; i++)
- {
- std::unique_ptr robot_state;
- benchmark::DoNotOptimize(robot_state = std::make_unique(robot_model));
- benchmark::ClobberMemory();
- }
+ std::vector states;
+ states.reserve(num);
+ for (size_t i = 0; i < num; i++)
+ states.emplace_back(robot_model);
+ return states;
+ }
+ std::vector constructStates(size_t num, const moveit::core::RobotState& state)
+ {
+ std::vector states;
+ states.reserve(num);
+ for (size_t i = 0; i < num; i++)
+ states.emplace_back(state);
+ return states;
}
-}
-
-// Benchmark time to copy a RobotState.
-static void robotStateCopy(benchmark::State& st)
-{
- int n_states = st.range(0);
- const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
- // Make sure the group exists, otherwise exit early with an error.
- if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP))
+ std::vector randomPermutation(size_t num)
{
- st.SkipWithError("The planning group doesn't exist.");
- return;
+ std::vector result(num);
+ std::iota(result.begin(), result.end(), 0);
+
+ std::mt19937 generator;
+ std::shuffle(result.begin(), result.end(), generator);
+ return result;
}
- // Robot state.
- moveit::core::RobotState robot_state(robot_model);
- robot_state.setToDefaultValues();
+ moveit::core::RobotModelPtr robot_model;
+};
+// Benchmark time to construct RobotStates
+BENCHMARK_DEFINE_F(RobotStateBenchmark, construct)(benchmark::State& st)
+{
for (auto _ : st)
{
- for (int i = 0; i < n_states; i++)
- {
- std::unique_ptr robot_state_copy;
- benchmark::DoNotOptimize(robot_state_copy = std::make_unique | 
-The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/ros-planning/moveit).
+The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/moveit/moveit). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro).
*Easy-to-use open source robotics manipulation platform for developing commercial applications, prototyping designs, and benchmarking algorithms.*
## Continuous Integration Status
-[](https://github.com/ros-planning/moveit2/actions/workflows/format.yaml?query=branch%3Amain)
-[](https://github.com/ros-planning/moveit2/actions/workflows/ci.yaml?query=branch%3Amain)
+[](https://github.com/moveit/moveit2/actions/workflows/format.yaml?query=branch%3Amain)
+[](https://github.com/moveit/moveit2/actions/workflows/ci.yaml?query=branch%3Amain)
[](https://codecov.io/gh/ros-planning/moveit2)
## Getting Started
@@ -56,47 +56,42 @@ research and innovation programme under grant agreement no. 732287.
See [How To Generate API Doxygen Reference Locally](https://moveit.picknik.ai/main/doc/how_to_guides/how_to_generate_api_doxygen_locally.html).
# Buildfarm
-| Package | Humble Binary | Iron Binary | Rolling Binary |
-|:---:|:---:|:---:|:---:|
-| geometric_shapes | [](https://build.ros2.org/job/Hbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) |
-| moveit | [](https://build.ros2.org/job/Hbin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit__ubuntu_jammy_amd64__binary) |
-| moveit_common | [](https://build.ros2.org/job/Hbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) |
-| moveit_core | [](https://build.ros2.org/job/Hbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) |
-| moveit_hybrid_planning | [](https://build.ros2.org/job/Hbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) |
-| moveit_kinematics | [](https://build.ros2.org/job/Hbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) |
-| moveit_msgs | [](https://build.ros2.org/job/Hbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) |
-| moveit_planners | [](https://build.ros2.org/job/Hbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) |
-| moveit_planners_ompl | [](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) |
-| moveit_planners_stomp | [](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) |
-| moveit_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) |
-| moveit_resources | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) |
-| moveit_resources_fanuc_description | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary) |
-| moveit_resources_fanuc_moveit_config | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary) |
-| moveit_resources_panda_description | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary) |
-| moveit_resources_panda_moveit_config | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary) |
-| moveit_resources_pr2_description | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary) |
-| moveit_ros | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) |
-| moveit_ros_benchmarks | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) |
-| moveit_ros_move_group | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) |
-| moveit_ros_occupancy_map_monitor | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) |
-| moveit_ros_perception | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) |
-| moveit_ros_planning | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) |
-| moveit_ros_planning_interface | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) |
-| moveit_ros_robot_interaction | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) |
-| moveit_ros_visualization | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) |
-| moveit_ros_warehouse | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) |
-| moveit_runtime | [](https://build.ros2.org/job/Hbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) |
-| moveit_servo | [](https://build.ros2.org/job/Hbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) |
-| moveit_setup_app_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) |
-| moveit_setup_assistant | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) |
-| moveit_setup_controllers | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) |
-| moveit_setup_core_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) |
-| moveit_setup_framework | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) |
-| moveit_setup_srdf_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) |
-| moveit_simple_controller_manager | [](https://build.ros2.org/job/Hbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) |
-| pilz_industrial_motion_planner | [](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) |
-| pilz_industrial_motion_planner_testutils | [](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) |
-| random_numbers | [](https://build.ros2.org/job/Hbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) |
-| srdfdom | [](https://build.ros2.org/job/Hbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) |
-| warehouse_ros | [](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) |
-| warehouse_ros_sqlite | [](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) |
+| Package | Humble Binary | Iron Binary | Rolling Binary | Jazzy Binary |
+|:---:|:---:|:---:|:---:|:---:|
+| geometric_shapes | [](https://build.ros2.org/job/Hbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__geometric_shapes__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__geometric_shapes__ubuntu_noble_amd64__binary) |
+| moveit | [](https://build.ros2.org/job/Hbin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit__ubuntu_noble_amd64__binary) |
+| moveit_common | [](https://build.ros2.org/job/Hbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_common__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_common__ubuntu_noble_amd64__binary) |
+| moveit_core | [](https://build.ros2.org/job/Hbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_core__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_core__ubuntu_noble_amd64__binary) |
+| moveit_hybrid_planning | [](https://build.ros2.org/job/Hbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_hybrid_planning__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_hybrid_planning__ubuntu_noble_amd64__binary) |
+| moveit_kinematics | [](https://build.ros2.org/job/Hbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_kinematics__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_kinematics__ubuntu_noble_amd64__binary) |
+| moveit_msgs | [](https://build.ros2.org/job/Hbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_msgs__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_msgs__ubuntu_noble_amd64__binary) |
+| moveit_planners | [](https://build.ros2.org/job/Hbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_planners__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_planners__ubuntu_noble_amd64__binary) |
+| moveit_planners_ompl | [](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_planners_ompl__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_planners_ompl__ubuntu_noble_amd64__binary) |
+| moveit_planners_stomp | [](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_planners_stomp__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_planners_stomp__ubuntu_noble_amd64__binary) |
+| moveit_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_plugins__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_plugins__ubuntu_noble_amd64__binary) |
+| moveit_resources | [](https://build.ros2.org/job/Hbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_resources__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_resources__ubuntu_noble_amd64__binary) |
+| moveit_ros | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros__ubuntu_noble_amd64__binary) |
+| moveit_ros_benchmarks | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_benchmarks__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_benchmarks__ubuntu_noble_amd64__binary) |
+| moveit_ros_move_group | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_move_group__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_move_group__ubuntu_noble_amd64__binary) |
+| moveit_ros_occupancy_map_monitor | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_occupancy_map_monitor__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_occupancy_map_monitor__ubuntu_noble_amd64__binary) |
+| moveit_ros_perception | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_perception__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_perception__ubuntu_noble_amd64__binary) |
+| moveit_ros_planning | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_planning__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_planning__ubuntu_noble_amd64__binary) |
+| moveit_ros_planning_interface | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_planning_interface__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_planning_interface__ubuntu_noble_amd64__binary) |
+| moveit_ros_robot_interaction | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_robot_interaction__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_robot_interaction__ubuntu_noble_amd64__binary) |
+| moveit_ros_visualization | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_visualization__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_visualization__ubuntu_noble_amd64__binary) |
+| moveit_ros_warehouse | [](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_ros_warehouse__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_ros_warehouse__ubuntu_noble_amd64__binary) |
+| moveit_runtime | [](https://build.ros2.org/job/Hbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_runtime__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_runtime__ubuntu_noble_amd64__binary) |
+| moveit_servo | [](https://build.ros2.org/job/Hbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_servo__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_servo__ubuntu_noble_amd64__binary) |
+| moveit_setup_app_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_setup_app_plugins__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_setup_app_plugins__ubuntu_noble_amd64__binary) |
+| moveit_setup_assistant | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_setup_assistant__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_setup_assistant__ubuntu_noble_amd64__binary) |
+| moveit_setup_controllers | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_setup_controllers__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_setup_controllers__ubuntu_noble_amd64__binary) |
+| moveit_setup_core_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_setup_core_plugins__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_setup_core_plugins__ubuntu_noble_amd64__binary) |
+| moveit_setup_framework | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_setup_framework__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_setup_framework__ubuntu_noble_amd64__binary) |
+| moveit_setup_srdf_plugins | [](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_setup_srdf_plugins__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_setup_srdf_plugins__ubuntu_noble_amd64__binary) |
+| moveit_simple_controller_manager | [](https://build.ros2.org/job/Hbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__moveit_simple_controller_manager__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__moveit_simple_controller_manager__ubuntu_noble_amd64__binary) |
+| pilz_industrial_motion_planner | [](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__pilz_industrial_motion_planner__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__pilz_industrial_motion_planner__ubuntu_noble_amd64__binary) |
+| pilz_industrial_motion_planner_testutils | [](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__pilz_industrial_motion_planner_testutils__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__pilz_industrial_motion_planner_testutils__ubuntu_noble_amd64__binary) |
+| random_numbers | [](https://build.ros2.org/job/Hbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__random_numbers__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__random_numbers__ubuntu_noble_amd64__binary) |
+| srdfdom | [](https://build.ros2.org/job/Hbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__srdfdom__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__srdfdom__ubuntu_noble_amd64__binary) |
+| warehouse_ros | [](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__warehouse_ros__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__warehouse_ros__ubuntu_noble_amd64__binary) |
+| warehouse_ros_sqlite | [](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [](https://build.ros2.org/job/Rbin_uN64__warehouse_ros_sqlite__ubuntu_noble_amd64__binary) | [](https://build.ros2.org/job/Jbin_uN64__warehouse_ros_sqlite__ubuntu_noble_amd64__binary) |
diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
index 0e984538ce..a20ae58929 100644
--- a/moveit/CHANGELOG.rst
+++ b/moveit/CHANGELOG.rst
@@ -2,6 +2,17 @@
Changelog for package moveit
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2.10.0 (2024-06-13)
+-------------------
+* Migrate ros-planning org to moveit (`#2847