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_model_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/robot/robot.h"
00033 #include "rviz/robot/tf_link_updater.h"
00034 #include "rviz/properties/property.h"
00035 #include "rviz/properties/property_manager.h"
00036
00037 #include <urdf/model.h>
00038
00039 #include <OGRE/OgreSceneNode.h>
00040 #include <OGRE/OgreSceneManager.h>
00041
00042 #include <tf/transform_listener.h>
00043
00044 namespace rviz
00045 {
00046
00047 void linkUpdaterStatusFunction(StatusLevel level, const std::string& link_name, const std::string& text, RobotModelDisplay* display)
00048 {
00049 display->setStatus(level, link_name, text);
00050 }
00051
00052 RobotModelDisplay::RobotModelDisplay()
00053 : Display()
00054 , description_param_("robot_description")
00055 , has_new_transforms_( false )
00056 , time_since_last_transform_( 0.0f )
00057 , update_rate_( 0.0f )
00058 , hidden_(false)
00059 {
00060 }
00061
00062 RobotModelDisplay::~RobotModelDisplay()
00063 {
00064 delete robot_;
00065 }
00066
00067 void RobotModelDisplay::onInitialize()
00068 {
00069 robot_ = new Robot( vis_manager_, "Robot: " + name_ );
00070
00071 setVisualVisible( true );
00072 setCollisionVisible( false );
00073
00074 setAlpha(1.0f);
00075 }
00076
00077 void RobotModelDisplay::setAlpha( float alpha )
00078 {
00079 alpha_ = alpha;
00080
00081 robot_->setAlpha(alpha_);
00082
00083 propertyChanged(alpha_property_);
00084 }
00085
00086 void RobotModelDisplay::setRobotDescription( const std::string& description_param )
00087 {
00088 description_param_ = description_param;
00089
00090 propertyChanged(robot_description_property_);
00091
00092 if ( isEnabled() )
00093 {
00094 load();
00095 causeRender();
00096 }
00097 }
00098
00099 void RobotModelDisplay::hideVisible()
00100 {
00101 hidden_ = true;
00102 robot_->setVisible( enabled_ && !hidden_ );
00103 }
00104
00105 void RobotModelDisplay::restoreVisible()
00106 {
00107 hidden_ = false;
00108 robot_->setVisible( enabled_ && !hidden_ );
00109 }
00110
00111 void RobotModelDisplay::setVisualVisible( bool visible )
00112 {
00113 robot_->setVisualVisible( visible && !hidden_ );
00114
00115 propertyChanged(visual_enabled_property_);
00116
00117 causeRender();
00118 }
00119
00120 void RobotModelDisplay::setCollisionVisible( bool visible )
00121 {
00122 robot_->setCollisionVisible( visible && !hidden_ );
00123
00124 propertyChanged(collision_enabled_property_);
00125
00126 causeRender();
00127 }
00128
00129 void RobotModelDisplay::setUpdateRate( float rate )
00130 {
00131 update_rate_ = rate;
00132
00133 propertyChanged(update_rate_property_);
00134
00135 causeRender();
00136 }
00137
00138 void RobotModelDisplay::setTFPrefix(const std::string& prefix)
00139 {
00140 clearStatuses();
00141
00142 tf_prefix_ = prefix;
00143
00144 propertyChanged(tf_prefix_property_);
00145
00146 causeRender();
00147 }
00148
00149 bool RobotModelDisplay::isVisualVisible()
00150 {
00151 return robot_->isVisualVisible();
00152 }
00153
00154 bool RobotModelDisplay::isCollisionVisible()
00155 {
00156 return robot_->isCollisionVisible();
00157 }
00158
00159 void RobotModelDisplay::load()
00160 {
00161 std::string content;
00162 if (!update_nh_.getParam(description_param_, content))
00163 {
00164 std::string loc;
00165 if (update_nh_.searchParam(description_param_, loc))
00166 {
00167 update_nh_.getParam(loc, content);
00168 }
00169 else
00170 {
00171 clear();
00172
00173 std::stringstream ss;
00174 ss << "Parameter [" << description_param_ << "] does not exist, and was not found by searchParam()";
00175 setStatus(status_levels::Error, "URDF", ss.str());
00176 return;
00177 }
00178 }
00179
00180 if (content.empty())
00181 {
00182 clear();
00183 setStatus(status_levels::Error, "URDF", "URDF is empty");
00184 return;
00185 }
00186
00187 if ( content == robot_description_ )
00188 {
00189 return;
00190 }
00191
00192 robot_description_ = content;
00193
00194 TiXmlDocument doc;
00195 doc.Parse(robot_description_.c_str());
00196 if (!doc.RootElement())
00197 {
00198 clear();
00199 setStatus(status_levels::Error, "URDF", "URDF failed XML parse");
00200 return;
00201 }
00202
00203 urdf::Model descr;
00204 if (!descr.initXml(doc.RootElement()))
00205 {
00206 clear();
00207 setStatus(status_levels::Error, "URDF", "URDF failed Model parse");
00208 return;
00209 }
00210
00211 setStatus(status_levels::Ok, "URDF", "URDF parsed OK");
00212 robot_->load( doc.RootElement(), descr );
00213 robot_->update( TFLinkUpdater(vis_manager_->getFrameManager(), boost::bind(linkUpdaterStatusFunction, _1, _2, _3, this), tf_prefix_) );
00214 }
00215
00216 void RobotModelDisplay::onEnable()
00217 {
00218 load();
00219 robot_->setVisible( enabled_ && !hidden_ );
00220 }
00221
00222 void RobotModelDisplay::onDisable()
00223 {
00224 robot_->setVisible( enabled_ && !hidden_ );
00225 clear();
00226 }
00227
00228 void RobotModelDisplay::update(float wall_dt, float ros_dt)
00229 {
00230 time_since_last_transform_ += wall_dt;
00231
00232 bool update = update_rate_ < 0.0001f || time_since_last_transform_ >= update_rate_;
00233
00234 if ( has_new_transforms_ || update )
00235 {
00236 robot_->update(TFLinkUpdater(vis_manager_->getFrameManager(), boost::bind(linkUpdaterStatusFunction, _1, _2, _3, this), tf_prefix_));
00237 causeRender();
00238
00239 has_new_transforms_ = false;
00240 time_since_last_transform_ = 0.0f;
00241 }
00242 }
00243
00244 void RobotModelDisplay::fixedFrameChanged()
00245 {
00246 has_new_transforms_ = true;
00247 }
00248
00249 void RobotModelDisplay::createProperties()
00250 {
00251 visual_enabled_property_ = property_manager_->createProperty<BoolProperty>( "Visual Enabled", property_prefix_, boost::bind( &RobotModelDisplay::isVisualVisible, this ),
00252 boost::bind( &RobotModelDisplay::setVisualVisible, this, _1 ), parent_category_, this );
00253 setPropertyHelpText(visual_enabled_property_, "Whether to display the visual representation of the robot.");
00254 collision_enabled_property_ = property_manager_->createProperty<BoolProperty>( "Collision Enabled", property_prefix_, boost::bind( &RobotModelDisplay::isCollisionVisible, this ),
00255 boost::bind( &RobotModelDisplay::setCollisionVisible, this, _1 ), parent_category_, this );
00256 setPropertyHelpText(collision_enabled_property_, "Whether to display the collision representation of the robot.");
00257 update_rate_property_ = property_manager_->createProperty<FloatProperty>( "Update Interval", property_prefix_, boost::bind( &RobotModelDisplay::getUpdateRate, this ),
00258 boost::bind( &RobotModelDisplay::setUpdateRate, this, _1 ), parent_category_, this );
00259 setPropertyHelpText(update_rate_property_, "Interval at which to update the links, in seconds. 0 means to update every update cycle.");
00260 FloatPropertyPtr float_prop = update_rate_property_.lock();
00261 float_prop->setMin( 0.0 );
00262 float_prop->addLegacyName("Update Rate");
00263
00264 alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &RobotModelDisplay::getAlpha, this ),
00265 boost::bind( &RobotModelDisplay::setAlpha, this, _1 ), parent_category_, this );
00266 setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the links.");
00267 float_prop = alpha_property_.lock();
00268 float_prop->setMin( 0.0 );
00269 float_prop->setMax( 1.0 );
00270
00271 robot_description_property_ = property_manager_->createProperty<StringProperty>( "Robot Description", property_prefix_, boost::bind( &RobotModelDisplay::getRobotDescription, this ),
00272 boost::bind( &RobotModelDisplay::setRobotDescription, this, _1 ), parent_category_, this );
00273 setPropertyHelpText(robot_description_property_, "Name of the parameter to search for to load the robot description.");
00274
00275 tf_prefix_property_ = property_manager_->createProperty<StringProperty>( "TF Prefix", property_prefix_, boost::bind( &RobotModelDisplay::getTFPrefix, this ),
00276 boost::bind( &RobotModelDisplay::setTFPrefix, this, _1 ), parent_category_, this );
00277 setPropertyHelpText(tf_prefix_property_, "Robot Model normally assumes the link name is the same as the tf frame name. This option allows you to set a prefix. Mainly useful for multi-robot situations.");
00278
00279 robot_->setPropertyManager( property_manager_, parent_category_ );
00280 }
00281
00282 void RobotModelDisplay::clear()
00283 {
00284 robot_->clear();
00285 clearStatuses();
00286 robot_description_.clear();
00287 }
00288
00289 void RobotModelDisplay::reset()
00290 {
00291 Display::reset();
00292 has_new_transforms_ = true;
00293 }
00294
00295 }
00296