// 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);
}
std::string HuboDescription::getJointName(size_t joint_index) const
{
return std::string(getJointInfo(joint_index).name);
}
