openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
opencvsaving.cpp
Go to the documentation of this file.
1 
10 #include <openrave-core.h>
11 #include <vector>
12 #include <opencv/cv.h>
13 #include <opencv/highgui.h>
14 
15 #include "orexample.h"
16 
17 using namespace OpenRAVE;
18 using namespace std;
19 
20 namespace cppexamples {
21 
23 {
24 public:
26  {
27  pcamera=psensor;
28  pdata = boost::static_pointer_cast<SensorBase::CameraSensorData>(pcamera->CreateSensorData(SensorBase::ST_Camera));
29  geom = *boost::static_pointer_cast<SensorBase::CameraGeomData>(pcamera->GetSensorGeometry(SensorBase::ST_Camera));
30  img = cvCreateImage(cvSize(geom.width,geom.height),IPL_DEPTH_8U,3);
31  }
32  virtual ~OpenRAVECamera() {
33  cvReleaseImage(&img);
34  }
35 
38  boost::shared_ptr<SensorBase::CameraSensorData> pdata;
39  IplImage* img;
40 };
41 
43 {
44 public:
45  virtual void demothread(int argc, char ** argv) {
46  penv->Load("data/pa10calib_envcamera.env.xml");
47 
48  std::vector<RobotBasePtr> vrobots;
49  penv->GetRobots(vrobots);
50  if( vrobots.size() > 0 ) {
51  RAVELOG_INFO("moving the robot a little\n");
52  Transform t = vrobots.at(0)->GetTransform();
53  t.trans.x += 0.6;
54  vrobots.at(0)->SetTransform(t);
55  }
56 
57  // extract all the cameras
58  std::vector<SensorBasePtr> allsensors;
59  penv->GetSensors(allsensors);
60  std::vector< boost::shared_ptr<OpenRAVECamera> > vcameras;
61  for( std::vector<SensorBasePtr>::iterator itsensor = allsensors.begin(); itsensor != allsensors.end(); ++itsensor) {
62  if( (*itsensor)->Supports(SensorBase::ST_Camera) ) {
63  (*itsensor)->Configure(SensorBase::CC_PowerOn);
64  (*itsensor)->Configure(SensorBase::CC_RenderDataOn);
65  vcameras.push_back(boost::shared_ptr<OpenRAVECamera>(new OpenRAVECamera(*itsensor)));
66  }
67  }
68  while(IsOk()) {
69  // read the camera data and save the image
70  for(size_t icamera = 0; icamera < vcameras.size(); ++icamera) {
71  vcameras[icamera]->pcamera->GetSensorData(vcameras[icamera]->pdata);
72  if( vcameras[icamera]->pdata->vimagedata.size() > 0 ) {
73  char* imageData = vcameras[icamera]->img->imageData;
74  uint8_t* src = &vcameras[icamera]->pdata->vimagedata.at(0);
75  for(int i=0; i < vcameras[icamera]->geom.height; i++, imageData += vcameras[icamera]->img->widthStep, src += 3*vcameras[icamera]->geom.width) {
76  for(int j=0; j<vcameras[icamera]->geom.width; j++) {
77  // opencv is bgr while openrave is rgb
78  imageData[3*j] = src[3*j+2];
79  imageData[3*j+1] = src[3*j+1];
80  imageData[3*j+2] = src[3*j+0];
81  }
82  }
83  string filename = str(boost::format("camera%d.jpg")%icamera);
84  RAVELOG_INFO(str(boost::format("saving image %s")%filename));
85  cvSaveImage(filename.c_str(),vcameras[icamera]->img);
86  }
87  }
88  boost::this_thread::sleep(boost::posix_time::milliseconds(200));
89  }
90 
91  }
92 };
93 
94 } // end namespace cppexamples
95 
96 int main(int argc, char ** argv)
97 {
99  return example.main(argc,argv);
100 }