void display(void)
{
if(boolpause==0){
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
robotOrientation();
gluLookAt(deltaX+betaX,deltaY+betaY,deltaZ+betaZ,0.0+deltaX,0.0,0.0+deltaZ, 0.0,1.0,0.0);
//Draw ground
glBegin(GL_QUADS);
glColor4f(0.0f,1.0f,0.0f,0.0f); //Green
glVertex3f(-5.0f, 0.0f, -5.0f);
glVertex3f(-5.0f, 0.0f, (float)(MaxDistance)+5.0f);
glVertex3f((float)(MaxDistance)+5.0f,0.0f,(float)(MaxDistance)+5.0f);
glVertex3f((float)(MaxDistance)+5.0f,0.0f,-5.0f);
glEnd();
//Draw Robot
drawRobot();
//Draw Buildings
drawBuildings(GL_RENDER);
glutSwapBuffers();
}
}
void mouse(int button, int state, int x, int y)
{
GLuint selectBuf[SIZE];
GLint hits;
GLint viewport[4];
if(button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
{
glGetIntegerv (GL_VIEWPORT, viewport);
glSelectBuffer(SIZE, selectBuf);
glRenderMode(GL_SELECT);
glInitNames();
glPushName(0);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), 5.0,5.0,viewport);
gluPerspective(60.0f,(float)Window_Width/(float)Window_Height,1.0f,100.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
robotOrientation();
gluLookAt(deltaX+betaX,deltaY+betaY,deltaZ+betaZ,0.0+deltaX,0.0,0.0+deltaZ, 0.0,1.0,0.0);
drawBuildings(GL_SELECT);
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glFlush();
glMatrixMode(GL_MODELVIEW);
hits = glRenderMode(GL_RENDER);
processHits(hits, selectBuf);
//printf("Left Button Press Detected! X: %i Y: %i \n", x, y);
glutPostRedisplay();
}
}
void RobotModel::publishTF(bool useMeasurement)
{
boost::shared_ptr<SingleSupportModel> model = m_models[0];
ros::Time now = ros::Time::now();
if(useMeasurement)
model->updateKinematics(SingleSupportModel::MeasurementData);
else
model->updateKinematics(SingleSupportModel::CommandData);
m_tf_buf[0].stamp_ = model->joint(1)->feedback.stamp;
m_tf_buf[0].setRotation(robotOrientation());
for(size_t i = 1; i < model->mBodies.size(); ++i)
{
ROS_ASSERT(i < m_tf_buf.size());
tf::StampedTransform* t = &m_tf_buf[i];
rbdlToTF(model->X_lambda[i], t);
t->stamp_ = model->joint(i-1)->feedback.stamp;
}
m_pub_tf.sendTransform(m_tf_buf);
ros::Time fixedTime = now + ros::Duration(0.5);
for(size_t i = 0; i < m_tf_fixed_buf.size(); ++i)
m_tf_fixed_buf[i].stamp_ = fixedTime;
m_pub_tf.sendTransform(m_tf_fixed_buf);
// Publish compass vector
geometry_msgs::Vector3Stamped msg;
msg.header.stamp = now;
msg.header.frame_id = "/trunk_link";
tf::vectorEigenToMsg(m_magneticFieldVector, msg.vector);
m_pub_compass.publish(msg);
robotcontrol::CompassHeading hmsg;
Eigen::Quaterniond qrot;
tf::quaternionTFToEigen(m_robotOrientation, qrot);
Eigen::Vector3d tilt_compensated = qrot * m_magneticFieldVector;
double angle = atan2(tilt_compensated.y(), tilt_compensated.x());
hmsg.stamp = now;
hmsg.heading = angle;
m_pub_compassHeading.publish(hmsg);
}