moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
Public Member Functions | Protected Slots | Protected Member Functions | Protected Attributes | List of all members
moveit_rviz_plugin::PlanningSceneDisplay Class Reference

#include <planning_scene_display.hpp>

Inheritance diagram for moveit_rviz_plugin::PlanningSceneDisplay:
Inheritance graph
[legend]
Collaboration diagram for moveit_rviz_plugin::PlanningSceneDisplay:
Collaboration graph
[legend]

Public Member Functions

 PlanningSceneDisplay (bool listen_to_planning_scene=true, bool show_scene_robot=true)
 
 ~PlanningSceneDisplay () override
 
void load (const rviz_common::Config &config) override
 
void save (rviz_common::Config config) const override
 
void update (float wall_dt, float ros_dt) override
 
void reset () override
 
void setLinkColor (const std::string &link_name, const QColor &color)
 
void unsetLinkColor (const std::string &link_name)
 
void queueRenderSceneGeometry ()
 
void addBackgroundJob (const std::function< void()> &job, const std::string &name)
 
void spawnBackgroundJob (const std::function< void()> &job)
 
void addMainLoopJob (const std::function< void()> &job)
 queue the execution of this function for the next time the main update() loop gets called
 
void waitForAllMainLoopJobs ()
 
void clearJobs ()
 remove all queued jobs
 
const std::string getMoveGroupNS () const
 
const moveit::core::RobotModelConstPtr & getRobotModel () const
 
bool waitForCurrentRobotState (const rclcpp::Time &t)
 wait for robot state more recent than t
 
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO () const
 get read-only access to planning scene
 
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW ()
 get write access to planning scene
 
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor ()
 

Protected Slots

virtual void changedAttachedBodyColor ()
 

Protected Member Functions

void loadRobotModel ()
 
virtual void clearRobotModel ()
 
virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor ()
 
virtual void onRobotModelLoaded ()
 This is an event called by loadRobotModel() in the MainLoop; do not call directly.
 
virtual void onNewPlanningSceneState ()
 This is called upon successful retrieval of the (initial) planning scene state.
 
void calculateOffsetPosition ()
 Set the scene node's position, given the target frame and the planning frame.
 
void executeMainLoopJobs ()
 
