bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: more tidying up

This commit is contained in:
tfield 2010-04-23 19:49:15 +00:00
parent c6b4ab66f9
commit 5b33005a0b
5 changed files with 120 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
collada_urdf/include/collada_urdf/ColladaWriter.h
View File

@ -69,34 +69,48 @@ private:
};
public:
ColladaWriter(std::string const& documentName, urdf::Model* robot);
/**
* \brief Create a ColladaWriter using the specified URDF robot model.
*/
ColladaWriter(urdf::Model* robot);
virtual ~ColladaWriter();
/**
* todo
* @return
* \brief Write the model to a COLLADA file.
*
* \param documentName The filename of the document to write to
* \return True if the file was successfully written
*/
bool writeScene();
bool writeDocument(std::string const& documentName);
protected:
virtual void handleError(daeString msg);
virtual void handleWarning(daeString msg);
private:
void initDocument(std::string const& documentName);
SCENE createScene();
void setupPhysics(SCENE scene);
void setupPhysics(SCENE const& scene);
void addGeometries();
void loadMesh(std::string const& filename, domGeometryRef geometry, std::string const& geometry_id);
bool loadMeshWithSTLLoader(resource_retriever::MemoryResource const& resource, domGeometryRef geometry, std::string const& geometry_id);
void buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, std::string const& geometry_id);
void buildMeshFromSTLLoader(boost::shared_ptr<Mesh> stl_mesh, daeElementRef parent, std::string const& geometry_id);
void addKinematics(SCENE const& scene);
void addJoints(daeElementRef parent);
void addBindings(SCENE scene);
void addKinematics(SCENE scene);
void addKinematicLink(boost::shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num);
void addVisuals(SCENE scene);
void addMaterials();
domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse);
void addVisualLink(boost::shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num);
void addVisuals(SCENE const& scene);
void addVisualLink(boost::shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num);
void addMaterials();
domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse);
void addBindings(SCENE const& scene);
domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position);
domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r);

18
collada_urdf/include/collada_urdf/STLLoader.h
View File

@ -40,10 +40,7 @@
#include <string>
#include <vector>
#define LINE_MAX_LEN 256
#define COR3_MAX 200000
#define ORDER_MAX 10
#define FACE_MAX 200000
#include <boost/shared_ptr.hpp>
namespace collada_urdf {
@ -79,13 +76,16 @@ public:
class STLLoader
{
public:
Mesh* load(std::string const& filename);
boost::shared_ptr<Mesh> load(std::string const& filename);
private:
void readBinary (FILE* filein, Mesh* mesh);
uint32_t readLongInt (FILE* filein);
uint16_t readShortInt(FILE* filein);
float readFloat (FILE* filein);
FILE* file_;
boost::shared_ptr<Mesh> mesh_;
void readBinary();
uint32_t readLongInt();
uint16_t readShortInt();
float readFloat();
};
}

49
collada_urdf/src/ColladaWriter.cpp
View File

