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 "properties/property_manager.h"
00034 #include "visualization_manager.h"
00035
00036 #include "ogre_tools/object.h"
00037 #include "ogre_tools/shape.h"
00038 #include "ogre_tools/axes.h"
00039
00040 #include <urdf/model.h>
00041
00042 #include <OGRE/OgreSceneNode.h>
00043 #include <OGRE/OgreSceneManager.h>
00044 #include <OGRE/OgreRibbonTrail.h>
00045 #include <OGRE/OgreEntity.h>
00046 #include <OGRE/OgreMaterialManager.h>
00047 #include <OGRE/OgreMaterial.h>
00048 #include <OGRE/OgreResourceGroupManager.h>
00049
00050 #include <ros/console.h>
00051
00052 namespace rviz
00053 {
00054
00055 Robot::Robot( VisualizationManager* manager, const std::string& name )
00056 : scene_manager_(manager->getSceneManager())
00057 , visual_visible_( true )
00058 , collision_visible_( false )
00059 , vis_manager_(manager)
00060 , property_manager_(NULL)
00061 , name_( name )
00062 {
00063 root_visual_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00064 root_collision_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00065 root_other_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00066
00067 setVisualVisible( visual_visible_ );
00068 setCollisionVisible( collision_visible_ );
00069 setAlpha(1.0f);
00070 }
00071
00072 Robot::~Robot()
00073 {
00074 clear();
00075
00076 scene_manager_->destroySceneNode( root_visual_node_->getName() );
00077 scene_manager_->destroySceneNode( root_collision_node_->getName() );
00078 scene_manager_->destroySceneNode( root_other_node_->getName() );
00079 }
00080
00081 void Robot::setVisible( bool visible )
00082 {
00083 if ( visible )
00084 {
00085 root_visual_node_->setVisible( visual_visible_ );
00086 root_collision_node_->setVisible( collision_visible_ );
00087 }
00088 else
00089 {
00090 root_visual_node_->setVisible( false );
00091 root_collision_node_->setVisible( false );
00092 }
00093 }
00094
00095 void Robot::setVisualVisible( bool visible )
00096 {
00097 visual_visible_ = visible;
00098 root_visual_node_->setVisible( visible );
00099 }
00100
00101 void Robot::setCollisionVisible( bool visible )
00102 {
00103 collision_visible_ = visible;
00104 root_collision_node_->setVisible( visible );
00105 }
00106
00107 bool Robot::isVisualVisible()
00108 {
00109 return visual_visible_;
00110 }
00111
00112 bool Robot::isCollisionVisible()
00113 {
00114 return collision_visible_;
00115 }
00116
00117 void Robot::setAlpha(float a)
00118 {
00119 alpha_ = a;
00120
00121 M_NameToLink::iterator it = links_.begin();
00122 M_NameToLink::iterator end = links_.end();
00123 for ( ; it != end; ++it )
00124 {
00125 RobotLink* info = it->second;
00126
00127 info->setAlpha(alpha_);
00128 }
00129 }
00130
00131 void Robot::clear()
00132 {
00133 if ( property_manager_ )
00134 {
00135 property_manager_->deleteByUserData( this );
00136 }
00137
00138 M_NameToLink::iterator link_it = links_.begin();
00139 M_NameToLink::iterator link_end = links_.end();
00140 for ( ; link_it != link_end; ++link_it )
00141 {
00142 RobotLink* info = link_it->second;
00143 delete info;
00144 }
00145
00146 links_category_.reset();
00147 links_.clear();
00148 root_visual_node_->removeAndDestroyAllChildren();
00149 root_collision_node_->removeAndDestroyAllChildren();
00150 }
00151
00152 void Robot::setPropertyManager( PropertyManager* property_manager, const CategoryPropertyWPtr& parent )
00153 {
00154 ROS_ASSERT( property_manager );
00155 ROS_ASSERT( parent.lock() );
00156
00157 property_manager_ = property_manager;
00158 parent_property_ = parent;
00159
00160 links_category_ = property_manager_->createCategory( "Links", name_, parent_property_, this );
00161
00162 if ( !links_.empty() )
00163 {
00164 M_NameToLink::iterator link_it = links_.begin();
00165 M_NameToLink::iterator link_end = links_.end();
00166 for ( ; link_it != link_end; ++link_it )
00167 {
00168 RobotLink* info = link_it->second;
00169
00170 info->setPropertyManager(property_manager);
00171 info->createProperties();
00172 }
00173 }
00174
00175 CategoryPropertyPtr cat_prop = links_category_.lock();
00176 cat_prop->collapse();
00177 }
00178
00179 class LinkComparator
00180 {
00181 public:
00182 bool operator()(const boost::shared_ptr<urdf::Link>& lhs, const boost::shared_ptr<urdf::Link>& rhs)
00183 {
00184 return lhs->name < rhs->name;
00185 }
00186 };
00187
00188 void Robot::load( TiXmlElement* root_element, urdf::Model &descr, bool visual, bool collision )
00189 {
00190 clear();
00191
00192 if ( property_manager_ )
00193 {
00194 ROS_ASSERT(!links_category_.lock());
00195 links_category_ = property_manager_->createCategory( "Links", name_, parent_property_, this );
00196 }
00197
00198 typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
00199 V_Link links;
00200 descr.getLinks(links);
00201 std::sort(links.begin(), links.end(), LinkComparator());
00202 V_Link::iterator it = links.begin();
00203 V_Link::iterator end = links.end();
00204 for (; it != end; ++it)
00205 {
00206 const boost::shared_ptr<urdf::Link>& link = *it;
00207
00208 RobotLink* link_info = new RobotLink(this, vis_manager_);
00209
00210 if (property_manager_)
00211 {
00212 link_info->setPropertyManager(property_manager_);
00213 }
00214 link_info->load(root_element, descr, link, visual, collision);
00215
00216 if (!link_info->isValid())
00217 {
00218 delete link_info;
00219 continue;
00220 }
00221
00222 bool inserted = links_.insert( std::make_pair( link_info->getName(), link_info ) ).second;
00223 ROS_ASSERT( inserted );
00224
00225 link_info->setAlpha(alpha_);
00226 }
00227
00228 if (property_manager_)
00229 {
00230 CategoryPropertyPtr cat_prop = links_category_.lock();
00231 cat_prop->collapse();
00232 }
00233
00234 setVisualVisible(isVisualVisible());
00235 setCollisionVisible(isCollisionVisible());
00236 }
00237
00238 RobotLink* Robot::getLink( const std::string& name )
00239 {
00240 M_NameToLink::iterator it = links_.find( name );
00241 if ( it == links_.end() )
00242 {
00243 ROS_WARN( "Link [%s] does not exist", name.c_str() );
00244 return NULL;
00245 }
00246
00247 return it->second;
00248 }
00249
00250 void Robot::update(const LinkUpdater& updater)
00251 {
00252 M_NameToLink::iterator link_it = links_.begin();
00253 M_NameToLink::iterator link_end = links_.end();
00254 for ( ; link_it != link_end; ++link_it )
00255 {
00256 RobotLink* info = link_it->second;
00257
00258 info->setToNormalMaterial();
00259
00260 Ogre::Vector3 visual_position, collision_position;
00261 Ogre::Quaternion visual_orientation, collision_orientation;
00262 bool apply_offset_transforms;
00263 if (updater.getLinkTransforms(info->getName(), visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms))
00264 {
00265 info->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms );
00266 }
00267 else
00268 {
00269 info->setToErrorMaterial();
00270 }
00271 }
00272 }
00273
00274 void Robot::setPosition( const Ogre::Vector3& position )
00275 {
00276 root_visual_node_->setPosition( position );
00277 root_collision_node_->setPosition( position );
00278 }
00279
00280 void Robot::setOrientation( const Ogre::Quaternion& orientation )
00281 {
00282 root_visual_node_->setOrientation( orientation );
00283 root_collision_node_->setOrientation( orientation );
00284 }
00285
00286 void Robot::setScale( const Ogre::Vector3& scale )
00287 {
00288 root_visual_node_->setScale( scale );
00289 root_collision_node_->setScale( scale );
00290 }
00291
00292 const Ogre::Vector3& Robot::getPosition()
00293 {
00294 return root_visual_node_->getPosition();
00295 }
00296
00297 const Ogre::Quaternion& Robot::getOrientation()
00298 {
00299 return root_visual_node_->getOrientation();
00300 }
00301
00302 }
