MoveitCpp - path constraints added from PlanningComponent (backport #752

) (#781)

Co-authored-by: Marco Lapolla <marco.lapolla5@gmail.com>

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<moveit_msgs::msg::Constraints> 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;

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())