Add assertion mechanism If shape is invalid

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

@@ -390,6 +390,15 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p
     std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.primitives[i]));
     if (shape)
     {
+      for(std::size_t j = 0; i < pc.constraint_region.primitives[i].dimensions.size(); ++j) 
+      { 
+        if(pc.constraint_region.primitives[i].dimensions[j] <= 0)
+        {
+          RCLCPP_WARN(LOGGER, "Constraint reigon primitive shape is invalid");
+          continue;
+        } 
+      }
+
       if (pc.constraint_region.primitive_poses.size() <= i)
       {
         RCLCPP_WARN(LOGGER, "Constraint region message does not contain enough primitive poses");

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -798,6 +798,18 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
     return motion_feasibility_;
   }
 
+  /** \brief Check if a given shape is valid to prevent segmentation fault at
+   * shapesAndPosesFromCollisionObjectMessage(). Return false if any dimension is negative */
+  bool isShapeValid(const shape_msgs::msg::SolidPrimitive& shape);
+
+  /** \brief Check if a given shape is valid to prevent segmentation fault at
+   * shapesAndPosesFromCollisionObjectMessage(). */
+  bool isShapeValid(const shape_msgs::msg::Mesh& shape);
+
+  /** \brief Check if a given shape is valid to prevent segmentation fault at
+   * shapesAndPosesFromCollisionObjectMessage(). */
+  bool isShapeValid(const shape_msgs::msg::Plane& shape);
+
   /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by
    * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */
   bool isStateFeasible(const moveit_msgs::msg::RobotState& state, bool verbose = false) const;

moveit_core/planning_scene/src/planning_scene.cpp

@@ -1738,16 +1738,23 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::
     shapes.emplace_back(shapes::ShapeConstPtr(s));
   };
 
-  auto treat_shape_vectors = [&append](const auto& shape_vector,        // the shape_msgs of each type
-                                       const auto& shape_poses_vector,  // std::vector<const geometry_msgs::Pose>
-                                       const std::string& shape_type) {
+  auto treat_shape_vectors = [&append, this](const auto& shape_vector,        // the shape_msgs of each type
+                                             const auto& shape_poses_vector,  // std::vector<const geometry_msgs::Pose>
+                                             const std::string& shape_type) {
     if (shape_vector.size() > shape_poses_vector.size())
     {
       RCLCPP_DEBUG_STREAM(LOGGER, "Number of " << shape_type
                                                << " does not match number of poses "
                                                   "in collision object message. Assuming identity.");
       for (std::size_t i = 0; i < shape_vector.size(); ++i)
       {
+        if(!isShapeValid(shape_vector[i]))
+        {
+          RCLCPP_ERROR_STREAM(LOGGER, "Shape type " << shape_type 
+                                                        << " is invalid.");
+          return false;
+        }
+
         if (i >= shape_poses_vector.size())
         {
           append(shapes::constructShapeFromMsg(shape_vector[i]),
@@ -1762,12 +1769,13 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::
       for (std::size_t i = 0; i < shape_vector.size(); ++i)
         append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
     }
+
+    return true;
   };
 
-  treat_shape_vectors(object.primitives, object.primitive_poses, std::string("primitive_poses"));
-  treat_shape_vectors(object.meshes, object.mesh_poses, std::string("meshes"));
-  treat_shape_vectors(object.planes, object.plane_poses, std::string("planes"));
-  return true;
+  return treat_shape_vectors(object.primitives, object.primitive_poses, std::string("primitive_poses")) &&
+         treat_shape_vectors(object.meshes, object.mesh_poses, std::string("meshes")) &&
+         treat_shape_vectors(object.planes, object.plane_poses, std::string("planes"));
 }
 
 bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object)
@@ -2040,6 +2048,25 @@ bool PlanningScene::isStateColliding(const std::string& group, bool verbose)
   }
 }
 
+bool PlanningScene::isShapeValid(const shape_msgs::msg::SolidPrimitive& shape)
+{
+  for(std::size_t i = 0; i < shape.dimensions.size(); ++i)
+  {
+    if(shape.dimensions[i] <= 0) { return false; }
+  }
+  return true;
+}
+
+bool PlanningScene::isShapeValid(const shape_msgs::msg::Mesh& shape)
+{
+  return true;
+}
+
+bool PlanningScene::isShapeValid(const shape_msgs::msg::Plane& shape)
+{
+  return true;
+}
+
 bool PlanningScene::isStateColliding(const moveit::core::RobotState& state, const std::string& group, bool verbose) const
 {
   collision_detection::CollisionRequest req;