SDF readFile not parsing world file
I am using ROS Indigo with Ubuntu 14.04.
I need to parse the elements of a simpleenvironment.world file using sdf::readFile
. The simpleenvironment.world file is as follows:
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<model name="box">
<pose>1.4 0.8 0.14 0 0 0</pose>
<static>true</static>
<link name='link_box'>
<pose>0 0 0 0 0 0</pose>
<collision name="collision_box">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>1 2 1.3</size>
</box>
</geometry>
</collision>
<visual name="visual_box">
<geometry>
<box>
<size>1 2 1.3</size>
</box>
</geometry>
</visual>
<self_collide>0</self_collide>
<gravity>1</gravity>
</link>
</model>
</world>
</sdf>
In a portion of my C++ code I try to parse the content of the world file as follows:
// Read the sdf
sdf::SDFPtr sdfParsed(new sdf::SDF());
sdf::init(sdfParsed);
const std::string sdfStringPath(argv[1]);
assert(sdf::readFile(sdfStringPath, sdfParsed));
But this does not seem to function as expected because if I try to print sdfParsed with
cout << "THE PARSED SDF IS: " << sdfParsed->ToString() << endl;
the output is the following:
THE PARSED SDF IS: <?xml version='1.0'?> <sdf version='1.5'/>
The argv[1]
is clearly the path to the above mentioned .world file. What's wrong with my code?
Thanks in advance.
Asked by dhindhimathai on 2018-03-20 09:59:15 UTC
Answers
I'm trying to reproduce your problem, but have been unsuccessful so far. Below are the steps I followed. Can you try this, and see if it works for you?
Wrote a simple test program:
#include <iostream>
#include <sdf/sdf.hh>
int main(int argc, char **argv)
{
sdf::SDFPtr sdf(new sdf::SDF());
if (!sdf::init(sdf))
{
std::cerr << "Error: SDF init failed" << std::endl;
return 1;
}
if (!sdf::readFile(argv[1], sdf))
{
std::cerr << "Error: SDF parsing the xml failed" << std::endl;
return 1;
}
std::cout << sdf->ToString() << std::endl;
return 0;
}
Compiled the above using (your paths may be different):
g++ main.cc -I /usr/local/include/sdformat-6.0 -I /usr/local/include/ignition/math5 -L /usr/local/lib/ -lsdformat -lignition-math4 -std=c++11
Ran it against your world file:
./a.out ~/test.world
This is the output (there are a few errors since I haven't setup the mechanism to find the URIs in the SDF file):
Error [SDF.cc:142] Tried to use callback in sdf::findFile(), but the callback is empty. Did you call sdf::setFindCallback()?Error [SDF.cc:142] Tried to use callback in sdf::findFile(), but the callback is empty. Did you call sdf::setFindCallback()?Error Code 11 Msg: Unable to find uri[model://ground_plane]
Error Code 11 Msg: Unable to find uri[model://sun]
<?xml version='1.0'?>
<sdf version='1.6'>
<world name='default'>
<model name='box'>
<pose frame=''>1.4 0.8 0.14 0 -0 0</pose>
<static>1</static>
<link name='link_box'>
<pose frame=''>0 0 0 0 -0 0</pose>
<collision name='collision_box'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>1 2 1.3</size>
</box>
</geometry>
</collision>
<visual name='visual_box'>
<geometry>
<box>
<size>1 2 1.3</size>
</box>
</geometry>
</visual>
<self_collide>0</self_collide>
<gravity>1</gravity>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
</world>
</sdf>
Asked by nkoenig on 2018-03-21 09:16:16 UTC
Comments