From 830ceda79b8f67c2224eb9ea8eb37f4aa0fe2596 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 27 Jul 2023 17:38:42 +0200 Subject: [PATCH] Cleanup planning request adapter interface (#2266) * Use default arguments instead of additional functions * Use generate param lib for default plan request adapters * Small cleanup of ResolveConstraintFrames * Remove dublicate yaml file entry * Move list_planning_adapter_plugins into own directory * Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Fix copy& paste error * Update parameter descriptions Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Apply suggestions from code review Co-authored-by: Kyle Cesare * EMPTY_PATH_INDEX_VECTOR -> empty_path_index_vector * Update parameter yaml * Make param listener unique * Fix build error * Use gt_eq instead of deprecated lower_bounds --------- Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Kyle Cesare --- .../planning_request_adapter.h | 82 +++++-------------- .../src/planning_request_adapter.cpp | 18 ---- moveit_ros/planning/CMakeLists.txt | 14 ++-- .../planning/introspection/CMakeLists.txt | 10 +++ .../src/list_planning_adapter_plugins.cpp} | 5 +- .../CMakeLists.txt | 18 +--- .../default_plan_request_adapter_params.yaml | 45 ++++++++++ .../src/add_ruckig_traj_smoothing.cpp | 4 - .../src/add_time_optimal_parameterization.cpp | 18 ++-- .../src/fix_start_state_bounds.cpp | 29 +++---- .../src/fix_start_state_collision.cpp | 43 ++++------ .../src/fix_start_state_path_constraints.cpp | 4 - .../src/fix_workspace_bounds.cpp | 23 +++--- .../src/resolve_constraint_frames.cpp | 6 +- 14 files changed, 138 insertions(+), 181 deletions(-) create mode 100644 moveit_ros/planning/introspection/CMakeLists.txt rename moveit_ros/planning/{planning_request_adapter_plugins/src/list.cpp => introspection/src/list_planning_adapter_plugins.cpp} (94%) create mode 100644 moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 9569edc8e1..1d2d8e762a 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -46,6 +46,10 @@ /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter { +namespace +{ +std::vector empty_path_index_vector = {}; +} MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc /** @brief Concept in MoveIt which can be used to modify the planning problem and resulting trajectory (pre-processing @@ -60,8 +64,6 @@ class PlanningRequestAdapter std::function; - virtual ~PlanningRequestAdapter() = default; - /** \brief Initialize parameters using the passed Node and parameter namespace. * @param node Node instance used by the adapter * @param parameter_namespace Parameter namespace for adapter @@ -71,23 +73,7 @@ class PlanningRequestAdapter /** \brief Get a description of this adapter * @return Returns a short string that identifies the planning request adapter */ - virtual std::string getDescription() const - { - return ""; - } - - /** \brief Adapt the planning request if needed, call the planner - function \e planner and update the planning response if - needed. - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; + [[nodiscard]] virtual std::string getDescription() const = 0; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -103,10 +89,11 @@ class PlanningRequestAdapter current state itself appears to touch obstacles). This is helpful because the added states should not be considered invalid in all situations. * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const; + [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index = empty_path_index_vector) const; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -122,31 +109,10 @@ class PlanningRequestAdapter current state itself appears to touch obstacles). This is helpful because the added states should not be considered invalid in all situations. * @return True if response got generated correctly */ - virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const = 0; - -protected: - /** \brief Helper param for getting a parameter using a namespace **/ - template - T getParam(const rclcpp::Node::SharedPtr& node, const rclcpp::Logger& logger, const std::string& parameter_namespace, - const std::string& parameter_name, T default_value = {}) const - { - std::string full_name = parameter_namespace.empty() ? parameter_name : parameter_namespace + "." + parameter_name; - T value; - if (!node->get_parameter(full_name, value)) - { - RCLCPP_INFO(logger, "Param '%s' was not set. Using default value: %s", full_name.c_str(), - std::to_string(default_value).c_str()); - return default_value; - } - else - { - RCLCPP_INFO(logger, "Param '%s' was set to %s", full_name.c_str(), std::to_string(value).c_str()); - return value; - } - } + [[nodiscard]] virtual bool + adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + std::vector& added_path_index = empty_path_index_vector) const = 0; }; /** \brief Apply a sequence of adapters to a motion plan */ @@ -157,17 +123,6 @@ class PlanningRequestAdapterChain * @param adapter Adapter to be added */ void addAdapter(const PlanningRequestAdapterConstPtr& adapter); - /** \brief Iterate through the chain and call all adapters and planners in the correct order - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; - /** \brief Iterate through the chain and call all adapters and planners in the correct order * @param planner Pointer to the planner used to solve the passed problem * @param planning_scene Representation of the environment for the planning @@ -178,10 +133,11 @@ class PlanningRequestAdapterChain current state itself appears to touch obstacles). This is helpful because the added states should not be considered invalid in all situations. * @return True if response got generated correctly */ - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const; + [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index = empty_path_index_vector) const; private: std::vector adapters_; 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 5144964e03..0a7a7bddfc 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -98,29 +98,11 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag planning_scene, req, res, added_path_index); } -bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - std::vector empty_added_path_index; - return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index); -} - void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter) { adapters_.push_back(adapter); } -bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - std::vector empty_added_path_index; - return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index); -} - bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 20c39ad0d8..4415d4e311 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -52,6 +52,7 @@ set(THIS_PACKAGE_LIBRARIES moveit_plan_execution moveit_planning_scene_monitor moveit_collision_plugin_loader + default_plan_request_adapter_parameters moveit_default_planning_request_adapter_plugins moveit_cpp ) @@ -79,19 +80,20 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) -add_subdirectory(rdf_loader) add_subdirectory(collision_plugin_loader) -add_subdirectory(kinematics_plugin_loader) -add_subdirectory(robot_model_loader) add_subdirectory(constraint_sampler_manager_loader) +add_subdirectory(introspection) +add_subdirectory(kinematics_plugin_loader) +add_subdirectory(moveit_cpp) +add_subdirectory(plan_execution) +add_subdirectory(planning_components_tools) add_subdirectory(planning_pipeline) add_subdirectory(planning_pipeline_interfaces) add_subdirectory(planning_request_adapter_plugins) add_subdirectory(planning_scene_monitor) -add_subdirectory(planning_components_tools) +add_subdirectory(rdf_loader) +add_subdirectory(robot_model_loader) add_subdirectory(trajectory_execution_manager) -add_subdirectory(plan_execution) -add_subdirectory(moveit_cpp) install( TARGETS ${THIS_PACKAGE_LIBRARIES} diff --git a/moveit_ros/planning/introspection/CMakeLists.txt b/moveit_ros/planning/introspection/CMakeLists.txt new file mode 100644 index 0000000000..23b5640b15 --- /dev/null +++ b/moveit_ros/planning/introspection/CMakeLists.txt @@ -0,0 +1,10 @@ +add_executable(moveit_list_planning_adapter_plugins src/list_planning_adapter_plugins.cpp) +ament_target_dependencies(moveit_list_planning_adapter_plugins + moveit_core + rclcpp + pluginlib +) + +install(TARGETS moveit_list_planning_adapter_plugins + DESTINATION lib/${PROJECT_NAME} +) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/list.cpp b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp similarity index 94% rename from moveit_ros/planning/planning_request_adapter_plugins/src/list.cpp rename to moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp index 9eb674ed78..30a1793966 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/list.cpp +++ b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp @@ -32,7 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan + * Desc: Simple executable to list the loadable PlanningRequestAdapter. To use it simply run: + * `ros2 run moveit_ros_planning moveit_list_planning_adapter_plugins` + */ #include #include diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 277e653311..bb046af912 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -1,4 +1,6 @@ -set(SOURCE_FILES +generate_parameter_library(default_plan_request_adapter_parameters res/default_plan_request_adapter_params.yaml) + +add_library(moveit_default_planning_request_adapter_plugins SHARED src/empty.cpp src/fix_start_state_bounds.cpp src/fix_start_state_collision.cpp @@ -9,7 +11,7 @@ set(SOURCE_FILES src/resolve_constraint_frames.cpp ) -add_library(moveit_default_planning_request_adapter_plugins SHARED ${SOURCE_FILES}) +target_link_libraries(moveit_default_planning_request_adapter_plugins default_plan_request_adapter_parameters) set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_default_planning_request_adapter_plugins @@ -18,15 +20,3 @@ ament_target_dependencies(moveit_default_planning_request_adapter_plugins rclcpp pluginlib ) - -add_executable(moveit_list_request_adapter_plugins src/list.cpp) -ament_target_dependencies(moveit_list_request_adapter_plugins - Boost - moveit_core - rclcpp - pluginlib -) - -install(TARGETS moveit_list_request_adapter_plugins - DESTINATION lib/${PROJECT_NAME} -) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml new file mode 100644 index 0000000000..b3f4d79b1b --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml @@ -0,0 +1,45 @@ +# This files contains the parameters for all of MoveIt's default PlanRequestAdapters +default_plan_request_adapter_parameters: + path_tolerance: { + type: double, + description: "AddTimeOptimalParameterization: Tolerance per joint in which the time parameterization is allowed to deviate from the original path.", + default_value: 0.1, + } + resample_dt: { + type: double, + description: "AddTimeOptimalParameterization: Time steps between two adjacent waypoints of the parameterized trajectory. The trajectory is re-sampled from the original path.", + default_value: 0.1, + } + min_angle_change: { + type: double, + description: "AddTimeOptimalParameterization: Minimum joint value change to consider two waypoints unique.", + default_value: 0.001, + } + start_state_max_dt: { + type: double, + description: "FixStartStateCollision/FixStartStateBounds: Maximum temporal distance of the fixed start state from the original state.", + default_value: 0.5, + } + jiggle_fraction: { + type: double, + description: "FixStartStateCollision: Joint value perturbation as a percentage of the total range of motion for the joint.", + default_value: 0.02, + } + max_sampling_attempts: { + type: int, + description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.", + default_value: 100, + validation: { + gt_eq<>: [ 0 ], + } + } + start_state_max_bounds_error: { + type: double, + description: "FixStartStateBounds: Maximum allowable error outside joint bounds for the starting configuration.", + default_value: 0.05, + } + default_workspace_bounds: { + type: double, + description: "FixWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", + default_value: 10.0, + } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index b27a8203c4..950ade9c60 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -52,10 +52,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_traj_smo class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRequestAdapter { public: - AddRuckigTrajectorySmoothing() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& /* unused */, const std::string& /* unused */) override { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 6ee7c21974..3e2dec6593 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -38,6 +38,8 @@ #include #include +#include + namespace default_planner_request_adapters { using namespace trajectory_processing; @@ -48,15 +50,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_time_opt class AddTimeOptimalParameterization : public planning_request_adapter::PlanningRequestAdapter { public: - AddTimeOptimalParameterization() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - path_tolerance_ = getParam(node, LOGGER, parameter_namespace, "path_tolerance", 0.1); - resample_dt_ = getParam(node, LOGGER, parameter_namespace, "resample_dt", 0.1); - min_angle_change_ = getParam(node, LOGGER, parameter_namespace, "min_angle_change", 0.001); + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -72,7 +69,8 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning if (result && res.trajectory) { RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str()); - TimeOptimalTrajectoryGeneration totg(path_tolerance_, resample_dt_, min_angle_change_); + const auto params = param_listener_->get_params(); + TimeOptimalTrajectoryGeneration totg(params.path_tolerance, params.resample_dt, params.min_angle_change); if (!totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed."); @@ -84,9 +82,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning } protected: - double path_tolerance_; - double resample_dt_; - double min_angle_change_; + std::unique_ptr param_listener_; }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index 9a8f572e83..c68dd6bb04 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -53,6 +53,8 @@ #include #include +#include + namespace default_planner_request_adapters { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_bounds"); @@ -61,14 +63,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdapter { public: - static const std::string BOUNDS_PARAM_NAME; - static const std::string DT_PARAM_NAME; - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - node_ = node; - bounds_dist_ = getParam(node_, LOGGER, parameter_namespace, BOUNDS_PARAM_NAME, 0.05); - max_dt_offset_ = getParam(node_, LOGGER, parameter_namespace, DT_PARAM_NAME, 0.5); + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -137,13 +135,16 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } } + // Read parameters + const auto params = param_listener_->get_params(); + // pointer to a prefix state we could possibly add, if we detect we have to make changes moveit::core::RobotStatePtr prefix_state; for (const moveit::core::JointModel* jmodel : jmodels) { if (!start_state.satisfiesBounds(jmodel)) { - if (start_state.satisfiesBounds(jmodel, bounds_dist_)) + if (start_state.satisfiesBounds(jmodel, params.start_state_max_bounds_error)) { if (!prefix_state) prefix_state = std::make_shared(start_state); @@ -168,9 +169,10 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } RCLCPP_WARN(LOGGER, "Joint '%s' from the starting state is outside bounds by a significant margin: [%s] should be in " - "the range [%s], [%s] but the error above the ~%s parameter (currently set to %f)", + "the range [%s], [%s] but the error above the 'start_state_max_bounds_error' parameter " + "(currently set to %f)", jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(), - joint_bounds_hi.str().c_str(), BOUNDS_PARAM_NAME.c_str(), bounds_dist_); + joint_bounds_hi.str().c_str(), params.start_state_max_bounds_error); } } } @@ -191,7 +193,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to // the computed trajectory) - res.trajectory->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory->setWayPointDurationFromPrevious(0, std::min(params.start_state_max_dt, res.trajectory->getAverageSegmentDuration())); res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions @@ -204,13 +206,8 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } private: - rclcpp::Node::SharedPtr node_; - double bounds_dist_; - double max_dt_offset_; + std::unique_ptr param_listener_; }; - -const std::string FixStartStateBounds::BOUNDS_PARAM_NAME = "start_state_max_bounds_error"; -const std::string FixStartStateBounds::DT_PARAM_NAME = "start_state_max_dt"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateBounds, 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 d01b0c0587..7e8d3a361c 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 @@ -49,6 +49,8 @@ #include #include +#include + namespace default_planner_request_adapters { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_state_collision"); @@ -58,21 +60,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStateCollision : public planning_request_adapter::PlanningRequestAdapter { public: - static const std::string DT_PARAM_NAME; - static const std::string JIGGLE_PARAM_NAME; - static const std::string ATTEMPTS_PARAM_NAME; - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - node_ = node; - max_dt_offset_ = getParam(node_, LOGGER, parameter_namespace, DT_PARAM_NAME, 0.5); - jiggle_fraction_ = getParam(node_, LOGGER, parameter_namespace, JIGGLE_PARAM_NAME, 0.02); - sampling_attempts_ = getParam(node_, LOGGER, parameter_namespace, ATTEMPTS_PARAM_NAME, 100); - if (sampling_attempts_ < 1) - { - sampling_attempts_ = 1; - RCLCPP_WARN(LOGGER, "Param '%s' needs to be at least 1.", ATTEMPTS_PARAM_NAME.c_str()); - } + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -120,14 +111,16 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA planning_scene->getRobotModel()->getJointModels(); bool found = false; - for (int c = 0; !found && c < sampling_attempts_; ++c) + const auto params = param_listener_->get_params(); + + for (int c = 0; !found && c < params.max_sampling_attempts; ++c) { for (std::size_t i = 0; !found && i < jmodels.size(); ++i) { std::vector sampled_variable_values(jmodels[i]->getVariableCount()); const double* original_values = prefix_state->getJointPositions(jmodels[i]); jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, - jmodels[i]->getMaximumExtent() * jiggle_fraction_); + jmodels[i]->getMaximumExtent() * params.jiggle_fraction); start_state.setJointPositions(jmodels[i], sampled_variable_values); collision_detection::CollisionResult cres; planning_scene->checkCollision(creq, cres, start_state); @@ -149,7 +142,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a // prefix to the computed trajectory) - res.trajectory->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory->setWayPointDurationFromPrevious(0, std::min(params.start_state_max_dt, res.trajectory->getAverageSegmentDuration())); res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions @@ -161,10 +154,11 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA } else { - RCLCPP_WARN(LOGGER, - "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling " - "attempts). Passing the original planning request to the planner.", - jiggle_fraction_, sampling_attempts_); + RCLCPP_WARN( + LOGGER, + "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %lu sampling " + "attempts). Passing the original planning request to the planner.", + params.jiggle_fraction, params.max_sampling_attempts); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; return false; // skip remaining adapters and/or planner } @@ -184,15 +178,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA } private: - rclcpp::Node::SharedPtr node_; - double max_dt_offset_; - double jiggle_fraction_; - int sampling_attempts_; + std::unique_ptr param_listener_; }; - -const std::string FixStartStateCollision::DT_PARAM_NAME = "start_state_max_dt"; -const std::string FixStartStateCollision::JIGGLE_PARAM_NAME = "jiggle_fraction"; -const std::string FixStartStateCollision::ATTEMPTS_PARAM_NAME = "max_sampling_attempts"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateCollision, diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index b94712847d..d628ef9bf4 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -59,10 +59,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter { public: - FixStartStatePathConstraints() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index eab718442a..35fbbb6134 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -42,7 +42,8 @@ #include #include #include -#include + +#include namespace default_planner_request_adapters { @@ -53,13 +54,10 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_workspac class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapter { public: - static const std::string WBOUNDS_PARAM_NAME; - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - node_ = node; - workspace_extent_ = getParam(node_, LOGGER, parameter_namespace, WBOUNDS_PARAM_NAME, 10.0); - workspace_extent_ /= 2.0; + param_listener_ = + std::make_unique(node, parameter_namespace); } std::string getDescription() const override @@ -80,8 +78,12 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt RCLCPP_DEBUG(LOGGER, "It looks like the planning volume was not specified. Using default values."); planning_interface::MotionPlanRequest req2 = req; moveit_msgs::msg::WorkspaceParameters& default_wp = req2.workspace_parameters; - default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = -workspace_extent_; - default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = workspace_extent_; + const auto params = param_listener_->get_params(); + + default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = + -params.default_workspace_bounds / 2.0; + default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = + params.default_workspace_bounds / 2.0; return planner(planning_scene, req2, res); } else @@ -91,11 +93,8 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt } private: - rclcpp::Node::SharedPtr node_; - double workspace_extent_; + std::unique_ptr param_listener_; }; - -const std::string FixWorkspaceBounds::WBOUNDS_PARAM_NAME = "default_workspace_bounds"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds, diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index b8c82c2b05..a259d4404c 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -45,10 +45,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.resolve_cons class ResolveConstraintFrames : public planning_request_adapter::PlanningRequestAdapter { public: - ResolveConstraintFrames() : planning_request_adapter::PlanningRequestAdapter() - { - } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override { } @@ -66,7 +62,9 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest planning_interface::MotionPlanRequest modified = req; kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints); for (moveit_msgs::msg::Constraints& constraint : modified.goal_constraints) + { kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); + } return planner(planning_scene, modified, res); } };