Add warning if more than one position primitive is used

henningkayser · Dec 15, 2023 · 5c6f611 · 5c6f611
1 parent 2daca07
commit 5c6f611
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
@@ -385,6 +385,10 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo
  }
 
  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)
  {
  RCLCPP_ERROR(getLogger(), "Unable to plan with the requested position constraint. "