58 #include <OgreSceneManager.h>
59 #include <OgreSceneNode.h>
69 : Display(), planning_scene_needs_render_(true), current_scene_time_(0.0
f)
72 "The name of the ROS namespace in "
73 "which the move_group node is running",
74 this, SLOT(changedMoveGroupNS()),
this);
75 robot_description_property_ =
77 "The name of the ROS parameter where the URDF for the robot is loaded",
this,
78 SLOT(changedRobotDescription()),
this);
80 if (listen_to_planning_scene)
81 planning_scene_topic_property_ =
83 ros::message_traits::datatype<moveit_msgs::PlanningScene>(),
84 "The topic on which the moveit_msgs::PlanningScene messages are received",
this,
85 SLOT(changedPlanningSceneTopic()),
this);
87 planning_scene_topic_property_ =
nullptr;
90 scene_category_ =
new rviz::Property(
"Scene Geometry", QVariant(),
"",
this);
92 scene_name_property_ =
new rviz::StringProperty(
"Scene Name",
"(noname)",
"Shows the name of the planning scene",
93 scene_category_, SLOT(changedSceneName()),
this);
94 scene_name_property_->setShouldBeSaved(
false);
95 scene_enabled_property_ =
96 new rviz::BoolProperty(
"Show Scene Geometry",
true,
"Indicates whether planning scenes should be displayed",
97 scene_category_, SLOT(changedSceneEnabled()),
this);
99 scene_alpha_property_ =
new rviz::FloatProperty(
"Scene Alpha", 1.0f,
"Specifies the alpha for the scene geometry",
100 scene_category_, SLOT(changedSceneAlpha()),
this);
101 scene_alpha_property_->setMin(0.0);
102 scene_alpha_property_->setMax(1.0);
104 scene_color_property_ =
106 "The color for the planning scene obstacles (if a color is not defined)", scene_category_,
107 SLOT(changedSceneColor()),
this);
109 octree_render_property_ =
new rviz::EnumProperty(
"Voxel Rendering",
"Occupied Voxels",
"Select voxel type.",
110 scene_category_, SLOT(changedOctreeRendering()),
this);
116 octree_coloring_property_ =
new rviz::EnumProperty(
"Voxel Coloring",
"Z-Axis",
"Select voxel coloring mode",
117 scene_category_, SLOT(changedOctreeRendering()),
this);
123 "The amount of wall-time to wait in between rendering "
124 "updates to the planning scene (if any)",
125 scene_category_, SLOT(changedSceneDisplayTime()),
this);
126 scene_display_time_property_->setMin(0.0001);
128 if (show_scene_robot)
130 robot_category_ =
new rviz::Property(
"Scene Robot", QVariant(),
"",
this);
132 scene_robot_visual_enabled_property_ =
134 "Indicates whether the robot state specified by the planning scene should be "
135 "displayed as defined for visualisation purposes.",
136 robot_category_, SLOT(changedSceneRobotVisualEnabled()),
this);
138 scene_robot_collision_enabled_property_ =
140 "Indicates whether the robot state specified by the planning scene should be "
141 "displayed as defined for collision detection purposes.",
142 robot_category_, SLOT(changedSceneRobotCollisionEnabled()),
this);
144 robot_alpha_property_ =
new rviz::FloatProperty(
"Robot Alpha", 1.0f,
"Specifies the alpha for the robot links",
145 robot_category_, SLOT(changedRobotSceneAlpha()),
this);
146 robot_alpha_property_->setMin(0.0);
147 robot_alpha_property_->setMax(1.0);
149 attached_body_color_property_ =
150 new rviz::ColorProperty(
"Attached Body Color", QColor(150, 50, 150),
"The color for the attached bodies",
151 robot_category_, SLOT(changedAttachedBodyColor()),
this);
155 robot_category_ =
nullptr;
156 scene_robot_visual_enabled_property_ =
nullptr;
157 scene_robot_collision_enabled_property_ =
nullptr;
158 robot_alpha_property_ =
nullptr;
159 attached_body_color_property_ =
nullptr;
188 Display::onInitialize();
229 boost::thread
t(job);
257 catch (std::exception& ex)
259 ROS_ERROR(
"Exception caught executing main loop job: %s", ex.what());
283 static moveit::core::RobotModelConstPtr empty;
337 rviz::Color env_color(color.redF(), color.greenF(), color.blueF());
340 rviz::Color attached_color(color.redF(), color.greenF(), color.blueF());
357 catch (std::exception& ex)
359 ROS_ERROR(
"Caught %s while rendering planning scene", ex.what());
386 auto bg_func = [=]() {
439 for (
const std::string& link : links)
449 const std::vector<std::string>& links =
getRobotModel()->getLinkModelNamesWithCollisionGeometry();
450 for (
const std::string& link : links)
463 for (
const std::string& link : links)
487 link->
setColor(color.redF(), color.greenF(), color.blueF());
509 return std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
533 if (psm->getPlanningScene())
580 QMetaObject::invokeMethod(
this,
"setSceneName", Qt::QueuedConnection,
622 Display::onDisable();
632 Display::update(wall_dt, ros_dt);
658 Display::load(config);
663 Display::save(config);
674 Ogre::Vector3 position;
675 Ogre::Quaternion orientation;
685 Display::fixedFrameChanged();