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

Use urdf::*ShredPtr instead of boost::shared_ptr (#144) 

* Use urdf::*SharedPtr instead of boost::shared_ptr

urdfdom_headers uses C++ std::shared_ptr. As it exports it as a custom
*SharedPtr type, we can use them to stay compatible.

* Add compatibility urdf::VoidSharedPtr

* Use urdfdom compatibility header

* Build kinetic for kinetic branch

* Add missing namespace and includes

* Revert "Build kinetic for kinetic branch"

This reverts commit f1c085ccdbe282de8f17b2150aa0ba6ba453db64.

* Add build dependency

* Revert "Add build dependency"

This reverts commit 94175094ffc656e9173c3a46684116471e68279f.

* Export urdfdom version to header

* Add dummy version in case it's not set by urdfdom_headers

* Add missing header
This commit is contained in:
Jochen Sprickerhof 2017-01-05 01:41:49 +01:00 committed by William Woodall
parent 97fff6d2d5
commit 409c4b923c
14 changed files with 90 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
collada_parser/CMakeLists.txt
View File

@ -3,7 +3,7 @@ project(collada_parser)
find_package(Boost REQUIRED system) find_package(Boost REQUIRED system)
find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin roscpp class_loader) find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin roscpp class_loader urdf)
find_package(urdfdom_headers REQUIRED) find_package(urdfdom_headers REQUIRED)
catkin_package( catkin_package(

4
collada_parser/include/collada_parser/collada_parser.h
View File

@ -41,13 +41,13 @@
#include <map> #include <map>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <urdf_model/model.h> #include <urdf/urdfdom_compatibility.h>
namespace urdf { namespace urdf {
/// \brief Load Model from string /// \brief Load Model from string
boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_string ); urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string );
} }

2
collada_parser/include/collada_parser/collada_parser_plugin.h
View File

@ -46,7 +46,7 @@ class ColladaURDFParser : public URDFParser
{ {
public: public:
virtual boost::shared_ptr<ModelInterface> parse(const std::string &xml_string); virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string);
}; };
} }

1
collada_parser/package.xml
View File

@ -25,6 +25,7 @@
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>urdf_parser_plugin</build_depend> <build_depend>urdf_parser_plugin</build_depend>
<build_depend>class_loader</build_depend> <build_depend>class_loader</build_depend>
<build_depend>urdf</build_depend>
<run_depend>collada-dom</run_depend> <run_depend>collada-dom</run_depend>
<run_depend>liburdfdom-headers-dev</run_depend> <run_depend>liburdfdom-headers-dev</run_depend>

56
collada_parser/src/collada_parser.cpp
View File

