Skip to main content

Chapter 2: ROS 2 - The Robotic Nervous System

Introduction to ROS 2 Architecture​

The Robot Operating System 2 (ROS 2) serves as the nervous system for modern humanoid robots, providing the communication infrastructure, development tools, and architectural patterns necessary to build complex robotic systems. Unlike its predecessor, ROS 2 addresses critical limitations in real-time performance, security, and deployment flexibility, making it particularly suitable for humanoid robotics applications where safety, reliability, and deterministic behavior are paramount.

ROS 2 is not an operating system in the traditional sense but rather a middleware framework that provides a collection of libraries, tools, and conventions for building robotic applications. It enables the creation of distributed systems where different components can run on separate processes or even separate machines, communicating through standardized interfaces. This architecture is essential for humanoid robots, which require coordination between perception, planning, control, and interaction modules running at different frequencies and with different computational requirements.

The evolution from ROS 1 to ROS 2 was driven by the need to address the growing complexity of robotic applications and the requirements of commercial deployment. Key improvements in ROS 2 include:

  • Real-time support: Deterministic timing guarantees critical for control systems
  • Security: Built-in authentication, encryption, and access control
  • Multi-language support: Improved support for languages beyond C++ and Python
  • Cross-platform compatibility: Native support for Windows, macOS, and Linux
  • DDS-based communication: More robust and scalable communication infrastructure

Understanding Nodes, Topics, Services, and Actions​

At the heart of ROS 2 lies a set of communication patterns that enable modular, distributed robotic systems. These patterns abstract the complexity of inter-process communication, allowing developers to focus on algorithm development rather than communication infrastructure.

Nodes: The Fundamental Building Blocks​

Nodes are the basic execution units in ROS 2, representing individual processes that perform specific functions. In humanoid robotics, nodes might handle perception (camera processing, sensor fusion), control (motion planning, trajectory execution), or interaction (speech recognition, gesture detection). Each node can publish and subscribe to messages, provide services, or execute actions.

The node architecture enables several key benefits:

  • Modularity: Each component can be developed, tested, and maintained independently
  • Scalability: Components can be distributed across multiple machines as needed
  • Fault tolerance: Failure of one node doesn't necessarily affect others
  • Reusability: Nodes can be reused across different robotic platforms

Topics: Asynchronous Data Streams​

Topics provide a publish-subscribe communication model for streaming data between nodes. This pattern is ideal for sensor data, robot state information, and other continuously updated information. Publishers send messages to topics without knowing which subscribers exist, and subscribers receive messages without knowing which publishers are active.

For humanoid robots, topics are commonly used for:

  • Sensor data streams (camera images, lidar scans, IMU readings)
  • Robot state information (joint positions, velocities, effort)
  • Command streams (velocity commands, trajectory points)
  • Debugging and logging information

The asynchronous nature of topics allows for flexible system design where components can operate at different frequencies. For example, a perception node might publish camera images at 30 Hz while a control node subscribes at 100 Hz, processing images as they arrive.

Services: Request-Response Communication​

Services provide synchronous request-response communication for operations that require a specific result. Unlike topics, services establish a direct connection between a client and a server, ensuring that requests are processed and responses are received. This pattern is suitable for operations that must complete before proceeding, such as calibration routines, configuration changes, or computational tasks.

In humanoid robotics, services might be used for:

  • Robot calibration and initialization
  • Map loading and saving
  • Path planning requests
  • Configuration parameter updates
  • Emergency stop activation

Actions: Goal-Oriented Long-Running Tasks​

Actions extend the service pattern to handle long-running operations that may take seconds or minutes to complete. Actions provide feedback during execution and allow clients to monitor progress, cancel operations, or receive results when complete. This pattern is essential for humanoid robotics tasks such as:

  • Navigation to distant locations
  • Complex manipulation sequences
  • Learning and adaptation processes
  • System diagnostics and maintenance

Actions include three message types:

  • Goal: Defines the desired outcome
  • Feedback: Provides ongoing status during execution
  • Result: Contains the final outcome when the action completes