@ -47,7 +47,26 @@ using boost::shared_ptr;
namespace collada_urdf {
ColladaWriter::ColladaWriter(string const& documentName, urdf::Model* robot) : robot_(robot) {
ColladaWriter::ColladaWriter(urdf::Model* robot) : robot_(robot) { }
bool ColladaWriter::writeDocument(string const& documentName) {
initDocument(documentName);
SCENE scene = createScene();
setupPhysics(scene);
addGeometries();
addKinematics(scene);
addVisuals(scene);
addMaterials();
addBindings(scene);
collada_->writeAll();
return true;
}
void ColladaWriter::initDocument(string const& documentName) {
daeErrorHandler::setErrorHandler(this);
collada_.reset(new DAE());
@ -113,21 +132,6 @@ ColladaWriter::~ColladaWriter() {
DAE::cleanup();
}
bool ColladaWriter::writeScene() {
SCENE scene = createScene();
setupPhysics(scene);
addGeometries();
addKinematics(scene);
addVisuals(scene);
addMaterials();
addBindings(scene);
collada_->writeAll();
return true;
}
// Implementation
ColladaWriter::SCENE ColladaWriter::createScene() {
@ -171,7 +175,7 @@ void ColladaWriter::handleWarning(daeString msg) {
std::cerr << "COLLADA warning: " << msg << std::endl;
}
void ColladaWriter::setupPhysics(SCENE scene) {
void ColladaWriter::setupPhysics(SCENE const& scene) {
// <technique_common>
domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{
@ -258,9 +262,8 @@ bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource con
// Import the mesh using STLLoader
STLLoader loader;
Mesh* stl_mesh = loader.load(string(tmp_filename));
shared_ptr<Mesh> stl_mesh = loader.load(string(tmp_filename));
buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id);
delete stl_mesh;
// Delete the temporary file
unlink(tmp_filename);
@ -268,7 +271,7 @@ bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource con
return true;
}
void ColladaWriter::buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, string const& geometry_id) {
void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElementRef parent, string const& geometry_id) {
// <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH));
{
@ -498,7 +501,7 @@ void ColladaWriter::addJoints(daeElementRef parent) {
}
}
void ColladaWriter::addBindings(SCENE scene) {
void ColladaWriter::addBindings(SCENE const& scene) {
string model_id = kmodel_->getID();
string inst_model_sid = string("inst_") + model_id;
@ -538,7 +541,7 @@ void ColladaWriter::addBindings(SCENE scene) {
}
}
void ColladaWriter::addKinematics(SCENE scene) {
void ColladaWriter::addKinematics(SCENE const& scene) {
// <kinematics_model id="k1" name="pr2">
domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL));
kmodel->setId("k1");
@ -632,7 +635,7 @@ void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, dae
// </link>
}
void ColladaWriter::addVisuals(SCENE scene) {
void ColladaWriter::addVisuals(SCENE const& scene) {
// <node id="v1" name="pr2">
domNodeRef root_node = daeSafeCast<domNode>(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE));
root_node->setId("v1");

77
collada_urdf/src/STLLoader.cpp
View File

@ -39,14 +39,21 @@
#include "collada_urdf/STLLoader.h"
using std::string;
using boost::shared_ptr;
namespace collada_urdf {
// Vector3
Vector3::Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { }
bool Vector3::operator==(const Vector3& v) const {
bool Vector3::operator==(Vector3 const& v) const {
return x == v.x && y == v.y && z == v.z;
}
// Mesh
Mesh::Mesh() {
}
@ -58,70 +65,74 @@ int Mesh::getVertexIndex(const Vector3& v) const {
return -1;
}
void Mesh::addVertex(const Vector3& v) { vertices.push_back(v); }
void Mesh::addNormal(const Vector3& n) { normals.push_back(n); }
void Mesh::addVertex(Vector3 const& v) { vertices.push_back(v); }
void Mesh::addNormal(Vector3 const& n) { normals.push_back(n); }
void Mesh::addIndex(unsigned int i) { indices.push_back(i); }
Mesh* STLLoader::load(const std::string& filename) {
Mesh* mesh = new Mesh();
// STLLoader
FILE* file = fopen(filename.c_str(), "r");
readBinary(file, mesh);
fclose(file);
shared_ptr<Mesh> STLLoader::load(std::string const& filename) {
mesh_ = shared_ptr<Mesh>(new Mesh);
return mesh;
file_ = fopen(filename.c_str(), "r");
readBinary();
fclose(file_);
file_ = NULL;
return mesh_;
}
void STLLoader::readBinary(FILE* filein, Mesh* mesh) {
void STLLoader::readBinary() {
// 80 byte Header
for (int i = 0; i < 80; i++)
fgetc(filein);
fgetc(file_);
int face_num = readLongInt(filein);
int face_num = readLongInt();
for (int iface = 0; iface < face_num; iface++) {
Vector3 normal(readFloat(filein), readFloat(filein), readFloat(filein));
Vector3 normal(readFloat(), readFloat(), readFloat());
for (int i = 0; i < 3; i++) {
double x = readFloat(filein);
double y = readFloat(filein);
double z = readFloat(filein);
Vector3 vertex(x,y,z);
int index = mesh->getVertexIndex(vertex);
Vector3 vertex(readFloat(), readFloat(), readFloat());
int index = mesh_->getVertexIndex(vertex);
if (index == -1) {
mesh->addVertex(vertex);
mesh->addNormal(normal);
index = mesh->vertices.size() - 1;
mesh_->addVertex(vertex);
mesh_->addNormal(normal);
index = mesh_->vertices.size() - 1;
}
mesh->addIndex(index);
mesh_->addIndex(index);
}
readShortInt(filein); // 2 byte attribute
readShortInt(); // 2 byte attribute
}
}
float STLLoader::readFloat(FILE* filein) {
float STLLoader::readFloat() {
float rval;
if (fread(&rval, sizeof(float), 1, filein) == 0)
if (fread(&rval, sizeof(float), 1, file_) == 0)
std::cerr << "Error in STLLoader::readFloat" << std::endl;
return rval;
}
uint32_t STLLoader::readLongInt(FILE* filein) {
uint32_t STLLoader::readLongInt() {
union
{
uint32_t yint;
char ychar[4];
} y;
y.ychar[0] = fgetc(filein);
y.ychar[1] = fgetc(filein);
y.ychar[2] = fgetc(filein);
y.ychar[3] = fgetc(filein);
y.ychar[0] = fgetc(file_);
y.ychar[1] = fgetc(file_);
y.ychar[2] = fgetc(file_);
y.ychar[3] = fgetc(file_);
return y.yint;
}
uint16_t STLLoader::readShortInt(FILE* filein) {
uint8_t c1 = fgetc(filein);
uint8_t c2 = fgetc(filein);
uint16_t STLLoader::readShortInt() {
uint8_t c1 = fgetc(file_);
uint8_t c2 = fgetc(file_);
uint16_t ival = c1 | (c2 << 8);

23
collada_urdf/src/urdf_to_collada.cpp
View File

@ -41,25 +41,32 @@ int main(int argc, char** argv)
return -1;
}
std::string input_filename(argv[1]);
std::string output_filename(argv[2]);
TiXmlDocument robot_model_xml;
robot_model_xml.LoadFile(argv[1]);
if (!robot_model_xml.LoadFile(input_filename.c_str())) {
std::cerr << "Error opening file " << argv[1] << std::endl;
return -1;
}
TiXmlElement* robot_xml = robot_model_xml.FirstChildElement("robot");
if (!robot_xml) {
std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
std::cerr << "Error parsing URDF model from XML" << std::endl;
return -1;
}
urdf::Model robot;
if (!robot.initXml(robot_xml)) {
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
std::cerr << "Error parsing URDF model from XML" << std::endl;
return -1;
}
std::string output_filename(argv[2]);
collada_urdf::ColladaWriter* writer = new collada_urdf::ColladaWriter(output_filename, &robot);
writer->writeScene();
delete writer;
collada_urdf::ColladaWriter writer(&robot);
if (!writer.writeDocument(output_filename)) {
std::cerr << "Error writing document" << std::endl;
return -1;
}
return 0;
}