void setPitch(Ogre::Entity* entity, int pitch){
std::vector<int> rollPitchYaw(3);
rollPitchYaw[0] = 0;
rollPitchYaw[1] = pitch;
rollPitchYaw[2] = 0;
(*::rotationRequests).insert(std::pair<Ogre::Entity*, std::vector<int>>(entity, rollPitchYaw));
}
virtual void draw(){
geometry_msgs::Vector3 v = rollPitchYaw(q);
glPushMatrix ();
//glRotatef(angle*180.0/M_PI,x,y,z);
glRotatef(v.z,0,0,1);
glRotatef(v.y,0,1,0);
glRotatef(v.x,1,0,0);
drawAxis(0.5);
Draw_Box (0.2);
glPopMatrix ();
//geometry_msgs::Vector3 v = rollPitchYaw(q);
char buff[256];
sprintf(buff,"Roll: %3.2f Pitch: %3.2f Yaw: %3.2f ",v.x,v.y,v.z);
//sprintf(buff,"Roll: %3.2f \nPitch: %3.2f \nYaw: %3.2f ",roll(q),pitch(q),yaw(q));
std::string rpy = buff;
glColor3f ( 0.9, 0.9, 0.9);
drawText(10,60,rpy.c_str());
}