ROS 2 Quality of Service (QoS) Settings​

Quality of Service (QoS) settings in ROS 2 allow fine-tuning of communication behavior to meet specific application requirements. This is particularly important in humanoid robotics where different data streams have different requirements for reliability, latency, and bandwidth.

Reliability Policy​

The reliability policy determines whether messages must be delivered reliably or if some loss is acceptable:

  • Reliable: All messages are guaranteed to be delivered, with retries for failed transmissions
  • Best Effort: Messages are sent without guarantee of delivery, prioritizing speed over reliability

For humanoid robots, sensor data streams might use best effort for high-frequency data like camera images (where some frames can be dropped), while critical control commands would use reliable delivery to ensure safety.

Durability Policy​

Durability determines how messages are handled with respect to late-joining subscribers:

  • Transient Local: Messages are stored and sent to new subscribers when they join
  • Volatile: Messages are not stored; new subscribers only receive future messages

Control parameter updates might use transient local durability to ensure that all nodes eventually receive configuration changes, while sensor data typically uses volatile durability.

History Policy​

History policy controls how many messages are stored for delivery:

  • Keep Last: Store only the most recent messages
  • Keep All: Store all messages (subject to resource limits)

For real-time control, keep last with a small history is often optimal to ensure the most recent data is available while minimizing latency.

rclpy: Python Client Library for ROS 2​

rclpy is the Python client library for ROS 2, providing Python developers with access to all ROS 2 functionality. Python is particularly well-suited for humanoid robotics development due to its ease of use, extensive scientific computing ecosystem, and rapid prototyping capabilities.

Node Creation and Lifecycle​

Creating a ROS 2 node in Python involves subclassing the Node class and implementing the desired functionality:

import rclpy
from rclpy.node import Node

class HumanoidController(Node):
    def __init__(self):
        super().__init__('humanoid_controller')
        # Initialize publishers, subscribers, services, etc.

The node lifecycle includes initialization, spinning (processing callbacks), and cleanup, with ROS 2 providing automatic resource management and error handling.

Publishers and Subscribers​

Publishers and subscribers in rclpy provide the interface for topic-based communication:

from std_msgs.msg import String

# Create publisher
publisher = self.create_publisher(String, 'topic_name', 10)

# Create subscriber
subscriber = self.create_subscription(
    String, 'topic_name', self.callback_function, 10
)

The third parameter specifies the queue size for message buffering, which is crucial for maintaining real-time performance in humanoid control systems.

Service Clients and Servers​

Service interfaces enable request-response communication:

from example_interfaces.srv import SetBool

# Service server
service = self.create_service(SetBool, 'service_name', self.service_callback)

# Service client
client = self.create_client(SetBool, 'service_name')

Service communication is synchronous, blocking until a response is received, making it suitable for operations that must complete before proceeding.

Action Clients and Servers​

Actions provide the interface for long-running operations:

from rclpy.action import ActionClient
from example_interfaces.action import Fibonacci

action_client = ActionClient(self, Fibonacci, 'fibonacci')

Action clients can monitor progress, receive feedback, and handle cancellation, providing robust control over complex humanoid behaviors.

Humanoid Robot Control with ROS 2​

Controlling a humanoid robot requires precise coordination between multiple subsystems, each operating at different frequencies and with different timing requirements. ROS 2 provides the infrastructure to manage this complexity through its communication patterns and real-time capabilities.

Joint Control Architecture​

Humanoid robots typically use a hierarchical control architecture:

  1. High-level planners generate desired trajectories and behaviors
  2. Mid-level controllers manage balance, coordination, and task execution
  3. Low-level controllers handle individual joint control and safety

ROS 2 facilitates this architecture through appropriate use of communication patterns:

  • High-frequency joint state feedback via topics
  • Trajectory commands via action interfaces for complex movements
  • Configuration changes via services
  • Emergency commands via reliable topics with high priority

Control Loop Implementation​

Implementing control loops in ROS 2 requires careful attention to timing and synchronization:

