Node Descriptions - Bigfire3/waymo GitHub Wiki

This package consists of several ROS2 nodes that work together:

lane_detection_node (View Code)

  • Functionality:

    • Subscribes to raw image data (/image_raw/compressed).
    • Uses OpenCV for image processing to detect lane lines (lane.py, edge_detection.py).
    • Applies perspective transformation for a top-down view.
    • Filters lines based on thickness.
    • Calculates the robot's lateral offset from the lane center.
    • Publishes annotated image (/lane/image_annotated).
    • Publishes calculated center offset (/lane/center_offset - std_msgs/Float64).

  • Dependencies: OpenCV, NumPy, cv_bridge.

obstacle_detection_node (View Code)

  • Functionality:
    • Subscribes to laser scan data (/scan - sensor_msgs/LaserScan).
    • Checks distance measurements in a frontal angular range.
    • Detects if an obstacle is closer than distance_to_stop.
    • Publishes obstacle status (/obstacle/blocked - std_msgs/Bool).
  • Dependencies: -

passing_obstacle_node (View Code)

  • Functionality:
    • Handles the automated obstacle passing maneuver.
    • Activated by state_manager_node when state becomes PASSING_OBSTACLE.
    • Takes exclusive control of /cmd_vel during the maneuver.
    • Subscribes to odometry (/odom) for orientation (yaw).
    • Subscribes to laser scan (/scan) to check if the side is clear of the obstacle.
    • Subscribes to lane offset (/lane/center_offset) to perform lane following on the adjacent lane during the pass.
    • Executes a multi-step sequence: 90° turn left, move sideways (following adjacent lane), 90° turn right, drive straight (following adjacent lane) until the side scan indicates the obstacle is passed, 90° turn right, move sideways back to original lane (following lane), 90° turn left.
    • Uses internal constants for maneuver parameters (turn angles, sideways distance, side scan angles, clear distance threshold).
    • Publishes a signal upon completion (/obstacle/passed - std_msgs/Bool) to notify state_manager_node.
  • Dependencies: NumPy, SciPy (for quaternion conversion).

traffic_light_detection_node (View Code)

  • Functionality:
    • Subscribes to raw image data (/image_raw/compressed - QoS: Best Effort).
    • Detects a specific predefined color range in the HSV color space (currently configured for dark pink/magenta: H=[160-180], S=[50-255], V=[50-255]).
    • Filters detections based on minimum blob area (currently 100 pixels).
    • Publishes the detection status on /traffic_light (std_msgs/Bool - QoS: Reliable).
    • Note: Publishes False if the configured color is detected (interpreted as "Stop") and True if the color is not detected (interpreted as "Go").
    • Includes optional OpenCV debug windows (show_debug_windows flag in code).
  • Dependencies: OpenCV, NumPy.

sign_detection_node (View Code)

  • Functionality:
    • Subscribes to raw image data (/image_raw/compressed).
    • Loads traffic sign template images (e.g., for parking signs) from the waymo/traffic_signs directory within the package.
    • Uses OpenCV's template matching (cv2.matchTemplate) to detect configured signs in the camera feed.
    • Publishes a string message (e.g., "parking_sign_detected") on the /sign topic when a parking sign is identified.
    • Optionally publishes debug images:
      • /debug/cam/template_matching (CompressedImage): Shows the result of the template matching process.
      • /debug/cam/binary_sign (CompressedImage): Shows the binarized image used for sign detection.
  • Dependencies: OpenCV, NumPy, cv_bridge.

