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 228254e2bc..76bff83da8 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 @@ -178,6 +178,9 @@ class PlanningComponent /** \brief Set the goal constraints generated from a named target state */ bool setGoal(const std::string& named_target); + /** \brief Set the path constraints generated from a moveit msg Constraints */ + bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ PlanSolution plan(); @@ -205,6 +208,7 @@ class PlanningComponent // The start state used in the planning motion request moveit::core::RobotStatePtr considered_start_state_; std::vector current_goal_constraints_; + moveit_msgs::msg::Constraints current_path_constraints_; PlanRequestParameters plan_request_parameters_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; bool workspace_parameters_set_ = false; diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 835be69687..f065640b92 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -107,6 +107,12 @@ const std::string& PlanningComponent::getPlanningGroupName() const return group_name_; } +bool PlanningComponent::setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints) +{ + current_path_constraints_ = path_constraints; + return true; +} + PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) { last_plan_solution_.reset(new PlanSolution()); @@ -155,6 +161,8 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet } req.goal_constraints = current_goal_constraints_; + req.path_constraints = current_path_constraints_; + // Run planning attempt ::planning_interface::MotionPlanResponse res; if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end())