void Robot::update(const LinkUpdater& updater)
{
M_NameToLink::iterator link_it = links_.begin();
M_NameToLink::iterator link_end = links_.end();
for ( ; link_it != link_end; ++link_it )
{
RobotLink* link = link_it->second;
link->setToNormalMaterial();
Ogre::Vector3 visual_position, collision_position;
Ogre::Quaternion visual_orientation, collision_orientation;
if( updater.getLinkTransforms( link->getName(),
visual_position, visual_orientation,
collision_position, collision_orientation
))
{
link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation );
std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin();
std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end();
for ( ; joint_it != joint_end ; ++joint_it )
{
RobotJoint *joint = getJoint(*joint_it);
if (joint)
{
joint->setTransforms(visual_position, visual_orientation);
}
}
}
else
{
link->setToErrorMaterial();
}
}
}
void Robot::update(const LinkUpdater& updater)
{
M_NameToLink::iterator link_it = links_.begin();
M_NameToLink::iterator link_end = links_.end();
for ( ; link_it != link_end; ++link_it )
{
RobotLink* link = link_it->second;
link->setToNormalMaterial();
Ogre::Vector3 visual_position, collision_position;
Ogre::Quaternion visual_orientation, collision_orientation;
if( updater.getLinkTransforms( link->getName(),
visual_position, visual_orientation,
collision_position, collision_orientation
))
{
// Check if visual_orientation, visual_position, collision_orientation, and collision_position are NaN.
if (visual_orientation.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"visual orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
link->getName().c_str()
);
continue;
}
if (visual_position.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"visual position of %s contains NaNs. Skipping render as long as the position is invalid.",
link->getName().c_str()
);
continue;
}
if (collision_orientation.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"collision orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
link->getName().c_str()
);
continue;
}
if (collision_position.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"collision position of %s contains NaNs. Skipping render as long as the position is invalid.",
link->getName().c_str()
);
continue;
}
link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation );
std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin();
std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end();
for ( ; joint_it != joint_end ; ++joint_it )
{
RobotJoint *joint = getJoint(*joint_it);
if (joint)
{
joint->setTransforms(visual_position, visual_orientation);
}
}
}
else
{
link->setToErrorMaterial();
}
}
}
