moveit2
The MoveIt Motion Planning Framework for ROS 2.
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_common/display.hpp>
40 #include <rviz_default_plugins/robot/robot.hpp>
41 #include <rviz_common/properties/string_property.hpp>
42 #include <rviz_common/properties/ros_topic_property.hpp>
43 #ifndef Q_MOC_RUN
47 #include <rclcpp/rclcpp.hpp>
48 #endif
49 
50 #include <moveit_planning_scene_rviz_plugin_core_export.h>
51 
52 namespace Ogre
53 {
54 class SceneNode;
55 }
56 
57 namespace rviz
58 {
59 class Robot;
60 class Property;
61 class StringProperty;
62 class BoolProperty;
63 class FloatProperty;
64 class RosTopicProperty;
65 class ColorProperty;
66 class EnumProperty;
67 } // namespace rviz
68 
69 namespace moveit_rviz_plugin
70 {
71 class MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT PlanningSceneDisplay : public rviz_common::Display
72 {
73  Q_OBJECT
74 
75 public:
76  PlanningSceneDisplay(bool listen_to_planning_scene = true, bool show_scene_robot = true);
77  ~PlanningSceneDisplay() override;
78 
79  void load(const rviz_common::Config& config) override;
80  void save(rviz_common::Config config) const override;
81 
82  void update(float wall_dt, float ros_dt) override;
83  void reset() override;
84 
85  void setLinkColor(const std::string& link_name, const QColor& color);
86  void unsetLinkColor(const std::string& link_name);
87 
88  void queueRenderSceneGeometry();
89 
92  void addBackgroundJob(const std::function<void()>& job, const std::string& name);
93 
98  void spawnBackgroundJob(const std::function<void()>& job);
99 
101  void addMainLoopJob(const std::function<void()>& job);
102 
103  void waitForAllMainLoopJobs();
104 
106  void clearJobs();
107 
108  const std::string getMoveGroupNS() const;
109  const moveit::core::RobotModelConstPtr& getRobotModel() const;
110 
112  bool waitForCurrentRobotState(const rclcpp::Time& t);
114  planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const;
117  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor();
118 
119 private Q_SLOTS:
120 
121  // ******************************************************************************************
122  // Slot Event Functions
123  // ******************************************************************************************
124  void changedMoveGroupNS();
125  void changedRobotDescription();
126  void changedSceneName();
127  void changedSceneEnabled();
128  void changedSceneRobotVisualEnabled();
129  void changedSceneRobotCollisionEnabled();
130  void changedRobotSceneAlpha();
131  void changedSceneAlpha();
132  void changedSceneColor();
133  void changedPlanningSceneTopic();
134  void changedSceneDisplayTime();
135  void changedOctreeRenderMode();
136  void changedOctreeColorMode();
137  void setSceneName(const QString& name);
138 
139 protected Q_SLOTS:
140  virtual void changedAttachedBodyColor();
141 
142 protected:
145  void loadRobotModel();
146 
149  virtual void clearRobotModel();
150 
153  virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor();
154 
156  virtual void onRobotModelLoaded();
158  virtual void onNewPlanningSceneState();
159 
163  void calculateOffsetPosition();
164 
165  void executeMainLoopJobs();
166  void sceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
167  void renderPlanningScene();
168  void setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name, const QColor& color);
169  void unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name);
170  void setGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name, const QColor& color);
171  void unsetGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name);
172  void unsetAllColors(rviz_default_plugins::robot::Robot* robot);
173 
174  // overrides from Display
175  void onInitialize() override;
176  void onEnable() override;
177  void onDisable() override;
178  void fixedFrameChanged() override;
179 
180  // new virtual functions added by this plugin
181  virtual void updateInternal(float wall_dt, float ros_dt);
182  virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
183 
184  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
186 
188  std::deque<std::function<void()> > main_loop_jobs_;
190  std::condition_variable main_loop_jobs_empty_condition_;
191 
192  Ogre::SceneNode* planning_scene_node_;
193 
194  // render the planning scene
195  RobotStateVisualizationPtr planning_scene_robot_;
196  PlanningSceneRenderPtr planning_scene_render_;
197 
198  // full update required
200  // or only the robot position (excluding attached object changes)
203 
204  rviz_common::properties::Property* scene_category_;
205  rviz_common::properties::Property* robot_category_;
206 
207  rviz_common::properties::StringProperty* move_group_ns_property_;
208  rviz_common::properties::StringProperty* robot_description_property_;
209  rviz_common::properties::StringProperty* scene_name_property_;
210  rviz_common::properties::BoolProperty* scene_enabled_property_;
211  rviz_common::properties::BoolProperty* scene_robot_visual_enabled_property_;
212  rviz_common::properties::BoolProperty* scene_robot_collision_enabled_property_;
213  rviz_common::properties::RosTopicProperty* planning_scene_topic_property_;
214  rviz_common::properties::FloatProperty* robot_alpha_property_;
215  rviz_common::properties::FloatProperty* scene_alpha_property_;
216  rviz_common::properties::ColorProperty* scene_color_property_;
217  rviz_common::properties::ColorProperty* attached_body_color_property_;
218  rviz_common::properties::FloatProperty* scene_display_time_property_;
219  rviz_common::properties::EnumProperty* octree_render_property_;
220  rviz_common::properties::EnumProperty* octree_coloring_property_;
221 
222  // rclcpp node
223  rclcpp::Node::SharedPtr node_;
224 };
225 
226 } // namespace moveit_rviz_plugin
This class provides simple API for executing background jobs. A queue of jobs is created and the spec...
rviz_common::properties::Property * scene_category_
rviz_common::properties::EnumProperty * octree_coloring_property_
rviz_common::properties::StringProperty * robot_description_property_
rviz_common::properties::BoolProperty * scene_robot_collision_enabled_property_
rviz_common::properties::ColorProperty * attached_body_color_property_
rviz_common::properties::EnumProperty * octree_render_property_
std::deque< std::function< void()> > main_loop_jobs_
rviz_common::properties::FloatProperty * scene_alpha_property_
rviz_common::properties::FloatProperty * robot_alpha_property_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
rviz_common::properties::StringProperty * move_group_ns_property_
rviz_common::properties::StringProperty * scene_name_property_
rviz_common::properties::FloatProperty * scene_display_time_property_
rviz_common::properties::BoolProperty * scene_robot_visual_enabled_property_
rviz_common::properties::Property * robot_category_
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_
rviz_common::properties::ColorProperty * scene_color_property_
moveit::tools::BackgroundProcessing background_process_
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.
robot
Definition: pick.py:53
name
Definition: setup.py:7