MoveitCpp - added ability to set path constraints for PlanningCompone…

…nt. (#2959)
moveit · Nov 16, 2021 · 928afab · 928afab
1 parent d0d76f1
commit 928afab
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
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
@@ -175,6 +175,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 used for planning */
+  bool setPathConstraints(const moveit_msgs::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();
@@ -202,6 +205,7 @@ class PlanningComponent
   // The start state used in the planning motion request
   moveit::core::RobotStatePtr considered_start_state_;
   std::vector<moveit_msgs::Constraints> current_goal_constraints_;
+  moveit_msgs::Constraints current_path_constraints_;
   PlanRequestParameters plan_request_parameters_;
   moveit_msgs::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
@@ -99,6 +99,12 @@ const std::string& PlanningComponent::getPlanningGroupName() const
   return group_name_;
 }
 
+bool PlanningComponent::setPathConstraints(const moveit_msgs::Constraints& path_constraints)
+{
+  current_path_constraints_ = path_constraints;
+  return true;
+}
+
 PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters)
 {
   last_plan_solution_ = std::make_shared<PlanSolution>();
@@ -147,6 +153,9 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   }
   req.goal_constraints = current_goal_constraints_;
 
+  // Set path 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())