def __init__(self):
    super().__init__('control_node')
    # Create timers for different control frequencies
    self.create_timer(0.01, self.high_freq_control)  # 100 Hz for joint control
    self.create_timer(0.1, self.mid_freq_control)     # 10 Hz for balance
    self.create_timer(1.0, self.low_freq_monitoring)  # 1 Hz for system monitoring

Each timer callback runs at its specified frequency, allowing different control tasks to operate at appropriate rates while maintaining system stability.

Safety and Emergency Systems​

Safety is paramount in humanoid robotics, and ROS 2 provides mechanisms to implement robust safety systems:

  • Emergency stop topics: High-priority topics that immediately halt all motion
  • Watchdog timers: Monitor node health and trigger safety responses
  • State machines: Manage operational modes and transitions
  • Resource monitoring: Track CPU, memory, and communication usage

These safety mechanisms ensure that humanoid robots operate within safe parameters and respond appropriately to failures or emergencies.

Wiring URDF Models with ROS 2​

The integration of URDF models with ROS 2 systems enables visualization, simulation, and control of humanoid robots. This integration involves several key components that work together to provide a complete representation of the robot.

Robot State Publisher​

The robot_state_publisher node is responsible for publishing the robot's joint states as coordinate transformations (TF). This enables other nodes to understand the spatial relationships between different parts of the robot:

<node pkg="robot_state_publisher" exec="robot_state_publisher" name="robot_state_publisher">
  <param name="publish_frequency" value="50.0"/>
</node>

The robot description parameter must be loaded with the URDF model, typically from a launch file or parameter server.

Joint State Publisher​

The joint_state_publisher provides a simple way to publish joint state information, often used for visualization or testing:

<node pkg="joint_state_publisher" exec="joint_state_publisher" name="joint_state_publisher">
  <param name="rate" value="50"/>
</node>

For actual robots, joint states typically come from hardware interfaces or simulation, but the joint state publisher can provide default values for visualization.

Hardware Interface Integration​

Real humanoid robots require hardware interfaces to connect ROS 2 with physical actuators and sensors:

<node pkg="controller_manager" exec="ros2_control_node" name="ros2_control_node">
  <param name="robot_description" value="$(var robot_description)"/>
</node>

The controller manager loads and manages different controllers for various robot components, enabling coordinated control of the entire humanoid system.

Advanced ROS 2 Concepts for Humanoid Robotics​

Multi-Node Coordination​

Complex humanoid behaviors often require coordination between multiple nodes, each responsible for different aspects of the robot's operation. ROS 2 provides several mechanisms for this coordination:

  • Parameter servers: Share configuration data across nodes
  • Action coordination: Synchronize long-running operations
  • State machines: Manage complex behavioral patterns
  • Lifecycle nodes: Control node startup and shutdown sequences

Real-time Considerations​

Humanoid control systems have strict timing requirements, particularly for balance and safety. ROS 2 supports real-time operation through:

  • Real-time DDS implementations: Ensuring deterministic message delivery
  • Process prioritization: Assigning appropriate priorities to control processes
  • Memory management: Avoiding garbage collection and allocation delays
  • Synchronization primitives: Managing access to shared resources

Performance Optimization​

Optimizing ROS 2 performance for humanoid robotics involves several strategies:

  • Message filtering: Reduce unnecessary data transmission
  • Efficient serialization: Minimize message processing overhead
  • Node colocation: Run related functions in the same process when possible
  • Resource monitoring: Track and optimize system performance

Conclusion​

ROS 2 provides the essential communication infrastructure for humanoid robotics, enabling the development of complex, distributed systems that can coordinate perception, planning, control, and interaction. The framework's flexibility, real-time capabilities, and rich ecosystem of tools make it the foundation for modern humanoid robot development.

Understanding the communication patterns, quality of service settings, and control architecture is crucial for developing robust humanoid robot systems. The next chapter will explore how these systems are simulated in digital environments, allowing for safe testing and development before deployment on physical hardware.

References​

[1] Quigley, M., Conley, K., & Gerkey, B. (2009). ROS: an open-source Robot Operating System. ICRA Workshop on Open Source Software, 3(3.2), 5.

