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 7aaa6ac113f..b5121f1ad4b 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 @@ -113,27 +113,6 @@ class PlanningRequestAdapter 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; - -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; - } - } }; /** \brief Apply a sequence of adapters to a motion plan */ diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 277e6533110..8714213605c 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/plan_request_adapter_param.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 @@ -27,6 +29,6 @@ ament_target_dependencies(moveit_list_request_adapter_plugins pluginlib ) -install(TARGETS moveit_list_request_adapter_plugins +install(TARGETS moveit_list_request_adapter_plugins default_plan_request_adapter_parameters DESTINATION lib/${PROJECT_NAME} ) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/plan_request_adapter_param.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/plan_request_adapter_param.yaml new file mode 100644 index 00000000000..dc3710a7a26 --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/res/plan_request_adapter_param.yaml @@ -0,0 +1,46 @@ +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 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: ?", + default_value: 0.001, + } + start_state_max_dt: { + type: double, + description: "FixStartStateCollision: Maximum temporal distance of the fixed start state from the original state.", + default_value: 0.5, + } + jiggle_fraction: { + type: double, + description: "FixStartStateCollision: ?", + default_value: 0.02, + } + max_sampling_attempts: { + type: double, + description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.", + default_value: 100, + } + start_state_max_bounds_error: { + type: double, + description: "FixStartStateBounds: ?", + default_value: 0.05, + } + start_state_max_dt: { + type: double, + description: "FixStartStateBounds: ?", + default_value: 0.5, + } + default_workspace_bounds: { + type: double, + description: "FixWorkspaceBounds: Default workspace bounds representing a 10m x 10m x 10m box around the robot's origin.", + 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 b27a8203c40..950ade9c606 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 6ee7c21974b..4ccf37b550b 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 @@ -48,10 +48,6 @@ 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); 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 9a8f572e83f..fec10c0d954 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 @@ -64,6 +64,9 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap static const std::string BOUNDS_PARAM_NAME; static const std::string DT_PARAM_NAME; + const std::string FixStartStateBounds::BOUNDS_PARAM_NAME = "start_state_max_bounds_error"; + const std::string FixStartStateBounds::DT_PARAM_NAME = "start_state_max_dt"; + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { node_ = node; @@ -208,9 +211,6 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap double bounds_dist_; double max_dt_offset_; }; - -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 d01b0c05875..444a4107b8c 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 @@ -62,6 +62,10 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA static const std::string JIGGLE_PARAM_NAME; static const std::string ATTEMPTS_PARAM_NAME; + 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"; + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { node_ = node; @@ -189,10 +193,6 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA double jiggle_fraction_; int sampling_attempts_; }; - -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 b94712847dc..d628ef9bf4c 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 eab718442a8..9dcfebb8864 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,12 +54,13 @@ 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); + const auto param_listener_ = + std::make_shared(node, parameter_namespace); + const auto params = param_listener_->get_params(); + workspace_extent_ = params.default_workspace_bounds; workspace_extent_ /= 2.0; } @@ -94,8 +96,6 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt rclcpp::Node::SharedPtr node_; double workspace_extent_; }; - -const std::string FixWorkspaceBounds::WBOUNDS_PARAM_NAME = "default_workspace_bounds"; } // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds,