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 <OgreSceneNode.h>
00031 #include <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 if ( initialized() )
00095 {
00096 delete robot_;
00097 }
00098 }
00099
00100 void RobotModelDisplay::onInitialize()
00101 {
00102 robot_ = new Robot( scene_node_, context_, "Robot: " + getName().toStdString(), this );
00103
00104 updateVisualVisible();
00105 updateCollisionVisible();
00106 updateAlpha();
00107 }
00108
00109 void RobotModelDisplay::updateAlpha()
00110 {
00111 robot_->setAlpha( alpha_property_->getFloat() );
00112 context_->queueRender();
00113 }
00114
00115 void RobotModelDisplay::updateRobotDescription()
00116 {
00117 if( isEnabled() )
00118 {
00119 load();
00120 context_->queueRender();
00121 }
00122 }
00123
00124 void RobotModelDisplay::updateVisualVisible()
00125 {
00126 robot_->setVisualVisible( visual_enabled_property_->getValue().toBool() );
00127 context_->queueRender();
00128 }
00129
00130 void RobotModelDisplay::updateCollisionVisible()
00131 {
00132 robot_->setCollisionVisible( collision_enabled_property_->getValue().toBool() );
00133 context_->queueRender();
00134 }
00135
00136 void RobotModelDisplay::updateTfPrefix()
00137 {
00138 clearStatuses();
00139 context_->queueRender();
00140 }
00141
00142 void RobotModelDisplay::load()
00143 {
00144 std::string content;
00145 if( !update_nh_.getParam( robot_description_property_->getStdString(), content ))
00146 {
00147 std::string loc;
00148 if( update_nh_.searchParam( robot_description_property_->getStdString(), loc ))
00149 {
00150 update_nh_.getParam( loc, content );
00151 }
00152 else
00153 {
00154 clear();
00155 setStatus( StatusProperty::Error, "URDF",
00156 "Parameter [" + robot_description_property_->getString()
00157 + "] does not exist, and was not found by searchParam()" );
00158 return;
00159 }
00160 }
00161
00162 if( content.empty() )
00163 {
00164 clear();
00165 setStatus( StatusProperty::Error, "URDF", "URDF is empty" );
00166 return;
00167 }
00168
00169 if( content == robot_description_ )
00170 {
00171 return;
00172 }
00173
00174 robot_description_ = content;
00175
00176 TiXmlDocument doc;
00177 doc.Parse( robot_description_.c_str() );
00178 if( !doc.RootElement() )
00179 {
00180 clear();
00181 setStatus( StatusProperty::Error, "URDF", "URDF failed XML parse" );
00182 return;
00183 }
00184
00185 urdf::Model descr;
00186 if( !descr.initXml( doc.RootElement() ))
00187 {
00188 clear();
00189 setStatus( StatusProperty::Error, "URDF", "URDF failed Model parse" );
00190 return;
00191 }
00192
00193 setStatus( StatusProperty::Ok, "URDF", "URDF parsed OK" );
00194 robot_->load( descr );
00195 robot_->update( TFLinkUpdater( context_->getFrameManager(),
00196 boost::bind( linkUpdaterStatusFunction, _1, _2, _3, this ),
00197 tf_prefix_property_->getStdString() ));
00198 }
00199
00200 void RobotModelDisplay::onEnable()
00201 {
00202 load();
00203 robot_->setVisible( true );
00204 }
00205
00206 void RobotModelDisplay::onDisable()
00207 {
00208 robot_->setVisible( false );
00209 clear();
00210 }
00211
00212 void RobotModelDisplay::update( float wall_dt, float ros_dt )
00213 {
00214 time_since_last_transform_ += wall_dt;
00215 float rate = update_rate_property_->getFloat();
00216 bool update = rate < 0.0001f || time_since_last_transform_ >= rate;
00217
00218 if( has_new_transforms_ || update )
00219 {
00220 robot_->update( TFLinkUpdater( context_->getFrameManager(),
00221 boost::bind( linkUpdaterStatusFunction, _1, _2, _3, this ),
00222 tf_prefix_property_->getStdString() ));
00223 context_->queueRender();
00224
00225 has_new_transforms_ = false;
00226 time_since_last_transform_ = 0.0f;
00227 }
00228 }
00229
00230 void RobotModelDisplay::fixedFrameChanged()
00231 {
00232 has_new_transforms_ = true;
00233 }
00234
00235 void RobotModelDisplay::clear()
00236 {
00237 robot_->clear();
00238 clearStatuses();
00239 robot_description_.clear();
00240 }
00241
00242 void RobotModelDisplay::reset()
00243 {
00244 Display::reset();
00245 has_new_transforms_ = true;
00246 }
00247
00248 }
00249
00250 #include <pluginlib/class_list_macros.h>
00251 PLUGINLIB_EXPORT_CLASS( rviz::RobotModelDisplay, rviz::Display )