robot_state_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 
41 #include <rviz/robot/robot.h>
42 #include <rviz/robot/robot_link.h>
43 
50 #include <rviz/display_context.h>
51 #include <rviz/frame_manager.h>
52 #include <tf/transform_listener.h>
53 
54 #include <OgreSceneManager.h>
55 #include <OgreSceneNode.h>
56 
57 namespace moveit_rviz_plugin
58 {
59 // ******************************************************************************************
60 // Base class contructor
61 // ******************************************************************************************
62 RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false), load_robot_model_(false)
63 {
65  "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
66  this, SLOT(changedRobotDescription()), this);
67 
69  "Robot State Topic", "display_robot_state", ros::message_traits::datatype<moveit_msgs::DisplayRobotState>(),
70  "The topic on which the moveit_msgs::RobotState messages are received", this, SLOT(changedRobotStateTopic()),
71  this);
72 
73  // Planning scene category -------------------------------------------------------------------------------------------
75  new rviz::StringProperty("Robot Root Link", "", "Shows the name of the root link for the robot model", this,
76  SLOT(changedRootLinkName()), this);
78 
79  robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 1.0f, "Specifies the alpha for the robot links", this,
80  SLOT(changedRobotSceneAlpha()), this);
83 
85  new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", this,
86  SLOT(changedAttachedBodyColor()), this);
87 
89  new rviz::BoolProperty("Show Highlights", true, "Specifies whether link highlighting is enabled", this,
90  SLOT(changedEnableLinkHighlight()), this);
92  new rviz::BoolProperty("Visual Enabled", true, "Whether to display the visual representation of the robot.", this,
93  SLOT(changedEnableVisualVisible()), this);
94  enable_collision_visible_ = new rviz::BoolProperty("Collision Enabled", false,
95  "Whether to display the collision representation of the robot.",
96  this, SLOT(changedEnableCollisionVisible()), this);
97 
98  show_all_links_ = new rviz::BoolProperty("Show All Links", true, "Toggle all links visibility on or off.", this,
99  SLOT(changedAllLinks()), this);
100 }
101 
102 // ******************************************************************************************
103 // Deconstructor
104 // ******************************************************************************************
106 {
107 }
108 
110 {
111  Display::onInitialize();
112  robot_.reset(new RobotStateVisualization(scene_node_, context_, "Robot State", this));
115  robot_->setVisible(false);
116 }
117 
119 {
120  robot_->clear();
121  rdf_loader_.reset();
122  Display::reset();
123 
124  loadRobotModel();
125 }
126 
128 {
129  Property* links_prop = subProp("Links");
130  QVariant value(show_all_links_->getBool());
131 
132  for (int i = 0; i < links_prop->numChildren(); ++i)
133  {
134  Property* link_prop = links_prop->childAt(i);
135  link_prop->setValue(value);
136  }
137 }
138 
139 void RobotStateDisplay::setHighlight(const std::string& link_name, const std_msgs::ColorRGBA& color)
140 {
141  rviz::RobotLink* link = robot_->getRobot().getLink(link_name);
142  if (link)
143  {
144  link->setColor(color.r, color.g, color.b);
145  link->setRobotAlpha(color.a * robot_alpha_property_->getFloat());
146  }
147 }
148 
149 void RobotStateDisplay::unsetHighlight(const std::string& link_name)
150 {
151  rviz::RobotLink* link = robot_->getRobot().getLink(link_name);
152  if (link)
153  {
154  link->unsetColor();
156  }
157 }
158 
160 {
162  {
163  for (std::map<std::string, std_msgs::ColorRGBA>::iterator it = highlights_.begin(); it != highlights_.end(); ++it)
164  {
165  setHighlight(it->first, it->second);
166  }
167  }
168  else
169  {
170  for (std::map<std::string, std_msgs::ColorRGBA>::iterator it = highlights_.begin(); it != highlights_.end(); ++it)
171  {
172  unsetHighlight(it->first);
173  }
174  }
175 }
176 
178 {
179  robot_->setVisualVisible(enable_visual_visible_->getBool());
180 }
181 
183 {
184  robot_->setCollisionVisible(enable_collision_visible_->getBool());
185 }
186 
187 static bool operator!=(const std_msgs::ColorRGBA& a, const std_msgs::ColorRGBA& b)
188 {
189  return a.r != b.r || a.g != b.g || a.b != b.b || a.a != b.a;
190 }
191 
192 void RobotStateDisplay::setRobotHighlights(const moveit_msgs::DisplayRobotState::_highlight_links_type& highlight_links)
193 {
194  if (highlight_links.empty() && highlights_.empty())
195  return;
196 
197  std::map<std::string, std_msgs::ColorRGBA> highlights;
198  for (moveit_msgs::DisplayRobotState::_highlight_links_type::const_iterator it = highlight_links.begin();
199  it != highlight_links.end(); ++it)
200  {
201  highlights[it->id] = it->color;
202  }
203 
205  {
206  std::map<std::string, std_msgs::ColorRGBA>::iterator ho = highlights_.begin();
207  std::map<std::string, std_msgs::ColorRGBA>::iterator hn = highlights.begin();
208  while (ho != highlights_.end() || hn != highlights.end())
209  {
210  if (ho == highlights_.end())
211  {
212  setHighlight(hn->first, hn->second);
213  ++hn;
214  }
215  else if (hn == highlights.end())
216  {
217  unsetHighlight(ho->first);
218  ++ho;
219  }
220  else if (hn->first < ho->first)
221  {
222  setHighlight(hn->first, hn->second);
223  ++hn;
224  }
225  else if (hn->first > ho->first)
226  {
227  unsetHighlight(ho->first);
228  ++ho;
229  }
230  else if (hn->second != ho->second)
231  {
232  setHighlight(hn->first, hn->second);
233  ++ho;
234  ++hn;
235  }
236  else
237  {
238  ++ho;
239  ++hn;
240  }
241  }
242  }
243 
244  swap(highlights, highlights_);
245 }
246 
248 {
249  if (robot_)
250  {
251  QColor color = attached_body_color_property_->getColor();
252  std_msgs::ColorRGBA color_msg;
253  color_msg.r = color.redF();
254  color_msg.g = color.greenF();
255  color_msg.b = color.blueF();
256  color_msg.a = robot_alpha_property_->getFloat();
257  robot_->setDefaultAttachedObjectColor(color_msg);
258  update_state_ = true;
259  }
260 }
261 
263 {
264  if (isEnabled())
265  reset();
266 }
267 
269 {
270 }
271 
273 {
274  if (robot_)
275  {
276  robot_->setAlpha(robot_alpha_property_->getFloat());
277  QColor color = attached_body_color_property_->getColor();
278  std_msgs::ColorRGBA color_msg;
279  color_msg.r = color.redF();
280  color_msg.g = color.greenF();
281  color_msg.b = color.blueF();
282  color_msg.a = robot_alpha_property_->getFloat();
283  robot_->setDefaultAttachedObjectColor(color_msg);
284  update_state_ = true;
285  }
286 }
287 
289 {
291 
292  // reset model to default state, we don't want to show previous messages
293  if (static_cast<bool>(kstate_))
294  kstate_->setToDefaultValues();
295  update_state_ = true;
296 
299 }
300 
301 void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotStateConstPtr& state_msg)
302 {
303  if (!kmodel_)
304  return;
305  if (!kstate_)
306  kstate_.reset(new robot_state::RobotState(kmodel_));
307  // possibly use TF to construct a robot_state::Transforms object to pass in to the conversion functio?
308  robot_state::robotStateMsgToRobotState(state_msg->state, *kstate_);
309  setRobotHighlights(state_msg->highlight_links);
310  update_state_ = true;
311 }
312 
313 void RobotStateDisplay::setLinkColor(const std::string& link_name, const QColor& color)
314 {
315  setLinkColor(&robot_->getRobot(), link_name, color);
316 }
317 
318 void RobotStateDisplay::unsetLinkColor(const std::string& link_name)
319 {
320  unsetLinkColor(&robot_->getRobot(), link_name);
321 }
322 
323 void RobotStateDisplay::setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color)
324 {
325  rviz::RobotLink* link = robot->getLink(link_name);
326 
327  // Check if link exists
328  if (link)
329  link->setColor(color.redF(), color.greenF(), color.blueF());
330 }
331 
332 void RobotStateDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& link_name)
333 {
334  rviz::RobotLink* link = robot->getLink(link_name);
335 
336  // Check if link exists
337  if (link)
338  link->unsetColor();
339 }
340 
341 // ******************************************************************************************
342 // Load
343 // ******************************************************************************************
345 {
346  load_robot_model_ = false;
347  if (!rdf_loader_)
349 
350  if (rdf_loader_->getURDF())
351  {
352  const srdf::ModelSharedPtr& srdf =
353  rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model());
354  kmodel_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
355  robot_->load(*kmodel_->getURDF());
356  kstate_.reset(new robot_state::RobotState(kmodel_));
357  kstate_->setToDefaultValues();
358  bool oldState = root_link_name_property_->blockSignals(true);
359  root_link_name_property_->setStdString(getRobotModel()->getRootLinkName());
360  root_link_name_property_->blockSignals(oldState);
361  update_state_ = true;
362  setStatus(rviz::StatusProperty::Ok, "RobotState", "Planning Model Loaded Successfully");
363 
366  robot_->setVisible(true);
367  }
368  else
369  setStatus(rviz::StatusProperty::Error, "RobotState", "No Planning Model Loaded");
370 
371  highlights_.clear();
372 }
373 
375 {
376  Display::onEnable();
377  load_robot_model_ = true; // allow loading of robot model in update()
379 }
380 
381 // ******************************************************************************************
382 // Disable
383 // ******************************************************************************************
385 {
387  if (robot_)
388  robot_->setVisible(false);
389  Display::onDisable();
390 }
391 
392 void RobotStateDisplay::update(float wall_dt, float ros_dt)
393 {
394  Display::update(wall_dt, ros_dt);
395 
396  if (load_robot_model_)
397  {
398  loadRobotModel();
400  }
401 
403  if (robot_ && update_state_ && kstate_)
404  {
405  update_state_ = false;
406  kstate_->update();
407  robot_->update(kstate_);
408  }
409 }
410 
411 // ******************************************************************************************
412 // Calculate Offset Position
413 // ******************************************************************************************
415 {
416  if (!getRobotModel())
417  return;
418 
419  Ogre::Vector3 position;
420  Ogre::Quaternion orientation;
421 
422  context_->getFrameManager()->getTransform(getRobotModel()->getModelFrame(), ros::Time(0), position, orientation);
423 
424  scene_node_->setPosition(position);
425  scene_node_->setOrientation(orientation);
426 }
427 
429 {
430  Display::fixedFrameChanged();
432 }
433 
434 } // namespace moveit_rviz_plugin
virtual QColor getColor() const
void setMin(float min)
void unsetLinkColor(const std::string &link_name)
void setMax(float max)
rviz::StringProperty * root_link_name_property_
Update the links of an rviz::Robot using a robot_state::RobotState.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
DisplayContext * context_
void calculateOffsetPosition()
Set the scene node&#39;s position, given the target frame and the planning frame.
f
virtual float getFloat() const
virtual void update(float wall_dt, float ros_dt)
RobotLink * getLink(const std::string &name)
Ogre::SceneNode * scene_node_
void setRobotHighlights(const moveit_msgs::DisplayRobotState::_highlight_links_type &highlight_links)
std::string getStdString()
bool isEnabled() const
virtual bool getBool() const
virtual Property * subProp(const QString &sub_name)
void unsetHighlight(const std::string &link_name)
void setHighlight(const std::string &link_name, const std_msgs::ColorRGBA &color)
const robot_model::RobotModelConstPtr & getRobotModel() const
rviz::StringProperty * robot_description_property_
virtual FrameManager * getFrameManager() const =0
boost::shared_ptr< Model > ModelSharedPtr
rviz::RosTopicProperty * robot_state_topic_property_
std::map< std::string, std_msgs::ColorRGBA > highlights_
rviz::ColorProperty * attached_body_color_property_
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)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
robot_model::RobotModelConstPtr kmodel_
static bool operator!=(const std_msgs::ColorRGBA &a, const std_msgs::ColorRGBA &b)
void setLinkColor(const std::string &link_name, const QColor &color)
void newRobotStateCallback(const moveit_msgs::DisplayRobotState::ConstPtr &state)
virtual void setReadOnly(bool read_only)
bool setStdString(const std::string &std_str)
int value
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:09