diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index a9be862e96..484f2ab84f 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -2,7 +2,7 @@ # ROS base image augmented with all MoveIt dependencies to use for CI ARG ROS_DISTRO=rolling -FROM ros:${ROS_DISTRO}-ros-base-focal +FROM ros:${ROS_DISTRO}-ros-base MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de ENV TERM xterm @@ -25,7 +25,7 @@ RUN \ # Some basic requirements wget git sudo curl \ # Preferred build tools - clang clang-format-10 clang-tidy clang-tools \ + clang clang-format-12 clang-tidy clang-tools \ ccache && \ # # Fetch all dependencies from moveit2.repos diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index bc8e7b2796..16ae7de119 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -2,7 +2,7 @@ # Full debian-based install of MoveIt using apt-get ARG ROS_DISTRO=rolling -FROM ros:${ROS_DISTRO}-ros-base-focal +FROM ros:${ROS_DISTRO}-ros-base MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 04671ccf1e..6f735f2826 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -20,11 +20,12 @@ jobs: CCOV: true ROS_DISTRO: rolling - IMAGE: rolling-ci-testing - IKFAST_TEST: true ROS_DISTRO: rolling + IKFAST_TEST: true CLANG_TIDY: pedantic env: - CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" + CXXFLAGS: >- + -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: moveit2.repos $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f) @@ -32,15 +33,19 @@ jobs: AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src # Clear the ccache stats before and log the stats after the build + AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G BEFORE_BUILD_UPSTREAM_WORKSPACE: ccache -z AFTER_BUILD_TARGET_WORKSPACE: ccache -s # Changing linker to lld as ld has a behavior where it takes a long time to finish + # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings) TARGET_CMAKE_ARGS: > -DCMAKE_EXE_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_SHARED_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_MODULE_LINKER_FLAGS=-fuse-ld=lld -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} - -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" + -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" + UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''" + DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }} diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index ddfa93f93a..542457d8ee 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -17,8 +17,8 @@ jobs: steps: - uses: actions/checkout@v2 - uses: actions/setup-python@v2 - - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 + - name: Install clang-format-12 + run: sudo apt-get install clang-format-12 - uses: pre-commit/action@v2.0.3 id: precommit - name: Upload pre-commit changes diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 962dc42249..a5f0f0e115 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,7 +33,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 20.8b1 + rev: 22.3.0 hooks: - id: black @@ -42,7 +42,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] diff --git a/MIGRATION.md b/MIGRATION.md index c18792e3aa..0c17e6b517 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,11 +3,16 @@ API changes in MoveIt releases ## ROS Rolling +- `lockSceneRead()` and `lockSceneWrite()` are now protected member functions, for internal use only. To lock the planning scene, use LockedPlanningSceneRO or LockedPlanningSceneRW: +``` + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + moveit::core::RobotModelConstPtr model = ls->getRobotModel(); +``` - ServoServer was renamed to ServoNode - `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes: - `getFrameTransform()` now returns this pose instead of the first shape's pose. - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest. - - Planning scene geometry text files (`.scene`) have changed format. Add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files if required. + - Planning scene geometry text files (`.scene`) have changed format. Loading old files is still supported. You can add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files. - add API for passing RNG to setToRandomPositionsNearBy - Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`. - Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic). @@ -32,6 +37,8 @@ API changes in MoveIt releases - Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead. - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase). - In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `/move_group/display_planned_path` instead of `/move_group/display_planned_path`). +- `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer. +- Moved the class `MoveItErrorCode` from both `moveit_ros_planning` and `moveit_ros_planning_interface` to `moveit_core`. The class now is in namespace `moveit::core`, access via `moveit::planning_interface` or `moveit_cpp::PlanningComponent` is deprecated. ## ROS Melodic diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py index 897755cda7..6dd624dcb1 100644 --- a/moveit/scripts/create_readme_table.py +++ b/moveit/scripts/create_readme_table.py @@ -29,6 +29,9 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md +# First update supported_distro_ubuntu_dict below! + from __future__ import print_function @@ -77,7 +80,7 @@ def create_line(package, ros_ubuntu_dict): "U": ubuntu[0].upper(), "ubuntu": ubuntu.lower(), "package": package, - "base_url": "http://build.ros.org", + "base_url": "https://build.ros.org", } for target in ["src", "bin"]: define_urls(target, params) @@ -102,7 +105,6 @@ def create_moveit_buildfarm_table(): # remove {"indigo":"trusty"} and add {"noetic":"fbuntu"} with "fbuntu" # being whatever the 20.04 distro is named supported_distro_ubuntu_dict = { - "kinetic": "xenial", "melodic": "bionic", "noetic": "focal", } diff --git a/moveit2.repos b/moveit2.repos index 71ee4bf3e0..5211f196ce 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -2,20 +2,12 @@ repositories: geometric_shapes: type: git url: https://github.com/ros-planning/geometric_shapes - version: 2.1.2 - moveit_msgs: - type: git - url: https://github.com/ros-planning/moveit_msgs - version: 2.2.0 + version: ros2 moveit_resources: type: git url: https://github.com/ros-planning/moveit_resources version: ros2 - launch_param_builder: + srdfdom: type: git - url: https://github.com/PickNikRobotics/launch_param_builder - version: main - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: 2.1.0 + url: https://github.com/ros-planning/srdfdom.git + version: ros2 diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 8bd6f9d2d1..3ab4e50426 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -2,7 +2,7 @@ moveit_commander - 1.1.6 + 1.1.7 Python interfaces to MoveIt Michael Görner Robert Haschke diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index f206007200..8c980bccaf 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -62,68 +62,68 @@ class MoveGroupCommander(object): def __init__( self, name, robot_description="robot_description", ns="", wait_for_servers=5.0 ): - """ Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """ + """Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error.""" self._g = _moveit_move_group_interface.MoveGroupInterface( name, robot_description, ns, wait_for_servers ) def get_name(self): - """ Get the name of the group this instance was initialized for """ + """Get the name of the group this instance was initialized for""" return self._g.get_name() def stop(self): - """ Stop the current execution, if any """ + """Stop the current execution, if any""" self._g.stop() def get_active_joints(self): - """ Get the active joints of this group """ + """Get the active joints of this group""" return self._g.get_active_joints() def get_joints(self): - """ Get the joints of this group """ + """Get the joints of this group""" return self._g.get_joints() def get_variable_count(self): - """ Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)""" + """Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)""" return self._g.get_variable_count() def has_end_effector_link(self): - """ Check if this group has a link that is considered to be an end effector """ + """Check if this group has a link that is considered to be an end effector""" return len(self._g.get_end_effector_link()) > 0 def get_end_effector_link(self): - """ Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector. """ + """Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector.""" return self._g.get_end_effector_link() def set_end_effector_link(self, link_name): - """ Set the name of the link to be considered as an end effector """ + """Set the name of the link to be considered as an end effector""" if not self._g.set_end_effector_link(link_name): raise MoveItCommanderException("Unable to set end effector link") def get_interface_description(self): - """ Get the description of the planner interface (list of planner ids) """ + """Get the description of the planner interface (list of planner ids)""" desc = PlannerInterfaceDescription() conversions.msg_from_string(desc, self._g.get_interface_description()) return desc def get_pose_reference_frame(self): - """ Get the reference frame assumed for poses of end-effectors """ + """Get the reference frame assumed for poses of end-effectors""" return self._g.get_pose_reference_frame() def set_pose_reference_frame(self, reference_frame): - """ Set the reference frame to assume for poses of end-effectors """ + """Set the reference frame to assume for poses of end-effectors""" self._g.set_pose_reference_frame(reference_frame) def get_planning_frame(self): - """ Get the name of the frame where all planning is performed """ + """Get the name of the frame where all planning is performed""" return self._g.get_planning_frame() def get_current_joint_values(self): - """ Get the current configuration of the group as a list (these are values published on /joint_states) """ + """Get the current configuration of the group as a list (these are values published on /joint_states)""" return self._g.get_current_joint_values() def get_current_pose(self, end_effector_link=""): - """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """ + """Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped( self._g.get_current_pose(end_effector_link), self.get_planning_frame() @@ -134,7 +134,7 @@ def get_current_pose(self, end_effector_link=""): ) def get_current_rpy(self, end_effector_link=""): - """ Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector. """ + """Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): return self._g.get_current_rpy(end_effector_link) else: @@ -180,14 +180,14 @@ def set_start_state(self, msg): self._g.set_start_state(conversions.msg_to_string(msg)) def get_current_state_bounded(self): - """ Get the current state of the robot bounded.""" + """Get the current state of the robot bounded.""" s = RobotState() c_str = self._g.get_current_state_bounded() conversions.msg_from_string(s, c_str) return s def get_current_state(self): - """ Get the current state of the robot.""" + """Get the current state of the robot.""" s = RobotState() c_str = self._g.get_current_state() conversions.msg_from_string(s, c_str) @@ -290,7 +290,7 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None): ) def set_rpy_target(self, rpy, end_effector_link=""): - """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" + """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if len(rpy) == 3: if not self._g.set_rpy_target( @@ -305,7 +305,7 @@ def set_rpy_target(self, rpy, end_effector_link=""): ) def set_orientation_target(self, q, end_effector_link=""): - """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" + """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if len(q) == 4: if not self._g.set_orientation_target( @@ -320,7 +320,7 @@ def set_orientation_target(self, q, end_effector_link=""): ) def set_position_target(self, xyz, end_effector_link=""): - """ Specify a target position for the end-effector. Any orientation of the end-effector is acceptable.""" + """Specify a target position for the end-effector. Any orientation of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if not self._g.set_position_target( xyz[0], xyz[1], xyz[2], end_effector_link @@ -332,7 +332,7 @@ def set_position_target(self, xyz, end_effector_link=""): ) def set_pose_target(self, pose, end_effector_link=""): - """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" + """Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): ok = False @@ -357,7 +357,7 @@ def set_pose_target(self, pose, end_effector_link=""): ) def set_pose_targets(self, poses, end_effector_link=""): - """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ + """Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw]""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if not self._g.set_pose_targets( [conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], @@ -368,7 +368,7 @@ def set_pose_targets(self, poses, end_effector_link=""): raise MoveItCommanderException("There is no end effector to set poses for") def shift_pose_target(self, axis, value, end_effector_link=""): - """ Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target """ + """Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target""" if len(end_effector_link) > 0 or self.has_end_effector_link(): pose = self._g.get_current_pose(end_effector_link) # by default we get orientation as a quaternion list @@ -385,23 +385,23 @@ def shift_pose_target(self, axis, value, end_effector_link=""): raise MoveItCommanderException("There is no end effector to set poses for") def clear_pose_target(self, end_effector_link): - """ Clear the pose target for a particular end-effector """ + """Clear the pose target for a particular end-effector""" self._g.clear_pose_target(end_effector_link) def clear_pose_targets(self): - """ Clear all known pose targets """ + """Clear all known pose targets""" self._g.clear_pose_targets() def set_random_target(self): - """ Set a random joint configuration target """ + """Set a random joint configuration target""" self._g.set_random_target() def get_named_targets(self): - """ Get a list of all the names of joint configurations.""" + """Get a list of all the names of joint configurations.""" return self._g.get_named_targets() def set_named_target(self, name): - """ Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF. """ + """Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF.""" if not self._g.set_named_target(name): raise MoveItCommanderException( "Unable to set target %s. Is the target within bounds?" % name @@ -412,21 +412,21 @@ def get_named_target_values(self, target): return self._g.get_named_target_values(target) def remember_joint_values(self, name, values=None): - """ Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded. """ + """Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded.""" if values is None: values = self.get_current_joint_values() self._g.remember_joint_values(name, values) def get_remembered_joint_values(self): - """ Get a dictionary that maps names to joint configurations for the group """ + """Get a dictionary that maps names to joint configurations for the group""" return self._g.get_remembered_joint_values() def forget_joint_values(self, name): - """ Forget a stored joint configuration """ + """Forget a stored joint configuration""" self._g.forget_joint_values(name) def get_goal_tolerance(self): - """ Return a tuple of goal tolerances: joint, position and orientation. """ + """Return a tuple of goal tolerances: joint, position and orientation.""" return ( self.get_goal_joint_tolerance(), self.get_goal_position_tolerance(), @@ -434,54 +434,54 @@ def get_goal_tolerance(self): ) def get_goal_joint_tolerance(self): - """ Get the tolerance for achieving a joint goal (distance for each joint variable) """ + """Get the tolerance for achieving a joint goal (distance for each joint variable)""" return self._g.get_goal_joint_tolerance() def get_goal_position_tolerance(self): - """ When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector """ + """When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector""" return self._g.get_goal_position_tolerance() def get_goal_orientation_tolerance(self): - """ When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector """ + """When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector""" return self._g.get_goal_orientation_tolerance() def set_goal_tolerance(self, value): - """ Set the joint, position and orientation goal tolerances simultaneously """ + """Set the joint, position and orientation goal tolerances simultaneously""" self._g.set_goal_tolerance(value) def set_goal_joint_tolerance(self, value): - """ Set the tolerance for a target joint configuration """ + """Set the tolerance for a target joint configuration""" self._g.set_goal_joint_tolerance(value) def set_goal_position_tolerance(self, value): - """ Set the tolerance for a target end-effector position """ + """Set the tolerance for a target end-effector position""" self._g.set_goal_position_tolerance(value) def set_goal_orientation_tolerance(self, value): - """ Set the tolerance for a target end-effector orientation """ + """Set the tolerance for a target end-effector orientation""" self._g.set_goal_orientation_tolerance(value) def allow_looking(self, value): - """ Enable/disable looking around for motion planning """ + """Enable/disable looking around for motion planning""" self._g.allow_looking(value) def allow_replanning(self, value): - """ Enable/disable replanning """ + """Enable/disable replanning""" self._g.allow_replanning(value) def get_known_constraints(self): - """ Get a list of names for the constraints specific for this group, as read from the warehouse """ + """Get a list of names for the constraints specific for this group, as read from the warehouse""" return self._g.get_known_constraints() def get_path_constraints(self): - """ Get the actual path constraints in form of a moveit_msgs.msgs.Constraints """ + """Get the actual path constraints in form of a moveit_msgs.msgs.Constraints""" c = Constraints() c_str = self._g.get_path_constraints() conversions.msg_from_string(c, c_str) return c def set_path_constraints(self, value): - """ Specify the path constraints to be used (as read from the database) """ + """Specify the path constraints to be used (as read from the database)""" if value is None: self.clear_path_constraints() else: @@ -493,18 +493,18 @@ def set_path_constraints(self, value): ) def clear_path_constraints(self): - """ Specify that no path constraints are to be used during motion planning """ + """Specify that no path constraints are to be used during motion planning""" self._g.clear_path_constraints() def get_trajectory_constraints(self): - """ Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints """ + """Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints""" c = TrajectoryConstraints() c_str = self._g.get_trajectory_constraints() conversions.msg_from_string(c, c_str) return c def set_trajectory_constraints(self, value): - """ Specify the trajectory constraints to be used (setting from database is not implemented yet)""" + """Specify the trajectory constraints to be used (setting from database is not implemented yet)""" if value is None: self.clear_trajectory_constraints() else: @@ -518,43 +518,43 @@ def set_trajectory_constraints(self, value): ) def clear_trajectory_constraints(self): - """ Specify that no trajectory constraints are to be used during motion planning """ + """Specify that no trajectory constraints are to be used during motion planning""" self._g.clear_trajectory_constraints() def set_constraints_database(self, host, port): - """ Specify which database to connect to for loading possible path constraints """ + """Specify which database to connect to for loading possible path constraints""" self._g.set_constraints_database(host, port) def set_planning_time(self, seconds): - """ Specify the amount of time to be used for motion planning. """ + """Specify the amount of time to be used for motion planning.""" self._g.set_planning_time(seconds) def get_planning_time(self): - """ Specify the amount of time to be used for motion planning. """ + """Specify the amount of time to be used for motion planning.""" return self._g.get_planning_time() def set_planning_pipeline_id(self, planning_pipeline): - """ Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner) """ + """Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner)""" self._g.set_planning_pipeline_id(planning_pipeline) def get_planning_pipeline_id(self): - """ Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner) """ + """Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner)""" return self._g.get_planning_pipeline_id() def set_planner_id(self, planner_id): - """ Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN) """ + """Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN)""" self._g.set_planner_id(planner_id) def get_planner_id(self): - """ Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline """ + """Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline""" return self._g.get_planner_id() def set_num_planning_attempts(self, num_planning_attempts): - """ Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1. """ + """Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1.""" self._g.set_num_planning_attempts(num_planning_attempts) def set_workspace(self, ws): - """ Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ] """ + """Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ]""" if len(ws) == 0: self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) else: @@ -589,7 +589,7 @@ def set_max_acceleration_scaling_factor(self, value): ) def go(self, joints=None, wait=True): - """ Set the target of the group and then move the group to the specified target """ + """Set the target of the group and then move the group to the specified target""" if type(joints) is bool: wait = joints joints = None @@ -639,7 +639,7 @@ def plan(self, joints=None): ) def construct_motion_plan_request(self): - """ Returns a MotionPlanRequest filled with the current goals of the move_group_interface""" + """Returns a MotionPlanRequest filled with the current goals of the move_group_interface""" mpr = MotionPlanRequest() return mpr.deserialize(self._g.construct_motion_plan_request()) @@ -651,7 +651,7 @@ def compute_cartesian_path( avoid_collisions=True, path_constraints=None, ): - """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ + """Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory.""" if path_constraints: if type(path_constraints) is Constraints: constraints_str = conversions.msg_to_string(path_constraints) @@ -687,11 +687,11 @@ def execute(self, plan_msg, wait=True): return self._g.async_execute(conversions.msg_to_string(plan_msg)) def attach_object(self, object_name, link_name="", touch_links=[]): - """ Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was successfully sent to the move_group node. This does not verify that the attach request also was successfully applied by move_group.""" + """Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was successfully sent to the move_group node. This does not verify that the attach request also was successfully applied by move_group.""" return self._g.attach_object(object_name, link_name, touch_links) def detach_object(self, name=""): - """ Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group.""" + """Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group.""" return self._g.detach_object(name) def pick(self, object_name, grasp=[], plan_only=False): @@ -750,7 +750,7 @@ def place(self, object_name, location=None, plan_only=False): return result def set_support_surface_name(self, value): - """ Set the support surface name for a place operation """ + """Set the support surface name for a place operation""" self._g.set_support_surface_name(value) def retime_trajectory( @@ -775,14 +775,14 @@ def retime_trajectory( return traj_out def get_jacobian_matrix(self, joint_values, reference_point=None): - """ Get the jacobian matrix of the group as a list""" + """Get the jacobian matrix of the group as a list""" return self._g.get_jacobian_matrix( joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point, ) def enforce_bounds(self, robot_state_msg): - """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """ + """Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds()""" s = RobotState() c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg)) conversions.msg_from_string(s, c_str) diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index efa16d08ac..acd442e77e 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -65,7 +65,7 @@ class PlanningSceneInterface(object): """ def __init__(self, ns="", synchronous=False, service_timeout=5.0): - """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """ + """Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics.""" self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns) self._pub_co = rospy.Publisher( @@ -94,31 +94,31 @@ def __submit(self, collision_object, attach=False): self._pub_co.publish(collision_object) def add_object(self, collision_object): - """ Add an object to the planning scene """ + """Add an object to the planning scene""" self.__submit(collision_object, attach=False) def add_sphere(self, name, pose, radius=1): - """ Add a sphere to the planning scene """ + """Add a sphere to the planning scene""" co = self.__make_sphere(name, pose, radius) self.__submit(co, attach=False) def add_cylinder(self, name, pose, height, radius): - """ Add a cylinder to the planning scene """ + """Add a cylinder to the planning scene""" co = self.__make_cylinder(name, pose, height, radius) self.__submit(co, attach=False) def add_mesh(self, name, pose, filename, size=(1, 1, 1)): - """ Add a mesh to the planning scene """ + """Add a mesh to the planning scene""" co = self.__make_mesh(name, pose, filename, size) self.__submit(co, attach=False) def add_box(self, name, pose, size=(1, 1, 1)): - """ Add a box to the planning scene """ + """Add a box to the planning scene""" co = self.__make_box(name, pose, size) self.__submit(co, attach=False) def add_plane(self, name, pose, normal=(0, 0, 1), offset=0): - """ Add a plane to the planning scene """ + """Add a plane to the planning scene""" co = CollisionObject() co.operation = CollisionObject.ADD co.id = name @@ -131,7 +131,7 @@ def add_plane(self, name, pose, normal=(0, 0, 1), offset=0): self.__submit(co, attach=False) def attach_object(self, attached_collision_object): - """ Attach an object in the planning scene """ + """Attach an object in the planning scene""" self.__submit(attached_collision_object, attach=True) def attach_mesh( @@ -162,7 +162,7 @@ def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]): self.__submit(aco, attach=True) def clear(self): - """ Remove all objects from the planning scene """ + """Remove all objects from the planning scene""" self.remove_attached_object() self.remove_world_object() diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py index 55f6e7b07c..e86e44d723 100644 --- a/moveit_commander/src/moveit_commander/robot.py +++ b/moveit_commander/src/moveit_commander/robot.py @@ -192,7 +192,7 @@ def get_robot_markers(self, *args): return mrkr def get_root_link(self): - """Get the name of the root link of the robot model """ + """Get the name of the root link of the robot model""" return self._r.get_robot_root_link() def get_active_joint_names(self, group=None): @@ -241,7 +241,7 @@ def get_group_names(self): return self._r.get_group_names() def get_current_state(self): - """ Get a RobotState message describing the current state of the robot""" + """Get a RobotState message describing the current state of the robot""" s = RobotState() s.deserialize(self._r.get_current_state()) return s diff --git a/moveit_configs_utils/moveit_configs_utils/launch_utils.py b/moveit_configs_utils/moveit_configs_utils/launch_utils.py new file mode 100644 index 0000000000..0d44927787 --- /dev/null +++ b/moveit_configs_utils/moveit_configs_utils/launch_utils.py @@ -0,0 +1,46 @@ +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +class DeclareBooleanLaunchArg(DeclareLaunchArgument): + """This launch action declares a launch argument with true and false as the only valid values""" + + def __init__(self, name, *, default_value=False, description=None, **kwargs): + super().__init__( + name, + default_value=str(default_value), + description=description, + choices=["true", "false", "True", "False"], + **kwargs, + ) + + +def add_debuggable_node( + ld, package, executable, condition_name="debug", commands_file=None, **kwargs +): + """Adds two versions of a Node to the launch description, one with gdb debugging, controlled by a launch config""" + standard_node = Node( + package=package, + executable=executable, + condition=UnlessCondition(LaunchConfiguration(condition_name)), + **kwargs, + ) + ld.add_action(standard_node) + + if commands_file: + dash_x_arg = f"-x {commands_file} " + else: + dash_x_arg = "" + prefix = [f"gdb {dash_x_arg}--ex run --args"] + + debug_node = Node( + package=package, + executable=executable, + condition=IfCondition(LaunchConfiguration(condition_name)), + prefix=prefix, + **kwargs, + ) + ld.add_action(debug_node) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py new file mode 100644 index 0000000000..fd7854bde8 --- /dev/null +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -0,0 +1,134 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +from moveit_configs_utils.launch_utils import ( + add_debuggable_node, + DeclareBooleanLaunchArg, +) + + +def generate_rsp_launch(moveit_config): + """Launch file for robot state publisher (rsp)""" + + ld = LaunchDescription() + ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0")) + + # Given the published joint states, publish tf for the robot links and the robot description + rsp_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + respawn=True, + output="screen", + parameters=[ + moveit_config.robot_description, + { + "publish_frequency": LaunchConfiguration("publish_frequency"), + }, + ], + ) + ld.add_action(rsp_node) + + return ld + + +def generate_rviz_launch(moveit_config): + """Launch file for rviz""" + ld = LaunchDescription() + + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + ld.add_action( + DeclareLaunchArgument( + "rviz_config", + default_value=str(moveit_config.package_path / "launch/moveit.rviz"), + ) + ) + + rviz_parameters = [ + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ] + + add_debuggable_node( + ld, + package="rviz2", + executable="rviz2", + output="log", + respawn=True, + arguments=["-d", LaunchConfiguration("rviz_config")], + parameters=rviz_parameters, + ) + + return ld + + +def generate_assistant_launch(moveit_config): + """Launch file for MoveIt Setup Assistant""" + ld = LaunchDescription() + + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + add_debuggable_node( + ld, + package="moveit_setup_assistant", + executable="moveit_setup_assistant", + arguments=[["--config_pkg=", str(moveit_config.package_path)]], + ) + + return ld + + +def generate_warehouse_launch(moveit_config): + """Launch file for warehouse database""" + ld = LaunchDescription() + ld.add_action( + DeclareLaunchArgument( + "moveit_warehouse_database_path", + default_value=str( + moveit_config.package_path / "default_warehouse_mongo_db" + ), + ) + ) + ld.add_action(DeclareBooleanLaunchArg("reset", default_value=False)) + + # The default DB port for moveit (not default MongoDB port to avoid potential conflicts) + ld.add_action(DeclareLaunchArgument("moveit_warehouse_port", default_value="33829")) + + # The default DB host for moveit + ld.add_action( + DeclareLaunchArgument("moveit_warehouse_host", default_value="localhost") + ) + + # Load warehouse parameters + db_parameters = [ + { + "overwrite": False, + "database_path": LaunchConfiguration("moveit_warehouse_database_path"), + "warehouse_port": LaunchConfiguration("moveit_warehouse_port"), + "warehouse_host": LaunchConfiguration("moveit_warehouse_host"), + "warehouse_exec": "mongod", + "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection", + }, + ] + # Run the DB server + db_node = Node( + package="warehouse_ros_mongo", + executable="mongo_wrapper_ros.py", + # TODO(dlu): Figure out if this needs to be run in a specific directory + # (ROS 1 version set cwd="ROS_HOME") + parameters=db_parameters, + ) + ld.add_action(db_node) + + # If we want to reset the database, run this node + reset_node = Node( + package="moveit_ros_warehouse", + executable="moveit_init_demo_warehouse", + output="screen", + condition=IfCondition(LaunchConfiguration("reset")), + ) + ld.add_action(reset_node) + + return ld 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 d5304a79de..1ba9607da6 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -21,6 +21,7 @@ Example: moveit_configs = MoveItConfigsBuilder("robot_name").to_moveit_configs() ... + moveit_configs.package_path moveit_configs.robot_description moveit_configs.robot_description_semantic moveit_configs.robot_description_kinematics @@ -34,7 +35,7 @@ # Or to get all the parameters as a dictionary moveit_configs.to_dict() -Each function in MoveItConfigsBuilder have a file_path as an argument which is used to override the default +Each function in MoveItConfigsBuilder has a file_path as an argument which is used to override the default path for the file Example: @@ -61,6 +62,7 @@ class MoveItConfigs(object): """Class containing MoveIt related parameters.""" __slots__ = [ + "__package_path", "__robot_description", "__robot_description_semantic", "__robot_description_kinematics", @@ -74,6 +76,8 @@ class MoveItConfigs(object): ] def __init__(self): + # A pathlib Path to the moveit config package + self.package_path = None # A dictionary that has the contents of the URDF file. self.robot_description = {} # A dictionary that has the contents of the SRDF file. @@ -95,6 +99,14 @@ def __init__(self): # A dictionary containing the cartesian limits for the Pilz planner. self.cartesian_limits = {} + @property + def package_path(self): + return self.__package_path + + @package_path.setter + def package_path(self, value): + self.__package_path = value + @property def robot_description(self): return self.__robot_description @@ -205,6 +217,7 @@ class MoveItConfigsBuilder(ParameterBuilder): # Look-up for robot_name_moveit_config package def __init__(self, robot_name: str, robot_description="robot_description"): super().__init__(robot_name + "_moveit_config") + self.__moveit_configs.package_path = self._package_path self.__robot_name = robot_name setup_assistant_file = self._package_path / ".setup_assistant" if not setup_assistant_file.exists(): @@ -216,7 +229,7 @@ def __init__(self, robot_name: str, robot_description="robot_description"): self.__urdf_file_path = self.__config_dir_path / ( self.__robot_name + ".urdf" ) - self.__srdf_filename = self.__config_dir_path / ( + self.__srdf_file_path = self.__config_dir_path / ( self.__robot_name + ".srdf" ) else: @@ -233,7 +246,7 @@ def __init__(self, robot_name: str, robot_description="robot_description"): "relative_path" ] ) - self.__srdf_filename = Path( + self.__srdf_file_path = Path( setup_assistant_yaml["moveit_setup_assistant_config"]["SRDF"][ "relative_path" ] @@ -270,7 +283,7 @@ def robot_description_semantic( self.__moveit_configs.robot_description_semantic = { self.__robot_description + "_semantic": load_xacro( - self._package_path / (file_path or self.__srdf_filename), + self._package_path / (file_path or self.__srdf_file_path), mappings=mappings, ) } diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index b1b7075397..b2c8782bc9 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -4,15 +4,18 @@ moveit_configs_utils 2.3.0 Python library for loading moveit config parameters in launch files - Jafar Abdi MoveIt Release Team BSD - - ament_lint_auto - ament_lint_common + Jafar Abdi + David V. Lu!! ament_index_python launch_param_builder + launch + launch_ros + + ament_lint_auto + ament_lint_common ament_python diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index 05df2799a0..d63bb5aaff 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -79,23 +79,23 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override { - return CollisionEnvPtr(new CollisionEnvType(robot_model, world)); + return std::make_shared(robot_model, world); } CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override { - return CollisionEnvPtr(new CollisionEnvType(dynamic_cast(*orig), world)); + return std::make_shared(dynamic_cast(*orig), world); } CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override { - return CollisionEnvPtr(new CollisionEnvType(robot_model)); + return std::make_shared(robot_model); } /** Create an allocator for collision detectors. */ static CollisionDetectorAllocatorPtr create() { - return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType()); + return std::make_shared(); } }; } // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 213bb49501..aed91aa469 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -87,43 +87,45 @@ class AllowedCollisionMatrix * will be ignored. */ AllowedCollisionMatrix(const std::vector& names, bool allowed = false); + /** @brief Construct from an SRDF representation */ + AllowedCollisionMatrix(const srdf::Model& srdf); + /** @brief Construct the structure from a message representation */ AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg); /** @brief Copy constructor */ AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; - /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the - * collision matrix. - * Return false if the entry is not found. + /** @brief Get the type of the allowed collision between two elements. + * Return true if the entry is included in the collision matrix. Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element * @param allowed_collision_type The allowed collision type will be filled here */ bool getEntry(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision_type) const; - /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included - * in the collision matrix - * (if the type is AllowedCollision::CONDITIONAL). Return false if the entry is not found. + /** @brief Get the allowed collision predicate between two elements. + * Return true if a predicate for this entry is available in the collision matrix. + * Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled * here */ bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; - /** @brief Check if the allowed collision matrix has an entry at all for an element. Returns true if the element is - * included. + /** @brief Check if the allowed collision matrix has an entry at all for an element. + * Returns true if the element is included. * @param name name of the element */ bool hasEntry(const std::string& name) const; - /** @brief Check if the allowed collision matrix has an entry for a pair of elements. Returns true if the pair is - * included. + /** @brief Check if the allowed collision matrix has an entry for a pair of elements. + * Returns true if the pair is included. * @param name1 name of first element * @param name2 name of second element*/ bool hasEntry(const std::string& name1, const std::string& name2) const; - /** @brief Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in the - * collision matrix. + /** @brief Remove an entry corresponding to a pair of elements. + * Nothing happens if the pair does not exist in the collision matrix. * @param name1 name of first element * @param name2 name of second element*/ void removeEntry(const std::string& name1, const std::string& name2); @@ -137,26 +139,25 @@ class AllowedCollisionMatrix * @param name2 name of second element * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::string& name1, const std::string& name2, bool allowed); - /** @brief Set an entry corresponding to a pair of elements. This sets the type of the entry to - * AllowedCollision::CONDITIONAL. + /** @brief Set an entry corresponding to a pair of elements. + * + * This sets the type of the entry to AllowedCollision::CONDITIONAL. * @param name1 name of first element * @param name2 name of second element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is - * expected here */ + * @param fn A callback function that is used to decide if collisions are allowed between the two elements */ void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn); - /** @brief Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a - * pair using the name - * specified as argument to this function and set the entry as indicated by \e allowed. + /** @brief Set the entries corresponding to a name. + * With each of the *known names* in the collision matrix, form a pair using the name + * specified as argument to this function and set the entry as indicated by \e allowed. + * As the set of known names might change in future, consider using setDefaultEntry() instead! * @param name the object name * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::string& name, bool allowed); /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names @@ -165,8 +166,7 @@ class AllowedCollisionMatrix * matrix entries will be set for all such pairs. * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::string& name, const std::vector& other_names, bool allowed); /** @brief Set an entry corresponding to all possible pairs between two sets of elements @@ -174,15 +174,13 @@ class AllowedCollisionMatrix * @param names2 Second set of names * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::vector& names1, const std::vector& names2, bool allowed); /** @brief Set an entry corresponding to all known pairs * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(bool allowed); /** @brief Get all the names known to the collision matrix */ @@ -200,65 +198,42 @@ class AllowedCollisionMatrix return entries_.size(); } - /** @brief Set the default value for entries that include \e name. If such a default value is set, queries to - * getAllowedCollision() that include - * \e name will return this value instead, @b even if a pair that includes \e name was previously specified with - * setEntry(). + /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry(). * @param name The name of the element for which to set the default value * @param allowed If false, indicates that collisions between \e name and any other element must be checked for and - * no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and any other - * element are ok and - * an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + * no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and + * any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setDefaultEntry(const std::string& name, bool allowed); - /** @brief Set the default value for entries that include \e name to be AllowedCollision::CONDITIONAL and specify the - * allowed contact predicate to be \e fn. - * If this function is called, queries to getAllowedCollision() that include \e name will return this value instead, - * @b even if a pair that includes \e name - * was previously specified with setEntry(). + /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry(). * @param name The name of the element for which to set the default value * @param fn A callback function that is used to decide if collisions are allowed between \e name and some other * element is expected here. */ void setDefaultEntry(const std::string& name, DecideContactFn& fn); - /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a - * default value was - * found for the specified element. Return false otherwise. + /** @brief Get the type of the allowed collision to be considered by default for an element. + * Return true if a default value was found for the specified element. Return false otherwise. * @param name name of the element * @param allowed_collision The default allowed collision type will be filled here */ bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const; - /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a - * default value was - * found for the specified element. Return false otherwise. + /** @brief Get the type of the allowed collision between to be considered by default for an element. + * Return true if a default value was found for the specified element. Return false otherwise. * @param name name of the element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled - * here. */ + * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const; - /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included - * in the collision matrix - * (if the type is AllowedCollision::CONDITIONAL) or if one was computed from defaults. Return false if the entry is - * not found. - * Default values take precedence. If a default value is specified for either \e name1 or \e name2, that value is - * returned instead (if both - * elements have default values specified, both predicates must be satisfied). If no default values are specified, - * getEntry() is used to look - * for the pair (\e name1, \e name2). + /** @brief Get the allowed collision predicate between two elements. + * Return true if a predicate for entry is included in the collision matrix (if the type is AllowedCollision::CONDITIONAL) + * or if one was computed from defaults. Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled - * here */ + * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; - /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the - * collision matrix or if - * specified defaults were found. Return false if the entry is not found. Default values take precedence. If a - * default value is specified - * for either \e name1 or \e name2, that value is returned instead (if both elements have default values specified, - * AllowedCollision::NEVER takes - * precedence). If no default values are specified, getEntry() is used to look for the pair (\e name1, \e name2). + /** @brief Get the type of the allowed collision between two elements. + * Return true if the entry is included in the collision matrix or if specified defaults were found. + * Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element * @param allowed_collision The allowed collision type will be filled here */ @@ -269,6 +244,9 @@ class AllowedCollisionMatrix void print(std::ostream& out) const; private: + bool getDefaultEntry(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const; + std::map > entries_; std::map > allowed_contacts_; diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index ca840ac763..571ba98081 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -78,15 +78,7 @@ class CollisionDetectorPandaTest : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_ = std::make_shared(); - // Use default collision operations in the SRDF to setup the acm - const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); - acm_->setEntry(collision_links, collision_links, false); - - // allow collisions for pairs that have been disabled - const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); - for (const srdf::Model::DisabledCollision& it : dc) - acm_->setEntry(it.link1_, it.link2_, true); + acm_ = std::make_shared(*robot_model_->getSRDF()); cenv_ = value_->allocateEnv(robot_model_); @@ -280,7 +272,7 @@ TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle) { collision_detection::DistanceResult res; - shapes::ShapeConstPtr shape(new shapes::Cylinder(rng.uniform01(), rng.uniform01())); + shapes::ShapeConstPtr shape = std::make_shared(rng.uniform01(), rng.uniform01()); Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; pose.translation() = Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7)); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index 7643dfab07..b3c3f00374 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -87,6 +87,12 @@ class CollisionDetectorTest : public ::testing::Test std::string kinect_dae_resource_; }; +#ifdef NDEBUG +#define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL) +#else // Don't perform timing checks in Debug mode (but evaluate expression) +#define EXPECT_TIME_LT(EXPR, VAL) (void)(EXPR) +#endif + TYPED_TEST_CASE_P(CollisionDetectorTest); TYPED_TEST_P(CollisionDetectorTest, InitOK) @@ -363,7 +369,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) double second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - EXPECT_LT(fabs(first_check - second_check), .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .05); std::vector shapes; shapes.resize(1); @@ -384,7 +390,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); // the first check is going to take a while, as data must be constructed - EXPECT_LT(second_check, .1); + EXPECT_TIME_LT(second_check, .1); collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); before = std::chrono::system_clock::now(); @@ -394,7 +400,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) new_cenv->checkSelfCollision(req, res, robot_state); second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - EXPECT_LT(fabs(first_check - second_check), .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .05); } TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) @@ -422,7 +428,7 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) double second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - EXPECT_LT(second_check, .05); + EXPECT_TIME_LT(second_check, .05); collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); this->cenv_->getWorld()->removeObject("kinect"); @@ -461,8 +467,8 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_); second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - EXPECT_LT(first_check, .05); - EXPECT_LT(fabs(first_check - second_check), .1); + EXPECT_TIME_LT(first_check, .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .1); } TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) @@ -472,13 +478,13 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) for (unsigned int i = 0; i < 10000; ++i) { poses.push_back(Eigen::Isometry3d::Identity()); - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01))); + shapes.push_back(std::make_shared(.01, .01, .01)); } auto start = std::chrono::system_clock::now(); this->cenv_->getWorld()->addToObject("map", shapes, poses); double t = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); // TODO (j-petit): investigate why bullet collision checking is considerably slower here - EXPECT_GE(5.0, t); + EXPECT_TIME_LT(t, 5.0); // this is not really a failure; it is just that slow; // looking into doing collision checking with a voxel grid. RCLCPP_INFO(rclcpp::get_logger("moveit.core.collision_detection.bullet"), "Adding boxes took %g", t); @@ -526,7 +532,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) this->cenv_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); + shapes.push_back(std::make_shared(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)); poses.push_back(Eigen::Isometry3d::Identity()); this->cenv_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; @@ -548,7 +554,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) this->cenv_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); + shapes.push_back(std::make_shared(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)); poses.push_back(Eigen::Isometry3d::Identity()); this->cenv_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index bba3cc2e4d..7e547cb3ab 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -74,7 +74,7 @@ static inline bool validatePadding(double padding) namespace collision_detection { CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding, double scale) - : robot_model_(model), world_(new World()), world_const_(world_) + : robot_model_(model), world_(std::make_shared()), world_const_(world_) { if (!validateScale(scale)) scale = 1.0; diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 60795b381e..4941883fbb 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan, E. Gil Jones */ #include -#include +#include #include #include "rclcpp/rclcpp.hpp" @@ -55,33 +55,50 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector& n setEntry(names[i], names[j], allowed); } +AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf) +{ + // load collision defaults + for (const std::string& name : srdf.getNoDefaultCollisionLinks()) + setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); + // re-enable specific collision pairs + for (auto const& collision : srdf.getEnabledCollisionPairs()) + setEntry(collision.link1_, collision.link2_, false); + // *finally* disable selected collision pairs + for (auto const& collision : srdf.getDisabledCollisionPairs()) + setEntry(collision.link1_, collision.link2_, true); +} + AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg) { if (msg.entry_names.size() != msg.entry_values.size() || msg.default_entry_names.size() != msg.default_entry_values.size()) { RCLCPP_ERROR(LOGGER, "The number of links does not match the number of entries in AllowedCollisionMatrix message"); + return; } - else + for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i) { - for (std::size_t i = 0; i < msg.entry_names.size(); ++i) + setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]); + } + + for (std::size_t i = 0; i < msg.entry_names.size(); ++i) + { + if (msg.entry_values[i].enabled.size() != msg.entry_names.size()) { - if (msg.entry_values[i].enabled.size() != msg.entry_names.size()) - { - RCLCPP_ERROR(LOGGER, "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", - msg.entry_names[i].c_str()); - } - else - { - for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j) - { - setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]); - } - } + RCLCPP_ERROR(LOGGER, "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", + msg.entry_names[i].c_str()); + return; } + for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j) + { + AllowedCollision::Type allowed_default, allowed_entry; + if (!getDefaultEntry(msg.entry_names[i], msg.entry_names[j], allowed_default)) + allowed_default = AllowedCollision::NEVER; + allowed_entry = msg.entry_values[i].enabled[j] ? AllowedCollision::ALWAYS : AllowedCollision::NEVER; - for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i) - setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]); + if (allowed_entry != allowed_default) + setEntry(msg.entry_names[i], msg.entry_names[j], allowed_entry); + } } } @@ -267,54 +284,52 @@ static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const { - DecideContactFn fn1, fn2; - bool found1 = getDefaultEntry(name1, fn1); - bool found2 = getDefaultEntry(name2, fn2); - - if (!found1 && !found2) - return getEntry(name1, name2, fn); - else + bool found = getEntry(name1, name2, fn); + if (!found) { + DecideContactFn fn1, fn2; + bool found1 = getDefaultEntry(name1, fn1); + bool found2 = getDefaultEntry(name2, fn2); if (found1 && !found2) fn = fn1; else if (!found1 && found2) fn = fn2; else if (found1 && found2) - fn = boost::bind(&andDecideContact, fn1, fn2, _1); + fn = std::bind(&andDecideContact, fn1, fn2, std::placeholders::_1); else return false; - return true; } + return true; } -bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, - AllowedCollision::Type& allowed_collision) const +bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const { AllowedCollision::Type t1, t2; bool found1 = getDefaultEntry(name1, t1); bool found2 = getDefaultEntry(name2, t2); - if (!found1 && !found2) - return getEntry(name1, name2, allowed_collision); - else + return false; + else if (found1 && !found2) + allowed_collision = t1; + else if (!found1 && found2) + allowed_collision = t2; + else if (found1 && found2) { - if (found1 && !found2) - allowed_collision = t1; - else if (!found1 && found2) - allowed_collision = t2; - else if (found1 && found2) - { - if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER) - allowed_collision = AllowedCollision::NEVER; - else if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL) - allowed_collision = AllowedCollision::CONDITIONAL; - else // ALWAYS is the only remaining case - allowed_collision = AllowedCollision::ALWAYS; - } - else - return false; - return true; + if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER) + allowed_collision = AllowedCollision::NEVER; + else if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL) + allowed_collision = AllowedCollision::CONDITIONAL; + else // ALWAYS is the only remaining case + allowed_collision = AllowedCollision::ALWAYS; } + return true; +} + +bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const +{ + return getEntry(name1, name2, allowed_collision) || getDefaultEntry(name1, name2, allowed_collision); } void AllowedCollisionMatrix::clear() @@ -363,10 +378,9 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix for (std::size_t j = i; j < msg.entry_names.size(); ++j) { - AllowedCollision::Type type; - bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type); - if (found) - msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS; + AllowedCollision::Type type = AllowedCollision::NEVER; + getAllowedCollision(msg.entry_names[i], msg.entry_names[j], type); + msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = (type == AllowedCollision::ALWAYS); } } } @@ -393,7 +407,7 @@ void AllowedCollisionMatrix::print(std::ostream& out) const // print indices along the top of the matrix for (std::size_t j = 0; j < number_digits; ++j) { - out << std::setw(spacing + number_digits + 4) << ""; + out << std::setw(spacing + number_digits + 8) << ""; for (std::size_t i = 0; i < names.size(); ++i) { std::stringstream ss; @@ -403,17 +417,25 @@ void AllowedCollisionMatrix::print(std::ostream& out) const out << '\n'; } + const char* indicator = "01?"; // ALWAYS / NEVER / CONDITIONAL for (std::size_t i = 0; i < names.size(); ++i) { out << std::setw(spacing) << names[i]; out << std::setw(number_digits + 1) << i; out << " | "; + // print default value + AllowedCollision::Type type; + if (getDefaultEntry(names[i], type)) + out << indicator[type]; + else + out << '-'; + out << " | "; + // print pairs for (std::size_t j = 0; j < names.size(); ++j) { - AllowedCollision::Type type; bool found = getAllowedCollision(names[i], names[j], type); if (found) - out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?')); + out << std::setw(3) << indicator[type]; else out << std::setw(3) << '-'; } diff --git a/moveit_core/collision_detection/src/world_diff.cpp b/moveit_core/collision_detection/src/world_diff.cpp index dcf75b6d4d..c43b9d920a 100644 --- a/moveit_core/collision_detection/src/world_diff.cpp +++ b/moveit_core/collision_detection/src/world_diff.cpp @@ -35,7 +35,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ #include -#include +#include namespace collision_detection { @@ -52,7 +52,8 @@ WorldDiff::WorldDiff() WorldDiff::WorldDiff(const WorldPtr& world) : world_(world) { - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); } WorldDiff::WorldDiff(WorldDiff& other) @@ -63,7 +64,8 @@ WorldDiff::WorldDiff(WorldDiff& other) changes_ = other.changes_; WorldWeakPtr(world).swap(world_); - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); } } @@ -87,7 +89,8 @@ void WorldDiff::reset(const WorldPtr& world) old_world->removeObserver(observer_handle_); WorldWeakPtr(world).swap(world_); - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); } void WorldDiff::setWorld(const WorldPtr& world) @@ -101,7 +104,8 @@ void WorldDiff::setWorld(const WorldPtr& world) WorldWeakPtr(world).swap(world_); - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); world->notifyObserverAllObjects(observer_handle_, World::CREATE | World::ADD_SHAPE); } diff --git a/moveit_core/collision_detection/test/test_world.cpp b/moveit_core/collision_detection/test/test_world.cpp index 7bc7f57414..1c4b553972 100644 --- a/moveit_core/collision_detection/test/test_world.cpp +++ b/moveit_core/collision_detection/test/test_world.cpp @@ -37,16 +37,16 @@ #include #include #include -#include +#include TEST(World, AddRemoveShape) { collision_detection::World world; // Create some shapes - shapes::ShapePtr ball(new shapes::Sphere(1.0)); - shapes::ShapePtr box(new shapes::Box(1, 2, 3)); - shapes::ShapePtr cyl(new shapes::Cylinder(4, 5)); + shapes::ShapePtr ball = std::make_shared(1.0); + shapes::ShapePtr box = std::make_shared(1, 2, 3); + shapes::ShapePtr cyl = std::make_shared(4, 5); EXPECT_EQ(1, ball.use_count()); @@ -236,12 +236,12 @@ TEST(World, TrackChanges) TestAction ta; collision_detection::World::ObserverHandle observer_ta; - observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2)); + observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2)); // Create some shapes - shapes::ShapePtr ball(new shapes::Sphere(1.0)); - shapes::ShapePtr box(new shapes::Box(1, 2, 3)); - shapes::ShapePtr cyl(new shapes::Cylinder(4, 5)); + shapes::ShapePtr ball = std::make_shared(1.0); + shapes::ShapePtr box = std::make_shared(1, 2, 3); + shapes::ShapePtr cyl = std::make_shared(4, 5); world.addToObject("obj1", ball, Eigen::Isometry3d::Identity()); @@ -267,7 +267,7 @@ TEST(World, TrackChanges) TestAction ta2; collision_detection::World::ObserverHandle observer_ta2; - observer_ta2 = world.addObserver(boost::bind(TrackChangesNotify, &ta2, _1, _2)); + observer_ta2 = world.addObserver(std::bind(TrackChangesNotify, &ta2, std::placeholders::_1, std::placeholders::_2)); world.addToObject("obj2", cyl, Eigen::Isometry3d::Identity()); @@ -305,7 +305,7 @@ TEST(World, TrackChanges) TestAction ta3; collision_detection::World::ObserverHandle observer_ta3; - observer_ta3 = world.addObserver(boost::bind(TrackChangesNotify, &ta3, _1, _2)); + observer_ta3 = world.addObserver(std::bind(TrackChangesNotify, &ta3, std::placeholders::_1, std::placeholders::_2)); bool rm_good = world.removeShapeFromObject("obj2", cyl); EXPECT_TRUE(rm_good); @@ -371,12 +371,12 @@ TEST(World, ObjectPoseAndSubframes) TestAction ta; collision_detection::World::ObserverHandle observer_ta; - observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2)); + observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2)); // Create shapes - shapes::ShapePtr ball(new shapes::Sphere(1.0)); - shapes::ShapePtr box(new shapes::Box(1, 1, 1)); - shapes::ShapePtr cyl(new shapes::Cylinder(0.5, 3)); // radius, length + shapes::ShapePtr ball = std::make_shared(1.0); + shapes::ShapePtr box = std::make_shared(1, 1, 1); + shapes::ShapePtr cyl = std::make_shared(0.5, 3); // radius, length // Confirm that setting object pose creates an object world.setObjectPose("mix1", Eigen::Isometry3d::Identity()); diff --git a/moveit_core/collision_detection/test/test_world_diff.cpp b/moveit_core/collision_detection/test/test_world_diff.cpp index f5dd079b8d..e073cb17c1 100644 --- a/moveit_core/collision_detection/test/test_world_diff.cpp +++ b/moveit_core/collision_detection/test/test_world_diff.cpp @@ -40,7 +40,7 @@ TEST(WorldDiff, TrackChanges) { - collision_detection::WorldPtr world(new collision_detection::World); + collision_detection::WorldPtr world = std::make_shared(); collision_detection::WorldDiff diff1(world); collision_detection::WorldDiff diff2; collision_detection::WorldDiff::const_iterator it; @@ -49,9 +49,9 @@ TEST(WorldDiff, TrackChanges) EXPECT_EQ(diff2.getChanges().size(), 0u); // Create some shapes - shapes::ShapePtr ball(new shapes::Sphere(1.0)); - shapes::ShapePtr box(new shapes::Box(1, 2, 3)); - shapes::ShapePtr cyl(new shapes::Cylinder(4, 5)); + shapes::ShapePtr ball = std::make_shared(1.0); + shapes::ShapePtr box = std::make_shared(1, 2, 3); + shapes::ShapePtr cyl = std::make_shared(4, 5); world->addToObject("obj1", ball, Eigen::Isometry3d::Identity()); @@ -190,16 +190,16 @@ TEST(WorldDiff, TrackChanges) TEST(WorldDiff, SetWorld) { - collision_detection::WorldPtr world1(new collision_detection::World); - collision_detection::WorldPtr world2(new collision_detection::World); + collision_detection::WorldPtr world1 = std::make_shared(); + collision_detection::WorldPtr world2 = std::make_shared(); collision_detection::WorldDiff diff1(world1); collision_detection::WorldDiff diff1b(world1); collision_detection::WorldDiff diff2(world2); collision_detection::WorldDiff::const_iterator it; - shapes::ShapePtr ball(new shapes::Sphere(1.0)); - shapes::ShapePtr box(new shapes::Box(1, 2, 3)); - shapes::ShapePtr cyl(new shapes::Cylinder(4, 5)); + shapes::ShapePtr ball = std::make_shared(1.0); + shapes::ShapePtr box = std::make_shared(1, 2, 3); + shapes::ShapePtr cyl = std::make_shared(4, 5); world1->addToObject("objA1", ball, Eigen::Isometry3d::Identity()); diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp index d54a29b2aa..712c03f7b8 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -41,7 +41,7 @@ namespace collision_detection_bullet { BulletCastBVHManagerPtr BulletCastBVHManager::clone() const { - BulletCastBVHManagerPtr manager(new BulletCastBVHManager()); + BulletCastBVHManagerPtr manager = std::make_shared(); for (const std::pair& cow : link2cow_) { diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp index a34890f030..8ea5ce55dd 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -39,7 +39,7 @@ namespace collision_detection_bullet { BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const { - BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager()); + auto manager = std::make_shared(); for (const std::pair& cow : link2cow_) { diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index 34d247fc64..eab7da4187 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include namespace collision_detection @@ -51,7 +51,8 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m : CollisionEnv(model, padding, scale) { // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); for (const std::pair& link : robot_model_->getURDF()->links_) { @@ -64,7 +65,8 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m : CollisionEnv(model, world, padding, scale) { // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); for (const std::pair& link : robot_model_->getURDF()->links_) { @@ -78,7 +80,8 @@ CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const Wo : CollisionEnv(other, world) { // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); for (const std::pair& link : other.robot_model_->getURDF()->links_) { @@ -301,7 +304,8 @@ void CollisionEnvBullet::setWorld(const WorldPtr& world) CollisionEnv::setWorld(world); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); // get notifications any objects already in the new world getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); @@ -336,9 +340,10 @@ void CollisionEnvBullet::addAttachedOjects(const moveit::core::RobotState& state try { - collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( - body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform, - collision_object_types, body->getTouchLinks())); + collision_detection_bullet::CollisionObjectWrapperPtr cow = + std::make_shared( + body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), + attached_body_transform, collision_object_types, body->getTouchLinks()); cows.push_back(cow); } catch (std::exception&) @@ -423,8 +428,9 @@ void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& lin try { - collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( - link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true)); + collision_detection_bullet::CollisionObjectWrapperPtr cow = + std::make_shared( + link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true); manager_->addCollisionObject(cow); manager_CCD_->addCollisionObject(cow->clone()); active_.push_back(cow->getName()); 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 0c2059c306..2c446892d7 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 @@ -78,15 +78,7 @@ class BulletCollisionDetectionTester : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_ = std::make_shared(); - // Use default collision operations in the SRDF to setup the acm - const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); - acm_->setEntry(collision_links, collision_links, false); - - // allow collisions for pairs that have been disabled - const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); - for (const srdf::Model::DisabledCollision& it : dc) - acm_->setEntry(it.link1_, it.link2_, true); + acm_ = std::make_shared(*robot_model_->getSRDF()); cenv_ = std::make_shared(robot_model_); @@ -116,7 +108,7 @@ void addCollisionObjects(cb::BulletCastBVHManager& checker) //////////////////////////// // Add static box to checker //////////////////////////// - shapes::ShapePtr static_box(new shapes::Box(1, 1, 1)); + shapes::ShapePtr static_box = std::make_shared(1, 1, 1); Eigen::Isometry3d static_box_pose; static_box_pose.setIdentity(); @@ -127,14 +119,14 @@ void addCollisionObjects(cb::BulletCastBVHManager& checker) obj1_poses.push_back(static_box_pose); obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); - cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper( - "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types)); + cb::CollisionObjectWrapperPtr cow_1 = std::make_shared( + "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types); checker.addCollisionObject(cow_1); //////////////////////////// // Add moving box to checker //////////////////////////// - shapes::ShapePtr moving_box(new shapes::Box(0.2, 0.2, 0.2)); + shapes::ShapePtr moving_box = std::make_shared(0.2, 0.2, 0.2); Eigen::Isometry3d moving_box_pose; moving_box_pose.setIdentity(); @@ -147,8 +139,8 @@ void addCollisionObjects(cb::BulletCastBVHManager& checker) obj2_poses.push_back(moving_box_pose); obj2_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); - cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper( - "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types)); + cb::CollisionObjectWrapperPtr cow_2 = std::make_shared( + "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types); checker.addCollisionObject(cow_2); } @@ -157,7 +149,7 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker) //////////////////////////// // Add static box to checker //////////////////////////// - shapes::ShapePtr static_box(new shapes::Box(0.3, 0.3, 0.3)); + shapes::ShapePtr static_box = std::make_shared(0.3, 0.3, 0.3); Eigen::Isometry3d static_box_pose; static_box_pose.setIdentity(); @@ -168,8 +160,8 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker) obj1_poses.push_back(static_box_pose); obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); - cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper( - "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types)); + cb::CollisionObjectWrapperPtr cow_1 = std::make_shared( + "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types); checker.addCollisionObject(cow_1); //////////////////////////// // Add moving mesh to checker @@ -191,8 +183,8 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker) obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL); obj2_poses.push_back(s_pose); - cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper( - "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types)); + cb::CollisionObjectWrapperPtr cow_2 = std::make_shared( + "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types); checker.addCollisionObject(cow_2); } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index ee5a00beab..e5c5beac41 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -206,21 +206,24 @@ struct FCLGeometry /** \brief Constructor for a robot link. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::LinkModel* link, int shape_index) - : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index)) + : collision_geometry_(collision_geometry) + , collision_geometry_data_(std::make_shared(link, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } /** \brief Constructor for an attached body. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index) - : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index)) + : collision_geometry_(collision_geometry) + , collision_geometry_data_(std::make_shared(ab, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } /** \brief Constructor for a world object. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const World::Object* obj, int shape_index) - : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index)) + : collision_geometry_(collision_geometry) + , collision_geometry_data_(std::make_shared(obj, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index da4c5d4549..d2bbe354ff 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -857,7 +857,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, if (cg_g) { cg_g->computeLocalAABB(); - FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index)); + FCLGeometryConstPtr res = std::make_shared(cg_g, data, shape_index); cache.map_[wptr] = res; cache.bumpUseCount(); return res; 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 1777efe62e..71f229a805 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -97,8 +97,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, // Every time this object is created, g->computeLocalAABB() is called which is // very expensive and should only be calculated once. To update the AABB, use the // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. - robot_fcl_objs_[index] = - FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_)); + robot_fcl_objs_[index] = FCLCollisionObjectConstPtr( + std::make_shared(link_geometry->collision_geometry_)); } else RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); @@ -107,7 +107,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, manager_ = std::make_unique(); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); } CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, @@ -133,7 +134,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, // Every time this object is created, g->computeLocalAABB() is called which is // very expensive and should only be calculated once. To update the AABB, use the // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. - robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); + robot_fcl_objs_[index] = std::make_shared(g->collision_geometry_); } else RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); @@ -142,7 +143,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, manager_ = std::make_unique(); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } @@ -164,7 +166,8 @@ CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& w // manager_->update(); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); } void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* ab, @@ -405,7 +408,8 @@ void CollisionEnvFCL::setWorld(const WorldPtr& world) CollisionEnv::setWorld(world); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); // get notifications any objects already in the new world getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); @@ -448,7 +452,7 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& li { index = lmodel->getFirstCollisionBodyTransformIndex() + j; robot_geoms_[index] = g; - robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); + robot_fcl_objs_[index] = std::make_shared(g->collision_geometry_); } } } 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 9fcd49e826..655b5ae7fb 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 @@ -82,7 +82,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons } } - BodyDecompositionConstPtr bdcp(new BodyDecomposition(shape, resolution)); + BodyDecompositionConstPtr bdcp = std::make_shared(shape, resolution); { boost::mutex::scoped_lock slock(cache.lock_); cache.map_[wptr] = bdcp; @@ -95,7 +95,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj, double resolution) { - PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector()); + PosedBodyPointDecompositionVectorPtr ret = std::make_shared(); for (unsigned int i = 0; i < obj.shapes_.size(); ++i) { PosedBodyPointDecompositionPtr pbd( @@ -109,7 +109,7 @@ PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, double resolution) { - PosedBodySphereDecompositionVectorPtr ret(new PosedBodySphereDecompositionVector()); + PosedBodySphereDecompositionVectorPtr ret = std::make_shared(); for (unsigned int i = 0; i < att->getShapes().size(); ++i) { PosedBodySphereDecompositionPtr pbd( @@ -123,11 +123,11 @@ PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const m PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, double resolution) { - PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector()); + PosedBodyPointDecompositionVectorPtr ret = std::make_shared(); for (unsigned int i = 0; i < att->getShapes().size(); ++i) { - PosedBodyPointDecompositionPtr pbd( - new PosedBodyPointDecomposition(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution))); + PosedBodyPointDecompositionPtr pbd = + std::make_shared(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution)); ret->addToVector(pbd); ret->updatePose(ret->getSize() - 1, att->getGlobalCollisionBodyTransforms()[i]); } 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 d069d47e4f..9dece47a2d 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 @@ -55,7 +55,7 @@ std::vector determineCollisionSpheres(const bodies::Body* body, body->computeBoundingCylinder(cyl); unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0)); double spacing = cyl.length / ((num_points * 1.0) - 1.0); - relative_transform = body->getPose().inverse() * cyl.pose; + relative_transform = cyl.pose; for (unsigned int i = 1; i < num_points - 1; ++i) { 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 33b55481e0..a5b2bd6af8 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 @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include @@ -71,7 +71,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); // request notifications about changes to world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); } CollisionEnvDistanceField::CollisionEnvDistanceField( @@ -87,7 +88,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); // request notifications about changes to world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } @@ -110,7 +112,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(const CollisionEnvDistanceF planning_scene_ = std::make_shared(robot_model_); // request notifications about changes to world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } @@ -707,7 +710,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache const std::string& group_name, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const { - DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry()); + DistanceFieldCacheEntryPtr dfce = std::make_shared(); if (robot_model_->getJointModelGroup(group_name) == nullptr) { @@ -966,9 +969,9 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions(double resolution) } RCLCPP_DEBUG(LOGGER, "Generating model for %s", link_model->getName().c_str()); - BodyDecompositionConstPtr bd(new BodyDecomposition(link_model->getShapes(), - link_model->getCollisionOriginTransforms(), resolution, - getLinkPadding(link_model->getName()))); + BodyDecompositionConstPtr bd = + std::make_shared(link_model->getShapes(), link_model->getCollisionOriginTransforms(), + resolution, getLinkPadding(link_model->getName())); link_body_decomposition_vector_.push_back(bd); link_body_decomposition_index_map_[link_model->getName()] = link_body_decomposition_vector_.size() - 1; } @@ -1057,8 +1060,9 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions( continue; } - BodyDecompositionPtr bd(new BodyDecomposition(link_model->getShapes(), link_model->getCollisionOriginTransforms(), - resolution, getLinkPadding(link_model->getName()))); + BodyDecompositionPtr bd = + std::make_shared(link_model->getShapes(), link_model->getCollisionOriginTransforms(), + resolution, getLinkPadding(link_model->getName())); RCLCPP_DEBUG(LOGGER, "Generated model for %s", link_model->getName().c_str()); @@ -1689,7 +1693,8 @@ void CollisionEnvDistanceField::setWorld(const WorldPtr& world) CollisionEnv::setWorld(world); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); // get notifications any objects already in the new world getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); @@ -1776,7 +1781,7 @@ void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, Dist CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld() { - DistanceFieldCacheEntryWorldPtr dfce(new DistanceFieldCacheEntryWorld()); + DistanceFieldCacheEntryWorldPtr dfce = std::make_shared(); dfce->distance_field_ = std::make_shared( size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(), origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_); diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp index f1abf3fb4c..7f1686022b 100644 --- a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -47,7 +47,7 @@ CollisionEnvHybrid::CollisionEnvHybrid( 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) : CollisionEnvFCL(robot_model) - , cenv_distance_(new collision_detection::CollisionEnvDistanceField( + , cenv_distance_(std::make_shared( robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance, padding, scale)) { @@ -59,7 +59,7 @@ CollisionEnvHybrid::CollisionEnvHybrid( 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) : CollisionEnvFCL(robot_model, world, padding, scale) - , cenv_distance_(new collision_detection::CollisionEnvDistanceField( + , cenv_distance_(std::make_shared( robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance, padding, scale)) { @@ -67,7 +67,8 @@ CollisionEnvHybrid::CollisionEnvHybrid( CollisionEnvHybrid::CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world) : CollisionEnvFCL(other, world) - , cenv_distance_(new collision_detection::CollisionEnvDistanceField(*other.getCollisionWorldDistanceField(), world)) + , cenv_distance_(std::make_shared( + *other.getCollisionWorldDistanceField(), world)) { } diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index fe36904ab1..de7af9f693 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -292,14 +292,13 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) const auto identity = Eigen::Isometry3d::Identity(); std::vector shapes; EigenSTL::vector_Isometry3d poses; - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.25, .25, .25))); + shapes.push_back(std::make_shared(.25, .25, .25)); poses.push_back(identity); std::set touch_links; trajectory_msgs::msg::JointTrajectory empty_state; - moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( - robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state); - robot_state.attachBody(attached_body); + robot_state.attachBody(std::make_unique( + robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state)); res = collision_detection::CollisionResult(); cenv_->checkSelfCollision(req, res, robot_state, *acm_); @@ -311,9 +310,8 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) touch_links.insert("r_gripper_palm_link"); shapes[0] = std::make_shared(.1, .1, .1); - moveit::core::AttachedBody* attached_body_1 = new moveit::core::AttachedBody( - robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state); - robot_state.attachBody(attached_body_1); + robot_state.attachBody(std::make_unique( + robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state)); res = collision_detection::CollisionResult(); cenv_->checkSelfCollision(req, res, robot_state, *acm_); diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index d69403a6fc..fdf8b850e9 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -107,7 +107,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni // if we have constrained every joint, then we just use a sampler using these constraints if (full_coverage) { - JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); + JointConstraintSamplerPtr sampler = std::make_shared(scene, jmg->getName()); if (sampler->configure(jc)) { RCLCPP_DEBUG(LOGGER, "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); @@ -119,7 +119,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni // sampler has been specified. if (!jc.empty()) { - JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); + JointConstraintSamplerPtr sampler = std::make_shared(scene, jmg->getName()); if (sampler->configure(jc)) { RCLCPP_DEBUG(LOGGER, @@ -166,7 +166,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni if (pc->configure(constr.position_constraints[p], scene->getTransforms()) && oc->configure(constr.orientation_constraints[o], scene->getTransforms())) { - IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName())); + IKConstraintSamplerPtr iks = std::make_shared(scene, jmg->getName()); if (iks->configure(IKSamplingPose(pc, oc))) { bool use = true; @@ -202,7 +202,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni new kinematic_constraints::PositionConstraint(scene->getRobotModel())); if (pc->configure(position_constraint, scene->getTransforms())) { - IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName())); + IKConstraintSamplerPtr iks = std::make_shared(scene, jmg->getName()); if (iks->configure(IKSamplingPose(pc))) { bool use = true; @@ -232,7 +232,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni new kinematic_constraints::OrientationConstraint(scene->getRobotModel())); if (oc->configure(orientation_constraint, scene->getTransforms())) { - IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName())); + IKConstraintSamplerPtr iks = std::make_shared(scene, jmg->getName()); if (iks->configure(IKSamplingPose(oc))) { bool use = true; @@ -258,7 +258,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni else { samplers.push_back(used_l.begin()->second); - return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers)); + return std::make_shared(scene, jmg->getName(), samplers); } } else if (used_l.size() > 1) @@ -284,7 +284,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni else { samplers.push_back(iks); - return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers)); + return std::make_shared(scene, jmg->getName(), samplers); } } } @@ -343,7 +343,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni { RCLCPP_DEBUG(LOGGER, "Constructing sampler for group '%s' as a union of %zu samplers", jmg->getName().c_str(), samplers.size()); - return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers)); + return std::make_shared(scene, jmg->getName(), samplers); } } diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 4dac6535f8..14ce4017c4 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include namespace constraint_samplers { @@ -197,19 +197,19 @@ IKSamplingPose::IKSamplingPose() } IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraint& pc) - : position_constraint_(new kinematic_constraints::PositionConstraint(pc)) + : position_constraint_(std::make_shared(pc)) { } IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc) - : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc)) + : orientation_constraint_(std::make_shared(oc)) { } IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraint& pc, const kinematic_constraints::OrientationConstraint& oc) - : position_constraint_(new kinematic_constraints::PositionConstraint(pc)) - , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc)) + : position_constraint_(std::make_shared(pc)) + , orientation_constraint_(std::make_shared(oc)) { } @@ -561,8 +561,8 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback; if (group_state_validity_callback_) - adapted_ik_validity_callback = - boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3); + adapted_ik_validity_callback = std::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); for (unsigned int a = 0; a < max_attempts; ++a) { diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index c1036af58b..fb30b530f3 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include "pr2_arm_kinematics_plugin.h" @@ -83,8 +83,8 @@ class LoadPlanningModelsPr2 : public testing::Test pr2_kinematics_plugin_left_arm_->initialize(node_, *robot_model_, "left_arm", "torso_lift_link", { "l_wrist_roll_link" }, .01); - func_right_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1); - func_left_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1); + func_right_arm_ = std::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, std::placeholders::_1); + func_left_arm_ = std::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, std::placeholders::_1); std::map allocators; allocators["right_arm"] = func_right_arm_; @@ -530,7 +530,8 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) std::vector js2; js2.push_back(jc2); - constraint_samplers::JointConstraintSamplerPtr jcsp2(new constraint_samplers::JointConstraintSampler(ps_, "arms")); + constraint_samplers::JointConstraintSamplerPtr jcsp2 = + std::make_shared(ps_, "arms"); EXPECT_TRUE(jcsp2->configure(js2)); kinematic_constraints::PositionConstraint pc(robot_model_); @@ -539,7 +540,8 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) kinematic_constraints::OrientationConstraint oc(robot_model_); EXPECT_TRUE(oc.configure(ocm, tf)); - constraint_samplers::IKConstraintSamplerPtr iksp(new constraint_samplers::IKConstraintSampler(ps_, "left_arm")); + constraint_samplers::IKConstraintSamplerPtr iksp = + std::make_shared(ps_, "left_arm"); EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc))); EXPECT_TRUE(iksp->isValid()); @@ -583,7 +585,8 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) kinematic_constraints::OrientationConstraint oc2(robot_model_); EXPECT_TRUE(oc2.configure(ocm, tf)); - constraint_samplers::IKConstraintSamplerPtr iksp2(new constraint_samplers::IKConstraintSampler(ps_, "right_arm")); + constraint_samplers::IKConstraintSamplerPtr iksp2 = + std::make_shared(ps_, "right_arm"); EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2))); EXPECT_TRUE(iksp2->isValid()); diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index 347983c5d3..3aa26b017e 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -908,7 +908,7 @@ TEST(TestSignedPropagationDistanceField, TestOcTree) std::cout << "Occupied cells " << countOccupiedCells(df_highres) << '\n'; // testing adding shape that happens to be octree - std::shared_ptr tree_shape(new octomap::OcTree(.05)); + auto tree_shape = std::make_shared(.05); octomap::point3d tpoint1(1.0, .5, 1.0); octomap::point3d tpoint2(1.7, .5, .5); octomap::point3d tpoint3(1.8, .5, .5); @@ -916,7 +916,7 @@ TEST(TestSignedPropagationDistanceField, TestOcTree) tree_shape->updateNode(tpoint2, true); tree_shape->updateNode(tpoint3, true); - std::shared_ptr shape_oc(new shapes::OcTree(tree_shape)); + auto shape_oc = std::make_shared(tree_shape); PropagationDistanceField df_test_shape_1(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION, PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 9e0d9d6f2d..acac1617b3 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -46,7 +46,7 @@ #else #include #endif -#include +#include #include #include #include @@ -758,7 +758,7 @@ void OrientationConstraint::print(std::ostream& out) const } VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model) - : KinematicConstraint(model), collision_env_(new collision_detection::CollisionEnvFCL(model)) + : KinematicConstraint(model), collision_env_(std::make_shared(model)) { type_ = VISIBILITY_CONSTRAINT; } diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index f95addbf92..4260de0773 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan */ #include -#include +#include #include #include "rclcpp/rclcpp.hpp" @@ -66,8 +66,9 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag planning_interface::MotionPlanResponse& res, std::vector& added_path_index) const { - return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res, - added_path_index); + return adaptAndPlan(std::bind(&callPlannerInterfaceSolve, planner.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + planning_scene, req, res, added_path_index); } bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, @@ -148,10 +149,12 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner // if there are adapters, construct a function pointer for each, in order, // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order. - PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3, - boost::ref(added_path_index_each.back())); + PlanningRequestAdapter::PlannerFn fn = + std::bind(&callAdapter1, adapters_.back().get(), planner, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::ref(added_path_index_each.back())); for (int i = adapters_.size() - 2; i >= 0; --i) - fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i])); + fn = std::bind(&callAdapter2, adapters_[i].get(), fn, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::ref(added_path_index_each[i])); bool result = fn(planning_scene, req, res); added_path_index.clear(); 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 5b3c06099a..089f3788d6 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 @@ -513,37 +513,42 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable, getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix()); } - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. + * Can be restricted to links part of or updated by \e group_name */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const moveit::core::RobotState& robot_state) const + const moveit::core::RobotState& robot_state, const std::string& group_name = "") const { - getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix()); + getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - Update the link transforms for \e robot_state if needed. */ + Update the link transforms for \e robot_state if needed. + Can be restricted to links part of or updated by \e group_name */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - moveit::core::RobotState& robot_state) const + moveit::core::RobotState& robot_state, const std::string& group_name = "") const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix()); + getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix(), + group_name); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */ + allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. + Can be restricted to links part of or updated by \e group_name*/ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name = "") const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), acm); + getCollidingPairs(contacts, static_cast(robot_state), acm, group_name); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm) */ + allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; + const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name = "") const; /**@}*/ diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index ec1848afae..6bc00b55a1 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -156,15 +156,7 @@ void PlanningScene::initialize() robot_state_->setToDefaultValues(); robot_state_->update(); - acm_ = std::make_shared(); - // Use default collision operations in the SRDF to setup the acm - const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); - acm_->setEntry(collision_links, collision_links, false); - - // allow collisions for pairs that have been disabled - const std::vector& dc = getRobotModel()->getSRDF()->getDisabledCollisionPairs(); - for (const srdf::Model::DisabledCollision& it : dc) - acm_->setEntry(it.link1_, it.link2_, true); + acm_ = std::make_shared(*getRobotModel()->getSRDF()); allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } @@ -173,7 +165,7 @@ void PlanningScene::initialize() moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model) { - moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model)); + auto robot_model = std::make_shared(urdf_model, srdf_model); if (!robot_model || !robot_model->getRootJoint()) return moveit::core::RobotModelPtr(); @@ -439,12 +431,14 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const + const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name) const { collision_detection::CollisionRequest req; req.contacts = true; req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1; req.max_contacts_per_pair = 1; + req.group_name = group_name; collision_detection::CollisionResult res; checkCollision(req, res, robot_state, acm); res.contacts.swap(contacts); @@ -493,7 +487,7 @@ moveit::core::RobotState& PlanningScene::getCurrentStateNonConst() moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const { - moveit::core::RobotStatePtr state(new moveit::core::RobotState(getCurrentState())); + auto state = std::make_shared(getCurrentState()); moveit::core::robotStateMsgToRobotState(getTransforms(), update, *state); return state; } @@ -907,7 +901,23 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet RCLCPP_ERROR(LOGGER, "Bad input stream when loading scene geometry"); return false; } + // Read scene name std::getline(in, name_); + + // Identify scene format version for backwards compatibility of parser + auto pos = in.tellg(); // remember current stream position + std::string line; + do + { + std::getline(in, line); + } while (in.good() && !in.eof() && (line.empty() || line[0] != '*')); // read * marker + std::getline(in, line); // next line determines format + boost::algorithm::trim(line); + // new format: line specifies position of object, with spaces as delimiter -> spaces indicate new format + // old format: line specifies number of shapes + bool uses_new_scene_format = line.find(' ') != std::string::npos; + in.seekg(pos); + Eigen::Isometry3d pose; // Transient do { @@ -929,8 +939,9 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } boost::algorithm::trim(object_id); - // Read in object pose - if (!readPoseFromText(in, pose)) + // Read in object pose (added in the new scene format) + pose.setIdentity(); + if (uses_new_scene_format && !readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read object pose from scene file"); return false; @@ -975,22 +986,25 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } } - // Read in subframes - moveit::core::FixedTransformsMap subframes; - unsigned int subframe_count; - in >> subframe_count; - for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i) + // Read in subframes (added in the new scene format) + if (uses_new_scene_format) { - std::string subframe_name; - in >> subframe_name; - if (!readPoseFromText(in, pose)) + moveit::core::FixedTransformsMap subframes; + unsigned int subframe_count; + in >> subframe_count; + for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i) { - RCLCPP_ERROR(LOGGER, "Failed to read subframe pose from scene file"); - return false; + std::string subframe_name; + in >> subframe_name; + if (!readPoseFromText(in, pose)) + { + RCLCPP_ERROR(LOGGER, "Failed to read subframe pose from scene file"); + return false; + } + subframes[subframe_name] = pose; } - subframes[subframe_name] = pose; + world_->setSubframesOfObject(object_id, subframes); } - world_->setSubframesOfObject(object_id, subframes); } else if (marker == ".") { @@ -1254,11 +1268,11 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::Octomap& map) if (!map.header.frame_id.empty()) { const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); + world_->addToObject(OCTOMAP_NS, std::make_shared(om), t); } else { - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Isometry3d::Identity()); + world_->addToObject(OCTOMAP_NS, std::make_shared(om), Eigen::Isometry3d::Identity()); } } @@ -1294,7 +1308,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& Eigen::Isometry3d p; PlanningScene::poseMsgToEigen(map.origin, p); p = t * p; - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p); + world_->addToObject(OCTOMAP_NS, std::make_shared(om), p); } void PlanningScene::processOctomapPtr(const std::shared_ptr& octree, const Eigen::Isometry3d& t) @@ -1327,7 +1341,7 @@ void PlanningScene::processOctomapPtr(const std::shared_ptrremoveObject(OCTOMAP_NS); - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(octree)), t); + world_->addToObject(OCTOMAP_NS, std::make_shared(octree), t); } bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& object) diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index a7ad233427..b41b0ec5cc 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -51,7 +51,7 @@ TEST(PlanningScene, LoadRestore) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf::ModelSharedPtr srdf_model = std::make_shared(); planning_scene::PlanningScene ps(urdf_model, srdf_model); moveit_msgs::msg::PlanningScene ps_msg; ps.getPlanningSceneMsg(ps_msg); @@ -65,14 +65,14 @@ TEST(PlanningScene, LoadRestore) TEST(PlanningScene, LoadRestoreDiff) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf::ModelSharedPtr srdf_model = std::make_shared(); auto ps = std::make_shared(urdf_model, srdf_model); collision_detection::World& world = *ps->getWorldNonConst(); /* add one object to ps's world */ Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); - world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id); + world.addToObject("sphere", std::make_shared(0.4), id); /* ps can be written to and set from message */ moveit_msgs::msg::PlanningScene ps_msg; @@ -90,7 +90,7 @@ TEST(PlanningScene, LoadRestoreDiff) EXPECT_TRUE(next->getWorld()->hasObject("sphere")); /* object in overlay is only added in overlay */ - next->getWorldNonConst()->addToObject("sphere_in_next_only", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id); + next->getWorldNonConst()->addToObject("sphere_in_next_only", std::make_shared(0.5), id); EXPECT_EQ(next->getWorld()->size(), 2u); EXPECT_EQ(ps->getWorld()->size(), 1u); @@ -125,13 +125,13 @@ TEST(PlanningScene, LoadRestoreDiff) TEST(PlanningScene, MakeAttachedDiff) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf::ModelSharedPtr srdf_model = std::make_shared(); auto ps = std::make_shared(urdf_model, srdf_model); /* add a single object to ps's world */ collision_detection::World& world = *ps->getWorldNonConst(); Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); - world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id); + world.addToObject("sphere", std::make_shared(0.4), id); /* attach object in diff */ planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff(); @@ -164,7 +164,7 @@ TEST(PlanningScene, isStateValid) } } -TEST(PlanningScene, loadGoodSceneGeometry) +TEST(PlanningScene, loadGoodSceneGeometryNewFormat) { moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); @@ -199,6 +199,32 @@ TEST(PlanningScene, loadGoodSceneGeometry) EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check. } +TEST(PlanningScene, loadGoodSceneGeometryOldFormat) +{ + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); + + std::istringstream good_scene_geometry; + good_scene_geometry.str("foobar_scene\n" + "* foo\n" + "2\n" + "box\n" + ".77 0.39 0.05\n" + "0 0 0.025\n" + "0 0 0 1\n" + "0.82 0.75 0.60 1\n" + "box\n" + ".77 0.39 0.05\n" + "0 0 1.445\n" + "0 0 0 1\n" + "0.82 0.75 0.60 1\n" + ".\n"); + EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry)); + EXPECT_EQ(ps->getName(), "foobar_scene"); + EXPECT_TRUE(ps->getWorld()->hasObject("foo")); + EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check. +} + TEST(PlanningScene, loadBadSceneGeometry) { moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); @@ -211,6 +237,8 @@ TEST(PlanningScene, loadBadSceneGeometry) std::istringstream malformed_scene_geometry; malformed_scene_geometry.str("malformed_scene_geometry\n" "* foo\n" + "0 0 0\n" + "0 0 0 1\n" "1\n" "box\n" "2.58 1.36\n" /* Only two tokens; should be 3 */ @@ -249,7 +277,7 @@ TEST_P(CollisionDetectorTests, ClearDiff) SCOPED_TRACE(plugin_name); urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf::ModelSharedPtr srdf_model = std::make_shared(); // create parent scene planning_scene::PlanningScenePtr parent = std::make_shared(urdf_model, srdf_model); diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 5a60d2821e..d7bf0175b3 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1022,7 +1022,7 @@ class RobotState @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, + const std::vector& tips, const std::vector>& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -1036,7 +1036,7 @@ class RobotState @param constraint A state validity constraint to be required for IK solutions */ bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, - const std::vector >& consistency_limits, double timeout = 0.0, + const std::vector>& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -1574,6 +1574,18 @@ class RobotState * @{ */ + /** \brief Add an attached body to this state. + * + * This only adds the given body to this RobotState + * instance. It does not change anything about other + * representations of the object elsewhere in the system. So if the + * body represents an object in a collision_detection::World (like + * from a planning_scene::PlanningScene), you will likely need to remove the + * corresponding object from that world to avoid having collisions + * detected against it. + **/ + void attachBody(std::unique_ptr attached_body); + /** \brief Add an attached body to this state. Ownership of the * memory for the attached body is assumed by the state. * @@ -1593,7 +1605,7 @@ class RobotState * the body positions will get corrupted. You need to make a fresh * copy of the AttachedBody object for each RobotState you attach it * to.*/ - void attachBody(AttachedBody* attached_body); + [[deprecated("Deprecated. Pass a unique_ptr instead")]] void attachBody(AttachedBody* attached_body); /** @brief Add an attached body to a link * @param id The string id associated with the attached body @@ -1897,7 +1909,7 @@ class RobotState unsigned char* dirty_joint_transforms_; /** \brief All attached bodies that are part of this state, indexed by their name */ - std::map attached_body_map_; + std::map> attached_body_map_; /** \brief This event is called when there is a change in the attached bodies for this state; The event specifies the body that changed and whether it was just attached or about to be detached. */ diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 79606d55dc..8785dd1495 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -46,7 +46,7 @@ #include #endif #include -#include +#include #include #include "rclcpp/rclcpp.hpp" @@ -109,6 +109,7 @@ void RobotState::allocMemory() // make the memory for transforms align at EIGEN_MAX_ALIGN_BYTES // https://eigen.tuxfamily.org/dox/classEigen_1_1aligned__allocator.html + // NOLINTNEXTLINE(performance-no-int-to-ptr) variable_joint_transforms_ = reinterpret_cast(((uintptr_t)memory_ + extra_alignment_bytes) & ~(uintptr_t)extra_alignment_bytes); global_link_transforms_ = variable_joint_transforms_ + robot_model_->getJointModelCount(); @@ -177,8 +178,8 @@ void RobotState::copyFrom(const RobotState& other) // copy attached bodies clearAttachedBodies(); - for (const std::pair& it : other.attached_body_map_) - attachBody(new AttachedBody(*it.second)); + for (const auto& attached_body : other.attached_body_map_) + attachBody(std::make_unique(*attached_body.second)); } bool RobotState::checkJointTransforms(const JointModel* joint) const @@ -741,9 +742,9 @@ void RobotState::updateLinkTransformsInternal(const JointModel* start) } // update attached bodies tf; these are usually very few, so we update them all - for (std::map::const_iterator it = attached_body_map_.begin(); - it != attached_body_map_.end(); ++it) - it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); + for (const auto& attached_body : attached_body_map_) + attached_body.second->computeTransform( + global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]); } void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward) @@ -793,9 +794,9 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome } // update attached bodies tf; these are usually very few, so we update them all - for (std::map::const_iterator it = attached_body_map_.begin(); - it != attached_body_map_.end(); ++it) - it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); + for (const auto& attached_body : attached_body_map_) + attached_body.second->computeTransform( + global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]); } const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const @@ -979,25 +980,30 @@ bool RobotState::hasAttachedBody(const std::string& id) const const AttachedBody* RobotState::getAttachedBody(const std::string& id) const { - std::map::const_iterator it = attached_body_map_.find(id); + const auto it = attached_body_map_.find(id); if (it == attached_body_map_.end()) { RCLCPP_ERROR(LOGGER, "Attached body '%s' not found", id.c_str()); return nullptr; } else - return it->second; + return it->second.get(); } -void RobotState::attachBody(AttachedBody* attached_body) +void RobotState::attachBody(std::unique_ptr attached_body) { // If an attached body with the same id exists, remove it clearAttachedBody(attached_body->getName()); - attached_body_map_[attached_body->getName()] = attached_body; attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink())); if (attached_body_update_callback_) - attached_body_update_callback_(attached_body, true); + attached_body_update_callback_(attached_body.get(), true); + attached_body_map_[attached_body->getName()] = std::move(attached_body); +} + +void RobotState::attachBody(AttachedBody* attached_body) +{ + attachBody(std::unique_ptr(attached_body)); } void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose, @@ -1006,90 +1012,81 @@ void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose const std::string& link, const trajectory_msgs::msg::JointTrajectory& detach_posture, const moveit::core::FixedTransformsMap& subframe_poses) { - const LinkModel* l = robot_model_->getLinkModel(link); - attachBody(new AttachedBody(l, id, pose, shapes, shape_poses, touch_links, detach_posture, subframe_poses)); + attachBody(std::make_unique(robot_model_->getLinkModel(link), id, pose, shapes, shape_poses, + touch_links, detach_posture, subframe_poses)); } void RobotState::getAttachedBodies(std::vector& attached_bodies) const { attached_bodies.clear(); attached_bodies.reserve(attached_body_map_.size()); - for (const std::pair& it : attached_body_map_) - attached_bodies.push_back(it.second); + for (const auto& it : attached_body_map_) + attached_bodies.push_back(it.second.get()); } void RobotState::getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const { attached_bodies.clear(); - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) if (group->hasLinkModel(it.second->getAttachedLinkName())) - attached_bodies.push_back(it.second); + attached_bodies.push_back(it.second.get()); } void RobotState::getAttachedBodies(std::vector& attached_bodies, const LinkModel* link_model) const { attached_bodies.clear(); - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) if (it.second->getAttachedLink() == link_model) - attached_bodies.push_back(it.second); + attached_bodies.push_back(it.second.get()); } void RobotState::clearAttachedBodies() { - for (std::map::const_iterator it = attached_body_map_.begin(); - it != attached_body_map_.end(); ++it) + for (const auto& it : attached_body_map_) { if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; + attached_body_update_callback_(it.second.get(), false); } attached_body_map_.clear(); } void RobotState::clearAttachedBodies(const LinkModel* link) { - std::map::iterator it = attached_body_map_.begin(); - while (it != attached_body_map_.end()) + for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it) { if (it->second->getAttachedLink() != link) { - ++it; continue; } if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; - std::map::iterator del = it++; + attached_body_update_callback_(it->second.get(), false); + const auto del = it++; attached_body_map_.erase(del); } } void RobotState::clearAttachedBodies(const JointModelGroup* group) { - std::map::iterator it = attached_body_map_.begin(); - while (it != attached_body_map_.end()) + for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it) { if (!group->hasLinkModel(it->second->getAttachedLinkName())) { - ++it; continue; } if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; - std::map::iterator del = it++; + attached_body_update_callback_(it->second.get(), false); + const auto del = it++; attached_body_map_.erase(del); } } bool RobotState::clearAttachedBody(const std::string& id) { - std::map::iterator it = attached_body_map_.find(id); + const auto it = attached_body_map_.find(id); if (it != attached_body_map_.end()) { if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; + attached_body_update_callback_(it->second.get(), false); attached_body_map_.erase(it); return true; } @@ -1138,7 +1135,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c robot_link = nullptr; // Check names of the attached bodies - std::map::const_iterator jt = attached_body_map_.find(frame_id); + const auto jt = attached_body_map_.find(frame_id); if (jt != attached_body_map_.end()) { const Eigen::Isometry3d& transform = jt->second->getGlobalPose(); @@ -1149,7 +1146,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c } // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const auto& body : attached_body_map_) { const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found); if (frame_found) @@ -1173,12 +1170,12 @@ bool RobotState::knowsFrameTransform(const std::string& frame_id) const return true; // Check if an AttachedBody with name frame_id exists - std::map::const_iterator it = attached_body_map_.find(frame_id); + const auto it = attached_body_map_.find(frame_id); if (it != attached_body_map_.end()) return !it->second->getGlobalCollisionBodyTransforms().empty(); // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const auto& body : attached_body_map_) { if (body.second->hasSubframeTransform(frame_id)) return true; @@ -1213,7 +1210,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons if (!link_model) continue; if (include_attached) - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) if (it.second->getAttachedLink() == link_model) { for (std::size_t j = 0; j < it.second->getShapes().size(); ++j) @@ -1779,7 +1776,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // set callback function kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) - ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3); + ik_callback_fn = std::bind(&ikCallbackFnAdapter, this, jmg, constraint, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); // Bijection const std::vector& bij = jmg->getKinematicsSolverJointBijection(); @@ -1922,7 +1920,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: std::vector ik_queries(poses_in.size()); kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) - ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3); + ik_callback_fn = std::bind(&ikCallbackFnAdapter, this, jmg, constraint, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); for (std::size_t i = 0; i < transformed_poses.size(); ++i) { @@ -2053,7 +2052,7 @@ void RobotState::computeAABB(std::vector& aabb) const transform.translate(link->getCenteredBoundingBoxOffset()); bounding_box.extendWithTransformedBox(transform, extents); } - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) { const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms(); const std::vector& shapes = it.second->getShapes(); @@ -2190,11 +2189,20 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { - ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry - Eigen::Quaterniond q(transform.linear()); - out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " - << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() - << "]\n"; + if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION, false)) + { + Eigen::Quaterniond q(transform.linear()); + out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " + << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() + << "]"; + } + else + { + out << "[NON-ISOMETRY] " + << transform.matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "; ", "", "", "[", "]")); + } + out << "\n"; } void RobotState::printTransforms(std::ostream& out) const diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 1421b98cdf..11162f8889 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -180,10 +180,8 @@ TEST(LoadingAndFK, SimpleRobot) state.setVariableAcceleration("base_joint/x", 0.0); - // making sure that values get copied - moveit::core::RobotState* new_state = new moveit::core::RobotState(state); + const auto new_state = std::make_unique(state); // making sure that values get copied EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0)); - delete new_state; std::vector jv(state.getVariableCount(), 0.0); jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 5.0; @@ -374,7 +372,7 @@ class OneRobot : public testing::Test ""; urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + auto srdf_model = std::make_shared(); srdf_model->initString(*urdf_model, SMODEL2); robot_model_ = std::make_shared(urdf_model, srdf_model); } @@ -701,7 +699,7 @@ TEST_F(OneRobot, rigidlyConnectedParent) EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); // attach "object" with "subframe" to link_b - state.attachBody(new moveit::core::AttachedBody( + state.attachBody(std::make_unique( link_b, "object", Eigen::Isometry3d::Identity(), std::vector{}, EigenSTL::vector_Isometry3d{}, std::set{}, trajectory_msgs::msg::JointTrajectory{}, moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } })); diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 0c0c2de208..88dbf676e1 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -229,7 +229,7 @@ class OneRobot : public testing::Test ""; urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf::ModelSharedPtr srdf_model = std::make_shared(); srdf_model->initString(*urdf_model, SMODEL2); robot_model_ = std::make_shared(urdf_model, srdf_model); } @@ -247,7 +247,7 @@ std::size_t generateTestTraj(std::vector robot_state(new moveit::core::RobotState(robot_model_)); + auto robot_state = std::make_shared(robot_model_); robot_state->setToDefaultValues(); double ja, jc; diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index fb7a9f8c51..c66f0ece6c 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -77,7 +77,7 @@ TEST_F(LoadPlanningModelsPr2, InitOK) TEST_F(LoadPlanningModelsPr2, ModelInit) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + auto srdf_model = std::make_shared(); // with no world multidof we should get a fixed joint moveit::core::RobotModel robot_model0(urdf_model_, srdf_model); @@ -121,7 +121,7 @@ TEST_F(LoadPlanningModelsPr2, GroupInit) "" ""; - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + auto srdf_model = std::make_shared(); srdf_model->initString(*urdf_model_, SMODEL1); moveit::core::RobotModel robot_model1(urdf_model_, srdf_model); @@ -150,7 +150,7 @@ TEST_F(LoadPlanningModelsPr2, GroupInit) ""; srdf_model->initString(*urdf_model_, SMODEL2); - moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model_, srdf_model)); + auto robot_model2 = std::make_shared(urdf_model_, srdf_model); left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip"); ASSERT_TRUE(left_arm_base_tip_group != nullptr); @@ -219,13 +219,13 @@ TEST_F(LoadPlanningModelsPr2, SubgroupInit) TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks) { - moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + auto model = std::make_shared(urdf_model_, srdf_model_); EXPECT_TRUE(model->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1); } TEST_F(LoadPlanningModelsPr2, FullTest) { - moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + auto robot_model = std::make_shared(urdf_model_, srdf_model_); moveit::core::RobotState ks(robot_model); ks.setToDefaultValues(); @@ -242,9 +242,9 @@ TEST_F(LoadPlanningModelsPr2, FullTest) std::set touch_links; trajectory_msgs::msg::JointTrajectory empty_state; - moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( - robot_model->getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state); - ks.attachBody(attached_body); + + ks.attachBody(std::make_unique(robot_model->getLinkModel("r_gripper_palm_link"), "box", + identity, shapes, poses, touch_links, empty_state)); std::vector attached_bodies_1; ks.getAttachedBodies(attached_bodies_1); @@ -268,7 +268,7 @@ TEST_F(LoadPlanningModelsPr2, FullTest) TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes) { - moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + auto robot_model = std::make_shared(urdf_model_, srdf_model_); moveit::core::RobotState ks(robot_model); ks.setToDefaultValues(); @@ -285,14 +285,12 @@ TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes) subframes["frame1"] = Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)); trajectory_msgs::msg::JointTrajectory empty_state; - moveit::core::AttachedBody* attached_body_a = - new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "boxA", pose_a, shapes, poses, - touch_links, empty_state, subframes); - moveit::core::AttachedBody* attached_body_b = - new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "boxB", pose_b, shapes, poses, - touch_links, empty_state, subframes); - ks.attachBody(attached_body_a); - ks.attachBody(attached_body_b); + ks.attachBody(std::make_unique(robot_model->getLinkModel("r_gripper_palm_link"), "boxA", + pose_a, shapes, poses, touch_links, empty_state, + subframes)); + ks.attachBody(std::make_unique(robot_model->getLinkModel("r_gripper_palm_link"), "boxB", + pose_b, shapes, poses, touch_links, empty_state, + subframes)); // Check position of shape in each body Eigen::Isometry3d p; diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index bfe2dae702..bbb3bcd3a3 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -92,7 +92,22 @@ double RobotTrajectory::getDuration() const double RobotTrajectory::getAverageSegmentDuration() const { if (duration_from_previous_.empty()) + { + RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "Too few waypoints to calculate a duration. Returning 0."); return 0.0; + } + + // If the initial segment has a duration of 0, exclude it from the average calculation + if (duration_from_previous_[0] == 0) + { + if (duration_from_previous_.size() <= 1) + { + RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "First and only waypoint has a duration of 0."); + return 0.0; + } + else + return getDuration() / static_cast(duration_from_previous_.size() - 1); + } else return getDuration() / static_cast(duration_from_previous_.size()); } @@ -372,7 +387,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo for (std::size_t i = 0; i < state_count; ++i) { this_time_stamp = rclcpp::Time(trajectory.header.stamp) + trajectory.points[i].time_from_start; - moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); + auto st = std::make_shared(copy); st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions); if (!trajectory.points[i].velocities.empty()) st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities); @@ -403,7 +418,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo for (std::size_t i = 0; i < state_count; ++i) { - moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); + auto st = std::make_shared(copy); if (trajectory.joint_trajectory.points.size() > i) { st->setVariablePositions(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions); diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 21c351d2b7..7f336585fd 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -116,6 +116,14 @@ class RobotTrajectoryTestFixture : public testing::Test std::vector trajectory_first_state_after_update; trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]); + + // Modify the first waypoint duration + double trajectory_first_duration_before_update = trajectory->getWayPointDurationFromPrevious(0); + double new_duration = trajectory_first_duration_before_update + 0.1; + trajectory->setWayPointDurationFromPrevious(0, new_duration); + + // Check that the trajectory's first duration was updated + EXPECT_EQ(trajectory->getWayPointDurationFromPrevious(0), new_duration); } void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) @@ -213,7 +221,7 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy) trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_copy_first_state_after_update); - // Check that we updated the value correctly in the trajectory + // Check that we updated the joint position correctly in the trajectory EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); } @@ -239,8 +247,10 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy) trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_copy_first_state_after_update); - // Check that we updated the value correctly in the trajectory + // Check that joint positions changed in the original trajectory but not the deep copy EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); + // Check that the first waypoint duration changed in the original trajectory but not the deep copy + EXPECT_NE(trajectory->getWayPointDurationFromPrevious(0), trajectory_copy->getWayPointDurationFromPrevious(0)); } TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryIterator) diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index 2b4b0ce6a4..9961101048 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -39,7 +39,7 @@ if(BUILD_TESTING) target_link_libraries(test_time_parameterization moveit_test_utils ${MOVEIT_LIB_NAME}) ament_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp) - target_link_libraries(test_time_optimal_trajectory_generation ${MOVEIT_LIB_NAME}) + target_link_libraries(test_time_optimal_trajectory_generation moveit_test_utils ${MOVEIT_LIB_NAME}) ament_add_gtest(test_ruckig_traj_smoothing test/test_ruckig_traj_smoothing.cpp) target_link_libraries(test_ruckig_traj_smoothing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index b2de9f8614..50000bfdda 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -39,6 +39,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include namespace trajectory_processing { @@ -68,14 +69,13 @@ namespace trajectory_processing /// velocity and acceleration limits, will result in a longer trajectory. /// If this is a problem, try retuning (increasing) the limits. /// -class IterativeSplineParameterization +class IterativeSplineParameterization : public TimeParameterization { public: IterativeSplineParameterization(bool add_points = true); - ~IterativeSplineParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const; + const double max_acceleration_scaling_factor = 1.0) const override; private: bool add_points_; /// @brief If true, add two points to trajectory (first and last segments). diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index 50b7e9547c..390476def6 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -38,19 +38,19 @@ #include #include "rclcpp/rclcpp.hpp" +#include namespace trajectory_processing { /// \brief This class modifies the timestamps of a trajectory to respect /// velocity and acceleration constraints. -class IterativeParabolicTimeParameterization +class IterativeParabolicTimeParameterization : public TimeParameterization { public: IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01); - ~IterativeParabolicTimeParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const; + const double max_acceleration_scaling_factor = 1.0) const override; private: unsigned int max_iterations_; /// @brief maximum number of iterations to find solution diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index cc22f4d0d7..d7103f3a58 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h @@ -31,7 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Author: Jack Center, Wyatt Rees, Andy Zelenak */ +/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */ #pragma once @@ -54,51 +54,23 @@ class RuckigSmoothing * \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint. * \param ruckig_output The previous output from Ruckig * \param next_waypoint The nominal, desired state at the next waypoint - * \param num_dof Number of actuated joints - * \param idx MoveIt list of joint group indices + * \param joint_group The MoveIt JointModelGroup of interest * \param ruckig_input Output. The Rucking parameters for the next iteration */ static void getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output, - const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof, - const std::vector& idx, ruckig::InputParameter<0>& ruckig_input); - - /** - * \brief Check for lagging motion of any joint at a waypoint. - * \param num_dof Number of actuated joints - * \param ruckig_input Input parameters to Ruckig - * \param ruckig_output Output parameters from Ruckig - * \return true if lagging motion is detected on any joint - */ - static bool checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input, - const ruckig::OutputParameter<0>& ruckig_output); - - /** - * \brief Return L2-norm of velocity, taking all joints into account. - * \param ruckig_input Input parameters to Ruckig - * \param num_dof Number of actuated joints - */ - static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof); - - /** - * \brief Check if the joint positions of two waypoints are very similar. - * \param prev_waypoint State at waypoint i-1 - * \param prev_waypoint State at waypoint i - * \joint_group The MoveIt JointModelGroup of interest - */ - static bool checkForIdenticalWaypoints(const moveit::core::RobotState& prev_waypoint, - const moveit::core::RobotState& next_waypoint, - const moveit::core::JointModelGroup* joint_group); + const moveit::core::RobotStatePtr& next_waypoint, + const moveit::core::JointModelGroup* joint_group, + ruckig::InputParameter<0>& ruckig_input); /** * \brief Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values * \param rucking_input Input parameters to Ruckig. Initialized here. * \param ruckig_output Output from the Ruckig algorithm. Initialized here. * \param first_waypoint The Ruckig input/output parameters are initialized to the values at this waypoint - * \param num_dof Number of actuated joints - * \param joint_idx MoveIt list of joint group indices + * \param joint_group The MoveIt JointModelGroup of interest */ static void initializeRuckigState(ruckig::InputParameter<0>& ruckig_input, ruckig::OutputParameter<0>& ruckig_output, - const moveit::core::RobotState& first_waypoint, size_t num_dof, - const std::vector& joint_idx); + const moveit::core::RobotState& first_waypoint, + const moveit::core::JointModelGroup* joint_group); }; } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 0ee4d13f41..cbbc6cd739 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace trajectory_processing { @@ -159,15 +160,14 @@ class Trajectory mutable std::list::const_iterator cached_trajectory_segment_; }; -class TimeOptimalTrajectoryGeneration +class TimeOptimalTrajectoryGeneration : public TimeParameterization { public: TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1, const double min_angle_change = 0.001); - ~TimeOptimalTrajectoryGeneration(); bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const; + const double max_acceleration_scaling_factor = 1.0) const override; private: const double path_tolerance_; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h new file mode 100644 index 0000000000..3a64960e54 --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +namespace trajectory_processing +{ +/** + * @brief Base class for trajectory parameterization algorithms + */ +class TimeParameterization +{ +public: + virtual ~TimeParameterization() = default; + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; +}; +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 49c23b931a..1b1646e6a9 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -68,7 +68,7 @@ struct SingleJointTrajectory double max_acceleration_; }; -void globalAdjustment(std::vector& t2, int num_joints, const int num_points, +void globalAdjustment(std::vector& t2, robot_trajectory::RobotTrajectory& trajectory, std::vector& time_diff); IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points) @@ -321,7 +321,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT } // Final adjustment forces the trajectory within bounds - globalAdjustment(t2, num_joints, num_points, time_diff); + globalAdjustment(t2, trajectory, time_diff); // Convert back to JointTrajectory form for (unsigned int i = 1; i < num_points; ++i) @@ -570,11 +570,16 @@ static double global_adjustment_factor(const int n, double x1[], double x2[], co } // Expands the entire trajectory to fit exactly within bounds -void globalAdjustment(std::vector& t2, int num_joints, const int num_points, +void globalAdjustment(std::vector& t2, robot_trajectory::RobotTrajectory& trajectory, std::vector& time_diff) { + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + + const unsigned int num_points = trajectory.getWayPointCount(); + const unsigned int num_joints = group->getVariableCount(); + double gtfactor = 1.0; - for (int j = 0; j < num_joints; ++j) + for (unsigned int j = 0; j < num_joints; ++j) { double tfactor; tfactor = global_adjustment_factor(num_points, &t2[j].velocities_[0], &t2[j].accelerations_[0], t2[j].max_velocity_, @@ -584,10 +589,10 @@ void globalAdjustment(std::vector& t2, int num_joints, co } // printf("# Global adjustment: %0.4f%%\n", 100.0 * (gtfactor - 1.0)); - for (int i = 0; i < num_points - 1; ++i) + for (unsigned int i = 0; i < num_points - 1; ++i) time_diff[i] *= gtfactor; - for (int j = 0; j < num_joints; ++j) + for (unsigned int j = 0; j < num_joints; ++j) { fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]); } diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 6f74994bf3..cdd940509d 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -31,7 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Author: Jack Center, Wyatt Rees, Andy Zelenak */ +/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */ #include #include @@ -46,20 +46,18 @@ namespace trajectory_processing namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing"); -constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s -constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 -constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 -constexpr double IDENTICAL_POSITION_EPSILON = 1e-3; // rad -constexpr double MAX_DURATION_EXTENSION_FACTOR = 5.0; +constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s +constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 +constexpr double DEFAULT_MAX_JERK = 1000; // rad/s^3 +constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0; constexpr double DURATION_EXTENSION_FRACTION = 1.1; -constexpr double MINIMUM_VELOCITY_SEARCH_MAGNITUDE = 0.01; // rad/s. Stop searching when velocity drops below this } // namespace bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) { - const moveit::core::JointModelGroup* group = trajectory.getGroup(); + moveit::core::JointModelGroup const* const group = trajectory.getGroup(); if (!group) { RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for"); @@ -69,10 +67,15 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto const size_t num_waypoints = trajectory.getWayPointCount(); if (num_waypoints < 2) { - RCLCPP_ERROR(LOGGER, "Trajectory does not have enough points to smooth with Ruckig"); - return false; + RCLCPP_WARN(LOGGER, + "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory."); + return true; } + // Cache the trajectory in case we need to reset it + robot_trajectory::RobotTrajectory original_trajectory = + robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */); + const size_t num_dof = group->getVariableCount(); // This lib does not actually work properly when angles wrap around, so we need to unwind the path first @@ -80,23 +83,20 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto // Instantiate the smoother double timestep = trajectory.getAverageSegmentDuration(); - std::unique_ptr> ruckig_ptr; - ruckig_ptr = std::make_unique>(num_dof, timestep); - ruckig::InputParameter<0> ruckig_input{ num_dof }; - ruckig::OutputParameter<0> ruckig_output{ num_dof }; + std::unique_ptr> ruckig_ptr; + ruckig_ptr = std::make_unique>(num_dof, timestep); + ruckig::InputParameter ruckig_input{ num_dof }; + ruckig::OutputParameter ruckig_output{ num_dof }; // Initialize the smoother - const std::vector& idx = group->getVariableIndexList(); - initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx); + initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group); // Kinematic limits (vel/accel/jerk) const std::vector& vars = group->getVariableNames(); const moveit::core::RobotModel& rmodel = group->getParentModel(); + const std::vector& move_group_idx = group->getVariableIndexList(); for (size_t i = 0; i < num_dof; ++i) { - // TODO(andyz): read this from the joint group if/when jerk limits are added to the JointModel - ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK; - const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i)); // This assumes min/max bounds are symmetric @@ -106,6 +106,10 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } else { + RCLCPP_WARN_STREAM_ONCE(LOGGER, + "Joint velocity limits are not defined. Using the default " + << DEFAULT_MAX_VELOCITY + << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml."); ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY; } if (bounds.acceleration_bounded_) @@ -114,94 +118,82 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } else { + RCLCPP_WARN_STREAM_ONCE(LOGGER, + "Joint acceleration limits are not defined. Using the default " + << DEFAULT_MAX_ACCELERATION + << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml."); ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION; } + ruckig_input.max_jerk.at(i) = bounds.jerk_bounded_ ? bounds.max_jerk_ : DEFAULT_MAX_JERK; + if (bounds.jerk_bounded_) + { + ruckig_input.max_jerk.at(i) = bounds.max_jerk_; + } + else + { + RCLCPP_WARN_STREAM_ONCE(LOGGER, "Joint jerk limits are not defined. Using the default " + << DEFAULT_MAX_JERK + << " rad/s^3. You can define jerk limits in joint_limits.yaml."); + ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK; + } } ruckig::Result ruckig_result; - bool smoothing_complete = false; double duration_extension_factor = 1; + bool smoothing_complete = false; while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete) { for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx) { moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1); - getNextRuckigInput(ruckig_output, next_waypoint, num_dof, idx, ruckig_input); + getNextRuckigInput(ruckig_output, next_waypoint, group, ruckig_input); // Run Ruckig ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output); - // If the requested velocity is too great, a joint can actually "move backward" to give itself more time to - // accelerate to the target velocity. Iterate and decrease velocities until that behavior is gone. - bool backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output); - - double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof); - while (backward_motion_detected && (velocity_magnitude > MINIMUM_VELOCITY_SEARCH_MAGNITUDE)) - { - // Skip repeated waypoints with no change in position. Ruckig does not handle this well and there's really no - // need to smooth it Simply set it equal to the previous (identical) waypoint. - if (checkForIdenticalWaypoints(*trajectory.getWayPointPtr(waypoint_idx), *next_waypoint, trajectory.getGroup())) - { - *next_waypoint = trajectory.getWayPoint(waypoint_idx); - continue; - } - - // decrease target velocity - for (size_t joint = 0; joint < num_dof; ++joint) - { - ruckig_input.target_velocity.at(joint) *= 0.9; - // Propagate the change in velocity to acceleration, too. - // We don't change the position to ensure the exact target position is achieved. - ruckig_input.target_acceleration.at(joint) = - (ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep; - } - velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof); - // Run Ruckig - ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output); - - // check for backward motion - backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output); - } - - // Overwrite pos/vel/accel of the target waypoint - for (size_t joint = 0; joint < num_dof; ++joint) + if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished) { - next_waypoint->setVariablePosition(idx.at(joint), ruckig_output.new_position.at(joint)); - next_waypoint->setVariableVelocity(idx.at(joint), ruckig_output.new_velocity.at(joint)); - next_waypoint->setVariableAcceleration(idx.at(joint), ruckig_output.new_acceleration.at(joint)); + smoothing_complete = true; + break; } - next_waypoint->update(); - } - // If ruckig failed, the duration of the seed trajectory likely wasn't long enough. - // Try duration extension several times. - // TODO: see issue 767. (https://github.com/ros-planning/moveit2/issues/767) - if (ruckig_result == ruckig::Result::Working) - { - smoothing_complete = true; - } - else - { - // If Ruckig failed, it's likely because the original seed trajectory did not have a long enough duration when - // jerk is taken into account. Extend the duration and try again. - initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx); - duration_extension_factor *= DURATION_EXTENSION_FRACTION; - for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx) + // Extend the trajectory duration if Ruckig could not reach the waypoint successfully + if (ruckig_result != ruckig::Result::Finished) { - trajectory.setWayPointDurationFromPrevious( - waypoint_idx, DURATION_EXTENSION_FRACTION * trajectory.getWayPointDurationFromPrevious(waypoint_idx)); - // TODO(andyz): re-calculate waypoint velocity and acceleration here? + duration_extension_factor *= DURATION_EXTENSION_FRACTION; + // Reset the trajectory + trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */); + for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx) + { + trajectory.setWayPointDurationFromPrevious( + time_stretch_idx, + duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx)); + // re-calculate waypoint velocity and acceleration + auto target_state = trajectory.getWayPointPtr(time_stretch_idx); + const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1); + timestep = trajectory.getAverageSegmentDuration(); + for (size_t joint = 0; joint < num_dof; ++joint) + { + target_state->setVariableVelocity(move_group_idx.at(joint), + (1 / duration_extension_factor) * + target_state->getVariableVelocity(move_group_idx.at(joint))); + + double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint)); + double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint)); + target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep); + } + target_state->update(); + } + ruckig_ptr = std::make_unique>(num_dof, timestep); + initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group); + // Begin the while() loop again + break; } - - timestep = trajectory.getAverageSegmentDuration(); - ruckig_ptr = std::make_unique>(num_dof, timestep); } } - // Either of these results is acceptable. - // Working means smoothing worked well but the final target position wasn't exactly achieved (I think) -- Andy Z. - if ((ruckig_result != ruckig::Result::Working) && (ruckig_result != ruckig::Result::Finished)) + if (ruckig_result != ruckig::Result::Finished) { RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result); return false; @@ -212,9 +204,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_input, ruckig::OutputParameter<0>& ruckig_output, - const moveit::core::RobotState& first_waypoint, size_t num_dof, - const std::vector& idx) + const moveit::core::RobotState& first_waypoint, + const moveit::core::JointModelGroup* joint_group) { + const size_t num_dof = joint_group->getVariableCount(); + const std::vector& idx = joint_group->getVariableIndexList(); + std::vector current_positions_vector(num_dof); std::vector current_velocities_vector(num_dof); std::vector current_accelerations_vector(num_dof); @@ -234,47 +229,17 @@ void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_in ruckig_output.new_acceleration = ruckig_input.current_acceleration; } -bool RuckigSmoothing::checkForIdenticalWaypoints(const moveit::core::RobotState& prev_waypoint, - const moveit::core::RobotState& next_waypoint, - const moveit::core::JointModelGroup* joint_group) -{ - double magnitude_position_difference = prev_waypoint.distance(next_waypoint, joint_group); - - return (magnitude_position_difference <= IDENTICAL_POSITION_EPSILON); -} - -double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof) -{ - double vel_magnitude = 0; - for (size_t joint = 0; joint < num_dof; ++joint) - { - vel_magnitude += ruckig_input.target_velocity.at(joint) * ruckig_input.target_velocity.at(joint); - } - return sqrt(vel_magnitude); -} - -bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input, - const ruckig::OutputParameter<0>& ruckig_output) -{ - // Check for backward motion of any joint - for (size_t joint = 0; joint < num_dof; ++joint) - { - // This indicates the jerk-limited output lags the target output - if ((ruckig_output.new_velocity.at(joint) / ruckig_input.target_velocity.at(joint)) < 1) - { - return true; - } - } - return false; -} - void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output, - const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof, - const std::vector& idx, ruckig::InputParameter<0>& ruckig_input) + const moveit::core::RobotStatePtr& next_waypoint, + const moveit::core::JointModelGroup* joint_group, + ruckig::InputParameter<0>& ruckig_input) { // TODO(andyz): https://github.com/ros-planning/moveit2/issues/766 // ruckig_output.pass_to_input(ruckig_input); + const size_t num_dof = joint_group->getVariableCount(); + const std::vector& idx = joint_group->getVariableIndexList(); + for (size_t joint = 0; joint < num_dof; ++joint) { // Feed output from the previous timestep back as input diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 44defe4684..295bf964cf 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -112,8 +112,10 @@ class CircularPathSegment : public PathSegment const Eigen::VectorXd start_direction = (intersection - start).normalized(); const Eigen::VectorXd end_direction = (end - intersection).normalized(); + const double start_dot_end = start_direction.dot(end_direction); - if ((start_direction - end_direction).norm() < 0.000001) + // catch division by 0 in computations below + if (start_dot_end > 0.999999 || start_dot_end < -0.999999) { length_ = 0.0; radius = 1.0; @@ -123,8 +125,7 @@ class CircularPathSegment : public PathSegment return; } - // directions must be different at this point so angle is always non-zero - const double angle = acos(std::max(-1.0, start_direction.dot(end_direction))); + const double angle = acos(start_dot_end); const double start_distance = (start - intersection).norm(); const double end_distance = (end - intersection).norm(); @@ -863,10 +864,6 @@ TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double pa { } -TimeOptimalTrajectoryGeneration::~TimeOptimalTrajectoryGeneration() -{ -} - bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) const diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp index c1fbcf0834..1a2b1f69b4 100644 --- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp @@ -59,48 +59,74 @@ class RuckigTests : public testing::Test } // namespace -TEST_F(RuckigTests, empty_trajectory) +TEST_F(RuckigTests, basic_trajectory) { - // This should fail because the trajectory is empty - EXPECT_FALSE( + moveit::core::RobotState robot_state(robot_model_); + robot_state.setToDefaultValues(); + // First waypoint is default joint positions + trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); + + // Second waypoint has slightly-different joint positions + std::vector joint_positions; + robot_state.copyJointGroupPositions(JOINT_GROUP, joint_positions); + joint_positions.at(0) += 0.05; + robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions); + robot_state.update(); + trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); + + EXPECT_TRUE( smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */)); } -TEST_F(RuckigTests, not_enough_waypoints) +TEST_F(RuckigTests, trajectory_duration) { + // Compare against the OJET online trajectory generator: https://www.trajectorygenerator.com/ojet-online/ + const double ideal_duration = 0.21025; + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - // First waypoint is default joint positions - trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); + // Special attention to Joint 0. It is the only joint to move in this test. + // Zero velocities and accelerations at the endpoints + robot_state.setVariablePosition("panda_joint1", 0.0); + robot_state.update(); + trajectory_->addSuffixWayPoint(robot_state, 0.0); + robot_state.setVariablePosition("panda_joint1", 0.1); robot_state.update(); + trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); - // Fails due to not enough waypoints - EXPECT_FALSE( + EXPECT_TRUE( smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */)); + EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * ideal_duration); + EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.3 * ideal_duration); } -TEST_F(RuckigTests, basic_trajectory) +TEST_F(RuckigTests, single_waypoint) { + // With only one waypoint, Ruckig cannot smooth the trajectory. + // It should simply pass the trajectory through unmodified and return true. + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); // First waypoint is default joint positions trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); - // Second waypoint has slightly-different joint positions - std::vector joint_positions; - robot_state.copyJointGroupPositions(JOINT_GROUP, joint_positions); - joint_positions.at(0) += 0.05; - robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions); - trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); + robot_state.update(); - joint_positions.at(0) += 0.05; - robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions); - trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP); + // Trajectory should not change + auto first_waypoint_input = robot_state; - robot_state.update(); + // Only one waypoint is acceptable. True is returned. EXPECT_TRUE( smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */)); + // And the waypoint did not change + auto const new_first_waypoint = trajectory_->getFirstWayPointPtr(); + auto const& variable_names = new_first_waypoint->getVariableNames(); + for (std::string const& variable_name : variable_names) + { + EXPECT_EQ(first_waypoint_input.getVariablePosition(variable_name), + new_first_waypoint->getVariablePosition(variable_name)); + } } int main(int argc, char** argv) diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index 7413afac44..4f2aabf72e 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -38,8 +38,10 @@ #include #include +#include using trajectory_processing::Path; +using trajectory_processing::TimeOptimalTrajectoryGeneration; using trajectory_processing::Trajectory; TEST(time_optimal_trajectory_generation, test1) @@ -227,6 +229,125 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) } } +TEST(time_optimal_trajectory_generation, testPluginAPI) +{ + constexpr auto robot_name{ "panda" }; + constexpr auto group_name{ "panda_arm" }; + + auto robot_model = moveit::core::loadTestingRobotModel(robot_name); + ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name; + auto group = robot_model->getJointModelGroup(group_name); + ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name; + moveit::core::RobotState waypoint_state(robot_model); + waypoint_state.setToDefaultValues(); + + robot_trajectory::RobotTrajectory trajectory(robot_model, group); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + + // Number TOTG iterations + constexpr size_t totg_iterations = 10; + + // Test computing the dynamics repeatedly with the same totg instance + moveit_msgs::msg::RobotTrajectory first_trajectory_msg_start, first_trajectory_msg_end; + { + robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */); + + // Test if the trajectory was copied correctly + ASSERT_EQ(test_trajectory.getDuration(), trajectory.getDuration()); + moveit::core::JointBoundsVector test_bounds = test_trajectory.getRobotModel()->getActiveJointModelsBounds(); + moveit::core::JointBoundsVector original_bounds = trajectory.getRobotModel()->getActiveJointModelsBounds(); + ASSERT_EQ(test_bounds.size(), original_bounds.size()); + for (size_t bound_idx = 0; bound_idx < test_bounds.at(0)->size(); ++bound_idx) + { + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_velocity_, original_bounds.at(0)->at(bound_idx).max_velocity_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_velocity_, original_bounds.at(0)->at(bound_idx).min_velocity_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).velocity_bounded_, + original_bounds.at(0)->at(bound_idx).velocity_bounded_); + + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_acceleration_, + original_bounds.at(0)->at(bound_idx).max_acceleration_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_acceleration_, + original_bounds.at(0)->at(bound_idx).min_acceleration_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).acceleration_bounded_, + original_bounds.at(0)->at(bound_idx).acceleration_bounded_); + } + ASSERT_EQ(test_trajectory.getWayPointDurationFromPrevious(1), trajectory.getWayPointDurationFromPrevious(1)); + + TimeOptimalTrajectoryGeneration totg; + ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps"; + + test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_start); + + // Iteratively recompute timestamps with same totg instance + for (size_t i = 0; i < totg_iterations; ++i) + { + bool totg_success = totg.computeTimeStamps(test_trajectory); + EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a same TOTG instance in iteration " << i; + } + + test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_end); + } + + // Test computing the dynamics repeatedly with one TOTG instance per call + moveit_msgs::msg::RobotTrajectory second_trajectory_msg_start, second_trajectory_msg_end; + { + robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */); + + { + TimeOptimalTrajectoryGeneration totg; + ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps"; + } + + test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_start); + + // Iteratively recompute timestamps with new totg instances + for (size_t i = 0; i < totg_iterations; ++i) + { + TimeOptimalTrajectoryGeneration totg; + bool totg_success = totg.computeTimeStamps(test_trajectory); + EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a new TOTG instance in iteration " << i; + } + + test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_end); + } + + // Make sure trajectories produce equal waypoints independent of TOTG instances + ASSERT_EQ(first_trajectory_msg_start, second_trajectory_msg_start); + ASSERT_EQ(first_trajectory_msg_end, second_trajectory_msg_end); + + // Iterate on the original trajectory again + moveit_msgs::msg::RobotTrajectory third_trajectory_msg_end; + + { + TimeOptimalTrajectoryGeneration totg; + ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps"; + } + + for (size_t i = 0; i < totg_iterations; ++i) + { + TimeOptimalTrajectoryGeneration totg; + bool totg_success = totg.computeTimeStamps(trajectory); + ASSERT_TRUE(totg_success) << "Failed to compute timestamps on a new TOTG instance in iteration " << i; + } + + // Compare with previous work + trajectory.getRobotTrajectoryMsg(third_trajectory_msg_end); + + // Make sure trajectories produce equal waypoints independent of TOTG instances + ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h new file mode 100644 index 0000000000..f027c3a718 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT 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. + *********************************************************************/ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** + * @brief a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function + */ +class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes +{ +public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } +}; + +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 9feacc7d37..5bfbd3b7e5 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -54,7 +54,7 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name) { urdf::ModelInterfaceSharedPtr urdf = loadModelInterface(robot_name); srdf::ModelSharedPtr srdf = loadSRDFModel(robot_name); - moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf, srdf)); + auto robot_model = std::make_shared(urdf, srdf); return robot_model; } @@ -83,7 +83,7 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name) { urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + auto srdf_model = std::make_shared(); std::string srdf_path; if (robot_name == "pr2") { @@ -102,12 +102,12 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name) } RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& base_link_name) - : urdf_model_(new urdf::ModelInterface()), srdf_writer_(new srdf::SRDFWriter()) + : urdf_model_(std::make_shared()), srdf_writer_(std::make_shared()) { urdf_model_->clear(); urdf_model_->name_ = name; - urdf::LinkSharedPtr base_link(new urdf::Link); + auto base_link = std::make_shared(); base_link->name = base_link_name; urdf_model_->links_.insert(std::make_pair(base_link_name, base_link)); @@ -151,10 +151,10 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& is_valid_ = false; return; } - urdf::LinkSharedPtr link(new urdf::Link); + auto link = std::make_shared(); link->name = link_names[i]; urdf_model_->links_.insert(std::make_pair(link_names[i], link)); - urdf::JointSharedPtr joint(new urdf::Joint); + auto joint = std::make_shared(); joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint"; // Default to Identity transform for origins. joint->parent_to_joint_origin_transform.clear(); @@ -190,7 +190,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& joint->axis = joint_axis; if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC) { - urdf::JointLimitsSharedPtr limits(new urdf::JointLimits); + auto limits = std::make_shared(); limits->lower = -boost::math::constants::pi(); limits->upper = boost::math::constants::pi(); @@ -210,7 +210,7 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g return; } - urdf::InertialSharedPtr inertial(new urdf::Inertial); + auto inertial = std::make_shared(); inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z); inertial->origin.rotation = urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w); @@ -230,8 +230,8 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector& size, geometry_msgs::msg::Pose origin) { - urdf::VisualSharedPtr vis(new urdf::Visual); - urdf::BoxSharedPtr geometry(new urdf::Box); + auto vis = std::make_shared(); + auto geometry = std::make_shared(); geometry->dim = urdf::Vector3(size[0], size[1], size[2]); vis->geometry = geometry; addLinkVisual(link_name, vis, origin); @@ -246,8 +246,8 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std: is_valid_ = false; return; } - urdf::CollisionSharedPtr coll(new urdf::Collision); - urdf::BoxSharedPtr geometry(new urdf::Box); + auto coll = std::make_shared(); + auto geometry = std::make_shared(); geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]); coll->geometry = geometry; addLinkCollision(link_name, coll, origin); @@ -256,8 +256,8 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std: void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::msg::Pose origin) { - urdf::CollisionSharedPtr coll(new urdf::Collision); - urdf::MeshSharedPtr geometry(new urdf::Mesh); + auto coll = std::make_shared(); + auto geometry = std::make_shared(); geometry->filename = filename; coll->geometry = geometry; addLinkCollision(link_name, coll, origin); diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp index 75ce900180..c71fca5b81 100644 --- a/moveit_core/version/version.cpp +++ b/moveit_core/version/version.cpp @@ -39,6 +39,6 @@ int main(int /*argc*/, char** /*argv*/) { - printf("%s\n", MOVEIT_VERSION); + printf("%s\n", MOVEIT_VERSION_STR); return 0; } diff --git a/moveit_core/version/version.h.in b/moveit_core/version/version.h.in index 573a2e72b5..8e596d8f9f 100644 --- a/moveit_core/version/version.h.in +++ b/moveit_core/version/version.h.in @@ -35,11 +35,17 @@ #ifndef MOVEIT_VERSION_ #define MOVEIT_VERSION_ -#define MOVEIT_VERSION "@MOVEIT_VERSION@" +#define MOVEIT_VERSION_STR "@MOVEIT_VERSION@" #define MOVEIT_VERSION_MAJOR @MOVEIT_VERSION_MAJOR@ #define MOVEIT_VERSION_MINOR @MOVEIT_VERSION_MINOR@ #define MOVEIT_VERSION_PATCH @MOVEIT_VERSION_PATCH@ #define MOVEIT_VERSION_EXTRA "@MOVEIT_VERSION_EXTRA@" +/// MOVEIT_VERSION is (major << 16) + (minor << 8) + patch. +#define MOVEIT_VERSION MOVEIT_VERSION_CHECK(MOVEIT_VERSION_MAJOR, MOVEIT_VERSION_MINOR, MOVEIT_VERSION_PATCH) + +/// Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0) +#define MOVEIT_VERSION_CHECK(major, minor, patch) ((major << 16) | (minor << 8) | (patch)) + #endif diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp index 605b1dfa95..158c050415 100644 --- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp +++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp @@ -45,7 +45,7 @@ #else #include #endif -#include +#include namespace kinematics_constraint_aware { @@ -165,7 +165,8 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame()); moveit::core::StateValidityCallbackFn constraint_callback_fn = - boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2); + std::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, + std::placeholders::_1, std::placeholders::_2); bool result = false; if (has_sub_groups_) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py index d72ab98cd8..a3d1bef186 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py @@ -44,7 +44,6 @@ from lxml import etree import shlex import sys -import io def doRound(values, decimal_places): @@ -82,19 +81,8 @@ def doRound(values, decimal_places): print("\nCollada Number Rounder") print("Rounding numbers to", decimal_places, "decimal places\n") - # Read string from file - f = open(input_file, "r") - xml = f.read() - - # Parse XML - # doc = etree.fromstring(xml) - # print(doc.tag) - # doc = etree.parse(io.BytesIO(xml)) - # element=doc.xpath('//ns:asset',namespaces={'ns','http://www.collada.org/2008/03/COLLADASchema'}) - # print(element) - namespace = "http://www.collada.org/2008/03/COLLADASchema" - dom = etree.parse(io.BytesIO(xml)) + dom = etree.parse(input_file) # find elements of particular name elements = dom.xpath("//ns:translate", namespaces={"ns": namespace}) @@ -122,6 +110,5 @@ def doRound(values, decimal_places): elements[i].text = doRound(elements[i].text, decimal_places) # save changes - f = open(output_file, "w") - f.write(etree.tostring(dom)) - f.close() + with open(output_file, "wb") as f: + dom.write(f, encoding="utf-8") diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 74760149b3..5757655b7e 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -159,6 +159,8 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase /// clip q_delta such that joint limits will not be violated void clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting) const; + static rclcpp::Clock steady_clock_; + bool initialized_; ///< Internal variable that indicates whether solver is configured and ready unsigned int dimension_; ///< Dimension of the group diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index 0cef9ec308..ca7cd366c7 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -53,6 +53,8 @@ namespace kdl_kinematics_plugin { static rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kdl_kinematics_plugin.kdl_kinematics_plugin"); +rclcpp::Clock KDLKinematicsPlugin::steady_clock_{ RCL_STEADY_TIME }; + KDLKinematicsPlugin::KDLKinematicsPlugin() : initialized_(false) { } @@ -272,7 +274,7 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const bool KDLKinematicsPlugin::timedOut(const rclcpp::Time& start_time, double duration) const { - return ((node_->now() - start_time).seconds() >= duration); + return ((steady_clock_.now() - start_time).seconds() >= duration); } bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, @@ -327,7 +329,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - rclcpp::Time start_time = node_->now(); + const rclcpp::Time start_time = steady_clock_.now(); if (!initialized_) { RCLCPP_ERROR(LOGGER, "kinematics solver not initialized"); @@ -413,13 +415,13 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po // solution passed consistency check and solution callback error_code.val = error_code.SUCCESS; - RCLCPP_DEBUG_STREAM(LOGGER, "Solved after " << (node_->now() - start_time).seconds() << " < " << timeout + RCLCPP_DEBUG_STREAM(LOGGER, "Solved after " << (steady_clock_.now() - start_time).seconds() << " < " << timeout << "s and " << attempt << " attempts"); return true; } } while (!timedOut(start_time, timeout)); - RCLCPP_DEBUG_STREAM(LOGGER, "IK timed out after " << (node_->now() - start_time).seconds() << " > " << timeout + RCLCPP_DEBUG_STREAM(LOGGER, "IK timed out after " << (steady_clock_.now() - start_time).seconds() << " > " << timeout << "s and " << attempt << " attempts"); error_code.val = error_code.TIMED_OUT; return false; diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index dff7eb2fc3..1e4063900f 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -35,7 +35,7 @@ urdfdom - python-lxml + python3-lxml moveit_ros_planning moveit_resources_fanuc_description diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 9e2ac6735e..79ce6e1c1e 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include #if __has_include() @@ -590,7 +590,9 @@ TEST_F(KinematicsTest, searchIKWithCallback) } kinematics_solver_->searchPositionIK(poses[0], fk_values, timeout_, solution, - boost::bind(&KinematicsTest::searchIKCallback, this, _1, _2, _3), error_code); + std::bind(&KinematicsTest::searchIKCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + error_code); if (error_code.val == error_code.SUCCESS) success++; else diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp index 0adc6d8bbf..84ec3f7119 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp @@ -55,14 +55,9 @@ void CHOMPInterface::loadParams() node_->get_parameter_or("chomp.obstacle_cost_weight", params_.obstacle_cost_weight_, 1.0); node_->get_parameter_or("chomp.learning_rate", params_.learning_rate_, 0.01); - // node_->param("add_randomness", params_.add_randomness_, false); node_->get_parameter_or("chomp.smoothness_cost_velocity", params_.smoothness_cost_velocity_, 0.0); node_->get_parameter_or("chomp.smoothness_cost_acceleration", params_.smoothness_cost_acceleration_, 1.0); node_->get_parameter_or("chomp.smoothness_cost_jerk", params_.smoothness_cost_jerk_, 0.0); - // node_->param("hmc_discretization", params_.hmc_discretization_, 0.01); - // node_->param("hmc_stochasticity", params_.hmc_stochasticity_, 0.01); - // node_->param("hmc_annealing_factor", params_.hmc_annealing_factor_, 0.99); - // node_->param("use_hamiltonian_monte_carlo", params_.use_hamiltonian_monte_carlo_, false); node_->get_parameter_or("chomp.ridge_factor", params_.ridge_factor_, 0.0); node_->get_parameter_or("chomp.use_pseudo_inverse", params_.use_pseudo_inverse_, false); node_->get_parameter_or("chomp.pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_, 1e-4); @@ -70,7 +65,6 @@ void CHOMPInterface::loadParams() node_->get_parameter_or("chomp.joint_update_limit", params_.joint_update_limit_, 0.1); node_->get_parameter_or("chomp.collision_clearance", params_.min_clearance_, 0.2); node_->get_parameter_or("chomp.collision_threshold", params_.collision_threshold_, 0.07); - // node_->param("random_jump_amount", params_.random_jump_amount_, 1.0); node_->get_parameter_or("chomp.use_stochastic_descent", params_.use_stochastic_descent_, true); params_.trajectory_initialization_method_ = "quintic-spline"; std::string method; diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp index 488b8f8870..485e67b12e 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp @@ -57,9 +57,9 @@ TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal) move_group_.setStartState(*(move_group_.getCurrentState())); move_group_.setJointValueTarget(std::vector({ 1.0, 1.0 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); } TEST_F(CHOMPMoveitTest, jointSpaceBadGoal) @@ -68,8 +68,8 @@ TEST_F(CHOMPMoveitTest, jointSpaceBadGoal) // joint2 is limited to [-PI/2, PI/2] move_group_.setJointValueTarget(std::vector({ 100.0, 2 * M_PI / 3.0 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_ROBOT_STATE); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_ROBOT_STATE); } TEST_F(CHOMPMoveitTest, cartesianGoal) @@ -82,17 +82,17 @@ TEST_F(CHOMPMoveitTest, cartesianGoal) target_pose1.position.z = 10000.; move_group_.setPoseTarget(target_pose1); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); // CHOMP doesn't support Cartesian-space goals at the moment - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS); } TEST_F(CHOMPMoveitTest, noStartState) { move_group_.setJointValueTarget(std::vector({ 0.2, 0.2 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); } TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) @@ -100,8 +100,8 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) move_group_.setStartState(*(move_group_.getCurrentState())); move_group_.setJointValueTarget(std::vector({ M_PI / 2.0, 0 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_MOTION_PLAN); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN); } int main(int argc, char** argv) diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml index d6681c1519..b6b2a82f11 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml +++ b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml @@ -5,14 +5,9 @@ smoothness_cost_weight: 0.1 obstacle_cost_weight: 1.0 learning_rate: 0.01 animate_path: true -add_randomness: false smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 -hmc_discretization: 0.01 -hmc_stochasticity: 0.01 -hmc_annealing_factor: 0.99 -use_hamiltonian_monte_carlo: false ridge_factor: 0.0 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1e-4 @@ -21,5 +16,4 @@ animate_endeffector_segment: "link3" joint_update_limit: 0.1 collision_clearance: 0.2 collision_threshold: 0.07 -random_jump_amount: 1.0 use_stochastic_descent: true diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch index ab54cf53fc..56100a65ff 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch @@ -27,9 +27,8 @@ - - - + + diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h index 4410ce753a..ab4a89c5e1 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h @@ -79,14 +79,9 @@ class ChompParameters double smoothness_cost_velocity_; /// variables associated with the cost in velocity double smoothness_cost_acceleration_; /// variables associated with the cost in acceleration double smoothness_cost_jerk_; /// variables associated with the cost in jerk - // bool add_randomness_; - // bool use_hamiltonian_monte_carlo_; bool use_stochastic_descent_; /// set this to true/false if you want to use stochastic descent while optimizing the /// cost. - // double hmc_stochasticity_; - // double hmc_discretization_; - // double hmc_annealing_factor_; double ridge_factor_; /// the noise added to the diagnal of the total quadratic cost matrix in the objective function bool use_pseudo_inverse_; /// enable pseudo inverse calculations or not. double pseudo_inverse_ridge_factor_; /// set the ridge factor if pseudo inverse is enabled @@ -95,7 +90,6 @@ class ChompParameters double min_clearance_; /// the minimum distance that needs to be maintained to avoid obstacles double collision_threshold_; /// the collision threshold cost that needs to be mainted to avoid collisions bool filter_mode_; - // double random_jump_amount_; static const std::vector VALID_INITIALIZATION_METHODS; std::string trajectory_initialization_method_; /// trajectory initialization method to be specified diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp index 26848eaac3..f8c5cde9f5 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp @@ -49,14 +49,9 @@ ChompParameters::ChompParameters() obstacle_cost_weight_ = 1.0; learning_rate_ = 0.01; - // add_randomness_ = false; smoothness_cost_velocity_ = 0.0; smoothness_cost_acceleration_ = 1.0; smoothness_cost_jerk_ = 0.0; - // hmc_discretization_ = 0.01; - // hmc_stochasticity_ = 0.01; - // hmc_annealing_factor_ = 0.99; - // use_hamiltonian_monte_carlo_ = false; ridge_factor_ = 0.0; use_pseudo_inverse_ = false; pseudo_inverse_ridge_factor_ = 1e-4; @@ -64,7 +59,6 @@ ChompParameters::ChompParameters() joint_update_limit_ = 0.1; min_clearance_ = 0.2; collision_threshold_ = 0.07; - // random_jump_amount_ = 1.0; use_stochastic_descent_ = true; filter_mode_ = false; trajectory_initialization_method_ = std::string("quintic-spline"); diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 77ed723b60..c19130877d 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -219,7 +219,7 @@ allocConstraintApproximationStateSampler(const ob::StateSpace* space, const std: if (sig != expected_signature) return ompl::base::StateSamplerPtr(); else - return ompl::base::StateSamplerPtr(new ConstraintApproximationStateSampler(space, state_storage, milestones)); + return std::make_shared(space, state_storage, milestones); } } // namespace ompl_interface @@ -343,9 +343,8 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: hexToMsg(serialization, msg); auto* cass = new ConstraintApproximationStateStorage(context_->getOMPLSimpleSetup()->getStateSpace()); cass->load((std::string{ path }.append("/").append(filename)).c_str()); - ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, - msg, filename, ompl::base::StateStoragePtr(cass), - milestones)); + auto cap = std::make_shared(group, state_space_parameterization, explicit_motions, msg, + filename, ompl::base::StateStoragePtr(cass), milestones); if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) RCLCPP_WARN(LOGGER, "Overwriting constraint approximation named '%s'", cap->getName().c_str()); constraint_approximations_[cap->getName()] = cap; @@ -461,11 +460,11 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs RCLCPP_INFO(LOGGER, "Spent %lf seconds constructing the database", (clock.now() - start).seconds()); if (state_storage) { - ConstraintApproximationPtr constraint_approx(new ConstraintApproximation( + auto constraint_approx = std::make_shared( group, options.state_space_parameterization, options.explicit_motions, constr_hard, group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", - state_storage, res.milestones)); + state_storage, res.milestones); if (constraint_approximations_.find(constraint_approx->getName()) != constraint_approximations_.end()) RCLCPP_WARN(LOGGER, "Overwriting constraint approximation named '%s'", constraint_approx->getName().c_str()); constraint_approximations_[constraint_approx->getName()] = constraint_approx; diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index ae1e264d4f..9a31b45d19 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -119,8 +119,7 @@ void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::Sh ompl::base::ScopedState<> ompl_start_state(spec_.constrained_state_space_); spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); ompl_simple_setup_->setStartState(ompl_start_state); - ompl_simple_setup_->setStateValidityChecker( - ob::StateValidityCheckerPtr(std::make_shared(this))); + ompl_simple_setup_->setStateValidityChecker(std::make_shared(this)); } else { @@ -166,7 +165,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str { std::string link_name = peval.substr(5, peval.length() - 6); if (getRobotModel()->hasLinkModel(link_name)) - return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name)); + return std::make_shared(this, link_name); else RCLCPP_ERROR(LOGGER, "Attempted to set projection evaluator with respect to position of link '%s', " @@ -213,7 +212,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str } else { - return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j)); + return std::make_shared(this, j); } } else @@ -266,7 +265,7 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp if (constraint_sampler) { RCLCPP_INFO(LOGGER, "%s: Allocating specialized state sampler for state space", name_.c_str()); - return ob::StateSamplerPtr(new ConstrainedSampler(this, constraint_sampler)); + return std::make_shared(this, constraint_sampler); } } RCLCPP_DEBUG(LOGGER, "%s: Allocating default state sampler for state space", name_.c_str()); @@ -429,8 +428,12 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout) { - ompl_simple_setup_->simplifySolution(timeout); + ompl::time::point start = ompl::time::now(); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); + registerTerminationCondition(ptc); + ompl_simple_setup_->simplifySolution(ptc); last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime(); + unregisterTerminationCondition(); } void ompl_interface::ModelBasedPlanningContext::interpolateSolution() @@ -506,14 +509,14 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() if (constraint_sampler) { - ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler)); + ob::GoalPtr goal = std::make_shared(this, goal_constraint, constraint_sampler); goals.push_back(goal); } } if (!goals.empty()) { - return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals)); + return goals.size() == 1 ? goals[0] : std::make_shared(goals); } else { diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 5edc97861d..b45b744ef0 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -51,7 +51,7 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model : node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(robot_model) - , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager()) + , constraint_sampler_manager_(std::make_shared()) , context_manager_(robot_model, constraint_sampler_manager_) , use_constraints_approximations_(true) , simplify_solutions_(true) @@ -67,7 +67,7 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model : node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(robot_model) - , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager()) + , constraint_sampler_manager_(std::make_shared()) , context_manager_(robot_model, constraint_sampler_manager_) , use_constraints_approximations_(true) , simplify_solutions_(true) @@ -182,29 +182,29 @@ void OMPLInterface::loadPlannerConfigurations() // get parameters specific for the robot planning group std::map specific_group_params; - for (const auto& k : KNOWN_GROUP_PARAMS) + for (const auto& [name, type] : KNOWN_GROUP_PARAMS) { std::string param_name{ group_name_param }; param_name += "."; - param_name += k.first; + param_name += name; if (node_->has_parameter(param_name)) { const rclcpp::Parameter parameter = node_->get_parameter(param_name); - if (parameter.get_type() != k.second) + if (parameter.get_type() != type) { - RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << k.first << "' expected [" - << rclcpp::to_string(k.second) << "] got [" + RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << name << "' expected [" + << rclcpp::to_string(type) << "] got [" << rclcpp::to_string(parameter.get_type()) << "]"); continue; } if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) - specific_group_params[k.first] = parameter.as_string(); + specific_group_params[name] = parameter.as_string(); else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) - specific_group_params[k.first] = moveit::core::toString(parameter.as_double()); + specific_group_params[name] = moveit::core::toString(parameter.as_double()); else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) - specific_group_params[k.first] = std::to_string(parameter.as_int()); + specific_group_params[name] = std::to_string(parameter.as_int()); else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) - specific_group_params[k.first] = std::to_string(parameter.as_bool()); + specific_group_params[name] = std::to_string(parameter.as_bool()); } } @@ -244,13 +244,13 @@ void OMPLInterface::loadPlannerConfigurations() } } - for (const std::pair& config : pconfig) + for (const auto& [name, config_settings] : pconfig) { - RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", config.first.c_str()); + RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", name.c_str()); - for (const std::pair& parameters : config.second.config) + for (const auto& [param_name, param_value] : config_settings.config) { - RCLCPP_DEBUG(LOGGER, " - %s = %s", parameters.first.c_str(), parameters.second.c_str()); + RCLCPP_DEBUG_STREAM(LOGGER, " - " << param_name << " = " << param_value); } } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 00b45a240c..ba06cf3d43 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -86,7 +86,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager const std::string& parameter_namespace) override { ompl_interface_ = std::make_unique(model, node, parameter_namespace); - config_settings_ = ompl_interface_->getPlannerConfigurations(); + setPlannerConfigurations(ompl_interface_->getPlannerConfigurations()); return true; } diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index c84074be5b..1a8caeef7a 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -308,9 +308,9 @@ void PlanningContextManager::registerDefaultPlanners() void PlanningContextManager::registerDefaultStateSpaces() { - registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new JointModelStateSpaceFactory())); - registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new PoseModelStateSpaceFactory())); - registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new ConstrainedPlanningStateSpaceFactory())); + registerStateSpaceFactory(std::make_shared()); + registerStateSpaceFactory(std::make_shared()); + registerStateSpaceFactory(std::make_shared()); } ConfiguredPlannerSelector PlanningContextManager::getPlannerSelector() const diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp index 672ba60cba..b860fa49cc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp @@ -27,32 +27,46 @@ #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" +namespace +{ +/** declare a parameter if not already declared. */ +template +void declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value) +{ + if (not node->has_parameter(name)) + { + node->declare_parameter(name, default_value); + } +} +} // namespace + namespace joint_limits { inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, const std::string& param_ns) { const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; + try { - node->declare_parameter(param_base_name + ".has_position_limits", false); - node->declare_parameter(param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_velocity_limits", false); - node->declare_parameter(param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_acceleration_limits", false); - node->declare_parameter(param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_jerk_limits", false); - node->declare_parameter(param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_effort_limits", false); - node->declare_parameter(param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".angle_wraparound", false); - node->declare_parameter(param_base_name + ".has_soft_limits", false); - node->declare_parameter(param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".has_position_limits", false); + declare_parameter(node, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".has_velocity_limits", false); + declare_parameter(node, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".has_acceleration_limits", false); + declare_parameter(node, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".has_jerk_limits", false); + declare_parameter(node, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".has_effort_limits", false); + declare_parameter(node, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".angle_wraparound", false); + declare_parameter(node, param_base_name + ".has_soft_limits", false); + declare_parameter(node, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + declare_parameter(node, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); } catch (const std::exception& ex) { diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h index b984f0a71e..e323ab80ae 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h @@ -140,15 +140,6 @@ class JointLimitsContainer */ bool verifyPositionLimit(const std::string& joint_name, const double& joint_position) const; - /** - * @brief verify position limits of multiple joints - * @param joint_names - * @param joint_positions - * @return - */ - bool verifyPositionLimits(const std::vector& joint_names, - const std::vector& joint_positions) const; - private: /** * @brief update the most strict limit with given joint limit diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index fda97af803..aa6b1e0677 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -218,3 +218,21 @@ bool isStateColliding(const bool test_for_self_collision, const planning_scene:: } // namespace pilz_industrial_motion_planner void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat); + +/** + * @brief Adapt goal pose, defined by position+orientation, to consider offset + * @param constraint to apply offset to + * @param offset to apply to the constraint + * @param orientation to apply to the offset + * @return final goal pose + */ +Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point& position, + const geometry_msgs::msg::Quaternion& orientation, + const geometry_msgs::msg::Vector3& offset); + +/** + * @brief Conviencency method, passing args from a goal constraint + * @param goal goal constraint + * @return final goal pose + */ +Eigen::Isometry3d getConstraintPose(const moveit_msgs::msg::Constraints& goal); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index d0d680eb82..6f5393040a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -225,7 +225,7 @@ class TrajectoryGenerator * - The start state velocity is below * TrajectoryGenerator::VELOCITY_TOLERANCE */ - void checkStartState(const moveit_msgs::msg::RobotState& start_state) const; + void checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const; void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::vector& expected_joint_names, const std::string& group_name) const; @@ -238,6 +238,12 @@ class TrajectoryGenerator const std::string& group_name) const; private: + /** + * @return joint state message including only active joints in group + */ + sensor_msgs::msg::JointState filterGroupValues(const sensor_msgs::msg::JointState& robot_state, + const std::string& group) const; + /** * @return True if scaling factor is valid, otherwise false. */ diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 9151b6c872..d8b4ee3378 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -119,25 +119,6 @@ bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, co (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position))); } -bool JointLimitsContainer::verifyPositionLimits(const std::vector& joint_names, - const std::vector& joint_positions) const -{ - if (joint_names.size() != joint_positions.size()) - { - throw std::out_of_range("joint_names vector has a different size than joint_positions vector."); - } - - for (std::size_t i = 0; i < joint_names.size(); ++i) - { - if (!verifyPositionLimit(joint_names.at(i), joint_positions.at(i))) - { - return false; - } - } - - return true; -} - void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit) { // check position limits diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index 4620ca83f6..fa8ed5d482 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -161,10 +161,10 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); + opt.before_execution_callback_ = std::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); - opt.plan_callback_ = boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), - boost::placeholders::_1); + opt.plan_callback_ = std::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, std::cref(goal->request), + std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp index 503a1a3c89..682c7b6bec 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp @@ -123,9 +123,9 @@ std::unique_ptr PathCircleGenerator::circleFromInterim(const KDL::Fra KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); try { - return std::unique_ptr(new KDL::Path_Circle(start_pose, center_point, kdl_aux_point, goal_pose.M, alpha, - rot_interpo, eqradius, - true /* take ownership of RotationalInterpolation */)); + return std::unique_ptr( + std::make_unique(start_pose, center_point, kdl_aux_point, goal_pose.M, alpha, rot_interpo, + eqradius, true /* take ownership of RotationalInterpolation */)); } catch (KDL::Error_MotionPlanning&) // LCOV_EXCL_START // The cases where // KDL would throw are already handled diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index fe078b2ef4..2e07bf8ce3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -111,7 +111,7 @@ void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr& { traj_tail_ = other; // Reserve space in container for new trajectory - traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName())); + traj_cont_.emplace_back(std::make_shared(model_, other->getGroupName())); return; } @@ -121,7 +121,7 @@ void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr& appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_); traj_tail_ = other; // Create new container element - traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName())); + traj_cont_.emplace_back(std::make_shared(model_, other->getGroupName())); return; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 767973a045..774d012c5a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -605,3 +605,27 @@ void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat) tf2::convert(quat, q); quat = tf2::toMsg(q.normalized()); } + +Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point& position, + const geometry_msgs::msg::Quaternion& orientation, + const geometry_msgs::msg::Vector3& offset) +{ + Eigen::Quaterniond quat; + tf2::fromMsg(orientation, quat); + quat.normalize(); + Eigen::Vector3d v; + tf2::fromMsg(position, v); + + Eigen::Isometry3d pose = Eigen::Translation3d(v) * quat; + + tf2::fromMsg(offset, v); + pose.translation() -= quat * v; + return pose; +} + +Eigen::Isometry3d getConstraintPose(const moveit_msgs::msg::Constraints& goal) +{ + return getConstraintPose(goal.position_constraints.front().constraint_region.primitive_poses.front().position, + goal.orientation_constraints.front().orientation, + goal.position_constraints.front().target_point_offset); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 6a6f091376..484d98604a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -43,6 +43,8 @@ #include #include #endif +#include + #include #include @@ -51,6 +53,31 @@ namespace pilz_industrial_motion_planner { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator"); + +sensor_msgs::msg::JointState TrajectoryGenerator::filterGroupValues(const sensor_msgs::msg::JointState& robot_state, + const std::string& group) const +{ + const std::vector& group_joints{ robot_model_->getJointModelGroup(group)->getActiveJointModelNames() }; + sensor_msgs::msg::JointState group_state; + group_state.name.reserve(group_joints.size()); + group_state.position.reserve(group_joints.size()); + group_state.velocity.reserve(group_joints.size()); + + for (size_t i = 0; i < robot_state.name.size(); ++i) + { + if (std::find(group_joints.begin(), group_joints.end(), robot_state.name.at(i)) != group_joints.end()) + { + group_state.name.push_back(robot_state.name.at(i)); + group_state.position.push_back(robot_state.position.at(i)); + if (i < robot_state.velocity.size()) + { + group_state.velocity.push_back(robot_state.velocity.at(i)); + } + } + } + return group_state; +} + void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& /*req*/) const { // Empty implementation, in case the derived class does not want @@ -89,7 +116,8 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name) } } -void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& start_state) const +void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& start_state, + const std::string& group) const { if (start_state.joint_state.name.empty()) { @@ -101,14 +129,26 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& st throw SizeMismatchInStartState("Joint state name and position do not match in start state"); } - if (!planner_limits_.getJointLimitContainer().verifyPositionLimits(start_state.joint_state.name, - start_state.joint_state.position)) + sensor_msgs::msg::JointState group_start_state{ filterGroupValues(start_state.joint_state, group) }; + + // verify joint position limits + const JointLimitsContainer& limits{ planner_limits_.getJointLimitContainer() }; + std::string error_msg; + for (auto joint : boost::combine(group_start_state.name, group_start_state.position)) + { + if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>())) + { + error_msg.append(error_msg.empty() ? "start state joints outside their position limits: " : ", "); + error_msg.append(joint.get<0>()); + } + } + if (!error_msg.empty()) { - throw JointsOfStartStateOutOfRange("Joint state out of range in start state"); + throw JointsOfStartStateOutOfRange(error_msg); } // does not allow start velocity - if (!std::all_of(start_state.joint_state.velocity.begin(), start_state.joint_state.velocity.end(), + if (!std::all_of(group_start_state.velocity.begin(), group_start_state.velocity.end(), [this](double v) { return std::fabs(v) < this->VELOCITY_TOLERANCE; })) { throw NonZeroVelocityInStartState("Trajectory Generator does not allow non-zero start velocity"); @@ -218,7 +258,7 @@ void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRe checkVelocityScaling(req.max_velocity_scaling_factor); checkAccelerationScaling(req.max_acceleration_scaling_factor); checkForValidGroupName(req.group_name); - checkStartState(req.start_state); + checkStartState(req.start_state, req.group_name); checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name); } @@ -227,7 +267,7 @@ void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& sta const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const { - robot_trajectory::RobotTrajectoryPtr rt(new robot_trajectory::RobotTrajectory(robot_model_, group_name)); + auto rt = std::make_shared(robot_model_, group_name); rt->setRobotTrajectoryMsg(start_state, joint_trajectory); res.trajectory_ = rt; @@ -250,9 +290,9 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca const double& max_acceleration_scaling_factor, const std::unique_ptr& path) const { - std::unique_ptr vp_trans(new KDL::VelocityProfile_Trap( + std::unique_ptr vp_trans = std::make_unique( max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(), - max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration())); + max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration()); if (path->PathLength() > std::numeric_limits::epsilon()) // avoid division by zero { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index d911ce2b60..782788b614 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -150,12 +150,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - geometry_msgs::msg::Pose goal_pose_msg; - goal_pose_msg.position = - req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; - goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; - normalizeQuaternion(goal_pose_msg.orientation); - tf2::convert(goal_pose_msg, info.goal_pose); + info.goal_pose = getConstraintPose(req.goal_constraints.front()); } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -187,13 +182,23 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni // LCOV_EXCL_STOP // not able to trigger here since lots of checks before // are in place } - - Eigen::Vector3d circ_path_point; - tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - circ_path_point); - info.circ_path_point.first = req.path_constraints.name; - info.circ_path_point.second = circ_path_point; + if (!req.goal_constraints.front().position_constraints.empty()) + { + const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); + info.circ_path_point.second = + getConstraintPose( + req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, + goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset) + .translation(); + } + else + { + Eigen::Vector3d circ_path_point; + tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, + circ_path_point); + info.circ_path_point.second = circ_path_point; + } } void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 1693e4dd7b..f6af37f2d8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -124,12 +124,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - geometry_msgs::msg::Pose goal_pose_msg; - goal_pose_msg.position = - req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; - goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; - normalizeQuaternion(goal_pose_msg.orientation); - tf2::convert(goal_pose_msg, info.goal_pose); + info.goal_pose = getConstraintPose(req.goal_constraints.front()); } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -204,7 +199,8 @@ std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affin planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); - return std::unique_ptr(new KDL::Path_Line(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true)); + return std::unique_ptr( + std::make_unique(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true)); } } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index c65ed801a3..ae888e72bc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -237,23 +237,12 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position; } } - // slove the ik + // solve the ik else { - geometry_msgs::msg::Point p = - req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position; - p.x -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.x; - p.y -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.y; - p.z -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.z; - - geometry_msgs::msg::Pose pose; - pose.position = p; - pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation; - Eigen::Isometry3d pose_eigen; - normalizeQuaternion(pose.orientation); - tf2::convert(pose, pose_eigen); + Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front()); if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, - pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) { throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md index 4f2abec293..867c7f6587 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md @@ -41,7 +41,7 @@ to be moved out of singularities. ## Test Sequence: 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` - 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner` 3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) ![moveit_1](img/acceptance_test_lin_img1.png) 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute". diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md index 732ec8ec4f..be2dce5489 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md @@ -40,7 +40,7 @@ This test checks that the real robot system is able to perform a PTP Motion to a ## Test Sequence: 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` - 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner` 3. The motion planning widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) ![moveit_1](img/acceptance_test_ptp_img1.png) 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. Click on "plan and execute". diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning.test index ba73e8b56b..6d26187e4d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning.test @@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_frankaemika_panda.test index d360fbcafa..44f7d54b3d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_frankaemika_panda.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_frankaemika_panda.test @@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_with_gripper.test index 64fb904bde..27212a2b92 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_with_gripper.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_with_gripper.test @@ -34,17 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability.test index 9eec6f308b..049cb5a253 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability.test @@ -33,19 +33,7 @@ POSSIBILITY OF SUCH DAMAGE. --> - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_frankaemika_panda.test index dbec926599..604d6ace07 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_frankaemika_panda.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_frankaemika_panda.test @@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_with_gripper.test index f214d4baf7..0c516453ac 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_with_gripper.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_with_gripper.test @@ -33,17 +33,7 @@ POSSIBILITY OF SUCH DAMAGE. --> - - - [/move_group/fake_controller_joint_states] - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp index 9995755495..62c9c32d3b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp @@ -628,13 +628,13 @@ TEST_F(IntegrationTestCommandListManager, TestGroupSpecificStartState) seq.erase(4, seq.size()); Gripper& gripper{ seq.getCmd(0) }; - gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, _1)); + gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, std::placeholders::_1)); // By deleting the model we guarantee that the start state only consists // of joints of the gripper group without the manipulator gripper.getStartConfiguration().clearModel(); PtpJointCart& ptp{ seq.getCmd(1) }; - ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, _1)); + ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, std::placeholders::_1)); // By deleting the model we guarantee that the start state only consists // of joints of the manipulator group without the gripper ptp.getStartConfiguration().clearModel(); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp index 5baec63179..9908ac959d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp @@ -249,7 +249,9 @@ TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState) // Set start state using std::placeholders::_1; - JointConfiguration config{ "MyGroupName", { -1., 2., -3., 4., -5., 0. }, std::bind(&createJointName, _1) }; + JointConfiguration config{ "MyGroupName", + { -1., 2., -3., 4., -5., 0. }, + std::bind(&createJointName, std::placeholders::_1) }; req_list.items[1].req.start_state.joint_state = config.toSensorMsg(); moveit_msgs::GetMotionSequence srv; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp index 3863973613..8d64ff1cff 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp @@ -203,25 +203,6 @@ TEST_F(JointLimitsContainerTest, FirstPositionEmpty) EXPECT_EQ(-1, limits.min_position); } -/** - * @brief Check position limits - */ -TEST_F(JointLimitsContainerTest, CheckVerifyPositionLimits) -{ - // positive check: inside limits - std::vector joint_names{ "joint1", "joint2" }; - std::vector joint_positions{ 0.5, 0.5 }; - EXPECT_TRUE(container_.verifyPositionLimits(joint_names, joint_positions)); - - // outside limit2 - joint_positions[1] = 7; - EXPECT_FALSE(container_.verifyPositionLimits(joint_names, joint_positions)); - - // invalid size - std::vector joint_positions1{ 0. }; - EXPECT_THROW(container_.verifyPositionLimits(joint_names, joint_positions1), std::out_of_range); -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp index 83eda91c35..1214b00646 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp @@ -103,7 +103,8 @@ TEST(CommandPlannerTestDirect, FailOnLoadContext) }; /// Registered a found loader - pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader(new TestPlanningContextLoader()); + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader = + std::make_shared(); planner.registerContextLoader(planning_context_loader); moveit_msgs::msg::MotionPlanRequest req; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 2987dfabbd..7e9b7c0d7c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -121,10 +121,10 @@ class PlanningContextTest : public ::testing::Test new typename T::Type_("TestPlanningContext", planning_group_, robot_model_, limits)); // Define and set the current scene - planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); + auto scene = std::make_shared(robot_model_); moveit::core::RobotState current_state(robot_model_); current_state.setToDefaultValues(); - current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 }); + current_state.setJointGroupPositions(planning_group_, std::vector{ 0, 1.57, 1.57, 0, 0.2, 0 }); scene->setCurrentState(current_state); planning_context_->setPlanningScene(scene); // TODO Check what happens if this is missing } @@ -148,8 +148,9 @@ class PlanningContextTest : public ::testing::Test // state state in joint space, used as initial positions, since IK does not // work at zero positions rstate.setJointGroupPositions(this->planning_group_, - { 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452, - 1.1801525390026025e-11, 1.8236082178909834, 8.591793942969161e-12 }); + std::vector{ 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452, + 1.1801525390026025e-11, 1.8236082178909834, + 8.591793942969161e-12 }); Eigen::Isometry3d start_pose(Eigen::Isometry3d::Identity()); start_pose.translation() = Eigen::Vector3d(0.3, 0, 0.65); rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), start_pose); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index f509a1dd47..d5fcb73c7b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -466,7 +466,7 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) // // std::vector ik_state; // std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state), -// boost::bind(&std::map::value_type::second, _1)); +// std::bind(&std::map::value_type::second, std::placeholders::_1)); // // rstate.setJointGroupPositions(jmg, ik_state); // rstate.update(); @@ -487,7 +487,7 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) // // std::vector ik_state2; // std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2), -// boost::bind(&std::map::value_type::second, _1)); +// std::bind(&std::map::value_type::second, std::placeholders::_1)); // rstate.setJointGroupPositions(jmg, ik_state2); // rstate.update(); // diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index 06eabc333a..b4fe4c7600 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -131,8 +131,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test planner_limits.setCartesianLimits(cart_limits); // create planner instance - trajectory_generator_ = - std::unique_ptr(new typename T::Type_(robot_model_, planner_limits, planning_group_)); + trajectory_generator_ = std::make_unique(robot_model_, planner_limits, planning_group_); ASSERT_NE(nullptr, trajectory_generator_) << "failed to create trajectory generator"; // create a valid motion plan request with goal in joint space as basis for @@ -142,7 +141,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test req_.max_acceleration_scaling_factor = 1.0; moveit::core::RobotState rstate(robot_model_); rstate.setToDefaultValues(); - rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); + rstate.setJointGroupPositions(planning_group_, std::vector{ 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0)); moveit::core::robotStateToRobotStateMsg(rstate, req_.start_state, false); moveit_msgs::msg::Constraints goal_constraint; diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp index 69054bc45d..38b5f4549b 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -141,27 +141,27 @@ XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename) : Testdat pt::read_xml(path_filename_, tree_, pt::xml_parser::no_comments); using std::placeholders::_1; - cmd_getter_funcs_["ptp"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1))); + cmd_getter_funcs_["ptp"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJoint, this, std::placeholders::_1))); cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1))); - cmd_getter_funcs_["ptp_cart_cart"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpCart, this, _1))); - - cmd_getter_funcs_["lin"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinJoint, this, _1))); - cmd_getter_funcs_["lin_cart"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinCart, this, _1))); - - cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1))); - cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1))); - cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1))); - - cmd_getter_funcs_["gripper"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getGripper, this, _1))); + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJointCart, this, std::placeholders::_1))); + cmd_getter_funcs_["ptp_cart_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpCart, this, std::placeholders::_1))); + + cmd_getter_funcs_["lin"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinJoint, this, std::placeholders::_1))); + cmd_getter_funcs_["lin_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinCart, this, std::placeholders::_1))); + + cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter( + std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, std::placeholders::_1))); + cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter( + std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, std::placeholders::_1))); + cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter( + std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, std::placeholders::_1))); + + cmd_getter_funcs_["gripper"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getGripper, this, std::placeholders::_1))); } XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename, diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp index c7753b3c0c..c162f6578a 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp @@ -647,15 +647,15 @@ void EnvironmentChain3D::setMotionPrimitives(const std::string& group_name) for (unsigned int i = 0; i < jmg->getActiveDOFNames().size(); ++i) { const planning_models::RobotModel::JointModel* joint = jmg->getJointModel(jmg->getActiveDOFNames()[i]); - boost::shared_ptr jmw(new JointMotionWrapper(joint)); + auto jmw = boost::make_shared(joint); joint_motion_wrappers_.push_back(jmw); // TODO - figure out which DOFs have something to do with end effector position if (!planning_parameters_.use_bfs_ || i < 4) { - boost::shared_ptr sing_pos( - new SingleJointMotionPrimitive(jmw, i, planning_parameters_.joint_motion_primitive_distance_)); - boost::shared_ptr sing_neg( - new SingleJointMotionPrimitive(jmw, i, -planning_parameters_.joint_motion_primitive_distance_)); + auto sing_pos = + boost::make_shared(jmw, i, planning_parameters_.joint_motion_primitive_distance_); + auto sing_neg = boost::make_shared( + jmw, i, -planning_parameters_.joint_motion_primitive_distance_); possible_actions_.push_back(sing_pos); possible_actions_.push_back(sing_neg); } diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp index afafeba5d8..f60464db37 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp @@ -49,7 +49,7 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ start_state); ros::WallTime wt = ros::WallTime::now(); - boost::shared_ptr env_chain(new EnvironmentChain3D(planning_scene)); + auto env_chain = boost::make_shared(planning_scene); if (!env_chain->setupForMotionPlan(planning_scene, req, res, params)) { // std::cerr << "Env chain setup failing" << '\n'; @@ -60,7 +60,7 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ boost::this_thread::interruption_point(); // DummyEnvironment* dummy_env = new DummyEnvironment(); - boost::shared_ptr planner(new ARAPlanner(env_chain.get(), true)); + auto planner = boost::make_shared(env_chain.get(), true); planner->set_initialsolution_eps(100.0); planner->set_search_mode(true); planner->force_planning_from_scratch(); diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp index a847ace996..bf0e6b0aec 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp @@ -58,10 +58,10 @@ bool SBPLMetaInterface::solve(const planning_scene::PlanningSceneConstPtr& plann PlanningParameters param_no_bfs; param_no_bfs.use_bfs_ = false; moveit_msgs::srv::GetMotionPlan::Response res1, res2; - boost::thread thread1(boost::bind(&SBPLMetaInterface::runSolver, this, true, boost::cref(planning_scene), - boost::cref(req), boost::ref(res1), param_bfs)); - boost::thread thread2(boost::bind(&SBPLMetaInterface::runSolver, this, false, boost::cref(planning_scene), - boost::cref(req), boost::ref(res2), param_no_bfs)); + boost::thread thread1(std::bind(&SBPLMetaInterface::runSolver, this, true, boost::cref(planning_scene), + boost::cref(req), boost::ref(res1), param_bfs)); + boost::thread thread2(std::bind(&SBPLMetaInterface::runSolver, this, false, boost::cref(planning_scene), + boost::cref(req), boost::ref(res2), param_no_bfs)); boost::mutex::scoped_lock lock(planner_done_mutex_); planner_done_condition_.wait(lock); diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt index ee7e2d0bd5..3349523adc 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt @@ -4,10 +4,12 @@ project(moveit_resources_prbt_ikfast_manipulator_plugin) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) -add_compile_options(-Wextra) -add_compile_options(-Wno-unused-parameter) -add_compile_options(-Wno-unused-variable) +if(NOT WIN32) + add_compile_options(-Wall) + add_compile_options(-Wextra) + add_compile_options(-Wno-unused-parameter) + add_compile_options(-Wno-unused-variable) +endif() # enable aligned new in gcc7+ if(CMAKE_COMPILER_IS_GNUCXX) @@ -29,8 +31,11 @@ include_directories(include) add_library(prbt_manipulator_moveit_ikfast_plugin SHARED src/prbt_manipulator_ikfast_moveit_plugin.cpp) -# suppress warnings about unused variables in OpenRave's solver code -target_compile_options(prbt_manipulator_moveit_ikfast_plugin PRIVATE -Wno-unused-variable) + +if(NOT WIN32) + # suppress warnings about unused variables in OpenRave's solver code + target_compile_options(prbt_manipulator_moveit_ikfast_plugin PRIVATE -Wno-unused-variable) +endif() ament_target_dependencies(prbt_manipulator_moveit_ikfast_plugin moveit_core pluginlib diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp index d9d6f8cb8e..7479184c92 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp @@ -384,8 +384,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, const std::string& to, Eigen::Isometry3d& transform, bool& differs_from_identity) { - RobotStatePtr robot_state; - robot_state.reset(new RobotState(robot_model_)); + RobotStatePtr robot_state = std::make_shared(robot_model_); robot_state->setToDefaultValues(); auto* from_link = robot_model_->getLinkModel(from); diff --git a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py index 678fe0dd4a..824dd83460 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py +++ b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py @@ -187,10 +187,7 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index 6ad995e4ff..3a84346eee 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -286,8 +286,7 @@ struct CartPoseTermInfo : public TermInfo static TermInfoPtr create() { - TermInfoPtr out(new CartPoseTermInfo()); - return out; + return std::make_shared(); } }; @@ -326,8 +325,7 @@ struct JointPoseTermInfo : public TermInfo static TermInfoPtr create() { - TermInfoPtr out(new JointPoseTermInfo()); - return out; + return std::make_shared(); } }; @@ -356,8 +354,7 @@ struct JointVelTermInfo : public TermInfo static TermInfoPtr create() { - TermInfoPtr out(new JointVelTermInfo()); - return out; + return std::make_shared(); } }; diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp index 6fc2df1860..7b492c2f0c 100644 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ b/moveit_planners/trajopt/src/problem_description.cpp @@ -177,7 +177,7 @@ TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) if ((use_time == false) && (pci.basic_info.use_time == true)) PRINT_AND_THROW("No terms use time and basic_info is not set correctly. Try basic_info.use_time = false"); - TrajOptProblemPtr prob(new TrajOptProblem(pci)); + auto prob = std::make_shared(pci); // Generate initial trajectory and check its size moveit::core::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); @@ -291,15 +291,15 @@ void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) } else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) { - sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp)); - prob.addCost(sco::CostPtr(new sco::CostFromErrFunc(f, prob.GetVarRow(timestep, 0, n_dof), - concatVector(rot_coeffs, pos_coeffs), sco::ABS, name))); + sco::VectorOfVectorPtr f = std::make_shared(input_pose, prob.GetPlanningScene(), link, tcp); + prob.addCost(std::make_shared(f, prob.GetVarRow(timestep, 0, n_dof), + concatVector(rot_coeffs, pos_coeffs), sco::ABS, name)); } else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) { - sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp)); - prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( - f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name))); + sco::VectorOfVectorPtr f = std::make_shared(input_pose, prob.GetPlanningScene(), link, tcp); + prob.addConstraint(std::make_shared( + f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name)); } else { @@ -361,15 +361,15 @@ void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost if (is_upper_zeros && is_lower_zeros) { - prob.addCost(sco::CostPtr(new trajopt::JointPosEqCost(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), first_step, last_step))); + prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), first_step, last_step)); prob.getCosts().back()->setName(name); } else { - prob.addCost(sco::CostPtr(new trajopt::JointPosIneqCost(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step))); + prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step)); prob.getCosts().back()->setName(name); } } @@ -378,15 +378,15 @@ void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint if (is_upper_zeros && is_lower_zeros) { - prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosEqConstraint( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step))); + prob.addConstraint(std::make_shared( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)); prob.getConstraints().back()->setName(name); } else { - prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosIneqConstraint( + prob.addConstraint(std::make_shared( joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step))); + util::toVectorXd(lower_tols), first_step, last_step)); prob.getConstraints().back()->setName(name); } } @@ -458,19 +458,19 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) if (is_upper_zeros && is_lower_zeros) { trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addCost(sco::CostPtr(new sco::CostFromErrFunc( - sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), - sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j)))); + prob.addCost(std::make_shared( + std::make_shared(targets[j], upper_tols[j], lower_tols[j]), + std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j))); } // Otherwise it's a hinged "inequality" cost else { trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addCost(sco::CostPtr(new sco::CostFromErrFunc( - sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), - sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j)))); + prob.addCost(std::make_shared( + std::make_shared(targets[j], upper_tols[j], lower_tols[j]), + std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j))); } } } @@ -489,18 +489,18 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) if (is_upper_zeros && is_lower_zeros) { trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( - sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), - sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j)))); + prob.addConstraint(std::make_shared( + std::make_shared(targets[j], upper_tols[j], lower_tols[j]), + std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j))); } // Otherwise it's a hinged "inequality" constraint else { trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( - sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), - sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + prob.addConstraint(std::make_shared( + std::make_shared(targets[j], upper_tols[j], lower_tols[j]), + std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), util::toVectorXd(single_jnt_coeffs), sco::INEQ, name + "_j" + std::to_string(j)))); } } @@ -510,15 +510,15 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost if (is_upper_zeros && is_lower_zeros) { - prob.addCost(sco::CostPtr(new trajopt::JointVelEqCost(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), first_step, last_step))); + prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), first_step, last_step)); prob.getCosts().back()->setName(name); } else { - prob.addCost(sco::CostPtr(new trajopt::JointVelIneqCost(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step))); + prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step)); prob.getCosts().back()->setName(name); } } @@ -527,15 +527,15 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint if (is_upper_zeros && is_lower_zeros) { - prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelEqConstraint( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step))); + prob.addConstraint(std::make_shared( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)); prob.getConstraints().back()->setName(name); } else { - prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelIneqConstraint( + prob.addConstraint(std::make_shared( joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step))); + util::toVectorXd(lower_tols), first_step, last_step)); prob.getConstraints().back()->setName(name); } } diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/src/trajopt_interface.cpp index bf9da1494d..cf78e2667f 100644 --- a/moveit_planners/trajopt/src/trajopt_interface.cpp +++ b/moveit_planners/trajopt/src/trajopt_interface.cpp @@ -60,7 +60,7 @@ namespace trajopt_interface { TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface") { - trajopt_problem_ = TrajOptProblemPtr(new TrajOptProblem); + trajopt_problem_ = std::make_shared(); setDefaultTrajOPtParams(); // TODO: callbacks should be defined by the user @@ -87,7 +87,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni bool robot_model_ok = static_cast(robot_model); if (!robot_model_ok) ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); - moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model)); + auto current_state = std::make_shared(robot_model); *current_state = planning_scene->getCurrentState(); const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); if (joint_model_group == nullptr) @@ -151,7 +151,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Cartesian Constraints"); if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty()) { - CartPoseTermInfoPtr cart_goal_pos(new CartPoseTermInfo); + CartPoseTermInfoPtr cart_goal_pos = std::make_shared(); // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file // TODO: Multiple Cartesian constraints @@ -177,7 +177,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Constraints from request goal_constraints"); for (auto goal_cnt : req.goal_constraints) { - JointPoseTermInfoPtr joint_pos_term(new JointPoseTermInfo); + JointPoseTermInfoPtr joint_pos_term = std::make_shared(); // When using MotionPlanning Display in RViz, the created request has no name for the constriant setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp"); @@ -192,7 +192,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Constraints from request start_state"); // add the start pos from request as a constraint - JointPoseTermInfoPtr joint_start_pos(new JointPoseTermInfo); + auto joint_start_pos = std::make_shared(); joint_start_pos->targets = start_joint_values; setJointPoseTermInfoParams(joint_start_pos, "start_pos"); @@ -200,7 +200,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Velocity Constraints, hard-coded"); // TODO: should be defined by user, its parameters should be added to setup.yaml - JointVelTermInfoPtr joint_vel(new JointVelTermInfo); + auto joint_vel = std::make_shared(); joint_vel->coeffs = std::vector(dof, 5.0); joint_vel->targets = std::vector(dof, 0.0); diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp index 1ed418f5ca..c8f591e102 100644 --- a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp +++ b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp @@ -65,8 +65,7 @@ class TrajOptPlannerManager : public planning_interface::PlannerManager { ROS_INFO(" ======================================= group name: %s, robot model: %s", gpName.c_str(), model->getName().c_str()); - planning_contexts_[gpName] = - TrajOptPlanningContextPtr(new TrajOptPlanningContext("trajopt_planning_context", gpName, model)); + planning_contexts_[gpName] = std::make_shared("trajopt_planning_context", gpName, model); } return true; diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp index af6e3ab0f9..24590366b1 100644 --- a/moveit_planners/trajopt/src/trajopt_planning_context.cpp +++ b/moveit_planners/trajopt/src/trajopt_planning_context.cpp @@ -17,7 +17,7 @@ TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name, : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) { ROS_INFO(" ======================================= TrajOptPlanningContext is constructed"); - trajopt_interface_ = TrajOptInterfacePtr(new TrajOptInterface()); + trajopt_interface_ = std::make_shared(); } bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) @@ -28,8 +28,7 @@ bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedRespons if (trajopt_solved) { res.trajectory_.resize(1); - res.trajectory_[0] = - robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); + res.trajectory_[0] = std::make_shared(robot_model_, getGroupName()); moveit::core::RobotState start_state(robot_model_); moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp index 7d3e6aaed0..4f571ddb9d 100644 --- a/moveit_planners/trajopt/test/trajectory_test.cpp +++ b/moveit_planners/trajopt/test/trajectory_test.cpp @@ -91,7 +91,7 @@ TEST_F(TrajectoryTest, goalTolerance) const std::string NODE_NAME = "trajectory_test"; // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group - moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model_)); + auto current_state = std::make_shared(robot_model_); current_state->setToDefaultValues(); const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); @@ -99,7 +99,7 @@ TEST_F(TrajectoryTest, goalTolerance) const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); EXPECT_EQ(joint_names.size(), 7); - planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_)); + auto planning_scene = std::make_shared(robot_model_); // Create response and request planning_interface::MotionPlanRequest req; @@ -108,7 +108,7 @@ TEST_F(TrajectoryTest, goalTolerance) // Set start state // ====================================================================================== std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; - moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model_)); + auto start_state = std::make_shared(robot_model_); start_state->setJointGroupPositions(joint_model_group, start_joint_values); start_state->update(); @@ -119,7 +119,7 @@ TEST_F(TrajectoryTest, goalTolerance) // Set the goal state and joints tolerance // ======================================================================================== - moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(robot_model_)); + auto goal_state = std::make_shared(robot_model_); std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); goal_state->update(); diff --git a/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp b/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp index 5f319c1ca2..70efcb7469 100644 --- a/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp +++ b/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp @@ -84,7 +84,7 @@ class MoveItControllerManagerExample : public moveit_controller_manager::MoveItC moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override { - return moveit_controller_manager::MoveItControllerHandlePtr(new ExampleControllerHandle(name)); + return std::make_shared(name); } /* diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 81741d4484..ec71aac3be 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -51,11 +51,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle( node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle") , allow_failure_(false) , parallel_jaw_gripper_(false) + , max_effort_(max_effort) { } @@ -118,7 +120,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle idx) + { goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx]; + } + else + { + goal.command.max_effort = max_effort_; + } } rclcpp_action::Client::SendGoalOptions send_goal_options; // Active callback @@ -208,6 +215,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle(node_, controller_name, action_ns); + double max_effort; + const std::string& max_effort_param = makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"); + if (!node->get_parameter(max_effort_param, max_effort)) + { + RCLCPP_INFO_STREAM(LOGGER, "Max effort set to 0.0"); + max_effort = 0.0; + } + + new_handle = std::make_shared(node_, controller_name, action_ns, max_effort); bool parallel_gripper = false; if (node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "parallel"), parallel_gripper) && parallel_gripper) { diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 4c241f0f38..bd43bca148 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -10,6 +10,7 @@ moveit_package() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_warehouse REQUIRED) find_package(pluginlib REQUIRED) @@ -23,6 +24,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp Boost tf2_eigen + moveit_core moveit_ros_planning moveit_ros_warehouse pluginlib diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index 4aa9bfe629..b35618e224 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -1281,11 +1281,11 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR // Compute IK ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ..."); ros::WallTime startTime = ros::WallTime::now(); - success = - robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, - req.motion_plan_request.num_planning_attempts, - req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + success = robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, + req.motion_plan_request.num_planning_attempts, + req.motion_plan_request.allowed_planning_time, + std::bind(&isIKSolutionCollisionFree, planning_scene_.get(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, &reachable)); if (success) { ROS_INFO(" Success!"); @@ -1375,7 +1375,8 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + std::bind(&isIKSolutionCollisionFree, planning_scene_.get(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, &reachable)); double duration = (ros::WallTime::now() - startTime).toSec(); if (success) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index cffcc2b7fa..54d20d2555 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -18,6 +18,7 @@ ament_cmake moveit_common + moveit_core libboost-dev libboost-date-time-dev diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 0ccf5f44ca..fd0a70261e 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1111,7 +1111,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std: return; } - out << "MoveIt version " << MOVEIT_VERSION << '\n'; + out << "MoveIt version " << MOVEIT_VERSION_STR << '\n'; out << "Experiment " << brequest.name << '\n'; out << "Running on " << hostname << '\n'; out << "Starting at " << start_time << '\n'; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index f4b2ca46d0..feb36fa16e 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -235,14 +235,13 @@ void LocalPlannerComponent::executeIteration() // If the planner received an action request and a global solution it starts to plan locally case LocalPlannerState::LOCAL_PLANNING_ACTIVE: { - // Read current planning scene planning_scene_monitor_->updateSceneWithCurrentState(); - planning_scene_monitor_->lockSceneRead(); // LOCK planning scene - planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor_->getPlanningScene(); - planning_scene_monitor_->unlockSceneRead(); // UNLOCK planning scene - // Get current state - auto current_robot_state = planning_scene->getCurrentStateNonConst(); + // Read current robot state + const moveit::core::RobotState current_robot_state = [this] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor_); + return ls->getCurrentState(); + }(); // Check if the global goal is reached if (trajectory_operator_instance_->getTrajectoryProgress(current_robot_state) > PROGRESS_THRESHOLD) diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index be58f6038c..9322498a69 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -187,10 +187,7 @@ def generate_common_hybrid_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index 709a9cb8fc..d6aece727c 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp @@ -54,14 +54,16 @@ void move_group::MoveGroupPickPlaceAction::initialize() // start the pickup action server pickup_action_server_ = std::make_unique>( - root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false); - pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); + root_node_handle_, PICKUP_ACTION, + std::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, std::placeholders::_1), false); + pickup_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); pickup_action_server_->start(); // start the place action server place_action_server_ = std::make_unique>( - root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false); - place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this)); + root_node_handle_, PLACE_ACTION, + std::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, std::placeholders::_1), false); + place_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this)); place_action_server_->start(); } @@ -259,17 +261,18 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute( opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this); + opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), &action_res, _1); + opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), + &action_res, std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { - opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - _1, opt.plan_callback_, goal->planning_options.look_around_attempts, - goal->planning_options.max_safe_execution_cost); + opt.plan_callback_ = + std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), + std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts, + goal->planning_options.max_safe_execution_cost); context_->plan_with_sensing_->setBeforeLookCallback( - boost::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this)); + std::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this)); } plan_execution::ExecutableMotionPlan plan; @@ -290,16 +293,17 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute( opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), &action_res, _1); + opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this); + opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), + &action_res, std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { - opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - _1, opt.plan_callback_, goal->planning_options.look_around_attempts, - goal->planning_options.max_safe_execution_cost); + opt.plan_callback_ = + std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), + std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts, + goal->planning_options.max_safe_execution_cost); context_->plan_with_sensing_->setBeforeLookCallback( - boost::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this)); + std::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this)); } plan_execution::ExecutableMotionPlan plan; diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 73f270b240..b4b3900194 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -2,7 +2,7 @@ moveit_ros_manipulation - 1.1.6 + 1.1.7 Components of MoveIt used for manipulation Michael Görner Henning Kayser diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index 7e32d25d42..d2954826ec 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -98,7 +98,7 @@ bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const moveit::cor double min_distance, unsigned int attempts) { // initialize with scene state - moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(reference_state)); + auto token_state = std::make_shared(reference_state); for (unsigned int j = 0; j < attempts; ++j) { double min_d = std::numeric_limits::infinity(); @@ -147,9 +147,9 @@ bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_d ok = ps->processAttachedCollisionObjectMsg(msg); } motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent( - (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)( - planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY + - planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE)); + (planning_scene_monitor::PlanningSceneMonitor:: + SceneUpdateType)(planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY + + planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE)); return ok; } @@ -164,7 +164,7 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, moveit::core::RobotStatePtr ee_closed_state( new moveit::core::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint())); - robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory( + auto ee_closed_traj = std::make_shared( ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_); // If user has defined a time for it's gripper movement time, don't add the @@ -185,7 +185,8 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, plan_execution::ExecutableTrajectory et(ee_closed_traj, name); // Add a callback to attach the object to the EE after closing the gripper - et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1); + et.effect_on_success_ = + std::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, std::placeholders::_1); et.allowed_collision_matrix_ = collision_matrix; plan->trajectories_.push_back(et); } @@ -229,8 +230,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping moveit::core::GroupStateValidityCallbackFn approach_valid_callback = - boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, - &plan->approach_posture_, _1, _2, _3); + std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, + &plan->approach_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); plan->goal_sampler_->setVerbose(verbose_); std::size_t attempted_possible_goal_states = 0; do // continuously sample possible goal states @@ -242,7 +243,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const if (plan->shared_data_->minimize_object_distance_) { static const double MAX_CLOSE_UP_DIST = 1.0; - moveit::core::RobotStatePtr close_up_state(new moveit::core::RobotState(*plan->possible_goal_states_[i])); + auto close_up_state = std::make_shared(*plan->possible_goal_states_[i]); std::vector close_up_states; double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath( close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, @@ -254,7 +255,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const } // try to compute a straight line path that arrives at the goal using the specified approach direction - moveit::core::RobotStatePtr first_approach_state(new moveit::core::RobotState(*plan->possible_goal_states_[i])); + auto first_approach_state = std::make_shared(*plan->possible_goal_states_[i]); std::vector approach_states; double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath( @@ -280,8 +281,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the // actual grasp moveit::core::GroupStateValidityCallbackFn retreat_valid_callback = - boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, - &plan->retreat_posture_, _1, _2, _3); + std::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, + &plan->retreat_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); // try to compute a straight line path that moves from the goal in a desired direction moveit::core::RobotStatePtr last_retreat_state( @@ -299,13 +300,13 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const { // Create approach trajectory std::reverse(approach_states.begin(), approach_states.end()); - robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory( + auto approach_traj = std::make_shared( planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName())); for (const moveit::core::RobotStatePtr& approach_state : approach_states) approach_traj->addSuffixWayPoint(approach_state, 0.0); // Create retreat trajectory - robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory( + auto retreat_traj = std::make_shared( planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName())); for (const moveit::core::RobotStatePtr& retreat_state : retreat_states) retreat_traj->addSuffixWayPoint(retreat_state, 0.0); @@ -338,7 +339,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // Create approach trajectory std::reverse(approach_states.begin(), approach_states.end()); - robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory( + auto approach_traj = std::make_shared( planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName())); for (const moveit::core::RobotStatePtr& approach_state : approach_states) approach_traj->addSuffixWayPoint(approach_state, 0.0); diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp index 60640fdccc..91db160592 100644 --- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp +++ b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp @@ -114,7 +114,7 @@ void ManipulationPipeline::start() stage->resetStopSignal(); for (std::size_t i = 0; i < processing_threads_.size(); ++i) if (!processing_threads_[i]) - processing_threads_[i] = new boost::thread(boost::bind(&ManipulationPipeline::processingThread, this, i)); + processing_threads_[i] = new boost::thread(std::bind(&ManipulationPipeline::processingThread, this, i)); } void ManipulationPipeline::signalStop() diff --git a/moveit_ros/manipulation/pick_place/src/pick.cpp b/moveit_ros/manipulation/pick_place/src/pick.cpp index 2d83fbff40..ec1067dc31 100644 --- a/moveit_ros/manipulation/pick_place/src/pick.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick.cpp @@ -123,7 +123,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, ros::WallTime start_time = ros::WallTime::now(); // construct common data for possible manipulation plans - ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData()); + auto plan_data = std::make_shared(); ManipulationPlanSharedDataConstPtr const_plan_data = plan_data; plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group); plan_data->end_effector_group_ = eef; @@ -162,8 +162,8 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, pipeline_.reset(); ManipulationStagePtr stage1( new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager())); - ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm)); - ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline())); + ManipulationStagePtr stage2 = std::make_shared(planning_scene, approach_grasp_acm); + ManipulationStagePtr stage3 = std::make_shared(planning_scene, pick_place_->getPlanningPipeline()); pipeline_.addStage(stage1).addStage(stage2).addStage(stage3); initialize(); @@ -180,7 +180,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, // feed the available grasps to the stages we set up for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i) { - ManipulationPlanPtr p(new ManipulationPlan(const_plan_data)); + auto p = std::make_shared(const_plan_data); const moveit_msgs::msg::Grasp& g = goal.possible_grasps[grasp_order[i]]; p->approach_ = g.pre_grasp_approach; p->retreat_ = g.post_grasp_retreat; diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp index 57fc48971f..d4aefc7f93 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp @@ -51,8 +51,8 @@ const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0; // sec PickPlacePlanBase::PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name) : pick_place_(pick_place), pipeline_(name, 4), last_plan_time_(0.0), done_(false) { - pipeline_.setSolutionCallback(boost::bind(&PickPlacePlanBase::foundSolution, this)); - pipeline_.setEmptyQueueCallback(boost::bind(&PickPlacePlanBase::emptyQueue, this)); + pipeline_.setSolutionCallback(std::bind(&PickPlacePlanBase::foundSolution, this)); + pipeline_.setEmptyQueueCallback(std::bind(&PickPlacePlanBase::emptyQueue, this)); } PickPlacePlanBase::~PickPlacePlanBase() = default; diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp index addc270d44..7102d23715 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp @@ -49,8 +49,8 @@ class DynamicReconfigureImpl public: DynamicReconfigureImpl() : dynamic_reconfigure_server_(ros::NodeHandle("~/pick_place")) { - dynamic_reconfigure_server_.setCallback( - boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); + dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, + std::placeholders::_1, std::placeholders::_2)); } const PickPlaceParams& getParams() const diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index 0852bf2aab..a383cb1ed5 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -272,7 +272,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene ros::WallTime start_time = ros::WallTime::now(); // construct common data for possible manipulation plans - ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData()); + auto plan_data = std::make_shared(); ManipulationPlanSharedDataConstPtr const_plan_data = plan_data; plan_data->planning_group_ = jmg; plan_data->end_effector_group_ = eef; @@ -315,8 +315,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene ManipulationStagePtr stage1( new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager())); - ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm)); - ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline())); + ManipulationStagePtr stage2 = std::make_shared(planning_scene, approach_place_acm); + ManipulationStagePtr stage3 = std::make_shared(planning_scene, pick_place_->getPlanningPipeline()); pipeline_.addStage(stage1).addStage(stage2).addStage(stage3); initialize(); @@ -334,7 +334,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // add possible place locations for (std::size_t i = 0; i < goal.place_locations.size(); ++i) { - ManipulationPlanPtr p(new ManipulationPlan(const_plan_data)); + auto p = std::make_shared(const_plan_data); const moveit_msgs::action::PlaceLocation& pl = goal.place_locations[place_locations_order[i]]; if (goal.place_eef) diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp index daa1f8e51f..27c639c436 100644 --- a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp @@ -78,8 +78,8 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const // We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable if (!plan->approach_posture_.joint_names.empty()) { - moveit::core::RobotStatePtr pre_approach_state(new moveit::core::RobotState(res.trajectory_->getLastWayPoint())); - robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory( + auto pre_approach_state = std::make_shared(res.trajectory_->getLastWayPoint())); + auto pre_approach_traj = std::make_shared( pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_); diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp index f1165849f7..39e9d3e488 100644 --- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp +++ b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp @@ -41,7 +41,7 @@ #else #include #endif -#include +#include #include pick_place::ReachableAndValidPoseFilter::ReachableAndValidPoseFilter( @@ -111,7 +111,7 @@ bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const Manipulati bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const { // initialize with scene state - moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(planning_scene_->getCurrentState())); + auto token_state = std::make_shared(planning_scene_->getCurrentState()); if (isEndEffectorFree(plan, *token_state)) { // update the goal pose message if anything has changed; this is because the name of the frame in the input goal @@ -136,8 +136,9 @@ bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_); if (plan->goal_sampler_) { - plan->goal_sampler_->setGroupStateValidityCallback(boost::bind( - &isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), _1, _2, _3)); + plan->goal_sampler_->setGroupStateValidityCallback( + std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); plan->goal_sampler_->setVerbose(verbose_); if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_)) { diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 1b071950cc..6e24559bfc 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -150,11 +150,11 @@ bool MoveGroupCartesianPathService::computeService(const std::shared_ptr(context_->planning_scene_monitor_); kset = std::make_unique((*ls)->getRobotModel()); kset->add(req->path_constraints, (*ls)->getTransforms()); - constraint_fn = boost::bind( + constraint_fn = std::bind( &isStateValid, req->avoid_collisions ? static_cast(*ls).get() : nullptr, - kset->empty() ? nullptr : kset.get(), boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3); + kset->empty() ? nullptr : kset.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); } bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req->header.frame_id); RCLCPP_INFO(LOGGER, diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp index 8e7561fff2..8956521ca7 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp @@ -61,7 +61,8 @@ void MoveGroupExecuteService::initialize() ros::AdvertiseServiceOptions ops; ops.template init( - EXECUTE_SERVICE_NAME, boost::bind(&MoveGroupExecuteService::executeTrajectoryService, this, _1, _2)); + EXECUTE_SERVICE_NAME, std::bind(&MoveGroupExecuteService::executeTrajectoryService, this, std::placeholders::_1, + std::placeholders::_2)); ops.callback_queue = &callback_queue_; execute_service_ = root_node_handle_.advertiseService(ops); spinner_.start(); diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 7d631447b8..7357893271 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -172,12 +172,12 @@ bool MoveGroupKinematicsService::computeIKService(const std::shared_ptrgetCurrentState(); kset.add(req->ik_request.constraints, ls->getTransforms()); computeIK(req->ik_request, res->solution, res->error_code, rs, - boost::bind(&isIKSolutionValid, - req->ik_request.avoid_collisions ? - static_cast(ls).get() : - nullptr, - kset.empty() ? nullptr : &kset, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3)); + std::bind(&isIKSolutionValid, + req->ik_request.avoid_collisions ? + static_cast(ls).get() : + nullptr, + kset.empty() ? nullptr : &kset, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); } else { diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 8ce7c61fe6..962e52aec5 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -58,9 +58,6 @@ MoveGroupMoveAction::MoveGroupMoveAction() void MoveGroupMoveAction::initialize() { // start the move action server - using std::placeholders::_1; - using std::placeholders::_2; - auto node = context_->moveit_cpp_->getNode(); execute_action_server_ = rclcpp_action::create_server( node, MOVE_ACTION, @@ -72,7 +69,7 @@ void MoveGroupMoveAction::initialize() RCLCPP_INFO(LOGGER, "Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; }, - std::bind(&MoveGroupMoveAction::executeMoveCallback, this, _1)); + std::bind(&MoveGroupMoveAction::executeMoveCallback, this, std::placeholders::_1)); } void MoveGroupMoveAction::executeMoveCallback(std::shared_ptr goal) @@ -147,17 +144,17 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt opt.replan_ = goal->get_goal()->planning_options.replan; opt.replan_attempts_ = goal->get_goal()->planning_options.replan_attempts; opt.replan_delay_ = goal->get_goal()->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this); + opt.before_execution_callback_ = std::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this); - opt.plan_callback_ = boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, - boost::cref(motion_plan_request), boost::placeholders::_1); + opt.plan_callback_ = std::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, + boost::cref(motion_plan_request), std::placeholders::_1); if (goal->get_goal()->planning_options.look_around && context_->plan_with_sensing_) { - opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - boost::placeholders::_1, opt.plan_callback_, - goal->get_goal()->planning_options.look_around_attempts, - goal->get_goal()->planning_options.max_safe_execution_cost); - context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this)); + opt.plan_callback_ = + std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), + std::placeholders::_1, opt.plan_callback_, goal->get_goal()->planning_options.look_around_attempts, + goal->get_goal()->planning_options.max_safe_execution_cost); + context_->plan_with_sensing_->setBeforeLookCallback(std::bind(&MoveGroupMoveAction::startMoveLookCallback, this)); } plan_execution::ExecutableMotionPlan plan; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp index 341aa796d8..6977a0d3e2 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py index 735608c89f..29143be647 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -69,16 +69,13 @@ def generate_launch_description(): ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[moveit_config.robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py index 640412e0ce..1e699cf2a6 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py @@ -70,16 +70,13 @@ def generate_launch_description(): ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers @@ -147,10 +144,7 @@ def generate_launch_description(): robot_description, robot_description_semantic, ], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) return LaunchDescription( diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index a4a819ecdc..493c208140 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -47,16 +47,13 @@ def generate_servo_test_description( ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[moveit_config.robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index 62c09b163c..ce9e48bf42 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -34,16 +34,13 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[moveit_config.robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index 3680e01d00..ad5073cd23 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -113,7 +113,7 @@ class ServoFixture : public ::testing::Test // Otherwise the Servo is still running when another test starts... if (!client_servo_stop_) { - client_servo_stop_->async_send_request(std::make_shared()); + stop(); } executor_->cancel(); if (executor_thread_.joinable()) diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index c74a7ab406..a644a26031 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -158,14 +158,17 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) // when we had one updater only, we passed directly the transform cache callback to that updater if (map_updaters_.size() == 2) { - map_updaters_[0]->setTransformCacheCallback( - boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3)); - map_updaters_[1]->setTransformCacheCallback( - boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3)); + map_updaters_[0]->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + map_updaters_[1]->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); } else - map_updaters_.back()->setTransformCacheCallback( - boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3)); + map_updaters_.back()->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, + map_updaters_.size() - 1, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); } else updater->setTransformCacheCallback(transform_cache_callback_); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index b57afceb17..63a739f1b2 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -81,7 +81,7 @@ int main(int argc, char** argv) std::shared_ptr buffer = std::make_shared(clock_ptr, tf2::durationFromSec(5.0)); std::shared_ptr listener = std::make_shared(*buffer, node); occupancy_map_monitor::OccupancyMapMonitor server(node, buffer); - server.setUpdateCallback(boost::bind(&publishOctomap, octree_binary_pub, &server)); + server.setUpdateCallback(std::bind(&publishOctomap, octree_binary_pub, &server)); server.startMonitor(); rclcpp::spin(node); diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index eec2b5e09e..c3b1b1f818 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -82,6 +82,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater rclcpp::Time last_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); std::string filtered_cloud_topic_; + std::string ns_; std::string sensor_type_; std::string image_topic_; std::size_t queue_size_; diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 49b4a0bf5e..73b9b0251e 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -97,7 +97,8 @@ bool DepthImageOctomapUpdater::setParams(const std::string& name_space) node_->get_parameter(name_space + ".max_update_rate", max_update_rate_) && node_->get_parameter(name_space + ".skip_vertical_pixels", skip_vertical_pixels_) && node_->get_parameter(name_space + ".skip_horizontal_pixels", skip_horizontal_pixels_) && - node_->get_parameter(name_space + ".filtered_cloud_topic", filtered_cloud_topic_); + node_->get_parameter(name_space + ".filtered_cloud_topic", filtered_cloud_topic_) && + node_->get_parameter(name_space + ".ns", ns_); return true; } catch (const rclcpp::exceptions::InvalidParameterTypeException& e) @@ -125,8 +126,8 @@ bool DepthImageOctomapUpdater::initialize(const rclcpp::Node::SharedPtr& node) mesh_filter_->setShadowThreshold(shadow_threshold_); mesh_filter_->setPaddingOffset(padding_offset_); mesh_filter_->setPaddingScale(padding_scale_); - mesh_filter_->setTransformCallback(boost::bind(&DepthImageOctomapUpdater::getShapeTransform, this, - boost::placeholders::_1, boost::placeholders::_2)); + mesh_filter_->setTransformCallback( + std::bind(&DepthImageOctomapUpdater::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2)); return true; } @@ -136,18 +137,22 @@ void DepthImageOctomapUpdater::start() rmw_qos_profile_t custom_qos = rmw_qos_profile_system_default; pub_model_depth_image_ = model_depth_transport_->advertiseCamera("model_depth", 1); + std::string prefix = ""; + if (!ns_.empty()) + prefix = ns_ + "/"; + + pub_model_depth_image_ = model_depth_transport_->advertiseCamera(prefix + "model_depth", 1); if (!filtered_cloud_topic_.empty()) - pub_filtered_depth_image_ = filtered_depth_transport_->advertiseCamera(filtered_cloud_topic_, 1); + pub_filtered_depth_image_ = filtered_depth_transport_->advertiseCamera(prefix + filtered_cloud_topic_, 1); else - pub_filtered_depth_image_ = filtered_depth_transport_->advertiseCamera("filtered_depth", 1); + pub_filtered_depth_image_ = filtered_depth_transport_->advertiseCamera(prefix + "filtered_depth", 1); - pub_filtered_label_image_ = filtered_label_transport_->advertiseCamera("filtered_label", 1); + pub_filtered_label_image_ = filtered_label_transport_->advertiseCamera(prefix + "filtered_label", 1); - sub_depth_image_ = - image_transport::create_camera_subscription(node_.get(), image_topic_, - boost::bind(&DepthImageOctomapUpdater::depthImageCallback, this, - boost::placeholders::_1, boost::placeholders::_2), - "raw", custom_qos); + sub_depth_image_ = image_transport::create_camera_subscription( + node_.get(), image_topic_, + std::bind(&DepthImageOctomapUpdater::depthImageCallback, this, std::placeholders::_1, std::placeholders::_2), + "raw", custom_qos); } void DepthImageOctomapUpdater::stop() diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index b0d6d863b0..75bdf0182a 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -49,8 +49,8 @@ LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTree , max_sensor_delta_(1e-3) // 1mm , process_occupied_cells_set_(nullptr) , process_model_cells_set_(nullptr) - , update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)) - , process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this)) + , update_thread_(std::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)) + , process_thread_(std::bind(&LazyFreeSpaceUpdater::processThread, this)) { } diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index acad8248c5..5bbe882391 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -91,7 +91,7 @@ void mesh_filter::DepthSelfFiltering::onInit() model_label_ptr_ = std::make_shared(); mesh_filter_ = std::make_shared>( - bind(&TransformProvider::getTransform, &transform_provider_, _1, _2), + bind(&TransformProvider::getTransform, &transform_provider_, std::placeholders::_1, std::placeholders::_2), mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS); mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_); mesh_filter_->setShadowThreshold(shadow_threshold_); diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index d29e7dde20..513e307524 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -171,7 +171,7 @@ mesh_filter::MeshHandle mesh_filter::MeshFilterBase::addMesh(const shapes::Mesh& { std::unique_lock _(meshes_mutex_); - JobPtr job(new FilterJob(std::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh))); + JobPtr job = std::make_shared>(std::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh)); addJob(job); job->wait(); mesh_filter::MeshHandle ret = next_handle_; @@ -217,16 +217,17 @@ void mesh_filter::MeshFilterBase::setShadowThreshold(float threshold) void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const { - JobPtr job(new FilterJob(std::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels))); + JobPtr job = std::make_shared>( + std::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels)); addJob(job); job->wait(); } void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const { - JobPtr job1(new FilterJob(std::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth))); - JobPtr job2(new FilterJob( - std::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth))); + JobPtr job1 = std::make_shared>(std::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth)); + JobPtr job2 = std::make_shared>( + std::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth)); { std::unique_lock lock(jobs_mutex_); jobs_queue_.push(job1); @@ -239,9 +240,9 @@ void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const { - JobPtr job1(new FilterJob(std::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth))); - JobPtr job2(new FilterJob( - std::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth))); + JobPtr job1 = std::make_shared>(std::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth)); + JobPtr job2 = std::make_shared>( + std::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth)); { std::unique_lock lock(jobs_mutex_); jobs_queue_.push(job1); @@ -254,7 +255,8 @@ void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const void mesh_filter::MeshFilterBase::getFilteredLabels(LabelType* labels) const { - JobPtr job(new FilterJob(std::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels))); + JobPtr job = std::make_shared>( + std::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels)); addJob(job); job->wait(); } @@ -294,7 +296,7 @@ void mesh_filter::MeshFilterBase::filter(const void* sensor_data, GLushort type, throw std::runtime_error(msg.str()); } - JobPtr job(new FilterJob(std::bind(&MeshFilterBase::doFilter, this, sensor_data, type))); + JobPtr job = std::make_shared>(std::bind(&MeshFilterBase::doFilter, this, sensor_data, type)); addJob(job); if (wait) job->wait(); diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index a727114db3..f3fc5d1ff5 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -118,7 +118,8 @@ MeshFilterTest::MeshFilterTest(unsigned width, unsigned height, double nea , shadow_(shadow) , epsilon_(epsilon) , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1) - , filter_(std::bind(&MeshFilterTest::transformCallback, this, _1, _2), sensor_parameters_) + , filter_(std::bind(&MeshFilterTest::transformCallback, this, std::placeholders::_1, std::placeholders::_2), + sensor_parameters_) , sensor_data_(width_ * height_) , distance_(0.0) { diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 7da1c66bee..b38fbf7530 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -90,6 +90,7 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater unsigned int point_subsample_; double max_update_rate_; std::string filtered_cloud_topic_; + std::string ns_; rclcpp::Publisher::SharedPtr filtered_cloud_publisher_; message_filters::Subscriber* point_cloud_subscriber_; diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index b1458915a2..ba2abf589b 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -79,7 +79,8 @@ bool PointCloudOctomapUpdater::setParams(const std::string& name_space) node_->get_parameter(name_space + ".padding_scale", scale_) && node_->get_parameter(name_space + ".point_subsample", point_subsample_) && node_->get_parameter(name_space + ".max_update_rate", max_update_rate_) && - node_->get_parameter(name_space + ".filtered_cloud_topic", filtered_cloud_topic_); + node_->get_parameter(name_space + ".filtered_cloud_topic", filtered_cloud_topic_) && + node_->get_parameter(name_space + ".ns", ns_); } bool PointCloudOctomapUpdater::initialize(const rclcpp::Node::SharedPtr& node) @@ -91,15 +92,21 @@ bool PointCloudOctomapUpdater::initialize(const rclcpp::Node::SharedPtr& node) tf_buffer_->setCreateTimerInterface(create_timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); shape_mask_ = std::make_unique(); - shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, - boost::placeholders::_1, boost::placeholders::_2)); + shape_mask_->setTransformCallback( + std::bind(&PointCloudOctomapUpdater::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2)); + return true; } void PointCloudOctomapUpdater::start() { + std::string prefix = ""; + if (!ns_.empty()) + prefix = ns_ + "/"; + if (!filtered_cloud_topic_.empty()) - filtered_cloud_publisher_ = node_->create_publisher(filtered_cloud_topic_, 10); + filtered_cloud_publisher_ = + node_->create_publisher(prefix + filtered_cloud_topic_, 10); if (point_cloud_subscriber_) return; @@ -110,14 +117,14 @@ void PointCloudOctomapUpdater::start() point_cloud_filter_ = new tf2_ros::MessageFilter( *point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, node_); point_cloud_filter_->registerCallback( - boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, boost::placeholders::_1)); + std::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, std::placeholders::_1)); RCLCPP_INFO(LOGGER, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str()); } else { point_cloud_subscriber_->registerCallback( - boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, boost::placeholders::_1)); + std::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, std::placeholders::_1)); RCLCPP_INFO(LOGGER, "Listening to '%s'", point_cloud_topic_.c_str()); } } diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index aa51d0cde9..95e55bd783 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -110,6 +110,7 @@ if(BUILD_TESTING) set(ament_cmake_cppcheck_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) # Run all lint tests in package.xml except those listed above ament_lint_auto_find_test_dependencies() diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp index 650805e979..84556b1280 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp +++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp @@ -91,7 +91,7 @@ class ConstraintSamplerManagerLoader::Helper ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader( const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) : constraint_sampler_manager_(csm ? csm : std::make_shared()) - , impl_(new Helper(node, constraint_sampler_manager_)) + , impl_(std::make_shared(node, constraint_sampler_manager_)) { } } // namespace constraint_sampler_manager_loader diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index dec2bdb2ed..9506db0ec0 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -271,7 +271,7 @@ void KinematicsPluginLoader::status() const moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() { if (loader_) - return boost::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), boost::placeholders::_1); + return std::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), std::placeholders::_1); rdf_loader::RDFLoader rml(node_, robot_description_); robot_description_ = rml.getRobotDescription(); @@ -422,7 +422,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const iksolver_to_tip_links); } - return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), - boost::placeholders::_1); + return std::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), + std::placeholders::_1); } } // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index bff2bed980..68db705ba2 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -167,8 +168,9 @@ class MoveItCpp /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. * If blocking is set to false, the execution is run in background and the function returns immediately. */ - bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, - bool blocking = true); + moveit_controller_manager::ExecutionStatus execute(const std::string& group_name, + const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking = true); private: // Core properties and instances diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 6686128e75..597b023c7a 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include namespace moveit_cpp { @@ -53,34 +53,7 @@ class PlanningComponent public: MOVEIT_STRUCT_FORWARD(PlanSolution); - class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes - { - public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int code) const - { - return val == code; - } - bool operator!=(const int code) const - { - return val != code; - } - }; + using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; /// The representation of a plan solution struct PlanSolution @@ -92,7 +65,7 @@ class PlanningComponent robot_trajectory::RobotTrajectoryPtr trajectory; /// Reason why the plan failed - MoveItErrorCode error_code; + moveit::core::MoveItErrorCode error_code; explicit operator bool() const { diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 7d9254dc22..7780373d8c 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -35,6 +35,8 @@ /* Author: Henning Kayser */ #include + +#include #include #if __has_include() #include @@ -165,7 +167,6 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) { for (const auto& group_name : group_names) { - groups_pipelines_map_[group_name] = {}; const auto& pipeline = pipeline_entry.second; for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations()) { @@ -218,30 +219,14 @@ const std::map& MoveItCpp:: std::set MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const { - std::set result_names; - if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) + if (group_name.empty() || groups_pipelines_map_.count(group_name) == 0) { RCLCPP_ERROR(LOGGER, "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", group_name.c_str()); - return result_names; // empty - } - for (const auto& pipeline_entry : planning_pipelines_) - { - const std::string& pipeline_name = pipeline_entry.first; - // If group_name is defined and valid, skip pipelines that don't belong to the planning group - if (!group_name.empty()) - { - const auto& group_pipelines = groups_pipelines_map_.at(group_name); - if (group_pipelines.find(pipeline_name) == group_pipelines.end()) - continue; - } - result_names.insert(pipeline_name); + return {}; // empty } - // No valid planning pipelines - if (result_names.empty()) - RCLCPP_ERROR(LOGGER, "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", - group_name.c_str()); - return result_names; + + return groups_pipelines_map_.at(group_name); } const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const @@ -264,20 +249,21 @@ trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajec return trajectory_execution_manager_; } -bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, - bool blocking) +moveit_controller_manager::ExecutionStatus +MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking) { if (!robot_trajectory) { RCLCPP_ERROR(LOGGER, "Robot trajectory is undefined"); - return false; + return moveit_controller_manager::ExecutionStatus::ABORTED; } // Check if there are controllers that can handle the execution if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) { RCLCPP_ERROR(LOGGER, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); - return false; + return moveit_controller_manager::ExecutionStatus::ABORTED; } // Execute trajectory @@ -290,7 +276,7 @@ bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::R return trajectory_execution_manager_->waitForExecution(); } trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg); - return true; + return moveit_controller_manager::ExecutionStatus::RUNNING; } const std::shared_ptr& MoveItCpp::getTFBuffer() const diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index d66bb15c6e..9c40dbc93a 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -119,7 +119,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (!joint_model_group_) { RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); + last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; return *last_plan_solution_; } @@ -127,11 +127,11 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); planning_scene_monitor->updateFrameTransforms(); - planning_scene_monitor->lockSceneRead(); // LOCK planning scene - planning_scene::PlanningScenePtr planning_scene = - planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); - planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene - planning_scene_monitor.reset(); // release this pointer + const planning_scene::PlanningScenePtr planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + planning_scene_monitor.reset(); // release this pointer // Init MotionPlanRequest ::planning_interface::MotionPlanRequest req; @@ -156,11 +156,12 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (current_goal_constraints_.empty()) { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return *last_plan_solution_; } req.goal_constraints = current_goal_constraints_; + // Set path constraints req.path_constraints = current_path_constraints_; // Run planning attempt @@ -168,7 +169,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) { RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE; return *last_plan_solution_; } const planning_pipeline::PlanningPipelinePtr pipeline = diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 0163f346f9..1fd8e0c98a 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -56,7 +56,8 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.plan_executi // : owner_(owner) //, dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution")) // { // // dynamic_reconfigure_server_.setCallback( -// // boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); +// // std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, std::placeholders::_1, +// std::placeholders::_2)); // } // // private: @@ -86,7 +87,7 @@ plan_execution::PlanExecution::PlanExecution( // we want to be notified when new information is available planning_scene_monitor_->addUpdateCallback( - boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, boost::placeholders::_1)); + std::bind(&PlanExecution::planningSceneUpdatedCallback, this, std::placeholders::_1)); // start the dynamic-reconfigure server // reconfigure_impl_ = new DynamicReconfigureImpl(this); @@ -414,8 +415,8 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni // start a trajectory execution thread trajectory_execution_manager_->execute( - boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, boost::placeholders::_1), - boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, boost::placeholders::_1)); + std::bind(&PlanExecution::doneWithTrajectoryExecution, this, std::placeholders::_1), + std::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, std::placeholders::_1)); // wait for path to be done, while checking that the path does not become invalid rclcpp::WallRate r(100); path_became_invalid_ = false; diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp index 6eb5ac99ee..7519ef7720 100644 --- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp @@ -56,7 +56,8 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.plan_with_se // : owner_(owner) /*, dynamic_reconfigure_server_(ros::NodeHandle("~/sense_for_plan"))*/ // { // // dynamic_reconfigure_server_.setCallback( -// // boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); +// // std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, std::placeholders::_1, +// std::placeholders::_2)); // } // // private: diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp index 7dfd0d5505..1cb46d46b0 100644 --- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -305,7 +305,7 @@ int main(int argc, char** argv) robot_model = moveit::core::loadTestingRobotModel("panda"); - planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); + auto scene = std::make_shared(robot_model); planning_scene_monitor::PlanningSceneMonitor psm(planning_scene, ROBOT_DESCRIPTION); psm.startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); psm.startSceneMonitor(); diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index 475074074c..4f92339f26 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -67,7 +67,7 @@ int main(int argc, char** argv) robot_model_loader::RobotModelLoader::Options opt; opt.robot_description_ = "robot_description"; - robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(node, opt)); + auto rml = std::make_shared(node, opt); planning_scene_monitor::PlanningSceneMonitor psm(node, rml); psm.startWorldGeometryMonitor(); psm.startSceneMonitor(); diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index 9c018c2581..e1ed8c855e 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv) runCollisionDetection(10, trials, psm.getPlanningScene().get(), states[0].get()); for (unsigned int i = 0; i < states.size(); ++i) threads.push_back(new boost::thread( - boost::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get()))); + std::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get()))); for (unsigned int i = 0; i < states.size(); ++i) { diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index 40ceac4d5e..d73033fc16 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -76,7 +76,8 @@ int main(int argc, char** argv) robot_model_loader::RobotModelLoader::Options opt; opt.robot_description_ = "robot_description"; opt.load_kinematics_solvers_ = false; - robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(node, opt)); + + auto rml = std::make_shared(node, opt); planning_scene::PlanningScene ps(rml->getModel()); std::ifstream f(argv[filename_index]); diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index f894c625e0..fe4bca6537 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -107,7 +107,7 @@ int main(int argc, char** argv) t.translation() = Eigen::Vector3d(rng.uniformReal(aabb[0], aabb[1]), rng.uniformReal(aabb[2], aabb[3]), rng.uniformReal(aabb[4], aabb[5])); scene->getWorldNonConst()->clearObjects(); - scene->getWorldNonConst()->addToObject("test", shapes::ShapeConstPtr(new shapes::Sphere(radius)), t); + scene->getWorldNonConst()->addToObject("test", std::make_shared(radius), t); collision_detection::CollisionResult res; scene->checkCollision(req, res); if (res.collision) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 6b970d9a73..a33a8f0f34 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -287,6 +287,11 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla RCLCPP_DEBUG(LOGGER, "Motion planner reported a solution path with %ld states", state_count); if (check_solution_paths_) { + visualization_msgs::msg::MarkerArray arr; + visualization_msgs::msg::Marker m; + m.action = visualization_msgs::msg::Marker::DELETEALL; + arr.markers.push_back(m); + std::vector index; if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index)) { @@ -327,7 +332,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla << contacts_publisher_->get_topic_name()); // call validity checks in verbose mode for the problematic states - visualization_msgs::msg::MarkerArray arr; for (std::size_t it : index) { // check validity with verbose on @@ -351,10 +355,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } } RCLCPP_ERROR(LOGGER, "Completed listing of explanations for invalid states."); - if (!arr.markers.empty()) - { - contacts_publisher_->publish(arr); - } } } else @@ -366,6 +366,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } else RCLCPP_DEBUG(LOGGER, "Planned path was found to be valid when rechecked"); + contacts_publisher_->publish(arr); } } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index 92e2abc57c..ad3b4242b3 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -96,7 +96,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA else RCLCPP_INFO(LOGGER, "Start state appears to be in collision with respect to group %s", creq.group_name.c_str()); - moveit::core::RobotStatePtr prefix_state(new moveit::core::RobotState(start_state)); + auto prefix_state = std::make_shared(start_state); random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator(); const std::vector& jmodels = diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index 46a837d9df..8a67220ef0 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -404,6 +404,17 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: */ bool waitForCurrentRobotState(const rclcpp::Time& t, double wait_time = 1.); + void clearOctomap(); + + // Called to update the planning scene with a new message. + bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene); + +protected: + /** @brief Initialize the planning scene monitor + * @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated) + */ + void initialize(const planning_scene::PlanningScenePtr& scene); + /** \brief Lock the scene for reading (multiple threads can lock for reading at the same time) */ void lockSceneRead(); @@ -418,17 +429,6 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: */ void unlockSceneWrite(); - void clearOctomap(); - - // Called to update the planning scene with a new message. - bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene); - -protected: - /** @brief Initialize the planning scene monitor - * @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated) - */ - void initialize(const planning_scene::PlanningScenePtr& scene); - /** @brief Configure the collision matrix for a particular scene */ void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene); @@ -606,6 +606,9 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_; bool use_sim_time_; + + friend class LockedPlanningSceneRO; + friend class LockedPlanningSceneRW; }; /** \brief This is a convenience class for obtaining access to an @@ -648,7 +651,7 @@ class LockedPlanningSceneRO return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene(); } - operator const planning_scene::PlanningSceneConstPtr &() const + operator const planning_scene::PlanningSceneConstPtr&() const { return static_cast(planning_scene_monitor_.get())->getPlanningScene(); } @@ -729,7 +732,7 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO { } - operator const planning_scene::PlanningScenePtr &() + operator const planning_scene::PlanningScenePtr&() { return planning_scene_monitor_->getPlanningScene(); } diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp index 3f44e08703..ff72d429fd 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp @@ -88,15 +88,15 @@ bool CurrentStateMonitorMiddlewareHandle::sleepFor(const std::chrono::nanosecond return rclcpp::sleep_for(nanoseconds); } -void CurrentStateMonitorMiddlewareHandle::createStaticTfSubscription(TfCallback callback) +void CurrentStateMonitorMiddlewareHandle::createDynamicTfSubscription(TfCallback callback) { - static_transform_subscriber_ = + transform_subscriber_ = node_->create_subscription("/tf", tf2_ros::DynamicListenerQoS(), callback); } -void CurrentStateMonitorMiddlewareHandle::createDynamicTfSubscription(TfCallback callback) +void CurrentStateMonitorMiddlewareHandle::createStaticTfSubscription(TfCallback callback) { - transform_subscriber_ = + static_transform_subscriber_ = node_->create_subscription("/tf_static", tf2_ros::StaticListenerQoS(), callback); } diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index dd7f9c02f4..e43a00c9a3 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -191,10 +191,10 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc { // The scene_ is loaded on the collision loader only if it was correctly instantiated collision_loader_.setupScene(node_, scene_); - scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, + std::placeholders::_1, std::placeholders::_2)); } } else @@ -329,10 +329,10 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) parent_scene_ = scene_; scene_ = parent_scene_->diff(); scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); } } else @@ -385,7 +385,7 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t RCLCPP_INFO(LOGGER, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str()); monitorDiffs(true); publish_planning_scene_ = - std::make_unique(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)); + std::make_unique(std::bind(&PlanningSceneMonitor::scenePublishingThread, this)); } } @@ -443,11 +443,10 @@ void PlanningSceneMonitor::scenePublishingThread() scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); scene_->pushDiffs(parent_scene_); scene_->clearDiffs(); - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, - boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in @@ -688,10 +687,10 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann parent_scene_ = scene_; scene_ = parent_scene_->diff(); scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, - this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, + std::placeholders::_1, std::placeholders::_2)); } if (octomap_monitor_) { @@ -1197,10 +1196,10 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio excludeAttachedBodiesFromOctree(); excludeWorldObjectsFromOctree(); - octomap_monitor_->setTransformCacheCallback(boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, - boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3)); - octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this)); + octomap_monitor_->setTransformCacheCallback(std::bind(&PlanningSceneMonitor::getShapeTransformCache, this, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + octomap_monitor_->setUpdateCallback(std::bind(&PlanningSceneMonitor::octomapUpdateCallback, this)); } octomap_monitor_->startMonitor(); } @@ -1234,7 +1233,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top std::make_shared(pnode_, getRobotModel(), tf_buffer_, use_sim_time_); } current_state_monitor_->addUpdateCallback( - boost::bind(&PlanningSceneMonitor::onStateUpdate, this, boost::placeholders::_1)); + std::bind(&PlanningSceneMonitor::onStateUpdate, this, std::placeholders::_1)); current_state_monitor_->startStateMonitor(joint_states_topic); { diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index 5741e9e654..8d51528e5a 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -86,7 +86,7 @@ void planning_scene_monitor::TrajectoryMonitor::startTrajectoryMonitor() { if (sampling_frequency_ > std::numeric_limits::epsilon() && !record_states_thread_) { - record_states_thread_ = std::make_unique(boost::bind(&TrajectoryMonitor::recordStates, this)); + record_states_thread_ = std::make_unique(std::bind(&TrajectoryMonitor::recordStates, this)); RCLCPP_DEBUG(LOGGER, "Started trajectory monitor"); } } diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index 654687c083..93e0529909 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -151,6 +151,7 @@ bool RDFLoader::loadFileToString(std::string& buffer, const std::string& path) bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& path, const std::vector& xacro_args) { + buffer.clear(); if (path.empty()) { RCLCPP_ERROR(LOGGER, "Path is empty"); @@ -163,7 +164,7 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa return false; } - std::string cmd = "ros2 run xacro xacro"; + std::string cmd = "ros2 run xacro xacro "; for (const std::string& xacro_arg : xacro_args) cmd += xacro_arg + " "; cmd += path; diff --git a/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro b/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro new file mode 100644 index 0000000000..6ebcdbc339 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp index b1f9ba4bc5..9d813402b5 100644 --- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp +++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp @@ -74,11 +74,9 @@ TEST(RDFIntegration, executor) rclcpp::Node::SharedPtr node = std::make_shared("executor"); // Create a thread to spin an Executor. - std::thread([node]() { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - }).detach(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread spinning_thread([&executor] { executor.spin(); }); // WHEN the RDFLoader is created rdf_loader::RDFLoader loader(node, "topic_description"); @@ -88,6 +86,17 @@ TEST(RDFIntegration, executor) EXPECT_EQ("gonzo", loader.getURDF()->name_); ASSERT_NE(nullptr, loader.getSRDF()); EXPECT_EQ("gonzo", loader.getSRDF()->getName()); + executor.cancel(); + spinning_thread.join(); +} + +TEST(RDFIntegration, xacro_test) +{ + std::string output; + std::vector xacro_args; + ASSERT_TRUE(rdf_loader::RDFLoader::loadPkgFileToString(output, "moveit_ros_planning", + "rdf_loader/test/data/robin.srdf.xacro", xacro_args)); + EXPECT_GT(output.size(), 0u); } int main(int argc, char** argv) diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 07f28fc216..6443a17839 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -400,7 +400,7 @@ bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::msg::RobotTra continuous_execution_queue_.push_back(context); if (!continuous_execution_thread_) continuous_execution_thread_ = - std::make_unique(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)); + std::make_unique(std::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)); } last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; continuous_execution_condition_.notify_all(); @@ -569,6 +569,18 @@ void TrajectoryExecutionManager::reloadControllerInformation() known_controllers_[ci.name_] = ci; } + names.clear(); + controller_manager_->getActiveControllers(names); + for (const auto& active_name : names) + { + auto found_it = std::find_if(known_controllers_.begin(), known_controllers_.end(), + [&](const auto& known_controller) { return known_controller.first == active_name; }); + if (found_it != known_controllers_.end()) + { + found_it->second.state_.active_ = true; + } + } + for (std::map::iterator it = known_controllers_.begin(); it != known_controllers_.end(); ++it) for (std::map::iterator jt = known_controllers_.begin(); @@ -1577,6 +1589,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) } else { + RCLCPP_ERROR(LOGGER, "Active status of required controllers can not be assured."); last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED; return false; } @@ -1804,8 +1817,12 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector originally_active; for (std::map::const_iterator it = known_controllers_.begin(); it != known_controllers_.end(); ++it) + { if (it->second.state_.active_) + { originally_active.insert(it->first); + } + } return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end()); } } diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp index 1d4a48c173..a7c8e7e50c 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp @@ -45,7 +45,7 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("test_trajectory_execution_manager"); - robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(node)); + auto rml = std::make_shared(node); planning_scene_monitor::PlanningSceneMonitor psm(node, rml); trajectory_execution_manager::TrajectoryExecutionManager tem(node, rml->getModel(), psm.getStateMonitor(), true); diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h index 7ff17e7802..a157678483 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h @@ -115,7 +115,7 @@ class TestMoveItControllerManager : public moveit_controller_manager::MoveItCont moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override { - return moveit_controller_manager::MoveItControllerHandlePtr(new TestMoveItControllerHandle(name)); + return std::make_shared(name); } void getControllersList(std::vector& names) override diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index da84587ae8..7d28410b0b 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -67,34 +68,7 @@ namespace moveit /** \brief Simple interface to MoveIt components */ namespace planning_interface { -class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes -{ -public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int c) const - { - return val == c; - } - bool operator!=(const int c) const - { - return val != c; - } -}; +using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc @@ -726,7 +700,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is not blocking (does not wait for the execution of the trajectory to complete). */ - MoveItErrorCode asyncMove(); + moveit::core::MoveItErrorCode asyncMove(); /** \brief Get the move_group action client used by the \e MoveGroupInterface. The client can be used for querying the execution state of the trajectory and abort trajectory execution @@ -738,24 +712,24 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface target. This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous spinner to be started.*/ - MoveItErrorCode move(); + moveit::core::MoveItErrorCode move(); /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the specified target. No execution is performed. The resulting plan is stored in \e plan*/ - MoveItErrorCode plan(Plan& plan); + moveit::core::MoveItErrorCode plan(Plan& plan); /** \brief Given a \e plan, execute it without waiting for completion. */ - MoveItErrorCode asyncExecute(const Plan& plan); + moveit::core::MoveItErrorCode asyncExecute(const Plan& plan); /** \brief Given a \e robot trajectory, execute it without waiting for completion. */ - MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory); + moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory); /** \brief Given a \e plan, execute it while waiting for completion. */ - MoveItErrorCode execute(const Plan& plan); + moveit::core::MoveItErrorCode execute(const Plan& plan); /** \brief Given a \e robot trajectory, execute it while waiting for completion. */ - MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory); + moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -835,20 +809,20 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Pick up an object This applies a number of hard-coded default grasps */ - // MoveItErrorCode pick(const std::string& object, bool plan_only = false) + // moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false) // { // return pick(constructPickupGoal(object, std::vector(), plan_only)); // } // /** \brief Pick up an object given a grasp pose */ - // MoveItErrorCode pick(const std::string& object, const moveit_msgs::msg::Grasp& grasp, bool plan_only = false) + // moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::msg::Grasp& grasp, bool plan_only = false) // { // return pick(constructPickupGoal(object, { grasp }, plan_only)); // } // /** \brief Pick up an object given possible grasp poses */ - // MoveItErrorCode pick(const std::string& object, std::vector grasps, bool plan_only = - // false) + // moveit::core::MoveItErrorCode pick(const std::string& object, std::vector grasps, bool + // plan_only = false) // { // return pick(constructPickupGoal(object, std::move(grasps), plan_only)); // } @@ -858,40 +832,40 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other desired variable in the goal, and finally pass it on to this function */ - // MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal); + // moveit::core::MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal); /** \brief Pick up an object calls the external moveit_msgs::srv::GraspPlanning service "plan_grasps" to compute possible grasps */ - // MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false); + // moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false); /** \brief Pick up an object calls the external moveit_msgs::srv::GraspPlanning service "plan_grasps" to compute possible grasps */ - // MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false); + // moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false); /** \brief Place an object somewhere safe in the world (a safe location will be detected) */ - // MoveItErrorCode place(const std::string& object, bool plan_only = false) + // moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false) // { // return place(constructPlaceGoal(object, std::vector(), plan_only)); // } /** \brief Place an object at one of the specified possible locations */ - // MoveItErrorCode place(const std::string& object, std::vector locations, + // moveit::core::MoveItErrorCode place(const std::string& object, std::vector locations, // bool plan_only = false) // { // return place(constructPlaceGoal(object, std::move(locations), plan_only)); // } /** \brief Place an object at one of the specified possible locations */ - // MoveItErrorCode place(const std::string& object, const std::vector& poses, + // moveit::core::MoveItErrorCode place(const std::string& object, const std::vector& poses, // bool plan_only = false) // { // return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only)); // } /** \brief Place an object at one of the specified possible location */ - // MoveItErrorCode place(const std::string& object, const geometry_msgs::msg::PoseStamped& pose, bool plan_only = - // false) + // moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::msg::PoseStamped& pose, bool + // plan_only = false) // { // return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only)); // } @@ -901,7 +875,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other desired variable in the goal, and finally pass it on to this function */ - // MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal); + // moveit::core::MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal); /** \brief Given the name of an object in the planning scene, make the object attached to a link of the robot. If no link name is diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 67b696d01f..0e2d80e09b 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -644,12 +644,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // return locations; // } - // MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal) + // moveit::core::MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal) // { // if (!place_action_client_ || !place_action_client_->action_server_is_ready()) // { // RCLCPP_ERROR_STREAM(LOGGER, "Place action client not found/not ready"); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } // // int64_t timeout = 3.0; @@ -658,17 +658,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // rclcpp::FutureReturnCode::SUCCESS) // { // RCLCPP_ERROR_STREAM(LOGGER, "Place action timeout reached"); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // return moveit::core::MoveItErrorCode::SUCCESS; // } - // MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal) + // moveit::core::MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal) // { // if (!pick_action_client_ || !pick_action_client_->action_server_is_ready()) // { // RCLCPP_ERROR_STREAM(LOGGER, "Pick action client not found/not ready"); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } // // int64_t timeout = 3.0; @@ -677,12 +677,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // rclcpp::FutureReturnCode::SUCCESS) // { // RCLCPP_ERROR_STREAM(LOGGER, "Pick action timeout reached"); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // return moveit::core::MoveItErrorCode::SUCCESS; // } - // MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false) + // moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false) // { // if (object.empty()) // { @@ -697,13 +697,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // { // RCLCPP_ERROR_STREAM(LOGGER, "Asked for grasps for the object '" // << object << "', but the object could not be found"); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::INVALID_OBJECT_NAME); + // return moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME; // } // // return planGraspsAndPick(objects[object], plan_only); // } - // MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false) + // moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false) // { // if (!plan_grasps_service_) // { @@ -711,7 +711,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // << GRASP_PLANNING_SERVICE_NAME // << "' is not available." // " This has to be implemented and started separately."); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } // // auto request = std::make_shared(); @@ -728,23 +728,23 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // rclcpp::FutureReturnCode::SUCCESS) // { // RCLCPP_ERROR(LOGGER, "Grasp planning failed. Unable to pick."); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } // response = res.get(); // if (response->error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) // { // RCLCPP_ERROR(LOGGER, "Grasp planning failed. Unable to pick."); - // return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + // return moveit::core::MoveItErrorCode::FAILURE; // } // return pick(constructPickupGoal(object.id, std::move(response->grasps), plan_only)); // } - MoveItErrorCode plan(Plan& plan) + moveit::core::MoveItErrorCode plan(Plan& plan) { if (!move_action_client_ || !move_action_client_->action_server_is_ready()) { RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server not ready"); - return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server ready"); @@ -818,12 +818,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return res->error_code; } - MoveItErrorCode move(bool wait) + moveit::core::MoveItErrorCode move(bool wait) { if (!move_action_client_ || !move_action_client_->action_server_is_ready()) { RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server not ready"); - return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } moveit_msgs::action::MoveGroup::Goal goal; @@ -875,7 +875,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl }; auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts); if (!wait) - return MoveItErrorCode::SUCCESS; + return moveit::core::MoveItErrorCode::SUCCESS; // wait until send_goal_opts.result_callback is called while (!done) @@ -892,12 +892,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return res->error_code; } - MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait) + moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait) { if (!execute_action_client_ || !execute_action_client_->action_server_is_ready()) { RCLCPP_INFO_STREAM(LOGGER, "execute_action_client_ client/server not ready"); - return MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } bool done = false; @@ -945,7 +945,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts); if (!wait) - return MoveItErrorCode::SUCCESS; + return moveit::core::MoveItErrorCode::SUCCESS; // wait until send_goal_opts.result_callback is called while (!done) @@ -1305,7 +1305,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (constraints_init_thread_) constraints_init_thread_->join(); constraints_init_thread_ = std::make_unique( - boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)); + std::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)); } void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) @@ -1529,7 +1529,7 @@ void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } -MoveItErrorCode MoveGroupInterface::asyncMove() +moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove() { return impl_->move(false); } @@ -1539,32 +1539,32 @@ rclcpp_action::Client& MoveGroupInterface::getMo return impl_->getMoveGroupClient(); } -MoveItErrorCode MoveGroupInterface::move() +moveit::core::MoveItErrorCode MoveGroupInterface::move() { return impl_->move(true); } -MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) { return impl_->execute(plan.trajectory_, false); } -MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory) +moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory) { return impl_->execute(trajectory, false); } -MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) { return impl_->execute(plan.trajectory_, true); } -MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory) +moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory) { return impl_->execute(trajectory, true); } -MoveItErrorCode MoveGroupInterface::plan(Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) { return impl_->plan(plan); } @@ -1588,23 +1588,23 @@ MoveItErrorCode MoveGroupInterface::plan(Plan& plan) // return impl_->posesToPlaceLocations(poses); //} // -// MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::action::Pickup::Goal& goal) +// moveit::core::MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::action::Pickup::Goal& goal) //{ // return impl_->pick(goal); //} // -// MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) +// moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) //{ // return impl_->planGraspsAndPick(object, plan_only); //} // -// MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool -// plan_only) +// moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, +// bool plan_only) //{ // return impl_->planGraspsAndPick(object, plan_only); //} // -// MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::action::Place::Goal& goal) +// moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::action::Place::Goal& goal) //{ // return impl_->place(goal); //} @@ -1647,7 +1647,10 @@ void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start if (start_state.is_diff) impl_->getCurrentState(rs); else + { rs = std::make_shared(getRobotModel()); + rs->setToDefaultValues(); // initialize robot state values for conversion + } moveit::core::robotStateMsgToRobotState(start_state, *rs); setStartState(*rs); } diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 3022776fb6..452edfea60 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -271,7 +271,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer msg.header.frame_id = getPoseReferenceFrame(); msg.header.stamp = ros::Time::now(); GILReleaser gr; - return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, msg, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false) @@ -281,7 +281,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); GILReleaser gr; - return place(object_name, poses, plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, poses, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, @@ -290,7 +290,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector locations(1); py_bindings_tools::deserializeMsg(location_str, locations[0]); GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false) @@ -300,13 +300,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placeAnywhere(const std::string& object_name, bool plan_only = false) { GILReleaser gr; - return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } void convertListToArrayOfPoses(const bp::list& poses, std::vector& msg) @@ -449,12 +449,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bool movePython() { GILReleaser gr; - return move() == MoveItErrorCode::SUCCESS; + return move() == moveit::core::MoveItErrorCode::SUCCESS; } bool asyncMovePython() { - return asyncMove() == MoveItErrorCode::SUCCESS; + return asyncMove() == moveit::core::MoveItErrorCode::SUCCESS; } bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links) @@ -467,14 +467,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); GILReleaser gr; - return execute(plan) == MoveItErrorCode::SUCCESS; + return execute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str) { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - return asyncExecute(plan) == MoveItErrorCode::SUCCESS; + return asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } bp::tuple planPython() @@ -676,7 +676,7 @@ static void wrap_move_group_interface() move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); - moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = + moveit::core::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = &MoveGroupInterfaceWrapper::pick; move_group_interface_class.def("pick", pick_1); move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py index 2c08c686c2..b7fbf86cda 100644 --- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -14,7 +14,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .trajectory_execution(file_path="config/panda_gripper_controllers.yaml") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") .to_moveit_configs() ) @@ -55,16 +55,13 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[moveit_config.robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="screen", ) # Load controllers diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 4fe1dd5f88..ab276641b9 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -134,8 +134,8 @@ class MoveGroupTestFixture : public ::testing::Test { SCOPED_TRACE("planAndMove"); moveit::planning_interface::MoveGroupInterface::Plan my_plan; - ASSERT_EQ(move_group_->plan(my_plan), moveit::planning_interface::MoveItErrorCode::SUCCESS); - ASSERT_EQ(move_group_->move(), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->plan(my_plan), moveit::core::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->move(), moveit::core::MoveItErrorCode::SUCCESS); } void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual) @@ -326,7 +326,7 @@ TEST_F(MoveGroupTestFixture, CartPathTest) ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0); // Execute trajectory - EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS); + EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS); // get the pose after the movement testPose(target_waypoint); diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test index 9975f257d2..02d1f7201c 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test @@ -1,27 +1,5 @@ - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - + setSupportSurfaceName("table1"); // Call pick to pick up the object using the grasps given - ASSERT_EQ(move_group_->pick("object", grasps), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->pick("object", grasps), moveit::core::MoveItErrorCode::SUCCESS); // Ideally, you would create a vector of place locations to be attempted although in this example, we only create // a single place location. @@ -278,7 +278,7 @@ TEST_F(PickPlaceTestFixture, PickPlaceTest) // Set support surface as table2. move_group_->setSupportSurfaceName("table2"); // Call place to place the object using the place locations given. - ASSERT_EQ(move_group_->place("object", place_location), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->place("object", place_location), moveit::core::MoveItErrorCode::SUCCESS); } int main(int argc, char** argv) diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.test b/moveit_ros/planning_interface/test/move_group_pick_place_test.test index 9dd7dbdcc3..bf20f60b5d 100644 --- a/moveit_ros/planning_interface/test/move_group_pick_place_test.test +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.test @@ -1,27 +1,5 @@ - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - + - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - + process_feedback(*state, *feedback); bool error_state_changed = setErrorState(g->marker_name_suffix, !ok); if (update_callback_) - *callback = boost::bind(update_callback_, boost::placeholders::_1, error_state_changed); + *callback = std::bind(update_callback_, std::placeholders::_1, error_state_changed); } // MUST hold state_lock_ when calling this! @@ -312,7 +312,7 @@ void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state, bool ok = kinematic_options.setStateFromIK(*state, eef->parent_group, eef->parent_link, *pose); bool error_state_changed = setErrorState(eef->parent_group, !ok); if (update_callback_) - *callback = boost::bind(update_callback_, boost::placeholders::_1, error_state_changed); + *callback = std::bind(update_callback_, std::placeholders::_1, error_state_changed); } // MUST hold state_lock_ when calling this! @@ -330,7 +330,7 @@ void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const state->update(); if (update_callback_) - *callback = boost::bind(update_callback_, boost::placeholders::_1, false); + *callback = std::bind(update_callback_, std::placeholders::_1, false); } bool InteractionHandler::inError(const EndEffectorInteraction& eef) const diff --git a/moveit_ros/robot_interaction/src/locked_robot_state.cpp b/moveit_ros/robot_interaction/src/locked_robot_state.cpp index 27ed768cb8..262711e45c 100644 --- a/moveit_ros/robot_interaction/src/locked_robot_state.cpp +++ b/moveit_ros/robot_interaction/src/locked_robot_state.cpp @@ -39,13 +39,13 @@ #include robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotState& state) - : state_(new moveit::core::RobotState(state)) + : state_(std::make_shared(state)) { state_->update(); } robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotModelPtr& model) - : state_(new moveit::core::RobotState(model)) + : state_(std::make_shared(model)) { state_->setToDefaultValues(); state_->update(); diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 06d03503e2..a610bc7f67 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -73,7 +73,7 @@ const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interactio RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, const std::string& ns) - : robot_model_(robot_model), kinematic_options_map_(new KinematicOptionsMap) + : robot_model_(robot_model), kinematic_options_map_(std::make_shared()) { topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC; node_ = node; @@ -81,7 +81,7 @@ RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot // spin a thread that will process feedback events run_processing_thread_ = true; - processing_thread_ = std::make_unique(boost::bind(&RobotInteraction::processingThread, this)); + processing_thread_ = std::make_unique(std::bind(&RobotInteraction::processingThread, this)); } RobotInteraction::~RobotInteraction() @@ -291,56 +291,53 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera const std::pair& smap = jmg->getGroupKinematics(); - // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group - if (smap.first) - { + auto add_active_end_effectors_for_single_group = [&](const moveit::core::JointModelGroup* single_group) { + bool found_eef{ false }; for (const srdf::Model::EndEffector& eef : eefs) - if ((jmg->hasLinkModel(eef.parent_link_) || jmg->getName() == eef.parent_group_) && - jmg->canSetStateFromIK(eef.parent_link_)) + if ((single_group->hasLinkModel(eef.parent_link_) || single_group->getName() == eef.parent_group_) && + single_group->canSetStateFromIK(eef.parent_link_)) { // We found an end-effector whose parent is the group. EndEffectorInteraction ee; - ee.parent_group = group; + ee.parent_group = single_group->getName(); ee.parent_link = eef.parent_link_; ee.eef_group = eef.component_group_; ee.interaction = style; active_eef_.push_back(ee); + found_eef = true; } - // No end effectors found. Use last link in group as the "end effector". - if (active_eef_.empty() && !jmg->getLinkModelNames().empty()) + // No end effectors found. Use last link in group as the "end effector". + if (!found_eef && !single_group->getLinkModelNames().empty()) { - EndEffectorInteraction ee; - ee.parent_group = group; - ee.parent_link = jmg->getLinkModelNames().back(); - ee.eef_group = group; - ee.interaction = style; - active_eef_.push_back(ee); + std::string last_link{ single_group->getLinkModelNames().back() }; + + if (single_group->canSetStateFromIK(last_link)) + { + EndEffectorInteraction ee; + ee.parent_group = single_group->getName(); + ee.parent_link = last_link; + ee.eef_group = single_group->getName(); + ee.interaction = style; + active_eef_.push_back(ee); + } } + }; + + // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group + if (smap.first) + { + add_active_end_effectors_for_single_group(jmg); } + // if the group contains subgroups with IK, add markers for them individually else if (!smap.second.empty()) { for (const std::pair& it : smap.second) - { - for (const srdf::Model::EndEffector& eef : eefs) - { - if ((it.first->hasLinkModel(eef.parent_link_) || jmg->getName() == eef.parent_group_) && - it.first->canSetStateFromIK(eef.parent_link_)) - { - // We found an end-effector whose parent is a subgroup of the group. (May be more than one) - EndEffectorInteraction ee; - ee.parent_group = it.first->getName(); - ee.parent_link = eef.parent_link_; - ee.eef_group = eef.component_group_; - ee.interaction = style; - active_eef_.push_back(ee); - break; - } - } - } + add_active_end_effectors_for_single_group(it.first); } + // lastly determine automatic marker sizes for (EndEffectorInteraction& eef : active_eef_) { // if we have a separate group for the eef, we compute the scale based on it; @@ -551,8 +548,8 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle for (const visualization_msgs::msg::InteractiveMarker& im : ims) { int_marker_server_->insert(im); - int_marker_server_->setCallback(im.name, boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, - boost::placeholders::_1)); + int_marker_server_->setCallback(im.name, std::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, + std::placeholders::_1)); // Add menu handler to all markers that this interaction handler creates. if (std::shared_ptr mh = handler->getMenuHandler()) diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index 890848db7c..bf35644cb5 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -222,7 +222,7 @@ static moveit::core::RobotModelPtr getModel() if (!model) { urdf::ModelInterfaceSharedPtr urdf(urdf::parseURDF(URDF_STR)); - srdf::ModelSharedPtr srdf(new srdf::Model()); + auto srdf = std::make_shared(); srdf->initString(*urdf, SRDF_STR); model = std::make_shared(urdf, srdf); } @@ -240,7 +240,7 @@ TEST(LockedRobotState, load) state2.setToDefaultValues(); robot_interaction::LockedRobotState ls2(state2); - robot_interaction::LockedRobotStatePtr ls4(new robot_interaction::LockedRobotState(model)); + auto ls4 = std::make_shared(model); } // sanity test the URDF and enum @@ -433,7 +433,7 @@ void MyInfo::modifyThreadFunc(robot_interaction::LockedRobotState* locked_state, { val += 0.0001; - locked_state->modifyState(boost::bind(&MyInfo::modifyFunc, this, boost::placeholders::_1, val)); + locked_state->modifyState(std::bind(&MyInfo::modifyFunc, this, std::placeholders::_1, val)); } cnt_lock_.lock(); @@ -551,62 +551,70 @@ TEST(LockedRobotState, set1) runThreads(1, 1, 0); } -TEST(LockedRobotState, set2) +// skip all more complex locking checks when optimizations are disabled +// they can easily outrun 20 minutes with Debug builds +#ifdef NDEBUG +#define OPT_TEST(F, N) TEST(F, N) +#else +#define OPT_TEST(F, N) TEST(F, DISABLED_##N) +#endif + +OPT_TEST(LockedRobotState, set2) { runThreads(1, 2, 0); } -TEST(LockedRobotState, set3) +OPT_TEST(LockedRobotState, set3) { runThreads(1, 3, 0); } -TEST(LockedRobotState, mod1) +OPT_TEST(LockedRobotState, mod1) { runThreads(1, 0, 1); } -TEST(LockedRobotState, mod2) +OPT_TEST(LockedRobotState, mod2) { runThreads(1, 0, 1); } -TEST(LockedRobotState, mod3) +OPT_TEST(LockedRobotState, mod3) { runThreads(1, 0, 1); } -TEST(LockedRobotState, set1mod1) +OPT_TEST(LockedRobotState, set1mod1) { runThreads(1, 1, 1); } -TEST(LockedRobotState, set2mod1) +OPT_TEST(LockedRobotState, set2mod1) { runThreads(1, 2, 1); } -TEST(LockedRobotState, set1mod2) +OPT_TEST(LockedRobotState, set1mod2) { runThreads(1, 1, 2); } -TEST(LockedRobotState, set3mod1) +OPT_TEST(LockedRobotState, set3mod1) { runThreads(1, 3, 1); } -TEST(LockedRobotState, set1mod3) +OPT_TEST(LockedRobotState, set1mod3) { runThreads(1, 1, 3); } -TEST(LockedRobotState, set3mod3) +OPT_TEST(LockedRobotState, set3mod3) { runThreads(1, 3, 3); } -TEST(LockedRobotState, set3mod3c3) +OPT_TEST(LockedRobotState, set3mod3c3) { runThreads(3, 3, 3); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 201b58e780..9d675df359 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -209,6 +209,7 @@ private Q_SLOTS: OUTSIDE_BOUNDS_LINK }; + void clearRobotModel() override; void onRobotModelLoaded() override; void onNewPlanningSceneState() override; void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 16027a3aed..12dc6817b9 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -108,6 +108,7 @@ class MotionPlanningFrame : public QWidget MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_common::DisplayContext* context, QWidget* parent = nullptr); ~MotionPlanningFrame() override; + void clearRobotModel(); void changePlanningGroup(); void enable(); void disable(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index e2de501104..679cf7d1a8 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -126,6 +126,7 @@ class MotionPlanningFrameJointsWidget : public QWidget MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); ~MotionPlanningFrameJointsWidget() override; + void clearRobotModel(); void changePlanningGroup(const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, const robot_interaction::InteractionHandlerPtr& goal_state_handler); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 6f78cf1270..768b6309ae 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */ #include +#include #include #include #include @@ -85,8 +86,8 @@ MotionPlanningDisplay::MotionPlanningDisplay() , text_to_display_(nullptr) , frame_(nullptr) , frame_dock_(nullptr) - , menu_handler_start_(new interactive_markers::MenuHandler) - , menu_handler_goal_(new interactive_markers::MenuHandler) + , menu_handler_start_(std::make_shared()) + , menu_handler_goal_(std::make_shared()) , int_marker_display_(nullptr) { // Category Groups @@ -183,7 +184,7 @@ MotionPlanningDisplay::MotionPlanningDisplay() // Start background jobs background_process_.setJobUpdateEvent( - boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, boost::placeholders::_1, boost::placeholders::_2)); + std::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, std::placeholders::_1, std::placeholders::_2)); } // ****************************************************************************************** @@ -295,7 +296,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable) void MotionPlanningDisplay::selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr msg) { // synchronize ROS callback with main loop - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, msg->data)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::changePlanningGroup, this, msg->data)); } void MotionPlanningDisplay::reset() @@ -323,7 +324,7 @@ void MotionPlanningDisplay::reset() void MotionPlanningDisplay::backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent /*unused*/, const std::string& /*unused*/) { - addMainLoopJob(boost::bind(&MotionPlanningDisplay::updateBackgroundJobProgressBar, this)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::updateBackgroundJobProgressBar, this)); } void MotionPlanningDisplay::updateBackgroundJobProgressBar() @@ -335,28 +336,20 @@ void MotionPlanningDisplay::updateBackgroundJobProgressBar() if (n == 0) { - p->setValue(p->maximum()); - p->update(); p->hide(); p->setMaximum(0); + p->setValue(0); } else { - if (n == 1) - { - if (p->maximum() == 0) - p->setValue(0); - else - p->setValue(p->maximum() - 1); - } - else + if (p->maximum() < n) // increase max { - if (p->maximum() < n) - p->setMaximum(n); - else - p->setValue(p->maximum() - n); + p->setMaximum(n); + if (n > 1) // only show bar if there will be a progress to show + p->show(); } - p->show(); + else // progress + p->setValue(p->maximum() - n); p->update(); } } @@ -721,7 +714,7 @@ void MotionPlanningDisplay::changedQueryStartState() setStatusTextColor(query_start_color_property_->getColor()); addStatusText("Changed start state"); drawQueryStartState(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), "publishInteractiveMarkers"); } @@ -732,7 +725,7 @@ void MotionPlanningDisplay::changedQueryGoalState() setStatusTextColor(query_goal_color_property_->getColor()); addStatusText("Changed goal state"); drawQueryGoalState(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), "publishInteractiveMarkers"); } @@ -805,7 +798,7 @@ void MotionPlanningDisplay::resetInteractiveMarkers() { query_start_state_->clearError(); query_goal_state_->clearError(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), "publishInteractiveMarkers"); } @@ -911,7 +904,7 @@ void MotionPlanningDisplay::scheduleDrawQueryStartState(robot_interaction::Inter { if (!planning_scene_monitor_) return; - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); updateQueryStartState(); } @@ -921,7 +914,7 @@ void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::Intera { if (!planning_scene_monitor_) return; - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); updateQueryGoalState(); } @@ -930,7 +923,7 @@ void MotionPlanningDisplay::updateQueryStartState() { queryStartStateChanged(); recomputeQueryStartStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryStartState, this)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::changedQueryStartState, this)); context_->queueRender(); } @@ -938,7 +931,7 @@ void MotionPlanningDisplay::updateQueryGoalState() { queryGoalStateChanged(); recomputeQueryGoalStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryGoalState, this)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::changedQueryGoalState, this)); context_->queueRender(); } @@ -1048,7 +1041,7 @@ void MotionPlanningDisplay::changedPlanningGroup() if (frame_) frame_->changePlanningGroup(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), "publishInteractiveMarkers"); } @@ -1118,7 +1111,7 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptrinsert(menu_states, state_name, - boost::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_name)); + std::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_name)); } // // Group commands, which end up being the same for both interaction handlers @@ -1126,7 +1119,24 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptrinsert("Planning Group", immh::FeedbackCallback()); // for (int i = 0; i < group_names.size(); ++i) // mh->insert(menu_groups, group_names[i], - // boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i])); + // std::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i])); +} + +void MotionPlanningDisplay::clearRobotModel() +{ + // Invalidate all references to the RobotModel ... + if (frame_) + frame_->clearRobotModel(); + if (trajectory_visual_) + trajectory_visual_->clearRobotModel(); + previous_state_.reset(); + query_start_state_.reset(); + query_goal_state_.reset(); + kinematics_metrics_.reset(); + robot_interaction_.reset(); + dynamics_solver_.clear(); + // ... before calling the parent's method, which finally destroys the creating RobotModelLoader. + PlanningSceneDisplay::clearRobotModel(); } void MotionPlanningDisplay::onRobotModelLoaded() @@ -1137,8 +1147,8 @@ void MotionPlanningDisplay::onRobotModelLoaded() robot_interaction_ = std::make_shared( getRobotModel(), node_, rclcpp::names::append(getMoveGroupNS(), "rviz_moveit_motion_planning_display")); robot_interaction::KinematicOptions o; - o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, - boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); + o.state_validity_callback_ = std::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); robot_interaction_->getKinematicOptionsMap()->setOptions( robot_interaction::KinematicOptionsMap::ALL, o, robot_interaction::KinematicOptions::STATE_VALIDITY_CALLBACK); @@ -1153,10 +1163,10 @@ void MotionPlanningDisplay::onRobotModelLoaded() robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient()); query_goal_state_ = std::make_shared( robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient()); - query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, - boost::placeholders::_1, boost::placeholders::_2)); - query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, - boost::placeholders::_1, boost::placeholders::_2)); + query_start_state_->setUpdateCallback(std::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, + std::placeholders::_1, std::placeholders::_2)); + query_goal_state_->setUpdateCallback(std::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, + std::placeholders::_1, std::placeholders::_2)); // Interactive marker menus populateMenuHandler(menu_handler_start_); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 54058ad8eb..83143d48c2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -34,6 +34,8 @@ /* Author: Ioan Sucan */ +#include + #include #include #include @@ -226,7 +228,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c // semantic_world_.reset(); // if (semantic_world_) // { - // semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this)); + // semantic_world_->addTableCallback(std::bind(&MotionPlanningFrame::updateTables, this)); // } // } // catch (std::exception& ex) @@ -354,14 +356,13 @@ void MotionPlanningFrame::changePlanningGroupHelper() if (!planning_display_->getPlanningSceneMonitor()) return; - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::fillStateSelectionOptions, this)); - planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::populateConstraintsList, this, std::vector())); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::fillStateSelectionOptions, this)); + planning_display_->addMainLoopJob([this]() { populateConstraintsList(std::vector()); }); const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); std::string group = planning_display_->getCurrentPlanningGroup(); planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group)); + std::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group)); planning_display_->addMainLoopJob( [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); }); @@ -395,7 +396,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() RCLCPP_ERROR(LOGGER, "%s", ex.what()); } planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_)); + std::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_)); if (move_group_) { move_group_->allowLooking(ui_->allow_looking->isChecked()); @@ -404,9 +405,8 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(has_unique_endeffector); }); std::vector desc; if (move_group_->getInterfaceDescriptions(desc)) - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlannersList, this, desc)); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this), - "populateConstraintsList"); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlannersList, this, desc)); + planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList"); if (first_time_) { @@ -421,15 +421,22 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->useApproximateIK(ui_->approximate_ik->isChecked()); if (ui_->allow_external_program->isChecked()) planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::allowExternalProgramCommunication, this, true)); + std::bind(&MotionPlanningFrame::allowExternalProgramCommunication, this, true)); } } } } +void MotionPlanningFrame::clearRobotModel() +{ + ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr()); + joints_tab_->clearRobotModel(); + move_group_.reset(); +} + void MotionPlanningFrame::changePlanningGroup() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), "Frame::changePlanningGroup"); joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(), planning_display_->getQueryStartStateHandler(), @@ -439,7 +446,7 @@ void MotionPlanningFrame::changePlanningGroup() void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) { if (update_type & planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY) - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } void MotionPlanningFrame::addSceneObject() @@ -516,11 +523,11 @@ void MotionPlanningFrame::addSceneObject() } setLocalSceneEdited(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); // Automatically select the inserted object so that its IM is displayed planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list)); + std::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list)); planning_display_->queueRenderSceneGeometry(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index 59f3613f31..245709fd84 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -53,7 +53,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualizatio void MotionPlanningFrame::databaseConnectButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClicked, this), "connect to database"); } @@ -108,7 +108,7 @@ void MotionPlanningFrame::resetDbButtonClicked() return; planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeResetDbButtonClicked, this, response.toStdString()), "reset database"); + std::bind(&MotionPlanningFrame::computeResetDbButtonClicked, this, response.toStdString()), "reset database"); } void MotionPlanningFrame::computeDatabaseConnectButtonClicked() @@ -121,12 +121,12 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked() robot_state_storage_.reset(); constraints_storage_.reset(); planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 1)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 1)); } else { planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 2)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 2)); try { warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(node_); @@ -140,19 +140,19 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked() else { planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); return; } } catch (std::exception& ex) { planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); RCLCPP_ERROR(LOGGER, "%s", ex.what()); return; } planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 4)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 4)); } } @@ -206,8 +206,7 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper(int mode) if (move_group_) { move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value()); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this), - "populateConstraintsList"); + planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList"); } } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 152a4eb515..04e6e385b5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -202,15 +202,21 @@ MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget() delete ui_; } -void MotionPlanningFrameJointsWidget::changePlanningGroup( - const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, - const robot_interaction::InteractionHandlerPtr& goal_state_handler) +void MotionPlanningFrameJointsWidget::clearRobotModel() { - // release previous models (if any) ui_->joints_view_->setModel(nullptr); + start_state_handler_.reset(); + goal_state_handler_.reset(); start_state_model_.reset(); goal_state_model_.reset(); +} +void MotionPlanningFrameJointsWidget::changePlanningGroup( + const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler) +{ + // release previous models (if any) + clearRobotModel(); // create new models start_state_handler_ = start_state_handler; goal_state_handler_ = goal_state_handler; @@ -339,6 +345,9 @@ void MotionPlanningFrameJointsWidget::updateNullspaceSliders() if (i == 0) nullspace_.resize(0, 0); + // show/hide dummy slider + ui_->dummy_ns_slider_->setVisible(i == 0); + // hide remaining sliders for (; i < ns_sliders_.size(); ++i) ns_sliders_[i]->hide(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index 874411ad4b..0538bc2d43 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -66,11 +66,10 @@ void MotionPlanningFrame::detectObjectsButtonClicked() // } // if (semantic_world_) // { - // semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this)); + // semantic_world_->addTableCallback(std::bind(&MotionPlanningFrame::updateTables, this)); // } // } - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::triggerObjectDetection, this), - "detect objects"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::triggerObjectDetection, this), "detect objects"); } void MotionPlanningFrame::processDetectedObjects() @@ -168,7 +167,7 @@ void MotionPlanningFrame::listenDetectedObjects( const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr /*msg*/) { rclcpp::sleep_for(std::chrono::seconds(1)); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::processDetectedObjects, this)); } void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids) @@ -198,7 +197,7 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vectoraddBackgroundJob(boost::bind(&MotionPlanningFrame::publishTables, this), "publish tables"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::publishTables, this), "publish tables"); } void MotionPlanningFrame::publishTables() @@ -206,7 +205,7 @@ void MotionPlanningFrame::publishTables() // TODO (ddengster): Enable when moveit_ros_perception is ported // semantic_world_->addTablesToCollisionWorld(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::updateSupportSurfacesList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::updateSupportSurfacesList, this)); } void MotionPlanningFrame::selectedSupportSurfaceChanged() @@ -304,7 +303,7 @@ void MotionPlanningFrame::pickObjectButtonClicked() // } // RCLCPP_INFO(LOGGER, "Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(), // support_surface_name_.c_str()); - // planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::pickObject, this), "pick"); + // planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::pickObject, this), "pick"); } void MotionPlanningFrame::placeObjectButtonClicked() @@ -351,7 +350,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() // upright_orientation, 0.1); // planning_display_->visualizePlaceLocations(place_poses_); // place_object_name_ = attached_bodies[0]->getName(); - // planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::placeObject, this), "place"); + // planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::placeObject, this), "place"); } // void MotionPlanningFrame::pickObject() @@ -374,7 +373,6 @@ void MotionPlanningFrame::placeObjectButtonClicked() //} // // void MotionPlanningFrame::placeObject() -//{ // move_group_->place(place_object_name_, place_poses_); // return; //} diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 66b35662d5..5ce812d9eb 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -151,7 +151,7 @@ void MotionPlanningFrame::clearScene() ps->getPlanningSceneMsg(msg); planning_scene_publisher_->publish(msg); setLocalSceneEdited(false); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } } @@ -233,7 +233,7 @@ void MotionPlanningFrame::removeSceneObject() ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString()); scene_marker_.reset(); setLocalSceneEdited(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } } @@ -511,7 +511,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() RCLCPP_DEBUG(LOGGER, "Copied collision object to '%s'", name.c_str()); } setLocalSceneEdited(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } void MotionPlanningFrame::computeSaveSceneButtonClicked() @@ -530,7 +530,7 @@ void MotionPlanningFrame::computeSaveSceneButtonClicked() RCLCPP_ERROR(LOGGER, "%s", ex.what()); } - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); } } @@ -551,7 +551,7 @@ void MotionPlanningFrame::computeSaveQueryButtonClicked(const std::string& scene RCLCPP_ERROR(LOGGER, "%s", ex.what()); } - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); } } @@ -588,7 +588,7 @@ void MotionPlanningFrame::computeDeleteSceneButtonClicked() RCLCPP_ERROR(LOGGER, "%s", ex.what()); } } - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); } } } @@ -614,7 +614,7 @@ void MotionPlanningFrame::computeDeleteQueryButtonClicked() RCLCPP_ERROR(LOGGER, "%s", ex.what()); } planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s)); + std::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s)); } } } @@ -751,7 +751,7 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() mp->start_state, *start_state); planning_display_->setQueryStartState(*start_state); - moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(*planning_display_->getQueryGoalState())); + auto goal_state = std::make_shared(*planning_display_->getQueryGoalState()); for (const moveit_msgs::msg::Constraints& goal_constraint : mp->goal_constraints) if (!goal_constraint.joint_constraints.empty()) { @@ -865,7 +865,7 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (scene_marker_) { scene_marker_.reset(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this)); } } } @@ -878,12 +878,11 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (ab) { known_collision_objects_[item->type()].first = item_text; - moveit::core::AttachedBody* new_ab = - new moveit::core::AttachedBody(ab->getAttachedLink(), known_collision_objects_[item->type()].first, - ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), - ab->getDetachPosture(), ab->getSubframes()); + auto new_ab = std::make_unique( + ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getPose(), ab->getShapes(), + ab->getShapePoses(), ab->getTouchLinks(), ab->getDetachPosture(), ab->getSubframes()); cs.clearAttachedBody(ab->getName()); - cs.attachBody(new_ab); + cs.attachBody(std::move(new_ab)); } } setLocalSceneEdited(); @@ -1016,7 +1015,7 @@ void MotionPlanningFrame::exportGeometryAsTextButtonClicked() QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text"); + std::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text"); } void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path) @@ -1046,7 +1045,7 @@ void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) if (ps->loadGeometryFromStream(fin)) { RCLCPP_INFO(LOGGER, "Loaded scene geometry from '%s'", path.c_str()); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); setLocalSceneEdited(); } @@ -1065,6 +1064,6 @@ void MotionPlanningFrame::importGeometryFromTextButtonClicked() QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text"); + std::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text"); } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index ec02a67b01..19ef6e2fc4 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -59,15 +59,14 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualizatio void MotionPlanningFrame::planButtonClicked() { publishSceneIfNeeded(); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanButtonClicked, this), - "compute plan"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computePlanButtonClicked, this), "compute plan"); } void MotionPlanningFrame::executeButtonClicked() { ui_->execute_button->setEnabled(false); // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution - planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this)); + planning_display_->spawnBackgroundJob(std::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this)); } void MotionPlanningFrame::planAndExecuteButtonClicked() @@ -76,13 +75,13 @@ void MotionPlanningFrame::planAndExecuteButtonClicked() ui_->plan_and_execute_button->setEnabled(false); ui_->execute_button->setEnabled(false); // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution - planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this)); + planning_display_->spawnBackgroundJob(std::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this)); } void MotionPlanningFrame::stopButtonClicked() { ui_->stop_button->setEnabled(false); // avoid clicking again - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop"); } void MotionPlanningFrame::allowReplanningToggled(bool checked) @@ -174,7 +173,7 @@ bool MotionPlanningFrame::computeCartesianPlan() bool MotionPlanningFrame::computeJointSpacePlan() { current_plan_ = std::make_shared(); - return move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + return move_group_->plan(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS; } void MotionPlanningFrame::computePlanButtonClicked() @@ -211,7 +210,7 @@ void MotionPlanningFrame::computeExecuteButtonClicked() if (mgi && current_plan_) { ui_->stop_button->setEnabled(true); // enable stopping - bool success = mgi->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + bool success = mgi->execute(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS; onFinishedExecution(success); } } @@ -233,7 +232,7 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() } else { - bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS; + bool success = move_group_->move() == moveit::core::MoveItErrorCode::SUCCESS; onFinishedExecution(success); } ui_->plan_and_execute_button->setEnabled(true); @@ -280,8 +279,8 @@ void MotionPlanningFrame::onNewPlanningSceneState() void MotionPlanningFrame::startStateTextChanged(const QString& start_state) { // use background job: fetching the current state might take up to a second - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this, - start_state.toStdString()), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::startStateTextChangedExec, this, + start_state.toStdString()), "update start state"); } @@ -296,7 +295,7 @@ void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state) { // use background job: fetching the current state might take up to a second planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::goalStateTextChangedExec, this, goal_state.toStdString()), "update goal state"); + std::bind(&MotionPlanningFrame::goalStateTextChangedExec, this, goal_state.toStdString()), "update goal state"); } void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state) @@ -471,8 +470,7 @@ void MotionPlanningFrame::populatePlannerDescription(const moveit_msgs::msg::Pla void MotionPlanningFrame::populateConstraintsList() { if (move_group_) - planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::populateConstraintsList, this, move_group_->getKnownConstraints())); + planning_display_->addMainLoopJob([this]() { populateConstraintsList(move_group_->getKnownConstraints()); }); } void MotionPlanningFrame::populateConstraintsList(const std::vector& constr) @@ -610,7 +608,7 @@ void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState())); + auto state = std::make_shared(ps->getCurrentState()); moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); planning_display_->setQueryStartState(*state); } @@ -628,7 +626,7 @@ void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs: const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState())); + auto state = std::make_shared(ps->getCurrentState()); moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); planning_display_->setQueryGoalState(*state); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index 282d5f0e1a..793cebc3bd 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -110,7 +110,7 @@ void MotionPlanningFrame::saveSceneButtonClicked() } } - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this), "save scene"); } } @@ -134,7 +134,7 @@ void MotionPlanningFrame::saveQueryButtonClicked() { std::string scene = s->text(0).toStdString(); planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query"); + std::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query"); } else { @@ -179,7 +179,7 @@ void MotionPlanningFrame::saveQueryButtonClicked() } } planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query"); + std::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query"); } } } @@ -187,25 +187,25 @@ void MotionPlanningFrame::saveQueryButtonClicked() void MotionPlanningFrame::deleteSceneButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this), "delete scene"); } void MotionPlanningFrame::deleteQueryButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this), "delete query"); } void MotionPlanningFrame::loadSceneButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this), "load scene"); } void MotionPlanningFrame::loadQueryButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this), "load query"); } @@ -223,7 +223,7 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co if (planning_scene_storage->hasPlanningScene(new_name)) { - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); QMessageBox::warning(this, "Scene not renamed", QString("The scene name '").append(item->text(column)).append("' already exists")); return; @@ -241,7 +241,7 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co std::string new_name = item->text(column).toStdString(); if (planning_scene_storage->hasPlanningQuery(scene, new_name)) { - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); QMessageBox::warning(this, "Query not renamed", QString("The query name '") .append(item->text(column)) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 221318fd01..346228701e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -2042,82 +2042,12 @@ This is usually achieved by random seeding, which can flip the robot configurati 0 - - - 0 - 5 - - 16777215 5 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 0 - 0 - - - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 0 - 0 - - - - - - - - - 240 - 240 - 240 - - - - - - - 255 - 0 - 0 - - - - - - <html><head/><body><p>Progress of background jobs</p></body></html> diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui index 389365e9bd..ecde6d0514 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui @@ -43,14 +43,44 @@ - + + + The sliders below allow for jogging the nullspace of the current configuration, +i.e. trigger joint motions that don't affect the end-effector pose. + +Typically, redundant arms (with 7+ joints) offer such a nullspace. +However, also singular configurations provide a nullspace. + +Each basis vector of the (linear) nullspace is represented by a separate slider. + Nullspace exploration: - + + + + + false + + + The slider will become active if the current robot configuration has a nullspace. +That's typically the case for redundant robots, i.e. 7+ joints, or singular configurations. + + + -1 + + + 1 + + + Qt::Horizontal + + + + diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 2de19a78d4..07a82d9784 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -25,7 +25,6 @@ class_loader eigen - libogre-dev libqt5-opengl-dev qtbase5-dev diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index b613c1022f..99449b946f 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -146,7 +146,7 @@ protected Q_SLOTS: /// This function is used by loadRobotModel() and should only be called in the MainLoop /// You probably should not call this function directly - void clearRobotModel(); + virtual void clearRobotModel(); /// This function constructs a new planning scene. Probably this should be called in a background thread /// as it may take some time to complete its execution diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp index 7cf8ea0fb0..4042eb6cfb 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp @@ -49,7 +49,7 @@ BackgroundProcessing::BackgroundProcessing() // spin a thread that will process user events run_processing_thread_ = true; processing_ = false; - processing_thread_ = std::make_unique(boost::bind(&BackgroundProcessing::processingThread, this)); + processing_thread_ = std::make_unique(std::bind(&BackgroundProcessing::processingThread, this)); } BackgroundProcessing::~BackgroundProcessing() diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 80704801d9..c75c949be0 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -174,8 +174,7 @@ PlanningSceneDisplay::~PlanningSceneDisplay() planning_scene_render_.reset(); if (context_ && context_->getSceneManager() && planning_scene_node_) context_->getSceneManager()->destroySceneNode(planning_scene_node_); - if (planning_scene_robot_) - planning_scene_robot_.reset(); + planning_scene_robot_.reset(); planning_scene_monitor_.reset(); } @@ -205,8 +204,8 @@ void PlanningSceneDisplay::onInitialize() if (robot_category_) { - planning_scene_robot_.reset( - new RobotStateVisualization(planning_scene_node_, context_, "Planning Scene", robot_category_)); + planning_scene_robot_ = + std::make_shared(planning_scene_node_, context_, "Planning Scene", robot_category_); planning_scene_robot_->setVisible(true); planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool()); planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool()); @@ -222,7 +221,7 @@ void PlanningSceneDisplay::reset() Display::reset(); if (isEnabled()) - addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); + addBackgroundJob(std::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); if (planning_scene_robot_) { @@ -398,7 +397,7 @@ void PlanningSceneDisplay::changedPlanningSceneTopic() service_name = rclcpp::names::append(getMoveGroupNS(), service_name); auto bg_func = [=]() { if (planning_scene_monitor_->requestPlanningSceneState(service_name)) - addMainLoopJob(boost::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this)); + addMainLoopJob(std::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this)); else setStatus(rviz_common::properties::StatusProperty::Warn, "PlanningScene", "Requesting initial scene failed"); }; @@ -526,8 +525,8 @@ planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlan void PlanningSceneDisplay::clearRobotModel() { planning_scene_render_.reset(); - planning_scene_monitor_.reset(); // this so that the destructor of the PlanningSceneMonitor gets called before a new - // instance of a scene monitor is constructed + // Ensure old PSM is destroyed before we attempt to create a new one + planning_scene_monitor_.reset(); } void PlanningSceneDisplay::loadRobotModel() @@ -538,7 +537,7 @@ void PlanningSceneDisplay::loadRobotModel() // we need to make sure the clearing of the robot model is in the main thread, // so that rendering operations do not have data removed from underneath, // so the next function is executed in the main loop - addMainLoopJob(boost::bind(&PlanningSceneDisplay::clearRobotModel, this)); + addMainLoopJob(std::bind(&PlanningSceneDisplay::clearRobotModel, this)); waitForAllMainLoopJobs(); @@ -547,8 +546,8 @@ void PlanningSceneDisplay::loadRobotModel() { planning_scene_monitor_.swap(psm); planning_scene_monitor_->addUpdateCallback( - boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, boost::placeholders::_1)); - addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); + std::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, std::placeholders::_1)); + addMainLoopJob(std::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); waitForAllMainLoopJobs(); } else @@ -610,7 +609,7 @@ void PlanningSceneDisplay::onEnable() { Display::onEnable(); - addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); + addBackgroundJob(std::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); if (planning_scene_robot_) { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index 11ff67c831..e7b7efbe45 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -37,12 +37,11 @@ #pragma once #include -#include #include -#include +#include #include -#include -#include +#include +#include namespace octomap { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index b314ebedad..0cad7c6bd2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -92,6 +92,7 @@ class TrajectoryVisualization : public QObject void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context); + void clearRobotModel(); void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); void onEnable(); void onDisable(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index d186fcf18f..11f46757b5 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -537,7 +537,7 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::msg:: trajectory_message_to_display_.reset(); - robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, "")); + auto t = std::make_shared(robot_model_, ""); try { for (std::size_t i = 0; i < msg->trajectory.size(); ++i) @@ -623,4 +623,10 @@ void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(bool enable) trajectory_slider_panel_->onDisable(); } +void TrajectoryVisualization::clearRobotModel() +{ + robot_model_.reset(); + robot_state_.reset(); +} + } // namespace moveit_rviz_plugin diff --git a/moveit_ros/warehouse/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/warehouse/src/broadcast.cpp index 5a340ae438..eafc03c17a 100644 --- a/moveit_ros/warehouse/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/warehouse/src/broadcast.cpp @@ -68,6 +68,7 @@ int main(int argc, char** argv) // time to wait in between publishing messages double delay = 0.001; + // clang-format off boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " @@ -80,7 +81,7 @@ int main(int argc, char** argv) "state", boost::program_options::value(), "Name of the robot state to publish.")("delay", boost::program_options::value()->default_value(delay), "Time to wait in between publishing messages (s)"); - + // clang-format on boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); boost::program_options::notify(vm); diff --git a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp index c1d56be830..0ca7394679 100644 --- a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp @@ -216,12 +216,14 @@ int main(int argc, char** argv) node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("import_from_text_to_warehouse", node_options); + // clang-format off boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("queries", boost::program_options::value(), "Name of file containing motion planning queries.")( "scene", boost::program_options::value(), "Name of file containing motion planning scene.")( "host", boost::program_options::value(), "Host for the DB.")("port", boost::program_options::value(), "Port for the DB."); + // clang-format on boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp index 5b6d7a3451..8575f8cc93 100644 --- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp @@ -186,7 +186,7 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, " * %s", name.c_str()); } - psm.addUpdateCallback(boost::bind(&onSceneUpdate, &psm, &pss)); + psm.addUpdateCallback(std::bind(&onSceneUpdate, &psm, &pss)); auto callback1 = [&](moveit_msgs::msg::MotionPlanRequest::ConstSharedPtr msg) -> void { return onMotionPlanRequest(msg, &psm, &pss); diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp index 314290e13d..96623f3d8b 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp @@ -313,7 +313,7 @@ class MoveItConfigData std::map getInitialJoints() const; bool outputSetupAssistantFile(const std::string& file_path); bool outputOMPLPlanningYAML(const std::string& file_path); - bool outputCHOMPPlanningYAML(const std::string& file_path); + bool outputSTOMPPlanningYAML(const std::string& file_path); bool outputKinematicsYAML(const std::string& file_path); bool outputJointLimitsYAML(const std::string& file_path); bool outputFakeControllersYAML(const std::string& file_path); @@ -334,13 +334,6 @@ class MoveItConfigData */ std::string getGazeboCompatibleURDF(); - /** - * \brief Set list of collision link pairs in SRDF; sorted; with optional filter - * \param link_pairs list of collision link pairs - * \param skip_mask mask of shifted moveit_setup_assistant::DisabledReason values that will be skipped - */ - void setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask = 0); - /** * \brief Decide the best two joints to be used for the projection evaluator * \param planning_group name of group to use diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 6f74c84ad9..fb1192ed95 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -20,7 +20,6 @@ ament_cmake moveit_common - libogre-dev ompl ament_index_cpp diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index 5264d7872e..92ba93b4e0 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -100,7 +100,11 @@ bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old, } if (!keep_old) - config_data.srdf_->disabled_collisions_.clear(); + { + config_data.srdf_->no_default_collision_links_.clear(); + config_data.srdf_->enabled_collision_pairs_.clear(); + config_data.srdf_->disabled_collision_pairs_.clear(); + } return true; } @@ -114,6 +118,52 @@ moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfig trials > 0, trials, min_collision_fraction, verbose); } +// less operation for two CollisionPairs +struct CollisionPairLess +{ + bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const + { + return left.link1_ < right.link1_ && left.link2_ < right.link2_; + } +}; + +// Update collision pairs +void updateCollisionLinkPairs(std::vector& target_pairs, + const moveit_setup_assistant::LinkPairMap& source_pairs, size_t skip_mask) +{ + // remove duplicates + std::set filtered; + for (auto& p : target_pairs) + { + if (p.link1_ >= p.link2_) + std::swap(p.link1_, p.link2_); // unify link1, link2 sorting + filtered.insert(p); + } + + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format + for (const auto& link_pair : source_pairs) + { + // Only copy those that are actually disabled + if (!link_pair.second.disable_check) + continue; + + // Filter out pairs matching the skip_mask + if ((1 << link_pair.second.reason) & skip_mask) + continue; + + srdf::Model::CollisionPair pair; + pair.link1_ = link_pair.first.first; + pair.link2_ = link_pair.first.second; + if (pair.link1_ >= pair.link2_) + std::swap(pair.link1_, pair.link2_); + pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); + + filtered.insert(pair); + } + + target_pairs.assign(filtered.begin(), filtered.end()); +} + int main(int argc, char* argv[]) { std::string config_pkg_path; @@ -127,6 +177,7 @@ int main(int argc, char* argv[]) uint32_t never_trials = 0; + // clang-format off po::options_description desc("Allowed options"); desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")( "urdf", po::value(&urdf_path), "path to URDF ( or xacro)")("srdf", po::value(&srdf_path), @@ -141,6 +192,7 @@ int main(int argc, char* argv[]) "number of trials for searching never colliding pairs")( "min-collision-fraction", po::value(&min_collision_fraction), "fraction of small sample size to determine links that are always colliding"); + // clang-format on po::positional_options_description pos_desc; pos_desc.add("xacro-args", -1); @@ -201,7 +253,7 @@ int main(int argc, char* argv[]) if (!include_always) skip_mask |= (1 << moveit_setup_assistant::ALWAYS); - config_data.setCollisionLinkPairs(link_pairs, skip_mask); + updateCollisionLinkPairs(config_data.srdf_->disabled_collision_pairs_, link_pairs, skip_mask); config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path); diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp index 7865772c6c..2a22178710 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/src/setup_assistant_main.cpp @@ -59,10 +59,12 @@ int main(int argc, char** argv) namespace po = boost::program_options; // Declare the supported options + // clang-format off po::options_description desc("Allowed options"); desc.add_options()("help,h", "Show help message")("debug,g", "Run in debug/test mode")( "urdf_path,u", po::value(), "Optional, path to URDF file in ROS package")( "config_pkg,c", po::value(), "Optional, pass in existing config package to load"); + // clang-format on // Process options po::variables_map vm; diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index e21eb7eeb1..171a2bfb36 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -52,8 +52,8 @@ const boost::unordered_map REASONS_TO_STRING = boos DEFAULT, "Default")(ADJACENT, "Adjacent")(ALWAYS, "Always")(USER, "User")(NOT_DISABLED, "Not Disabled"); const boost::unordered_map REASONS_FROM_STRING = - boost::assign::map_list_of("Never", NEVER)("Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)( - "User", USER)("Not Disabled", NOT_DISABLED); + boost::assign::map_list_of("Never", NEVER)("Default", DEFAULT)( + "Adjacent", ADJACENT)("Always", ALWAYS)("User", USER)("Not Disabled", NOT_DISABLED); // Used for logging static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); @@ -566,7 +566,7 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce for (int i = 0; i < num_threads; ++i) { ThreadComputation tc(scene, req, i, num_trials / num_threads, &links_seen_colliding, &lock, progress); - bgroup.create_thread(boost::bind(&disableNeverInCollisionThread, tc)); + bgroup.create_thread(std::bind(&disableNeverInCollisionThread, tc)); } try diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 491b5a6d7b..4d3c985cac 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -141,14 +141,17 @@ planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene() // ****************************************************************************************** void MoveItConfigData::loadAllowedCollisionMatrix() { - // Clear the allowed collision matrix allowed_collision_matrix_.clear(); - // Update the allowed collision matrix, in case there has been a change - for (const auto& disabled_collision : srdf_->disabled_collisions_) - { - allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true); - } + // load collision defaults + for (const std::string& name : srdf_->no_default_collision_links_) + allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); + // re-enable specific collision pairs + for (auto const& collision : srdf_->enabled_collision_pairs_) + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false); + // *finally* disable selected collision pairs + for (auto const& collision : srdf_->disabled_collision_pairs_) + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true); } // ****************************************************************************************** @@ -183,7 +186,8 @@ bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path) emitter << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "author_name" << YAML::Value << author_name_; emitter << YAML::Key << "author_email" << YAML::Value << author_email_; - emitter << YAML::Key << "generated_timestamp" << YAML::Value << std::time(nullptr); // TODO: is this cross-platform? + auto cur_time = std::time(nullptr); + emitter << YAML::Key << "generated_timestamp" << YAML::Value << cur_time; // TODO: is this cross-platform? emitter << YAML::EndMap; emitter << YAML::EndMap; @@ -198,6 +202,10 @@ bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path) output_stream << emitter.c_str(); output_stream.close(); + /// Update the parsed setup_assistant timestamp + // NOTE: Needed for when people run the MSA generator multiple times in a row. + config_pkg_generated_timestamp_ = cur_time; + return true; // file created successfully } @@ -282,31 +290,133 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path) } // ****************************************************************************************** -// Output CHOMP Planning config files +// Output STOMP Planning config files // ****************************************************************************************** -bool MoveItConfigData::outputCHOMPPlanningYAML(const std::string& file_path) +bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path) { YAML::Emitter emitter; + emitter << YAML::BeginMap; + + // Add STOMP default for every group + for (srdf::Model::Group& group : srdf_->groups_) + { + emitter << YAML::Key << "stomp/" + group.name_; + emitter << YAML::BeginMap; + emitter << YAML::Key << "group_name"; + emitter << YAML::Value << group.name_; + + emitter << YAML::Key << "optimization"; + emitter << YAML::BeginMap; + emitter << YAML::Key << "num_timesteps"; + emitter << YAML::Value << "60"; + emitter << YAML::Key << "num_iterations"; + emitter << YAML::Value << "40"; + emitter << YAML::Key << "num_iterations_after_valid"; + emitter << YAML::Value << "0"; + emitter << YAML::Key << "num_rollouts"; + emitter << YAML::Value << "30"; + emitter << YAML::Key << "max_rollouts"; + emitter << YAML::Value << "30"; + emitter << YAML::Key << "initialization_method"; + emitter << YAML::Value << "1"; + emitter << YAML::Comment("[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]"); + emitter << YAML::Key << "control_cost_weight"; + emitter << YAML::Value << "0.0"; + emitter << YAML::EndMap; + + emitter << YAML::Key << "task"; + emitter << YAML::BeginMap; + + emitter << YAML::Key << "noise_generator"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/NormalDistributionSampling"; + emitter << YAML::Key << "stddev"; + emitter << YAML::Flow; + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); + std::vector stddev(joint_models.size(), 0.05); + emitter << YAML::Value << stddev; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::Key << "cost_functions"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/CollisionCheck"; + emitter << YAML::Key << "collision_penalty"; + emitter << YAML::Value << "1.0"; + emitter << YAML::Key << "cost_weight"; + emitter << YAML::Value << "1.0"; + emitter << YAML::Key << "kernel_window_percentage"; + emitter << YAML::Value << "0.2"; + emitter << YAML::Key << "longest_valid_joint_move"; + emitter << YAML::Value << "0.05"; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::Key << "noisy_filters"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/JointLimits"; + emitter << YAML::Key << "lock_start"; + emitter << YAML::Value << "True"; + emitter << YAML::Key << "lock_goal"; + emitter << YAML::Value << "True"; + emitter << YAML::EndMap; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/MultiTrajectoryVisualization"; + emitter << YAML::Key << "line_width"; + emitter << YAML::Value << "0.02"; + emitter << YAML::Key << "rgb"; + emitter << YAML::Flow; + std::vector noisy_filters_rgb{ 255, 255, 0 }; + emitter << YAML::Value << noisy_filters_rgb; + emitter << YAML::Key << "marker_array_topic"; + emitter << YAML::Value << "stomp_trajectories"; + emitter << YAML::Key << "marker_namespace"; + emitter << YAML::Value << "noisy"; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::Key << "update_filters"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/PolynomialSmoother"; + emitter << YAML::Key << "poly_order"; + emitter << YAML::Value << "6"; + emitter << YAML::EndMap; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/TrajectoryVisualization"; + emitter << YAML::Key << "line_width"; + emitter << YAML::Value << "0.05"; + emitter << YAML::Key << "rgb"; + emitter << YAML::Flow; + std::vector update_filters_rgb{ 0, 191, 255 }; + emitter << YAML::Value << update_filters_rgb; + emitter << YAML::Key << "error_rgb"; + emitter << YAML::Flow; + std::vector update_filters_error_rgb{ 255, 0, 0 }; + emitter << YAML::Value << update_filters_error_rgb; + emitter << YAML::Key << "publish_intermediate"; + emitter << YAML::Value << "True"; + emitter << YAML::Key << "marker_topic"; + emitter << YAML::Value << "stomp_trajectory"; + emitter << YAML::Key << "marker_namespace"; + emitter << YAML::Value << "optimized"; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::EndMap; + emitter << YAML::EndMap; + } - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "planning_time_limit" << YAML::Value << "10.0"; - emitter << YAML::Key << "max_iterations" << YAML::Value << "200"; - emitter << YAML::Key << "max_iterations_after_collision_free" << YAML::Value << "5"; - emitter << YAML::Key << "smoothness_cost_weight" << YAML::Value << "0.1"; - emitter << YAML::Key << "obstacle_cost_weight" << YAML::Value << "1.0"; - emitter << YAML::Key << "learning_rate" << YAML::Value << "0.01"; - emitter << YAML::Key << "smoothness_cost_velocity" << YAML::Value << "0.0"; - emitter << YAML::Key << "smoothness_cost_acceleration" << YAML::Value << "1.0"; - emitter << YAML::Key << "smoothness_cost_jerk" << YAML::Value << "0.0"; - emitter << YAML::Key << "ridge_factor" << YAML::Value << "0.01"; - emitter << YAML::Key << "use_pseudo_inverse" << YAML::Value << "false"; - emitter << YAML::Key << "pseudo_inverse_ridge_factor" << YAML::Value << "1e-4"; - emitter << YAML::Key << "joint_update_limit" << YAML::Value << "0.1"; - emitter << YAML::Key << "collision_clearance" << YAML::Value << "0.2"; - emitter << YAML::Key << "collision_threshold" << YAML::Value << "0.07"; - emitter << YAML::Key << "use_stochastic_descent" << YAML::Value << "true"; - emitter << YAML::Key << "enable_failure_recovery" << YAML::Value << "true"; - emitter << YAML::Key << "max_recovery_attempts" << YAML::Value << "5"; emitter << YAML::EndMap; std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); @@ -1196,59 +1306,6 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) return true; // file created successfully } -// ****************************************************************************************** -// Set list of collision link pairs in SRDF; sorted; with optional filter -// ****************************************************************************************** - -class SortableDisabledCollision -{ -public: - SortableDisabledCollision(const srdf::Model::DisabledCollision& dc) - : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_)) - { - } - operator const srdf::Model::DisabledCollision &() const - { - return dc_; - } - bool operator<(const SortableDisabledCollision& other) const - { - return key_ < other.key_; - } - -private: - const srdf::Model::DisabledCollision dc_; - const std::string key_; -}; - -void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask) -{ - // Create temp disabled collision - srdf::Model::DisabledCollision dc; - - std::set disabled_collisions; - disabled_collisions.insert(srdf_->disabled_collisions_.begin(), srdf_->disabled_collisions_.end()); - - // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format - for (const std::pair, LinkPairData>& link_pair : link_pairs) - { - // Only copy those that are actually disabled - if (link_pair.second.disable_check) - { - if ((1 << link_pair.second.reason) & skip_mask) - continue; - - dc.link1_ = link_pair.first.first; - dc.link2_ = link_pair.first.second; - dc.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); - - disabled_collisions.insert(SortableDisabledCollision(dc)); - } - } - - srdf_->disabled_collisions_.assign(disabled_collisions.begin(), disabled_collisions.end()); -} - // ****************************************************************************************** // Decide the best two joints to be used for the projection evaluator // ****************************************************************************************** @@ -1287,7 +1344,7 @@ template bool parse(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T()) { const YAML::Node& n = node[key]; - bool valid = n; + bool valid = n.IsDefined(); storage = valid ? n.as() : default_value; return valid; } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index c9f263b38f..12d0332b6e 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -226,7 +226,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; template_path = config_data_->appendPaths(config_data_->template_package_path_, "package.xml.template"); file.description_ = "Defines a ROS package"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = MoveItConfigData::AUTHOR_INFO; gen_files_.push_back(file); @@ -235,7 +235,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; template_path = config_data_->appendPaths(config_data_->template_package_path_, file.file_name_); file.description_ = "CMake build system configuration file"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -249,7 +249,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; file.description_ = "Folder containing all MoveIt configuration files for your robot. This folder is required and " "cannot be disabled."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::createFolder, this, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -263,7 +263,7 @@ bool ConfigurationFilesWidget::loadGenFiles() "representation of semantic information about robots. This format is intended to represent " "information about the robot that is not in the URDF file, but it is useful for a variety of " "applications. The intention is to include information that has a semantic aspect to it."; - file.gen_func_ = boost::bind(&srdf::SRDFWriter::writeSRDF, config_data_->srdf_, _1); + file.gen_func_ = std::bind(&srdf::SRDFWriter::writeSRDF, config_data_->srdf_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::SRDF; gen_files_.push_back(file); // special step required so the generated .setup_assistant yaml has this value @@ -280,15 +280,23 @@ bool ConfigurationFilesWidget::loadGenFiles() "planner_configs tag. While defining a planner configuration, the only mandatory parameter is " "'type', which is the name of the motion planner to be used. Any other planner-specific " "parameters can be defined but are optional."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputOMPLPlanningYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputOMPLPlanningYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); // chomp_planning.yaml -------------------------------------------------------------------------------------- file.file_name_ = "chomp_planning.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); file.description_ = "Specifies which chomp planning plugin parameters to be used for the CHOMP planner"; - file.gen_func_ = boost::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); + gen_files_.push_back(file); + + // stomp_planning.yaml -------------------------------------------------------------------------------------- + file.file_name_ = "stomp_planning.yaml"; + file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + file.description_ = "Specifies which stomp planning plugin parameters to be used for the STOMP planner"; + file.gen_func_ = std::bind(&MoveItConfigData::outputSTOMPPlanningYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; // need to double check if this is actually correct! gen_files_.push_back(file); @@ -297,7 +305,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Specifies which kinematic solver plugin to use for each planning group in the SRDF, as well as " "the kinematic solver search resolution."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS | MoveItConfigData::GROUP_KINEMATICS; gen_files_.push_back(file); @@ -309,7 +317,7 @@ bool ConfigurationFilesWidget::loadGenFiles() "and acceleration than those contained in your URDF. This information is used by our trajectory " "filtering system to assign reasonable velocities and timing for the trajectory before it is " "passed to the robots controllers."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, std::placeholders::_1); file.write_on_changes = 0; // Can they be changed? gen_files_.push_back(file); @@ -320,7 +328,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Cartesian velocity for planning in the workspace." "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " "planning requests scaled by the velocity scaling factor of an individual planning request."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; // Can they be changed? gen_files_.push_back(file); @@ -329,7 +337,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates dummy configurations for controllers that correspond to defined groups. This is mostly " "useful for testing."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputFakeControllersYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputFakeControllersYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -337,7 +345,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = "simple_moveit_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates controller configuration for SimpleMoveItControllerManager"; - file.gen_func_ = boost::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -346,14 +354,14 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); file.description_ = "Configuration of Gazebo controllers"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); gen_files_.push_back(file); // ros_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "ros_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates controller configurations for ros_control."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -361,7 +369,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = "sensors_3d.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates configurations 3d sensors."; - file.gen_func_ = boost::bind(&MoveItConfigData::output3DSensorPluginYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::output3DSensorPluginYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::SENSORS_CONFIG; gen_files_.push_back(file); @@ -376,7 +384,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; file.description_ = "Folder containing all MoveIt launch files for your robot. " "This folder is required and cannot be disabled."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::createFolder, this, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -387,7 +395,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Launches the move_group node that provides the MoveGroup action and other parameters MoveGroup action"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -397,7 +405,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads settings for the ROS parameter server, required for running MoveIt. This includes the " "SRDF, joints_limits.yaml file, ompl_planning.yaml file, optionally the URDF, etc"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -407,7 +415,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Visualize in Rviz the robot's planning groups running with interactive markers that allow goal " "states to be set."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -419,7 +427,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Intended to be included in other launch files that require the OMPL planning plugin. Defines " "the proper plugin name on the parameter server and a default selection of planning request " "adapters."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -431,7 +439,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Intended to be included in other launch files that require the Pilz industrial motion planner " "planning plugin. Defines the proper plugin name on the parameter server and a default selection " "of planning request adapters."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -443,7 +451,32 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Intended to be included in other launch files that require the CHOMP planning plugin. Defines " "the proper plugin name on the parameter server and a default selection of planning request " "adapters."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); + file.write_on_changes = 0; + gen_files_.push_back(file); + + // stomp_planning_pipeline.launch + // -------------------------------------------------------------------------------------- + file.file_name_ = "stomp_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Intended to be included in other launch files that require the STOMP planning plugin. Defines " + "the proper plugin name on the parameter server and a default selection of planning request " + "adapters."; + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); + file.write_on_changes = 0; + gen_files_.push_back(file); + + // ompl-chomp_planning_pipeline.launch.xml + // -------------------------------------------------------------------------------------- + file.file_name_ = "ompl-chomp_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = + "Intended to be included in other launch files that require the OMPL-CHOMP planning plugins. Defines " + "the proper plugin name on the parameter server and a default selection of planning request " + "adapters."; + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -452,7 +485,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Helper launch file that can choose between different planning pipelines to be loaded."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -461,7 +494,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Helper launch file that specifies default settings for MongoDB."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -470,7 +503,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Launch file for starting MongoDB."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -479,7 +512,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Launch file for starting the warehouse with a default MongoDB."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -488,7 +521,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Launch file for benchmarking OMPL planners"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -497,7 +530,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Helper launch file that can choose between different sensor managers to be loaded."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -506,7 +539,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, "moveit_sensor_manager.launch.xml"); file.description_ = "Placeholder for settings specific to the MoveIt sensor manager implemented for you robot."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -516,7 +549,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads settings for the ROS parameter server required for executing trajectories using the " "trajectory_execution_manager::TrajectoryExecutionManager."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -524,7 +557,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads the fake controller plugin."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -532,7 +565,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads the default controller plugin."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -540,7 +573,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads the ros_control controller plugin."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -549,7 +582,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Run a demo of MoveIt."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -558,7 +591,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Run a demo of MoveIt with Gazebo and Rviz"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -568,7 +601,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch"); file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, " "then using gazebo_ros pkg the robot is spawned to Gazebo"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); gen_files_.push_back(file); // joystick_control.launch ------------------------------------------------------------------ @@ -576,7 +609,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Control the Rviz Motion Planning Plugin with a joystick"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -588,7 +621,7 @@ bool ConfigurationFilesWidget::loadGenFiles() // up with the SA's real launch file file.description_ = "Launch file for easily re-starting the MoveIt Setup Assistant to edit this robot's generated " "configuration package."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -597,7 +630,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, "ros_controllers.launch"); file.description_ = "ros_controllers launch file"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -607,7 +640,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, "moveit.rviz"); file.description_ = "Configuration file for Rviz with the Motion Planning Plugin already setup. Used by passing " "roslaunch moveit_rviz.launch config:=true"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -619,7 +652,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = SETUP_ASSISTANT_FILE; file.rel_path_ = file.file_name_; file.description_ = "MoveIt Setup Assistant's hidden settings file. You should not need to edit this file."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, std::placeholders::_1); file.write_on_changes = -1; // write on any changes gen_files_.push_back(file); @@ -641,7 +674,7 @@ bool ConfigurationFilesWidget::checkDependencies() } // Check that at least 1 link pair is disabled from collision checking - if (config_data_->srdf_->disabled_collisions_.empty()) + if (config_data_->srdf_->disabled_collision_pairs_.empty()) { dependencies << "No self-collisions have been disabled"; } diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index 76be30b8db..938d54505f 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -235,7 +235,8 @@ void DefaultCollisionsWidget::startGeneratingCollisionTable() btn_revert_->setEnabled(true); // allow to interrupt and revert // create a MonitorThread running generateCollisionTable() in a worker thread and monitoring the progress - worker_ = new MonitorThread(boost::bind(&DefaultCollisionsWidget::generateCollisionTable, this, _1), progress_bar_); + worker_ = new MonitorThread(std::bind(&DefaultCollisionsWidget::generateCollisionTable, this, std::placeholders::_1), + progress_bar_); connect(worker_, SIGNAL(finished()), this, SLOT(finishGeneratingCollisionTable())); worker_->start(); // start after having finished() signal connected } @@ -676,12 +677,12 @@ void DefaultCollisionsWidget::checkedFilterChanged() void DefaultCollisionsWidget::linkPairsToSRDF() { // reset the data in the SRDF Writer class - config_data_->srdf_->disabled_collisions_.clear(); + config_data_->srdf_->disabled_collision_pairs_.clear(); // Create temp disabled collision - srdf::Model::DisabledCollision dc; + srdf::Model::CollisionPair dc; - // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format for (moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end(); ++pair_it) { @@ -691,7 +692,7 @@ void DefaultCollisionsWidget::linkPairsToSRDF() dc.link1_ = pair_it->first.first; dc.link2_ = pair_it->first.second; dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason); - config_data_->srdf_->disabled_collisions_.push_back(dc); + config_data_->srdf_->disabled_collision_pairs_.push_back(dc); } } @@ -718,7 +719,7 @@ void DefaultCollisionsWidget::linkPairsFromSRDF() std::pair link_pair; // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created - for (const auto& disabled_collision : config_data_->srdf_->disabled_collisions_) + for (const auto& disabled_collision : config_data_->srdf_->disabled_collision_pairs_) { // Set the link names link_pair.first = disabled_collision.link1_; @@ -820,7 +821,7 @@ moveit_setup_assistant::MonitorThread::MonitorThread(const boost::functiongetRobotModel()->getJointModelGroup(group_name); if (joint_model_group) { - const std::vector& link_models = joint_model_group->getLinkModels(); // Iterate through the links - for (std::vector::const_iterator link_it = link_models.begin(); - link_it < link_models.end(); ++link_it) - highlightLink((*link_it)->getName(), QColor(255, 0, 0)); + for (const moveit::core::LinkModel* lm : joint_model_group->getLinkModels()) + highlightLink(lm->getName(), QColor(255, 0, 0)); } } @@ -477,12 +475,12 @@ void SetupAssistantWidget::unhighlightAll() } // Iterate through the links - for (std::vector::const_iterator link_it = links.begin(); link_it < links.end(); ++link_it) + for (const std::string& link : links) { - if ((*link_it).empty()) + if (link.empty()) continue; - robot_state_display_->unsetLinkColor(*link_it); + robot_state_display_->unsetLinkColor(link); } } diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index d0e9b02a52..e18b1e079e 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -395,7 +395,7 @@ bool StartScreenWidget::loadExistingFiles() QApplication::processEvents(); // Load the SRDF - if (!loadSRDFFile(config_data_->srdf_path_)) + if (!loadSRDFFile(config_data_->srdf_path_, config_data_->xacro_args_)) return false; // error occurred // Progress Indicator @@ -546,9 +546,7 @@ bool StartScreenWidget::loadNewFiles() // ****************************************************************************************** bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args) { - const std::vector vec_xacro_args = { xacro_args }; - - if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, vec_xacro_args)) + if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, { xacro_args })) { QMessageBox::warning(this, "Error Loading Files", QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str())); @@ -592,12 +590,10 @@ bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const st // ****************************************************************************************** // Load SRDF File to Parameter Server // ****************************************************************************************** -bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path) +bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args) { - const std::vector xacro_args; - std::string srdf_string; - if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, xacro_args)) + if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, { xacro_args })) { QMessageBox::warning(this, "Error Loading Files", QString("SRDF file not found: ").append(srdf_file_path.c_str())); return false; diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h index bcceea6dd1..995ad325ac 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h @@ -146,7 +146,7 @@ private Q_SLOTS: bool loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args); /// Load SRDF File - bool loadSRDFFile(const std::string& srdf_file_path); + bool loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args); /// Put SRDF File on Parameter Server bool setSRDFFile(const std::string& srdf_string); diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml new file mode 100644 index 0000000000..eb9c91225b --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000..b078992417 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml index dec8cff842..f05ca303f4 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml @@ -3,5 +3,6 @@ + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000..c1de09f874 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + +