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