diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp index 2b411448ea..9bf8f42fa1 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -371,48 +371,65 @@ 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_constraints; - if (num_pos_con > 1) - { - RCLCPP_WARN(getLogger(), "Only a single position constraint is supported. Using the first one."); - } - if (num_ori_con > 1) - { - RCLCPP_WARN(getLogger(), "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(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) { - pos_con = std::make_shared(robot_model, group, num_dofs); + 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) + { + 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(robot_model, group, num_dofs); + BaseConstraintPtr pos_con; + if (constraints.name == "use_equality_constraints") + { + pos_con = std::make_shared(robot_model, group, num_dofs); + } + else + { + pos_con = std::make_shared(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(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(getLogger(), "No path constraints found in planning request."); + RCLCPP_ERROR(getLogger(), "Failed to parse any supported path constraints from planning request."); return nullptr; } + return std::make_shared(num_dofs, ompl_constraints); } } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index bb435c6a4c..b6424fcd7c 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -381,6 +381,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.