[2] Macenski, S., & Merz, J. (2022). Effective Robotics Programming with ROS 2. Packt Publishing.

[3] ROS.org. (2023). ROS 2 Documentation. Retrieved from https://docs.ros.org/en/rolling/

[4] Faconti, G., & Paluri, M. (2018). Control and communication in multi-robot systems. IEEE Robotics & Automation Magazine, 25(2), 8-20.

[5] Siciliano, B., & Khatib, O. (Eds.). (2016). Springer handbook of robotics. Springer.

Code Examples​

Example 1: Basic Humanoid Controller Node​

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64MultiArray
import numpy as np

class HumanoidController(Node):
    def __init__(self):
        super().__init__('humanoid_controller')

        # Publishers for different control interfaces
        self.joint_cmd_pub = self.create_publisher(
            Float64MultiArray,
            '/joint_group_position_controller/commands',
            10
        )

        # Subscriber for sensor feedback
        self.joint_state_sub = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            10
        )

        # Timer for control loop
        self.control_timer = self.create_timer(0.01, self.control_loop)  # 100 Hz

        # Internal state
        self.current_joint_positions = {}
        self.target_positions = {}

        self.get_logger().info('Humanoid Controller initialized')

    def joint_state_callback(self, msg):
        """Update internal joint state from sensor feedback"""
        for i, name in enumerate(msg.name):
            if i < len(msg.position):
                self.current_joint_positions[name] = msg.position[i]

    def control_loop(self):
        """Main control loop for humanoid robot"""
        # Implement control logic here
        # This could include balance control, trajectory following, etc.
        pass

    def send_joint_commands(self, joint_positions):
        """Send position commands to robot joints"""
        msg = Float64MultiArray()
        msg.data = joint_positions
        self.joint_cmd_pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    controller = HumanoidController()

    try:
        rclpy.spin(controller)
    except KeyboardInterrupt:
        controller.get_logger().info('Shutting down controller')
    finally:
        controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Example 2: ROS 2 Service for Robot Calibration​

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from example_interfaces.srv import Trigger
from sensor_msgs.msg import JointState
import time

class CalibrationService(Node):
    def __init__(self):
        super().__init__('calibration_service')

        # Create service server
        self.srv = self.create_service(
            Trigger,
            'calibrate_robot',
            self.calibrate_robot_callback
        )

        # Subscriber for joint states
        self.joint_sub = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            10
        )

        self.current_positions = {}
        self.get_logger().info('Calibration service ready')

    def joint_state_callback(self, msg):
        """Update current joint positions"""
        for i, name in enumerate(msg.name):
            if i < len(msg.position):
                self.current_positions[name] = msg.position[i]

    def calibrate_robot_callback(self, request, response):
        """Handle calibration request"""
        self.get_logger().info('Starting calibration procedure')

        try:
            # Move to calibration pose
            calibration_pose = self.get_calibration_pose()
            self.move_to_pose(calibration_pose)

            # Wait for stabilization
            time.sleep(2.0)

            # Perform calibration calculations
            self.perform_calibration()

            response.success = True
            response.message = 'Calibration completed successfully'
            self.get_logger().info('Calibration completed')

        except Exception as e:
            response.success = False
            response.message = f'Calibration failed: {str(e)}'
            self.get_logger().error(f'Calibration error: {str(e)}')

        return response

    def get_calibration_pose(self):
        """Define calibration pose for all joints"""
        # Return a dictionary of joint names to target positions
        return {
            'head_joint': 0.0,
            'left_shoulder_pitch': 0.0,
            'left_shoulder_roll': 0.0,
            'left_elbow': 0.0,
            'right_shoulder_pitch': 0.0,
            'right_shoulder_roll': 0.0,
            'right_elbow': 0.0,
            'left_hip_pitch': 0.0,
            'left_hip_roll': 0.0,
            'left_knee': 0.0,
            'right_hip_pitch': 0.0,
            'right_hip_roll': 0.0,
            'right_knee': 0.0
        }

    def move_to_pose(self, pose_dict):
        """Move robot to specified pose"""
        # Implementation would send commands to joint controllers
        pass

    def perform_calibration(self):
        """Perform actual calibration calculations"""
        # Implementation would use current sensor readings
        # to determine calibration offsets
        pass

