I use both OpenCV library and C++ NAOqi API in my code. In particular, I need to use the method setAngles() to set the head joints of the robot. Executing these lines:
//(1) set desired joint values on the desired joint names
pan = 0.0;
tilt = 0.0;
AL::ALValue names = AL::ALValue::array(PAN_JOINT,TILT_JOINT);
AL::ALValue angles = AL::ALValue::array(pan,tilt);
cout << "pan: " << pan << endl;
cout << "tilt: " << tilt << endl;
motionPtr->setAngles(names,angles,0.1f);
//(2) capture image from subscribed camera
AL::ALValue img = cameraPtr->getImageRemote(cameraClient);
imgHeader.data = (uchar*) img[6].GetBinary();
cameraPtr->releaseImage(cameraClient);
//(3) show image
imshow("test",imgHeader);
waitKey(30);
results into a crash with the following error:
terminate called after throwing an instance of 'AL::ALError' what(): ALMotion::setAngles ALBroker::methodCall: method: setAngles, params: [["HeadYaw", "HeadPitch"], [0, 0], 0] ALMotion::setAngles ALMotion::setAngles fractionMaxSpeed: Expected a fraction of max speed between 0.0 and 1.0
i.e. it says that the third argument of setAngles()
is not a value between 0.0 and 1.0 as expected. Actually, it reads it as a 0 (maybe casted to int
?). Of course, this is not true, as you can see from the code. The "funny" aspect is that, if I comment the cv::imshow()
, the code normally works and no errors are raised.
Does anybody knows a possible reason why this could happen? Apparently, there are no connections between the operations of setting angles on the robot and displaying the image through OpenCV. Can you help me? Thanks
UPDATE: Maybe I forgot to say that this code is inside a while loop. Anyway, using the post
attribute of NAOqi proxies objects, I made the method setAngles
run in background in a parallel thread. In this way the image is properly shown and no errors are raised. However, the robot joints are not set, and the robot is still. Again, this does not happen if the cv::imshow
is not called. Here is the full function where the whole while loop is contained.
//these are member variables of a class
double pan = 0.0;
double tilt = 0.0;
Mat imgHeader = Mat(320,240,CV8UC3);
bool headset = false;
//main loop
while(!headset){
motionPtr->setStiffnesses("Head", 1.0f);
int dtilt, dpan;
int res;
/**** Get inputs from joystick ***/
res = joy->readEv();
if(res != -1){
if(jse->type & JS_EVENT_BUTTON){
if((int)jse->number == X_BUTTON){//tilt down
dtilt = -1;
}
else if((int)jse->number == TRIANGLE_BUTTON){//tilt up
dtilt = 1;
}
else if((int)jse->number == START_BUTTON){//tilt down
(firstKeyIgnored) ? (headset = true) : (firstKeyIgnored = true) ;
}
else{
dtilt = 0;
}
}
updateTilt(dtilt); //<-- it just changes the value of the member variable `tilt`
}
AL::ALValue names = AL::ALValue::array(PAN_JOINT,TILT_JOINT);
AL::ALValue angles = AL::ALValue::array(pan,tilt);
int id;
id = motionPtr->post.setAngles(names,angles,0.1f);
//capture image from subscribed camera
ALimg = cameraPtr->getImageRemote(cameraClient);
imgHeader.data = (uchar*) ALimg[6].getObject();
imshow("test",imgHeader);
waitKey(30);//*/
}