parking_node (View Code)

  • Functionality:
    • Manages the automated parking and unparking maneuver.
    • Activated by the state_manager_node when the robot's state changes to PARKING.
    • Takes exclusive control over /cmd_vel during the parking sequence.
    • Subscribes to:
      • /robot/state (std_msgs/String): To monitor the overall robot state.
      • /scan (sensor_msgs/LaserScan): For detecting the initial parking spot opening using side-mounted laser scan checks and ensuring the area is clear during movements.
      • /odom (nav_msgs/Odometry): To get the current orientation (yaw) for precise turns.
      • /lane/center_offset (std_msgs/Float64): Used for lane following when driving to potential parking spots.
      • Executes a multi-phase parking sequence:
        • DRIVING_FOR_INITIAL_LASER_SCAN: Drives forward slowly, following the lane, until an initial laser scan indicates a potential area (e.g., passed the visual sign).
        • INITIAL_SIGN_STOPPING & INITIAL_SIGN_WAITING: Stops briefly after the initial laser detection.
        • DRIVING_TO_SCAN_POINT: Drives a predefined distance to the first (or next) potential parking spot, following the lane.
        • STOPPING_BEFORE_SCAN & SCANNING_FOR_SPOT: Stops and uses laser scans to check if the adjacent parking spot is clear. If not clear, and MAX_PARKING_ATTEMPTS not reached, it moves to the next potential spot.
        • TURNING_RIGHT_FOR_PARKING: Executes a 90° right turn towards the spot.
        • MOVING_INTO_SPOT: Drives a short distance forward into the parking spot.
        • TURNING_LEFT_IN_SPOT_TO_PARK: Executes a 90° left turn to align parallel in the spot.
        • WAITING_IN_SPOT: Pauses for a defined duration in the parked position.
        • TURNING_LEFT_IN_SPOT_TO_GET_OUT: Executes a 90° left turn to prepare for exiting.
        • MOVING_OUT_OF_SPOT: Drives a short distance forward to leave the spot.
        • TURNING_RIGHT_FOR_LANE_FOLLOWING: Executes a 90° right turn to align with the lane again.
        • MANEUVER_COMPLETE: Signals completion.
    • Publishes a std_msgs/Bool message on /parking/finished topic (True when the full maneuver is completed or aborted after max attempts).
    • Dependencies: NumPy, SciPy (for quaternion to Euler conversion).

state_manager_node (View Code)

  • Functionality:
    • Acts as the main state machine. Key states include: STATE_STOPPED_AT_TRAFFIC_LIGHT, STATE_FOLLOW_LANE, STATE_STOPPED_AT_OBSTACLE, STATE_PASSING_OBSTACLE, STATE_STOPPED_AT_TRAFFIC_LIGHT, STATE_PARKING, MANUAL_PAUSE_STATE.
    • Starts in the STATE_STOPPED_AT_TRAFFIC_LIGHT state.
    • Subscribes to:
      • Obstacle status (/obstacle/blocked - Bool, QoS: Best Effort).
      • Lane offset (/lane/center_offset - Float64, QoS: Best Effort).
      • Obstacle passed signal (/obstacle/passed - Bool, QoS: Reliable).
      • Traffic light status (/traffic_light - Bool, QoS: Reliable).
      • Keyboard commands (/keyboard_command - String, QoS: Reliable).
      • Sign detection (/sign - String, QoS: Reliable).
      • Parking finished signal (/parking/finished- Bool, QoS: Reliable).
    • Determines the robot's current operational state based on inputs and internal logic.
    • Initial Traffic Light Check: Waits in STATE_STOPPED_AT_TRAFFIC_LIGHT until the /traffic_light topic sends True (indicating the configured stop-color is not detected). Once True is received, the initial traffic light check is considered done (subscription is destroyed).
    • Manual Pause: Handles the toggle_pause command from /keyboard_command to enter/exit the MANUAL_PAUSE state. Stops the robot immediately upon pausing. Transitions directly to the previous state upon resuming.
    • Publishes the current state (/robot/state - String, QoS: Reliable).
    • Sends velocity commands (/cmd_vel - geometry_msgs/Twist) based on state (e.g., lane following uses P-control on offset with a 200Hz timer).
    • Relinquishes control of /cmd_vel during the PASSING_OBSTACLE state.
  • Dependencies: NumPy.

gui_debug_node (View Code)

  • Functionality:
    • Subscribes to several debug images, which are being published by the corresponding nodes.
      • Used topics begin with /debug/cam.
      • Debug images are being utilized to create a debug canvas with all the debug images.
    • Subscribes to robot state (/robot/state).
    • Subscribes to keyboard handler command (/keyboard_command)
    • Displays the debug canva in an OpenCV window, if toggled by the keyboard command.
    • Logs changes in the robot's state to the console.
    • Primarily serves for debugging and visualization.
  • Dependencies: OpenCV, cv_bridge.

keyboard_handler_node(View Code)

  • Functionality:

    • Listens for keyboard input in its terminal (requires interactive TTY).
    • Detects the 's' and 'd' key press.
    • Publishes a "toggle_pause" or "toggle_debug_canvas" message (std_msgs/String, Reliable QoS) on the /keyboard_command topic.
    • Used to remotely toggle pause/resume for the state_manager_node.
    • Restores terminal settings on exit.

  • Usage:

    • Important: Must be run manually in a separate, interactive terminal
    • In the new terminal, run the following commands: 
      cd ~/ros2_ws/src/waymo
chmod +x run_keyboard_handler_node.bash
source install/setup.bash
./run_keyboard_handler_node.bash
    • Ensure the terminal running the node has keyboard focus.
    • Press s to toggle the pause state (MANUAL_PAUSE <-> previous state).
    • Press d to toggle the debug canvas.
    • Press Ctrl+C in the handler's terminal to stop it cleanly.