srdf_writer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman */
36 
37 #include <tinyxml.h>
38 #include <srdfdom/srdf_writer.h>
39 
40 namespace srdf
41 {
42 
43 // ******************************************************************************************
44 // Constructor
45 // ******************************************************************************************
47 {
48  // Intialize the SRDF model
49  srdf_model_.reset( new srdf::Model() );
50 }
51 
52 // ******************************************************************************************
53 // Destructor
54 // ******************************************************************************************
56 {
57 }
58 
59 // ******************************************************************************************
60 // Load SRDF data from a pre-populated string
61 // ******************************************************************************************
62 bool SRDFWriter::initString( const urdf::ModelInterface &robot_model, const std::string &srdf_string )
63 {
64  // Parse string into srdf_model_ and Error check
65  if( !srdf_model_->initString( robot_model, srdf_string ) )
66  {
67  return false; // error loading file. improper format?
68  }
69 
70  // copy fields into this object
71  initModel( robot_model, *srdf_model_ );
72 
73  return true;
74 }
75 
76 // ******************************************************************************************
77 // Load SRDF data from a pre-populated string
78 // ******************************************************************************************
79 void SRDFWriter::initModel( const urdf::ModelInterface &robot_model, const srdf::Model &srdf_model )
80 {
81  // copy to internal srdf_model_
82  if (srdf_model_.get() != &srdf_model)
83  {
84  *srdf_model_ = srdf_model;
85  }
86 
87  // Copy all read-only data from srdf model to this object
88  disabled_collisions_ = srdf_model_->getDisabledCollisionPairs();
89  link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
90  groups_ = srdf_model_->getGroups();
91  virtual_joints_ = srdf_model_->getVirtualJoints();
92  end_effectors_ = srdf_model_->getEndEffectors();
93  group_states_ = srdf_model_->getGroupStates();
94  passive_joints_ = srdf_model_->getPassiveJoints();
95 
96  // Copy the robot name b/c the root xml element requires this attribute
97  robot_name_ = robot_model.getName();
98 }
99 
100 // ******************************************************************************************
101 // Update the SRDF Model class using a new SRDF string
102 // ******************************************************************************************
103 void SRDFWriter::updateSRDFModel( const urdf::ModelInterface &robot_model )
104 {
105  // Get an up to date SRDF String
106  const std::string srdf_string = getSRDFString();
107 
108  // Error check
109  if( !srdf_model_->initString( robot_model, srdf_string ) )
110  {
111  throw std::runtime_error( "Unable to update the SRDF Model" );
112  }
113 }
114 
115 // ******************************************************************************************
116 // Save to file a generated SRDF document
117 // ******************************************************************************************
118 bool SRDFWriter::writeSRDF( const std::string &file_path )
119 {
120  // Generate the SRDF
121  TiXmlDocument document = generateSRDF();
122 
123  // Save to file
124  return document.SaveFile( file_path );
125 }
126 
127 // ******************************************************************************************
128 // Get a string of a generated SRDF document
129 // ******************************************************************************************
131 {
132  // Generate the SRDF
133  TiXmlDocument document = generateSRDF();
134 
135  // Setup printer
136  TiXmlPrinter printer;
137  printer.SetIndent( " " );
138  document.Accept( &printer );
139 
140  // Return string
141  return printer.CStr();
142 }
143 
144 // ******************************************************************************************
145 // Generate SRDF XML of all contained data
146 // ******************************************************************************************
148 {
149  TiXmlDocument document;
150  TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
151  document.LinkEndChild( decl );
152 
153  // Convenience comments
154  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" );
155  document.LinkEndChild( comment );
156 
157  // Root
158  TiXmlElement* robot_root = new TiXmlElement("robot");
159  robot_root->SetAttribute("name", robot_name_ ); // robot name
160  document.LinkEndChild( robot_root );
161 
162  // Add Groups
163  createGroupsXML( robot_root );
164 
165  // Add Group States
166  createGroupStatesXML( robot_root );
167 
168  // Add End Effectors
169  createEndEffectorsXML( robot_root );
170 
171  // Add Virtual Joints
172  createVirtualJointsXML( robot_root );
173 
174  // Add Passive Joints
175  createPassiveJointsXML( robot_root );
176 
177  // Add Link Sphere approximations
178  createLinkSphereApproximationsXML( robot_root );
179 
180  // Add Disabled Collisions
181  createDisabledCollisionsXML( robot_root );
182 
183  // Save
184  return document;
185 }
186 
187 // ******************************************************************************************
188 // Generate XML for SRDF groups
189 // ******************************************************************************************
190 void SRDFWriter::createGroupsXML( TiXmlElement *root )
191 {
192  // Convenience comments
193  if( groups_.size() ) // only show comments if there are corresponding elements
194  {
195  TiXmlComment *comment;
196  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" );
197  root->LinkEndChild( comment );
198  comment = new TiXmlComment( "LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included" );
199  root->LinkEndChild( comment );
200  comment = new TiXmlComment( "JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included" );
201  root->LinkEndChild( comment );
202  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");
203  root->LinkEndChild( comment );
204  comment = new TiXmlComment( "SUBGROUPS: Groups can also be formed by referencing to already defined group names" );
205  root->LinkEndChild( comment );
206  }
207 
208  // Loop through all of the top groups
209  for( std::vector<srdf::Model::Group>::iterator group_it = groups_.begin();
210  group_it != groups_.end(); ++group_it )
211  {
212 
213  // Create group element
214  TiXmlElement *group = new TiXmlElement("group");
215  group->SetAttribute("name", group_it->name_ ); // group name
216  root->LinkEndChild(group);
217 
218  // LINKS
219  for( std::vector<std::string>::const_iterator link_it = group_it->links_.begin();
220  link_it != group_it->links_.end(); ++link_it )
221  {
222  TiXmlElement *link = new TiXmlElement("link");
223  link->SetAttribute("name", *link_it ); // link name
224  group->LinkEndChild( link );
225  }
226 
227  // JOINTS
228  for( std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
229  joint_it != group_it->joints_.end(); ++joint_it )
230  {
231  TiXmlElement *joint = new TiXmlElement("joint");
232  joint->SetAttribute("name", *joint_it ); // joint name
233  group->LinkEndChild( joint );
234  }
235 
236  // CHAINS
237  for( std::vector<std::pair<std::string,std::string> >::const_iterator chain_it = group_it->chains_.begin();
238  chain_it != group_it->chains_.end(); ++chain_it )
239  {
240  TiXmlElement *chain = new TiXmlElement("chain");
241  chain->SetAttribute("base_link", chain_it->first );
242  chain->SetAttribute("tip_link", chain_it->second );
243  group->LinkEndChild( chain );
244  }
245 
246  // SUBGROUPS
247  for( std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
248  subgroup_it != group_it->subgroups_.end(); ++subgroup_it )
249  {
250  TiXmlElement *subgroup = new TiXmlElement("group");
251  subgroup->SetAttribute("name", *subgroup_it ); // subgroup name
252  group->LinkEndChild( subgroup );
253  }
254 
255  }
256 }
257 
258 // ******************************************************************************************
259 // Generate XML for SRDF link collision spheres
260 // ******************************************************************************************
262 {
263  if( link_sphere_approximations_.empty() ) // skip it if there are none
264  return;
265 
266  // Convenience comments
267  TiXmlComment *comment = new TiXmlComment();
268  comment->SetValue( "COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link." );
269  root->LinkEndChild( comment );
270 
271 
272  for ( std::vector<srdf::Model::LinkSpheres>::const_iterator link_sphere_it = link_sphere_approximations_.begin();
273  link_sphere_it != link_sphere_approximations_.end() ; ++link_sphere_it)
274  {
275  if (link_sphere_it->spheres_.empty()) // skip if no spheres for this link
276  continue;
277 
278  // Create new element for the link
279  TiXmlElement *link = new TiXmlElement("link_sphere_approximation");
280  link->SetAttribute("link", link_sphere_it->link_);
281  root->LinkEndChild( link );
282 
283  // Add all spheres for the link
284  for( std::vector<srdf::Model::Sphere>::const_iterator sphere_it = link_sphere_it->spheres_.begin();
285  sphere_it != link_sphere_it->spheres_.end(); ++sphere_it )
286  {
287  TiXmlElement *sphere = new TiXmlElement("sphere");
288  std::stringstream center;
289  center.precision(20);
290  center << sphere_it->center_x_ << " " << sphere_it->center_y_ << " " << sphere_it->center_z_;
291  sphere->SetAttribute("center", center.str() );
292  sphere->SetDoubleAttribute("radius", sphere_it->radius_ );
293  link->LinkEndChild( sphere );
294  }
295  }
296 }
297 
298 // ******************************************************************************************
299 // Generate XML for SRDF disabled collisions of robot link pairs
300 // ******************************************************************************************
301 void SRDFWriter::createDisabledCollisionsXML( TiXmlElement *root )
302 {
303  // Convenience comments
304  if( disabled_collisions_.size() ) // only show comments if there are corresponding elements
305  {
306  TiXmlComment *comment = new TiXmlComment();
307  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. " );
308  root->LinkEndChild( comment );
309  }
310 
311  for ( std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it = disabled_collisions_.begin();
312  pair_it != disabled_collisions_.end() ; ++pair_it)
313  {
314  // Create new element for each link pair
315  TiXmlElement *link_pair = new TiXmlElement("disable_collisions");
316  link_pair->SetAttribute("link1", pair_it->link1_ );
317  link_pair->SetAttribute("link2", pair_it->link2_ );
318  link_pair->SetAttribute("reason", pair_it->reason_ );
319 
320  root->LinkEndChild( link_pair );
321  }
322 }
323 
324 // ******************************************************************************************
325 // Generate XML for SRDF group states
326 // ******************************************************************************************
327 void SRDFWriter::createGroupStatesXML( TiXmlElement *root )
328 {
329  // Convenience comments
330  if( group_states_.size() ) // only show comments if there are corresponding elements
331  {
332  TiXmlComment *comment = new TiXmlComment();
333  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'" );
334  root->LinkEndChild( comment );
335  }
336 
337  for ( std::vector<srdf::Model::GroupState>::const_iterator state_it = group_states_.begin();
338  state_it != group_states_.end() ; ++state_it)
339  {
340  // Create new element for each group state
341  TiXmlElement *state = new TiXmlElement("group_state");
342  state->SetAttribute("name", state_it->name_ );
343  state->SetAttribute("group", state_it->group_ );
344  root->LinkEndChild( state );
345 
346  // Add all joints
347  for( std::map<std::string, std::vector<double> >::const_iterator value_it = state_it->joint_values_.begin();
348  value_it != state_it->joint_values_.end(); ++value_it )
349  {
350  TiXmlElement *joint = new TiXmlElement("joint");
351  joint->SetAttribute("name", value_it->first ); // joint name
352  joint->SetDoubleAttribute("value", value_it->second[0] ); // joint value
353 
354  // TODO: use the vector to support multi-DOF joints
355  state->LinkEndChild( joint );
356  }
357  }
358 }
359 
360 // ******************************************************************************************
361 // Generate XML for SRDF end effectors
362 // ******************************************************************************************
363 void SRDFWriter::createEndEffectorsXML( TiXmlElement *root )
364 {
365  // Convenience comments
366  if( end_effectors_.size() ) // only show comments if there are corresponding elements
367  {
368  TiXmlComment *comment = new TiXmlComment();
369  comment->SetValue( "END EFFECTOR: Purpose: Represent information about an end effector." );
370  root->LinkEndChild( comment );
371  }
372 
373  for ( std::vector<srdf::Model::EndEffector>::const_iterator effector_it = end_effectors_.begin();
374  effector_it != end_effectors_.end() ; ++effector_it)
375  {
376  // Create new element for each link pair
377  TiXmlElement *effector = new TiXmlElement("end_effector");
378  effector->SetAttribute("name", effector_it->name_ );
379  effector->SetAttribute("parent_link", effector_it->parent_link_ );
380  effector->SetAttribute("group", effector_it->component_group_ );
381  if (!effector_it->parent_group_.empty())
382  effector->SetAttribute("parent_group", effector_it->parent_group_ );
383  root->LinkEndChild( effector );
384  }
385 }
386 
387 // ******************************************************************************************
388 // Generate XML for SRDF virtual joints
389 // ******************************************************************************************
390 void SRDFWriter::createVirtualJointsXML( TiXmlElement *root )
391 {
392  // Convenience comments
393  if( virtual_joints_.size() ) // only show comments if there are corresponding elements
394  {
395  TiXmlComment *comment = new TiXmlComment();
396  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)" );
397  root->LinkEndChild( comment );
398  }
399 
400  for ( std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it = virtual_joints_.begin();
401  virtual_it != virtual_joints_.end() ; ++virtual_it)
402  {
403  // Create new element for each link pair
404  TiXmlElement *virtual_joint = new TiXmlElement("virtual_joint");
405  virtual_joint->SetAttribute("name", virtual_it->name_ );
406  virtual_joint->SetAttribute("type", virtual_it->type_ );
407  virtual_joint->SetAttribute("parent_frame", virtual_it->parent_frame_ );
408  virtual_joint->SetAttribute("child_link", virtual_it->child_link_ );
409 
410  root->LinkEndChild( virtual_joint );
411  }
412 }
413 
414 void SRDFWriter::createPassiveJointsXML( TiXmlElement *root )
415 {
416  if ( passive_joints_.size() )
417  {
418  TiXmlComment *comment = new TiXmlComment();
419  comment->SetValue( "PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated" );
420  root->LinkEndChild( comment );
421  }
422  for ( std::vector<srdf::Model::PassiveJoint>::const_iterator p_it = passive_joints_.begin();
423  p_it != passive_joints_.end() ; ++p_it)
424  {
425  // Create new element for each link pair
426  TiXmlElement *p_joint = new TiXmlElement("passive_joint");
427  p_joint->SetAttribute("name", p_it->name_ );
428  root->LinkEndChild( p_joint );
429  }
430 }
431 
432 
433 }
void createEndEffectorsXML(TiXmlElement *root)
void createDisabledCollisionsXML(TiXmlElement *root)
Representation of semantic information about the robot.
Definition: model.h:53
void createGroupStatesXML(TiXmlElement *root)
void updateSRDFModel(const urdf::ModelInterface &robot_model)
void initModel(const urdf::ModelInterface &robot_model, const srdf::Model &srdf_model)
Definition: srdf_writer.cpp:79
std::vector< srdf::Model::VirtualJoint > virtual_joints_
Definition: srdf_writer.h:170
void createPassiveJointsXML(TiXmlElement *root)
std::vector< srdf::Model::DisabledCollision > disabled_collisions_
Definition: srdf_writer.h:173
std::vector< srdf::Model::EndEffector > end_effectors_
Definition: srdf_writer.h:171
std::vector< srdf::Model::LinkSpheres > link_sphere_approximations_
Definition: srdf_writer.h:172
std::string robot_name_
Definition: srdf_writer.h:180
void createVirtualJointsXML(TiXmlElement *root)
srdf::ModelSharedPtr srdf_model_
Definition: srdf_writer.h:177
void createLinkSphereApproximationsXML(TiXmlElement *root)
bool writeSRDF(const std::string &file_path)
Main namespace.
Definition: model.h:49
TiXmlDocument generateSRDF()
void createGroupsXML(TiXmlElement *root)
bool initString(const urdf::ModelInterface &robot_model, const std::string &srdf_string)
Definition: srdf_writer.cpp:62
std::vector< srdf::Model::Group > groups_
Definition: srdf_writer.h:168
std::string getSRDFString()
std::vector< srdf::Model::GroupState > group_states_
Definition: srdf_writer.h:169
std::vector< srdf::Model::PassiveJoint > passive_joints_
Definition: srdf_writer.h:174


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Jun 6 2019 19:59:13