Handle unsupported position constraints in OMPL (#2417)

* Handle unsupported position constraints in OMPL

OMPL constrained planning assumes that all position constraints have three
dimensions, meaning that they are represented by a BOX bounding volume.
If another shape is used (like a SPHERE from moveit_core/kinematic_constraints/utils.hpp),
the constraint adapter implementation will produce a segfault because of
the lack of dimensions. This fix prevents this by checking for the
required BOX type.

* Add warning if more than one position primitive is used

---------

Co-authored-by: Sebastian Jahr <[email protected]>
(cherry picked from commit b0401e9)

# Conflicts:
#	moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

Author: henningkayser

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

@@ -364,17 +364,15 @@ 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)
+  const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount();
+
+  // Parse Position Constraints
+  if (!constraints.position_constraints.empty())
   {
+<<<<<<< HEAD
     RCLCPP_WARN(LOGGER, "Only a single position constraint is supported. Using the first one.");
   }
   if (num_ori_con > 1)
@@ -385,27 +383,63 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo
   {
     BaseConstraintPtr pos_con;
     if (constraints.name == "use_equality_constraints")
+=======
+    if (constraints.position_constraints.size() > 1)
+>>>>>>> b0401e91a (Handle unsupported position constraints in OMPL (#2417))
+    {
+      RCLCPP_WARN(getLogger(), "Only a single position constraint is supported. Using the first one.");
+    }
+
+    const auto& primitives = constraints.position_constraints.at(0).constraint_region.primitives;
+    if (primitives.size() > 1)
+    {
+      RCLCPP_WARN(getLogger(), "Only a single position primitive is supported. Using the first one.");
+    }
+    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(getLogger(), "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(getLogger(), "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())
   {
+<<<<<<< HEAD
     RCLCPP_ERROR(LOGGER, "No path constraints found in planning request.");
+=======
+    RCLCPP_ERROR(getLogger(), "Failed to parse any supported path constraints from planning request.");
+>>>>>>> b0401e91a (Handle unsupported position constraints in OMPL (#2417))
     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

@@ -372,6 +372,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.