@ -176,7 +176,11 @@ public:
USERDATA(double scale) : scale(scale) { USERDATA(double scale) : scale(scale) {
} }
double scale; double scale;
#if URDFDOM_HEADERS_MAJOR_VERSION < 1
boost::shared_ptr<void> p; ///< custom managed data boost::shared_ptr<void> p; ///< custom managed data
#else
std::shared_ptr<void> p; ///< custom managed data
#endif
}; };
enum GeomType { enum GeomType {
@ -409,7 +413,7 @@ public:
}; };
public: public:
ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) { ColladaModelReader(urdf::ModelInterfaceSharedPtr model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
daeErrorHandler::setErrorHandler(this); daeErrorHandler::setErrorHandler(this);
_resourcedir = "."; _resourcedir = ".";
} }
@ -715,7 +719,7 @@ protected:
} }
// find the target joint // find the target joint
boost::shared_ptr<Joint> pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf); urdf::JointSharedPtr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
if (!pjoint) { if (!pjoint) {
continue; continue;
} }
@ -785,7 +789,7 @@ protected:
} }
BOOST_ASSERT(psymboljoint->hasAttribute("encoding")); BOOST_ASSERT(psymboljoint->hasAttribute("encoding"));
BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA")); BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA"));
boost::shared_ptr<Joint> pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf); urdf::JointSharedPtr pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
if( !!pbasejoint ) { if( !!pbasejoint ) {
// set the mimic properties // set the mimic properties
pjoint->mimic.reset(new JointMimic()); pjoint->mimic.reset(new JointMimic());
@ -801,7 +805,7 @@ protected:
} }
/// \brief Extract Link info and add it to an existing body /// \brief Extract Link info and add it to an existing body
boost::shared_ptr<Link> _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) { urdf::LinkSharedPtr _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) {
const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings; const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
// Set link name with the name of the COLLADA's Link // Set link name with the name of the COLLADA's Link
std::string linkname = _ExtractLinkName(pdomlink); std::string linkname = _ExtractLinkName(pdomlink);
@ -817,7 +821,7 @@ protected:
} }
} }
boost::shared_ptr<Link> plink; urdf::LinkSharedPtr plink;
_model->getLink(linkname,plink); _model->getLink(linkname,plink);
if( !plink ) { if( !plink ) {
plink.reset(new Link()); plink.reset(new Link());
@ -921,7 +925,7 @@ protected:
if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) { if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) {
ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint())); ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint()));
return boost::shared_ptr<Link>(); return urdf::LinkSharedPtr();
} }
// get direct child link // get direct child link
@ -952,7 +956,7 @@ protected:
} }
// create the joints before creating the child links // create the joints before creating the child links
std::vector<boost::shared_ptr<Joint> > vjoints(vdomaxes.getCount()); std::vector<urdf::JointSharedPtr > vjoints(vdomaxes.getCount());
for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
bool joint_active = true; // if not active, put into the passive list bool joint_active = true; // if not active, put into the passive list
FOREACHC(itaxisbinding,listAxisBindings) { FOREACHC(itaxisbinding,listAxisBindings) {
@ -966,7 +970,7 @@ protected:
} }
} }
boost::shared_ptr<Joint> pjoint(new Joint()); urdf::JointSharedPtr pjoint(new Joint());
pjoint->limits.reset(new JointLimits()); pjoint->limits.reset(new JointLimits());
pjoint->limits->velocity = 0.0; pjoint->limits->velocity = 0.0;
pjoint->limits->effort = 0.0; pjoint->limits->effort = 0.0;
@ -995,12 +999,16 @@ protected:
} }
_getUserData(pdomjoint)->p = pjoint; _getUserData(pdomjoint)->p = pjoint;
#if URDFDOM_HEADERS_MAJOR_VERSION < 1
_getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size())); _getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size()));
#else
_getUserData(pdomaxis)->p = std::shared_ptr<int>(new int(_model->joints_.size()));
#endif
_model->joints_[pjoint->name] = pjoint; _model->joints_[pjoint->name] = pjoint;
vjoints[ic] = pjoint; vjoints[ic] = pjoint;
} }
boost::shared_ptr<Link> pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings); urdf::LinkSharedPtr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
if (!pchildlink) { if (!pchildlink) {
ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name)); ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name));
@ -1035,7 +1043,7 @@ protected:
} }
ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic)); ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic));
boost::shared_ptr<Joint> pjoint = vjoints[ic]; urdf::JointSharedPtr pjoint = vjoints[ic];
pjoint->child_link_name = pchildlink->name; pjoint->child_link_name = pchildlink->name;
#define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \ #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \
@ -1154,8 +1162,8 @@ protected:
plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties); plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
// visual_groups deprecated // visual_groups deprecated
//boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss; //boost::shared_ptr<std::vector<urdf::VisualSharedPtr > > viss;
//viss.reset(new std::vector<boost::shared_ptr<Visual > >); //viss.reset(new std::vector<urdf::VisualSharedPtr >);
//viss->push_back(plink->visual); //viss->push_back(plink->visual);
//plink->visual_groups.insert(std::make_pair("default", viss)); //plink->visual_groups.insert(std::make_pair("default", viss));
@ -1170,15 +1178,15 @@ protected:
} }
// collision_groups deprecated // collision_groups deprecated
//boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols; //boost::shared_ptr<std::vector<urdf::CollisionSharedPtr > > cols;
//cols.reset(new std::vector<boost::shared_ptr<Collision > >); //cols.reset(new std::vector<urdf::CollisionSharedPtr >);
//cols->push_back(plink->collision); //cols->push_back(plink->collision);
//plink->collision_groups.insert(std::make_pair("default", cols)); //plink->collision_groups.insert(std::make_pair("default", cols));
return plink; return plink;
} }
boost::shared_ptr<Geometry> _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties) urdf::GeometrySharedPtr _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
{ {
std::vector<std::vector<Vector3> > vertices; std::vector<std::vector<Vector3> > vertices;
std::vector<std::vector<int> > indices; std::vector<std::vector<int> > indices;
@ -1219,12 +1227,12 @@ protected:
} }
if (vert_counter == 0) { if (vert_counter == 0) {
boost::shared_ptr<Mesh> ret; urdf::MeshSharedPtr ret;
ret.reset(); ret.reset();
return ret; return ret;
} }
boost::shared_ptr<Mesh> geometry(new Mesh()); urdf::MeshSharedPtr geometry(new Mesh());
geometry->type = Geometry::MESH; geometry->type = Geometry::MESH;
geometry->scale.x = 1; geometry->scale.x = 1;
geometry->scale.y = 1; geometry->scale.y = 1;
@ -2020,7 +2028,7 @@ protected:
//std::string aname = pextra->getAttribute("name"); //std::string aname = pextra->getAttribute("name");
domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array()); domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
if( !!tec ) { if( !!tec ) {
boost::shared_ptr<Joint> pjoint; urdf::JointSharedPtr pjoint;
daeElementRef domactuator; daeElementRef domactuator;
{ {
daeElementRef bact = tec->getChild("bind_actuator"); daeElementRef bact = tec->getChild("bind_actuator");
@ -2413,7 +2421,7 @@ protected:
return name.substr(pos+1)==type; return name.substr(pos+1)==type;
} }
boost::shared_ptr<Joint> _getJointFromRef(xsToken targetref, daeElementRef peltref) { urdf::JointSharedPtr _getJointFromRef(xsToken targetref, daeElementRef peltref) {
daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt; daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint); domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
@ -2426,10 +2434,10 @@ protected:
if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) { if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref)); ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref));
return boost::shared_ptr<Joint>(); return urdf::JointSharedPtr();
} }
boost::shared_ptr<Joint> pjoint; urdf::JointSharedPtr pjoint;
std::string name(pdomjoint->getName()); std::string name(pdomjoint->getName());
if (_model->joints_.find(name) == _model->joints_.end()) { if (_model->joints_.find(name) == _model->joints_.end()) {
pjoint.reset(); pjoint.reset();
@ -2797,7 +2805,7 @@ protected:
int _nGlobalSensorId, _nGlobalManipulatorId; int _nGlobalSensorId, _nGlobalManipulatorId;
std::string _filename; std::string _filename;
std::string _resourcedir; std::string _resourcedir;
boost::shared_ptr<ModelInterface> _model; urdf::ModelInterfaceSharedPtr _model;
Pose _RootOrigin; Pose _RootOrigin;
Pose _VisualRootOrigin; Pose _VisualRootOrigin;
}; };
@ -2805,9 +2813,9 @@ protected:
boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str) urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_str)
{ {
boost::shared_ptr<ModelInterface> model(new ModelInterface); urdf::ModelInterfaceSharedPtr model(new ModelInterface);
ColladaModelReader reader(model); ColladaModelReader reader(model);
if (!reader.InitFromData(xml_str)) if (!reader.InitFromData(xml_str))

2
collada_parser/src/collada_parser_plugin.cpp
View File

@ -38,7 +38,7 @@
#include "collada_parser/collada_parser.h" #include "collada_parser/collada_parser.h"
#include <class_loader/class_loader.h> #include <class_loader/class_loader.h>
boost::shared_ptr<urdf::ModelInterface> urdf::ColladaURDFParser::parse(const std::string &xml_string) urdf::ModelInterfaceSharedPtr urdf::ColladaURDFParser::parse(const std::string &xml_string)
{ {
return urdf::parseCollada(xml_string); return urdf::parseCollada(xml_string);
} }

14
collada_urdf/src/collada_to_urdf.cpp
View File

@ -188,7 +188,7 @@ void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz,
} }
} }
void addChildLinkNamesXML(boost::shared_ptr<const Link> link, ofstream& os) void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
{ {
os << " <link name=\"" << link->name << "\">" << endl; os << " <link name=\"" << link->name << "\">" << endl;
if ( !!link->visual ) { if ( !!link->visual ) {
@ -405,14 +405,14 @@ void addChildLinkNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
} }
#endif #endif
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
addChildLinkNamesXML(*child, os); addChildLinkNamesXML(*child, os);
} }
void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os) void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
{ {
double r, p, y; double r, p, y;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
(*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
std::string jtype; std::string jtype;
if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) { if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) {
@ -443,7 +443,7 @@ void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
os << " <axis xyz=\"" << (*child)->parent_joint->axis.x << " "; os << " <axis xyz=\"" << (*child)->parent_joint->axis.x << " ";
os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl; os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl;
{ {
boost::shared_ptr<urdf::Joint> jt((*child)->parent_joint); urdf::JointSharedPtr jt((*child)->parent_joint);
if ( !!jt->limits ) { if ( !!jt->limits ) {
os << " <limit "; os << " <limit ";
@ -501,7 +501,7 @@ void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
} }
} }
void printTreeXML(boost::shared_ptr<const Link> link, string name, string file) void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
{ {
std::ofstream os; std::ofstream os;
os.open(file.c_str()); os.open(file.c_str());
@ -667,7 +667,7 @@ int main(int argc, char** argv)
} }
xml_file.close(); xml_file.close();
boost::shared_ptr<ModelInterface> robot; urdf::ModelInterfaceSharedPtr robot;
if( xml_string.find("<COLLADA") != std::string::npos ) if( xml_string.find("<COLLADA") != std::string::npos )
{ {
ROS_DEBUG("Parsing robot collada xml string"); ROS_DEBUG("Parsing robot collada xml string");

32
collada_urdf/src/collada_urdf.cpp
View File

@ -544,7 +544,7 @@ private:
domInstance_with_extraRef piscene; domInstance_with_extraRef piscene;
}; };
typedef std::map< boost::shared_ptr<const urdf::Link>, urdf::Pose > MAPLINKPOSES; typedef std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES;
struct LINKOUTPUT struct LINKOUTPUT
{ {
list<pair<int,string> > listusedlinks; list<pair<int,string> > listusedlinks;
@ -568,7 +568,7 @@ private:
axis_output() : iaxis(0) { axis_output() : iaxis(0) {
} }
string sid, nodesid; string sid, nodesid;
boost::shared_ptr<const urdf::Joint> pjoint; urdf::JointConstSharedPtr pjoint;
int iaxis; int iaxis;
string jointnodesid; string jointnodesid;
}; };
@ -794,7 +794,7 @@ protected:
for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof)); string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof));
boost::shared_ptr<const urdf::Joint> pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint; urdf::JointConstSharedPtr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof); BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof);
//int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis; //int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis;
@ -972,7 +972,7 @@ protected:
kmout->vlinksids.resize(_robot.links_.size()); kmout->vlinksids.resize(_robot.links_.size());
FOREACHC(itjoint, _robot.joints_) { FOREACHC(itjoint, _robot.joints_) {
boost::shared_ptr<urdf::Joint> pjoint = itjoint->second; urdf::JointSharedPtr pjoint = itjoint->second;
int index = _mapjointindices[itjoint->second]; int index = _mapjointindices[itjoint->second];
domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT)); domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
string jointid = _ComputeId(pjoint->name); //str(boost::format("joint%d")%index); string jointid = _ComputeId(pjoint->name); //str(boost::format("joint%d")%index);
@ -1045,7 +1045,7 @@ protected:
// create the formulas for all mimic joints // create the formulas for all mimic joints
FOREACHC(itjoint, _robot.joints_) { FOREACHC(itjoint, _robot.joints_) {
string jointsid = _ComputeId(itjoint->second->name); string jointsid = _ComputeId(itjoint->second->name);
boost::shared_ptr<urdf::Joint> pjoint = itjoint->second; urdf::JointSharedPtr pjoint = itjoint->second;
if( !pjoint->mimic ) { if( !pjoint->mimic ) {
continue; continue;
} }
@ -1131,7 +1131,7 @@ protected:
/// \param pkinparent Kinbody parent /// \param pkinparent Kinbody parent
/// \param pnodeparent Node parent /// \param pnodeparent Node parent
/// \param strModelUri /// \param strModelUri
virtual LINKOUTPUT _WriteLink(boost::shared_ptr<const urdf::Link> plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri) virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
{ {
LINKOUTPUT out; LINKOUTPUT out;
int linkindex = _maplinkindices[plink]; int linkindex = _maplinkindices[plink];
@ -1147,8 +1147,8 @@ protected:
pnode->setSid(nodesid.c_str()); pnode->setSid(nodesid.c_str());
pnode->setName(plink->name.c_str()); pnode->setName(plink->name.c_str());
boost::shared_ptr<urdf::Geometry> geometry; urdf::GeometrySharedPtr geometry;
boost::shared_ptr<urdf::Material> material; urdf::MaterialSharedPtr material;
urdf::Pose geometry_origin; urdf::Pose geometry_origin;
if( !!plink->visual ) { if( !!plink->visual ) {
geometry = plink->visual->geometry; geometry = plink->visual->geometry;
@ -1167,7 +1167,7 @@ protected:
if ( !!plink->visual ) { if ( !!plink->visual ) {
if (plink->visual_array.size() > 1) { if (plink->visual_array.size() > 1) {
int igeom = 0; int igeom = 0;
for (std::vector<boost::shared_ptr<urdf::Visual > >::const_iterator it = plink->visual_array.begin(); for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin();
it != plink->visual_array.end(); it++) { it != plink->visual_array.end(); it++) {
// geom // geom
string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
@ -1214,7 +1214,7 @@ protected:
// process all children // process all children
FOREACHC(itjoint, plink->child_joints) { FOREACHC(itjoint, plink->child_joints) {
boost::shared_ptr<urdf::Joint> pjoint = *itjoint; urdf::JointSharedPtr pjoint = *itjoint;
int index = _mapjointindices[pjoint]; int index = _mapjointindices[pjoint];
// <attachment_full joint="k1/joint0"> // <attachment_full joint="k1/joint0">
@ -1275,7 +1275,7 @@ protected:
return out; return out;
} }
domGeometryRef _WriteGeometry(boost::shared_ptr<urdf::Geometry> geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL) domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL)
{ {
domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY)); domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
cgeometry->setId(geometry_id.c_str()); cgeometry->setId(geometry_id.c_str());
@ -1314,7 +1314,7 @@ protected:
return cgeometry; return cgeometry;
} }
void _WriteMaterial(const string& geometry_id, boost::shared_ptr<urdf::Material> material) void _WriteMaterial(const string& geometry_id, urdf::MaterialSharedPtr material)
{ {
string effid = geometry_id+string("_eff"); string effid = geometry_id+string("_eff");
string matid = geometry_id+string("_mat"); string matid = geometry_id+string("_mat");
@ -1392,7 +1392,7 @@ protected:
rigid_body->setSid(rigidsid.c_str()); rigid_body->setSid(rigidsid.c_str());
rigid_body->setName(itlink->second->name.c_str()); rigid_body->setName(itlink->second->name.c_str());
domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
boost::shared_ptr<urdf::Inertial> inertial = itlink->second->inertial; urdf::InertialSharedPtr inertial = itlink->second->inertial;
if( !!inertial ) { if( !!inertial ) {
daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true)); //!!inertial)); daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true)); //!!inertial));
domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS)); domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
@ -1922,9 +1922,9 @@ private:
boost::shared_ptr<instance_kinematics_model_output> _ikmout; boost::shared_ptr<instance_kinematics_model_output> _ikmout;
boost::shared_ptr<instance_articulated_system_output> _iasout; boost::shared_ptr<instance_articulated_system_output> _iasout;
std::map< boost::shared_ptr<const urdf::Joint>, int > _mapjointindices; std::map< urdf::JointConstSharedPtr, int > _mapjointindices;
std::map< boost::shared_ptr<const urdf::Link>, int > _maplinkindices; std::map< urdf::LinkConstSharedPtr, int > _maplinkindices;
std::map< boost::shared_ptr<const urdf::Material>, int > _mapmaterialindices; std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices;
Assimp::Importer _importer; Assimp::Importer _importer;
}; };

9
kdl_parser/src/kdl_parser.cpp
View File

@ -36,6 +36,7 @@
#include "kdl_parser/kdl_parser.hpp" #include "kdl_parser/kdl_parser.hpp"
#include <urdf/model.h> #include <urdf/model.h>
#include <urdf/urdfdom_compatibility.h>
#include <kdl/frames_io.hpp> #include <kdl/frames_io.hpp>
#include <ros/console.h> #include <ros/console.h>
@ -64,7 +65,7 @@ Frame toKdl(urdf::Pose p)
} }
// construct joint // construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt) Joint toKdl(urdf::JointSharedPtr jnt)
{ {
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform); Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
@ -93,7 +94,7 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
} }
// construct inertia // construct inertia
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i) RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
{ {
Frame origin = toKdl(i->origin); Frame origin = toKdl(i->origin);
@ -124,9 +125,9 @@ RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
// recursive function to walk through tree // recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree) bool addChildrenToTree(urdf::LinkConstSharedPtr root, Tree& tree)
{ {
std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links; std::vector<urdf::LinkSharedPtr > children = root->child_links;
ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size()); ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
// constructs the optional inertia // constructs the optional inertia

12
urdf/CMakeLists.txt
View File

@ -10,13 +10,13 @@ find_package(catkin REQUIRED COMPONENTS
find_package(TinyXML REQUIRED) find_package(TinyXML REQUIRED)
if("${urdfdom_headers_VERSION}" VERSION_LESS "1.0") # Find version components
# for Wily: maintain compatibility between urdfdom 0.3 and 0.4 (definining SharedPtr types) if(NOT urdfdom_headers_VERSION)
set(URDFDOM_DECLARE_TYPES 1) set(urdfdom_headers_VERSION "0.0.0")
else()
# declare ModelInterface's SharedPtr
set(URDFDOM_DECLARE_TYPES 0)
endif() endif()
string(REGEX REPLACE "^([0-9]+).*" "\\1" URDFDOM_HEADERS_MAJOR_VERSION "${urdfdom_headers_VERSION}")
string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_MINOR_VERSION "${urdfdom_headers_VERSION}")
string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_REVISION_VERSION "${urdfdom_headers_VERSION}")
set(generated_compat_header "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/urdfdom_compatibility.h") set(generated_compat_header "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/urdfdom_compatibility.h")
include_directories("${CATKIN_DEVEL_PREFIX}/include") include_directories("${CATKIN_DEVEL_PREFIX}/include")
configure_file(urdfdom_compatibility.h.in "${generated_compat_header}" @ONLY) configure_file(urdfdom_compatibility.h.in "${generated_compat_header}" @ONLY)

2
urdf/src/model.cpp
View File

@ -136,7 +136,7 @@ bool Model::initXml(TiXmlElement *robot_xml)
bool Model::initString(const std::string& xml_string) bool Model::initString(const std::string& xml_string)
{ {
boost::shared_ptr<ModelInterface> model; urdf::ModelInterfaceSharedPtr model;
// necessary for COLLADA compatibility // necessary for COLLADA compatibility
if( IsColladaData(xml_string) ) { if( IsColladaData(xml_string) ) {

8
urdf/test/test_robot_model_parser.cpp
View File

@ -54,7 +54,7 @@ public:
bool checkModel(urdf::Model & robot) bool checkModel(urdf::Model & robot)
{ {
// get root link // get root link
boost::shared_ptr<const Link> root_link = robot.getRoot(); urdf::LinkConstSharedPtr root_link = robot.getRoot();
if (!root_link) if (!root_link)
{ {
ROS_ERROR("no root link %s", robot.getName().c_str()); ROS_ERROR("no root link %s", robot.getName().c_str());
@ -79,12 +79,12 @@ protected:
{ {
} }
bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0) bool traverse_tree(urdf::LinkConstSharedPtr link,int level = 0)
{ {
ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size()); ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
level+=2; level+=2;
bool retval = true; bool retval = true;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) for (std::vector<urdf::LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{ {
++num_links; ++num_links;
if (*child && (*child)->parent_joint) if (*child && (*child)->parent_joint)
@ -142,7 +142,7 @@ TEST_F(TestParser, test)
ASSERT_TRUE(robot.initFile(folder + file)); ASSERT_TRUE(robot.initFile(folder + file));
EXPECT_EQ(robot.getName(), robot_name); EXPECT_EQ(robot.getName(), robot_name);
boost::shared_ptr<const urdf::Link> root = robot.getRoot(); urdf::LinkConstSharedPtr root = robot.getRoot();
ASSERT_TRUE(static_cast<bool>(root)); ASSERT_TRUE(static_cast<bool>(root));
EXPECT_EQ(root->name, root_name); EXPECT_EQ(root->name, root_name);

12
urdf/urdfdom_compatibility.h.in
View File

@ -37,7 +37,12 @@
#ifndef URDF_URDFDOM_COMPATIBILITY_ #ifndef URDF_URDFDOM_COMPATIBILITY_
#define URDF_URDFDOM_COMPATIBILITY_ #define URDF_URDFDOM_COMPATIBILITY_
#if @URDFDOM_DECLARE_TYPES@ // This is needed for urdfdom <= 0.4 #define URDFDOM_HEADERS_MAJOR_VERSION @URDFDOM_HEADERS_MAJOR_VERSION@
#define URDFDOM_HEADERS_MINOR_VERSION @URDFDOM_HEADERS_MINOR_VERSION@
#define URDFDOM_HEADERS_REVISION_VERSION @URDFDOM_HEADERS_REVISION_VERSION@
// for Wily: maintain compatibility between urdfdom 0.3 and 0.4 (definining SharedPtr types)
#if URDFDOM_HEADERS_MAJOR_VERSION == 0 && URDFDOM_HEADERS_MINOR_VERSION <= 4
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
@ -75,9 +80,14 @@ URDF_TYPEDEF_CLASS_POINTER(ModelInterface);
#else // urdfdom <= 0.4 #else // urdfdom <= 0.4
#include <urdf_model/types.h>
#include <urdf_world/types.h>
namespace urdf {
typedef std::shared_ptr<ModelInterface> ModelInterfaceSharedPtr; typedef std::shared_ptr<ModelInterface> ModelInterfaceSharedPtr;
typedef std::shared_ptr<const ModelInterface> ModelInterfaceConstSharedPtr; typedef std::shared_ptr<const ModelInterface> ModelInterfaceConstSharedPtr;
typedef std::weak_ptr<ModelInterface> ModelInterfaceWeakPtr; typedef std::weak_ptr<ModelInterface> ModelInterfaceWeakPtr;
}
#endif // urdfdom > 0.4 #endif // urdfdom > 0.4

4
urdf_parser_plugin/include/urdf_parser_plugin/parser.h
View File

@ -37,7 +37,7 @@
#ifndef URDF_PARSER_PLUGIN_H #ifndef URDF_PARSER_PLUGIN_H
#define URDF_PARSER_PLUGIN_H #define URDF_PARSER_PLUGIN_H
#include <urdf_model/model.h> #include <urdf/urdfdom_compatibility.h>
namespace urdf namespace urdf
{ {
@ -54,7 +54,7 @@ public:
} }
/// \brief Load Model from string /// \brief Load Model from string
virtual boost::shared_ptr<ModelInterface> parse(const std::string &xml_string) = 0; virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string) = 0;
}; };
} }