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

fixed typo for detecting collada file, removed commented out manipulator and sensor collada parsing funtions until urdf api can support them

This commit is contained in:
rdiankov 2010-12-01 21:31:35 +00:00
parent fa86ca0801
commit f53e2c0e6b
4 changed files with 7 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
collada_urdf/manifest.xml
View File

@ -3,6 +3,8 @@
This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents. This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.
Implements robot-specific COLLADA extensions as defined by
http://openrave.programmingvision.com/index.php/Started:COLLADA
</description> </description>
<author>Tim Field</author> <author>Tim Field</author>
<license>BSD</license> <license>BSD</license>

2
urdf/manifest.xml
View File

@ -5,7 +5,7 @@
The code API of the parser has been through our review process and will remain The code API of the parser has been through our review process and will remain
backwards compatible in future releases. backwards compatible in future releases.
</description> </description>
<author>Wim Meeussen, John Hsu</author> <author>Wim Meeussen, John Hsu, Rosen Diankov</author>
<license>BSD</license> <license>BSD</license>
<review status="Doc reviewed" notes=""/> <review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/urdf</url> <url>http://ros.org/wiki/urdf</url>

170
urdf/src/collada_model_reader.cpp
View File

@ -1,7 +1,7 @@
/********************************************************************* /*********************************************************************
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* Copyright (c) 2010, Rosen Diankov, Willow Garage, Inc. * Copyright (c) 2010, University of Tokyo
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
@ -1763,179 +1763,15 @@ protected:
/// \brief extract the robot manipulators /// \brief extract the robot manipulators
void _ExtractRobotManipulators(const domArticulated_systemRef as) void _ExtractRobotManipulators(const domArticulated_systemRef as)
{ {
for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) { ROS_DEBUG("collada manipulators not supported yet");
domExtraRef pextra = as->getExtra_array()[ie];
if( strcmp(pextra->getType(), "manipulator") == 0 ) {
std::string name = pextra->getAttribute("name");
if( name.size() == 0 ) {
name = str(boost::format("manipulator%d")%_nGlobalManipulatorId++);
}
domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
if( !!tec ) {
// RobotBase::ManipulatorPtr pmanip(new RobotBase::Manipulator(probot));
// pmanip->_name = name;
// daeElement* pframe_origin = tec->getChild("frame_origin");
// daeElement* pframe_tip = tec->getChild("frame_tip");
// if( !!pframe_origin ) {
// domLinkRef plink = daeSafeCast<domLink>(daeSidRef(pframe_origin->getAttribute("link"), as).resolve().elt);
// if( !!plink ) {
// pmanip->_pBase = boost::static_pointer_cast<KinBody::Link>(_getUserData(plink)->p);
// }
// if( !pmanip->_pBase ) {
// ROS_WARN_STREAM(str(boost::format("failed to find manipulator %s frame origin %s\n")%name%pframe_origin->getAttribute("link")));
// continue;
// }
// }
// if( !!pframe_tip ) {
// daeElementRef plink = daeSafeCast<domLink>(daeSidRef(pframe_tip->getAttribute("link"), as).resolve().elt);
// if( !!plink ) {
// pmanip->_pEndEffector = boost::static_pointer_cast<KinBody::Link>(_getUserData(plink)->p);
// }
// if( !pmanip->_pEndEffector ) {
// ROS_WARN_STREAM(str(boost::format("failed to find manipulator %s frame tip %s\n")%name%pframe_tip->getAttribute("link")));
// continue;
// }
// pmanip->_tGrasp = _ExtractFullTransformFromChildren(pframe_tip);
// }
//
// for(size_t ic = 0; ic < tec->getContents().getCount(); ++ic) {
// daeElementRef pgripper_axis = tec->getContents()[ic];
// if( pgripper_axis->getElementName() == std::string("gripper_axis") ) {
// domAxis_constraintRef paxis = daeSafeCast<domAxis_constraint>(daeSidRef(pgripper_axis->getAttribute("axis"), as).resolve().elt);
// if( !!paxis ) {
// boost::shared_ptr<int> pdofindex = boost::static_pointer_cast<int>(_getUserData(paxis)->p);
// if( !!pdofindex ) {
// if( *pdofindex < 0 ) {
// ROS_WARN_STREAM(str(boost::format("manipulator %s gripper axis %s references passive joint\n")%name%pgripper_axis->getAttribute("axis")));
// }
// else {
// float closingdirection = 0;
// daeElementRef pclosingdirection = daeElementRef(pgripper_axis->getChild("closingdirection"));
// if( !pclosingdirection || !resolveCommon_float_or_param(pclosingdirection,as,closingdirection) ) {
// ROS_WARN_STREAM(str(boost::format("manipulator %s gripper axis %s failed to process closing direction\n")%name%pgripper_axis->getAttribute("axis")));
// }
// pmanip->_vgripperdofindices.push_back(*pdofindex);
// pmanip->_vClosingDirection.push_back((double)closingdirection);
// }
// continue;
// }
// }
// ROS_WARN_STREAM(str(boost::format("could not find manipulator gripper axis %s\n")%pgripper_axis->getAttribute("axis")));
// }
// }
// probot->GetManipulators().push_back(pmanip);
}
else {
ROS_WARN_STREAM(str(boost::format("cannot create robot %s manipulator %s\n")%_model.name_%name));
}
}
}
} }
/// \brief Extract Sensors attached to a Robot /// \brief Extract Sensors attached to a Robot
void _ExtractRobotAttachedSensors(const domArticulated_systemRef as) void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
{ {
for (size_t ie = 0; ie < as->getExtra_array().getCount(); ie++) { ROS_DEBUG("collada sensors not supported yet");
domExtraRef pextra = as->getExtra_array()[ie];
if( strcmp(pextra->getType(), "sensor") == 0 ) {
std::string name = pextra->getAttribute("name");
if( name.size() == 0 ) {
name = str(boost::format("sensor%d")%_nGlobalSensorId++);
}
domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
if( !!tec ) {
// RobotBase::AttachedSensorPtr pattachedsensor(new RobotBase::AttachedSensor(probot));
// pattachedsensor->_name = name;
// daeElement* pframe_origin = tec->getChild("frame_origin");
// if( !!pframe_origin ) {
// domLinkRef plink = daeSafeCast<domLink>(daeSidRef(pframe_origin->getAttribute("link"), as).resolve().elt);
// if( !!plink ) {
// pattachedsensor->pattachedlink = boost::static_pointer_cast<KinBody::Link>(_getUserData(plink)->p);
// }
// if( !pattachedsensor->pattachedlink.lock() ) {
// ROS_WARN_STREAM(str(boost::format("failed to find manipulator %s frame origin %s\n")%name%pframe_origin->getAttribute("link")));
// continue;
// }
// pattachedsensor->trelative = _ExtractFullTransformFromChildren(pframe_origin);
// }
// if( !_ExtractSensor(pattachedsensor->psensor,tec->getChild("instance_sensor")) ) {
// ROS_WARN_STREAM(str(boost::format("cannot find instance_sensor for attached sensor %s:%s\n")%_model.name_%name));
// }
// else {
// pattachedsensor->pdata = pattachedsensor->GetSensor()->CreateSensorData();
// }
// probot->GetAttachedSensors().push_back(pattachedsensor);
}
else {
ROS_WARN_STREAM(str(boost::format("cannot create robot %s attached sensor %s\n")%_model.name_%name));
}
}
}
} }
/// \brief Extract an instance of a sensor
// bool _ExtractSensor(SensorBasePtr& psensor, daeElementRef instance_sensor)
// {
// if( !instance_sensor ) {
// return false;
// }
// if( instance_sensor->hasAttribute("url") ) {
// ROS_WARN_STREAM("instance_sensor has no url\n");
// return false;
// }
//
// std::string instance_id = instance_sensor->getAttribute("id");
// std::string instance_url = instance_sensor->getAttribute("url");
// daeElementRef domsensor = _getElementFromUrl(daeURI(*instance_sensor,instance_url));
// if( !domsensor ) {
// ROS_WARN_STREAM(str(boost::format("failed to find senor id %s url=%s\n")%instance_id%instance_url));
// return false;
// }
// if( !domsensor->hasAttribute("type") ) {
// ROS_WARN_STREAM("collada <sensor> needs type attribute\n");
// return false;
// }
// psensor = RaveCreateSensor(_penv, domsensor->getAttribute("type"));
// if( !psensor ) {
// return false;
// }
//
// // Create the custom XML reader to read in the data (determined by users)
// BaseXMLReaderPtr pcurreader = RaveCallXMLReader(PT_Sensor,psensor->GetXMLId(),psensor, std::list<std::pair<std::string,std::string> >());
// if( !pcurreader ) {
// pcurreader.reset();
// return false;
// }
// _ProcessXMLReader(pcurreader,domsensor);
// psensor->__mapReadableInterfaces[psensor->GetXMLId()] = pcurreader->GetReadable();
// if( !psensor->Init(instance_sensor->getAttribute("args")) ) {
// ROS_WARN_STREAM(str(boost::format("failed to initialize sensor %s\n")));
// psensor.reset();
// }
// return true;
// }
/// \brief feed the collada data into the base readers xml class
// static void _ProcessXMLReader(BaseXMLReaderPtr preader, daeElementRef elt)
// {
// daeTArray<daeElementRef> children;
// elt->getChildren(children);
// std::list<std::pair<std::string,std::string> > atts;
// for (size_t i = 0; i < children.getCount(); i++) {
// std::string xmltag = tolowerstring(children[i]->getElementName());
// daeTArray<daeElement::attr> domatts;
// children[i]->getAttributes(domatts);
// atts.clear();
// for(size_t j = 0; j < domatts.getCount(); ++j) {
// atts.push_back(std::make_pair(domatts[j].name,domatts[j].value));
// }
// preader->startElement(xmltag,atts);
// _ProcessXMLReader(preader,children[i]);
// preader->characters(children[i]->getCharData());
// preader->endElement(xmltag);
// }
// }
inline daeElementRef _getElementFromUrl(const daeURI &uri) inline daeElementRef _getElementFromUrl(const daeURI &uri)
{ {
return daeStandardURIResolver(*_collada).resolveElement(uri); return daeStandardURIResolver(*_collada).resolveElement(uri);

2
urdf/src/model.cpp
View File

@ -65,7 +65,7 @@ void Model::clear()
bool Model::initFile(const std::string& filename) bool Model::initFile(const std::string& filename)
{ {
// necessary for COLLADA compatibility // necessary for COLLADA compatibility
if( 0&&IsColladaFile(filename) ) { if( IsColladaFile(filename) ) {
return urdfFromColladaFile(filename,*this); return urdfFromColladaFile(filename,*this);
} }
TiXmlDocument xml_doc; TiXmlDocument xml_doc;