diff --git a/collada_urdf/manifest.xml b/collada_urdf/manifest.xml
index 1507da3..7addbdb 100644
--- a/collada_urdf/manifest.xml
+++ b/collada_urdf/manifest.xml
@@ -3,6 +3,8 @@
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
Tim Field
BSD
diff --git a/urdf/manifest.xml b/urdf/manifest.xml
index d03ca3f..f857168 100644
--- a/urdf/manifest.xml
+++ b/urdf/manifest.xml
@@ -5,7 +5,7 @@
The code API of the parser has been through our review process and will remain
backwards compatible in future releases.
- Wim Meeussen, John Hsu
+ Wim Meeussen, John Hsu, Rosen Diankov
BSD
http://ros.org/wiki/urdf
diff --git a/urdf/src/collada_model_reader.cpp b/urdf/src/collada_model_reader.cpp
index 6f03f61..c5cc462 100644
--- a/urdf/src/collada_model_reader.cpp
+++ b/urdf/src/collada_model_reader.cpp
@@ -1,7 +1,7 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
-* Copyright (c) 2010, Rosen Diankov, Willow Garage, Inc.
+* Copyright (c) 2010, University of Tokyo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -1763,179 +1763,15 @@ protected:
/// \brief extract the robot manipulators
void _ExtractRobotManipulators(const domArticulated_systemRef as)
{
- for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
- 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(daeSidRef(pframe_origin->getAttribute("link"), as).resolve().elt);
-// if( !!plink ) {
-// pmanip->_pBase = boost::static_pointer_cast(_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(daeSidRef(pframe_tip->getAttribute("link"), as).resolve().elt);
-// if( !!plink ) {
-// pmanip->_pEndEffector = boost::static_pointer_cast(_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(daeSidRef(pgripper_axis->getAttribute("axis"), as).resolve().elt);
-// if( !!paxis ) {
-// boost::shared_ptr pdofindex = boost::static_pointer_cast(_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));
- }
- }
- }
+ ROS_DEBUG("collada manipulators not supported yet");
}
/// \brief Extract Sensors attached to a Robot
void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
{
- for (size_t ie = 0; ie < as->getExtra_array().getCount(); ie++) {
- 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(daeSidRef(pframe_origin->getAttribute("link"), as).resolve().elt);
-// if( !!plink ) {
-// pattachedsensor->pattachedlink = boost::static_pointer_cast(_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));
- }
- }
- }
+ ROS_DEBUG("collada sensors not supported yet");
}
- /// \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 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 >());
-// 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 children;
-// elt->getChildren(children);
-// std::list > atts;
-// for (size_t i = 0; i < children.getCount(); i++) {
-// std::string xmltag = tolowerstring(children[i]->getElementName());
-// daeTArray 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)
{
return daeStandardURIResolver(*_collada).resolveElement(uri);
diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp
index f898828..c4106d7 100644
--- a/urdf/src/model.cpp
+++ b/urdf/src/model.cpp
@@ -65,7 +65,7 @@ void Model::clear()
bool Model::initFile(const std::string& filename)
{
// necessary for COLLADA compatibility
- if( 0&&IsColladaFile(filename) ) {
+ if( IsColladaFile(filename) ) {
return urdfFromColladaFile(filename,*this);
}
TiXmlDocument xml_doc;