void vision::saveImageRaw(){
//First you have to declare an ALVisionImage to get the video buffer.
// ( definition included in alvisiondefinitions.h and alvisiondefinitions.cpp )
ALVisionImage* imageIn;
//Now you can get the pointer to the video structure.
try
{
imageIn = ( ALVisionImage* ) ( camera->call<int>( "getDirectRawImageLocal", name ) );
}catch( ALError& e)
{
log->error( "vision", "could not call the getImageLocal method of the NaoCam module" );
}
std::cout<< imageIn->toString();
static int saved_frames = 0;
int MAX_FRAMES = 150;
if (saved_frames > MAX_FRAMES)
return;
string EXT(".NFRM");
string BASE("/");
int NUMBER = saved_frames;
string FOLDER("/home/root/frames");
stringstream FRAME_PATH;
FRAME_PATH << FOLDER << BASE << NUMBER << EXT;
fstream fout(FRAME_PATH.str().c_str(), fstream::out);
// Retrive joints
//vector<float> joints = getVisionBodyAngles();
// Lock and write image
fout.write(reinterpret_cast<const char*>(imageIn->getFrame()), IMAGE_BYTE_SIZE);
// Write joints
//for (vector<float>::const_iterator i = joints.begin(); i < joints.end();
// i++) {
// fout << *i << " ";
//}
// Write sensors
//vector<float> sensor_data = getAllSensors();
//for (vector<float>::const_iterator i = sensor_data.begin();
// i != sensor_data.end(); i++) {
// fout << *i << " ";
//}
fout.close();
cout << "Saved frame #" << saved_frames++ << endl;
//Now you have finished with the image, you have to release it in the V.I.M.
try {
camera->call<int>( "releaseDirectRawImage", name );
} catch (ALError& e) {
log->error( "vision", "could not call the releaseImage method of the NaoCam module" );
}
}
void
Man::waitForImage ()
{
try {
const unsigned char *data;
#ifndef MAN_IS_REMOTE
ALVisionImage *image = NULL;
#else
ALValue image;
image.arraySetSize(6);
#endif
SleepMs(100);
data = NULL;
#ifndef MAN_IS_REMOTE
#ifdef DEBUG_IMAGE_REQUESTS
printf("Requesting local image of size %ix%i, color space %i\n",
IMAGE_WIDTH, IMAGE_HEIGHT, NAO_COLOR_SPACE);
#endif
// Attempt to retrive the next image
try {
image = (ALVisionImage*) (lem->call<int>("fetchNextLocal"));
}catch (ALError &e) {
log->error("NaoMain", "Could not call the fetchNextLocal method of the "
"NaoCam module");
}
if (image != NULL)
data = image->getFrame();
#else
#ifdef DEBUG_IMAGE_REQUESTS
printf("Requesting remote image of size %ix%i, color space %i\n",
IMAGE_WIDTH, IMAGE_HEIGHT, NAO_COLOR_SPACE);
#endif
// Attempt to retrive the next image
try {
image = lem->call<ALValue>("fetchNextRemote");
}catch (ALError &e) {
log->error("NaoMain", "Could not call the fetchNextRemote method of the "
"NaoCam module");
}
data = static_cast<const unsigned char *>(image[5].GetBinary());
#endif
if (data != NULL) {
// Update Sensors image pointer
sensors.lockImage();
sensors.setImage(data);
sensors.releaseImage();
}
}catch (ALError &e) {
log->error("NaoMain", "Caught an error in run():\n" + e.toString());
}
}
/**
* saveImage : save the last image received.
* @param pName path of the file
*/
void vision::saveImage( std::string pName ){
//First you have to declare an ALVisionImage to get the video buffer.
// ( definition included in alvisiondefinitions.h and alvisiondefinitions.cpp )
ALVisionImage* imageIn;
//Now you can get the pointer to the video structure.
try
{
imageIn = ( ALVisionImage* ) ( camera->call<int>( "getImageLocal", name ) );
}catch( ALError& e)
{
log->error( "vision", "could not call the getImageLocal method of the NaoCam module" );
}
std::cout<< imageIn->toString();
//You can get some informations of the image.
int width = imageIn->fWidth;
int height = imageIn->fHeight;
int nbLayers = imageIn->fNbLayers;
int colorSpace = imageIn->fColorSpace;
long long timeStamp = imageIn->fTimeStamp;
int seconds = (int)(timeStamp/1000000LL);
//You can get the pointer of the image.
uInt8 *dataPointerIn = imageIn->getFrame();
// now you create an openCV image and you save it in a file.
IplImage* image = cvCreateImage( cvSize( width, height ), 8, nbLayers );
// image->imageData = ( char* ) imageIn->getFrame();
image->imageData = ( char* ) dataPointerIn;
const char* imageName = ( pName + DecToString(seconds) + ".jpg" ).c_str();
cvSaveImage( imageName, image );
cvReleaseImage( &image );
//Now you have finished with the image, you have to release it in the V.I.M.
try
{
camera->call<int>( "releaseImage", name );
}catch( ALError& e)
{
log->error( "vision", "could not call the releaseImage method of the NaoCam module" );
}
}
void ALImageTranscriber::waitForImage ()
{
try {
#ifndef MAN_IS_REMOTE
#ifdef DEBUG_IMAGE_REQUESTS
printf("Requesting local image of size %ix%i, color space %i\n",
IMAGE_WIDTH, IMAGE_HEIGHT, NAO_COLOR_SPACE);
#endif
ALVisionImage *ALimage = NULL;
// Attempt to retrieve the next image
try {
ALimage = (ALVisionImage*) (camera->call<int>("getDirectRawImageLocal",lem_name));
}catch (ALError &e) {
log->error("NaoMain", "Could not call the getImageLocal method of the "
"NaoCam module");
}
if (ALimage != NULL) {
memcpy(&image[0], ALimage->getFrame(), IMAGE_BYTE_SIZE);
//image = ALimage->getFrame();
}
else
cout << "\tALVisionImage from camera was null!!" << endl;
#ifdef DEBUG_IMAGE_REQUESTS
//You can get some informations of the image.
int width = ALimage->fWidth;
int height = ALimage->fHeight;
int nbLayers = ALimage->fNbLayers;
int colorSpace = ALimage->fColorSpace;
long long timeStamp = ALimage->fTimeStamp;
int seconds = (int)(timeStamp/1000000LL);
printf("Retrieved an image of dimensions %ix%i, color space %i,"
"with %i layers and a time stamp of %is \n",
width, height, colorSpace,nbLayers,seconds);
#endif
#else//Frame is remote:
#ifdef DEBUG_IMAGE_REQUESTS
printf("Requesting remote image of size %ix%i, color space %i\n",
IMAGE_WIDTH, IMAGE_HEIGHT, NAO_COLOR_SPACE);
#endif
ALValue ALimage;
ALimage.arraySetSize(7);
// Attempt to retrive the next image
try {
ALimage = camera->call<ALValue>("getDirectRawImageRemote",
lem_name);
}catch (ALError &e) {
log->error("NaoMain", "Could not call the getImageRemote method of the "
"NaoCam module");
}
//image = static_cast<const unsigned char*>(ALimage[6].GetBinary());
memcpy(&image[0], ALimage[6].GetBinary(), IMAGE_BYTE_SIZE);
#ifdef DEBUG_IMAGE_REQUESTS
//You can get some informations of the image.
int width = (int) ALimage[0];
int height = (int) ALimage[1];
int nbLayers = (int) ALimage[2];
int colorSpace = (int) ALimage[3];
long long timeStamp = ((long long)(int)ALimage[4])*1000000LL +
((long long)(int)ALimage[5]);
int seconds = (int)(timeStamp/1000000LL);
printf("Retrieved an image of dimensions %ix%i, color space %i,"
"with %i layers and a time stamp of %is \n",
width, height, colorSpace,nbLayers,seconds);
#endif
#endif//IS_REMOTE
if (image != NULL) {
// Update Sensors image pointer
sensors->lockImage();
sensors->setImage(image);
sensors->releaseImage();
}
}catch (ALError &e) {
log->error("NaoMain", "Caught an error in run():\n" + e.toString());
}
}
ALValue vision::getBalls() {
ALValue resultBallRect;
resultBallRect.arraySetSize(4);
resultBallRect[0] = 0;
resultBallRect[1] = 0;
resultBallRect[2] = 0;
resultBallRect[3] = 0;
//First you have to declare an ALVisionImage to get the video buffer.
// ( definition included in alvisiondefinitions.h and alvisiondefinitions.cpp )
ALVisionImage* imageIn;
//Now you can get the pointer to the video structure.
try
{
imageIn = ( ALVisionImage* ) ( camera->call<int>( "getImageLocal", name ) );
}catch( ALError& e)
{
log->error( "vision", "could not call the getImageLocal method of the NaoCam module" );
}
//You can get some informations of the image.
int width = imageIn->fWidth;
int height = imageIn->fHeight;
int nbLayers = imageIn->fNbLayers;
int colorSpace = imageIn->fColorSpace;
long long timeStamp = imageIn->fTimeStamp;
int seconds = (int)(timeStamp/1000000LL);
// log->info( "vision", "Creating OpenCV image" );
//You can get the pointer of the image.
uInt8 *dataPointerIn = imageIn->getFrame();
// now you create an openCV image and you save it in a file.
IplImage* src = cvCreateImage( cvSize( width, height ), 8, nbLayers );
// src->imageData = ( char* ) imageIn->getFrame();
src->imageData = ( char* ) dataPointerIn;
//log->info( "vision", "Searching field" );
IplImage* mask = 0;
IplImage* imageClipped = 0;
// Get field
CvRect* fieldRect = new CvRect[1];
//printf("before getLargestColoredContour\n");
// Green field
// parameters for pan/tilt camera
//CvSeq* field = getLargestColoredContour(src, 155, 5, 100, 300, fieldRect);
// parameters for Nao camera in lab
// CvSeq* field = getLargestColoredContour(src, 175, 30, 25, 1000, fieldRect);
// Params for WEBOTS
CvSeq* field = getLargestColoredContour(src, 125, 30, 25, 100, &fieldRect, 1)[0];
if (field != NULL) {
// printf("Field: %d, %d, %d, %d\n", fieldRect.x, fieldRect.y, fieldRect.width, fieldRect.height);
//log->info( "vision", "Searching ball1" );
CvSize imageSize = cvSize(src->width, src->height);
mask = cvCreateImage( imageSize, 8, 1 );
cvZero(mask);
CvScalar colorWHITE = CV_RGB(255, 255, 255);
int elementCount = field->total;
CvPoint* temp = new CvPoint[elementCount];
CvPoint pt0 = **CV_GET_SEQ_ELEM( CvPoint*, field, elementCount - 1 );
for (int i = 0; i < elementCount; i++) {
CvPoint pt = **CV_GET_SEQ_ELEM( CvPoint*, field, i );
temp[i].x = pt.x;
temp[i].y = pt.y;
}
cvFillConvexPoly(mask, temp, elementCount, colorWHITE, 8, 0);
imageClipped = cvCreateImage( imageSize, 8, 3 );
cvZero(imageClipped);
cvCopy(src, imageClipped, mask);
// Get ball
CvRect* ballRect= new CvRect[10];
//log->info( "vision", "Searching ball2" );
// parameters for pan/tilt camera
//getLargestColoredContour(imageClipped, 17, 10, 100, 50, ballRect);
// parameters for Nao camera in lab
// CvSeq* ballHull = getLargestColoredContour(imageClipped, 40, 25, 50, 30, ballRect);
// Params for webots
CvSeq** ballHull = getLargestColoredContour(imageClipped, 55, 125, 50, 30, &ballRect, 0);
//log->info( "vision", "Searching ball3" );
int* X_Arr= new int[10];
int* Y_Arr= new int[10];
int* Width_Arr= new int[10];
int* Height_Arr= new int[10];
for(int i=0; ballHull[i] != NULL; i++) {
X_Arr[i]= ballRect[i].x;
Y_Arr[i]= ballRect[i].y;
Width_Arr[i]= ballRect[i].width;
Height_Arr[i]= ballRect[i].height;
}
if (ballHull != NULL) {
// printf("ballrect: %d, %d, %d, %d\n", ballRect.x, ballRect.y, ballRect.width, ballRect.height);
resultBallRect[0] = X_Arr;
resultBallRect[1] = Y_Arr;
resultBallRect[2] = Width_Arr;
resultBallRect[3] = Height_Arr;
// printf("Clearing ball Hull\n");
// cvClearSeq(ballHull);
// printf("Ball Hull cleared\n");
} else {
// printf("Ball not found!\n");
resultBallRect[0] = -2;
resultBallRect[1] = -2;
resultBallRect[2] = -2;
resultBallRect[3] = -2;
}
} else {
