moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
planning_scene_display.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Willow Garage, Inc.
5 * Copyright (c) 2013, Ioan A. Sucan
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Ioan Sucan */
37
42
43#include <rviz_common/transformation/transformation_manager.hpp>
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/bool_property.hpp>
49#include <rviz_common/properties/float_property.hpp>
50#include <rviz_common/properties/color_property.hpp>
51#include <rviz_common/properties/enum_property.hpp>
52#include <rviz_common/display_context.hpp>
53#include <rviz_common/logging.hpp>
54#include <rviz_common/frame_manager_iface.hpp>
55// For Rolling, Kilted, and newer
56#if RCLCPP_VERSION_GTE(29, 6, 0)
57#include <tf2_ros/buffer.hpp>
58// For Jazzy and older
59#else
60#include <tf2_ros/buffer.h>
61#endif
62
63#include <OgreSceneManager.h>
64#include <OgreSceneNode.h>
65
68
69#include <cmath>
70
71namespace moveit_rviz_plugin
72{
73// ******************************************************************************************
74// Base class constructor
75// ******************************************************************************************
76PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool show_scene_robot)
77 : Display()
80 , logger_(moveit::getLogger("moveit.ros.planning_scene_display"))
81{
82 move_group_ns_property_ = new rviz_common::properties::StringProperty("Move Group Namespace", "",
83 "The name of the ROS namespace in "
84 "which the move_group node is running",
85 this, SLOT(changedMoveGroupNS()), this);
86 robot_description_property_ = new rviz_common::properties::StringProperty(
87 "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
88 this, SLOT(changedRobotDescription()), this);
89
90 if (listen_to_planning_scene)
91 {
92 planning_scene_topic_property_ = new rviz_common::properties::RosTopicProperty(
93 "Planning Scene Topic", "/monitored_planning_scene",
94 rosidl_generator_traits::data_type<moveit_msgs::msg::PlanningScene>(),
95 "The topic on which the moveit_msgs::msg::PlanningScene messages are received", this,
96 SLOT(changedPlanningSceneTopic()), this);
97 }
98 else
99 {
101 }
102
103 // Planning scene category -------------------------------------------------------------------------------------------
104 scene_category_ = new rviz_common::properties::Property("Scene Geometry", QVariant(), "", this);
105
107 new rviz_common::properties::StringProperty("Scene Name", "(noname)", "Shows the name of the planning scene",
108 scene_category_, SLOT(changedSceneName()), this);
109 scene_name_property_->setShouldBeSaved(false);
111 new rviz_common::properties::BoolProperty("Show Scene Geometry", true,
112 "Indicates whether planning scenes should be displayed",
113 scene_category_, SLOT(changedSceneEnabled()), this);
114
116 new rviz_common::properties::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry",
117 scene_category_, SLOT(changedSceneAlpha()), this);
118 scene_alpha_property_->setMin(0.0);
119 scene_alpha_property_->setMax(1.0);
120
121 scene_color_property_ = new rviz_common::properties::ColorProperty(
122 "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)",
123 scene_category_, SLOT(changedSceneColor()), this);
124
126 new rviz_common::properties::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.",
127 scene_category_, SLOT(changedOctreeRenderMode()), this);
128
129 octree_render_property_->addOption("Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS);
130 octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS);
132 octree_render_property_->addOption("Disabled", OCTOMAP_DISABLED);
133
134 octree_coloring_property_ = new rviz_common::properties::EnumProperty(
135 "Voxel Coloring", "Z-Axis", "Select voxel coloring mode", scene_category_, SLOT(changedOctreeColorMode()), this);
136
138 octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
139
141 new rviz_common::properties::FloatProperty("Scene Display Time", 0.01f,
142 "The amount of wall-time to wait in between rendering "
143 "updates to the planning scene (if any)",
144 scene_category_, SLOT(changedSceneDisplayTime()), this);
145 scene_display_time_property_->setMin(0.0001);
146
147 if (show_scene_robot)
148 {
149 robot_category_ = new rviz_common::properties::Property("Scene Robot", QVariant(), "", this);
150
151 scene_robot_visual_enabled_property_ = new rviz_common::properties::BoolProperty(
152 "Show Robot Visual", true,
153 "Indicates whether the robot state specified by the planning scene should be "
154 "displayed as defined for visualisation purposes.",
155 robot_category_, SLOT(changedSceneRobotVisualEnabled()), this);
156
157 scene_robot_collision_enabled_property_ = new rviz_common::properties::BoolProperty(
158 "Show Robot Collision", false,
159 "Indicates whether the robot state specified by the planning scene should be "
160 "displayed as defined for collision detection purposes.",
161 robot_category_, SLOT(changedSceneRobotCollisionEnabled()), this);
162
164 new rviz_common::properties::FloatProperty("Robot Alpha", 1.0f, "Specifies the alpha for the robot links",
165 robot_category_, SLOT(changedRobotSceneAlpha()), this);
166 robot_alpha_property_->setMin(0.0);
167 robot_alpha_property_->setMax(1.0);
168
170 new rviz_common::properties::ColorProperty("Attached Body Color", QColor(150, 50, 150),
171 "The color for the attached bodies", robot_category_,
172 SLOT(changedAttachedBodyColor()), this);
173 }
174 else
175 {
176 robot_category_ = nullptr;
179 robot_alpha_property_ = nullptr;
181 }
182}
183
184// ******************************************************************************************
185// Deconstructor
186// ******************************************************************************************
188{
189 clearJobs();
190
192 if (context_ && context_->getSceneManager() && planning_scene_node_)
193 context_->getSceneManager()->destroySceneNode(planning_scene_node_);
194 planning_scene_robot_.reset();
196}
197
199{
200 background_process_.clear();
201 {
202 std::unique_lock<std::mutex> ulock(main_loop_jobs_lock_);
203 main_loop_jobs_.clear();
204 }
205}
206
208{
209 Display::onInitialize();
210 auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
211 if (!ros_node_abstraction)
212 {
213 RVIZ_COMMON_LOG_WARNING("Unable to lock weak_ptr from DisplayContext in PlanningSceneDisplay constructor");
214 return;
215 }
216 node_ = ros_node_abstraction->get_raw_node();
217 planning_scene_topic_property_->initialize(ros_node_abstraction);
218
219 // the scene node that contains everything and is located at the planning frame
220 planning_scene_node_ = scene_node_->createChildSceneNode();
221
222 if (robot_category_)
223 {
225 std::make_shared<RobotStateVisualization>(planning_scene_node_, context_, "Planning Scene", robot_category_);
226 planning_scene_robot_->setVisible(true);
228 planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
229 changedRobotSceneAlpha();
231 }
232}
233
235{
237 planning_scene_robot_->clear();
238 Display::reset();
239
240 if (isEnabled())
241 addBackgroundJob([this] { loadRobotModel(); }, "loadRobotModel");
242
244 {
245 planning_scene_robot_->setVisible(true);
247 planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
248 }
249}
250
251void PlanningSceneDisplay::addBackgroundJob(const std::function<void()>& job, const std::string& name)
252{
253 background_process_.addJob(job, name);
254}
255
256void PlanningSceneDisplay::spawnBackgroundJob(const std::function<void()>& job)
257{
258 std::thread t(job);
259 t.detach();
260}
261
262void PlanningSceneDisplay::addMainLoopJob(const std::function<void()>& job)
263{
264 std::unique_lock<std::mutex> ulock(main_loop_jobs_lock_);
265 main_loop_jobs_.push_back(job);
266}
267
269{
270 std::unique_lock<std::mutex> ulock(main_loop_jobs_lock_);
271 while (!main_loop_jobs_.empty())
273}
274
276{
278 while (!main_loop_jobs_.empty())
279 {
280 std::function<void()> fn = main_loop_jobs_.front();
281 main_loop_jobs_.pop_front();
282 main_loop_jobs_lock_.unlock();
283 try
284 {
285 fn();
286 }
287 catch (std::exception& ex)
288 {
289 RCLCPP_ERROR(logger_, "Exception caught executing main loop job: %s", ex.what());
290 }
292 }
294 main_loop_jobs_lock_.unlock();
295}
296
297const planning_scene_monitor::PlanningSceneMonitorPtr& PlanningSceneDisplay::getPlanningSceneMonitor()
298{
300}
301
303{
304 return move_group_ns_property_->getStdString();
305}
306
307const moveit::core::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const
308{
310 {
311 return planning_scene_monitor_->getRobotModel();
312 }
313 else
314 {
315 static moveit::core::RobotModelConstPtr empty;
316 return empty;
317 }
318}
319
321{
323 return planning_scene_monitor_->waitForCurrentRobotState(t);
324 return false;
325}
326
331
336
341
342void PlanningSceneDisplay::changedSceneColor()
343{
345}
346
347void PlanningSceneDisplay::changedMoveGroupNS()
348{
349 if (isEnabled())
350 reset();
351}
352
353void PlanningSceneDisplay::changedRobotDescription()
354{
355 if (isEnabled())
356 reset();
357}
358
359void PlanningSceneDisplay::changedSceneName()
360{
361 planning_scene_monitor::LockedPlanningSceneRW ps = getPlanningSceneRW();
362 if (ps)
363 ps->setName(scene_name_property_->getStdString());
364}
365
367{
368 QColor color = scene_color_property_->getColor();
369 Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF(), scene_alpha_property_->getFloat());
371 color = attached_body_color_property_->getColor();
372 Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF(), robot_alpha_property_->getFloat());
373
374 try
375 {
378 {
379 planning_scene_render_->renderPlanningScene(
380 ps, env_color, attached_color, static_cast<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()),
381 static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt()),
382 scene_alpha_property_->getFloat());
383 }
384 else
385 {
386 planning_scene_render_->updateRobotPosition(ps);
387 }
388 }
389 catch (std::exception& ex)
390 {
391 RCLCPP_ERROR(logger_, "Caught %s while rendering planning scene", ex.what());
392 }
393 planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
394}
395
396void PlanningSceneDisplay::changedSceneAlpha()
397{
399}
400
401void PlanningSceneDisplay::changedRobotSceneAlpha()
402{
404 {
405 planning_scene_robot_->setAlpha(robot_alpha_property_->getFloat());
407 }
408}
409
410void PlanningSceneDisplay::changedPlanningSceneTopic()
411{
413 {
414 planning_scene_monitor_->startSceneMonitor(planning_scene_topic_property_->getStdString());
416 if (!getMoveGroupNS().empty())
417 service_name = rclcpp::names::append(getMoveGroupNS(), service_name);
418 auto bg_func = [this, service_name]() {
419 if (planning_scene_monitor_->requestPlanningSceneState(service_name))
420 {
422 }
423 else
424 {
425 setStatus(rviz_common::properties::StatusProperty::Warn, "PlanningScene", "Requesting initial scene failed");
426 }
427 };
428 addBackgroundJob(bg_func, "requestPlanningSceneState");
429 }
430}
431
432void PlanningSceneDisplay::changedSceneDisplayTime()
433{
434}
435
436void PlanningSceneDisplay::changedOctreeRenderMode()
437{
438}
439
440void PlanningSceneDisplay::changedOctreeColorMode()
441{
442}
443
444void PlanningSceneDisplay::changedSceneRobotVisualEnabled()
445{
446 if (isEnabled() && planning_scene_robot_)
447 {
448 planning_scene_robot_->setVisible(true);
451 }
452}
453
454void PlanningSceneDisplay::changedSceneRobotCollisionEnabled()
455{
456 if (isEnabled() && planning_scene_robot_)
457 {
458 planning_scene_robot_->setVisible(true);
459 planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
461 }
462}
463
464void PlanningSceneDisplay::changedSceneEnabled()
465{
467 planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
468}
469
470void PlanningSceneDisplay::setGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name,
471 const QColor& color)
472{
473 if (getRobotModel())
474 {
475 const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name);
476 if (jmg)
477 {
478 const std::vector<std::string>& links = jmg->getLinkModelNamesWithCollisionGeometry();
479 for (const std::string& link : links)
480 setLinkColor(robot, link, color);
481 }
482 }
483}
484
485void PlanningSceneDisplay::unsetAllColors(rviz_default_plugins::robot::Robot* robot)
486{
487 if (getRobotModel())
488 {
489 const std::vector<std::string>& links = getRobotModel()->getLinkModelNamesWithCollisionGeometry();
490 for (const std::string& link : links)
491 unsetLinkColor(robot, link);
492 }
493}
494
495void PlanningSceneDisplay::unsetGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name)
496{
497 if (getRobotModel())
498 {
499 const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name);
500 if (jmg)
501 {
502 const std::vector<std::string>& links = jmg->getLinkModelNamesWithCollisionGeometry();
503 for (const std::string& link : links)
504 unsetLinkColor(robot, link);
505 }
506 }
507}
508
509void PlanningSceneDisplay::setLinkColor(const std::string& link_name, const QColor& color)
510{
512 setLinkColor(&planning_scene_robot_->getRobot(), link_name, color);
513}
514
515void PlanningSceneDisplay::unsetLinkColor(const std::string& link_name)
516{
518 unsetLinkColor(&planning_scene_robot_->getRobot(), link_name);
519}
520
521void PlanningSceneDisplay::setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name,
522 const QColor& color)
523{
524 rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
525
526 // Check if link exists
527 if (link)
528 link->setColor(color.redF(), color.greenF(), color.blueF());
529}
530
531void PlanningSceneDisplay::unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name)
532{
533 rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
534
535 // Check if link exists
536 if (link)
537 link->unsetColor();
538}
539// ******************************************************************************************
540// Load
541// ******************************************************************************************
542planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlanningSceneMonitor()
543{
545 return std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(node_, rml,
546 getNameStd() + "_planning_scene_monitor");
547}
548
550{
552 // Ensure old PSM is destroyed before we attempt to create a new one
554}
555
557{
558 // wait for other robot loadRobotModel() calls to complete;
559 std::scoped_lock _(robot_model_loading_lock_);
560
561 // we need to make sure the clearing of the robot model is in the main thread,
562 // so that rendering operations do not have data removed from underneath,
563 // so the next function is executed in the main loop
564 addMainLoopJob([this] { clearRobotModel(); });
565
567
568 planning_scene_monitor::PlanningSceneMonitorPtr psm = createPlanningSceneMonitor();
569 if (psm->getPlanningScene())
570 {
571 planning_scene_monitor_.swap(psm);
572 planning_scene_monitor_->addUpdateCallback(
575 });
576 addMainLoopJob([this] { onRobotModelLoaded(); });
578 }
579 else
580 {
581 addMainLoopJob([this]() {
582 setStatus(rviz_common::properties::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded");
583 });
584 }
585}
586
587// This should always run in the main GUI thread!
589{
590 changedPlanningSceneTopic();
591 planning_scene_render_ = std::make_shared<PlanningSceneRender>(planning_scene_node_, context_, planning_scene_robot_);
592 planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
593
596 {
597 planning_scene_robot_->load(*getRobotModel()->getURDF());
598 moveit::core::RobotState* rs = new moveit::core::RobotState(ps->getCurrentState());
599 rs->update();
600 planning_scene_robot_->update(moveit::core::RobotStateConstPtr(rs));
601 }
602
603 bool old_state = scene_name_property_->blockSignals(true);
604 scene_name_property_->setStdString(ps->getName());
605 scene_name_property_->blockSignals(old_state);
606
607 setStatus(rviz_common::properties::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully");
608}
609
613
619
622{
623 getPlanningSceneRW()->getCurrentStateNonConst().update();
624 QMetaObject::invokeMethod(this, "setSceneName", Qt::QueuedConnection,
625 Q_ARG(QString, QString::fromStdString(getPlanningSceneRO()->getName())));
627}
628
629void PlanningSceneDisplay::setSceneName(const QString& name)
630{
631 scene_name_property_->setString(name);
632}
633
635{
636 Display::onEnable();
637
638 addBackgroundJob([this] { loadRobotModel(); }, "loadRobotModel");
639
641 {
642 planning_scene_robot_->setVisible(true);
644 planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
645 }
647 planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
648
651}
652
653// ******************************************************************************************
654// Disable
655// ******************************************************************************************
657{
659 {
660 planning_scene_monitor_->stopSceneMonitor();
662 planning_scene_render_->getGeometryNode()->setVisible(false);
663 }
665 planning_scene_robot_->setVisible(false);
666 Display::onDisable();
667}
668
673
674// For Rolling, L-turtle, and newer
675#if RCLCPP_VERSION_GTE(30, 0, 0)
676void PlanningSceneDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
677{
678 Display::update(wall_dt, ros_dt);
679
681
683
685 updateInternal(wall_dt, ros_dt);
686}
687// For Kilted and older
688#else
689void PlanningSceneDisplay::update(float wall_dt, float ros_dt)
690{
691 Display::update(wall_dt, ros_dt);
692
694
696
698 updateInternal(wall_dt, ros_dt);
699}
700#endif
701
702void PlanningSceneDisplay::updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds /*ros_dt*/)
703{
704 current_scene_time_ += std::chrono::duration_cast<std::chrono::duration<double>>(wall_dt).count();
708 {
710 current_scene_time_ = 0.0f;
713 }
714}
715
716void PlanningSceneDisplay::updateInternal(double wall_dt, double ros_dt)
717{
718 updateInternal(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(ros_dt)));
719}
720
721void PlanningSceneDisplay::load(const rviz_common::Config& config)
722{
723 Display::load(config);
724}
725
726void PlanningSceneDisplay::save(rviz_common::Config config) const
727{
728 Display::save(config);
729}
730
731// ******************************************************************************************
732// Calculate Offset Position
733// ******************************************************************************************
735{
736 if (!getRobotModel())
737 return;
738
739 Ogre::Vector3 position;
740 Ogre::Quaternion orientation;
741
742 context_->getFrameManager()->getTransform(getRobotModel()->getModelFrame(), rclcpp::Time(0, 0, RCL_ROS_TIME),
743 position, orientation);
744
745 planning_scene_node_->setPosition(position);
746 planning_scene_node_->setOrientation(orientation);
747}
748
750{
751 Display::fixedFrameChanged();
753}
754
755} // namespace moveit_rviz_plugin
const std::vector< std::string > & getLinkModelNamesWithCollisionGeometry() const
Get the names of the links that are part of this joint group and also have geometry associated with t...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void update(bool force=false)
Update all transforms.
rviz_common::properties::Property * scene_category_
rviz_common::properties::EnumProperty * octree_coloring_property_
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
rviz_common::properties::StringProperty * robot_description_property_
void unsetGroupColor(rviz_default_plugins::robot::Robot *robot, const std::string &group_name)
rviz_common::properties::BoolProperty * scene_robot_collision_enabled_property_
rviz_common::properties::ColorProperty * attached_body_color_property_
void setLinkColor(const std::string &link_name, const QColor &color)
rviz_common::properties::EnumProperty * octree_render_property_
void sceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void save(rviz_common::Config config) const override
virtual void updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
void unsetLinkColor(const std::string &link_name)
void spawnBackgroundJob(const std::function< void()> &job)
std::deque< std::function< void()> > main_loop_jobs_
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
rviz_common::properties::FloatProperty * scene_alpha_property_
void setGroupColor(rviz_default_plugins::robot::Robot *robot, const std::string &group_name, const QColor &color)
void calculateOffsetPosition()
Set the scene node's position, given the target frame and the planning frame.
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
void addMainLoopJob(const std::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor()
rviz_common::properties::FloatProperty * robot_alpha_property_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
const moveit::core::RobotModelConstPtr & getRobotModel() const
void addBackgroundJob(const std::function< void()> &job, const std::string &name)
rviz_common::properties::StringProperty * move_group_ns_property_
rviz_common::properties::StringProperty * scene_name_property_
void update(float wall_dt, float ros_dt) override
PlanningSceneDisplay(bool listen_to_planning_scene=true, bool show_scene_robot=true)
rviz_common::properties::FloatProperty * scene_display_time_property_
virtual void onNewPlanningSceneState()
This is called upon successful retrieval of the (initial) planning scene state.
rviz_common::properties::BoolProperty * scene_robot_visual_enabled_property_
rviz_common::properties::Property * robot_category_
void unsetAllColors(rviz_default_plugins::robot::Robot *robot)
rviz_common::properties::RosTopicProperty * planning_scene_topic_property_
Ogre::SceneNode * planning_scene_node_
displays planning scene with everything in it
rviz_common::properties::BoolProperty * scene_enabled_property_
bool waitForCurrentRobotState(const rclcpp::Time &t)
wait for robot state more recent than t
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
rviz_common::properties::ColorProperty * scene_color_property_
moveit::tools::BackgroundProcessing background_process_
void load(const rviz_common::Config &config) override
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state.
robot_model_loader::RobotModelLoaderPtr getSharedRobotModelLoader(const rclcpp::Node::SharedPtr &node, const std::string &robot_description)
Main namespace for MoveIt.
std::string append(const std::string &left, const std::string &right)