Guest User

MWE camera model

a guest
Dec 12th, 2014
207
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <gazebo/gazebo.hh>
  2. #include <gazebo/physics/physics.hh>
  3. #include <gazebo/common/common.hh>
  4. #include <iostream>
  5.  
  6. using namespace std;
  7.  
  8. int main(int _argc, char **_argv)
  9. {
  10.     // setup Gazebo server
  11.     if(gazebo::setupServer())
  12.     {
  13.         cout << "Gazebo server setup successful\n";
  14.     }
  15.     else
  16.     {
  17.         cout << "Gazebo server setup failed!\n";
  18.     }
  19.     gazebo::physics::WorldPtr world = gazebo::loadWorld("worlds/empty.world");
  20.     world->Run(0);
  21.     // Start the Gazebo GUI
  22.     system("gzclient &");
  23.    
  24.     string cameraString = "<?xml version=\"1.0\" ?>\
  25.                            <sdf version=\"1.5\">\
  26.                              <model name=\"camera\">\
  27.                                <static>true</static>\
  28.                                <pose>4 -4 4 0 0.5 2.4</pose>\
  29.                                <link name=link>\
  30.                                  <pose>0.05 0.05 0.05 0 0 0</pose>\
  31.                                  <inertial>\
  32.                                    <mass>0.1</mass>\
  33.                                  </inertial>\
  34.                                  <visual name=\"visual\">\
  35.                                      <geometry>\
  36.                                          <box>\
  37.                                              <size>0.1 0.1 0.1</size>\
  38.                                          </box>\
  39.                                      </geometry>\
  40.                                  </visual>\
  41.                                  <sensor name=\"camera\" type=\"camera\">\
  42.                                    <camera>\
  43.                                      <horizontal_fov>1.047</horizontal_fov>\
  44.                                      <image>\
  45.                                        <width>1280</width>\
  46.                                        <height>960</height>\
  47.                                      </image>\
  48.                                      <clip>\
  49.                                        <near>0.1</near>\
  50.                                        <far>100</far>\
  51.                                      </clip>\
  52.                                      <save enabled=\"true\">\
  53.                                        <path>/tmp/gazebo_frames</path>\
  54.                                      </save>\
  55.                                    </camera>\
  56.                                    <always_on>1</always_on>\
  57.                                    <update_rate>50</update_rate>\
  58.                                    <visualize>true</visualize>\
  59.                                  </sensor>\
  60.                                </link>\
  61.                              </model>\
  62.                            </sdf>";
  63.    
  64.     // insert the camera
  65.     world->SetPaused(true);
  66.     int modelCountBefore = world->GetModelCount();
  67.     world->InsertModelString(cameraString);
  68.     world->SetPaused(false);
  69.     int retry=0;
  70.     while (world->GetModelCount() == modelCountBefore)
  71.     {
  72.         gazebo::common::Time::MSleep(100);
  73.          retry++;
  74.          if (retry>1000)
  75.              break;
  76.     }
  77.     if(world->GetModelCount() == modelCountBefore+1)
  78.     {
  79.         cout << "Successfully inserted model.\n";
  80.     }
  81.     else
  82.     {
  83.         cout << "Failed inserting model\n";
  84.     }
  85.    
  86.     cin.ignore();
  87.    
  88.     gazebo::shutdown();
  89. }
RAW Paste Data