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 #include <QTimer>
33 
34 #include <urdf/model.h>
35 
36 #include "rviz/display_context.h"
37 #include "rviz/robot/robot.h"
38 #include "rviz/robot/robot_link.h"
43 
44 #include "robot_model_display.h"
45 
46 namespace rviz
47 {
49  const std::string& link_name,
50  const std::string& text,
51  RobotModelDisplay* display)
52 {
53  display->setStatus(level, QString::fromStdString(link_name), QString::fromStdString(text));
54 }
55 
57  : Display(), has_new_transforms_(false), time_since_last_transform_(0.0f)
58 {
60  new Property("Visual Enabled", true, "Whether to display the visual representation of the robot.",
61  this, SLOT(updateVisualVisible()));
62 
64  new Property("Collision Enabled", false,
65  "Whether to display the collision representation of the robot.", this,
66  SLOT(updateCollisionVisible()));
67 
68  update_rate_property_ = new FloatProperty("Update Interval", 0,
69  "Interval at which to update the links, in seconds. "
70  "0 means to update every update cycle.",
71  this);
73 
74  alpha_property_ = new FloatProperty("Alpha", 1, "Amount of transparency to apply to the links.", this,
75  SLOT(updateAlpha()));
76  alpha_property_->setMin(0.0);
77  alpha_property_->setMax(1.0);
78 
80  new StringProperty("Robot Description", "robot_description",
81  "Name of the parameter to search for to load the robot description.", this,
82  SLOT(updateRobotDescription()));
83 
85  "TF Prefix", "",
86  "Robot Model normally assumes the link name is the same as the tf frame name. "
87  " This option allows you to set a prefix. Mainly useful for multi-robot situations.",
88  this, SLOT(updateTfPrefix()));
89 }
90 
92 {
93  if (initialized())
94  {
95  delete robot_;
96  }
97 }
98 
100 {
101  robot_ = new Robot(scene_node_, context_, "Robot: " + getName().toStdString(), this);
102 
105  updateAlpha();
106 }
107 
109 {
112 }
113 
115 {
116  if (isEnabled())
117  load();
118 }
119 
121 {
124 }
125 
127 {
130 }
131 
133 {
134  clearStatuses();
136 }
137 
139 {
140  clearStatuses();
142 
143  std::string content;
144  try
145  {
147  {
148  std::string loc;
150  update_nh_.getParam(loc, content);
151  else
152  {
153  clear();
155  QString("Parameter [%1] does not exist, and was not found by searchParam()")
157  // try again in a second
158  QTimer::singleShot(1000, this, SLOT(updateRobotDescription()));
159  return;
160  }
161  }
162  }
163  catch (const ros::InvalidNameException& e)
164  {
165  clear();
167  QString("Invalid parameter name: %1.\n%2")
168  .arg(robot_description_property_->getString(), e.what()));
169  return;
170  }
171 
172  if (content.empty())
173  {
174  clear();
175  setStatus(StatusProperty::Error, "URDF", "URDF is empty");
176  return;
177  }
178 
179  if (content == robot_description_)
180  {
181  return;
182  }
183 
184  robot_description_ = content;
185 
186  urdf::Model descr;
187  if (!descr.initString(robot_description_))
188  {
189  clear();
190  setStatus(StatusProperty::Error, "URDF", "Failed to parse URDF model");
191  return;
192  }
193 
194  setStatus(StatusProperty::Ok, "URDF", "URDF parsed OK");
195  robot_->load(descr);
196  std::stringstream ss;
197  for (const auto& name_link_pair : robot_->getLinks())
198  {
199  const std::string& err = name_link_pair.second->getGeometryErrors();
200  if (!err.empty())
201  ss << "\n• for link '" << name_link_pair.first << "':\n" << err;
202  }
203  if (ss.tellp())
205  QString("Errors loading geometries:").append(ss.str().c_str()));
206 
208  boost::bind(linkUpdaterStatusFunction, _1, _2, _3, this),
210 }
211 
213 {
214  load();
215  robot_->setVisible(true);
216 }
217 
219 {
220  robot_->setVisible(false);
221  clear();
222 }
223 
224 void RobotModelDisplay::update(float wall_dt, float /*ros_dt*/)
225 {
226  time_since_last_transform_ += wall_dt;
227  float rate = update_rate_property_->getFloat();
228  bool update = rate < 0.0001f || time_since_last_transform_ >= rate;
229 
230  if (has_new_transforms_ || update)
231  {
233  boost::bind(linkUpdaterStatusFunction, _1, _2, _3, this),
236 
237  has_new_transforms_ = false;
239  }
240 }
241 
243 {
244  has_new_transforms_ = true;
245 }
246 
248 {
249  robot_->clear();
250  clearStatuses();
251  robot_description_.clear();
252 }
253 
255 {
256  Display::reset();
257  has_new_transforms_ = true;
258 }
259 
260 } // namespace rviz
261 
virtual void update(const LinkUpdater &updater)
Definition: robot.cpp:700
void setMin(float min)
void setMax(float max)
virtual void clear()
Clears all data loaded from a URDF.
Definition: robot.cpp:189
void onDisable() override
Derived classes override this to do the actual work of disabling themselves.
const M_NameToLink & getLinks() const
Definition: robot.h:158
URDF_EXPORT bool initString(const std::string &xmlstring)
void setAlpha(float a)
Definition: robot.cpp:175
f
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
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:295
std::string getStdString()
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:233
void fixedFrameChanged() override
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
void onInitialize() override
Override this function to do subclass-specific initialization.
void reset() override
Called to tell the display to clear its state.
virtual void clearStatuses()
Delete all status children. This is thread-safe.
Definition: display.cpp:210
void setCollisionVisible(bool visible)
Set whether the collision meshes/primitives of the robot should be visible.
Definition: robot.cpp:143
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:287
bool getParam(const std::string &key, std::string &s) const
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:164
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=nullptr, const char *changed_slot=nullptr, QObject *receiver=nullptr)
Constructor.
Definition: property.cpp:58
Robot * robot_
Handles actually drawing the robot.
void onEnable() override
Derived classes override this to do the actual work of enabling themselves.
bool searchParam(const std::string &key, std::string &result) const
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.
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:268
virtual float getFloat() const
void setVisualVisible(bool visible)
Set whether the visual meshes of the robot should be visible.
Definition: robot.cpp:137
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
Uses a robot xml description to display the pieces of a robot at the transforms broadcast by rosTF...
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:150
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:275
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:120
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:175


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat May 27 2023 02:06:25