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>
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 <tf2_ros/buffer.h>
55#include <OgreSceneManager.h>
56#include <OgreSceneNode.h>
68 , planning_scene_needs_render_(true)
69 , current_scene_time_(0.0f)
70 , logger_(
moveit::getLogger(
"moveit.ros.planning_scene_display"))
73 "The name of the ROS namespace in "
74 "which the move_group node is running",
75 this, SLOT(changedMoveGroupNS()),
this);
77 "Robot Description",
"robot_description",
"The name of the ROS parameter where the URDF for the robot is loaded",
78 this, SLOT(changedRobotDescription()),
this);
80 if (listen_to_planning_scene)
83 "Planning Scene Topic",
"/monitored_planning_scene",
84 rosidl_generator_traits::data_type<moveit_msgs::msg::PlanningScene>(),
85 "The topic on which the moveit_msgs::msg::PlanningScene messages are received",
this,
86 SLOT(changedPlanningSceneTopic()),
this);
94 scene_category_ =
new rviz_common::properties::Property(
"Scene Geometry", QVariant(),
"",
this);
97 new rviz_common::properties::StringProperty(
"Scene Name",
"(noname)",
"Shows the name of the planning scene",
101 new rviz_common::properties::BoolProperty(
"Show Scene Geometry",
true,
102 "Indicates whether planning scenes should be displayed",
106 new rviz_common::properties::FloatProperty(
"Scene Alpha", 0.9f,
"Specifies the alpha for the scene geometry",
112 "Scene Color", QColor(50, 230, 50),
"The color for the planning scene obstacles (if a color is not defined)",
116 new rviz_common::properties::EnumProperty(
"Voxel Rendering",
"Occupied Voxels",
"Select voxel type.",
125 "Voxel Coloring",
"Z-Axis",
"Select voxel coloring mode",
scene_category_, SLOT(changedOctreeColorMode()),
this);
131 new rviz_common::properties::FloatProperty(
"Scene Display Time", 0.01f,
132 "The amount of wall-time to wait in between rendering "
133 "updates to the planning scene (if any)",
137 if (show_scene_robot)
139 robot_category_ =
new rviz_common::properties::Property(
"Scene Robot", QVariant(),
"",
this);
142 "Show Robot Visual",
true,
143 "Indicates whether the robot state specified by the planning scene should be "
144 "displayed as defined for visualisation purposes.",
148 "Show Robot Collision",
false,
149 "Indicates whether the robot state specified by the planning scene should be "
150 "displayed as defined for collision detection purposes.",
154 new rviz_common::properties::FloatProperty(
"Robot Alpha", 1.0f,
"Specifies the alpha for the robot links",
160 new rviz_common::properties::ColorProperty(
"Attached Body Color", QColor(150, 50, 150),
199 Display::onInitialize();
200 auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
201 if (!ros_node_abstraction)
203 RVIZ_COMMON_LOG_WARNING(
"Unable to lock weak_ptr from DisplayContext in PlanningSceneDisplay constructor");
206 node_ = ros_node_abstraction->get_raw_node();
219 changedRobotSceneAlpha();
277 catch (std::exception& ex)
279 RCLCPP_ERROR(
logger_,
"Exception caught executing main loop job: %s", ex.what());
305 static moveit::core::RobotModelConstPtr empty;
332void PlanningSceneDisplay::changedSceneColor()
337void PlanningSceneDisplay::changedMoveGroupNS()
343void PlanningSceneDisplay::changedRobotDescription()
349void PlanningSceneDisplay::changedSceneName()
359 Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF());
362 Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF());
379 catch (std::exception& ex)
381 RCLCPP_ERROR(
logger_,
"Caught %s while rendering planning scene", ex.what());
386void PlanningSceneDisplay::changedSceneAlpha()
391void PlanningSceneDisplay::changedRobotSceneAlpha()
400void PlanningSceneDisplay::changedPlanningSceneTopic()
408 auto bg_func = [=]() {
415 setStatus(rviz_common::properties::StatusProperty::Warn,
"PlanningScene",
"Requesting initial scene failed");
422void PlanningSceneDisplay::changedSceneDisplayTime()
426void PlanningSceneDisplay::changedOctreeRenderMode()
430void PlanningSceneDisplay::changedOctreeColorMode()
434void PlanningSceneDisplay::changedSceneRobotVisualEnabled()
444void PlanningSceneDisplay::changedSceneRobotCollisionEnabled()
454void PlanningSceneDisplay::changedSceneEnabled()
469 for (
const std::string& link : links)
479 const std::vector<std::string>& links =
getRobotModel()->getLinkModelNamesWithCollisionGeometry();
480 for (
const std::string& link : links)
493 for (
const std::string& link : links)
514 rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
518 link->setColor(color.redF(), color.greenF(), color.blueF());
523 rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
535 return std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
node_, rml,
536 getNameStd() +
"_planning_scene_monitor");
559 if (psm->getPlanningScene())
572 setStatus(rviz_common::properties::StatusProperty::Error,
"PlanningScene",
"No Planning Scene Loaded");
580 changedPlanningSceneTopic();
597 setStatus(rviz_common::properties::StatusProperty::Ok,
"PlanningScene",
"Planning Scene Loaded Successfully");
614 QMetaObject::invokeMethod(
this,
"setSceneName", Qt::QueuedConnection,
619void PlanningSceneDisplay::setSceneName(
const QString& name)
656 Display::onDisable();
666 Display::update(wall_dt, ros_dt);
692 Display::load(config);
697 Display::save(config);
708 Ogre::Vector3 position;
709 Ogre::Quaternion orientation;
711 context_->getFrameManager()->getTransform(
getRobotModel()->getModelFrame(), rclcpp::Time(0, 0, RCL_ROS_TIME),
712 position, orientation);
720 Display::fixedFrameChanged();
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_
double current_scene_time_
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_
rclcpp::Node::SharedPtr node_
void fixedFrameChanged() override
rviz_common::properties::ColorProperty * attached_body_color_property_
void setLinkColor(const std::string &link_name, const QColor &color)
const std::string getMoveGroupNS() const
rviz_common::properties::EnumProperty * octree_render_property_
void onInitialize() override
void sceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void renderPlanningScene()
void save(rviz_common::Config config) const override
virtual void clearRobotModel()
virtual void updateInternal(double wall_dt, double ros_dt)
void waitForAllMainLoopJobs()
PlanningSceneRenderPtr planning_scene_render_
void unsetLinkColor(const std::string &link_name)
void spawnBackgroundJob(const std::function< void()> &job)
std::deque< std::function< void()> > main_loop_jobs_
std::mutex main_loop_jobs_lock_
void onDisable() override
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
rviz_common::properties::FloatProperty * scene_alpha_property_
bool planning_scene_needs_render_
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.
void executeMainLoopJobs()
~PlanningSceneDisplay() override
RobotStateVisualizationPtr planning_scene_robot_
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_
void queueRenderSceneGeometry()
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.
void clearJobs()
remove all queued jobs
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()
std::mutex robot_model_loading_lock_
rviz_common::properties::ColorProperty * scene_color_property_
bool robot_state_needs_render_
moveit::tools::BackgroundProcessing background_process_
std::condition_variable main_loop_jobs_empty_condition_
virtual void changedAttachedBodyColor()
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)
@ OCTOMAP_PROBABLILTY_COLOR
@ OCTOMAP_OCCUPIED_VOXELS
Main namespace for MoveIt.
std::string append(const std::string &left, const std::string &right)