From b88af3296cda51953f8b05627c39a4d9a2fe7e37 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Sun, 6 Oct 2024 07:29:04 -0400 Subject: [PATCH 1/9] have moveit_planners depend on chomp (#3015) by default, the packages generated by moveit_setup_assistant now include a chomp configuration - but do not depend on chomp. the packages do depend on moveit_planners, so it makes sense to include chomp here. --- moveit_planners/moveit_planners/package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 2eb5719919..5f966f2e4a 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -19,10 +19,7 @@ ament_cmake - moveit_planners_ompl moveit_planners_stomp pilz_industrial_motion_planner From 9f00fd0da2da29f8e6b6980746b616891369d530 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 11 Oct 2024 14:09:18 +0200 Subject: [PATCH 2/9] Update urdf/model.h -> urdf/model.hpp (#3003) Co-authored-by: Sebastian Castro Co-authored-by: Sebastian Jahr --- .github/workflows/ci.yaml | 2 +- moveit_core/constraint_samplers/test/pr2_arm_ik.h | 4 ++++ .../constraint_samplers/test/pr2_arm_kinematics_plugin.h | 4 ++++ .../utils/include/moveit/utils/robot_model_test_utils.h | 4 ++++ .../rdf_loader/include/moveit/rdf_loader/rdf_loader.h | 4 ++++ .../include/moveit_setup_framework/data/urdf_config.hpp | 6 +++++- 6 files changed, 22 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 93435c5069..23d837195c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -34,7 +34,7 @@ jobs: # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option # https://stackoverflow.com/a/41673702 CXXFLAGS: >- - -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option + -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option -Wno-cpp CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: > diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index a7633c6ef4..3fb50afe8f 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -37,7 +37,11 @@ #pragma once #include +#if __has_include() +#include +#else #include +#endif #include #include // provides LU decomposition #include diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index 45c73fe87b..6db3c908d4 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -50,7 +50,11 @@ #include +#if __has_include() +#include +#else #include +#endif #include diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index 802fb89759..f8fe1b052c 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -38,7 +38,11 @@ #pragma once #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index cd89eda6a6..6445876478 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -38,7 +38,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp index 3402cf59a4..e13810be1d 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp @@ -35,7 +35,11 @@ #pragma once #include -#include // for testing a valid urdf is loaded +#if __has_include() // for testing a valid urdf is loaded +#include +#else +#include +#endif namespace moveit_setup { From 865ad364e1a8ae1582f9dfb8428eb4c152f3aa10 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 11 Oct 2024 09:36:27 -0400 Subject: [PATCH 3/9] remove dead pick and place code (#3025) most of the other stuff was removed as part of #1177 --- .../move_group_interface.h | 4 - .../src/move_group_interface.cpp | 106 ------------------ 2 files changed, 110 deletions(-) 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 be02e8c6d2..3c15fbafbe 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 @@ -302,10 +302,6 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ void setStartStateToCurrentState(); - /** \brief For pick/place operations, the name of the support surface is used to specify the fact that attached - * objects are allowed to touch the support surface */ - void setSupportSurfaceName(const std::string& name); - /** * \name Setting a joint state target (goal) * 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 84a46005ce..9281ac82d3 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 @@ -585,11 +585,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl pose_reference_frame_ = pose_reference_frame; } - void setSupportSurfaceName(const std::string& support_surface) - { - support_surface_ = support_surface; - } - const std::string& getPoseReferenceFrame() const { return pose_reference_frame_; @@ -1083,61 +1078,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl constructMotionPlanRequest(goal.request); } - // moveit_msgs::action::Pickup::Goal constructPickupGoal(const std::string& object, - // std::vector&& grasps, - // bool plan_only = false) const - // { - // moveit_msgs::action::Pickup::Goal goal; - // goal.target_name = object; - // goal.group_name = opt_.group_name; - // goal.end_effector = getEndEffector(); - // goal.support_surface_name = support_surface_; - // goal.possible_grasps = std::move(grasps); - // if (!support_surface_.empty()) - // goal.allow_gripper_support_collision = true; - // - // if (path_constraints_) - // goal.path_constraints = *path_constraints_; - // - // goal.planner_id = planner_id_; - // goal.allowed_planning_time = allowed_planning_time_; - // - // goal.planning_options.plan_only = plan_only; - // goal.planning_options.look_around = can_look_; - // goal.planning_options.replan = can_replan_; - // goal.planning_options.replan_delay = replan_delay_; - // goal.planning_options.planning_scene_diff.is_diff = true; - // goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - // return goal; - // } - - // moveit_msgs::action::Place::Goal constructPlaceGoal(const std::string& object, - // std::vector&& locations, - // bool plan_only = false) const - // { - // moveit_msgs::action::Place::Goal goal; - // goal.group_name = opt_.group_name; - // goal.attached_object_name = object; - // goal.support_surface_name = support_surface_; - // goal.place_locations = std::move(locations); - // if (!support_surface_.empty()) - // goal.allow_gripper_support_collision = true; - // - // if (path_constraints_) - // goal.path_constraints = *path_constraints_; - // - // goal.planner_id = planner_id_; - // goal.allowed_planning_time = allowed_planning_time_; - // - // goal.planning_options.plan_only = plan_only; - // goal.planning_options.look_around = can_look_; - // goal.planning_options.replan = can_replan_; - // goal.planning_options.replan_delay = replan_delay_; - // goal.planning_options.planning_scene_diff.is_diff = true; - // goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - // return goal; - // } - void setPathConstraints(const moveit_msgs::msg::Constraints& constraint) { path_constraints_ = std::make_unique(constraint); @@ -1308,7 +1248,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::unique_ptr trajectory_constraints_; std::string end_effector_link_; std::string pose_reference_frame_; - std::string support_surface_; // ROS communication rclcpp::Publisher::SharedPtr trajectory_event_publisher_; @@ -1511,46 +1450,6 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) return impl_->plan(plan); } -// moveit_msgs::action::Pickup::Goal MoveGroupInterface::constructPickupGoal(const std::string& object, -// std::vector grasps, -// bool plan_only = false) const -//{ -// return impl_->constructPickupGoal(object, std::move(grasps), plan_only); -//} -// -// moveit_msgs::action::Place::Goal MoveGroupInterface::constructPlaceGoal( -// const std::string& object, std::vector locations, bool plan_only = false) const -//{ -// return impl_->constructPlaceGoal(object, std::move(locations), plan_only); -//} -// -// std::vector -// MoveGroupInterface::posesToPlaceLocations(const std::vector& poses) const -//{ -// return impl_->posesToPlaceLocations(poses); -//} -// -// moveit::core::MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::action::Pickup::Goal& goal) -//{ -// return impl_->pick(goal); -//} -// -// moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) -//{ -// return impl_->planGraspsAndPick(object, plan_only); -//} -// -// moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, -// bool plan_only) -//{ -// return impl_->planGraspsAndPick(object, plan_only); -//} -// -// moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::action::Place::Goal& goal) -//{ -// return impl_->place(goal); -//} - double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) @@ -2310,11 +2209,6 @@ double MoveGroupInterface::getPlanningTime() const return impl_->getPlanningTime(); } -void MoveGroupInterface::setSupportSurfaceName(const std::string& name) -{ - impl_->setSupportSurfaceName(name); -} - const rclcpp::Node::SharedPtr& MoveGroupInterface::getNode() const { return impl_->node_; From da38ad6cd01ebc17e121e913d34c9b334360f8ff Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 13 Oct 2024 03:10:15 -0600 Subject: [PATCH 4/9] Remove plugins from export set (#3024) Signed-off-by: Paul Gesel --- moveit_ros/planning/CMakeLists.txt | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 17071ad226..9b4136d012 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -49,8 +49,6 @@ set(THIS_PACKAGE_LIBRARIES moveit_collision_plugin_loader moveit_constraint_sampler_manager_loader moveit_cpp - moveit_default_planning_request_adapter_plugins - moveit_default_planning_response_adapter_plugins moveit_kinematics_plugin_loader moveit_plan_execution moveit_planning_pipeline @@ -111,6 +109,16 @@ install( INCLUDES DESTINATION include/moveit_ros_planning) +# install plugins as separate export set +install( + TARGETS moveit_default_planning_response_adapter_plugins + moveit_default_planning_request_adapter_plugins + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_ros_planning) + ament_export_targets(moveit_ros_planningTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) From 123be897513553c229d15f1f6a77bab47b7b84b3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 13 Oct 2024 20:23:45 +0200 Subject: [PATCH 5/9] Fix CI issues (#3031) * CI: Build against latest srdfdom * CI: Revert to custom cache action save-always: true was never working --- .github/workflows/ci.yaml | 12 ++++++------ .github/workflows/format.yaml | 2 +- .github/workflows/sonar.yaml | 9 +++++---- moveit2.repos | 4 ++++ 4 files changed, 16 insertions(+), 11 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 23d837195c..7bed61c64d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -105,36 +105,36 @@ jobs: with: file: moveit2.repos - name: Cache upstream workspace - uses: actions/cache@v4 + uses: rhaschke/cache@main with: path: ${{ env.BASEDIR }}/upstream_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} - save-always: true env: + GHA_CACHE_SAVE: always CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - name: Cache target workspace if: "!matrix.env.CCOV" - uses: actions/cache@v4 + uses: rhaschke/cache@main with: path: ${{ env.BASEDIR }}/target_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} - save-always: true env: + GHA_CACHE_SAVE: always CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }} - name: Cache ccache - uses: actions/cache@v4 + uses: rhaschke/cache@main with: path: ${{ env.CCACHE_DIR }} key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ${{ env.CACHE_PREFIX }}-${{ github.sha }} ${{ env.CACHE_PREFIX }} - save-always: true env: + GHA_CACHE_SAVE: always CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }} - name: Configure ccache run: | diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index b55c526b64..1bcc4779c8 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -19,7 +19,7 @@ jobs: - uses: actions/checkout@v4 - uses: actions/setup-python@v5 with: - python-version: '3.10' + python-version: 3.x - name: Install clang-format-14 run: sudo apt-get install clang-format-14 - uses: pre-commit/action@v3.0.1 diff --git a/.github/workflows/sonar.yaml b/.github/workflows/sonar.yaml index 8febc84e0c..513ba386cf 100644 --- a/.github/workflows/sonar.yaml +++ b/.github/workflows/sonar.yaml @@ -72,23 +72,24 @@ jobs: with: file: moveit2.repos - name: Cache upstream workspace - uses: actions/cache@v4 + uses: rhaschke/cache@main with: path: ${{ env.BASEDIR }}/upstream_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} - save-always: true env: + GHA_CACHE_SAVE: always CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-rolling-ci-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} - name: Cache ccache - uses: actions/cache@v4 + uses: rhaschke/cache@main with: path: ${{ env.CCACHE_DIR }} key: ccache-rolling-ci-ccov-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-rolling-ci-ccov-${{ github.sha }} ccache-rolling-ci-ccov - save-always: true + env: + GHA_CACHE_SAVE: always - name: Configure ccache run: | mkdir -p ${{ env.CCACHE_DIR }} diff --git a/moveit2.repos b/moveit2.repos index aec6f2851e..6ce029c513 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/moveit/moveit_resources.git version: ros2 + srdfdom: + type: git + url: https://github.com/moveit/srdfdom.git + version: ros2 From f92f4437b6bb16a9ce4eeff1ad0a0e8e62bc6d9c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 14 Oct 2024 08:23:55 +0200 Subject: [PATCH 6/9] Fix Cartesian interpolation (#3020) --- .../src/cartesian_interpolator.cpp | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 995338badd..71b927daaf 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -285,23 +285,22 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, precision, validCallback, options, cost_function, link_offset); + + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = static_cast(i + 1) / static_cast(waypoints.size()); - std::vector::iterator start = waypoint_traj.begin(); - if (i > 0 && !waypoint_traj.empty()) - std::advance(start, 1); - traj.insert(traj.end(), start, waypoint_traj.end()); } else { percentage_solved += wp_percentage_solved / static_cast(waypoints.size()); - std::vector::iterator start = waypoint_traj.begin(); - if (i > 0 && !waypoint_traj.empty()) - std::advance(start, 1); - traj.insert(traj.end(), start, waypoint_traj.end()); break; } + start_state = traj.back().get(); } return percentage_solved; @@ -490,21 +489,19 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset); #pragma GCC diagnostic pop + + std::vector::iterator start = waypoint_path.begin(); + if (i > 0 && !waypoint_path.empty()) + std::advance(start, 1); + path.insert(path.end(), start, waypoint_path.end()); + if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = static_cast((i + 1)) / static_cast(waypoints.size()); - std::vector::iterator start = waypoint_path.begin(); - if (i > 0 && !waypoint_path.empty()) - std::advance(start, 1); - path.insert(path.end(), start, waypoint_path.end()); } else { percentage_solved += wp_percentage_solved / static_cast(waypoints.size()); - std::vector::iterator start = waypoint_path.begin(); - if (i > 0 && !waypoint_path.empty()) - std::advance(start, 1); - path.insert(path.end(), start, waypoint_path.end()); break; } } From 3a94d101ceb3dd7b4a8544d7a84ee3279a6e4766 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Mon, 14 Oct 2024 08:34:08 +0200 Subject: [PATCH 7/9] Fixed Typo get_trajectory_execution_manager in planning.pyi file (#3029) --- moveit_py/moveit/planning.pyi | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index 09435678ff..4073786524 100644 --- a/moveit_py/moveit/planning.pyi +++ b/moveit_py/moveit/planning.pyi @@ -16,7 +16,7 @@ class MoveItPy: def get_planning_component(self, *args, **kwargs) -> Any: ... def get_planning_scene_monitor(self, *args, **kwargs) -> Any: ... def get_robot_model(self, *args, **kwargs) -> Any: ... - def get_trajactory_execution_manager(self, *args, **kwargs) -> Any: ... + def get_trajectory_execution_manager(self, *args, **kwargs) -> Any: ... def shutdown(self, *args, **kwargs) -> Any: ... class MultiPipelinePlanRequestParameters: From 56d1ab1af8615793ba6be6dd47cca16de1107df6 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Mon, 14 Oct 2024 11:55:08 -0400 Subject: [PATCH 8/9] fix move_group_capabilities usage (#3018) --- moveit_configs_utils/moveit_configs_utils/launches.py | 7 ++++++- .../moveit_configs_utils/moveit_configs_builder.py | 4 +++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index c8e3bb533c..a423395000 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -204,7 +204,12 @@ def generate_move_group_launch(moveit_config): ) ) # inhibit these default MoveGroup capabilities (space separated) - ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value="")) + ld.add_action( + DeclareLaunchArgument( + "disable_capabilities", + default_value=moveit_config.move_group_capabilities["disable_capabilities"], + ) + ) # do not copy dynamics information from /joint_states to internal robot monitoring # default to false, because almost nothing in move_group relies on this information 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 04cb077514..4d547d6209 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -108,7 +108,9 @@ class MoveItConfigs: # A dictionary that has the sensor 3d configuration parameters. sensors_3d: Dict = field(default_factory=dict) # A dictionary containing move_group's non-default capabilities. - move_group_capabilities: Dict = field(default_factory=dict) + move_group_capabilities: Dict = field( + default_factory=lambda: {"capabilities": "", "disable_capabilities": ""} + ) # A dictionary containing the overridden position/velocity/acceleration limits. joint_limits: Dict = field(default_factory=dict) # A dictionary containing MoveItCpp related parameters. From 7db58f80e96fef5217b701cbd3dc64868f31f8a6 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Wed, 16 Oct 2024 03:54:24 +0200 Subject: [PATCH 9/9] Obtain time from node. (#3032) Signed-off-by: Jelmer de Wolde --- moveit_ros/moveit_servo/src/servo.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 329e4a84a8..c62488660f 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -648,8 +648,8 @@ KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const bool have_current_state = false; while (rclcpp::ok() && !have_current_state) { - have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState( - rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */); + have_current_state = + planning_scene_monitor_->getStateMonitor()->waitForCurrentState(node_->now(), ROBOT_STATE_WAIT_TIME /* s */); if (!have_current_state) { RCLCPP_WARN(logger_, "Waiting for the current state");