fix: apply scale and padding to mesh collision bodies

include/robot_self_filter/bodies.h

@@ -253,6 +253,21 @@ class ConvexMesh : public Body
   const std::vector<unsigned int> &getTriangles() const { return m_triangles; }
   const std::vector<tf2::Vector3> &getScaledVertices() const { return m_scaledVertices; }
 
+  // A mesh takes one scale and one padding, the same way a sphere does.
+  void setScale(double s)
+  {
+    // The containment test divides by this, so a non-positive value would turn the
+    // padding term into infinity, report every point as inside, and silently delete
+    // the whole cloud. Fall back to unscaled rather than doing that.
+    m_scale = (s > 0.0) ? s : 1.0;
+    updateInternalData();
+  }
+  void setPadding(double p)
+  {
+    m_padding = p;
+    updateInternalData();
+  }
+
 protected:
   void useDimensions(const shapes::Shape *shape) override;
   void updateInternalData() override;
@@ -265,6 +280,8 @@ class ConvexMesh : public Body
   std::vector<tf2::Vector3>    m_scaledVertices;
   std::vector<unsigned int>    m_triangles;
   tf2::Transform               m_iPose;
+  double                       m_scale   = 1.0;
+  double                       m_padding = 0.0;
 
   tf2::Vector3 m_center;
   tf2::Vector3 m_meshCenter;

include/robot_self_filter/self_mask.h

@@ -445,19 +445,13 @@ class SelfMask
             }
             case shapes::MESH:
             {
-              // For a mesh, you might do uniform scale/padding
-              // but there's no single "setScale" in the base class.
-              // In the improved code we do it similarly to a sphere: single scale/padding
-              auto mesh_body = dynamic_cast<bodies::ConvexMesh*>(sl.body);
-              // Possibly store your scale/padding if you want a custom approach
-              // For now, no direct function calls for multi-scale, so do uniform or skip
-              // You might implement your own approach. For demonstration:
-              // We'll ignore multi-dim box/cylinder arrays for the mesh
-              // because it's not natively supported.
-              // That means we'd do uniform scale/padding in `updateInternalData()`,
-              // if implemented in ConvexMesh.
-              // -> No direct calls needed here. 
-              // (Or you could design a custom method to do it.)
+              // A mesh takes one scale and one padding, the same way a sphere does.
+              auto *mesh_body = dynamic_cast<bodies::ConvexMesh*>(sl.body);
+              if (mesh_body)
+              {
+                mesh_body->setScale(linfo.scale);
+                mesh_body->setPadding(linfo.padding);
+              }
               break;
             }
             default:

src/bodies.cpp

@@ -408,7 +408,11 @@ bool ConvexMesh::containsPoint(const tf2::Vector3 &p, bool) const
 {
   if (!m_boundingBox.containsPoint(p)) return false;
   tf2::Vector3 ip = m_iPose * p;
-  ip = m_meshCenter + (ip - m_meshCenter); // uniform scale/padding can be applied if desired
+  // The planes describe the unscaled hull, so bring the query point back into that
+  // frame rather than growing the hull. Dividing is what makes a scale above 1 a
+  // larger body; the code this replaced multiplied, which shrank it instead.
+  if (m_scale != 1.0)
+    ip = m_meshCenter + (ip - m_meshCenter) / m_scale;
   return isPointInsidePlanes(ip);
 }
 bool ConvexMesh::intersectsRay(const tf2::Vector3 &origin,
@@ -593,25 +597,35 @@ void ConvexMesh::updateInternalData()
   tf2::Transform pose = m_pose;
   pose.setOrigin(m_pose * m_boxOffset);
   m_boundingBox.setPose(pose);
-  // Could apply uniform scale/padding if desired, left as-is for example
+  // The bounding box is the first rejection test in containsPoint, so it has to
+  // grow with the padding or a padded point never reaches the plane test.
+  m_boundingBox.setScale(m_scale, m_scale, m_scale);
+  m_boundingBox.setPadding(m_padding, m_padding, m_padding);
   m_iPose   = m_pose.inverse();
   m_center  = m_pose * m_meshCenter;
-  m_radiusB = m_meshRadiusB; 
+  m_radiusB = m_meshRadiusB * m_scale + m_padding;
   m_radiusBSqr = m_radiusB * m_radiusB;
 
   m_scaledVertices.resize(m_vertices.size());
   for (size_t i=0; i<m_vertices.size(); i++)
   {
-    // Uniform scaling placeholder
-    m_scaledVertices[i] = m_vertices[i];
+    tf2::Vector3 v = m_vertices[i] - m_meshCenter;
+    double norm = v.length();
+    if (norm > 1e-9)
+      m_scaledVertices[i] = m_meshCenter + v * (m_scale + m_padding / norm);
+    else
+      m_scaledVertices[i] = m_vertices[i];
   }
 }
 bool ConvexMesh::isPointInsidePlanes(const tf2::Vector3 &point) const
 {
   for (auto &plane : m_planes)
   {
     double dist = plane(0)*point.x() + plane(1)*point.y() + plane(2)*point.z() + plane(3);
-    if (dist > 0.0) return false;
+    // Plane normals are unit length, so dist is a signed distance, and the padding
+    // shifts every face of the hull outward by that much. containsPoint measures in
+    // the unscaled frame, so a padding given in metres divides by the scale here.
+    if (dist > m_padding / m_scale) return false;
   }
   return true;
 }