nao机器人视觉模块创建和调用
nao作为一个机器人，视觉对其非常重要，也就是它的摄像头。在nao这个平台上如何从摄像头获取图像数据进而进行处理?首先看看给的例子：
#include
#include
#include
// Opencv includes.
#include
#include
#include
#include
using namespace AL;
void showImages(const std::string& robotIp)
{
// Create a proxy to ALVideoDevice on the robot.
ALPtr camProxy = makeALPtr(new ALVideoDeviceProxy(robotIp, 9559));
// Subscribe a client image requiring 320*240 and BGR colorspace.
const std::string clientName = camProxy->subscribe("test", kQVGA, kBGRColorSpace, 30);
// Create an iplimage header to wrap into an opencv image.
IplImage* imgHeader = cvCreateImageHeader(cvSize(320, 240), 8, 3);
cvNamedWindow("images");
// Main loop. Exit when pressing ESC.
while (cvWaitKey(30) != 64)
{
// Retrieve an image from the camera.
ALValue img = camProxy->getImageRemote(clientName);
// Assign our image binary to the opencv image.
imgHeader->imageData = (char*)img[6].GetBinary();
// Display it on screen.
cvShowImage("images", imgHeader);
}
// Cleanup.
camProxy->unsubscribe(clientName);
cvReleaseImageHeader(&imgHeader);
}
int main(int argc, char* argv[])
{
if (argc < 2)
{
std::cerr << "Usage 'getimages robotIp'" << std::endl;
return 1;
}
const std::string robotIp(argv[1]);
try
{
showImages(robotIp);
}
catch (const ALError& e)
{
std::cerr << "Caught exception " << e.what() << std::endl;
}
return 0;
}
能够看出来首先是要定义一个ALVideoDeviceProxy 用于操作视频设备，其中最为重要的一个操作是const std::string clientName = camProxy->subscribe("test", kQVGA, kBGRColorSpace, 30);，这个相当是从nao的视频管理那订阅了视频，然后是ALValue img = camProxy->getImageRemote(clientName);，把图像取出来。剩下了就是不断的取出图像显示，当按下Esc时退出。最后在程序结束前释放订阅的视频，释放分配的图像内存。从这个例子我们可以看出，要操作图像有4个步骤：1、申请ALVideoDeviceProxy 的代理，2、订阅subscribe 3、获取图像，4、退定unsubscribe。这个例子是可执行的，可以直接在电脑上运行，远程的获取摄像头的图像数据。然后我们需要的是一个module。
在图像处理的过程中，一要考虑的是循环调用，二要考虑速度，也就是会不断的读取图像处理图像。在这里我们暂时没有用例子里的视觉模板，而是自己生成的module。
所在自己的module里主要有三个部分 申请 处理 退订。故而处理部分就会被循环的调用。
申请：
AL::ALPtr
openCamera = AL::ALPtr
moduleName =openCamera->subscribe(moduleName,kQVGA,kBGRColorSpace, 30 );
这里只是主要步骤，为了提高速度，会将图像部分的变量定义为成员变量。
处理：
imageln = (ALImage*)openCamera->getImageLocal(moduleName);
ballimg->imageData = (char*)imageln->getFrame();
openCamera->releaseImage(moduleName);
在ballimg->imageData里就是图像的数据，可以循环的调用进行你的处理了。
退订：
openCamera->unsubscribe(moduleName);
cvReleaseImage(&ballimg);
cvReleaseImage(&ballimg_gray);
cvReleaseImage(&ballimg_rgb);
cvReleaseImage(&ballimgtohsv);
cvReleaseImage(&dstContourBallImg);
cvReleaseImage(&dstcontourBallImg2);
在生成module后只需调用一次申请，然后循环调用处理，最后调用退订。
以下是一个参考：
///.h
/**
* @author
*
* This file was generated by Aldebaran Robotics ModuleGenerator
*/
#ifndef RECOGNIZEBALL_RECBALL_H
#define RECOGNIZEBALL_RECBALL_H
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
#include
#include
#include
namespace AL
{
class ALBroker;
}
/**
* DESCRIBE YOUR CLASS HERE
*/
class Recball : public AL::ALModule
{
public:
/**
* Default Constructor.
*/
Recball(AL::ALPtr broker, const std::string& name);
void Camera();
int OnFindBall();
void HeadStart();
void ReleaseImage();
float BallAngleAndDistance();
void getCameraHeight();
float CameraHeight;
float returnHeadYawAngle();
float returnHeadPitchAngle();
/**
* Destructor.
*/
virtual ~Recball();
float headyawangle;
float headpitchangle;
int FindBall;
/**
* dummy Function. An autogenerated example method.
* @param msg Message to show on screen
* @param foo The method will return this parameter
* @return The AL::ALValue sent as the foo parameter
*/
AL::ALValue dummyFunction(const std::string& msg, const AL::ALValue& foo);
private:
CvSeq * dstcontour;
CvMemStorage * dststorage;
CvPoint *p0;
CvPoint m_center;
AL::ALValue targetAngles ;
int loseBallTimes;
AL::ALImage *imageln;
IplImage* dstContourBallImg;
IplImage* dstcontourBallImg2;
IplImage* ballimg_rgb;
IplImage* smoothdst;
IplImage* ballimg_gray;
IplImage* ballimgtohsv;
IplImage* ballimg ;
// std::string ballpId;
std::string moduleName ;
int recflag;
int redballpixelnum;
int redballpixelnum2;
int blueballpixelnum;
float redballarea;
float blueballarea;
float redballangle;
float blueballangle;
//float redballr;
float blueballr;
float redballdis;
float blueballdis;
AL::ALValue robotToredballdis;
float robotToblueballdis;
//float robotheight;
float photoredballr;
float photoblueballr;
AL::ALPtr HeadYawMemoryData;
AL::ALPtr openCamera;
AL::ALPtr motionProxy;
};
#endif // RECOGNIZEBALL_RECBALL_H
///.cpp
/**
* @author
*
* Version : $Id$
* This file was generated by Aldebaran Robotics ModuleGenerator
*/
#include "recball.h"
#include
#include
#include
#include
#include
//use opencv
#include
#include
#include
#include
using namespace AL;
using namespace std;
/**
* Constructor for Recball object
* @param broker The parent broker
* @param name The name of the module
*/
Recball::Recball(
AL::ALPtr broker,
const std::string& name): AL::ALModule(broker, name)
{
setModuleDescription("This is an autogenerated module, this descriptio needs to be updated.");
functionName("dummyFunction", "Recball", "An autogenerated dummyFunction.");
addParam("msg", "A string describing a message: the module will print this message when this method is called");
addParam("foo", "A second parameter parameter. An AL::ALValue that will be returned.");
setReturn("return", "Returns the foo parameter");
BIND_METHOD(Recball::dummyFunction);
functionName("Camera", "Recball", "An function get redballimage from video.");
BIND_METHOD(Recball::Camera);
functionName("OnFindBall", "Recball", "return a Vaule to mark is nao find a ball .");
setReturn("return", "Returns a value if it is 1 ,find a ball else not find a ball ");
BIND_METHOD(Recball::OnFindBall);
functionName("BallAngleAndDistance", "Recball", "An function to get the head yaw horizontal angle.");
setReturn("return", "Returns the distance between the ball and the robot");
BIND_METHOD(Recball::BallAngleAndDistance);
functionName("returnHeadYawAngle", "Recball", "Return the ball's angle relative to the head Yaw.");
setReturn("return", "Returns the ball angle relative to nao's yaw ");
BIND_METHOD(Recball::returnHeadYawAngle);
functionName("returnHeadPitchAngle", "Recball", "Return the ball's angle relative to the head pitch.");
setReturn("return", "Returns the ball angle relative to nao's pitch ");
BIND_METHOD(Recball::returnHeadPitchAngle);
functionName("ReleaseImage", "Recball", "release the memory of all image and unsubscribe the camera ");
BIND_METHOD(Recball::ReleaseImage);
functionName("HeadStart", "Recball", "start head follow the ball ");
BIND_METHOD(Recball::HeadStart);
}
/**
* Destructor for Recball object
*/
Recball::~Recball()
{
}
/**
* dummyFunction
* @param msg Message to show on screen
* @param foo The function will return this parameter
* @return The AL::ALValue given by the foo parameter
*/
AL::ALValue Recball::dummyFunction(
const std::string& msg,
const AL::ALValue& foo)
{
std::cout << "Recball received message : " << msg << std::endl;
if (foo.isValid())
std::cout << "Return value: " << foo.toString(AL::VerbosityMini) << std::endl;
else
std::cout << "The value you sent was invalid." << std::endl;
return foo;
}
//*****************************************************************//
// The function to get the Camera height //
//*****************************************************************//
void Recball::getCameraHeight()
{
std::vectorresult = motionProxy->getPosition("CameraBottom",2,true);
CameraHeight = result.at(2);
}
//******camera() in orger to open the camera and select the bottom or top camera *********//
void Recball::Camera()
{
//open the camera but in the choregraph we can use the selectcamera box instead
HeadYawMemoryData = getParentBroker()->getMemoryProxy();
motionProxy = getParentBroker()->getMotionProxy();
// openCamera=getParentBroker()->getProxy( "ALVideoDevice" );
openCamera = AL::ALPtr(new ALVideoDeviceProxy(getParentBroker()));
// motionProxy = getParentBroker()->getMotionProxy();
//bianliang
CameraHeight = 0.0;
recflag=0;
redballpixelnum=0;
blueballpixelnum=0;
redballarea=0.0;
redballangle=0.0;
//redballr=0.0;
redballdis=0.0;
//robotToredballdis=0.0;
//robotheight=0.0;
photoredballr=0.0;
FindBall = 0;
loseBallTimes = 0.0;
targetAngles = AL::ALValue::array(0.0,0.0);
moduleName = "my_GVM1";
ballimgtohsv=cvCreateImage(cvSize(320,240),8,3);
ballimg = cvCreateImageHeader(cvSize(320, 240), 8, 3);
ballimg_rgb=cvCreateImage(cvSize(320,240),8,3);
ballimg_gray=cvCreateImage(cvSize(320,240),8,1);
smoothdst = cvCreateImage(cvSize(320,240),8,1);
dstContourBallImg = cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1);
dstcontourBallImg2=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1);
imageln = NULL;
headyawangle = 0.0;
headpitchangle = 0.0;
moduleName =openCamera->subscribe(moduleName,kQVGA,kBGRColorSpace, 30 );
}
//********function findball() in order to find ball through the camera *********//
int Recball::OnFindBall()
{
return FindBall;
}
float Recball::BallAngleAndDistance()
{
imageln = (ALImage*)openCamera->getImageLocal(moduleName);
ballimg->imageData = (char*)imageln->getFrame();
openCamera->releaseImage(moduleName);
cvCvtColor(ballimg,ballimgtohsv,CV_BGR2HSV);
//find the target ball
CvScalar pixel= {};
for (int i=0;iheight;i++)
{
for (int j=0;jwidth;j++)
{
pixel=cvGet2D(ballimgtohsv,i,j);
if ((pixel.val[0]<15&&50100&&pixel.val[1]>45))
{
//recflag=1;
pixel.val[0]=0;
pixel.val[1]=1;
pixel.val[2]=255;
cvSet2D(ballimgtohsv, i, j, pixel);
}
else
{
pixel.val[0]=0;
pixel.val[1]=0;
pixel.val[2]=0;
cvSet2D(ballimgtohsv, i, j, pixel);
}
}
}
//find the contour
cvCvtColor(ballimgtohsv,ballimg_rgb,CV_HSV2BGR);
cvCvtColor(ballimg_rgb,ballimg_gray,CV_BGR2GRAY);
cvErode(ballimg_gray,ballimg_gray,NULL,1);
cvSmooth(ballimg_gray,smoothdst,CV_MEDIAN ,3,0,0,0);
dststorage = cvCreateMemStorage(0);
dstcontour = 0;
int mode = CV_RETR_EXTERNAL;
int contournum=cvFindContours(smoothdst,dststorage, &dstcontour, sizeof(CvContour), mode,CV_CHAIN_APPROX_NONE);
if(contournum>0)
{
FindBall = 1;
}
else
{
FindBall = 0;
return 0.0;
}
double maxcontourarea=0.0;
double contourtemparea=0.0;
CvSeq* c=0;
CvSeq* area_max_contour = 0 ;
for(c=dstcontour;c!=NULL;c=c->h_next)
{
contourtemparea = fabs(cvContourArea(c, CV_WHOLE_SEQ ));
if (contourtemparea>maxcontourarea)
{
maxcontourarea=contourtemparea;
area_max_contour=c;
p0 = NULL;
m_center=cvPoint(0,0);
for(int i=0;itotal;i++)
{
p0= (CvPoint *)cvGetSeqElem( area_max_contour, i );
m_center.x+=p0->x;
m_center.y+=p0->y;
}
m_center.x/=area_max_contour->total;
m_center.y/=area_max_contour->total;
}
}
// the ball angle seen with nao
float pixelradian=(34.8*3.1415)/180;
headyawangle=((m_center.x-160.0)/320.0)*pixelradian;
headpitchangle=((m_center.y-120.0)/240.0)*pixelradian;
// the distance between ball and nao's camera.
getCameraHeight();
float ballImageRadius = sqrt(maxcontourarea/3.1415);
float ballAngle = ((ballImageRadius*2.0)/240.0)*34.8;
float ballToCameraDis = 0.035/sin((ballAngle/(2*180))*3.1415);
float ballToRobotDis = sqrt(ballToCameraDis*ballToCameraDis-CameraHeight*CameraHeight);
// openCamera->call("releaseImage",ballpId);
// openCamera->unsubscribe(moduleName);
return ballToRobotDis;
}
float Recball::returnHeadYawAngle()
{
return headyawangle;
}
float Recball::returnHeadPitchAngle()
{
return headpitchangle;
}
void Recball::HeadStart()
{
AL::ALValue headNames = AL::ALValue::array("HeadYaw","HeadPitch");
AL::ALValue names = "Head";
float maxSpeedFraction = 0.5f;
if(FindBall==0)
{
headyawangle = 0.0;
headpitchangle = 0.0;
loseBallTimes = loseBallTimes + 1;
if(loseBallTimes == 2)
{
targetAngles = AL::ALValue::array(0.0,0.15);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
}
if(loseBallTimes == 5)
{
targetAngles = AL::ALValue::array(0.15,0.15);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
}
if(loseBallTimes ==8 )
{
targetAngles = AL::ALValue::array(-0.15,0.15);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
}
if(loseBallTimes == 11)
{
targetAngles = AL::ALValue::array(0.0,-0.15);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
}
if(loseBallTimes == 14)
{
targetAngles = AL::ALValue::array(0.0,-0.25);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
}
if(loseBallTimes ==17)
{
//targetAngles = AL::ALValue::array(0.0,-0.25);
targetAngles = AL::ALValue::array(0.0,-0.1);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
motionProxy->walkTo(0.5,0.0,0.0);
}
if(loseBallTimes >17)
{
loseBallTimes =0;
AL::ALValue targetAngles = AL::ALValue::array(0.0,-0.1);
motionProxy->setAngles(headNames,targetAngles,maxSpeedFraction);
}
}
else
{
if(headpitchangle<0.0)
{
headpitchangle = 0.0;
}
AL::ALValue targetAngles = AL::ALValue::array(-headyawangle,headpitchangle);
float maxSpeedFraction = 0.4f;
motionProxy->changeAngles(headNames,targetAngles,maxSpeedFraction);
}
}
void Recball::ReleaseImage()
{
openCamera->unsubscribe(moduleName);
cvReleaseImage(&ballimg);
cvReleaseImage(&ballimg_gray);
cvReleaseImage(&ballimg_rgb);
cvReleaseImage(&ballimgtohsv);
cvReleaseImage(&dstContourBallImg);
cvReleaseImage(&dstcontourBallImg2);
}
这样在循环里只进行图像处理会提升不少处理速度。