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 <OGRE/OgreSceneNode.h>
00031 #include <OGRE/OgreSceneManager.h>
00032
00033 #include <urdf/model.h>
00034
00035 #include <tf/transform_listener.h>
00036
00037 #include "rviz/display_context.h"
00038 #include "rviz/robot/robot.h"
00039 #include "rviz/robot/tf_link_updater.h"
00040 #include "rviz/properties/float_property.h"
00041 #include "rviz/properties/property.h"
00042 #include "rviz/properties/string_property.h"
00043
00044 #include "robot_model_display.h"
00045
00046 namespace rviz
00047 {
00048
00049 void linkUpdaterStatusFunction( StatusProperty::Level level,
00050 const std::string& link_name,
00051 const std::string& text,
00052 RobotModelDisplay* display )
00053 {
00054 display->setStatus( level, QString::fromStdString( link_name ), QString::fromStdString( text ));
00055 }
00056
00057 RobotModelDisplay::RobotModelDisplay()
00058 : Display()
00059 , has_new_transforms_( false )
00060 , time_since_last_transform_( 0.0f )
00061 {
00062 visual_enabled_property_ = new Property( "Visual Enabled", true,
00063 "Whether to display the visual representation of the robot.",
00064 this, SLOT( updateVisualVisible() ));
00065
00066 collision_enabled_property_ = new Property( "Collision Enabled", false,
00067 "Whether to display the collision representation of the robot.",
00068 this, SLOT( updateCollisionVisible() ));
00069
00070 update_rate_property_ = new FloatProperty( "Update Interval", 0,
00071 "Interval at which to update the links, in seconds. "
00072 " 0 means to update every update cycle.",
00073 this );
00074 update_rate_property_->setMin( 0 );
00075
00076 alpha_property_ = new FloatProperty( "Alpha", 1,
00077 "Amount of transparency to apply to the links.",
00078 this, SLOT( updateAlpha() ));
00079 alpha_property_->setMin( 0.0 );
00080 alpha_property_->setMax( 1.0 );
00081
00082 robot_description_property_ = new StringProperty( "Robot Description", "robot_description",
00083 "Name of the parameter to search for to load the robot description.",
00084 this, SLOT( updateRobotDescription() ));
00085
00086 tf_prefix_property_ = new StringProperty( "TF Prefix", "",
00087 "Robot Model normally assumes the link name is the same as the tf frame name. "
00088 " This option allows you to set a prefix. Mainly useful for multi-robot situations.",
00089 this, SLOT( updateTfPrefix() ));
00090 }
00091
00092 RobotModelDisplay::~RobotModelDisplay()
00093 {
00094 delete robot_;
00095 }
00096
00097 void RobotModelDisplay::onInitialize()
00098 {
00099 robot_ = new Robot( context_, "Robot: " + getName().toStdString(), this );
00100
00101 updateVisualVisible();
00102 updateCollisionVisible();
00103 updateAlpha();
00104 }
00105
00106 void RobotModelDisplay::updateAlpha()
00107 {
00108 robot_->setAlpha( alpha_property_->getFloat() );
00109 context_->queueRender();
00110 }
00111
00112 void RobotModelDisplay::updateRobotDescription()
00113 {
00114 if( isEnabled() )
00115 {
00116 load();
00117 context_->queueRender();
00118 }
00119 }
00120
00121 void RobotModelDisplay::updateVisualVisible()
00122 {
00123 robot_->setVisualVisible( visual_enabled_property_->getValue().toBool() );
00124 context_->queueRender();
00125 }
00126
00127 void RobotModelDisplay::updateCollisionVisible()
00128 {
00129 robot_->setCollisionVisible( collision_enabled_property_->getValue().toBool() );
00130 context_->queueRender();
00131 }
00132
00133 void RobotModelDisplay::updateTfPrefix()
00134 {
00135 context_->queueRender();
00136 }
00137
00138 void RobotModelDisplay::load()
00139 {
00140 std::string content;
00141 if( !update_nh_.getParam( robot_description_property_->getStdString(), content ))
00142 {
00143 std::string loc;
00144 if( update_nh_.searchParam( robot_description_property_->getStdString(), loc ))
00145 {
00146 update_nh_.getParam( loc, content );
00147 }
00148 else
00149 {
00150 clear();
00151 setStatus( StatusProperty::Error, "URDF",
00152 "Parameter [" + robot_description_property_->getString()
00153 + "] does not exist, and was not found by searchParam()" );
00154 return;
00155 }
00156 }
00157
00158 if( content.empty() )
00159 {
00160 clear();
00161 setStatus( StatusProperty::Error, "URDF", "URDF is empty" );
00162 return;
00163 }
00164
00165 if( content == robot_description_ )
00166 {
00167 return;
00168 }
00169
00170 robot_description_ = content;
00171
00172 TiXmlDocument doc;
00173 doc.Parse( robot_description_.c_str() );
00174 if( !doc.RootElement() )
00175 {
00176 clear();
00177 setStatus( StatusProperty::Error, "URDF", "URDF failed XML parse" );
00178 return;
00179 }
00180
00181 urdf::Model descr;
00182 if( !descr.initXml( doc.RootElement() ))
00183 {
00184 clear();
00185 setStatus( StatusProperty::Error, "URDF", "URDF failed Model parse" );
00186 return;
00187 }
00188
00189 setStatus( StatusProperty::Ok, "URDF", "URDF parsed OK" );
00190 robot_->load( doc.RootElement(), descr );
00191 robot_->update( TFLinkUpdater( context_->getFrameManager(),
00192 boost::bind( linkUpdaterStatusFunction, _1, _2, _3, this ),
00193 tf_prefix_property_->getStdString() ));
00194 }
00195
00196 void RobotModelDisplay::onEnable()
00197 {
00198 load();
00199 robot_->setVisible( true );
00200 }
00201
00202 void RobotModelDisplay::onDisable()
00203 {
00204 robot_->setVisible( false );
00205 clear();
00206 }
00207
00208 void RobotModelDisplay::update( float wall_dt, float ros_dt )
00209 {
00210 time_since_last_transform_ += wall_dt;
00211 float rate = update_rate_property_->getFloat();
00212 bool update = rate < 0.0001f || time_since_last_transform_ >= rate;
00213
00214 if( has_new_transforms_ || update )
00215 {
00216 robot_->update( TFLinkUpdater( context_->getFrameManager(),
00217 boost::bind( linkUpdaterStatusFunction, _1, _2, _3, this ),
00218 tf_prefix_property_->getStdString() ));
00219 context_->queueRender();
00220
00221 has_new_transforms_ = false;
00222 time_since_last_transform_ = 0.0f;
00223 }
00224 }
00225
00226 void RobotModelDisplay::fixedFrameChanged()
00227 {
00228 has_new_transforms_ = true;
00229 }
00230
00231 void RobotModelDisplay::clear()
00232 {
00233 robot_->clear();
00234 clearStatuses();
00235 robot_description_.clear();
00236 }
00237
00238 void RobotModelDisplay::reset()
00239 {
00240 Display::reset();
00241 has_new_transforms_ = true;
00242 }
00243
00244 }
00245
00246 #include <pluginlib/class_list_macros.h>
00247 PLUGINLIB_DECLARE_CLASS( rviz, RobotModel, rviz::RobotModelDisplay, rviz::Display )