void onIdle() {
float move = sinf(glutGet(GLUT_ELAPSED_TIME) / 1000.0 * (2*3.14) / 5); // -1<->+1 every 5 seconds
float angle = glutGet(GLUT_ELAPSED_TIME) / 1000.0 * 45; // 45° per second
glm::vec3 axis_z(0, 0, 1);
// glm::mat4 m_transform = glm::translate(glm::mat4(1.0f), glm::vec3(move, 0.0, 0.0))
// * glm::rotate(glm::mat4(1.0f), glm::radians(angle), axis_z);
// Experiment!
// Remember what we mentioned about applying matrices in the reverse order?
// In our example, we rotate first, then translate.
// Try to do it the other way around: you'll make the triangle rotate after it's been moved,
// which means it will rotate around the origin rather than around its own center.
int val = glutGet(GLUT_ELAPSED_TIME) % 10000;
glm::mat4 m_transform = glm::mat4(1.0f);
if (val < 4800) {
m_transform = glm::rotate(glm::mat4(1.0f), glm::radians(angle), axis_z) * glm::translate(glm::mat4(1.0f), glm::vec3(move, 0.0, 0.0));
} else if (val > 5200) {
m_transform = glm::translate(glm::mat4(1.0f), glm::vec3(move, 0.0, 0.0))
* glm::rotate(glm::mat4(1.0f), glm::radians(angle), axis_z);
}
// This would result in the same as in 03.1
// Just moving to left and right as no rotation is aplied
// glm::radians(0.0f)....
//
// glm::mat4 m_transform = glm::translate(glm::mat4(1.0f), glm::vec3(move, 0.0, 0.0))
// * glm::rotate(glm::mat4(1.0f), glm::radians(0.0f), axis_z);
glUseProgram(program);
glUniformMatrix4fv(uniform_m_transform, 1, GL_FALSE, glm::value_ptr(m_transform));
glutPostRedisplay();
}
void renderFrame()
{
//glClear(GL_COLOR_BUFFER_BIT);
// Clear the backbuffer and depth-buffer
glClearColor( 0.0f, 0.0f, 0.5f, 1.0f );
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
// Set the shader program and the texture
glUseProgram( g_hShaderProgram );
float angle=0.0f;
float move=0.0f;move=0.5*glm::sin(omp_get_wtime());glm::vec3 Scale(move,move,move);
glm::vec3 axis_z(1.0f, 1.0f, 1.0f);
glm::vec3 scale(0.5f,0.5f,0.5f);
glm::mat4 m_transform = glm::translate(glm::mat4(1.0f), Scale)
* glm::rotate(glm::mat4(1.0f), angle, axis_z)*glm::scale(glm::mat4(1.0f),scale);
glUniformMatrix4fv(uniform_m_transform, 1, GL_FALSE, glm::value_ptr(m_transform));
// Draw the colored triangle
glBindBuffer(GL_ARRAY_BUFFER,vbo_triangle);
glEnableVertexAttribArray(attribute_coord2d);
glVertexAttribPointer(
attribute_coord2d, // attribute
2, // number of elements per vertex, here (x,y)
GL_FLOAT, // the type of each element
GL_FALSE, // take our values as-is
0, // no extra data between each position
0 // pointer to the C array
);
glDrawArrays(GL_TRIANGLES,0,6);
glDisableVertexAttribArray( attribute_coord2d );
}
// This is our callback to handle an incoming message.
void EffortDisplay::processMessage( const sensor_msgs::JointState::ConstPtr& msg )
{
// We are keeping a circular buffer of visual pointers. This gets
// the next one, or creates and stores it if the buffer is not full
boost::shared_ptr<EffortVisual> visual;
if( visuals_.full() )
{
visual = visuals_.front();
}
else
{
visual.reset(new EffortVisual( context_->getSceneManager(), scene_node_, robot_model_ ));
}
V_string joints;
int joint_num = msg->name.size();
for (int i = 0; i < joint_num; i++ )
{
std::string joint_name = msg->name[i];
JointInfo* joint_info = getJointInfo(joint_name);
if ( !joint_info ) continue; // skip joints..
// set effort
joint_info->setEffort(msg->effort[i]);
// update effort property ???
if ( ros::Time::now() - joint_info->last_update_ > ros::Duration(0.2) ) {
joint_info->last_update_ = ros::Time::now();
}
const urdf::Joint* joint = robot_model_->getJoint(joint_name).get();
int joint_type = joint->type;
if ( joint_type == urdf::Joint::REVOLUTE )
{
// we expects that parent_link_name equals to frame_id.
std::string parent_link_name = joint->child_link_name;
Ogre::Quaternion orientation;
Ogre::Vector3 position;
// Here we call the rviz::FrameManager to get the transform from the
// fixed frame to the frame in the header of this Effort message. If
// it fails, we can't do anything else so we return.
if( !context_->getFrameManager()->getTransform( parent_link_name,
ros::Time(),
//msg->header.stamp, // ???
position, orientation ))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
parent_link_name.c_str(), qPrintable( fixed_frame_) );
continue;
}
;
tf::Vector3 axis_joint(joint->axis.x, joint->axis.y, joint->axis.z);
tf::Vector3 axis_z(0,0,1);
tf::Quaternion axis_rotation(tf::tfCross(axis_joint, axis_z), tf::tfAngle(axis_joint, axis_z));
if ( std::isnan(axis_rotation.x()) ||
std::isnan(axis_rotation.y()) ||
std::isnan(axis_rotation.z()) ) axis_rotation = tf::Quaternion::getIdentity();
tf::Quaternion axis_orientation(orientation.x, orientation.y, orientation.z, orientation.w);
tf::Quaternion axis_rot = axis_orientation * axis_rotation;
Ogre::Quaternion joint_orientation(Ogre::Real(axis_rot.w()), Ogre::Real(axis_rot.x()), Ogre::Real(axis_rot.y()), Ogre::Real(axis_rot.z()));
visual->setFramePosition( joint_name, position );
visual->setFrameOrientation( joint_name, joint_orientation );
visual->setFrameEnabled( joint_name, joint_info->getEnabled() );
}
}
// Now set or update the contents of the chosen visual.
float scale = scale_property_->getFloat();
float width = width_property_->getFloat();
visual->setWidth( width );
visual->setScale( scale );
visual->setMessage( msg );
visuals_.push_back(visual);
}
