Handle unsupported position constraints in OMPL

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

@@ -364,48 +364,61 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo
                                                 const std::string& group,
                                                 const moveit_msgs::msg::Constraints& constraints)
 {
-  // TODO(bostoncleek): does this reach the end w/o a return ?
-
-  const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount();
-  const std::size_t num_pos_con = constraints.position_constraints.size();
-  const std::size_t num_ori_con = constraints.orientation_constraints.size();
-
   // This factory method contains template code to support position and/or orientation constraints.
   // If the specified constraints are invalid, a nullptr is returned.
   std::vector<ompl::base::ConstraintPtr> ompl_constraints;
-  if (num_pos_con > 1)
-  {
-    RCLCPP_WARN(LOGGER, "Only a single position constraint is supported. Using the first one.");
-  }
-  if (num_ori_con > 1)
-  {
-    RCLCPP_WARN(LOGGER, "Only a single orientation constraint is supported. Using the first one.");
-  }
-  if (num_pos_con > 0)
+  const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount();
+
+  // Parse Position Constraints
+  if (!constraints.position_constraints.empty())
   {
-    BaseConstraintPtr pos_con;
-    if (constraints.name == "use_equality_constraints")
+    if (constraints.position_constraints.size() > 1)
+    {
+      RCLCPP_WARN(LOGGER, "Only a single position constraint is supported. Using the first one.");
+    }
+
+    const auto& primitives = constraints.position_constraints.at(0).constraint_region.primitives;
+    if (primitives.empty() || primitives.at(0).type != shape_msgs::msg::SolidPrimitive::BOX)
     {
-      pos_con = std::make_shared<EqualityPositionConstraint>(robot_model, group, num_dofs);
+      RCLCPP_ERROR(LOGGER, "Unable to plan with the requested position constraint. "
+                           "Only BOX primitive shapes are supported as constraint region.");
     }
     else
     {
-      pos_con = std::make_shared<BoxConstraint>(robot_model, group, num_dofs);
+      BaseConstraintPtr pos_con;
+      if (constraints.name == "use_equality_constraints")
+      {
+        pos_con = std::make_shared<EqualityPositionConstraint>(robot_model, group, num_dofs);
+      }
+      else
+      {
+        pos_con = std::make_shared<BoxConstraint>(robot_model, group, num_dofs);
+      }
+      pos_con->init(constraints);
+      ompl_constraints.emplace_back(pos_con);
     }
-    pos_con->init(constraints);
-    ompl_constraints.emplace_back(pos_con);
   }
-  if (num_ori_con > 0)
+
+  // Parse Orientation Constraints
+  if (!constraints.orientation_constraints.empty())
   {
+    if (constraints.orientation_constraints.size() > 1)
+    {
+      RCLCPP_WARN(LOGGER, "Only a single orientation constraint is supported. Using the first one.");
+    }
+
     auto ori_con = std::make_shared<OrientationConstraint>(robot_model, group, num_dofs);
     ori_con->init(constraints);
     ompl_constraints.emplace_back(ori_con);
   }
-  if (num_pos_con < 1 && num_ori_con < 1)
+
+  // Check if we have any constraints to plan with
+  if (ompl_constraints.empty())
   {
-    RCLCPP_ERROR(LOGGER, "No path constraints found in planning request.");
+    RCLCPP_ERROR(LOGGER, "Failed to parse any supported path constraints from planning request.");
     return nullptr;
   }
+
   return std::make_shared<ompl::base::ConstraintIntersection>(num_dofs, ompl_constraints);
 }
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

@@ -374,6 +374,12 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf
       ompl::base::ConstraintPtr ompl_constraint =
           createOMPLConstraints(robot_model_, config.group, req.path_constraints);
 
+      // Fail if ompl constraints could not be parsed successfully
+      if (!ompl_constraint)
+      {
+        return ModelBasedPlanningContextPtr();
+      }
+
       // Create a constrained state space of type "projected state space".
       // Other types are available, so we probably should add another setting to ompl_planning.yaml
       // to choose between them.