planning_scene_display.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 
37 #pragma once
38 
39 #include <rviz/display.h>
40 
41 #ifndef Q_MOC_RUN
45 #include <ros/ros.h>
46 #endif
47 
48 namespace Ogre
49 {
50 class SceneNode;
51 }
52 
53 namespace rviz
54 {
55 class Robot;
56 class Property;
57 class StringProperty;
58 class BoolProperty;
59 class FloatProperty;
60 class RosTopicProperty;
61 class ColorProperty;
62 class EnumProperty;
63 } // namespace rviz
64 
65 namespace moveit_rviz_plugin
66 {
68 {
69  Q_OBJECT
70 
71 public:
72  PlanningSceneDisplay(bool listen_to_planning_scene = true, bool show_scene_robot = true);
73  ~PlanningSceneDisplay() override;
74 
75  void load(const rviz::Config& config) override;
76  void save(rviz::Config config) const override;
77 
78  void update(float wall_dt, float ros_dt) override;
79  void reset() override;
80 
81  void setLinkColor(const std::string& link_name, const QColor& color);
82  void unsetLinkColor(const std::string& link_name);
83 
85 
88  void addBackgroundJob(const boost::function<void()>& job, const std::string& name);
89 
94  void spawnBackgroundJob(const boost::function<void()>& job);
95 
97  void addMainLoopJob(const boost::function<void()>& job);
98 
100 
102  void clearJobs();
103 
104  const std::string getMoveGroupNS() const;
105  const moveit::core::RobotModelConstPtr& getRobotModel() const;
106 
113  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor();
114 
115 private Q_SLOTS:
116 
117  // ******************************************************************************************
118  // Slot Event Functions
119  // ******************************************************************************************
120  void changedMoveGroupNS();
122  void changedSceneName();
123  void changedSceneEnabled();
126  void changedRobotSceneAlpha();
127  void changedSceneAlpha();
128  void changedSceneColor();
131  void changedOctreeRendering();
132  void setSceneName(const QString& name);
133 
134 protected Q_SLOTS:
135  virtual void changedAttachedBodyColor();
136 
137 protected:
140  void loadRobotModel();
141 
144  virtual void clearRobotModel();
145 
148  virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor();
149 
151  virtual void onRobotModelLoaded();
153  virtual void onNewPlanningSceneState();
154 
159 
160  void executeMainLoopJobs();
161  void renderPlanningScene();
162  void setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color);
163  void unsetLinkColor(rviz::Robot* robot, const std::string& link_name);
164  void setGroupColor(rviz::Robot* robot, const std::string& group_name, const QColor& color);
165  void unsetGroupColor(rviz::Robot* robot, const std::string& group_name);
166  void unsetAllColors(rviz::Robot* robot);
167 
168  // overrides from Display
169  void onInitialize() override;
170  void onEnable() override;
171  void onDisable() override;
172  void fixedFrameChanged() override;
173 
174  // new virtual functions added by this plugin
175  virtual void updateInternal(float wall_dt, float ros_dt);
177 
178  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
180 
182  std::deque<boost::function<void()> > main_loop_jobs_;
183  boost::mutex main_loop_jobs_lock_;
184  boost::condition_variable main_loop_jobs_empty_condition_;
185 
186  Ogre::SceneNode* planning_scene_node_;
187 
188  // render the planning scene
189  RobotStateVisualizationPtr planning_scene_robot_;
190  PlanningSceneRenderPtr planning_scene_render_;
191 
192  // full update required
194  // or only the robot position (excluding attached object changes)
197 
200 
215 };
216 
217 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::PlanningSceneDisplay::move_group_ns_property_
rviz::StringProperty * move_group_ns_property_
Definition: planning_scene_display.h:201
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneRobotVisualEnabled
void changedSceneRobotVisualEnabled()
Definition: planning_scene_display.cpp:438
rviz::RosTopicProperty
moveit_rviz_plugin::PlanningSceneDisplay::onRobotModelLoaded
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
Definition: planning_scene_display.cpp:583
moveit_rviz_plugin::PlanningSceneDisplay::octree_coloring_property_
rviz::EnumProperty * octree_coloring_property_
Definition: planning_scene_display.h:214
Ogre
moveit_rviz_plugin::PlanningSceneDisplay::calculateOffsetPosition
void calculateOffsetPosition()
Set the scene node's position, given the target frame and the planning frame.
Definition: planning_scene_display.cpp:702
moveit_rviz_plugin::PlanningSceneDisplay::robot_description_property_
rviz::StringProperty * robot_description_property_
Definition: planning_scene_display.h:202
moveit_rviz_plugin::PlanningSceneDisplay::fixedFrameChanged
void fixedFrameChanged() override
Definition: planning_scene_display.cpp:716
moveit_rviz_plugin::PlanningSceneDisplay::robot_category_
rviz::Property * robot_category_
Definition: planning_scene_display.h:199
moveit_rviz_plugin::PlanningSceneDisplay::changedPlanningSceneTopic
void changedPlanningSceneTopic()
Definition: planning_scene_display.cpp:411
moveit_rviz_plugin::PlanningSceneDisplay::updateInternal
virtual void updateInternal(float wall_dt, float ros_dt)
Definition: planning_scene_display.cpp:675
moveit_rviz_plugin::PlanningSceneDisplay::scene_robot_collision_enabled_property_
rviz::BoolProperty * scene_robot_collision_enabled_property_
Definition: planning_scene_display.h:206
moveit_rviz_plugin::PlanningSceneDisplay::changedOctreeRendering
void changedOctreeRendering()
Definition: planning_scene_display.cpp:433
moveit_rviz_plugin::PlanningSceneDisplay::loadRobotModel
void loadRobotModel()
Definition: planning_scene_display.cpp:553
moveit_rviz_plugin::PlanningSceneDisplay::setSceneName
void setSceneName(const QString &name)
Definition: planning_scene_display.cpp:618
moveit_rviz_plugin::PlanningSceneDisplay::main_loop_jobs_lock_
boost::mutex main_loop_jobs_lock_
Definition: planning_scene_display.h:183
moveit_rviz_plugin::PlanningSceneDisplay::waitForCurrentRobotState
bool waitForCurrentRobotState(const ros::Time &t=ros::Time::now())
wait for robot state more recent than t
Definition: planning_scene_display.cpp:321
moveit_rviz_plugin::PlanningSceneDisplay::scene_color_property_
rviz::ColorProperty * scene_color_property_
Definition: planning_scene_display.h:210
ros.h
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneRobotCollisionEnabled
void changedSceneRobotCollisionEnabled()
Definition: planning_scene_display.cpp:448
moveit_rviz_plugin::PlanningSceneDisplay::onDisable
void onDisable() override
Definition: planning_scene_display.cpp:645
rviz::BoolProperty
moveit_rviz_plugin::PlanningSceneDisplay::scene_category_
rviz::Property * scene_category_
Definition: planning_scene_display.h:198
moveit_rviz_plugin::PlanningSceneDisplay::scene_name_property_
rviz::StringProperty * scene_name_property_
Definition: planning_scene_display.h:203
moveit_rviz_plugin::PlanningSceneDisplay::update
void update(float wall_dt, float ros_dt) override
Definition: planning_scene_display.cpp:663
moveit_rviz_plugin::PlanningSceneDisplay::~PlanningSceneDisplay
~PlanningSceneDisplay() override
Definition: planning_scene_display.cpp:199
moveit_rviz_plugin::PlanningSceneDisplay::PlanningSceneDisplay
PlanningSceneDisplay(bool listen_to_planning_scene=true, bool show_scene_robot=true)
Definition: planning_scene_display.cpp:101
moveit_rviz_plugin::PlanningSceneDisplay::octree_render_property_
rviz::EnumProperty * octree_render_property_
Definition: planning_scene_display.h:213
moveit_rviz_plugin::PlanningSceneDisplay::changedRobotDescription
void changedRobotDescription()
Definition: planning_scene_display.cpp:354
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneMonitor
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
Definition: planning_scene_display.cpp:300
moveit_rviz_plugin::PlanningSceneDisplay::robot_state_needs_render_
bool robot_state_needs_render_
Definition: planning_scene_display.h:195
display.h
moveit_rviz_plugin::PlanningSceneDisplay::clearRobotModel
virtual void clearRobotModel()
Definition: planning_scene_display.cpp:546
moveit_rviz_plugin::PlanningSceneDisplay::addMainLoopJob
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
Definition: planning_scene_display.cpp:265
moveit_rviz_plugin::PlanningSceneDisplay::getMoveGroupNS
const std::string getMoveGroupNS() const
Definition: planning_scene_display.cpp:305
moveit_rviz_plugin::PlanningSceneDisplay::waitForAllMainLoopJobs
void waitForAllMainLoopJobs()
Definition: planning_scene_display.cpp:271
rviz::ColorProperty
rviz::Display
rviz::EnumProperty
rviz::FloatProperty
moveit_rviz_plugin::PlanningSceneDisplay::scene_enabled_property_
rviz::BoolProperty * scene_enabled_property_
Definition: planning_scene_display.h:204
planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType
SceneUpdateType
rviz::Property
planning_scene_monitor::LockedPlanningSceneRW
moveit_rviz_plugin::PlanningSceneDisplay::onInitialize
void onInitialize() override
Definition: planning_scene_display.cpp:219
moveit_rviz_plugin::PlanningSceneDisplay::executeMainLoopJobs
void executeMainLoopJobs()
Definition: planning_scene_display.cpp:278
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRO
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
Definition: planning_scene_display.cpp:328
moveit_rviz_plugin::PlanningSceneDisplay::current_scene_time_
float current_scene_time_
Definition: planning_scene_display.h:196
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneAlpha
void changedSceneAlpha()
Definition: planning_scene_display.cpp:397
moveit_rviz_plugin::PlanningSceneDisplay::createPlanningSceneMonitor
virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor()
Definition: planning_scene_display.cpp:535
rviz
planning_scene_monitor.h
moveit_rviz_plugin::PlanningSceneDisplay::main_loop_jobs_
std::deque< boost::function< void()> > main_loop_jobs_
Definition: planning_scene_display.h:182
rviz::StringProperty
moveit_rviz_plugin::PlanningSceneDisplay::attached_body_color_property_
rviz::ColorProperty * attached_body_color_property_
Definition: planning_scene_display.h:211
moveit_rviz_plugin::PlanningSceneDisplay::unsetLinkColor
void unsetLinkColor(const std::string &link_name)
Definition: planning_scene_display.cpp:508
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneColor
void changedSceneColor()
Definition: planning_scene_display.cpp:343
moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_robot_
RobotStateVisualizationPtr planning_scene_robot_
Definition: planning_scene_display.h:189
moveit_rviz_plugin::PlanningSceneDisplay
Definition: planning_scene_display.h:67
moveit_rviz_plugin::PlanningSceneDisplay::unsetAllColors
void unsetAllColors(rviz::Robot *robot)
Definition: planning_scene_display.cpp:478
moveit_rviz_plugin::PlanningSceneDisplay::changedMoveGroupNS
void changedMoveGroupNS()
Definition: planning_scene_display.cpp:348
moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_node_
Ogre::SceneNode * planning_scene_node_
displays planning scene with everything in it
Definition: planning_scene_display.h:186
moveit_rviz_plugin::PlanningSceneDisplay::spawnBackgroundJob
void spawnBackgroundJob(const boost::function< void()> &job)
Definition: planning_scene_display.cpp:260
moveit_rviz_plugin::PlanningSceneDisplay::unsetGroupColor
void unsetGroupColor(rviz::Robot *robot, const std::string &group_name)
Definition: planning_scene_display.cpp:488
Robot
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneDisplayTime
void changedSceneDisplayTime()
Definition: planning_scene_display.cpp:429
name
name
moveit_rviz_plugin::PlanningSceneDisplay::addBackgroundJob
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
Definition: planning_scene_display.cpp:255
rviz::Robot
moveit_rviz_plugin::PlanningSceneDisplay::main_loop_jobs_empty_condition_
boost::condition_variable main_loop_jobs_empty_condition_
Definition: planning_scene_display.h:184
moveit_rviz_plugin::PlanningSceneDisplay::changedAttachedBodyColor
virtual void changedAttachedBodyColor()
Definition: planning_scene_display.cpp:338
moveit_rviz_plugin::PlanningSceneDisplay::robot_alpha_property_
rviz::FloatProperty * robot_alpha_property_
Definition: planning_scene_display.h:208
moveit_rviz_plugin::PlanningSceneDisplay::setGroupColor
void setGroupColor(rviz::Robot *robot, const std::string &group_name, const QColor &color)
Definition: planning_scene_display.cpp:464
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::PlanningSceneDisplay::background_process_
moveit::tools::BackgroundProcessing background_process_
Definition: planning_scene_display.h:181
ros::Time
background_processing.h
moveit_rviz_plugin::PlanningSceneDisplay::scene_alpha_property_
rviz::FloatProperty * scene_alpha_property_
Definition: planning_scene_display.h:209
moveit_rviz_plugin::PlanningSceneDisplay::reset
void reset() override
Definition: planning_scene_display.cpp:238
planning_scene_monitor::LockedPlanningSceneRO
moveit_rviz_plugin::PlanningSceneDisplay::clearJobs
void clearJobs()
remove all queued jobs
Definition: planning_scene_display.cpp:210
moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_monitor_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
Definition: planning_scene_display.h:178
moveit_rviz_plugin::PlanningSceneDisplay::scene_display_time_property_
rviz::FloatProperty * scene_display_time_property_
Definition: planning_scene_display.h:212
moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_render_
PlanningSceneRenderPtr planning_scene_render_
Definition: planning_scene_display.h:190
moveit_rviz_plugin::PlanningSceneDisplay::onNewPlanningSceneState
virtual void onNewPlanningSceneState()
This is called upon successful retrieval of the (initial) planning scene state.
Definition: planning_scene_display.cpp:605
moveit_rviz_plugin::PlanningSceneDisplay::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: planning_scene_display.cpp:310
moveit_rviz_plugin::PlanningSceneDisplay::robot_model_loading_lock_
boost::mutex robot_model_loading_lock_
Definition: planning_scene_display.h:179
moveit_rviz_plugin::PlanningSceneDisplay::scene_robot_visual_enabled_property_
rviz::BoolProperty * scene_robot_visual_enabled_property_
Definition: planning_scene_display.h:205
moveit_rviz_plugin::PlanningSceneDisplay::onSceneMonitorReceivedUpdate
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
Definition: planning_scene_display.cpp:609
moveit_rviz_plugin::PlanningSceneDisplay::renderPlanningScene
void renderPlanningScene()
Definition: planning_scene_display.cpp:367
moveit_rviz_plugin::PlanningSceneDisplay::onEnable
void onEnable() override
Definition: planning_scene_display.cpp:623
moveit::tools::BackgroundProcessing
planning_scene_render.h
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneEnabled
void changedSceneEnabled()
Definition: planning_scene_display.cpp:458
moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_topic_property_
rviz::RosTopicProperty * planning_scene_topic_property_
Definition: planning_scene_display.h:207
moveit_rviz_plugin::PlanningSceneDisplay::setLinkColor
void setLinkColor(const std::string &link_name, const QColor &color)
Definition: planning_scene_display.cpp:502
t
dictionary t
moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_needs_render_
bool planning_scene_needs_render_
Definition: planning_scene_display.h:193
config
config
moveit_rviz_plugin::PlanningSceneDisplay::save
void save(rviz::Config config) const override
Definition: planning_scene_display.cpp:694
moveit_rviz_plugin::PlanningSceneDisplay::load
void load(const rviz::Config &config) override
Definition: planning_scene_display.cpp:689
rviz::Config
moveit_rviz_plugin::PlanningSceneDisplay::changedRobotSceneAlpha
void changedRobotSceneAlpha()
Definition: planning_scene_display.cpp:402
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRW
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
Definition: planning_scene_display.cpp:333
ros::Time::now
static Time now()
moveit_rviz_plugin::PlanningSceneDisplay::queueRenderSceneGeometry
void queueRenderSceneGeometry()
Definition: planning_scene_display.cpp:658
moveit_rviz_plugin::PlanningSceneDisplay::changedSceneName
void changedSceneName()
Definition: planning_scene_display.cpp:360


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:26