[melodic backport] Added body operations constructShapeFromBody() and constructMarkerFromBody().

include/geometric_shapes/bodies.h

@@ -199,6 +199,9 @@ class Body
   /** \brief Get the dimensions associated to this body (as read from corresponding shape) */
   virtual std::vector<double> getDimensions() const = 0;
 
+  /** \brief Get the dimensions associated to this body (scaled and padded) */
+  virtual std::vector<double> getScaledDimensions() const = 0;
+
   /** \brief Set the dimensions of the body (from corresponding shape) */
   void setDimensions(const shapes::Shape* shape);
 
@@ -313,6 +316,7 @@ class Sphere : public Body
 
   /** \brief Get the radius of the sphere */
   std::vector<double> getDimensions() const override;
+  std::vector<double> getScaledDimensions() const override;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;
@@ -372,6 +376,7 @@ class Cylinder : public Body
 
   /** \brief Get the radius & length of the cylinder */
   std::vector<double> getDimensions() const override;
+  std::vector<double> getScaledDimensions() const override;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;
@@ -444,6 +449,7 @@ class Box : public Body
 
   /** \brief Get the length & width & height (x, y, z) of the box */
   std::vector<double> getDimensions() const override;
+  std::vector<double> getScaledDimensions() const override;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;
@@ -507,6 +513,8 @@ class ConvexMesh : public Body
 
   /** \brief Returns an empty vector */
   std::vector<double> getDimensions() const override;
+  /** \brief Returns an empty vector */
+  std::vector<double> getScaledDimensions() const override;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;

include/geometric_shapes/body_operations.h

@@ -41,6 +41,7 @@
 #include "geometric_shapes/bodies.h"
 #include "geometric_shapes/shape_messages.h"
 #include <geometry_msgs/Pose.h>
+#include <visualization_msgs/Marker.h>
 #include <vector>
 
 namespace bodies
@@ -60,6 +61,12 @@ Body* constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape, const geomet
 /** \brief Create a body from a given shape */
 Body* constructBodyFromMsg(const shapes::ShapeMsg& shape, const geometry_msgs::Pose& pose);
 
+/** \brief Get a shape that corresponds to this (scaled and padded) body. */
+shapes::ShapeConstPtr constructShapeFromBody(const bodies::Body* body);
+
+/** \brief Construct a Marker message that corresponds to this (scaled and padded) body. */
+void constructMarkerFromBody(const bodies::Body* body, visualization_msgs::Marker& msg);
+
 /** \brief Compute a bounding sphere to enclose a set of bounding spheres */
 void mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSphere& mergedSphere);
 

src/bodies.cpp

@@ -193,8 +193,12 @@ void bodies::Sphere::useDimensions(const shapes::Shape* shape)  // radius
 
 std::vector<double> bodies::Sphere::getDimensions() const
 {
-  std::vector<double> d(1, radius_);
-  return d;
+  return { radius_ };
+}
+
+std::vector<double> bodies::Sphere::getScaledDimensions() const
+{
+  return { radiusU_ };
 }
 
 void bodies::Sphere::updateInternalData()
@@ -359,10 +363,12 @@ void bodies::Cylinder::useDimensions(const shapes::Shape* shape)  // (length, ra
 
 std::vector<double> bodies::Cylinder::getDimensions() const
 {
-  std::vector<double> d(2);
-  d[0] = radius_;
-  d[1] = length_;
-  return d;
+  return { radius_, length_ };
+}
+
+std::vector<double> bodies::Cylinder::getScaledDimensions() const
+{
+  return { radiusU_, 2 * length2_ };
 }
 
 void bodies::Cylinder::updateInternalData()
@@ -585,11 +591,12 @@ void bodies::Box::useDimensions(const shapes::Shape* shape)  // (x, y, z) = (len
 
 std::vector<double> bodies::Box::getDimensions() const
 {
-  std::vector<double> d(3);
-  d[0] = length_;
-  d[1] = width_;
-  d[2] = height_;
-  return d;
+  return { length_, width_, height_ };
+}
+
+std::vector<double> bodies::Box::getScaledDimensions() const
+{
+  return { 2 * length2_, 2 * width2_, 2 * height2_ };
 }
 
 void bodies::Box::updateInternalData()
