spawn physical objects

include/GazeboZoneSpawner.h

@@ -59,6 +59,8 @@ public:
    */
   static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
 
+  static void spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, double mass = 1.0, bool is_static = false);
+
 };
 
 

models/hardbox.sdf

+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="box">
+    <static>false</static>
+    <link name="link">
+      <pose>2 3 0.5 0 0 0</pose>
+      <inertial>
+        <mass>1.0</mass>
+        <inertia> <!-- inertias are tricky to compute -->
+          <!-- http://gazebosim.org/tutorials?tut=inertia&cat=build_robot -->
+          <ixx>0.083</ixx>       <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
+          <ixy>0.0</ixy>         <!-- for a box: ixy = 0 -->
+          <ixz>0.0</ixz>         <!-- for a box: ixz = 0 -->
+          <iyy>0.083</iyy>       <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
+          <iyz>0.0</iyz>         <!-- for a box: iyz = 0 -->
+          <izz>0.083</izz>       <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <material>
+          <script>
+            <uri>box.material</uri>
+            <name>Zone/DarkMagentaTransparent</name>
+          </script>
+        </material>
+        <geometry>
+          <box>
+            <size>1 1 1</size>
+          </box>
+        </geometry>
+      </visual>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>1 1 1</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>
\ No newline at end of file

src/GazeboZoneSpawner.cpp

@@ -69,3 +69,77 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom
     ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
   }
 }
+
+void GazeboZoneSpawner::spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, double mass, bool is_static) {
+  if (shape.type != shape.BOX) {
+    ROS_ERROR_STREAM("Safety-zone could not be created due to wrong shape-type");
+    return;
+  }
+
+  sdf::SDFPtr sdf(new sdf::SDF());
+  sdf::init(sdf);
+
+  auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/hardbox.sdf";
+  auto materialFileName = "file://" + ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.material";
+
+  assert(sdf::readFile(sdfFileName, sdf));
+
+  auto modelElement = sdf->Root()->GetElement("model");
+  auto linkElement = modelElement->GetElement("link");
+  auto poseElement = linkElement->GetElement("pose");
+
+  auto visualElement = linkElement->GetElement("visual");
+  auto visualMaterialUriElement = visualElement->GetElement("material")->GetElement("script")->GetElement("uri");
+  auto visualSizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
+
+  auto collisionElement = linkElement->GetElement("collision");
+  auto collisionSizeElement = collisionElement->GetElement("geometry")->GetElement("box")->GetElement("size");
+
+  tf::Quaternion rot;
+  rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+  double roll, pitch, yaw;
+  tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
+
+  std::ostringstream poseStream;
+  poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " "
+             << roll << " " << pitch << " " << yaw;
+  poseElement->Set(poseStream.str());
+
+  // set the element to static
+  modelElement->GetElement("static")->Set(is_static);
+
+  double sx = shape.dimensions[0];
+  double sy = shape.dimensions[1];
+  double sz = shape.dimensions[2];
+
+  // set the size both for visual and collision objects
+  std::ostringstream sizeStream;
+  sizeStream << sx << " " << sy << " " << sz;
+  visualSizeElement->Set(sizeStream.str());
+  collisionSizeElement->Set(sizeStream.str());
+
+  // set inertia matrix
+  auto inertialElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("inertial");
+  inertialElement->GetElement("mass")->Set(mass);
+  inertialElement->GetElement("inertia")->GetElement("ixx")->Set(0.083 * mass * sy*sy + sz*sz);
+  inertialElement->GetElement("inertia")->GetElement("iyy")->Set(0.083 * mass * sx*sx + sz*sz);
+  inertialElement->GetElement("inertia")->GetElement("izz")->Set(0.083 * mass * sx*sx + sy*sy);
+
+  ros::NodeHandle n;
+  ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
+  gazebo_msgs::SpawnModel srv;
+  srv.request.robot_namespace = "box space";
+  // srv.request.initial_pose = pose; // not required here
+  srv.request.model_name = name;
+  srv.request.model_xml = sdf->ToString();
+  // srv.request.reference_frame = ; // if left empty, world is used
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      ROS_INFO_STREAM("Spawned box '" << srv.request.model_name << "'. " << srv.response.status_message);
+    } else {
+      ROS_ERROR_STREAM("Failed to spawn box '" << srv.request.model_name << "'. " << srv.response.status_message);
+    }
+  } else {
+    ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
+  }
+}
\ No newline at end of file