moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
40 
41 // #include <rviz/visualization_manager.h>
42 #include <rviz_default_plugins/robot/robot.hpp>
43 #include <rviz_default_plugins/robot/robot_link.hpp>
44 
45 #include <rviz_common/properties/property.hpp>
46 #include <rviz_common/properties/string_property.hpp>
47 #include <rviz_common/properties/bool_property.hpp>
48 #include <rviz_common/properties/float_property.hpp>
49 #include <rviz_common/properties/ros_topic_property.hpp>
50 #include <rviz_common/properties/color_property.hpp>
51 #include <rviz_common/display_context.hpp>
52 #include <rviz_common/frame_manager_iface.hpp>
53 
54 #include <OgreSceneManager.h>
55 #include <OgreSceneNode.h>
56 
57 #include <memory>
58 
59 namespace moveit_rviz_plugin
60 {
61 // ******************************************************************************************
62 // Base class constructor
63 // ******************************************************************************************
64 RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false)
65 {
66  robot_description_property_ = new rviz_common::properties::StringProperty(
67  "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
68  this, SLOT(changedRobotDescription()), this);
69 
70  robot_state_topic_property_ = new rviz_common::properties::RosTopicProperty(
71  "Robot State Topic", "display_robot_state",
72  rosidl_generator_traits::data_type<moveit_msgs::msg::DisplayRobotState>(),
73  "The topic on which the moveit_msgs::msg::DisplayRobotState messages are received", this,
74  SLOT(changedRobotStateTopic()), this);
75 
76  // Planning scene category -------------------------------------------------------------------------------------------
78  new rviz_common::properties::StringProperty("Robot Root Link", "",
79  "Shows the name of the root link for the robot model", this,
80  SLOT(changedRootLinkName()), this);
81  root_link_name_property_->setReadOnly(true);
82 
83  robot_alpha_property_ = new rviz_common::properties::FloatProperty(
84  "Robot Alpha", 1.0f, "Specifies the alpha for the robot links", this, SLOT(changedRobotSceneAlpha()), this);
85  robot_alpha_property_->setMin(0.0);
86  robot_alpha_property_->setMax(1.0);
87 
89  new rviz_common::properties::ColorProperty("Attached Body Color", QColor(150, 50, 150),
90  "The color for the attached bodies", this,
91  SLOT(changedAttachedBodyColor()), this);
92 
93  enable_link_highlight_ = new rviz_common::properties::BoolProperty("Show Highlights", true,
94  "Specifies whether link highlighting is enabled",
95  this, SLOT(changedEnableLinkHighlight()), this);
97  new rviz_common::properties::BoolProperty("Visual Enabled", true,
98  "Whether to display the visual representation of the robot.", this,
99  SLOT(changedEnableVisualVisible()), this);
101  new rviz_common::properties::BoolProperty("Collision Enabled", false,
102  "Whether to display the collision representation of the robot.", this,
103  SLOT(changedEnableCollisionVisible()), this);
104 
105  show_all_links_ = new rviz_common::properties::BoolProperty(
106  "Show All Links", true, "Toggle all links visibility on or off.", this, SLOT(changedAllLinks()), this);
107 }
108 
109 // ******************************************************************************************
110 // Deconstructor
111 // ******************************************************************************************
113 
115 {
116  Display::onInitialize();
117  auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
118  if (!ros_node_abstraction)
119  {
120  RVIZ_COMMON_LOG_WARNING("Unable to lock weak_ptr from DisplayContext in RobotStateDisplay constructor");
121  return;
122  }
123  robot_state_topic_property_->initialize(ros_node_abstraction);
124  node_ = ros_node_abstraction->get_raw_node();
125  robot_ = std::make_shared<RobotStateVisualization>(scene_node_, context_, "Robot State", this);
126  changedEnableVisualVisible();
127  changedEnableCollisionVisible();
128  robot_->setVisible(false);
129 }
130 
132 {
133  robot_->clear();
134  rdf_loader_.reset();
135  Display::reset();
136  if (isEnabled())
137  onEnable();
138 }
139 
140 void RobotStateDisplay::changedAllLinks()
141 {
142  Property* links_prop = subProp("Links");
143  QVariant value(show_all_links_->getBool());
144 
145  for (int i = 0; i < links_prop->numChildren(); ++i)
146  {
147  Property* link_prop = links_prop->childAt(i);
148  link_prop->setValue(value);
149  }
150 }
151 
152 void RobotStateDisplay::setHighlight(const std::string& link_name, const std_msgs::msg::ColorRGBA& color)
153 {
154  rviz_default_plugins::robot::RobotLink* link = robot_->getRobot().getLink(link_name);
155  if (link)
156  {
157  link->setColor(color.r, color.g, color.b);
158  link->setRobotAlpha(color.a * robot_alpha_property_->getFloat());
159  }
160 }
161 
162 void RobotStateDisplay::unsetHighlight(const std::string& link_name)
163 {
164  rviz_default_plugins::robot::RobotLink* link = robot_->getRobot().getLink(link_name);
165  if (link)
166  {
167  link->unsetColor();
168  link->setRobotAlpha(robot_alpha_property_->getFloat());
169  }
170 }
171 
172 void RobotStateDisplay::changedEnableLinkHighlight()
173 {
174  if (enable_link_highlight_->getBool())
175  {
176  for (std::pair<const std::string, std_msgs::msg::ColorRGBA>& highlight : highlights_)
177  {
178  setHighlight(highlight.first, highlight.second);
179  }
180  }
181  else
182  {
183  for (std::pair<const std::string, std_msgs::msg::ColorRGBA>& highlight : highlights_)
184  {
185  unsetHighlight(highlight.first);
186  }
187  }
188 }
189 
190 void RobotStateDisplay::changedEnableVisualVisible()
191 {
192  robot_->setVisualVisible(enable_visual_visible_->getBool());
193 }
194 
195 void RobotStateDisplay::changedEnableCollisionVisible()
196 {
197  robot_->setCollisionVisible(enable_collision_visible_->getBool());
198 }
199 
201  const moveit_msgs::msg::DisplayRobotState::_highlight_links_type& highlight_links)
202 {
203  if (highlight_links.empty() && highlights_.empty())
204  return;
205 
206  std::map<std::string, std_msgs::msg::ColorRGBA> highlights;
207  for (const moveit_msgs::msg::ObjectColor& highlight_link : highlight_links)
208  {
209  highlights[highlight_link.id] = highlight_link.color;
210  }
211 
212  if (enable_link_highlight_->getBool())
213  {
214  std::map<std::string, std_msgs::msg::ColorRGBA>::iterator ho = highlights_.begin();
215  std::map<std::string, std_msgs::msg::ColorRGBA>::iterator hn = highlights.begin();
216  while (ho != highlights_.end() || hn != highlights.end())
217  {
218  if (ho == highlights_.end())
219  {
220  setHighlight(hn->first, hn->second);
221  ++hn;
222  }
223  else if (hn == highlights.end())
224  {
225  unsetHighlight(ho->first);
226  ++ho;
227  }
228  else if (hn->first < ho->first)
229  {
230  setHighlight(hn->first, hn->second);
231  ++hn;
232  }
233  else if (hn->first > ho->first)
234  {
235  unsetHighlight(ho->first);
236  ++ho;
237  }
238  else if (hn->second != ho->second)
239  {
240  setHighlight(hn->first, hn->second);
241  ++ho;
242  ++hn;
243  }
244  else
245  {
246  ++ho;
247  ++hn;
248  }
249  }
250  }
251 
252  swap(highlights, highlights_);
253 }
254 
255 void RobotStateDisplay::changedAttachedBodyColor()
256 {
257  if (robot_)
258  {
259  QColor color = attached_body_color_property_->getColor();
260  std_msgs::msg::ColorRGBA color_msg;
261  color_msg.r = color.redF();
262  color_msg.g = color.greenF();
263  color_msg.b = color.blueF();
264  color_msg.a = robot_alpha_property_->getFloat();
265  robot_->setDefaultAttachedObjectColor(color_msg);
266  update_state_ = true;
267  }
268 }
269 
270 void RobotStateDisplay::changedRobotDescription()
271 {
272  if (isEnabled())
273  reset();
274 }
275 
276 void RobotStateDisplay::changedRootLinkName()
277 {
278 }
279 
280 void RobotStateDisplay::changedRobotSceneAlpha()
281 {
282  if (robot_)
283  {
284  robot_->setAlpha(robot_alpha_property_->getFloat());
285  QColor color = attached_body_color_property_->getColor();
286  std_msgs::msg::ColorRGBA color_msg;
287  color_msg.r = color.redF();
288  color_msg.g = color.greenF();
289  color_msg.b = color.blueF();
290  color_msg.a = robot_alpha_property_->getFloat();
291  robot_->setDefaultAttachedObjectColor(color_msg);
292  update_state_ = true;
293  }
294 }
295 
296 void RobotStateDisplay::changedRobotStateTopic()
297 {
298  using namespace std::placeholders;
299  // reset model to default state, we don't want to show previous messages
300  if (static_cast<bool>(robot_state_))
301  robot_state_->setToDefaultValues();
302  update_state_ = true;
303  robot_->setVisible(false);
304  setStatus(rviz_common::properties::StatusProperty::Warn, "RobotState", "No msg received");
305 
306  robot_state_subscriber_ = node_->create_subscription<moveit_msgs::msg::DisplayRobotState>(
307  robot_state_topic_property_->getStdString(), rclcpp::SystemDefaultsQoS(),
308  [this](const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state) { return newRobotStateCallback(state); });
309 }
310 
311 void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state_msg)
312 {
313  if (!robot_model_)
314  return;
315  if (!robot_state_)
316  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
317  // possibly use TF to construct a moveit::core::Transforms object to pass in to the conversion function?
318  try
319  {
320  if (!moveit::core::isEmpty(state_msg->state))
322  setRobotHighlights(state_msg->highlight_links);
323  }
324  catch (const moveit::Exception& e)
325  {
326  robot_state_->setToDefaultValues();
327  setRobotHighlights(moveit_msgs::msg::DisplayRobotState::_highlight_links_type());
328  setStatus(rviz_common::properties::StatusProperty::Error, "RobotState", e.what());
329  robot_->setVisible(false);
330  return;
331  }
332 
333  if (robot_->isVisible() != !state_msg->hide)
334  {
335  robot_->setVisible(!state_msg->hide);
336  if (robot_->isVisible())
337  setStatus(rviz_common::properties::StatusProperty::Ok, "RobotState", "");
338  else
339  setStatus(rviz_common::properties::StatusProperty::Warn, "RobotState", "Hidden");
340  }
341 
342  update_state_ = true;
343 }
344 
345 void RobotStateDisplay::setLinkColor(const std::string& link_name, const QColor& color)
346 {
347  setLinkColor(&robot_->getRobot(), link_name, color);
348 }
349 
350 void RobotStateDisplay::unsetLinkColor(const std::string& link_name)
351 {
352  unsetLinkColor(&robot_->getRobot(), link_name);
353 }
354 
356 {
357  robot_->setVisible(visible);
358 }
359 
360 void RobotStateDisplay::setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name,
361  const QColor& color)
362 {
363  rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
364 
365  // Check if link exists
366  if (link)
367  link->setColor(color.redF(), color.greenF(), color.blueF());
368 }
369 
370 void RobotStateDisplay::unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name)
371 {
372  rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
373 
374  // Check if link exists
375  if (link)
376  link->unsetColor();
377 }
378 
379 // ******************************************************************************************
380 // Load
381 // ******************************************************************************************
383 {
384  if (robot_description_property_->getStdString().empty())
385  {
386  setStatus(rviz_common::properties::StatusProperty::Error, "RobotModel", "`Robot Description` field can't be empty");
387  return;
388  }
389 
390  rdf_loader_ = std::make_shared<rdf_loader::RDFLoader>(node_, robot_description_property_->getStdString(), true);
391  loadRobotModel();
392  rdf_loader_->setNewModelCallback([this]() { return loadRobotModel(); });
393 }
394 
396 {
397  if (rdf_loader_->getURDF())
398  {
399  try
400  {
401  const srdf::ModelSharedPtr& srdf =
402  rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : std::make_shared<srdf::Model>();
403  robot_model_ = std::make_shared<moveit::core::RobotModel>(rdf_loader_->getURDF(), srdf);
404  robot_->load(*robot_model_->getURDF());
405  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
406  robot_state_->setToDefaultValues();
407  bool old_state = root_link_name_property_->blockSignals(true);
408  root_link_name_property_->setStdString(getRobotModel()->getRootLinkName());
409  root_link_name_property_->blockSignals(old_state);
410  update_state_ = true;
411  setStatus(rviz_common::properties::StatusProperty::Ok, "RobotModel", "Loaded successfully");
412 
413  changedEnableVisualVisible();
414  changedEnableCollisionVisible();
415  }
416  catch (std::exception& e)
417  {
418  setStatus(rviz_common::properties::StatusProperty::Error, "RobotModel",
419  QString("Loading failed: %1").arg(e.what()));
420  }
421  }
422  else
423  setStatus(rviz_common::properties::StatusProperty::Error, "RobotModel", "Loading failed");
424 
425  highlights_.clear();
426 }
427 
428 void RobotStateDisplay::load(const rviz_common::Config& config)
429 {
430  // This property needs to be loaded in onEnable() below, which is triggered
431  // in the beginning of Display::load() before the other property would be available
432  robot_description_property_->load(config.mapGetChild("Robot Description"));
433  Display::load(config);
434 }
435 
437 {
438  Display::onEnable();
439  if (!rdf_loader_)
441  changedRobotStateTopic();
443 }
444 
445 // ******************************************************************************************
446 // Disable
447 // ******************************************************************************************
449 {
450  robot_state_subscriber_.reset();
451  if (robot_)
452  robot_->setVisible(false);
453  Display::onDisable();
454 }
455 
456 void RobotStateDisplay::update(float wall_dt, float ros_dt)
457 {
458  Display::update(wall_dt, ros_dt);
461  {
462  update_state_ = false;
463  robot_state_->update();
464  robot_->update(robot_state_);
465  }
466 }
467 
468 // ******************************************************************************************
469 // Calculate Offset Position
470 // ******************************************************************************************
472 {
473  if (!getRobotModel())
474  return;
475 
476  Ogre::Vector3 position;
477  Ogre::Quaternion orientation;
478 
479  context_->getFrameManager()->getTransform(getRobotModel()->getModelFrame(), rclcpp::Time(0, 0, RCL_ROS_TIME),
480  position, orientation);
481 
482  scene_node_->setPosition(position);
483  scene_node_->setOrientation(orientation);
484 }
485 
487 {
488  Display::fixedFrameChanged();
490 }
491 
492 } // namespace moveit_rviz_plugin
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
rviz_common::properties::RosTopicProperty * robot_state_topic_property_
void load(const rviz_common::Config &config) override
moveit::core::RobotModelConstPtr robot_model_
void setHighlight(const std::string &link_name, const std_msgs::msg::ColorRGBA &color)
void update(float wall_dt, float ros_dt) override
void setRobotHighlights(const moveit_msgs::msg::DisplayRobotState::_highlight_links_type &highlight_links)
rviz_common::properties::FloatProperty * robot_alpha_property_
rviz_common::properties::ColorProperty * attached_body_color_property_
rviz_common::properties::StringProperty * root_link_name_property_
rclcpp::Subscription< moveit_msgs::msg::DisplayRobotState >::SharedPtr robot_state_subscriber_
void unsetHighlight(const std::string &link_name)
rviz_common::properties::BoolProperty * enable_visual_visible_
void calculateOffsetPosition()
Set the scene node's position, given the target frame and the planning frame.
void unsetLinkColor(const std::string &link_name)
void newRobotStateCallback(const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr &state)
rviz_common::properties::BoolProperty * enable_link_highlight_
rviz_common::properties::BoolProperty * enable_collision_visible_
rviz_common::properties::BoolProperty * show_all_links_
rviz_common::properties::StringProperty * robot_description_property_
void setLinkColor(const std::string &link_name, const QColor &color)
std::map< std::string, std_msgs::msg::ColorRGBA > highlights_
moveit::core::RobotStatePtr robot_state_
const moveit::core::RobotModelConstPtr & getRobotModel() const
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
robot
Definition: pick.py:53