srdf_writer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman */
00036 
00037 #include <tinyxml.h>
00038 #include <srdfdom/srdf_writer.h>
00039 
00040 namespace srdf
00041 {
00042 
00043 // ******************************************************************************************
00044 // Constructor
00045 // ******************************************************************************************
00046 SRDFWriter::SRDFWriter()
00047 {
00048   // Intialize the SRDF model
00049   srdf_model_.reset( new srdf::Model() );
00050 }
00051 
00052 // ******************************************************************************************
00053 // Destructor
00054 // ******************************************************************************************
00055 SRDFWriter::~SRDFWriter()
00056 {
00057 }
00058 
00059 // ******************************************************************************************
00060 // Load SRDF data from a pre-populated string
00061 // ******************************************************************************************
00062 bool SRDFWriter::initString( const urdf::ModelInterface &robot_model, const std::string &srdf_string )
00063 {
00064   // Parse string into srdf_model_ and Error check
00065   if( !srdf_model_->initString( robot_model, srdf_string ) )
00066   {
00067     return false; // error loading file. improper format?
00068   }
00069 
00070   // copy fields into this object
00071   initModel( robot_model, *srdf_model_ );
00072 
00073   return true;
00074 }
00075 
00076 // ******************************************************************************************
00077 // Load SRDF data from a pre-populated string
00078 // ******************************************************************************************
00079 void SRDFWriter::initModel( const urdf::ModelInterface &robot_model, const srdf::Model &srdf_model )
00080 {
00081   // copy to internal srdf_model_
00082   if (srdf_model_.get() != &srdf_model)
00083   {
00084     *srdf_model_ = srdf_model;
00085   }
00086 
00087   // Copy all read-only data from srdf model to this object
00088   disabled_collisions_ = srdf_model_->getDisabledCollisionPairs();
00089   link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
00090   groups_ = srdf_model_->getGroups();
00091   virtual_joints_ = srdf_model_->getVirtualJoints();
00092   end_effectors_ = srdf_model_->getEndEffectors();
00093   group_states_ = srdf_model_->getGroupStates();
00094   passive_joints_ = srdf_model_->getPassiveJoints();
00095 
00096   // Copy the robot name b/c the root xml element requires this attribute
00097   robot_name_ = robot_model.getName();
00098 }
00099 
00100 // ******************************************************************************************
00101 // Update the SRDF Model class using a new SRDF string
00102 // ******************************************************************************************
00103 void SRDFWriter::updateSRDFModel( const urdf::ModelInterface &robot_model )
00104 {
00105   // Get an up to date SRDF String
00106   const std::string srdf_string = getSRDFString();
00107 
00108   // Error check
00109   if( !srdf_model_->initString( robot_model, srdf_string ) )
00110   {
00111     throw std::runtime_error( "Unable to update the SRDF Model" );
00112   }
00113 }
00114 
00115 // ******************************************************************************************
00116 // Save to file a generated SRDF document
00117 // ******************************************************************************************
00118 bool SRDFWriter::writeSRDF( const std::string &file_path )
00119 {
00120   // Generate the SRDF
00121   TiXmlDocument document = generateSRDF();
00122 
00123   // Save to file
00124   return document.SaveFile( file_path );
00125 }
00126 
00127 // ******************************************************************************************
00128 // Get a string of a generated SRDF document
00129 // ******************************************************************************************
00130 std::string SRDFWriter::getSRDFString()
00131 {
00132   // Generate the SRDF
00133   TiXmlDocument document = generateSRDF();
00134 
00135   // Setup printer
00136   TiXmlPrinter printer;
00137   printer.SetIndent( "    " );
00138   document.Accept( &printer );
00139 
00140   // Return string
00141   return printer.CStr();
00142 }
00143 
00144 // ******************************************************************************************
00145 // Generate SRDF XML of all contained data
00146 // ******************************************************************************************
00147 TiXmlDocument SRDFWriter::generateSRDF()
00148 {
00149   TiXmlDocument document;
00150   TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
00151   document.LinkEndChild( decl );
00152 
00153   // Convenience comments
00154   TiXmlComment * comment = new TiXmlComment( "This does not replace URDF, and is not an extension of URDF.\n    This is a format for representing semantic information about the robot structure.\n    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined\n" );
00155   document.LinkEndChild( comment );
00156 
00157   // Root
00158   TiXmlElement* robot_root = new TiXmlElement("robot");
00159   robot_root->SetAttribute("name", robot_name_ ); // robot name
00160   document.LinkEndChild( robot_root );
00161 
00162   // Add Groups
00163   createGroupsXML( robot_root );
00164 
00165   // Add Group States
00166   createGroupStatesXML( robot_root );
00167 
00168   // Add End Effectors
00169   createEndEffectorsXML( robot_root );
00170 
00171   // Add Virtual Joints
00172   createVirtualJointsXML( robot_root );
00173 
00174   // Add Passive Joints
00175   createPassiveJointsXML( robot_root );
00176 
00177   // Add Link Sphere approximations
00178   createLinkSphereApproximationsXML( robot_root );
00179 
00180   // Add Disabled Collisions
00181   createDisabledCollisionsXML( robot_root );
00182 
00183   // Save
00184   return document;
00185 }
00186 
00187 // ******************************************************************************************
00188 // Generate XML for SRDF groups
00189 // ******************************************************************************************
00190 void SRDFWriter::createGroupsXML( TiXmlElement *root )
00191 {
00192   // Convenience comments
00193   if( groups_.size() ) // only show comments if there are corresponding elements
00194   {
00195     TiXmlComment *comment;
00196     comment = new TiXmlComment( "GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc" );
00197     root->LinkEndChild( comment );
00198     comment = new TiXmlComment( "LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included" );
00199     root->LinkEndChild( comment );
00200     comment = new TiXmlComment( "JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included" );
00201     root->LinkEndChild( comment );
00202     comment = new TiXmlComment( "CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group");
00203     root->LinkEndChild( comment );
00204     comment = new TiXmlComment( "SUBGROUPS: Groups can also be formed by referencing to already defined group names" );
00205     root->LinkEndChild( comment );
00206   }
00207 
00208   // Loop through all of the top groups
00209   for( std::vector<srdf::Model::Group>::iterator group_it = groups_.begin();
00210        group_it != groups_.end();  ++group_it )
00211   {
00212 
00213     // Create group element
00214     TiXmlElement *group = new TiXmlElement("group");
00215     group->SetAttribute("name", group_it->name_ ); // group name
00216     root->LinkEndChild(group);
00217 
00218     // LINKS
00219     for( std::vector<std::string>::const_iterator link_it = group_it->links_.begin();
00220          link_it != group_it->links_.end(); ++link_it )
00221     {
00222       TiXmlElement *link = new TiXmlElement("link");
00223       link->SetAttribute("name", *link_it ); // link name
00224       group->LinkEndChild( link );
00225     }
00226 
00227     // JOINTS
00228     for( std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
00229          joint_it != group_it->joints_.end(); ++joint_it )
00230     {
00231       TiXmlElement *joint = new TiXmlElement("joint");
00232       joint->SetAttribute("name", *joint_it ); // joint name
00233       group->LinkEndChild( joint );
00234     }
00235 
00236     // CHAINS
00237     for( std::vector<std::pair<std::string,std::string> >::const_iterator chain_it = group_it->chains_.begin();
00238          chain_it != group_it->chains_.end(); ++chain_it )
00239     {
00240       TiXmlElement *chain = new TiXmlElement("chain");
00241       chain->SetAttribute("base_link", chain_it->first );
00242       chain->SetAttribute("tip_link", chain_it->second );
00243       group->LinkEndChild( chain );
00244     }
00245 
00246     // SUBGROUPS
00247     for( std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
00248          subgroup_it != group_it->subgroups_.end(); ++subgroup_it )
00249     {
00250       TiXmlElement *subgroup = new TiXmlElement("group");
00251       subgroup->SetAttribute("name", *subgroup_it ); // subgroup name
00252       group->LinkEndChild( subgroup );
00253     }
00254 
00255   }
00256 }
00257 
00258 // ******************************************************************************************
00259 // Generate XML for SRDF link collision spheres
00260 // ******************************************************************************************
00261 void SRDFWriter::createLinkSphereApproximationsXML( TiXmlElement *root )
00262 {
00263   if( link_sphere_approximations_.empty() ) // skip it if there are none
00264     return;
00265 
00266   // Convenience comments
00267   TiXmlComment *comment = new TiXmlComment();
00268   comment->SetValue( "COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link." );
00269   root->LinkEndChild( comment );
00270 
00271 
00272   for ( std::vector<srdf::Model::LinkSpheres>::const_iterator link_sphere_it = link_sphere_approximations_.begin();
00273         link_sphere_it != link_sphere_approximations_.end() ; ++link_sphere_it)
00274   {
00275     if (link_sphere_it->spheres_.empty())  // skip if no spheres for this link
00276       continue;
00277 
00278     // Create new element for the link
00279     TiXmlElement *link = new TiXmlElement("link_sphere_approximation");
00280     link->SetAttribute("link", link_sphere_it->link_);
00281     root->LinkEndChild( link );
00282 
00283     // Add all spheres for the link
00284     for( std::vector<srdf::Model::Sphere>::const_iterator sphere_it = link_sphere_it->spheres_.begin();
00285          sphere_it != link_sphere_it->spheres_.end(); ++sphere_it )
00286     {
00287       TiXmlElement *sphere = new TiXmlElement("sphere");
00288       std::stringstream center;
00289       center.precision(20);
00290       center << sphere_it->center_x_ << " " << sphere_it->center_y_ << " " << sphere_it->center_z_;
00291       sphere->SetAttribute("center", center.str() );
00292       sphere->SetDoubleAttribute("radius", sphere_it->radius_ );
00293       link->LinkEndChild( sphere );
00294     }
00295   }
00296 }
00297 
00298 // ******************************************************************************************
00299 // Generate XML for SRDF disabled collisions of robot link pairs
00300 // ******************************************************************************************
00301 void SRDFWriter::createDisabledCollisionsXML( TiXmlElement *root )
00302 {
00303   // Convenience comments
00304   if( disabled_collisions_.size() ) // only show comments if there are corresponding elements
00305   {
00306     TiXmlComment *comment = new TiXmlComment();
00307     comment->SetValue( "DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. " );
00308     root->LinkEndChild( comment );
00309   }
00310 
00311   for ( std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it = disabled_collisions_.begin();
00312         pair_it != disabled_collisions_.end() ; ++pair_it)
00313   {
00314     // Create new element for each link pair
00315     TiXmlElement *link_pair = new TiXmlElement("disable_collisions");
00316     link_pair->SetAttribute("link1", pair_it->link1_ );
00317     link_pair->SetAttribute("link2", pair_it->link2_ );
00318     link_pair->SetAttribute("reason", pair_it->reason_ );
00319 
00320     root->LinkEndChild( link_pair );
00321   }
00322 }
00323 
00324 // ******************************************************************************************
00325 // Generate XML for SRDF group states
00326 // ******************************************************************************************
00327 void SRDFWriter::createGroupStatesXML( TiXmlElement *root )
00328 {
00329   // Convenience comments
00330   if( group_states_.size() ) // only show comments if there are corresponding elements
00331   {
00332     TiXmlComment *comment = new TiXmlComment();
00333     comment->SetValue( "GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'" );
00334     root->LinkEndChild( comment );
00335   }
00336 
00337   for ( std::vector<srdf::Model::GroupState>::const_iterator state_it = group_states_.begin();
00338         state_it != group_states_.end() ; ++state_it)
00339   {
00340     // Create new element for each group state
00341     TiXmlElement *state = new TiXmlElement("group_state");
00342     state->SetAttribute("name", state_it->name_ );
00343     state->SetAttribute("group", state_it->group_ );
00344     root->LinkEndChild( state );
00345 
00346     // Add all joints
00347     for( std::map<std::string, std::vector<double> >::const_iterator value_it = state_it->joint_values_.begin();
00348          value_it != state_it->joint_values_.end(); ++value_it )
00349     {
00350       TiXmlElement *joint = new TiXmlElement("joint");
00351       joint->SetAttribute("name", value_it->first ); // joint name
00352       joint->SetDoubleAttribute("value", value_it->second[0] ); // joint value
00353 
00354       // TODO: use the vector to support multi-DOF joints
00355       state->LinkEndChild( joint );
00356     }
00357   }
00358 }
00359 
00360 // ******************************************************************************************
00361 // Generate XML for SRDF end effectors
00362 // ******************************************************************************************
00363 void SRDFWriter::createEndEffectorsXML( TiXmlElement *root )
00364 {
00365   // Convenience comments
00366   if( end_effectors_.size() ) // only show comments if there are corresponding elements
00367   {
00368     TiXmlComment *comment = new TiXmlComment();
00369     comment->SetValue( "END EFFECTOR: Purpose: Represent information about an end effector." );
00370     root->LinkEndChild( comment );
00371   }
00372 
00373   for ( std::vector<srdf::Model::EndEffector>::const_iterator effector_it = end_effectors_.begin();
00374         effector_it != end_effectors_.end() ; ++effector_it)
00375   {
00376     // Create new element for each link pair
00377     TiXmlElement *effector = new TiXmlElement("end_effector");
00378     effector->SetAttribute("name", effector_it->name_ );
00379     effector->SetAttribute("parent_link", effector_it->parent_link_ );
00380     effector->SetAttribute("group", effector_it->component_group_ );
00381     if (!effector_it->parent_group_.empty())
00382       effector->SetAttribute("parent_group", effector_it->parent_group_ );
00383     root->LinkEndChild( effector );
00384   }
00385 }
00386 
00387 // ******************************************************************************************
00388 // Generate XML for SRDF virtual joints
00389 // ******************************************************************************************
00390 void SRDFWriter::createVirtualJointsXML( TiXmlElement *root )
00391 {
00392   // Convenience comments
00393   if( virtual_joints_.size() ) // only show comments if there are corresponding elements
00394   {
00395     TiXmlComment *comment = new TiXmlComment();
00396     comment->SetValue( "VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)" );
00397     root->LinkEndChild( comment );
00398   }
00399 
00400   for ( std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it = virtual_joints_.begin();
00401         virtual_it != virtual_joints_.end() ; ++virtual_it)
00402   {
00403     // Create new element for each link pair
00404     TiXmlElement *virtual_joint = new TiXmlElement("virtual_joint");
00405     virtual_joint->SetAttribute("name", virtual_it->name_ );
00406     virtual_joint->SetAttribute("type", virtual_it->type_ );
00407     virtual_joint->SetAttribute("parent_frame", virtual_it->parent_frame_ );
00408     virtual_joint->SetAttribute("child_link", virtual_it->child_link_ );
00409 
00410     root->LinkEndChild( virtual_joint );
00411   }
00412 }
00413 
00414 void SRDFWriter::createPassiveJointsXML( TiXmlElement *root )
00415 {
00416   if ( passive_joints_.size() )
00417   {
00418     TiXmlComment *comment = new TiXmlComment();
00419     comment->SetValue( "PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated" );
00420     root->LinkEndChild( comment );
00421   }
00422   for ( std::vector<srdf::Model::PassiveJoint>::const_iterator p_it = passive_joints_.begin();
00423         p_it != passive_joints_.end() ; ++p_it)
00424   {
00425     // Create new element for each link pair
00426     TiXmlElement *p_joint = new TiXmlElement("passive_joint");
00427     p_joint->SetAttribute("name", p_it->name_ );
00428     root->LinkEndChild( p_joint );
00429   }
00430 }
00431 
00432 
00433 }


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Feb 9 2017 03:50:57