Register default projection for OMPL constrained planner

moveit · May 19, 2021 · d64fecd · d64fecd
1 parent 3b6e0d1
commit d64fecd
Showing 1 changed file with 13 additions and 4 deletions.
diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
@@ -154,14 +154,23 @@ void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::Sh
 
 void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std::string& peval)
 {
-  if (!spec_.state_space_)
+  if (spec_.state_space_)
+  {
+    ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval);
+    if (projection_eval)
+      spec_.state_space_->registerDefaultProjection(projection_eval);
+  }
+  else if (spec_.constrained_state_space_)
+  {
+    ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval);
+    if (projection_eval)
+      spec_.constrained_state_space_->registerDefaultProjection(projection_eval);
+  }
+  else
   {
     RCLCPP_ERROR(LOGGER, "No state space is configured yet");
     return;
   }
-  ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval);
-  if (projection_eval)
-    spec_.state_space_->registerDefaultProjection(projection_eval);
 }
 
 ompl::base::ProjectionEvaluatorPtr