Skip to content
Snippets Groups Projects
Commit 4661b62f authored by Johannes Mey's avatar Johannes Mey
Browse files

spawn physical objects

parent 1bb3837a
Branches
No related tags found
No related merge requests found
...@@ -59,6 +59,8 @@ public: ...@@ -59,6 +59,8 @@ public:
*/ */
static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose); 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);
}; };
......
<?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
...@@ -69,3 +69,77 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom ...@@ -69,3 +69,77 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom
ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'"); 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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment