435 lines
12 KiB
C++
435 lines
12 KiB
C++
/*
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
#include <sdf/sdf.hh>
|
|
#include "gazebo/math/Pose.hh"
|
|
#include "gazebo/common/Console.hh"
|
|
#include "gazebo/common/MeshManager.hh"
|
|
#include "gazebo/common/Mesh.hh"
|
|
#include "gazebo/common/Material.hh"
|
|
|
|
std::vector<std::string> params;
|
|
|
|
// To Render high quality: povray +H480 +W640 +R5 +AM2 +A0.3 +Q11 <filename>
|
|
|
|
// Convert radians to degrees
|
|
#define RTOD(r) ((r) * 180 / M_PI)
|
|
|
|
// Convert degrees to radians
|
|
#define DTOR(d) ((d) * M_PI / 180)
|
|
|
|
void help()
|
|
{
|
|
printf("help");
|
|
}
|
|
|
|
bool parse(int argc, char **argv)
|
|
{
|
|
if (argc == 1 || std::string(argv[1]) == "help")
|
|
{
|
|
help();
|
|
return false;
|
|
}
|
|
|
|
// Get parameters from command line
|
|
for (int i = 1; i < argc; i++)
|
|
{
|
|
std::string p = argv[i];
|
|
boost::trim(p);
|
|
params.push_back(p);
|
|
}
|
|
|
|
// Get parameters from stdin
|
|
if (!isatty(fileno(stdin)))
|
|
{
|
|
char str[1024];
|
|
while (!feof(stdin))
|
|
{
|
|
if (fgets(str, 1024, stdin)== NULL)
|
|
break;
|
|
|
|
if (feof(stdin))
|
|
break;
|
|
std::string p = str;
|
|
boost::trim(p);
|
|
params.push_back(p);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
gazebo::math::Vector3 Convert(const gazebo::math::Vector3 &_vec)
|
|
{
|
|
gazebo::math::Vector3 result;
|
|
gazebo::math::Quaternion rot1(0, M_PI*.5, 0);
|
|
gazebo::math::Quaternion rot2(0, -M_PI*.5, 0);
|
|
|
|
result = rot1.RotateVector(_vec);
|
|
result = rot2.RotateVector(result);
|
|
|
|
return result;
|
|
}
|
|
|
|
void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose &_pose)
|
|
{
|
|
const gazebo::common::Mesh *mesh;
|
|
|
|
mesh = gazebo::common::MeshManager::Instance()->Load(
|
|
_elem->GetValueString("filename"));
|
|
|
|
const_cast<gazebo::common::Mesh*>(mesh)->RecalculateNormals();
|
|
|
|
|
|
for (unsigned int i = 0; i < mesh->GetSubMeshCount(); i++)
|
|
{
|
|
const gazebo::common::SubMesh *subMesh = mesh->GetSubMesh(i);
|
|
printf("mesh2 {\n");
|
|
printf(" vertex_vectors {\n");
|
|
printf(" %u, \n ", subMesh->GetVertexCount());
|
|
for (unsigned int v = 0; v < subMesh->GetVertexCount(); v++)
|
|
{
|
|
gazebo::math::Vector3 vert = subMesh->GetVertex(v);
|
|
// vert = _pose.CoordPositionAdd(vert);
|
|
printf("<%f, %f, %f>, ", vert.x, vert.y, vert.z);
|
|
}
|
|
printf(" }\n");
|
|
|
|
printf(" normal_vectors {\n");
|
|
printf(" %u, \n ", subMesh->GetNormalCount());
|
|
for (unsigned int n = 0; n < subMesh->GetNormalCount(); n++)
|
|
{
|
|
gazebo::math::Vector3 norm = subMesh->GetNormal(n);
|
|
printf("<%f, %f, %f>, ", norm.x, norm.y, norm.z);
|
|
}
|
|
printf(" }\n");
|
|
|
|
printf(" uv_vectors {\n");
|
|
printf(" %u, \n", subMesh->GetTexCoordCount());
|
|
for (unsigned int j = 0; j < subMesh->GetTexCoordCount(); j++)
|
|
{
|
|
printf(" <%f, %f>, \n", subMesh->GetTexCoord(j).x,
|
|
1.0 - subMesh->GetTexCoord(j).y);
|
|
}
|
|
printf(" }\n");
|
|
|
|
const gazebo::common::Material *mat = mesh->GetMaterial(
|
|
subMesh->GetMaterialIndex());
|
|
if (mat)
|
|
{
|
|
printf(" texture_list {\n");
|
|
printf(" 1, \n");
|
|
printf(" texture {\n");
|
|
if (!mat->GetTextureImage().empty())
|
|
{
|
|
printf(" uv_mapping pigment { image_map ");
|
|
printf("{ tiff \"%s\" } }\n",
|
|
mat->GetTextureImage().c_str());
|
|
}
|
|
else
|
|
{
|
|
printf(" pigment { color rgb <%f, %f, %f> }\n",
|
|
mat->GetDiffuse().r, mat->GetDiffuse().g, mat->GetDiffuse().b);
|
|
}
|
|
|
|
printf(" finish {\n");
|
|
printf(" ambient color rgb <%f, %f, %f>\n",
|
|
mat->GetAmbient().r, mat->GetAmbient().g, mat->GetAmbient().b);
|
|
printf(" specular %f\n", 1.0);
|
|
printf(" }\n");
|
|
|
|
printf(" }\n");
|
|
printf(" }\n");
|
|
}
|
|
|
|
printf(" face_indices {\n");
|
|
printf(" %d, \n", subMesh->GetIndexCount() / 3);
|
|
for (unsigned int j = 0; j < subMesh->GetIndexCount(); j+= 3)
|
|
{
|
|
if (mat)
|
|
{
|
|
printf(" <%u, %u, %u>, 0\n", subMesh->GetIndex(j),
|
|
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
|
|
}
|
|
else
|
|
{
|
|
printf(" <%u, %u, %u>\n", subMesh->GetIndex(j),
|
|
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
|
|
}
|
|
}
|
|
printf(" }\n");
|
|
|
|
printf(" normal_indices {\n");
|
|
printf(" %d, \n", subMesh->GetIndexCount() / 3);
|
|
for (unsigned int j = 0; j < subMesh->GetIndexCount(); j+= 3)
|
|
{
|
|
printf(" <%u, %u, %u>, \n", subMesh->GetIndex(j),
|
|
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
|
|
}
|
|
printf(" }\n");
|
|
/*
|
|
printf(" uv_indices {\n");
|
|
printf(" %d, \n", subMesh->GetIndexCount() / 3);
|
|
for (unsigned int j = 0; j < subMesh->GetIndexCount(); j+= 3)
|
|
{
|
|
printf(" <%d, %d, %d>, \n", subMesh->GetIndex(j),
|
|
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
|
|
}
|
|
printf(" }\n");
|
|
*/
|
|
|
|
gazebo::math::Vector3 rpy = _pose.rot.GetAsEuler();
|
|
printf(" translate <%f, %f, %f>\n", _pose.pos.x, _pose.pos.y, _pose.pos.z);
|
|
printf(" rotate <%f, %f, %f>\n", RTOD(rpy.x), RTOD(rpy.y), RTOD(rpy.z));
|
|
|
|
printf("}\n");
|
|
}
|
|
}
|
|
|
|
void ProcessLight(sdf::ElementPtr _elem)
|
|
{
|
|
gazebo::math::Pose pose;
|
|
gazebo::common::Color diffuse, specular;
|
|
|
|
pose = _elem->GetOrCreateElement("origin")->GetValuePose("pose");
|
|
diffuse = _elem->GetValueColor("diffuse");
|
|
specular = _elem->GetValueColor("specular");
|
|
// double fadeDist =
|
|
// _elem->GetElement("attenuation")->GetValueDouble("range");
|
|
// double constant =
|
|
// _elem->GetElement("attenuation")->GetValueDouble("constant");
|
|
// double linear = _elem->GetElement("attenuation")->GetValueDouble("linear");
|
|
// double quadratic =
|
|
// _elem->GetElement("attenuation")->GetValueDouble("quadratic");
|
|
|
|
printf("light_source {\n");
|
|
printf(" <%f, %f, %f>, rgb <%f, %f, %f>\n",
|
|
pose.pos.x, pose.pos.y, pose.pos.z,
|
|
diffuse.r, diffuse.g, diffuse.b);
|
|
|
|
std::string type = _elem->GetValueString("type");
|
|
if (type == "point")
|
|
{
|
|
// printf(" pointlight\n");
|
|
}
|
|
else if (type == "directional")
|
|
{
|
|
printf(" parallel\n");
|
|
}
|
|
else if (type == "spot")
|
|
{
|
|
double innerAngle, outerAngle, falloff;
|
|
innerAngle = _elem->GetElement("spot")->GetValueDouble("inner_angle");
|
|
outerAngle = _elem->GetElement("spot")->GetValueDouble("outer_angle");
|
|
falloff = _elem->GetElement("spot")->GetValueDouble("falloff");
|
|
printf(" spotlight\n");
|
|
printf(" radius %f\n", RTOD(innerAngle));
|
|
printf(" falloff %f\n", RTOD(outerAngle));
|
|
printf(" tightness %f\n", falloff);
|
|
}
|
|
|
|
if (_elem->HasElement("direction"))
|
|
{
|
|
gazebo::math::Vector3 dir =
|
|
_elem->GetElement("direction")->GetValueVector3("xyz");
|
|
gazebo::math::Plane plane(gazebo::math::Vector3(0, 0, 1));
|
|
|
|
double d = plane.Distance(pose.pos, dir);
|
|
double t;
|
|
t = atan2(dir.x, dir.z*-1);
|
|
double x = sin(t) * d;
|
|
t = atan2(dir.y, dir.z*-1);
|
|
double y = sin(t) * d;
|
|
printf(" point_at <%f, %f, 0.0>\n", x, y);
|
|
}
|
|
|
|
printf("}\n");
|
|
}
|
|
|
|
void ProcessScene(sdf::ElementPtr _elem)
|
|
{
|
|
gazebo::common::Color color;
|
|
if (_elem->HasElement("background"))
|
|
{
|
|
color = _elem->GetValueColor("background");
|
|
printf("background { rgb <%f, %f, %f> }\n", color.r, color.g, color.b);
|
|
}
|
|
|
|
if (_elem->HasElement("ambient"))
|
|
{
|
|
color = _elem->GetValueColor("ambient");
|
|
// printf("global_settings { ambient_light rgb <%f, %f, %f> }\n",
|
|
// color.R(), color.G(), color.B());
|
|
}
|
|
|
|
// int count = 35;
|
|
int count = 1600;
|
|
|
|
// int recursionLimit = 3;
|
|
int recursionLimit = 20;
|
|
|
|
// float errorBound = 1.8;
|
|
float errorBound = 1.0;
|
|
|
|
// Note: Extreme quality
|
|
printf("global_settings { radiosity{\n");
|
|
printf(" count %d\n", count);
|
|
printf(" recursion_limit %d\n", recursionLimit);
|
|
printf(" error_bound %f\n", errorBound);
|
|
|
|
printf("} }\n");
|
|
}
|
|
|
|
void ProcessGeometry(sdf::ElementPtr _elem, const gazebo::math::Pose &_pose)
|
|
{
|
|
if (_elem->HasElement("plane"))
|
|
{
|
|
sdf::ElementPtr planeElem = _elem->GetElement("plane");
|
|
gazebo::math::Vector3 normal = planeElem->GetValueVector3("normal");
|
|
printf("plane {\n");
|
|
printf(" <%f, %f, %f>, 0\n", normal.x, normal.y, normal.z);
|
|
printf(" texture {pigment { color Yellow } }\n");
|
|
printf("}\n");
|
|
}
|
|
else if (_elem->HasElement("box"))
|
|
{
|
|
sdf::ElementPtr boxElem = _elem->GetElement("box");
|
|
gazebo::math::Vector3 size = boxElem->GetValueVector3("size");
|
|
printf("box {\n");
|
|
gazebo::math::Vector3 corner1 = _pose.pos - (size/2.0);
|
|
gazebo::math::Vector3 corner2 = _pose.pos + (size/2.0);
|
|
corner1 = _pose.rot.RotateVector(corner1);
|
|
corner2 = _pose.rot.RotateVector(corner2);
|
|
printf(" <%f, %f, %f, >, <%f, %f, %f>\n", corner1.x, corner1.y, corner1.z,
|
|
corner2.x, corner2.y, corner2.z);
|
|
printf("}\n");
|
|
}
|
|
else if (_elem->HasElement("cylinder"))
|
|
{
|
|
sdf::ElementPtr cylinderElem = _elem->GetElement("cylinder");
|
|
double radius = cylinderElem->GetValueDouble("radius");
|
|
double length = cylinderElem->GetValueDouble("length");
|
|
gazebo::math::Vector3 capPoint = _pose.pos;
|
|
capPoint.z += length;
|
|
capPoint = _pose.rot.RotateVector(capPoint);
|
|
printf("cylinder {\n");
|
|
printf(" <%f, %f, %f>, <%f, %f, %f>, %f\n",
|
|
_pose.pos.x, _pose.pos.y, _pose.pos.z,
|
|
capPoint.x, capPoint.y, capPoint.z, radius);
|
|
printf("}\n");
|
|
}
|
|
else if (_elem->HasElement("sphere"))
|
|
{
|
|
sdf::ElementPtr sphereElem = _elem->GetElement("sphere");
|
|
double radius = sphereElem->GetValueDouble("radius");
|
|
|
|
printf("sphere {\n");
|
|
printf(" <%f, %f, %f> %f\n",
|
|
_pose.pos.x, _pose.pos.y, _pose.pos.z, radius);
|
|
printf("}\n");
|
|
}
|
|
else if (_elem->HasElement("mesh"))
|
|
{
|
|
ProcessMesh(_elem->GetElement("mesh"), _pose);
|
|
}
|
|
}
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
if (!parse(argc, argv))
|
|
return 0;
|
|
|
|
// Load the world file
|
|
sdf::SDFPtr sdf(new sdf::SDF);
|
|
if (!sdf::init(sdf))
|
|
{
|
|
gzerr << "Unable to initialize sdf\n";
|
|
return false;
|
|
}
|
|
|
|
if (!sdf::readFile(params[0], sdf))
|
|
{
|
|
gzerr << "Unable to read sdf file[" << params[0] << "]\n";
|
|
return false;
|
|
}
|
|
|
|
printf("#include \"colors.inc\"\n");
|
|
printf("#include \"shapes.inc\"\n");
|
|
printf("camera {\n");
|
|
printf(" location <-2, 0, 2>\n");
|
|
printf(" look_at <0, 0, 1>\n");
|
|
printf(" sky <0, 0, 1>\n");
|
|
printf(" direction <1, 0, 0>\n");
|
|
printf(" up <0, 0, -1>\n");
|
|
printf(" right <0, 1, 0>\n");
|
|
printf("}\n");
|
|
|
|
sdf::ElementPtr root = sdf->Root();
|
|
|
|
gazebo::math::Pose modelPose, linkPose, visualPose;
|
|
|
|
sdf::ElementPtr worldElem = root->GetElement("world");
|
|
while (worldElem)
|
|
{
|
|
if (worldElem->HasElement("scene"))
|
|
ProcessScene(worldElem->GetElement("scene"));
|
|
|
|
if (worldElem->HasElement("light"))
|
|
{
|
|
sdf::ElementPtr lightElem = worldElem->GetElement("light");
|
|
while (lightElem)
|
|
{
|
|
ProcessLight(lightElem);
|
|
lightElem = lightElem->GetNextElement("light");
|
|
}
|
|
}
|
|
|
|
sdf::ElementPtr modelElem = worldElem->GetElement("model");
|
|
while (modelElem)
|
|
{
|
|
modelPose = modelElem->GetOrCreateElement("origin")->GetValuePose("pose");
|
|
|
|
sdf::ElementPtr linkElem = modelElem->GetElement("link");
|
|
while (linkElem)
|
|
{
|
|
linkPose = linkElem->GetOrCreateElement("origin")->GetValuePose("pose");
|
|
|
|
if (linkElem->HasElement("visual"))
|
|
{
|
|
sdf::ElementPtr visualElem = linkElem->GetElement("visual");
|
|
while (visualElem)
|
|
{
|
|
visualPose =
|
|
visualElem->GetOrCreateElement("origin")->GetValuePose("pose");
|
|
// visualPose = (visualPose + linkPose) + modelPose;
|
|
visualPose = modelPose + (linkPose + visualPose);
|
|
// visualPose.pos = modelPose.pos + linkPose.pos + visualPose.pos;
|
|
// visualPose.rot = visualPose.rot * linkPose.rot * modelPose.rot;
|
|
sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
|
|
ProcessGeometry(geomElem, visualPose);
|
|
|
|
visualElem = visualElem->GetNextElement("visual");
|
|
}
|
|
}
|
|
linkElem = linkElem->GetNextElement("link");
|
|
}
|
|
modelElem = modelElem->GetNextElement("model");
|
|
}
|
|
worldElem = worldElem->GetNextElement("world");
|
|
}
|
|
}
|