def main(args=None):
    rclpy.init(args=args)
    service = CalibrationService()

    try:
        rclpy.spin(service)
    except KeyboardInterrupt:
        service.get_logger().info('Shutting down calibration service')
    finally:
        service.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Example 3: ROS 2 Action Server for Walking Pattern​

#!/usr/bin/env python3

import rclpy
from rclpy.action import ActionServer, CancelResponse
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup

from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration

import threading
import time

class WalkingActionServer(Node):
    def __init__(self):
        super().__init__('walking_action_server')

        # Create action server
        self._action_server = ActionServer(
            self,
            FollowJointTrajectory,
            'walking_controller/follow_joint_trajectory',
            execute_callback=self.execute_trajectory,
            callback_group=ReentrantCallbackGroup(),
            cancel_callback=self.cancel_trajectory
        )

        # Publisher for joint commands
        self.trajectory_pub = self.create_publisher(
            JointTrajectory,
            '/joint_trajectory_controller/joint_trajectory',
            10
        )

        self.current_trajectory = None
        self.is_executing = False
        self.get_logger().info('Walking action server initialized')

    def cancel_trajectory(self, goal_handle):
        """Handle trajectory cancellation"""
        self.get_logger().info('Received cancel request')
        if self.is_executing:
            self.is_executing = False
            return CancelResponse.ACCEPT
        return CancelResponse.REJECT

    def execute_trajectory(self, goal_handle):
        """Execute the requested walking trajectory"""
        self.get_logger().info('Executing walking trajectory')

        feedback_msg = FollowJointTrajectory.Feedback()
        result = FollowJointTrajectory.Result()

        trajectory = goal_handle.request.trajectory
        self.current_trajectory = trajectory
        self.is_executing = True

        try:
            # Execute trajectory points
            for i, point in enumerate(trajectory.points):
                if not self.is_executing:
                    goal_handle.canceled()
                    result.error_code = FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED
                    return result

                # Publish trajectory point
                self.publish_trajectory_point(point)

                # Update feedback
                feedback_msg.actual.positions = point.positions
                feedback_msg.actual.velocities = point.velocities
                feedback_msg.desired = point
                feedback_msg.error.positions = [0.0] * len(point.positions)

                goal_handle.publish_feedback(feedback_msg)

                # Wait for execution time
                time.sleep(point.time_from_start.sec + point.time_from_start.nanosec / 1e9)

                if i == len(trajectory.points) - 1:
                    # Last point reached
                    break

            if self.is_executing:
                goal_handle.succeed()
                result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
                self.get_logger().info('Trajectory execution completed successfully')
            else:
                goal_handle.canceled()
                result.error_code = FollowJointTrajectory.Result.INVALID_GOAL

        except Exception as e:
            self.get_logger().error(f'Trajectory execution failed: {str(e)}')
            goal_handle.abort()
            result.error_code = FollowJointTrajectory.Result.INVALID_GOAL

        self.is_executing = False
        return result

    def publish_trajectory_point(self, point):
        """Publish a single trajectory point"""
        trajectory_msg = JointTrajectory()
        trajectory_msg.joint_names = self.get_joint_names()  # Implementation specific
        trajectory_msg.points = [point]
        self.trajectory_pub.publish(trajectory_msg)

    def get_joint_names(self):
        """Return list of joint names for the humanoid"""
        return [
            'left_hip_pitch', 'left_hip_roll', 'left_hip_yaw',
            'left_knee', 'left_ankle_pitch', 'left_ankle_roll',
            'right_hip_pitch', 'right_hip_roll', 'right_hip_yaw',
            'right_knee', 'right_ankle_pitch', 'right_ankle_roll',
            'left_shoulder_pitch', 'left_shoulder_roll', 'left_elbow',
            'right_shoulder_pitch', 'right_shoulder_roll', 'right_elbow'
        ]

