00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "robot.h"
00031 #include "robot_link.h"
00032 #include "properties/property.h"
00033 #include "display_context.h"
00034
00035 #include "ogre_helpers/object.h"
00036 #include "ogre_helpers/shape.h"
00037 #include "ogre_helpers/axes.h"
00038
00039 #include <urdf/model.h>
00040
00041 #include <OGRE/OgreSceneNode.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 #include <OGRE/OgreEntity.h>
00044 #include <OGRE/OgreMaterialManager.h>
00045 #include <OGRE/OgreMaterial.h>
00046 #include <OGRE/OgreResourceGroupManager.h>
00047
00048 #include <ros/console.h>
00049 #include <ros/assert.h>
00050
00051 namespace rviz
00052 {
00053
00054 Robot::Robot( DisplayContext* context, const std::string& name, Property* parent_property )
00055 : scene_manager_( context->getSceneManager() )
00056 , visual_visible_( true )
00057 , collision_visible_( false )
00058 , context_( context )
00059 , name_( name )
00060 {
00061 root_visual_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00062 root_collision_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00063 root_other_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00064
00065 setVisualVisible( visual_visible_ );
00066 setCollisionVisible( collision_visible_ );
00067 setAlpha(1.0f);
00068
00069 links_category_ = new Property( "Links", QVariant(), "", parent_property );
00070 }
00071
00072
00073 Robot::~Robot()
00074 {
00075 clear();
00076
00077 scene_manager_->destroySceneNode( root_visual_node_->getName() );
00078 scene_manager_->destroySceneNode( root_collision_node_->getName() );
00079 scene_manager_->destroySceneNode( root_other_node_->getName() );
00080 }
00081
00082 void Robot::setVisible( bool visible )
00083 {
00084 if ( visible )
00085 {
00086 root_visual_node_->setVisible( visual_visible_ );
00087 root_collision_node_->setVisible( collision_visible_ );
00088 }
00089 else
00090 {
00091 root_visual_node_->setVisible( false );
00092 root_collision_node_->setVisible( false );
00093 }
00094 }
00095
00096 void Robot::setVisualVisible( bool visible )
00097 {
00098 visual_visible_ = visible;
00099 updateLinkVisibilities();
00100 }
00101
00102 void Robot::setCollisionVisible( bool visible )
00103 {
00104 collision_visible_ = visible;
00105 updateLinkVisibilities();
00106 }
00107
00108 void Robot::updateLinkVisibilities()
00109 {
00110 M_NameToLink::iterator it = links_.begin();
00111 M_NameToLink::iterator end = links_.end();
00112 for ( ; it != end; ++it )
00113 {
00114 RobotLink* link = it->second;
00115 link->updateVisibility();
00116 }
00117 }
00118
00119 bool Robot::isVisualVisible()
00120 {
00121 return visual_visible_;
00122 }
00123
00124 bool Robot::isCollisionVisible()
00125 {
00126 return collision_visible_;
00127 }
00128
00129 void Robot::setAlpha(float a)
00130 {
00131 alpha_ = a;
00132
00133 M_NameToLink::iterator it = links_.begin();
00134 M_NameToLink::iterator end = links_.end();
00135 for ( ; it != end; ++it )
00136 {
00137 RobotLink* info = it->second;
00138
00139 info->setRobotAlpha(alpha_);
00140 }
00141 }
00142
00143 void Robot::clear()
00144 {
00145 M_NameToLink::iterator link_it = links_.begin();
00146 M_NameToLink::iterator link_end = links_.end();
00147 for ( ; link_it != link_end; ++link_it )
00148 {
00149 RobotLink* info = link_it->second;
00150 delete info;
00151 }
00152
00153 links_.clear();
00154 root_visual_node_->removeAndDestroyAllChildren();
00155 root_collision_node_->removeAndDestroyAllChildren();
00156 root_other_node_->removeAndDestroyAllChildren();
00157 }
00158
00159 class LinkComparator
00160 {
00161 public:
00162 bool operator()(const boost::shared_ptr<urdf::Link>& lhs, const boost::shared_ptr<urdf::Link>& rhs)
00163 {
00164 return lhs->name < rhs->name;
00165 }
00166 };
00167
00168 void Robot::load( TiXmlElement* root_element, urdf::Model &descr, bool visual, bool collision )
00169 {
00170 clear();
00171
00172 typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
00173 V_Link links;
00174 descr.getLinks( links );
00175 std::sort( links.begin(), links.end(), LinkComparator() );
00176 V_Link::iterator it = links.begin();
00177 V_Link::iterator end = links.end();
00178 for( ; it != end; ++it )
00179 {
00180 const boost::shared_ptr<urdf::Link>& link = *it;
00181
00182 RobotLink* link_info = new RobotLink( this, context_, links_category_ );
00183
00184 link_info->load( root_element, descr, link, visual, collision );
00185
00186 if( !link_info->isValid() )
00187 {
00188 delete link_info;
00189 continue;
00190 }
00191
00192 bool inserted = links_.insert( std::make_pair( link_info->getName(), link_info ) ).second;
00193 ROS_ASSERT( inserted );
00194
00195 link_info->setRobotAlpha( alpha_ );
00196 }
00197
00198 links_category_->collapse();
00199
00200 setVisualVisible( isVisualVisible() );
00201 setCollisionVisible( isCollisionVisible() );
00202 }
00203
00204 RobotLink* Robot::getLink( const std::string& name )
00205 {
00206 M_NameToLink::iterator it = links_.find( name );
00207 if ( it == links_.end() )
00208 {
00209 ROS_WARN( "Link [%s] does not exist", name.c_str() );
00210 return NULL;
00211 }
00212
00213 return it->second;
00214 }
00215
00216 void Robot::update(const LinkUpdater& updater)
00217 {
00218 M_NameToLink::iterator link_it = links_.begin();
00219 M_NameToLink::iterator link_end = links_.end();
00220 for ( ; link_it != link_end; ++link_it )
00221 {
00222 RobotLink* info = link_it->second;
00223
00224 info->setToNormalMaterial();
00225
00226 Ogre::Vector3 visual_position, collision_position;
00227 Ogre::Quaternion visual_orientation, collision_orientation;
00228 bool apply_offset_transforms;
00229 if( updater.getLinkTransforms( info->getName(),
00230 visual_position, visual_orientation,
00231 collision_position, collision_orientation,
00232 apply_offset_transforms ))
00233 {
00234 info->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms );
00235 }
00236 else
00237 {
00238 info->setToErrorMaterial();
00239 }
00240 }
00241 }
00242
00243 void Robot::setPosition( const Ogre::Vector3& position )
00244 {
00245 root_visual_node_->setPosition( position );
00246 root_collision_node_->setPosition( position );
00247 }
00248
00249 void Robot::setOrientation( const Ogre::Quaternion& orientation )
00250 {
00251 root_visual_node_->setOrientation( orientation );
00252 root_collision_node_->setOrientation( orientation );
00253 }
00254
00255 void Robot::setScale( const Ogre::Vector3& scale )
00256 {
00257 root_visual_node_->setScale( scale );
00258 root_collision_node_->setScale( scale );
00259 }
00260
00261 const Ogre::Vector3& Robot::getPosition()
00262 {
00263 return root_visual_node_->getPosition();
00264 }
00265
00266 const Ogre::Quaternion& Robot::getOrientation()
00267 {
00268 return root_visual_node_->getOrientation();
00269 }
00270
00271 }