12 #include <opencv/cv.h>
13 #include <opencv/highgui.h>
17 using namespace OpenRAVE;
20 namespace cppexamples {
30 img = cvCreateImage(cvSize(geom.width,geom.height),IPL_DEPTH_8U,3);
38 boost::shared_ptr<SensorBase::CameraSensorData>
pdata;
46 penv->Load(
"data/pa10calib_envcamera.env.xml");
48 std::vector<RobotBasePtr> vrobots;
49 penv->GetRobots(vrobots);
50 if( vrobots.size() > 0 ) {
52 Transform t = vrobots.at(0)->GetTransform();
54 vrobots.at(0)->SetTransform(t);
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) {
65 vcameras.push_back(boost::shared_ptr<OpenRAVECamera>(
new OpenRAVECamera(*itsensor)));
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++) {
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];
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);
88 boost::this_thread::sleep(boost::posix_time::milliseconds(200));
96 int main(
int argc,
char ** argv)
99 return example.
main(argc,argv);