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