robot_model_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 } // namespace rviz
00249 
00250 #include <pluginlib/class_list_macros.h>
00251 PLUGINLIB_EXPORT_CLASS( rviz::RobotModelDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:16