def main(args=None):
    rclpy.init(args=args)
    server = WalkingActionServer()

    executor = MultiThreadedExecutor()
    executor.add_node(server)

    try:
        executor.spin()
    except KeyboardInterrupt:
        server.get_logger().info('Shutting down walking action server')
    finally:
        server.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Example 4: ROS 2 Launch File for Humanoid Control System​

<launch>
  <!-- Load robot description -->
  <param name="robot_description" command="xacro $(find-pkg-share humanoid_description)/urdf/humanoid.urdf.xacro"/>

  <!-- Robot state publisher -->
  <node pkg="robot_state_publisher" exec="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" value="50.0"/>
  </node>

  <!-- Joint state publisher (for visualization) -->
  <node pkg="joint_state_publisher" exec="joint_state_publisher" name="joint_state_publisher">
    <param name="rate" value="50"/>
  </node>

  <!-- Humanoid controller -->
  <node pkg="humanoid_control" exec="humanoid_controller" name="humanoid_controller" output="screen">
    <param name="control_frequency" value="100"/>
    <param name="balance_control_enabled" value="true"/>
  </node>

  <!-- Walking pattern generator -->
  <node pkg="humanoid_locomotion" exec="walking_pattern_generator" name="walking_pattern_generator" output="screen">
    <param name="step_height" value="0.05"/>
    <param name="step_length" value="0.3"/>
    <param name="walking_speed" value="0.5"/>
  </node>

  <!-- Sensor processing nodes -->
  <node pkg="imu_processor" exec="imu_processor" name="imu_processor">
    <param name="imu_topic" value="/imu/data"/>
    <param name="filter_frequency" value="100"/>
  </node>

  <!-- Visualization -->
  <node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share humanoid_description)/rviz/humanoid.rviz"/>
</launch>

Example 5: Quality of Service Configuration for Safety-Critical Topics​

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
from std_msgs.msg import Bool
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist

class SafetyManager(Node):
    def __init__(self):
        super().__init__('safety_manager')

        # Define QoS profiles for different types of data
        # Critical safety messages - reliable, keep last
        safety_qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE
        )

        # Sensor data - best effort, keep last few
        sensor_qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE
        )

        # Create publishers with appropriate QoS
        self.emergency_stop_pub = self.create_publisher(
            Bool, 'emergency_stop', safety_qos
        )

        self.joint_state_pub = self.create_publisher(
            JointState, 'joint_states', sensor_qos
        )

        self.cmd_vel_pub = self.create_publisher(
            Twist, 'cmd_vel', safety_qos
        )

        # Create subscribers with matching QoS
        self.emergency_stop_sub = self.create_subscription(
            Bool,
            'emergency_stop',
            self.emergency_stop_callback,
            safety_qos
        )

        self.joint_state_sub = self.create_subscription(
            JointState,
            'joint_states',
            self.joint_state_callback,
            sensor_qos
        )

        # Timer for safety monitoring
        self.safety_timer = self.create_timer(0.01, self.safety_check)  # 100 Hz

        self.get_logger().info('Safety manager initialized with QoS profiles')

    def emergency_stop_callback(self, msg):
        """Handle emergency stop command"""
        if msg.data:
            self.get_logger().warn('Emergency stop activated!')
            # Implement emergency stop logic
            self.execute_emergency_stop()

    def joint_state_callback(self, msg):
        """Process joint state feedback"""
        # Monitor joint states for safety violations
        pass

    def safety_check(self):
        """Perform periodic safety checks"""
        # Check for safety violations
        # Monitor joint limits, velocities, temperatures
        # Check for communication timeouts
        pass

    def execute_emergency_stop(self):
        """Execute emergency stop procedure"""
        # Send stop commands to all controllers
        # Disable actuators if possible
        # Log safety event
        pass

def main(args=None):
    rclpy.init(args=args)
    safety_manager = SafetyManager()

    try:
        rclpy.spin(safety_manager)
    except KeyboardInterrupt:
        safety_manager.get_logger().info('Shutting down safety manager')
    finally:
        safety_manager.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

On this page