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