@@ -978,7 +985,12 @@ void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape)
 
 std::vector<double> bodies::ConvexMesh::getDimensions() const
 {
-  return std::vector<double>();
+  return {};
+}
+
+std::vector<double> bodies::ConvexMesh::getScaledDimensions() const
+{
+  return {};
 }
 
 void bodies::ConvexMesh::computeScaledVerticesFromPlaneProjections()

src/body_operations.cpp

@@ -78,6 +78,72 @@ bodies::Body* bodies::createBodyFromShape(const shapes::Shape* shape)
   return body;
 }
 
+shapes::ShapeConstPtr bodies::constructShapeFromBody(const bodies::Body* body)
+{
+  shapes::ShapePtr result;
+
+  switch (body->getType())
+  {
+    case shapes::SPHERE:
+    {
+      // As we already know body's type, we can skip the vtable lookup and use compile-time polymorphism
+      const auto& dims = static_cast<const bodies::Sphere*>(body)->bodies::Sphere::getScaledDimensions();
+      result.reset(new shapes::Sphere(dims[0]));
+      break;
+    }
+    case shapes::BOX:
+    {
+      const auto& dims = static_cast<const bodies::Box*>(body)->bodies::Box::getScaledDimensions();
+      result.reset(new shapes::Box(dims[0], dims[1], dims[2]));
+      break;
+    }
+    case shapes::CYLINDER:
+    {
+      const auto& dims = static_cast<const bodies::Cylinder*>(body)->bodies::Cylinder::getScaledDimensions();
+      result.reset(new shapes::Cylinder(dims[0], dims[1]));
+      break;
+    }
+    case shapes::MESH:
+    {
+      const auto mesh = static_cast<const bodies::ConvexMesh*>(body);
+      const auto& scaledVertices = mesh->getScaledVertices();
+
+      // createMeshFromVertices requires an "expanded" list of triangles where each triangle is
+      // represented by its three vertex positions
+      EigenSTL::vector_Vector3d vertexList;
+      vertexList.reserve(3 * mesh->getTriangles().size());
+      for (const auto& triangle : mesh->getTriangles())
+        vertexList.push_back(scaledVertices[triangle]);
+
+      result.reset(shapes::createMeshFromVertices(vertexList));
+      break;
+    }
+    default:
+    {
+      CONSOLE_BRIDGE_logError("Unknown body type: %d", (int)body->getType());
+      break;
+    }
+  }
+  return result;
+}
+
+void bodies::constructMarkerFromBody(const bodies::Body* body, visualization_msgs::Marker& msg)
+{
+  auto shape = bodies::constructShapeFromBody(body);
+  shapes::constructMarkerFromShape(shape.get(), msg, true);
+  const auto& pose = body->getPose();
+  msg.pose.position.x = pose.translation().x();
+  msg.pose.position.y = pose.translation().y();
+  msg.pose.position.z = pose.translation().z();
+
+  ASSERT_ISOMETRY(pose);
+  Eigen::Quaterniond quat(pose.linear().matrix());
+  msg.pose.orientation.x = quat.x();
+  msg.pose.orientation.y = quat.y();
+  msg.pose.orientation.z = quat.z();
+  msg.pose.orientation.w = quat.w();
+}
+
 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSphere& mergedSphere)
 {
   if (spheres.empty())

test/CMakeLists.txt

@@ -32,3 +32,6 @@ target_link_libraries(test_shapes ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LI
 
 catkin_add_gtest(test_ray_intersection test_ray_intersection.cpp)
 target_link_libraries(test_ray_intersection ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+catkin_add_gtest(test_body_operations test_body_operations.cpp)
+target_link_libraries(test_body_operations ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})