Inserting camera model programmatically

asked 2014-12-12 13:22:44 -0500

NickDP
186 ●12 ●14 ●18

updated 2014-12-15 12:32:45 -0500

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 <sensor> element, inserting the model is successful.

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 ...

(more)

edit retag flag offensive close merge delete

Comments

Can you get a backtrace from the segmentation fault? Let me know if you need more explanation than that link.

scpeters ( 2014-12-14 04:27:56 -0500 )edit

I edited the question to include the backtrace. Can you conclude anything from it?

NickDP ( 2014-12-15 11:26:27 -0500 )edit

I don't see the seg-fault in that portion of the backtrace. Can you try thread apply all bt and also paste the gdb error (ie. Program received signal SIGABRT, Aborted.)?

scpeters ( 2014-12-15 12:19:07 -0500 )edit

sorry about that. Hopefully now you can find what you're looking for.

NickDP ( 2014-12-15 12:34:57 -0500 )edit

Thanks, I'll take a look.

scpeters ( 2014-12-15 12:46:03 -0500 )edit

You might be missing some of the calls from Server::Run, such as sensors::run_once(true), and sensors::run_threads()

scpeters ( 2014-12-15 12:55:47 -0500 )edit

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.

NickDP ( 2014-12-15 13:20:16 -0500 )edit

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.

scpeters ( 2014-12-15 14:00:53 -0500 )edit

add a comment

answered 2014-12-16 09:56:54 -0500

nkoenig
7676 ●8 ●44 ●71 http://gazebosim.org/

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();
}

edit flag offensive delete link

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?

NickDP ( 2014-12-16 13:24:50 -0500 )edit

add a comment

Inserting camera model programmatically

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Inserting camera model programmatically edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Inserting camera model programmatically