Inserting camera model programmatically
I want to be able to record a simulation in Gazebo. For this I use a camera model, as discussed in this question.
The following camera model works fine if I insert it from the Gazebo GUI or if I include it in a .world file. (The model is taken from the above question, with the only notable difference that I moved <save> element into the <camera> element, which appears to be necessary):
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="camera">
<static>true</static>
<pose>4 -4 4 0 0.5 2.4</pose>
<link name="link">
<pose>0.05 0.05 0.05 0 0 0</pose>
<inertial>
<mass>0.1</mass>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="camera" type="camera">
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>1280</width>
<height>960</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<save enabled="true">
<path>/tmp/gazebo_frames</path>
</save>
</camera>
<always_on>1</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
</sensor>
</link>
</model>
</sdf>
The problem
Now I want to be able to insert the model programmatically, when using Gazebo as a library.
I have created a minimum working example here: Pastebin
When I run this MWE I get a segmentation fault (after inserting the model, in the while loop where I wait for the model to show up). If I remove the
What am I doing wrong?
Thanks for your help.
EDIT:
Below is the backtrace from running the MWE:
Starting program: /home/nicolas/GazeboAgentCameraExample/build/main
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7fffdd4c4700 (LWP 13481)]
[New Thread 0x7fffdccc3700 (LWP 13482)]
[New Thread 0x7fffdc4c2700 (LWP 13483)]
[New Thread 0x7fffdb3d3700 (LWP 13484)]
[New Thread 0x7fffd8dc4700 (LWP 13485)]
[New Thread 0x7fffd0a10700 (LWP 13486)]
[New Thread 0x7fffcbfff700 (LWP 13487)]
[New Thread 0x7fffcb7fe700 (LWP 13488)]
[New Thread 0x7fffcaffd700 (LWP 13489)]
Gazebo server setup successful
[New Thread 0x7fffca3c4700 (LWP 13490)]
[New Thread 0x7fffc9bc2700 (LWP 13492)]
[New Thread 0x7fffc9fc3700 (LWP 13491)]
[New Thread 0x7fffc97c1700 (LWP 13493)]
[New Thread 0x7fffc8fc0700 (LWP 13495)]
Program received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffc97c1700 (LWP 13493)]
0x00007ffff1f93fa9 in gazebo::sensors::SensorFactory::NewSensor(std::string const&) () from /usr/lib/x86_64-linux-gnu/libgazebo_sensors.so.4
(gdb) thread apply all bt
Thread 15 (Thread 0x7fffc8fc0700 (LWP 13495)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007ffff7b53efb in ?? () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#2 0x00007ffff7b4aec7 in gazebo::physics::World::LogWorker() () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#3 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#4 0x00007ffff7151182 in start_thread (arg=0x7fffc8fc0700) at pthread_create.c:312
#5 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 14 (Thread 0x7fffc97c1700 (LWP 13493)):
#0 0x00007ffff1f93fa9 in gazebo::sensors::SensorFactory::NewSensor(std::string const&) () from /usr/lib/x86_64-linux-gnu/libgazebo_sensors.so.4
#1 0x00007ffff1f96e67 in gazebo::sensors::SensorManager::CreateSensor(boost::shared_ptr<sdf::Element>, std::string const&, std::string const&, unsigned int) ()
from /usr/lib/x86_64-linux-gnu/libgazebo_sensors.so.4
#2 0x00007ffff1f8e6ab in gazebo::sensors::create_sensor(boost::shared_ptr<sdf::Element>, std::string const&, std::string const&, unsigned int) () from /usr/lib/x86_64-linux-gnu/libgazebo_sensors.so.4
#3 0x00007ffff7b0d43b in gazebo::physics::Link::Load(boost::shared_ptr<sdf::Element>) () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#4 0x00007ffff63c9ec1 in gazebo::physics::ODELink::Load(boost::shared_ptr<sdf::Element>) () from /usr/lib/x86_64-linux-gnu/libgazebo_physics_ode.so.4
#5 0x00007ffff7b1df0b in gazebo::physics::Model::LoadLinks() () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#6 0x00007ffff7b20371 in gazebo::physics::Model::Load(boost::shared_ptr<sdf::Element>) () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#7 0x00007ffff7b4846c in gazebo::physics::World::LoadModel(boost::shared_ptr<sdf::Element>, boost::shared_ptr<gazebo::physics::Base>) () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#8 0x00007ffff7b4e792 in gazebo::physics::World::ProcessFactoryMsgs() () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#9 0x00007ffff7b4f614 in gazebo::physics::World::ProcessMessages() () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#10 0x00007ffff7b50850 in gazebo::physics::World::Step() () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#11 0x00007ffff7b50d45 in gazebo::physics::World::RunLoop() () from /usr/lib/x86_64-linux-gnu/libgazebo_physics.so.4
#12 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#13 0x00007ffff7151182 in start_thread (arg=0x7fffc97c1700) at pthread_create.c:312
#14 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 13 (Thread 0x7fffc9fc3700 (LWP 13491)):
#0 syscall () at ../sysdeps/unix/sysv/linux/x86_64/syscall.S:38
#1 0x00007ffff1d0b2db in ?? () from /usr/lib/libtbb.so.2
#2 0x00007ffff1d0b2f9 in ?? () from /usr/lib/libtbb.so.2
#3 0x00007ffff7151182 in start_thread (arg=0x7fffc9fc3700) at pthread_create.c:312
#4 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 12 (Thread 0x7fffc9bc2700 (LWP 13492)):
#0 syscall () at ../sysdeps/unix/sysv/linux/x86_64/syscall.S:38
#1 0x00007ffff1d0b2db in ?? () from /usr/lib/libtbb.so.2
#2 0x00007ffff1d0b2f9 in ?? () from /usr/lib/libtbb.so.2
#3 0x00007ffff7151182 in start_thread (arg=0x7fffc9bc2700) at pthread_create.c:312
#4 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 11 (Thread 0x7fffca3c4700 (LWP 13490)):
#0 syscall () at ../sysdeps/unix/sysv/linux/x86_64/syscall.S:38
#1 0x00007ffff1d0b2db in ?? () from /usr/lib/libtbb.so.2
#2 0x00007ffff1d0b2f9 in ?? () from /usr/lib/libtbb.so.2
#3 0x00007ffff7151182 in start_thread (arg=0x7fffca3c4700) at pthread_create.c:312
#4 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 10 (Thread 0x7fffcaffd700 (LWP 13489)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007fffea02d690 in ?? () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#2 0x00007fffea02c6af in Ogre::DefaultWorkQueue::waitForNextRequest() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#3 0x00007fffea02cb24 in Ogre::DefaultWorkQueue::_threadMain() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#4 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#5 0x00007ffff7151182 in start_thread (arg=0x7fffcaffd700) at pthread_create.c:312
#6 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
---Type <return> to continue, or q <return> to quit---
Thread 9 (Thread 0x7fffcb7fe700 (LWP 13488)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007fffea02d690 in ?? () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#2 0x00007fffea02c6af in Ogre::DefaultWorkQueue::waitForNextRequest() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#3 0x00007fffea02cb24 in Ogre::DefaultWorkQueue::_threadMain() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#4 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#5 0x00007ffff7151182 in start_thread (arg=0x7fffcb7fe700) at pthread_create.c:312
#6 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 8 (Thread 0x7fffcbfff700 (LWP 13487)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007fffea02d690 in ?? () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#2 0x00007fffea02c6af in Ogre::DefaultWorkQueue::waitForNextRequest() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#3 0x00007fffea02cb24 in Ogre::DefaultWorkQueue::_threadMain() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#4 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#5 0x00007ffff7151182 in start_thread (arg=0x7fffcbfff700) at pthread_create.c:312
#6 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 7 (Thread 0x7fffd0a10700 (LWP 13486)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007fffea02d690 in ?? () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#2 0x00007fffea02c6af in Ogre::DefaultWorkQueue::waitForNextRequest() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#3 0x00007fffea02cb24 in Ogre::DefaultWorkQueue::_threadMain() () from /usr/lib/x86_64-linux-gnu/libOgreMain.so.1.8.1
#4 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#5 0x00007ffff7151182 in start_thread (arg=0x7fffd0a10700) at pthread_create.c:312
#6 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 6 (Thread 0x7fffd8dc4700 (LWP 13485)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007fffd9e42263 in ?? () from /usr/lib/x86_64-linux-gnu/dri/radeonsi_dri.so
#2 0x00007fffd9e41ba7 in ?? () from /usr/lib/x86_64-linux-gnu/dri/radeonsi_dri.so
#3 0x00007ffff7151182 in start_thread (arg=0x7fffd8dc4700) at pthread_create.c:312
#4 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 5 (Thread 0x7fffdb3d3700 (LWP 13484)):
#0 pthread_cond_timedwait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:238
#1 0x00007ffff5c1f5a0 in gazebo::transport::ConnectionManager::Run() () from /usr/lib/x86_64-linux-gnu/libgazebo_transport.so.4
#2 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#3 0x00007ffff7151182 in start_thread (arg=0x7fffdb3d3700) at pthread_create.c:312
#4 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 4 (Thread 0x7fffdc4c2700 (LWP 13483)):
#0 __clock_nanosleep (clock_id=0, flags=0, req=0x7fffdc4c1d00, rem=0xffffffffffffffff) at ../sysdeps/unix/sysv/linux/clock_nanosleep.c:49
#1 0x00007ffff782ca83 in gazebo::common::Time::Sleep(gazebo::common::Time const&) () from /usr/lib/x86_64-linux-gnu/libgazebo_common.so.4
#2 0x00007ffff782cc47 in gazebo::common::Time::MSleep(unsigned int) () from /usr/lib/x86_64-linux-gnu/libgazebo_common.so.4
#3 0x00007ffff75937ed in gazebo::Master::Run() () from /usr/lib/x86_64-linux-gnu/libgazebo.so.4
#4 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#5 0x00007ffff7151182 in start_thread (arg=0x7fffdc4c2700) at pthread_create.c:312
#6 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 3 (Thread 0x7fffdccc3700 (LWP 13482)):
#0 0x00007ffff6964593 in epoll_wait () at ../sysdeps/unix/syscall-template.S:81
#1 0x00007ffff5c113a8 in ?? () from /usr/lib/x86_64-linux-gnu/libgazebo_transport.so.4
#2 0x00007ffff5c157b1 in ?? () from /usr/lib/x86_64-linux-gnu/libgazebo_transport.so.4
#3 0x00007ffff5c24a86 in ?? () from /usr/lib/x86_64-linux-gnu/libgazebo_transport.so.4
#4 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#5 0x00007ffff7151182 in start_thread (arg=0x7fffdccc3700) at pthread_create.c:312
---Type <return> to continue, or q <return> to quit---
#6 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 2 (Thread 0x7fffdd4c4700 (LWP 13481)):
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#1 0x00007ffff781966b in ?? () from /usr/lib/x86_64-linux-gnu/libgazebo_common.so.4
#2 0x00007ffff7814566 in gazebo::common::ModelDatabase::UpdateModelCache(bool) () from /usr/lib/x86_64-linux-gnu/libgazebo_common.so.4
#3 0x00007ffff4c45a4a in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#4 0x00007ffff7151182 in start_thread (arg=0x7fffdd4c4700) at pthread_create.c:312
#5 0x00007ffff6963efd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111
Thread 1 (Thread 0x7ffff7f8c880 (LWP 13477)):
#0 __clock_nanosleep (clock_id=0, flags=0, req=0x7fffffffddb0, rem=0xffffffffffffffff) at ../sysdeps/unix/sysv/linux/clock_nanosleep.c:49
#1 0x00007ffff782ca83 in gazebo::common::Time::Sleep(gazebo::common::Time const&) () from /usr/lib/x86_64-linux-gnu/libgazebo_common.so.4
#2 0x00007ffff782cc47 in gazebo::common::Time::MSleep(unsigned int) () from /usr/lib/x86_64-linux-gnu/libgazebo_common.so.4
#3 0x000000000040a7da in main (_argc=1, _argv=0x7fffffffdf78) at /home/nicolas/bzrobot/application/GazeboAgentCameraExample/main.cc:72
Asked by NickDP on 2014-12-12 14:22:44 UTC
Answers
The following is a modified version of your code that should work. The key bit is running a sensor update loop. Sensors that rely on the rendering engine must be run in the main thread. This is a requirement of opengl.
I've create this issue to make it easier to use sensors and create a better example.
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/common/common.hh>
#include <iostream>
int main(int _argc, char **_argv)
{
// setup Gazebo server
if(gazebo::setupServer())
{
std::cout << "Gazebo server setup successful\n";
}
else
{
std::cout << "Gazebo server setup failed!\n";
}
gazebo::physics::WorldPtr world = gazebo::loadWorld("worlds/empty.world");
gazebo::sensors::run_once(true);
gazebo::sensors::run_threads();
gazebo::physics::run_worlds();
// Start the Gazebo GUI
system("gzclient &");
std::string cameraString =
"<?xml version='1.0' ?>"
"<sdf version='1.5'>"
"<model name='camera'>"
"<static>true</static>"
"<pose>4 -4 4 0 0.5 2.4</pose>"
"<link name=link>"
"<visual name='visual'>"
"<geometry>"
"<box>"
"<size>0.1 0.1 0.1</size>"
"</box>"
"</geometry>"
"</visual>"
"<sensor name='camera' type='camera'>"
"<camera>"
"<horizontal_fov>1.047</horizontal_fov>"
"<image>"
"<width>1280</width>"
"<height>960</height>"
"</image>"
"<clip>"
"<near>0.1</near>"
"<far>100</far>"
"</clip>"
"<save enabled='true'>"
"<path>/tmp/gazebo_frames</path>"
"</save>"
"</camera>"
"<always_on>1</always_on>"
"<update_rate>50</update_rate>"
"<visualize>true</visualize>"
"</sensor>"
"</link>"
"</model>"
"</sdf>";
// insert the camera
world->SetPaused(true);
int modelCountBefore = world->GetModelCount();
world->InsertModelString(cameraString);
world->SetPaused(false);
int retry=0;
while (world->GetModelCount() == modelCountBefore)
{
gazebo::common::Time::MSleep(100);
retry++;
if (retry>1000)
break;
}
if(world->GetModelCount() == modelCountBefore+1)
{
std::cout << "Successfully inserted model.\n";
}
else
{
std::cout << "Failed inserting model\n";
}
while(true)
{
gazebo::sensors::run_once(0);
gazebo::common::Time::MSleep(100);
}
gazebo::shutdown();
}
Asked by nkoenig on 2014-12-16 10:56:54 UTC
Comments
With this addition the example code works. However, in my actual application I still can't get it to work. I wrote a library that runs the Gazebo server and allows me to insert and manipulate models. In my main I have an instance of that class and go through some sample poses etc. So I can't have my main be stuck in this while loop. Am I understanding correctly that there is no way to move the sensor update into my library, for example in a callback to Events::ConnectWorldUpdateBegin?
Asked by NickDP on 2014-12-16 14:24:50 UTC
Comments
Can you get a backtrace from the segmentation fault? Let me know if you need more explanation than that link.
Asked by scpeters on 2014-12-14 05:27:56 UTC
I edited the question to include the backtrace. Can you conclude anything from it?
Asked by NickDP on 2014-12-15 12:26:27 UTC
I don't see the seg-fault in that portion of the backtrace. Can you try
thread apply all btand also paste the gdb error (ie.Program received signal SIGABRT, Aborted.)?Asked by scpeters on 2014-12-15 13:19:07 UTC
sorry about that. Hopefully now you can find what you're looking for.
Asked by NickDP on 2014-12-15 13:34:57 UTC
Thanks, I'll take a look.
Asked by scpeters on 2014-12-15 13:46:03 UTC
You might be missing some of the calls from Server::Run, such as
sensors::run_once(true), andsensors::run_threads()Asked by scpeters on 2014-12-15 13:55:47 UTC
Interesting, do I have to call these myself? I would have expected setupServer() to take care of this kind of thing. If I add these two commands to my main after loading the world, I the model is loaded, i.e. I can see the box in the world. However, I can't see the camera visualization and there are no frames saved to /tmp/gazebo_frames.
Asked by NickDP on 2014-12-15 14:20:16 UTC
Server.cc is our reference implementation; if it makes those calls, I would expect your program to need to do it as well. I cc'd some of my colleagues who may be able to provide more assistance.
Asked by scpeters on 2014-12-15 15:00:53 UTC