robot_model_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <OgreSceneNode.h>
31 #include <OgreSceneManager.h>
32 
33 #include <tinyxml.h>
34 #include <urdf/model.h>
35 
36 #include <tf/transform_listener.h>
37 
38 #include "rviz/display_context.h"
39 #include "rviz/robot/robot.h"
44 
45 #include "robot_model_display.h"
46 
47 namespace rviz
48 {
49 
51  const std::string& link_name,
52  const std::string& text,
53  RobotModelDisplay* display )
54 {
55  display->setStatus( level, QString::fromStdString( link_name ), QString::fromStdString( text ));
56 }
57 
59  : Display()
60  , has_new_transforms_( false )
61  , time_since_last_transform_( 0.0f )
62 {
63  visual_enabled_property_ = new Property( "Visual Enabled", true,
64  "Whether to display the visual representation of the robot.",
65  this, SLOT( updateVisualVisible() ));
66 
67  collision_enabled_property_ = new Property( "Collision Enabled", false,
68  "Whether to display the collision representation of the robot.",
69  this, SLOT( updateCollisionVisible() ));
70 
71  update_rate_property_ = new FloatProperty( "Update Interval", 0,
72  "Interval at which to update the links, in seconds. "
73  " 0 means to update every update cycle.",
74  this );
76 
77  alpha_property_ = new FloatProperty( "Alpha", 1,
78  "Amount of transparency to apply to the links.",
79  this, SLOT( updateAlpha() ));
80  alpha_property_->setMin( 0.0 );
81  alpha_property_->setMax( 1.0 );
82 
83  robot_description_property_ = new StringProperty( "Robot Description", "robot_description",
84  "Name of the parameter to search for to load the robot description.",
85  this, SLOT( updateRobotDescription() ));
86 
87  tf_prefix_property_ = new StringProperty( "TF Prefix", "",
88  "Robot Model normally assumes the link name is the same as the tf frame name. "
89  " This option allows you to set a prefix. Mainly useful for multi-robot situations.",
90  this, SLOT( updateTfPrefix() ));
91 }
92 
94 {
95  if ( initialized() )
96  {
97  delete robot_;
98  }
99 }
100 
102 {
103  robot_ = new Robot( scene_node_, context_, "Robot: " + getName().toStdString(), this );
104 
107  updateAlpha();
108 }
109 
111 {
114 }
115 
117 {
118  if( isEnabled() )
119  {
120  load();
122  }
123 }
124 
126 {
129 }
130 
132 {
135 }
136 
138 {
139  clearStatuses();
141 }
142 
144 {
145  clearStatuses();
146 
147  std::string content;
149  {
150  std::string loc;
152  {
153  update_nh_.getParam( loc, content );
154  }
155  else
156  {
157  clear();
159  "Parameter [" + robot_description_property_->getString()
160  + "] does not exist, and was not found by searchParam()" );
161  return;
162  }
163  }
164 
165  if( content.empty() )
166  {
167  clear();
168  setStatus( StatusProperty::Error, "URDF", "URDF is empty" );
169  return;
170  }
171 
172  if( content == robot_description_ )
173  {
174  return;
175  }
176 
177  robot_description_ = content;
178 
179  TiXmlDocument doc;
180  doc.Parse( robot_description_.c_str() );
181  if( !doc.RootElement() )
182  {
183  clear();
184  setStatus( StatusProperty::Error, "URDF", "URDF failed XML parse" );
185  return;
186  }
187 
188  urdf::Model descr;
189  if( !descr.initXml( doc.RootElement() ))
190  {
191  clear();
192  setStatus( StatusProperty::Error, "URDF", "URDF failed Model parse" );
193  return;
194  }
195 
196  setStatus( StatusProperty::Ok, "URDF", "URDF parsed OK" );
197  robot_->load( descr );
199  boost::bind( linkUpdaterStatusFunction, _1, _2, _3, this ),
201 }
202 
204 {
205  load();
206  robot_->setVisible( true );
207 }
208 
210 {
211  robot_->setVisible( false );
212  clear();
213 }
214 
215 void RobotModelDisplay::update( float wall_dt, float ros_dt )
216 {
217  time_since_last_transform_ += wall_dt;
218  float rate = update_rate_property_->getFloat();
219  bool update = rate < 0.0001f || time_since_last_transform_ >= rate;
220 
221  if( has_new_transforms_ || update )
222  {
224  boost::bind( linkUpdaterStatusFunction, _1, _2, _3, this ),
227 
228  has_new_transforms_ = false;
230  }
231 }
232 
234 {
235  has_new_transforms_ = true;
236 }
237 
239 {
240  robot_->clear();
241  clearStatuses();
242  robot_description_.clear();
243 }
244 
246 {
247  Display::reset();
248  has_new_transforms_ = true;
249 }
250 
251 } // namespace rviz
252 
virtual void update(const LinkUpdater &updater)
Definition: robot.cpp:728
void setMin(float min)
void setMax(float max)
virtual void clear()
Clears all data loaded from a URDF.
Definition: robot.cpp:207
void setAlpha(float a)
Definition: robot.cpp:193
f
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
URDF_EXPORT bool initXml(TiXmlElement *xml)
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:269
virtual float getFloat() const
virtual void onInitialize()
Override this function to do subclass-specific initialization.
Property specialized to enforce floating point max/min.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
std::string getStdString()
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:281
virtual void load(const urdf::ModelInterface &urdf, bool visual=true, bool collision=true)
Loads meshes/primitives from a robot description. Calls clear() before loading.
Definition: robot.cpp:254
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
virtual void clearStatuses()
Delete all status children. This is thread-safe.
Definition: display.cpp:224
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
void setCollisionVisible(bool visible)
Set whether the collision meshes/primitives of the robot should be visible.
Definition: robot.cpp:161
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:300
bool searchParam(const std::string &key, std::string &result) const
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
Robot * robot_
Handles actually drawing the robot.
bool has_new_transforms_
Callback sets this to tell our update function it needs to update the transforms. ...
FloatProperty * update_rate_property_
void linkUpdaterStatusFunction(StatusProperty::Level level, const std::string &link_name, const std::string &text, RobotModelDisplay *display)
Property specialized for string values.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
Constructor.
Definition: property.cpp:54
void setVisualVisible(bool visible)
Set whether the visual meshes of the robot should be visible.
Definition: robot.cpp:155
bool getParam(const std::string &key, std::string &s) const
virtual void reset()
Called to tell the display to clear its state.
virtual QVariant getValue() const
Return the value of this Property as a QVariant. If the value has never been set, an invalid QVariant...
Definition: property.cpp:145
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
Uses a robot xml description to display the pieces of a robot at the transforms broadcast by rosTF...
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:247
virtual void load()
Loads a URDF from the ros-param named by our "Robot Description" property, iterates through the links...
virtual void setVisible(bool visible)
Set the robot as a whole to be visible or not.
Definition: robot.cpp:138
StringProperty * tf_prefix_property_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
StringProperty * robot_description_property_
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:186
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:159
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42