PlanningDisplay::PlanningDisplay( const std::string& name, VisualizationManager* manager )
: Display( name, manager )
, kinematic_model_( NULL )
, new_kinematic_path_( false )
, animating_path_( false )
, state_display_time_( 0.05f )
, visual_enabled_property_( NULL )
, collision_enabled_property_( NULL )
, state_display_time_property_( NULL )
, robot_description_property_( NULL )
, topic_property_( NULL )
{
robot_ = new Robot( scene_manager_ );
setVisualVisible( false );
setCollisionVisible( true );
robot_->setUserData( Ogre::Any( (void*)this ) );
}
RobotModelDisplay::RobotModelDisplay( const std::string& name, VisualizationManager* manager )
: Display( name, manager )
, mechanism_topic_( "mechanism_state" )
, has_new_transforms_( false )
, has_new_mechanism_state_( false )
, time_since_last_transform_( 0.0f )
, update_rate_( 0.1f )
, visual_enabled_property_( NULL )
, collision_enabled_property_( NULL )
, update_rate_property_( NULL )
, robot_description_property_( NULL )
, mechanism_topic_property_( NULL )
{
robot_ = new Robot( scene_manager_, "Robot: " + name_ );
setVisualVisible( true );
setCollisionVisible( false );
robot_->setUserData( Ogre::Any( (void*)this ) );
urdf_ = new robot_desc::URDF();
}
Robot::Robot( Ogre::SceneNode* root_node, DisplayContext* context, const std::string& name, Property* parent_property )
: scene_manager_( context->getSceneManager() )
, visible_( true )
, visual_visible_( true )
, collision_visible_( false )
, context_( context )
, name_( name )
, doing_set_checkbox_( false )
, robot_loaded_( false )
, inChangedEnableAllLinks( false )
{
root_visual_node_ = root_node->createChildSceneNode();
root_collision_node_ = root_node->createChildSceneNode();
root_other_node_ = root_node->createChildSceneNode();
link_factory_ = new LinkFactory();
setVisualVisible( visual_visible_ );
setCollisionVisible( collision_visible_ );
setAlpha(1.0f);
link_tree_ = new Property( "Links", QVariant(), "", parent_property );
link_tree_->hide(); // hide until loaded
link_tree_style_ = new EnumProperty(
"Link Tree Style",
"",
"How the list of links is displayed",
link_tree_,
SLOT( changedLinkTreeStyle() ),
this );
initLinkTreeStyle();
expand_tree_= new BoolProperty(
"Expand Tree",
false,
"Expand or collapse link tree",
link_tree_,
SLOT( changedExpandTree() ),
this );
expand_link_details_ = new BoolProperty(
"Expand Link Details",
false,
"Expand link details (sub properties) to see all info for all links.",
link_tree_,
SLOT( changedExpandLinkDetails() ),
this );
expand_joint_details_ = new BoolProperty(
"Expand Joint Details",
false,
"Expand joint details (sub properties) to see all info for all joints.",
link_tree_,
SLOT( changedExpandJointDetails() ),
this );
enable_all_links_ = new BoolProperty(
"All Links Enabled",
true,
"Turn all links on or off.",
link_tree_,
SLOT( changedEnableAllLinks() ),
this );
}
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() );
}
