Skip to main content

Planning Scene Monitor

The Planning Scene Monitor is a crucial component in MoveIt and MoveIt Pro that maintains an up-to-date representation of the robot's environment. It acts as a wrapper around the Planning Scene itself, providing thread-safe concurrent access and real-time updates from various sources.

Overview​

The PlanningSceneMonitor class subscribes to various ROS topics to monitor changes in:

  • Robot joint states
  • Collision objects in the world
  • Attached collision objects
  • Full planning scene updates
  • Octomap/occupancy data
  • Transform information

It maintains a consistent, thread-safe view of the planning scene that can be safely accessed by multiple planning threads simultaneously.

Key Features​

Thread-Safe​

The Planning Scene Monitor provides thread-safe access to the planning scene through specialized locking mechanisms:

  • LockedPlanningSceneRO - Read-only access with shared locking
  • LockedPlanningSceneRW - Read-write access with exclusive locking

Real-Time Updates​

The monitor continuously updates the planning scene by subscribing to:

  • /joint_states - Current robot joint positions
  • /collision_object - Objects in the world
  • /attached_collision_object - Objects attached to the robot
  • /planning_scene - Full scene updates or diffs
  • /planning_scene_world - World geometry updates

Scene Update Types​

The monitor categorizes updates into different types using the SceneUpdateType enum:

  • UPDATE_NONE - No update
  • UPDATE_STATE - Robot state updated
  • UPDATE_TRANSFORMS - Transform updates
  • UPDATE_GEOMETRY - World geometry changes
  • UPDATE_SCENE - Complete scene update

Architecture​

The Planning Scene Monitor consists of several key components:

Current State Monitor​

Tracks the current joint states of the robot and updates the planning scene accordingly. This ensures that the planning scene always reflects the actual robot configuration.

Collision Object Monitoring​

Listens for collision object messages and updates the world representation in the planning scene. This includes both static objects and dynamic obstacles.

Transform Management​

Manages coordinate frame transforms using TF2, ensuring that all objects in the scene are properly positioned relative to the robot's base frame.

Occupancy Map Integration​

Optionally integrates with occupancy mapping systems (like OctoMap) to incorporate sensor data about unknown or dynamic obstacles.

Usage Patterns​

Basic Setup​

 // Create a planning scene monitor
 auto psm = std::make_shared<moveit_pro::planning_scene_monitor::PlanningSceneMonitor>(
  node, "robot_description", "planning_scene_monitor");
 
 // Start monitoring various sources
 psm->startStateMonitor(); // Monitor joint states
 psm->startSceneMonitor(); // Monitor planning scene topics
 psm->startWorldGeometryMonitor(); // Monitor collision objects

Thread-Safe Access​

 // Read-only access (allows concurrent readers)
 {
  moveit_pro::planning_scene_monitor::LockedPlanningSceneRO scene(psm);
  // Use scene.getPlanningScene() safely for read operations
  auto robot_state = scene->getCurrentState();
 }
 
 // Read-write access (exclusive lock)
 {
  moveit_pro::planning_scene_monitor::LockedPlanningSceneRW scene(psm);
  // Modify the scene safely
  scene->getCollisionEnvNonConst()->addToObject("object_id", shape, pose);
 }

Update Callbacks​

 // Register callback for scene updates
 psm->addUpdateCallback([](moveit_pro::planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) {
  if (update_type & moveit_pro::planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY) {
  // Handle geometry updates
  RCLCPP_INFO(logger, "World geometry updated");
  }
 });

Configuration Parameters​

Update Frequencies​

  • State Update Frequency: Controls how often robot state updates are processed (default: 10 Hz)
  • Planning Scene Publishing Frequency: Rate at which scene updates are published

Default Topics​

  • Joint States: /joint_states
  • Collision Objects: /collision_object
  • Attached Objects: /attached_collision_object
  • Planning Scene: /planning_scene
  • Scene World: /planning_scene_world
  • Service: /get_planning_scene

Padding and Scaling​

The monitor supports configurable collision checking parameters:

  • Robot Padding: Additional safety margin around robot links
  • Robot Scaling: Scale factor for robot geometry
  • Object Padding: Safety margin around world objects
  • Attached Object Padding: Safety margin for attached objects

Advanced Features​

Scene Differencing​

The monitor can maintain scene diffs to efficiently track changes:

 psm->monitorDiffs(true); // Enable diff monitoring

Publishing Scenes​

The monitor can republish the maintained scene for other nodes:

 psm->startPublishingPlanningScene(
  moveit_pro::planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
  "monitored_planning_scene"
 );

Service Integration​

Provide planning scene as a service for tools like RViz:

 psm->providePlanningSceneService("/get_planning_scene");

Error Handling and Robustness​

The Planning Scene Monitor includes several mechanisms to ensure robust operation:

  • Timeout Protection: Configurable timeouts for state updates and service calls
  • Automatic Recovery: Attempts to recover from temporary communication failures
  • Validation: Validates incoming messages before applying updates
  • Graceful Degradation: Continues operating even if some data sources become unavailable

Integration with MoveIt Pro​

In MoveIt Pro, the Planning Scene Monitor is typically configured through the robot configuration package and used by:

  • Motion Planners: Access current scene for collision checking
  • Behaviors: Query and modify the scene as needed
  • Simulation: Maintain virtual environment state
  • Visualization: Provide scene data to RViz and web interfaces

The monitor ensures that all components have access to a consistent, up-to-date view of the robot's environment, enabling safe and effective motion planning and execution.

Performance Considerations​

  • Memory Usage: The monitor maintains a complete copy of the planning scene in memory
  • Update Frequency: Higher update rates provide more current information but consume more CPU
  • Locking Overhead: Frequent lock acquisition can impact performance in high-throughput scenarios
  • Network Bandwidth: Scene publishing and updates can consume significant network resources

For optimal performance, tune update frequencies based on your application's requirements and available computational resources.

Relevant Headers​

  • moveit_pro/planning_scene_monitor/planning_scene_monitor.hpp - Main PlanningSceneMonitor class
  • moveit_pro/planning_scene_monitor/current_state_monitor.hpp - Robot state monitoring
  • planning_scene/planning_scene.hpp - Core planning scene representation
  • moveit_msgs/msg/planning_scene.hpp - ROS message definitions

See Also​

Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.