diff --git a/sdf/worlds/pr2.world b/sdf/worlds/pr2.world index 01e261b0d8..846ae27f05 100644 --- a/sdf/worlds/pr2.world +++ b/sdf/worlds/pr2.world @@ -33,7 +33,8 @@ - + + diff --git a/src/Server.cc b/src/Server.cc index a6f4bb8c2d..f5d8fd6a30 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -93,7 +93,7 @@ bool Server::Load(const std::string &_filename) gzthrow("Failed to load the World\n" << e); } - worldElem = sdf->root->GetNextElement("world", worldElem); + worldElem = worldElem->GetNextElement(); } return true; diff --git a/src/common/Mesh.cc b/src/common/Mesh.cc index f402a1b618..a04458fd99 100644 --- a/src/common/Mesh.cc +++ b/src/common/Mesh.cc @@ -669,9 +669,16 @@ void SubMesh::RecalculateNormals() math::Vector3 v2 = this->vertices[this->indices[i+1]]; math::Vector3 v3 = this->vertices[this->indices[i+2]]; math::Vector3 n = math::Vector3::GetNormal(v1, v2, v3); - this->normals[this->indices[i]] += n; - this->normals[this->indices[i+1]] += n; - this->normals[this->indices[i+2]] += n; + + for (unsigned int j=0; j< this->vertices.size(); j++) + { + if (this->vertices[j] == v1 || + this->vertices[j] == v2 || + this->vertices[j] == v3) + { + this->normals[j] += n; + } + } } // Normalize the results diff --git a/src/gui/InsertModelWidget.cc b/src/gui/InsertModelWidget.cc index 87699918d5..3b5c2b3685 100644 --- a/src/gui/InsertModelWidget.cc +++ b/src/gui/InsertModelWidget.cc @@ -186,10 +186,10 @@ void InsertModelWidget::OnModelSelection(QTreeWidgetItem *_item, int /*_column*/ this->visuals.push_back(visVisual); - visualElem = linkElem->GetNextElement("visual", visualElem); + visualElem = visualElem->GetNextElement(); } - linkElem = modelElem->GetNextElement("link", linkElem); + linkElem = linkElem->GetNextElement(); } msgs::Selection selectMsg; diff --git a/src/physics/Link.cc b/src/physics/Link.cc index 45c013a3d1..d63576b9cf 100644 --- a/src/physics/Link.cc +++ b/src/physics/Link.cc @@ -123,7 +123,7 @@ void Link::Load( sdf::ElementPtr &_sdf ) this->visPub->Publish(msg); this->visuals.push_back(msg.name()); - visualElem = this->sdf->GetNextElement("visual", visualElem); + visualElem = visualElem->GetNextElement(); } } @@ -135,7 +135,7 @@ void Link::Load( sdf::ElementPtr &_sdf ) { // Create and Load a collision, which will belong to this body. this->LoadCollision(collisionElem); - collisionElem = this->sdf->GetNextElement("collision", collisionElem); + collisionElem = collisionElem->GetNextElement(); } } @@ -145,7 +145,7 @@ void Link::Load( sdf::ElementPtr &_sdf ) while (sensorElem) { this->LoadSensor(sensorElem); - sensorElem = this->sdf->GetNextElement("sensor", sensorElem); + sensorElem = sensorElem->GetNextElement(); } } } @@ -294,7 +294,7 @@ void Link::UpdateParameters( sdf::ElementPtr &_sdf ) this->visPub->Publish(msg); - visualElem = this->sdf->GetNextElement("visual", visualElem); + visualElem = visualElem->GetNextElement(); } } @@ -308,7 +308,7 @@ void Link::UpdateParameters( sdf::ElementPtr &_sdf ) if (collision) collision->UpdateParameters(collisionElem); - collisionElem = this->sdf->GetNextElement("collision", collisionElem); + collisionElem = collisionElem->GetNextElement(); } } } diff --git a/src/physics/Model.cc b/src/physics/Model.cc index 33a6d9614a..4e9227bdcc 100644 --- a/src/physics/Model.cc +++ b/src/physics/Model.cc @@ -111,7 +111,7 @@ void Model::Load( sdf::ElementPtr &_sdf ) // bodies collisionetries link->Load(linkElem); - linkElem = _sdf->GetNextElement("link", linkElem); + linkElem = linkElem->GetNextElement(); } } @@ -122,7 +122,7 @@ void Model::Load( sdf::ElementPtr &_sdf ) while (jointElem) { this->LoadJoint(jointElem); - jointElem = _sdf->GetNextElement("joint", jointElem); + jointElem = jointElem->GetNextElement(); } } @@ -133,7 +133,7 @@ void Model::Load( sdf::ElementPtr &_sdf ) while (pluginElem) { this->LoadPlugin(pluginElem); - pluginElem = _sdf->GetNextElement("plugin", pluginElem); + pluginElem = pluginElem->GetNextElement(); } } } @@ -304,7 +304,7 @@ void Model::UpdateParameters( sdf::ElementPtr &_sdf ) LinkPtr link = boost::shared_dynamic_cast( this->GetChild(linkElem->GetValueString("name"))); link->UpdateParameters(linkElem); - linkElem = _sdf->GetNextElement("link", linkElem); + linkElem = linkElem->GetNextElement(); } } /* @@ -316,7 +316,7 @@ void Model::UpdateParameters( sdf::ElementPtr &_sdf ) { JointPtr joint = boost::shared_dynamic_cast(this->GetChild(jointElem->GetValueString("name"))); joint->UpdateParameters(jointElem); - jointElem = _sdf->GetNextElement("joint", jointElem); + jointElem = jointElem->GetNextElement(); } } */ diff --git a/src/physics/World.cc b/src/physics/World.cc index 310909ab97..89a49cd28c 100644 --- a/src/physics/World.cc +++ b/src/physics/World.cc @@ -438,7 +438,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent ) msgs::Light *lm = this->sceneMsg.add_light(); lm->CopyFrom( msgs::LightFromSDF(childElem) ); - childElem = _sdf->GetNextElement("light", childElem); + childElem = childElem->GetNextElement(); } } @@ -453,7 +453,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent ) // TODO : Put back in the ability to nest models. We should do this // without requiring a joint. - childElem = _sdf->GetNextElement("model", childElem); + childElem = childElem->GetNextElement(); } } @@ -464,7 +464,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent ) while (pluginElem) { this->LoadPlugin(pluginElem); - pluginElem = _sdf->GetNextElement("plugin", pluginElem); + pluginElem = pluginElem->GetNextElement(); } } } diff --git a/src/physics/server.cc b/src/physics/server.cc index 45de436433..59f075d2a4 100644 --- a/src/physics/server.cc +++ b/src/physics/server.cc @@ -114,7 +114,7 @@ void Load() gzthrow("Failed to load the World\n" << e); } - worldElem = sdf->root->GetNextElement("world", worldElem); + worldElem = worldElem->GetNextElement(); } gazebo::physics::init_worlds(); diff --git a/src/sdf/interface/SDF.cc b/src/sdf/interface/SDF.cc index 2af25e6b0f..648777ac41 100644 --- a/src/sdf/interface/SDF.cc +++ b/src/sdf/interface/SDF.cc @@ -428,21 +428,33 @@ ElementPtr Element::GetFirstElement() const return this->elements.front(); } -ElementPtr Element::GetNextElement(const std::string &_name, - const ElementPtr &_elem) const +ElementPtr Element::GetNextElement(const std::string &_name) const { - ElementPtr_V::const_iterator iter; - iter = std::find(this->elements.begin(), this->elements.end(), _elem); - - for (iter++; iter != this->elements.end(); iter++) + if (this->parent) { - if ( (*iter)->GetName() == _name ) - return (*iter); + ElementPtr_V::const_iterator iter; + iter = std::find(this->parent->elements.begin(), + this->parent->elements.end(), shared_from_this()); + + if (iter == this->parent->elements.end()) + return ElementPtr(); + + for (iter++; iter != this->parent->elements.end(); iter++) + { + if ( (*iter)->GetName() == _name ) + return (*iter); + } } return ElementPtr(); } +ElementPtr Element::GetNextElement() const +{ + return this->GetNextElement(this->name); +} + + boost::shared_ptr Element::GetOrCreateElement(const std::string &_name) { if (this->HasElement(_name)) diff --git a/src/sdf/interface/SDF.hh b/src/sdf/interface/SDF.hh index 30e6a91a17..8c658e4560 100644 --- a/src/sdf/interface/SDF.hh +++ b/src/sdf/interface/SDF.hh @@ -106,8 +106,10 @@ namespace sdf public: ElementPtr GetElement(const std::string &_name) const; public: ElementPtr GetFirstElement() const; - public: ElementPtr GetNextElement(const std::string &_name, - const ElementPtr &_elem) const; + + public: ElementPtr GetNextElement(const std::string &_name) const; + public: ElementPtr GetNextElement() const; + public: ElementPtr GetOrCreateElement(const std::string &_name); public: ElementPtr AddElement(const std::string &_name); diff --git a/src/sensors/ContactSensor.cc b/src/sensors/ContactSensor.cc index 6c81cd6634..4cf85473c4 100644 --- a/src/sensors/ContactSensor.cc +++ b/src/sensors/ContactSensor.cc @@ -110,8 +110,7 @@ void ContactSensor::Load() this->connections.push_back(collision->ConnectContact( boost::bind(&ContactSensor::OnContact, this, _1, _2))); } - collisionElem = this->sdf->GetElement("contact")->GetNextElement( - "collision", collisionElem); + collisionElem = collisionElem->GetNextElement(); } } diff --git a/src/sensors/Sensor.cc b/src/sensors/Sensor.cc index c59dabee1f..ca5002dbe3 100644 --- a/src/sensors/Sensor.cc +++ b/src/sensors/Sensor.cc @@ -102,7 +102,7 @@ void Sensor::Init() while (pluginElem) { this->LoadPlugin(pluginElem); - pluginElem = this->sdf->GetNextElement("plugin", pluginElem); + pluginElem = pluginElem->GetNextElement(); } } //SensorManager::Instance()->AddSensor(shared_from_this()); diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 89135cec96..88de4b9705 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -20,3 +20,7 @@ add_executable( gzstats gzstats.cc ) target_link_libraries(gzstats gazebo_msgs gazebo_common gazebo_transport ) INSTALL_EXECUTABLE( gzstats ) +add_executable( sdf2pov sdf2pov.cc ) +target_link_libraries(sdf2pov gazebo_common gazebo_sdf_parser) +INSTALL_EXECUTABLE( sdf2pov ) + diff --git a/tools/sdf2pov.cc b/tools/sdf2pov.cc new file mode 100644 index 0000000000..aa4cb4c89b --- /dev/null +++ b/tools/sdf2pov.cc @@ -0,0 +1,415 @@ +#include "sdf/sdf.h" +#include "sdf/sdf_parser.h" +#include "math/Pose.hh" +#include "common/Console.hh" +#include "common/MeshManager.hh" +#include "common/Mesh.hh" +#include "common/Material.hh" + +std::vector params; + +// 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(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(" %d,\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(" %d,\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(" %d,\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 { tiff \"/home/nkoenig/%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);//mat->GetSpecular().R()); + 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(" <%d, %d, %d>,0\n", subMesh->GetIndex(j), + subMesh->GetIndex(j+1), subMesh->GetIndex(j+2)); + } + else + { + printf(" <%d, %d, %d>\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(" <%d, %d, %d>,\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->GetElement("diffuse")->GetValueColor("rgba"); + specular = _elem->GetElement("specular")->GetValueColor("rgba"); + //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"); + double d = pose.pos.GetDistToPlane(dir, gazebo::math::Vector3(0,0,1), 0); + 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->GetElement("background")->GetValueColor("rgba"); + printf("background { rgb <%f, %f, %f> }\n", color.R(), color.G(), color.B()); + } + + if (_elem->HasElement("ambient")) + { + color = _elem->GetElement("ambient")->GetValueColor("rgba"); + //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(); + } + } + + 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(); + } + } + linkElem = linkElem->GetNextElement(); + } + modelElem = modelElem->GetNextElement(); + } + worldElem = worldElem->GetNextElement(); + } + +}