void Robot::updateLinkVisibilities()
{
M_NameToLink::iterator it = links_.begin();
M_NameToLink::iterator end = links_.end();
for ( ; it != end; ++it )
{
RobotLink* link = it->second;
link->updateVisibility();
}
}
void Robot::setAlpha(float a)
{
alpha_ = a;
M_NameToLink::iterator it = links_.begin();
M_NameToLink::iterator end = links_.end();
for ( ; it != end; ++it )
{
RobotLink* link = it->second;
link->setRobotAlpha(alpha_);
}
}
void Robot::calculateJointCheckboxes()
{
if (inChangedEnableAllLinks || !robot_loaded_)
return;
int links_with_geom_checked = 0;
int links_with_geom_unchecked = 0;
// check root link
RobotLink *link = root_link_;
if (link && link->hasGeometry())
{
bool checked = link->getLinkProperty()->getValue().toBool();
links_with_geom_checked += checked ? 1 : 0;
links_with_geom_unchecked += checked ? 0 : 1;
}
int links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
// check all child links and joints recursively
std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
{
RobotJoint* child_joint = getJoint( *child_joint_it );
if (child_joint)
{
int child_links_with_geom;
int child_links_with_geom_checked;
int child_links_with_geom_unchecked;
child_joint->calculateJointCheckboxesRecursive(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked);
links_with_geom_checked += child_links_with_geom_checked;
links_with_geom_unchecked += child_links_with_geom_unchecked;
}
}
links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
if (!links_with_geom)
{
setEnableAllLinksCheckbox(QVariant());
}
else
{
setEnableAllLinksCheckbox(links_with_geom_unchecked == 0);
}
}
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();
}
}
}
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
{
link_tree_->hide(); // hide until loaded
robot_loaded_ = false;
// clear out any data (properties, shapes, etc) from a previously loaded robot.
clear();
// the root link is discovered below. Set to NULL until found.
root_link_ = NULL;
// Create properties for each link.
// Properties are not added to display until changedLinkTreeStyle() is called (below).
{
typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
for( ; link_it != link_end; ++link_it )
{
const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
std::string parent_joint_name;
if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
{
parent_joint_name = urdf_link->parent_joint->name;
}
RobotLink* link = link_factory_->createLink( this,
urdf_link,
parent_joint_name,
visual,
collision );
if (urdf_link == urdf.getRoot())
{
root_link_ = link;
}
links_[urdf_link->name] = link;
link->setRobotAlpha( alpha_ );
}
}
// Create properties for each joint.
// Properties are not added to display until changedLinkTreeStyle() is called (below).
{
typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
for( ; joint_it != joint_end; ++joint_it )
{
const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );
joints_[urdf_joint->name] = joint;
joint->setRobotAlpha( alpha_ );
}
}
// robot is now loaded
robot_loaded_ = true;
link_tree_->show();
// set the link tree style and add link/joint properties to rviz pane.
setLinkTreeStyle(LinkTreeStyle(link_tree_style_->getOptionInt()));
changedLinkTreeStyle();
// at startup the link tree is collapsed since it is large and not often needed.
link_tree_->collapse();
setVisualVisible( isVisualVisible() );
setCollisionVisible( isCollisionVisible() );
}