void sceneMonitorReceivedUpdate (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
 
void renderPlanningScene ()
 
void setLinkColor (rviz_default_plugins::robot::Robot *robot, const std::string &link_name, const QColor &color)
 
void unsetLinkColor (rviz_default_plugins::robot::Robot *robot, const std::string &link_name)
 
void setGroupColor (rviz_default_plugins::robot::Robot *robot, const std::string &group_name, const QColor &color)
 
void unsetGroupColor (rviz_default_plugins::robot::Robot *robot, const std::string &group_name)
 
void unsetAllColors (rviz_default_plugins::robot::Robot *robot)
 
void onInitialize () override
 
void onEnable () override
 
void onDisable () override
 
void fixedFrameChanged () override
 
virtual void updateInternal (std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
 
virtual void updateInternal (double wall_dt, double ros_dt)
 
virtual void onSceneMonitorReceivedUpdate (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
 

Protected Attributes

planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
 
std::mutex robot_model_loading_lock_
 
moveit::tools::BackgroundProcessing background_process_
 
std::deque< std::function< void()> > main_loop_jobs_
 
std::mutex main_loop_jobs_lock_
 
std::condition_variable main_loop_jobs_empty_condition_
 
Ogre::SceneNode * planning_scene_node_
 displays planning scene with everything in it
 
RobotStateVisualizationPtr planning_scene_robot_
 
PlanningSceneRenderPtr planning_scene_render_
 
bool planning_scene_needs_render_
 
bool robot_state_needs_render_
 
double current_scene_time_
 
rviz_common::properties::Property * scene_category_
 
rviz_common::properties::Property * robot_category_
 
rviz_common::properties::StringProperty * move_group_ns_property_
 
rviz_common::properties::StringProperty * robot_description_property_
 
rviz_common::properties::StringProperty * scene_name_property_
 
rviz_common::properties::BoolProperty * scene_enabled_property_
 
rviz_common::properties::BoolProperty * scene_robot_visual_enabled_property_
 
rviz_common::properties::BoolProperty * scene_robot_collision_enabled_property_
 
rviz_common::properties::RosTopicProperty * planning_scene_topic_property_
 
rviz_common::properties::FloatProperty * robot_alpha_property_
 
rviz_common::properties::FloatProperty * scene_alpha_property_
 
rviz_common::properties::ColorProperty * scene_color_property_
 
rviz_common::properties::ColorProperty * attached_body_color_property_
 
rviz_common::properties::FloatProperty * scene_display_time_property_
 
rviz_common::properties::EnumProperty * octree_render_property_
 
rviz_common::properties::EnumProperty * octree_coloring_property_
 
rclcpp::Node::SharedPtr node_
 
rclcpp::Logger logger_
 

Detailed Description

Definition at line 75 of file planning_scene_display.hpp.

Constructor & Destructor Documentation

◆ PlanningSceneDisplay()

moveit_rviz_plugin::PlanningSceneDisplay::PlanningSceneDisplay ( bool  listen_to_planning_scene = true,
bool  show_scene_robot = true 
)

Definition at line 74 of file planning_scene_display.cpp.

Here is the call graph for this function:

◆ ~PlanningSceneDisplay()

moveit_rviz_plugin::PlanningSceneDisplay::~PlanningSceneDisplay ( )
override

Definition at line 185 of file planning_scene_display.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ addBackgroundJob()

void moveit_rviz_plugin::PlanningSceneDisplay::addBackgroundJob ( const std::function< void()> &  job,
const std::string &  name 
)

Queue this function call for execution within the background thread All jobs are queued and processed in order by a single background thread.

Definition at line 249 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addMainLoopJob()

void moveit_rviz_plugin::PlanningSceneDisplay::addMainLoopJob ( const std::function< void()> &  job)

queue the execution of this function for the next time the main update() loop gets called

Definition at line 260 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ calculateOffsetPosition()

void moveit_rviz_plugin::PlanningSceneDisplay::calculateOffsetPosition ( )
protected

Set the scene node's position, given the target frame and the planning frame.

Definition at line 732 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ changedAttachedBodyColor

void moveit_rviz_plugin::PlanningSceneDisplay::changedAttachedBodyColor ( )
protectedvirtualslot

Definition at line 335 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ clearJobs()

void moveit_rviz_plugin::PlanningSceneDisplay::clearJobs ( )

remove all queued jobs

Definition at line 196 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ clearRobotModel()

void moveit_rviz_plugin::PlanningSceneDisplay::clearRobotModel ( )
protectedvirtual

This function is used by loadRobotModel() and should only be called in the MainLoop You probably should not call this function directly

Reimplemented in moveit_rviz_plugin::MotionPlanningDisplay.

Definition at line 547 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ createPlanningSceneMonitor()

planning_scene_monitor::PlanningSceneMonitorPtr moveit_rviz_plugin::PlanningSceneDisplay::createPlanningSceneMonitor ( )
protectedvirtual

This function constructs a new planning scene. Probably this should be called in a background thread as it may take some time to complete its execution

Definition at line 540 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ executeMainLoopJobs()

void moveit_rviz_plugin::PlanningSceneDisplay::executeMainLoopJobs ( )
protected

Definition at line 273 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ fixedFrameChanged()

void moveit_rviz_plugin::PlanningSceneDisplay::fixedFrameChanged ( )
overrideprotected

Definition at line 747 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getMoveGroupNS()

const std::string moveit_rviz_plugin::PlanningSceneDisplay::getMoveGroupNS ( ) const

Definition at line 300 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ getPlanningSceneMonitor()

const planning_scene_monitor::PlanningSceneMonitorPtr & moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneMonitor ( )

Definition at line 295 of file planning_scene_display.cpp.

◆ getPlanningSceneRO()

planning_scene_monitor::LockedPlanningSceneRO moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRO ( ) const

get read-only access to planning scene

Definition at line 325 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ getPlanningSceneRW()

planning_scene_monitor::LockedPlanningSceneRW moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRW ( )

get write access to planning scene

Definition at line 330 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ getRobotModel()

const moveit::core::RobotModelConstPtr & moveit_rviz_plugin::PlanningSceneDisplay::getRobotModel ( ) const

Definition at line 305 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ load()

void moveit_rviz_plugin::PlanningSceneDisplay::load ( const rviz_common::Config &  config)
override

Definition at line 719 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ loadRobotModel()

void moveit_rviz_plugin::PlanningSceneDisplay::loadRobotModel ( )
protected

This function reloads the robot model and reinitializes the PlanningSceneMonitor It can be called either from the Main Loop or from a Background thread

Definition at line 554 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onDisable()

void moveit_rviz_plugin::PlanningSceneDisplay::onDisable ( )
overrideprotected

Definition at line 654 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ onEnable()

void moveit_rviz_plugin::PlanningSceneDisplay::onEnable ( )
overrideprotected

Definition at line 632 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onInitialize()

void moveit_rviz_plugin::PlanningSceneDisplay::onInitialize ( )
overrideprotected

Definition at line 205 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onNewPlanningSceneState()

void moveit_rviz_plugin::PlanningSceneDisplay::onNewPlanningSceneState ( )
protectedvirtual

This is called upon successful retrieval of the (initial) planning scene state.

Reimplemented in moveit_rviz_plugin::MotionPlanningDisplay.

Definition at line 608 of file planning_scene_display.cpp.

◆ onRobotModelLoaded()

void moveit_rviz_plugin::PlanningSceneDisplay::onRobotModelLoaded ( )
protectedvirtual

This is an event called by loadRobotModel() in the MainLoop; do not call directly.

Reimplemented in moveit_rviz_plugin::MotionPlanningDisplay.

Definition at line 586 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onSceneMonitorReceivedUpdate()

void moveit_rviz_plugin::PlanningSceneDisplay::onSceneMonitorReceivedUpdate ( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType  update_type)
protectedvirtual

Reimplemented in moveit_rviz_plugin::MotionPlanningDisplay.

Definition at line 618 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ queueRenderSceneGeometry()

void moveit_rviz_plugin::PlanningSceneDisplay::queueRenderSceneGeometry ( )

Definition at line 667 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ renderPlanningScene()

void moveit_rviz_plugin::PlanningSceneDisplay::renderPlanningScene ( )
protected

Definition at line 364 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset()

void moveit_rviz_plugin::PlanningSceneDisplay::reset ( )
override

Definition at line 232 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ save()

void moveit_rviz_plugin::PlanningSceneDisplay::save ( rviz_common::Config  config) const
override

Definition at line 724 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ sceneMonitorReceivedUpdate()

void moveit_rviz_plugin::PlanningSceneDisplay::sceneMonitorReceivedUpdate ( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType  update_type)
protected

Definition at line 612 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setGroupColor()

void moveit_rviz_plugin::PlanningSceneDisplay::setGroupColor ( rviz_default_plugins::robot::Robot *  robot,
const std::string &  group_name,
const QColor &  color 
)
protected

Definition at line 468 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setLinkColor() [1/2]

void moveit_rviz_plugin::PlanningSceneDisplay::setLinkColor ( const std::string &  link_name,
const QColor &  color 
)

Definition at line 507 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setLinkColor() [2/2]

void moveit_rviz_plugin::PlanningSceneDisplay::setLinkColor ( rviz_default_plugins::robot::Robot *  robot,
const std::string &  link_name,
const QColor &  color 
)
protected

Definition at line 519 of file planning_scene_display.cpp.

◆ spawnBackgroundJob()

void moveit_rviz_plugin::PlanningSceneDisplay::spawnBackgroundJob ( const std::function< void()> &  job)

Directly spawn a (detached) background thread for execution of this function call Should be used, when order of processing is not relevant / job can run in parallel. Must be used, when job will be blocking. Using addBackgroundJob() in this case will block other queued jobs as well

Definition at line 254 of file planning_scene_display.cpp.

◆ unsetAllColors()

void moveit_rviz_plugin::PlanningSceneDisplay::unsetAllColors ( rviz_default_plugins::robot::Robot *  robot)
protected

Definition at line 483 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ unsetGroupColor()

void moveit_rviz_plugin::PlanningSceneDisplay::unsetGroupColor ( rviz_default_plugins::robot::Robot *  robot,
const std::string &  group_name 
)
protected

Definition at line 493 of file planning_scene_display.cpp.

Here is the call graph for this function:

◆ unsetLinkColor() [1/2]

void moveit_rviz_plugin::PlanningSceneDisplay::unsetLinkColor ( const std::string &  link_name)

Definition at line 513 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ unsetLinkColor() [2/2]

void moveit_rviz_plugin::PlanningSceneDisplay::unsetLinkColor ( rviz_default_plugins::robot::Robot *  robot,
const std::string &  link_name 
)
protected

Definition at line 529 of file planning_scene_display.cpp.

◆ update()

void moveit_rviz_plugin::PlanningSceneDisplay::update ( float  wall_dt,
float  ros_dt 
)
override

Definition at line 687 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ updateInternal() [1/2]

void moveit_rviz_plugin::PlanningSceneDisplay::updateInternal ( double  wall_dt,
double  ros_dt 
)
protectedvirtual

Reimplemented in moveit_rviz_plugin::MotionPlanningDisplay.

Definition at line 714 of file planning_scene_display.cpp.

Here is the call graph for this function:

◆ updateInternal() [2/2]

void moveit_rviz_plugin::PlanningSceneDisplay::updateInternal ( std::chrono::nanoseconds  wall_dt,
std::chrono::nanoseconds  ros_dt 
)
protectedvirtual

Reimplemented in moveit_rviz_plugin::MotionPlanningDisplay.

Definition at line 700 of file planning_scene_display.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ waitForAllMainLoopJobs()

void moveit_rviz_plugin::PlanningSceneDisplay::waitForAllMainLoopJobs ( )

Definition at line 266 of file planning_scene_display.cpp.

Here is the caller graph for this function:

◆ waitForCurrentRobotState()

bool moveit_rviz_plugin::PlanningSceneDisplay::waitForCurrentRobotState ( const rclcpp::Time &  t)

wait for robot state more recent than t

Definition at line 318 of file planning_scene_display.cpp.

Member Data Documentation

◆ attached_body_color_property_

rviz_common::properties::ColorProperty* moveit_rviz_plugin::PlanningSceneDisplay::attached_body_color_property_
protected

Definition at line 238 of file planning_scene_display.hpp.

◆ background_process_

moveit::tools::BackgroundProcessing moveit_rviz_plugin::PlanningSceneDisplay::background_process_
protected

Definition at line 208 of file planning_scene_display.hpp.

◆ current_scene_time_

double moveit_rviz_plugin::PlanningSceneDisplay::current_scene_time_
protected

Definition at line 223 of file planning_scene_display.hpp.

◆ logger_

rclcpp::Logger moveit_rviz_plugin::PlanningSceneDisplay::logger_
protected

Definition at line 245 of file planning_scene_display.hpp.

◆ main_loop_jobs_

std::deque<std::function<void()> > moveit_rviz_plugin::PlanningSceneDisplay::main_loop_jobs_
protected

Definition at line 209 of file planning_scene_display.hpp.

◆ main_loop_jobs_empty_condition_

std::condition_variable moveit_rviz_plugin::PlanningSceneDisplay::main_loop_jobs_empty_condition_
protected

Definition at line 211 of file planning_scene_display.hpp.

◆ main_loop_jobs_lock_

std::mutex moveit_rviz_plugin::PlanningSceneDisplay::main_loop_jobs_lock_
protected

Definition at line 210 of file planning_scene_display.hpp.

◆ move_group_ns_property_

rviz_common::properties::StringProperty* moveit_rviz_plugin::PlanningSceneDisplay::move_group_ns_property_
protected

Definition at line 228 of file planning_scene_display.hpp.

◆ node_

rclcpp::Node::SharedPtr moveit_rviz_plugin::PlanningSceneDisplay::node_
protected

Definition at line 244 of file planning_scene_display.hpp.

◆ octree_coloring_property_

rviz_common::properties::EnumProperty* moveit_rviz_plugin::PlanningSceneDisplay::octree_coloring_property_
protected

Definition at line 241 of file planning_scene_display.hpp.

◆ octree_render_property_

rviz_common::properties::EnumProperty* moveit_rviz_plugin::PlanningSceneDisplay::octree_render_property_
protected

Definition at line 240 of file planning_scene_display.hpp.

◆ planning_scene_monitor_

planning_scene_monitor::PlanningSceneMonitorPtr moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_monitor_
protected

Definition at line 205 of file planning_scene_display.hpp.

◆ planning_scene_needs_render_

bool moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_needs_render_
protected

Definition at line 220 of file planning_scene_display.hpp.

◆ planning_scene_node_

Ogre::SceneNode* moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_node_
protected

displays planning scene with everything in it

Definition at line 213 of file planning_scene_display.hpp.

◆ planning_scene_render_

PlanningSceneRenderPtr moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_render_
protected

Definition at line 217 of file planning_scene_display.hpp.

◆ planning_scene_robot_

RobotStateVisualizationPtr moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_robot_
protected

Definition at line 216 of file planning_scene_display.hpp.

◆ planning_scene_topic_property_

rviz_common::properties::RosTopicProperty* moveit_rviz_plugin::PlanningSceneDisplay::planning_scene_topic_property_
protected

Definition at line 234 of file planning_scene_display.hpp.

◆ robot_alpha_property_

rviz_common::properties::FloatProperty* moveit_rviz_plugin::PlanningSceneDisplay::robot_alpha_property_
protected

Definition at line 235 of file planning_scene_display.hpp.

◆ robot_category_

rviz_common::properties::Property* moveit_rviz_plugin::PlanningSceneDisplay::robot_category_
protected

Definition at line 226 of file planning_scene_display.hpp.

◆ robot_description_property_

rviz_common::properties::StringProperty* moveit_rviz_plugin::PlanningSceneDisplay::robot_description_property_
protected

Definition at line 229 of file planning_scene_display.hpp.

◆ robot_model_loading_lock_

std::mutex moveit_rviz_plugin::PlanningSceneDisplay::robot_model_loading_lock_
protected

Definition at line 206 of file planning_scene_display.hpp.

◆ robot_state_needs_render_

bool moveit_rviz_plugin::PlanningSceneDisplay::robot_state_needs_render_
protected

Definition at line 222 of file planning_scene_display.hpp.

◆ scene_alpha_property_

rviz_common::properties::FloatProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_alpha_property_
protected

Definition at line 236 of file planning_scene_display.hpp.

◆ scene_category_

rviz_common::properties::Property* moveit_rviz_plugin::PlanningSceneDisplay::scene_category_
protected

Definition at line 225 of file planning_scene_display.hpp.

◆ scene_color_property_

rviz_common::properties::ColorProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_color_property_
protected

Definition at line 237 of file planning_scene_display.hpp.

◆ scene_display_time_property_

rviz_common::properties::FloatProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_display_time_property_
protected

Definition at line 239 of file planning_scene_display.hpp.

◆ scene_enabled_property_

rviz_common::properties::BoolProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_enabled_property_
protected

Definition at line 231 of file planning_scene_display.hpp.

◆ scene_name_property_

rviz_common::properties::StringProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_name_property_
protected

Definition at line 230 of file planning_scene_display.hpp.

◆ scene_robot_collision_enabled_property_

rviz_common::properties::BoolProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_robot_collision_enabled_property_
protected

Definition at line 233 of file planning_scene_display.hpp.

◆ scene_robot_visual_enabled_property_

rviz_common::properties::BoolProperty* moveit_rviz_plugin::PlanningSceneDisplay::scene_robot_visual_enabled_property_
protected

Definition at line 232 of file planning_scene_display.hpp.


The documentation for this class was generated from the following files: