Commit e3ba65a4 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added possibility to spawn cylinder

parent 547d35d1
......@@ -17,7 +17,16 @@ public:
void spawnCube(const std::string& name, const std::string& frame_id,
float x, float y, float z, float qx, float qy, float qz, float qw,
float _w=0.05, float _h=0.05, float _d=0.05, float _mass=0.05);
float w=0.05, float h=0.05, float d=0.05, float mass=0.05);
/**
* \param isCube if true, spawn a cube. If false, spawn cylinder,
* where \e w is the radius and \e h is the height (\e d will be ignored).
*/
void spawnPrimitive(const std::string& name, const bool isCube,
const std::string& frame_id,
float x, float y, float z, float qx, float qy, float qz, float qw,
float w=0.05, float h=0.05, float d=0.05, float mass=0.05);
private:
......
......@@ -18,8 +18,18 @@ GazeboCubeSpawner::GazeboCubeSpawner(NodeHandle &n) : nh(n){
spawn_object = n.serviceClient<gazebo_msgs::SpawnModel>(SPAWN_OBJECT_TOPIC);
}
void GazeboCubeSpawner::spawnCube(const std::string& name, const std::string& frame_id,
float x, float y, float z, float qx, float qy, float qz, float qw,
float width, float height, float depth, float mass)
{
spawnPrimitive(name, true, frame_id, x, y, z, qx, qy, qz, qw, width, height, depth, mass);
}
void GazeboCubeSpawner::spawnCube(const std::string& name, const std::string& frame_id, float x, float y, float z, float qx, float qy, float qz, float qw, float width, float height, float depth, float _mass) {
void GazeboCubeSpawner::spawnPrimitive(const std::string& name, const bool doCube,
const std::string& frame_id,
float x, float y, float z, float qx, float qy, float qz, float qw,
float widthOrRadius, float height, float depth, float _mass)
{
geometry_msgs::Pose pose;
pose.position.x=x;
......@@ -33,10 +43,27 @@ void GazeboCubeSpawner::spawnCube(const std::string& name, const std::string& fr
gazebo_msgs::SpawnModel spawn;
spawn.request.model_name=name;
float w=width;
// just so the variable names are shorter..
float w=widthOrRadius;
float h=height;
float d=depth;
std::stringstream _s;
if (doCube)
{
_s<<"<box>\
<size>"<<w<<" "<<h<<" "<<d<<"</size>\
</box>";
}else{
_s<<"<cylinder>\
<length>"<<h<<"</length>\
<radius>"<<w<<"</radius>\
</cylinder>";
}
std::string geometryString = _s.str();
float mass=_mass;
float mass12=mass/12.0;
......@@ -54,25 +81,38 @@ void GazeboCubeSpawner::spawnCube(const std::string& name, const std::string& fr
<model name='"<<name<<"'>\
<static>false</static>\
<link name='link'>";
// inertia according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia
if (do_inertia)
{
double xx, yy, zz;
if (doCube)
{
xx=mass12*(h*h+d*d);
yy=mass12*(w*w+d*d);
zz=mass12*(w*w+h*h);
}
else
{
xx=mass12*(3*w*w + h*h);
yy=mass12*(3*w*w + h*h);
zz=0.5*mass*w*w;
}
s<<"<inertial>\
<mass>"<<mass<<"</mass>\
<inertia>\
<ixx>"<<mass12*(h*h+d*d)<<"</ixx>\
<ixx>"<<xx<<"</ixx>\
<ixy>0.0</ixy>\
<ixz>0.0</ixz>\
<iyy>"<<mass12*(w*w+d*d)<<"</iyy>\
<iyy>"<<yy<<"</iyy>\
<iyz>0.0</iyz>\
<izz>"<<mass12*(w*w+h*h)<<"</izz>\
<izz>"<<zz<<"</izz>\
</inertia>\
</inertial>";
}
s<<"<collision name='collision'>\
<geometry>\
<box>\
<size>"<<w<<" "<<h<<" "<<d<<"</size>\
</box>\
</geometry>";
<geometry>"<<geometryString;
s<<"</geometry>";
if (do_surface)
s<<"<surface>\
<friction>\
......@@ -100,12 +140,9 @@ void GazeboCubeSpawner::spawnCube(const std::string& name, const std::string& fr
</contact>\
</surface>";
s<<"</collision>\
<visual name='visual'>\
<geometry>\
<box>\
<size>"<<w<<" "<<h<<" "<<d<<"</size>\
</box>\
</geometry>\
<visual name='visual'>";
s<<"<geometry>"<<geometryString;
s<<"</geometry>\
<material>\
<script>\
<uri>file://media/materials/scripts/gazebo.material</uri> \
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment